// add a node at a pose
// <pos> is x,y,th, with th in radians
// returns node position
int SysSPA2d::addNode(const Vector3d &pos, int id)
{
Node2d nd;
nd.nodeId = id;
nd.arot = pos(2);
nd.trans.head(2) = pos.head(2);
nd.trans(2) = 1.0;
// add in to system
nd.setTransform(); // set up world2node transform
nd.setDr();
int ndi = nodes.size();
nodes.push_back(nd);
return ndi;
}
// set up spiral frame/frame system
void
spa2d_spiral_setup(SysSPA2d &spa,
vector<Matrix<double,3,1>, Eigen::aligned_allocator<Matrix<double,3,1> > > &cps,
Matrix<double,3,3> prec, Matrix<double,3,3> vprec,
Matrix<double,3,3> a10prec, Matrix<double,3,3> a15prec,
double kfang, double initang, double cycles,
double pnoise, double qnoise, double snoise, double dpnoise, double dqnoise)
{
// random seed
unsigned short seed = (unsigned short)utime();
seed48(&seed);
//
// spiral trajectory
//
double radius = 8.0; // initial spiral radius, m
double rdiff = 1.0; // spiral radius expansion per cycle
double ang = initang; // initial angle
double cycle_kfs = 360.0/kfang;
int cyc_kfs = (int)(cycle_kfs+.01);
printf("\n[SPA Spiral] Setting up spiral with %d keyframes, %d per cycle\n", (int)(cycles*cycle_kfs), (int)cycle_kfs);
// printf("[SPA Spiral] Camera poses:\n");
for (int i=0; i<cycles*cycle_kfs; ++i, ang+=kfang*M_PI/180, radius+=rdiff/cycle_kfs)
{
if (ang > 2*M_PI)
ang -= 2*M_PI;
// frame rotation in the world
double a = ang+M_PI/2.0;
double z = radius*cos(ang);
double x = radius*sin(ang)+radius;
Vector3d frt(x,z,1.0); // frame position in the world
// make node
Node2d nd;
nd.arot = a;
nd.trans = frt;
// cout << "Translation: " << nd.trans.transpose() << endl << endl;
nd.normArot();
nd.setTransform(); // set up world2node transform
nd.setDr();
// save it
Matrix<double,3,1> pos;
pos.head(2) = frt.head(2);
pos(2) = nd.arot;
cps.push_back(pos);
// add to SPA
spa.nodes.push_back(nd);
// print out camera poses
// printf(" %f %f %f %f %f %f\n", x,y,z,0.0,ang+M_PI/2,0.0);
}
// add error to initial positions
qnoise = M_PI*qnoise/180.0; // convert to radians
Vector2d tinc;
double vinc = 1.0;
for (int i=0; i<(int)spa.nodes.size(); ++i)
{
Node2d &nd = spa.nodes[i];
// add cross-links between cycles
if (i+cyc_kfs < (int)spa.nodes.size()-2 &&
(1 || (i%cyc_kfs < 3 || (i+cyc_kfs/2)%cyc_kfs < 3))) // add in cross-links
{
add_p2con2d(spa, i+cyc_kfs, i, vprec, vinc-1.0, 0.0);
// add_p2con(spa, i+cyc_kfs, i, vprec, vinc-1.0, 0.0);
// add_p2con(spa, i+cyc_kfs, i, vprec, vinc-1.0, 0.0);
// printf("%d %d \n", i, i+cyc_kfs);
}
if (i>=spa.nFixed) // add error
{
Node2d &ndp = spa.nodes[i-1];
tinc = ndp.w2n * nd.trans;
// modify tinc each time...
double inc = 1.0 + snoise * (drand48() - 0.5);
tinc *= inc;
vinc *= inc;
Vector2d frt2(drand48()*pnoise - pnoise/2,
drand48()*pnoise - pnoise/2);
frt2 += tinc; // ok, randomly modified increment
// cout << endl << nd.trans.head(3).transpose() << endl;
nd.trans.head(2) = ndp.w2n.block<2,2>(0,0).transpose()*frt2 + ndp.trans.head(2);
// cout << nd.trans.head(3).transpose() << endl;
double a = drand48()*qnoise - qnoise/2;
nd.arot += a;
nd.normArot();
nd.setTransform(); // set up world2node transform
nd.setDr(); // set rotational derivatives
}
}
// loop over cameras, adding measurements
for (int i=1; i<(int)cps.size(); ++i)
{
add_p2con2d(spa, i-1, i, prec, 0.0, 0.0);
if (i>2)
add_p2con2d(spa, i-2, i, a10prec, 0.0, 0.0);
if (i>3)
add_p2con2d(spa, i-3, i, a15prec, 0.0, 0.0);
// if (i > cycle_kfs) // add in cross-links
// add_p2con(spa, i, i-cycle_kfs, vprec, 0.0, 0.0);
// printf("[SPA Spiral] Added a measurement %d -> %d\n", i-1, i);
}
// finally, some initial random noise on node positions and angles
for (int i=0; i<(int)spa.nodes.size(); ++i)
{
Node2d &nd = spa.nodes[i];
if (i>=spa.nFixed) // add error
{
Vector2d frt2(drand48()*dpnoise - dpnoise/2,
drand48()*dpnoise - dpnoise/2);
nd.trans.head(2) += frt2;
dqnoise = M_PI*dqnoise/180.0; // convert to radians
double a = drand48()*dqnoise - dqnoise/2;
nd.arot += a;
nd.normArot();
nd.setTransform(); // set up world2node transform
nd.setDr(); // set rotational derivatives
}
}
}
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;
}