int main(int argc, char **argv) { char *fin; if (argc < 2) { cout << "Arguments are: <input filename> [<number of nodes to use>]" << endl; return -1; } // number of nodes to use int nnodes = -1; if (argc > 2) nnodes = atoi(argv[2]); int doiters = 10; if (argc > 3) doiters = atoi(argv[3]); fin = argv[1]; // node translation std::vector< Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > ntrans; // node rotation std::vector< Eigen::Vector4d, Eigen::aligned_allocator<Eigen::Vector4d> > nqrot; // constraint indices std::vector< Eigen::Vector2i, Eigen::aligned_allocator<Eigen::Vector2i> > cind; // constraint local translation std::vector< Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > ctrans; // constraint local rotation as quaternion std::vector< Eigen::Vector4d, Eigen::aligned_allocator<Eigen::Vector4d> > cqrot; // constraint covariance std::vector< Eigen::Matrix<double,6,6>, Eigen::aligned_allocator<Eigen::Matrix<double,6,6> > > cvar; // tracks std::vector<struct tinfo> tracks; ReadSPAFile(fin,ntrans,nqrot,cind,ctrans,cqrot,cvar,tracks); cout << "# [ReadSPAFile] Found " << (int)ntrans.size() << " nodes and " << (int)cind.size() << " constraints" << endl; if (nnodes < 0) nnodes = ntrans.size(); if (nnodes > (int)ntrans.size()) nnodes = ntrans.size(); // system SysSPA spa; spa.verbose=false; // spa.useCholmod(true); spa.csp.useCholmod = true; // add first node Node nd; // rotation Quaternion<double> frq; frq.coeffs() = nqrot[0]; frq.normalize(); if (frq.w() <= 0.0) frq.coeffs() = -frq.coeffs(); nd.qrot = frq.coeffs(); // translation Vector4d v; v.head(3) = ntrans[0]; v(3) = 1.0; nd.trans = v; nd.setTransform(); // set up world2node transform nd.setDr(true); // add to system spa.nodes.push_back(nd); double cumtime = 0.0; //cout << nd.trans.transpose() << endl << endl; // add in nodes for (int i=0; i<nnodes-1; i+=doiters) { for (int j=0; j<doiters; ++j) if (i+j+1 < nnodes) addnode(spa, i+j+1, ntrans, nqrot, cind, ctrans, cqrot, cvar); long long t0, t1; spa.nFixed = 1; // one fixed frame t0 = utime(); // spa.doSPA(1,1.0e-4,SBA_SPARSE_CHOLESKY); spa.doSPA(1,1.0e-4,SBA_BLOCK_JACOBIAN_PCG,1.0e-8,15); t1 = utime(); cumtime += t1 - t0; cerr << "nodes= " << spa.nodes.size() << "\tedges= " << spa.p2cons.size() << "\t chi2= " << spa.calcCost() << "\t time= " << (t1 - t0) * 1e-6 << "\t cumTime= " << cumtime*1e-6 << endl; } printf("[TestSPA] Compute took %0.2f ms/node, total squared cost %0.2f ms\n", 0.001*(double)cumtime/(double)nnodes, cumtime*0.001); //ofstream ofs("sphere-ground.txt"); //for (int i=0; i<(int)ntrans.size(); ++i) //ofs << ntrans[i].transpose() << endl; //ofs.close(); //ofstream ofs2("sphere-opt.txt"); //for (int i=0; i<(int)spa.nodes.size(); ++i) //ofs2 << spa.nodes[i].trans.transpose().head(3) << endl; //ofs2.close(); //spa.writeSparseA("sphere-sparse.txt",true); return 0; }
int main(int argc, char **argv) { // args double lscale = 0.0; double con_weight = 1.0; printf("Args: <lscale 0.0> <scale weight 1.0>\n"); if (argc > 1) lscale = atof(argv[1]); if (argc > 2) con_weight = atof(argv[2]); // set up markers for visualization ros::init(argc, argv, "VisBundler"); ros::NodeHandle n ("~"); ros::Publisher link_pub = n.advertise<visualization_msgs::Marker>("links", 0); ros::Publisher cam_pub = n.advertise<visualization_msgs::Marker>("cameras", 0); // set up system SysSPA spa; Node::initDr(); // set up fixed jacobians initPrecs(); vector<Matrix<double,6,1>,Eigen::aligned_allocator<Matrix<double,6,1> > > cps; double kfang = 5.0; double kfrad = kfang*M_PI/180.0; // create a spiral test trajectory // connections are made between a frame and its three successors spa.nFixed = 1; // three fixed frames spa_spiral_setup(spa, true, cps, // use cross links #if 1 n2prec, n2vprec, n2aprec, n2bprec, // rank-deficient #else diagprec, diagprec, diagprec, diagprec, #endif kfang, M_PI/2.0-3*kfrad, 220*kfang/360.0, // angle per node, init angle, total nodes 0.01, 1.0, 0.1, 0.1, 5.0); // node noise (m,deg), scale noise (increment), cout << "[SPA Spiral] Initial cost is " << spa.calcCost() << endl; cout << "[SPA Spiral] Number of constraints is " << spa.p2cons.size() << endl; #if 0 // write out pose results and originals cout << "[SPAsys] Writing pose file" << endl; ofstream ofs3("P400.init.txt"); for (int i=0; i<(int)cps.size(); i++) { Vector3d tpn = spa.nodes[i].trans.head(3); ofs3 << tpn.transpose() << endl; } ofs3.close(); #endif // add in distance constraint // works with either n0 -> ni or ni -> ni+1 constraints #if 1 ConScale con; con.w = con_weight; // weight for (int i=0; i<(int)cps.size()-3; i++) { int k = i; if (i > 200) { k = 0; con.nd0 = i; // first node con.nd1 = i+1; // second node con.sv = k; // scale index con.ks = (cps[con.nd1].head(3) - cps[con.nd0].head(3)).squaredNorm(); // measured distance spa.scons.push_back(con); con.nd0 = i+1; // first node con.nd1 = i+2; // second node con.sv = k; // scale index con.ks = (cps[con.nd1].head(3) - cps[con.nd0].head(3)).squaredNorm(); // measured distance spa.scons.push_back(con); con.nd0 = i+2; // first node con.nd1 = i+3; // second node con.sv = k; // scale index con.ks = (cps[con.nd1].head(3) - cps[con.nd0].head(3)).squaredNorm(); // measured distance spa.scons.push_back(con); con.nd0 = i; // first node con.nd1 = i+3; // second node con.sv = k; // scale index con.ks = (cps[con.nd1].head(3) - cps[con.nd0].head(3)).squaredNorm(); // measured distance spa.scons.push_back(con); } else { spa.scales.push_back(1.0); Node nd0; Node nd1; con.nd0 = i; // first node con.nd1 = i+1; // second node con.sv = k; // scale index nd0 = spa.nodes[con.nd0]; nd1 = spa.nodes[con.nd1]; con.ks = (nd1.trans.head(3) - nd0.trans.head(3)).squaredNorm(); // measured distance spa.scons.push_back(con); con.nd0 = i+1; // first node con.nd1 = i+2; // second node con.sv = k; // scale index nd0 = spa.nodes[con.nd0]; nd1 = spa.nodes[con.nd1]; con.ks = (nd1.trans.head(3) - nd0.trans.head(3)).squaredNorm(); // measured distance spa.scons.push_back(con); con.nd0 = i+2; // first node con.nd1 = i+3; // second node con.sv = k; // scale index nd0 = spa.nodes[con.nd0]; nd1 = spa.nodes[con.nd1]; con.ks = (nd1.trans.head(3) - nd0.trans.head(3)).squaredNorm(); // measured distance spa.scons.push_back(con); con.nd0 = i; // first node con.nd1 = i+3; // second node con.sv = k; // scale index nd0 = spa.nodes[con.nd0]; nd1 = spa.nodes[con.nd1]; con.ks = (nd1.trans.head(3) - nd0.trans.head(3)).squaredNorm(); // measured distance spa.scons.push_back(con); } #if 0 // doesn't seem to help... if (i>2) { con.nd0 = i-3; // first node con.nd1 = i+1; // second node con.sv = 0; // scale index con.ks = (cps[con.nd1].head(3) - cps[con.nd0].head(3)).squaredNorm(); // measured distance spa.scons.push_back(con); } #endif } #endif // add in cross-link distance constraint // not much effect here... #if 0 { ConScale con; con.w = 0.1; // weight for (int i=72; i<(int)cps.size()-20; i++) { con.nd0 = i; // first node con.nd1 = i-2; // second node con.sv = i-72; // scale index Node nd0 = spa.nodes[con.nd0]; Node nd1 = spa.nodes[con.nd1]; con.ks = (nd1.trans.head(3) - nd0.trans.head(3)).squaredNorm(); // measured distance con.ks = (cps[con.nd1].head(3) - cps[con.nd0].head(3)).squaredNorm(); // measured distance spa.scons.push_back(con); con.nd0 = i-72; con.nd1 = i; con.sv = i-72; nd0 = spa.nodes[con.nd0]; nd1 = spa.nodes[con.nd1]; con.ks = (nd1.trans.head(3) - nd0.trans.head(3)).squaredNorm(); // measured distance con.ks = (cps[con.nd1].head(3) - cps[con.nd0].head(3)).squaredNorm(); // measured distance spa.scons.push_back(con); } } #endif // this adds in a global constraint connecting the two sides // and connecting first and last points if (lscale > 0.0) { // cross-links #if 0 { ConP2 con; con.ndr = 0; con.nd1 = 36; Node nd0 = spa.nodes[con.ndr]; Node nd1 = spa.nodes[con.nd1]; Vector4d trans; trans.head(3) = cps[con.nd1].head(3); trans(3) = 1.0; con.prec = 1000*diagprec; con.tmean = lscale * nd0.w2n * trans; // translation offset Quaternion<double> q0,q1; q0.coeffs() = nd0.qrot; q1.coeffs() = nd1.qrot; con.qpmean = (q0.inverse()*q1).inverse(); spa.p2cons.push_back(con); } { ConP2 con; con.ndr = 72; con.nd1 = 36+72; Node nd0 = spa.nodes[con.ndr]; Node nd1 = spa.nodes[con.nd1]; Vector4d trans; trans.head(3) = cps[con.nd1].head(3); trans(3) = 1.0; con.prec = 1000*diagprec; Quaternion<double> q0,q1; q0.vec() = cps[con.ndr].segment(3,3); q0.w() = sqrt(1.0 - q0.vec().squaredNorm()); q1.vec() = cps[con.nd1].segment(3,3); q1.w() = sqrt(1.0 - q1.vec().squaredNorm()); Matrix<double,3,4> w2n; Vector4d t0; t0.head(3) = cps[con.ndr].head(3); t0(3) = 1.0; transformW2F(w2n,t0,q0); con.tmean = lscale * w2n * trans; // translation offset con.qpmean = (q0.inverse()*q1).inverse(); spa.p2cons.push_back(con); } #endif // global cross-loop constraint // global { ConP2 con; con.ndr = 0; con.nd1 = 3*72; Node nd0 = spa.nodes[con.ndr]; Node nd1 = spa.nodes[con.nd1]; Vector4d trans; trans.head(3) = cps[con.nd1].head(3); trans(3) = 1.0; con.prec = 1000*diagprec; con.tmean = lscale * nd0.w2n * trans; // translation offset Quaternion<double> q0,q1; q0 = nd0.qrot; q1 = nd1.qrot; con.qpmean = (q0.inverse()*q1).inverse(); spa.p2cons.push_back(con); } } // end of global constraints { // test results double sqerr = 0.0; double asqerr = 0.0; for (int i=0; i<(int)cps.size(); i++) { Matrix<double,6,1> &cp = cps[i]; // old camera pose Vector3d tp = cp.head(3); Vector3d tpn = spa.nodes[i].trans.head(3); // printf("\n[TestSPA] Cam %d orig: %0.2f %0.2f %0.2f\n", i, tp[0], tp[1], tp[2]); // printf("[TestSPA] Cam %d new: %0.2f %0.2f %0.2f\n", i, tpn[0], tpn[1], tpn[2]); Vector3d err = tp-tpn; sqerr += err.squaredNorm(); Quaternion<double> qr; qr.vec() = cp.block<3,1>(3,0); qr.w() = sqrt(1.0 - qr.vec().squaredNorm()); Quaternion<double> qn; qn = spa.nodes[i].qrot; double da = qr.angularDistance(qn); asqerr += da; } sqerr = sqerr / (double)(cps.size()); asqerr = asqerr / (double)(cps.size()); printf("\n[TestSPA] RMSE pos is %0.3f m, angle is %0.3f deg\n", sqrt(sqerr), sqrt(asqerr)); } // why do we have to draw twice???? cout << endl << "drawing..." << endl << endl; drawgraph(spa,cam_pub,link_pub); sleep(1); drawgraph(spa,cam_pub,link_pub); printf("Press <ret> to continue\n"); getchar(); // optimize int iters = 20; long long t0, t1; double ttime = 0.0; for (int i=0; i<iters; i++) { t0 = utime(); int niters = spa.doSPA(1,0.0); t1 = utime(); ttime += (double)(t1-t0); drawgraph(spa,cam_pub,link_pub); sleep(1); } printf("[TestSPA] Compute took %0.2f ms/iter\n", 0.001*ttime/(double)iters); #if 0 // write out A matrix spa.setupSys(0.0); cout << "[SPAsys] Writing file" << endl; spa.writeSparseA("A400mono.txt"); #endif #if 0 // write out pose results and originals cout << "[SPAsys] Writing pose file" << endl; ofstream ofs1("P400.ground.txt"); ofstream ofs2("P400.optim.txt"); for (int i=0; i<(int)cps.size(); i++) { Matrix<double,6,1> &cp = cps[i]; // old camera pose Vector3d tp = cp.head(3); Vector3d tpn = spa.nodes[i].trans.head(3); // printf("\n[TestSPA] Cam %d orig: %0.2f %0.2f %0.2f\n", i, tp[0], tp[1], tp[2]); // printf("[TestSPA] Cam %d new: %0.2f %0.2f %0.2f\n", i, tpn[0], tpn[1], tpn[2]); ofs1 << tp.transpose() << endl; ofs2 << tpn.transpose() << endl; } ofs1.close(); ofs2.close(); #endif // test results double sqerr = 0.0; double asqerr = 0.0; for (int i=0; i<(int)cps.size(); i++) { Matrix<double,6,1> &cp = cps[i]; // old camera pose Vector3d tp = cp.head(3); Vector3d tpn = spa.nodes[i].trans.head(3); // printf("\n[TestSPA] Cam %d orig: %0.2f %0.2f %0.2f\n", i, tp[0], tp[1], tp[2]); // printf("[TestSPA] Cam %d new: %0.2f %0.2f %0.2f\n", i, tpn[0], tpn[1], tpn[2]); Vector3d err = tp-tpn; sqerr += err.squaredNorm(); Quaternion<double> qr; qr.vec() = cp.block<3,1>(3,0); qr.w() = sqrt(1.0 - qr.vec().squaredNorm()); Quaternion<double> qn; qn = spa.nodes[i].qrot; double da = qr.angularDistance(qn); asqerr += da; } sqerr = sqerr / (double)(cps.size()); asqerr = asqerr / (double)(cps.size()); printf("\n[TestSPA] RMSE pos is %0.3f m, angle is %0.3f deg\n", sqrt(sqerr), sqrt(asqerr)); // draw graph cout << endl << "drawing..." << endl << endl; drawgraph(spa,cam_pub,link_pub); return 0; }