int SysSPA2d::doSPAwindowed(int window, int niter, double sLambda, int useCSparse)
{
// number of nodes
int nnodes = nodes.size();
if (nnodes < 2) return 0;
int nlow = nnodes - window;
if (nlow < 1) nlow = 1; // always have one fixed node
if (verbose)
cout << "[SPA Window] From " << nlow << " to " << nnodes << endl;
// number of constraints
int ncons = p2cons.size();
// set up SPA
SysSPA2d spa;
spa.verbose = verbose;
// node, constraint vectors and index mapping
std::vector<Node2d,Eigen::aligned_allocator<Node2d> > &wnodes = spa.nodes;
std::vector<Con2dP2,Eigen::aligned_allocator<Con2dP2> > &wp2cons = spa.p2cons;
std::map<int,int> inds;
std::vector<int> rinds; // reverse indices
// loop through all constraints and set up fixed nodes and constraints
for (int i=0; i<ncons; i++)
{
Con2dP2 &con = p2cons[i];
if (con.ndr >= nlow || con.nd1 >= nlow)
wp2cons.push_back(con);
if (con.ndr >= nlow && con.nd1 < nlow) // have a winner
{
int j = getind(inds,con.nd1); // corresponding index
if (j < 0) // not present, add it in
{
inds.insert(std::pair<int,int>(con.nd1,wnodes.size()));
wnodes.push_back(nodes[con.nd1]);
}
rinds.push_back(con.nd1);
}
else if (con.nd1 >= nlow && con.ndr < nlow)
{
int j = getind(inds,con.ndr); // corresponding index
if (j < 0) // not present, add it in
{
inds.insert(std::pair<int,int>(con.ndr,wnodes.size()));
wnodes.push_back(nodes[con.ndr]);
}
rinds.push_back(con.ndr);
}
}
spa.nFixed = wnodes.size();
if (verbose)
cout << "[SPA Window] Fixed node count: " << spa.nFixed << endl;
// add in variable nodes
for (int i=0; i<(int)wp2cons.size(); i++)
{
Con2dP2 &con = wp2cons[i];
if (con.nd1 >= nlow && con.ndr >= nlow) // have a winner
{
int n0 = getind(inds,con.ndr);
if (n0 < 0)
{
inds.insert(std::pair<int,int>(con.ndr,wnodes.size()));
wnodes.push_back(nodes[con.ndr]);
rinds.push_back(con.ndr);
}
int n1 = getind(inds,con.nd1);
if (n1 < 0)
{
inds.insert(std::pair<int,int>(con.nd1,wnodes.size()));
wnodes.push_back(nodes[con.nd1]);
rinds.push_back(con.nd1);
}
}
}
if (verbose)
{
cout << "[SPA Window] Variable node count: " << spa.nodes.size() - spa.nFixed << endl;
cout << "[SPA Window] Constraint count: " << spa.p2cons.size() << endl;
}
// new constraint indices
for (int i=0; i<(int)wp2cons.size(); i++)
{
Con2dP2 &con = wp2cons[i];
con.ndr = getind(inds,con.ndr);
con.nd1 = getind(inds,con.nd1);
}
// run spa
niter = spa.doSPA(niter,sLambda,useCSparse);
// reset constraint indices
for (int i=0; i<(int)wp2cons.size(); i++)
{
Con2dP2 &con = wp2cons[i];
con.ndr = rinds[con.ndr];
con.nd1 = rinds[con.nd1];
}
return niter;
}
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 = 0;
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::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d> > ntrans;
// node rotation
std::vector< double > arots;
// constraint indices
std::vector< Eigen::Vector2i, Eigen::aligned_allocator<Eigen::Vector2i> > cind;
// constraint local translation
std::vector< Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d> > ctrans;
// constraint local rotation as quaternion
std::vector< double > carot;
// constraint precision
std::vector< Eigen::Matrix<double,3,3>, Eigen::aligned_allocator<Eigen::Matrix<double,3,3> > > cvar;
// scans
std::vector< std::vector< Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d> > > scans;
ReadSPA2dFile(fin,ntrans,arots,cind,ctrans,carot,cvar,scans);
cout << "# [ReadSPA2dFile] Found " << (int)ntrans.size() << " nodes and "
<< (int)cind.size() << " constraints" << endl;
// system
SysSPA2d spa;
spa.verbose=false;
spa.print_iros_stats=false;
// spa.useCholmod(false);
spa.useCholmod(true);
// use max nodes if we haven't specified it
if (nnodes == 0) nnodes = ntrans.size();
if (nnodes > (int)ntrans.size()) nnodes = ntrans.size();
// add first node
Node2d nd;
// rotation
nd.arot = arots[0];
// translation
Vector3d v;
v.head(2) = ntrans[0];
v(2) = 1.0;
nd.trans = v;
double cumtime = 0.0;
//cout << nd.trans.transpose() << endl << endl;
// add to system
nd.setTransform(); // set up world2node transform
nd.setDr();
spa.nodes.push_back(nd);
// add in nodes
for (int i=0; i<nnodes-1; i+=doiters)
{
for (int j=0; j<doiters; j++)
addnode(spa, i+j+1, ntrans, arots, cind, ctrans, carot, cvar);
// cout << "[SysSPA2d] Using " << (int)spa.nodes.size() << " nodes and "
// << (int)spa.p2cons.size() << " constraints" << endl;
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;
if (i%100 == 0)
{
cout << "[SPA2D] iteration: " << i << " squared cost " << spa.errcost << endl;
cerr << i << " " << cumtime*.001 << " " << spa.errcost << endl;
}
}
printf("[TestSPA2D] Compute took %0.2f ms/node, total time %0.2f ms; error %0.2f\n", 0.001*(double)cumtime/(double)nnodes, cumtime*0.001, spa.errcost);
// printf("[TestSPA] Accepted iterations: %d\n", niters);
// printf("[TestSPA] Distance cost: %0.3f m rms\n", sqrt(spa.calcCost(true)/(double)spa.p2cons.size()));
// if (verbose()){
// cerr << "iteration= " << niters
// << "\t chi2= " << spa.calcCost();
// << "\t time= " << 0.0
// << "\t cumTime= " << 0.0
// << "\t kurtChi2= " << this->kurtChi2()
// << endl;
// }
#if 0
ofstream ofs("opt2d-ground.txt");
for (int i=0; i<(int)ntrans.size(); i++)
ofs << ntrans[i].transpose() << endl;
ofs.close();
ofstream ofs2("opt2d-opt.txt");
for (int i=0; i<(int)spa.nodes.size(); i++)
ofs2 << spa.nodes[i].trans.transpose().head(2) << endl;
ofs2.close();
#endif
// spa.writeSparseA("sphere-sparse.txt",true);
return 0;
}