// 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; // Add error to both translation and rotation. temptrans.x() += transscale*(drand48() - 0.5); temptrans.y() += transscale*(drand48() - 0.5); temptrans.z() += transscale*(drand48() - 0.5); tempqrot.x() += rotscale*(drand48() - 0.5); tempqrot.y() += rotscale*(drand48() - 0.5); tempqrot.z() += rotscale*(drand48() - 0.5); tempqrot.normalize(); sys.nodes[i].trans = temptrans; sys.nodes[i].qrot = tempqrot; // These methods should be called to update the node. sys.nodes[i].normRot(); sys.nodes[i].setTransform(); sys.nodes[i].setProjection(); sys.nodes[i].setDr(true); } }