C++ SysSBA::addMonoProj方法代码示例

// add measurements, assuming ref counts present
// <cind> is the camera/node index
int add_points(SysSBA &sba, double inoise, int cind, 
               vector< Vector4d,Eigen::aligned_allocator<Vector4d> >& Wpts, vector<int> &Wptref,
               vector<int> &Wptind)
{
  int ntot = 0;
  double inoise2 = inoise*0.5;
  int npts = Wpts.size();
  Node &ci = sba.nodes[cind];
  double maxx = 2.0*ci.Kcam(0,2); // cx
  double maxy = 2.0*ci.Kcam(1,2); // cy
  for (int i=0; i<npts; ++i)
    {
      if (Wptref[i] < 2) continue; // have to have at least two points
      Vector2d qi;
      Vector3d qw;
      qw = ci.w2n * Wpts[i]; // point in camera coords
      if (qw[2] > near && qw[2] < far)
        {
          ci.project2im(qi,Wpts[i]); // point in image coords
          if (qi[0] > 0.5 && qi[0] < maxx &&
              qi[1] > 0.5 && qi[1] < maxy)
            {
              qi[0] += drand48() * inoise - inoise2; // add noise now
              qi[1] += drand48() * inoise - inoise2;
              //              cout << "Cam and pt indices: " << cind << " " << Wptind[i] << endl;
              sba.addMonoProj(cind, Wptind[i], qi);
              ++ntot;
            }
        }
    }  
  return ntot;
}

// add point to camera set
// <cinds> is a vector of camera node indices
void
add_sphere_points(SysSBA &sba, double inoise, set<int> &cinds, int ncpts)
{
  double inoise2 = inoise*0.5;

  for (int i=0; i<ncpts; ++i)
    {
      // make random point in 0.5m sphere
      Vector4d pt;
      pt[0] = drand48() - 0.5;
      pt[1] = drand48() - 0.5;
      pt[2] = drand48() - 0.5;
      pt[3] = 1.0;
      int pti = sba.addPoint(pt);

      // now add projections to each camera
      set<int>::iterator it;
      for (it=cinds.begin(); it!=cinds.end(); ++it)
	{
	  Node &nd = sba.nodes[*it];
	  Vector2d qi;
	  Vector3d qw;
	  qw = nd.w2n * pt; // point in camera coords
          nd.project2im(qi,pt); // point in image coords
	  qi[0] += drand48() * inoise - inoise2; // add noise now
	  qi[1] += drand48() * inoise - inoise2;
	  //              cout << "Cam and pt indices: " << cind << " " << Wptind[i] << endl;
	  sba.addMonoProj(*it, pti, qi);
	}
    }
}

void setupSBA(SysSBA &sys)
{
    // Create camera parameters.
    frame_common::CamParams cam_params;
    cam_params.fx = 430; // Focal length in x
    cam_params.fy = 430; // Focal length in y
    cam_params.cx = 320; // X position of principal point
    cam_params.cy = 240; // Y position of principal point
    cam_params.tx = 0;   // Baseline (no baseline since this is monocular)

    // Define dimensions of the image.
    int maxx = 640;
    int maxy = 480;

    // Create a plane containing a wall of points.
    int npts_x = 10; // Number of points on the plane in x
    int npts_y = 5;  // Number of points on the plane in y
    
    double plane_width = 5;     // Width of the plane on which points are positioned (x)
    double plane_height = 2.5;    // Height of the plane on which points are positioned (y)
    double plane_distance = 5; // Distance of the plane from the cameras (z)

    // Vector containing the true point positions.
    vector<Point, Eigen::aligned_allocator<Point> > points;

    for (int ix = 0; ix < npts_x ; ix++)
    {
      for (int iy = 0; iy < npts_y ; iy++)
      {
        // Create a point on the plane in a grid.
        points.push_back(Point(plane_width/npts_x*(ix+.5), -plane_height/npts_y*(iy+.5), plane_distance, 1.0));
      }
    }
    
    // Create nodes and add them to the system.
    unsigned int nnodes = 5; // Number of nodes.
    double path_length = 3; // Length of the path the nodes traverse.
    
    unsigned int i = 0, j = 0;
    
    for (i = 0; i < nnodes; i++)
    { 
      // Translate in the x direction over the node path.
      Vector4d trans(i/(nnodes-1.0)*path_length, 0, 0, 1);
            
      // Don't rotate.
      Quaterniond rot(1, 0, 0, 0);
      rot.normalize();
      
      // Add a new node to the system.
      sys.addNode(trans, rot, cam_params, false);
    }
        
    // Set the random seed.
    unsigned short seed = (unsigned short)time(NULL);
    seed48(&seed);
    double ptscale = 1.0;
        
    // Add points into the system, and add noise.
    for (i = 0; i < points.size(); i++)
    {
      // Add up to .5 points of noise.
      Vector4d temppoint = points[i];
      temppoint.x() += ptscale*(drand48() - 0.5);
      temppoint.y() += ptscale*(drand48() - 0.5);
      temppoint.z() += ptscale*(drand48() - 0.5);
      sys.addPoint(temppoint);
    }
    
    Vector2d proj;
    
    // Project points into nodes.
    for (i = 0; i < points.size(); i++)
    {
      for (j = 0; j < sys.nodes.size(); j++)
      {
        // Project the point into the node's image coordinate system.
        sys.nodes[j].setProjection();
        sys.nodes[j].project2im(proj, points[i]);
        
        // If valid (within the range of the image size), add the monocular 
        // projection to SBA.
        if (proj.x() > 0 && proj.x() < maxx-1 && proj.y() > 0 && proj.y() < maxy-1)
        {
          sys.addMonoProj(j, i, proj);
          //printf("Adding projection: Node: %d Point: %d Proj: %f %f\n", j, i, proj.x(), proj.y());
        }
      }
    }
    
    // Add noise to node position.
    
    double transscale = 1.0;
    double rotscale = 0.2;
    
    // Don't actually add noise to the first node, since it's fixed.
    for (i = 1; i < sys.nodes.size(); i++)
    {
      Vector4d temptrans = sys.nodes[i].trans;
      Quaterniond tempqrot = sys.nodes[i].qrot;
//.........这里部分代码省略.........