void setupSBA(SysSBA &sba) { // 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 = -30; // Baseline (no baseline since this is monocular) // Create a plane containing a wall of points. Plane plane0, plane1; plane0.resize(3, 2, 10, 5); plane1 = plane0; plane1.translate(0.1, 0.05, 0.0); plane1.rotate(PI/4.0, 1, 0, 0); plane1.translate(0.0, 0.0, 7.0); plane0.rotate(PI/4.0, 1, 0, 0); plane0.translate(0.0, 0.0, 7.0); //plane1.translate(0.05, 0.0, 0.05); // Create nodes and add them to the system. unsigned int nnodes = 2; // Number of nodes. double path_length = 2; // Length of the path the nodes traverse. // Set the random seed. unsigned short seed = (unsigned short)time(NULL); seed48(&seed); for (int 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); #if 0 if (i >= 0) { // perturb a little double tnoise = 0.5; // meters trans.x() += tnoise*(drand48()-0.5); trans.y() += tnoise*(drand48()-0.5); trans.z() += tnoise*(drand48()-0.5); } #endif // Don't rotate. Quaterniond rot(1, 0, 0, 0); #if 0 if (i > 0) { // perturb a little double qnoise = 0.1; // meters rot.x() += qnoise*(drand48()-0.5); rot.y() += qnoise*(drand48()-0.5); rot.z() += qnoise*(drand48()-0.5); } #endif rot.normalize(); // Add a new node to the system. sba.addNode(trans, rot, cam_params, false); } Vector3d imagenormal(0, 0, 1); Matrix3d covar0; covar0 << sqrt(imagenormal(0)), 0, 0, 0, sqrt(imagenormal(1)), 0, 0, 0, sqrt(imagenormal(2)); Matrix3d covar; Quaterniond rotation; Matrix3d rotmat; // Project points into nodes. addPointAndProjection(sba, plane0.points, 0); addPointAndProjection(sba, plane1.points, 1); int offset = plane0.points.size(); Vector3d normal0 = sba.nodes[0].qrot.toRotationMatrix().transpose()*plane0.normal; Vector3d normal1 = sba.nodes[1].qrot.toRotationMatrix().transpose()*plane1.normal; printf("Normal: %f %f %f -> %f %f %f\n", plane0.normal.x(), plane0.normal.y(), plane0.normal.z(), normal0.x(), normal0.y(), normal0.z()); printf("Normal: %f %f %f -> %f %f %f\n", plane1.normal.x(), plane1.normal.y(), plane1.normal.z(), normal1.x(), normal1.y(), normal1.z()); for (int i = 0; i < plane0.points.size(); i++) { sba.addPointPlaneMatch(0, i, normal0, 1, i+offset, normal1); Matrix3d covar; covar << 0.1, 0, 0, 0, 0.1, 0, 0, 0, 0.1; sba.setProjCovariance(0, i+offset, covar); sba.setProjCovariance(1, i, covar); } // Add noise to node position. double transscale = 1.0; double rotscale = 0.1; // Don't actually add noise to the first node, since it's fixed. for (int i = 1; i < sba.nodes.size(); i++) { Vector4d temptrans = sba.nodes[i].trans; Quaterniond tempqrot = sba.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(); sba.nodes[i].trans = temptrans; sba.nodes[i].qrot = tempqrot; // These methods should be called to update the node. sba.nodes[i].normRot(); sba.nodes[i].setTransform(); sba.nodes[i].setProjection(); sba.nodes[i].setDr(true); } }
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.3; // Baseline // Define dimensions of the image. int maxx = 640; int maxy = 480; // Create a plane containing a wall of points. Plane middleplane; middleplane.resize(3, 2, 10, 5); //middleplane.rotate(PI/4.0, PI/6.0, 1, 0); middleplane.rotate(PI/4.0, 1, 0, 1); middleplane.translate(0.0, 0.0, 5.0); // Create another plane containing a wall of points. Plane mp2; mp2.resize(3, 2, 10, 5); mp2.rotate(0, 0, 0, 1); mp2.translate(0.0, 0.0, 4.0); // Create another plane containing a wall of points. Plane mp3; mp3.resize(3, 2, 10, 5); mp3.rotate(-PI/4.0, 1, 0, 1); mp3.translate(0.0, 0.0, 4.5); // Vector containing the true point positions. vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > normals; points.insert(points.end(), middleplane.points.begin(), middleplane.points.end()); normals.insert(normals.end(), middleplane.points.size(), middleplane.normal); points.insert(points.end(), mp2.points.begin(), mp2.points.end()); normals.insert(normals.end(), mp2.points.size(), mp2.normal); points.insert(points.end(), mp3.points.begin(), mp3.points.end()); normals.insert(normals.end(), mp3.points.size(), mp3.normal); // Create nodes and add them to the system. unsigned int nnodes = 2; // Number of nodes. double path_length = 1.0; // Length of the path the nodes traverse. // Set the random seed. unsigned short seed = (unsigned short)time(NULL); seed48(&seed); unsigned int i = 0; Vector3d inormal0 = middleplane.normal; Vector3d inormal1 = middleplane.normal; Vector3d inormal20 = mp2.normal; Vector3d inormal21 = mp2.normal; Vector3d inormal30 = mp3.normal; Vector3d inormal31 = mp3.normal; 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); #if 1 if (i >= 2) { // perturb a little double tnoise = 0.5; // meters trans.x() += tnoise*(drand48()-0.5); trans.y() += tnoise*(drand48()-0.5); trans.z() += tnoise*(drand48()-0.5); } #endif // Don't rotate. Quaterniond rot(1, 0, 0, 0); #if 1 if (i >= 2) { // perturb a little double qnoise = 0.1; // meters rot.x() += qnoise*(drand48()-0.5); rot.y() += qnoise*(drand48()-0.5); rot.z() += qnoise*(drand48()-0.5); } #endif rot.normalize(); // Add a new node to the system. sys.addNode(trans, rot, cam_params, false); // set normal if (i == 0) { inormal0 = rot.toRotationMatrix().transpose() * inormal0; inormal20 = rot.toRotationMatrix().transpose() * inormal20; inormal30 = rot.toRotationMatrix().transpose() * inormal30; } else { inormal1 = rot.toRotationMatrix().transpose() * inormal1; inormal21 = rot.toRotationMatrix().transpose() * inormal21; inormal31 = rot.toRotationMatrix().transpose() * inormal31; } } double pointnoise = 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() += pointnoise*(drand48() - 0.5); temppoint.y() += pointnoise*(drand48() - 0.5); temppoint.z() += pointnoise*(drand48() - 0.5); sys.addPoint(temppoint); } // Each point gets added twice for (i = 0; i < points.size(); i++) { // Add up to .5 points of noise. Vector4d temppoint = points[i]; temppoint.x() += pointnoise*(drand48() - 0.5); temppoint.y() += pointnoise*(drand48() - 0.5); temppoint.z() += pointnoise*(drand48() - 0.5); sys.addPoint(temppoint); } Vector2d proj2d, proj2dp; Vector3d proj, projp, pc, pcp, baseline, baselinep; Vector3d imagenormal(0, 0, 1); Matrix3d covar0; covar0 << sqrt(imagenormal(0)), 0, 0, 0, sqrt(imagenormal(1)), 0, 0, 0, sqrt(imagenormal(2)); Matrix3d covar; Quaterniond rotation; Matrix3d rotmat; unsigned int midindex = middleplane.points.size(); printf("Normal for Middle Plane: [%f %f %f], index %d -> %d\n", middleplane.normal.x(), middleplane.normal.y(), middleplane.normal.z(), 0, midindex-1); int nn = points.size(); // Project points into nodes. for (i = 0; i < points.size(); i++) { int k=i; // Project the point into the node's image coordinate system. sys.nodes[0].setProjection(); sys.nodes[0].project2im(proj2d, points[k]); sys.nodes[1].setProjection(); sys.nodes[1].project2im(proj2dp, points[k]); // Camera coords for right camera baseline << sys.nodes[0].baseline, 0, 0; pc = sys.nodes[0].Kcam * (sys.nodes[0].w2n*points[k] - baseline); proj.head<2>() = proj2d; proj(2) = pc(0)/pc(2); baseline << sys.nodes[1].baseline, 0, 0; pcp = sys.nodes[1].Kcam * (sys.nodes[1].w2n*points[k] - baseline); projp.head<2>() = proj2dp; projp(2) = pcp(0)/pcp(2); // add noise to projections double prnoise = 0.5; // 0.5 pixels proj.head<2>() += Vector2d(prnoise*(drand48() - 0.5),prnoise*(drand48() - 0.5)); projp.head<2>() += Vector2d(prnoise*(drand48() - 0.5),prnoise*(drand48() - 0.5)); // If valid (within the range of the image size), add the stereo // projection to SBA. if (proj.x() > 0 && proj.x() < maxx && proj.y() > 0 && proj.y() < maxy) { // add point cloud shape-holding projections to each node sys.addStereoProj(0, k, proj); sys.addStereoProj(1, k+nn, projp); #ifdef USE_PP // add exact matches if (i == 20 || i == 65 || i == 142) sys.addStereoProj(1, k, projp); #endif #ifdef USE_PPL // add point-plane matches if (i < midindex) sys.addPointPlaneMatch(0, k, inormal0, 1, k+nn, inormal1); else if (i < 2*midindex) sys.addPointPlaneMatch(0, k, inormal20, 1, k+nn, inormal21); else sys.addPointPlaneMatch(0, k, inormal30, 1, k+nn, inormal31); // sys.addStereoProj(0, k+nn, projp); // sys.addStereoProj(1, k, proj); Matrix3d covar; double cv = 0.01; covar << cv, 0, 0, 0, cv, 0, 0, 0, cv; sys.setProjCovariance(0, k+nn, covar); sys.setProjCovariance(1, k, covar); #endif } else { cout << "ERROR! point not in view of nodes" << endl; //return; } } // Add noise to node position. double transscale = 2.0; double rotscale = 0.5; // 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); } }