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;
}
