41 #include <Eigen/Cholesky>
44 using namespace Eigen;
54 auto duration = std::chrono::system_clock::now().time_since_epoch();
55 return std::chrono::duration_cast<std::chrono::microseconds>(duration).count();
63 void Node2d::setTransform()
65 w2n(0,0) = w2n(1,1) = cos(arot);
69 w2n.col(2) = -w2n*trans;
78 dRdx(0,0) = dRdx(1,1) = w2n(1,0);
80 dRdx(1,0) = -w2n(0,0);
86 void Con2dP2::setJacobians(std::vector<
Node2d,Eigen::aligned_allocator<Node2d> > &nodes)
90 Matrix<double,3,1> &tr = nr.
trans;
92 Matrix<double,3,1> &t1 = n1.
trans;
95 Eigen::Matrix<double,2,1> pc = nr.
w2n * t1;
101 J0.block<2,2>(0,0) = -nr.
w2n.block<2,2>(0,0);
106 Eigen::Matrix<double,2,1> pwt;
107 pwt = (t1-tr).head(2);
110 Eigen::Matrix<double,2,1> dp = nr.
dRdx * pwt;
111 J0.block<2,1>(0,2) = dp;
114 J0.block<1,2>(2,0).setZero();
119 J0t = J0.transpose();
126 J1.block<2,2>(0,0) = nr.
w2n.block<2,2>(0,0);
129 J1.block<2,1>(0,2).setZero();
132 J1.block<1,2>(2,0).setZero();
138 J1t = J1.transpose();
150 err.block<2,1>(0,0) = nd0.
w2n * nd1.
trans - tmean;
151 double aerr = (nd1.
arot - nd0.
arot) - amean;
152 if (aerr > M_PI) aerr -= 2.0*M_PI;
153 if (aerr < -M_PI) aerr += 2.0*M_PI;
158 return err.dot(prec * err);
165 Vector2d derr = nd0.
w2n * nd1.
trans - tmean;
166 return derr.dot(derr);
173 double SysSPA2d::calcCost(
bool tcost)
180 for(
size_t i=0; i<p2cons.size(); i++)
191 for(
size_t i=0; i<p2cons.size(); i++)
207 int SysSPA2d::addNode(
const Vector3d &pos,
int id)
213 nd.
trans.head(2) = pos.head(2);
219 int ndi = nodes.size();
230 bool SysSPA2d::addConstraint(
int ndi0,
int ndi1,
const Vector3d &mean,
231 const Matrix3d &prec)
233 int ni0 = -1, ni1 = -1;
234 for (
int i=0; i<(int)nodes.size(); i++)
236 if (nodes[i].nodeId == ndi0)
238 if (nodes[i].nodeId == ndi1)
241 if (ni0 < 0 || ni1 < 0)
return false;
247 con.
tmean = mean.head(2);
250 p2cons.push_back(con);
258 void SysSPA2d::setupSys(
double sLambda)
262 int nFree = nodes.size() - nFixed;
263 A.setZero(3*nFree,3*nFree);
265 VectorXi dcnt(nFree);
269 double lam = 1.0 + sLambda;
272 for(
size_t pi=0; pi<p2cons.size(); pi++)
278 int i0 = 3*(con.
ndr-nFixed);
279 int i1 = 3*(con.
nd1-nFixed);
283 A.block<3,3>(i0,i0) += con.
J0t * con.
prec * con.
J0;
284 dcnt(con.
ndr - nFixed)++;
288 dcnt(con.
nd1 - nFixed)++;
289 Matrix<double,3,3> tp = con.
prec * con.
J1;
290 A.block<3,3>(i1,i1) += con.
J1t * tp;
293 A.block<3,3>(i0,i1) += con.
J0t * con.
prec * con.
J1;
294 A.block<3,3>(i1,i0) += con.
J1t * con.
prec * con.
J0;
300 B.block<3,1>(i0,0) -= con.
J0t * con.
prec * con.
err;
302 B.block<3,1>(i1,0) -= con.
J1t * con.
prec * con.
err;
310 for (
int i=0; i<3*nFree; i++)
311 for (
int j=0; j<3*nFree; j++)
312 if (std::isnan(A(i,j)) ) { printf(
"[SetupSys] NaN in A\n"); *(
int *)0x0 = 0; }
314 for (
int j=0; j<3*nFree; j++)
315 if (std::isnan(B[j]) ) { printf(
"[SetupSys] NaN in B\n"); *(
int *)0x0 = 0; }
318 for (
int i=0; i<nFree; i++)
319 if (dcnt(i) == 0) ndc++;
322 cout <<
"[SetupSys] " << ndc <<
" disconnected nodes" << endl;
328 void SysSPA2d::setupSparseSys(
double sLambda,
int iter,
int sparseType)
332 int nFree = nodes.size() - nFixed;
334 long long t0, t1, t2, t3;
338 csp.setupBlockStructure(nFree);
340 csp.setupBlockStructure(0);
344 VectorXi dcnt(nFree);
348 double lam = 1.0 + sLambda;
351 for(
size_t pi=0; pi<p2cons.size(); pi++)
357 int i0 = con.
ndr-nFixed;
358 int i1 = con.
nd1-nFixed;
362 Matrix<double,3,3> m = con.
J0t*con.
prec*con.
J0;
363 csp.addDiagBlock(m,i0);
364 dcnt(con.
ndr - nFixed)++;
368 dcnt(con.
nd1 - nFixed)++;
369 Matrix<double,3,3> tp = con.
prec * con.
J1;
370 Matrix<double,3,3> m = con.
J1t * tp;
371 csp.addDiagBlock(m,i1);
374 Matrix<double,3,3> m2 = con.
J0t * tp;
378 csp.addOffdiagBlock(m,i1,i0);
382 csp.addOffdiagBlock(m2,i0,i1);
389 csp.B.block<3,1>(i0*3,0) -= con.
J0t * con.
prec * con.
err;
391 csp.B.block<3,1>(i1*3,0) -= con.
J1t * con.
prec * con.
err;
398 csp.incDiagBlocks(lam);
400 csp.setupCSstructure(lam,iter==0);
404 printf(
"\n[SetupSparseSys] Block: %0.1f Cons: %0.1f CS: %0.1f\n",
405 (t1-t0)*.001, (t2-t1)*.001, (t3-t2)*.001);
408 for (
int i=0; i<nFree; i++)
409 if (dcnt(i) == 0) ndc++;
412 cout <<
"[SetupSparseSys] " << ndc <<
" disconnected nodes" << endl;
425 int SysSPA2d::doSPA(
int niter,
double sLambda,
int useCSparse,
double initTol,
429 int ncams = nodes.size();
432 int ncons = p2cons.size();
437 cout <<
"[doSPA2d] No fixed frames" << endl;
440 for (
int i=0; i<ncams; i++)
458 sqMinDelta = 1e-8 * 1e-8;
459 double cost = calcCost();
461 cout << iter <<
" Initial squared cost: " << cost <<
" which is "
462 << sqrt(cost/ncons) <<
" rms error" << endl;
466 for (; iter<niter; iter++)
472 long long t0, t1, t2, t3;
475 setupSparseSys(lambda,iter,useCSparse);
485 if (csp.B.rows() != 0)
487 int iters = csp.doBPCG(maxCGiters,initTol,iter);
489 cout <<
"[Block PCG] " << iters <<
" iterations" << endl;
494 else if (useCSparse == 3)
496 int res = csp.doPCG(maxCGiters);
498 cout <<
"[DoSPA] Sparse PCG failed with error " << res << endl;
501 else if (useCSparse > 0)
503 if (csp.B.rows() != 0)
505 bool ok = csp.doChol();
507 cout <<
"[DoSBA] Sparse Cholesky failed!" << endl;
513 A.ldlt().solveInPlace(B);
519 VectorXd &BB = useCSparse ? csp.B : B;
523 double sqDiff = BB.squaredNorm();
524 if (sqDiff < sqMinDelta)
527 cout <<
"Converged with delta: " << sqrt(sqDiff) << endl;
533 for(
int i=0; i < ncams; i++)
539 nd.
trans.head<2>() += BB.segment<2>(ci);
549 double newcost = calcCost();
551 cout << iter <<
" Updated squared cost: " << newcost <<
" which is "
552 << sqrt(newcost/ncons) <<
" rms error" << endl;
568 for(
int i=0; i<ncams; i++)
580 cout << iter <<
" Downdated cost: " << cost << endl;
585 if (iter == 0 && verbose)
587 printf(
"[SPA] Setup: %0.2f ms Solve: %0.2f ms Update: %0.2f ms\n",
588 0.001*(
double)(t1-t0),
589 0.001*(
double)(t2-t1),
590 0.001*(
double)(t3-t2));
593 double dt=1e-6*(double)(t3-t0);
595 if (print_iros_stats){
596 cerr <<
"iteration= " << iter
597 <<
"\t chi2= " << cost
599 <<
"\t cumTime= " << cumTime
600 <<
"\t kurtChi2= " << cost
619 static inline int getind(std::map<int,int> &m,
int ind)
621 std::map<int,int>::iterator it;
629 int SysSPA2d::doSPAwindowed(
int window,
int niter,
double sLambda,
int useCSparse)
632 int nnodes = nodes.size();
633 if (nnodes < 2)
return 0;
635 int nlow = nnodes - window;
636 if (nlow < 1) nlow = 1;
639 cout <<
"[SPA Window] From " << nlow <<
" to " << nnodes << endl;
642 int ncons = p2cons.size();
649 std::vector<Node2d,Eigen::aligned_allocator<Node2d> > &wnodes = spa.
nodes;
650 std::vector<Con2dP2,Eigen::aligned_allocator<Con2dP2> > &wp2cons = spa.
p2cons;
651 std::map<int,int> inds;
652 std::vector<int> rinds;
655 for (
int i=0; i<ncons; i++)
658 if (con.
ndr >= nlow || con.
nd1 >= nlow)
659 wp2cons.push_back(con);
661 if (con.
ndr >= nlow && con.
nd1 < nlow)
666 inds.insert(std::pair<int,int>(con.
nd1,wnodes.size()));
667 wnodes.push_back(nodes[con.
nd1]);
669 rinds.push_back(con.
nd1);
671 else if (con.
nd1 >= nlow && con.
ndr < nlow)
676 inds.insert(std::pair<int,int>(con.
ndr,wnodes.size()));
677 wnodes.push_back(nodes[con.
ndr]);
679 rinds.push_back(con.
ndr);
683 spa.
nFixed = wnodes.size();
685 cout <<
"[SPA Window] Fixed node count: " << spa.
nFixed << endl;
688 for (
int i=0; i<(int)wp2cons.size(); i++)
691 if (con.
nd1 >= nlow && con.
ndr >= nlow)
696 inds.insert(std::pair<int,int>(con.
ndr,wnodes.size()));
697 wnodes.push_back(nodes[con.
ndr]);
698 rinds.push_back(con.
ndr);
703 inds.insert(std::pair<int,int>(con.
nd1,wnodes.size()));
704 wnodes.push_back(nodes[con.
nd1]);
705 rinds.push_back(con.
nd1);
712 cout <<
"[SPA Window] Variable node count: " << spa.
nodes.size() - spa.
nFixed << endl;
713 cout <<
"[SPA Window] Constraint count: " << spa.
p2cons.size() << endl;
717 for (
int i=0; i<(int)wp2cons.size(); i++)
725 niter = spa.
doSPA(niter,sLambda,useCSparse);
728 for (
int i=0; i<(int)wp2cons.size(); i++)
744 void SysSPA2d::setupSparseDSIF(
int newnode)
748 int nFree = nodes.size() - nFixed;
754 csp.setupBlockStructure(nFree,
false);
759 for(
size_t pi=0; pi<p2cons.size(); pi++)
764 if (con.
ndr < newnode && con.
nd1 < newnode)
770 int i0 = con.
ndr-nFixed;
771 int i1 = con.
nd1-nFixed;
776 if (i0 != i1-1) fact = 0.99;
780 Matrix<double,3,3> m = con.
J0t*con.
prec*con.
J0;
781 csp.addDiagBlock(m,i0);
785 Matrix<double,3,3> m = con.
J1t*con.
prec*con.
J1;
786 csp.addDiagBlock(m,i1);
790 Matrix<double,3,3> m2 = con.
J0t*con.
prec*con.
J1;
791 m2 = m2 * fact * fact;
795 csp.addOffdiagBlock(m,i1,i0);
799 csp.addOffdiagBlock(m2,i0,i1);
806 csp.B.block<3,1>(i0*3,0) -= con.
J0t * con.
prec * con.
err;
808 csp.B.block<3,1>(i1*3,0) -= con.
J1t * con.
prec * con.
err;
817 csp.setupCSstructure(1.0,
true);
829 void SysSPA2d::doDSIF(
int newnode)
832 int nnodes = nodes.size();
835 int ncons = p2cons.size();
840 cout <<
"[doDSIF] No fixed frames" << endl;
845 if (newnode >= nnodes)
847 cout <<
"[doDSIF] no new nodes to add" << endl;
852 for (
int i=newnode; i<nnodes; i++)
854 nodes[i].oldtrans = nodes[i].trans;
855 nodes[i].oldarot = nodes[i].arot;
859 for (
int i=0; i<nnodes; i++)
861 Node2d &nd = nodes[i];
871 double cost = calcCost();
873 cout <<
" Initial squared cost: " << cost <<
" which is "
874 << sqrt(cost/ncons) <<
" rms error" << endl;
877 long long t0, t1, t2, t3;
879 setupSparseDSIF(newnode);
882 cout <<
"[doDSIF] B = " << csp.B.transpose() << endl;
884 cout <<
"[doDSIF] A = " << endl << A << endl;
889 bool ok = csp.doChol();
891 cout <<
"[doDSIF] Sparse Cholesky failed!" << endl;
896 VectorXd &BB = csp.B;
902 for(
int i=0; i < nnodes; i++)
904 Node2d &nd = nodes[i];
905 if (nd.isFixed)
continue;
906 nd.trans.head(2) = nd.oldtrans.head(2)+BB.segment<2>(ci);
907 nd.arot = nd.oldarot+BB(ci+2);
915 double newcost = calcCost();
917 cout <<
" Updated squared cost: " << newcost <<
" which is "
918 << sqrt(newcost/ncons) <<
" rms error" << endl;
931 cout <<
"Can't open file " << fname << endl;
938 setupSparseSys(0.0,0);
942 double *Ax = csp.A->x;
944 for (
int i=0; i<csp.csize; i++)
945 for (
int j=Ap[i]; j<Ap[i+1]; j++)
947 ofs << Ai[j] <<
" " << i << setprecision(16) <<
" " << Ax[j] << endl;
951 Eigen::IOFormat pfmt(16);
953 int nrows = A.rows();
954 int ncols = A.cols();
956 for (
int i=0; i<nrows; i++)
957 for (
int j=i; j<ncols; j++)
961 ofs << i <<
" " << j << setprecision(16) <<
" " << a << endl;
972 void SysSPA2d::getGraph(std::vector<float> &graph)
974 for (
int i=0; i<(int)p2cons.size(); i++)
979 graph.push_back(nd0.
trans(0));
980 graph.push_back(nd0.
trans(1));
981 graph.push_back(nd1.
trans(0));
982 graph.push_back(nd1.
trans(1));
1002 void SysSPA2d::setupSparseDSIF(
int newnode)
1004 cout <<
"[SetupDSIF] at " << newnode << endl;
1008 int nFree = nodes.size() - nFixed;
1014 csp.setupBlockStructure(nFree,
false);
1019 for(
size_t pi=0; pi<p2cons.size(); pi++)
1021 Con2dP2 &con = p2cons[pi];
1024 if (con.ndr < newnode && con.nd1 < newnode)
1030 Vector2d dRdth = nodes[con.ndr].dRdx.transpose() * con.tmean;
1033 Matrix3d Jt = J.transpose();
1035 u.head(2) = nodes[con.ndr].trans.head(2);
1036 u(2) = nodes[con.ndr].arot;
1038 f.head(2) = u.head(2) + nodes[con.ndr].w2n.transpose().block<2,2>(0,0) * con.tmean;
1039 f(2) = u(2) + con.amean;
1040 if (
f(2) > M_PI)
f(2) -= 2.0*M_PI;
1041 if (
f(2) < M_PI)
f(2) += 2.0*M_PI;
1044 cout <<
"[SetupDSIF] u = " << u.transpose() << endl;
1045 cout <<
"[SetupDSIF] f = " <<
f.transpose() << endl;
1046 cout <<
"[SetupDSIF] fo = " << nodes[con.nd1].trans.transpose() << endl;
1050 int i0 = con.ndr-nFixed;
1051 int i1 = con.nd1-nFixed;
1053 if (i0 != i1-1)
continue;
1057 Matrix<double,3,3> m = Jt*con.prec*J;
1058 csp.addDiagBlock(m,i0);
1062 Matrix<double,3,3> m = con.prec;
1063 csp.addDiagBlock(m,i1);
1067 Matrix<double,3,3> m2 = -con.prec * J;
1073 csp.addOffdiagBlock(m,i1,i0);
1077 csp.addOffdiagBlock(m2,i0,i1);
1084 Vector3d Juf = J*u -
f;
1085 if (Juf(2) > M_PI) Juf(2) -= 2.0*M_PI;
1086 if (Juf(2) < M_PI) Juf(2) += 2.0*M_PI;
1089 csp.B.block<3,1>(i0*3,0) += Jt * con.prec * Juf;
1093 csp.B.block<3,1>(i1*3,0) -= con.prec * Juf;
1103 csp.setupCSstructure(1.0,
true);
1116 void SysSPA2d::doDSIF(
int newnode,
bool useCSparse)
1119 int nnodes = nodes.size();
1122 int ncons = p2cons.size();
1127 cout <<
"[doDSIF] No fixed frames" << endl;
1132 if (newnode >= nnodes)
1134 cout <<
"[doDSIF] no new nodes to add" << endl;
1140 for (
int i=0; i<nnodes; i++)
1142 Node2d &nd = nodes[i];
1152 double cost = calcCost();
1154 cout <<
" Initial squared cost: " << cost <<
" which is "
1155 << sqrt(cost/ncons) <<
" rms error" << endl;
1160 csp.setupBlockStructure(1,
false);
1164 csp.addDiagBlock(m,0);
1166 u.head(2) = nodes[0].trans.head(2);
1167 u(2) = nodes[0].arot;
1168 csp.B.head(3) = u*1000000;
1170 cout <<
"[doDSIF] B = " << csp.B.transpose() << endl;
1177 long long t0, t1, t2, t3;
1180 setupSparseDSIF(newnode);
1185 cout <<
"[doDSIF] B = " << csp.B.transpose() << endl;
1187 cout <<
"[doDSIF] A = " << endl << A << endl;
1194 bool ok = csp.doChol();
1196 cout <<
"[doDSIF] Sparse Cholesky failed!" << endl;
1199 A.ldlt().solveInPlace(B);
1204 VectorXd &BB = useCSparse ? csp.B : B;
1206 cout <<
"[doDSIF] RES = " << BB.transpose() << endl;
1210 for(
int i=0; i < nnodes; i++)
1212 Node2d &nd = nodes[i];
1213 if (nd.isFixed)
continue;
1214 nd.trans.head<2>() = BB.segment<2>(ci);
1223 double newcost = calcCost();
1225 cout <<
" Updated squared cost: " << newcost <<
" which is "
1226 << sqrt(newcost/ncons) <<
" rms error" << endl;
1237 void SysSPA2d::setupSparseDSIF(
int newnode)
1239 cout <<
"[SetupDSIF] at " << newnode << endl;
1243 int nFree = nodes.size() - nFixed;
1249 csp.setupBlockStructure(nFree,
false);
1254 for(
size_t pi=0; pi<p2cons.size(); pi++)
1256 Con2dP2 &con = p2cons[pi];
1259 if (con.ndr < newnode && con.nd1 < newnode)
1262 con.setJacobians(nodes);
1265 int i0 = con.ndr-nFixed;
1266 int i1 = con.nd1-nFixed;
1268 if (i0 != i1-1)
continue;
1272 Matrix<double,3,3> m = con.J0t*con.prec*con.J0;
1273 csp.addDiagBlock(m,i0);
1277 Matrix<double,3,3> m = con.J1t*con.prec*con.J1;
1278 csp.addDiagBlock(m,i1);
1282 Matrix<double,3,3> m2 = con.J0t*con.prec*con.J1;
1286 csp.addOffdiagBlock(m,i1,i0);
1290 csp.addOffdiagBlock(m2,i0,i1);
1301 u.head(2) = nodes[con.ndr].trans.head(2);
1302 u(2) = nodes[con.ndr].arot;
1303 Vector3d bm = con.err + con.J0 * u;
1304 csp.B.block<3,1>(i0*3,0) += (bm.transpose() * con.prec * con.J0).transpose();
1309 u.head(2) = nodes[con.nd1].trans.head(2);
1310 u(2) = nodes[con.nd1].arot;
1311 Vector3d bm = con.err + con.J1 * u;
1312 csp.B.block<3,1>(i1*3,0) += (bm.transpose() * con.prec * con.J1).transpose();
1322 csp.setupCSstructure(1.0,
true);
1335 void SysSPA2d::doDSIF(
int newnode,
bool useCSparse)
1338 int nnodes = nodes.size();
1341 int ncons = p2cons.size();
1346 cout <<
"[doDSIF] No fixed frames" << endl;
1351 if (newnode >= nnodes)
1353 cout <<
"[doDSIF] no new nodes to add" << endl;
1357 for (
int i=0; i<nnodes; i++)
1359 Node2d &nd = nodes[i];
1369 double cost = calcCost();
1371 cout <<
" Initial squared cost: " << cost <<
" which is "
1372 << sqrt(cost/ncons) <<
" rms error" << endl;
1378 long long t0, t1, t2, t3;
1381 setupSparseDSIF(newnode);
1386 cout <<
"[doDSIF] B = " << csp.B.transpose() << endl;
1388 cout <<
"[doDSIF] A = " << endl << A << endl;
1395 bool ok = csp.doChol();
1397 cout <<
"[doDSIF] Sparse Cholesky failed!" << endl;
1400 A.ldlt().solveInPlace(B);
1405 VectorXd &BB = useCSparse ? csp.B : B;
1407 cout <<
"[doDSIF] RES = " << BB.transpose() << endl;
1411 for(
int i=0; i < nnodes; i++)
1413 Node2d &nd = nodes[i];
1414 if (nd.isFixed)
continue;
1415 nd.trans.head<2>() = BB.segment<2>(ci);
1424 double newcost = calcCost();
1426 cout <<
" Updated squared cost: " << newcost <<
" which is "
1427 << sqrt(newcost/ncons) <<
" rms error" << endl;
1436 void SysSPA2d::removeNode(
int id)
1439 for (
int i=0; i<(int)nodes.size(); i++)
1441 if (nodes[i].nodeId == ind)
1444 if (ind < 0)
return;
1447 nodes.erase(nodes.begin() + ind);
1453 while (i <(
int)p2cons.size())
1455 std::vector<Con2dP2,Eigen::aligned_allocator<Con2dP2> >::iterator iter = p2cons.begin() + i;
1456 if (iter->ndr == ind || iter->nd1 == ind)
1462 if (iter->ndr > ind) iter->ndr--;
1463 if (iter->nd1 > ind) iter->nd1--;
1471 bool SysSPA2d::removeConstraint(
int ndi0,
int ndi1)
1473 int ni0 = -1, ni1 = -1;
1474 for (
int i=0; i<(int)nodes.size(); i++)
1476 if (nodes[i].nodeId == ndi0)
1478 if (nodes[i].nodeId == ndi1)
1481 if (ni0 < 0 || ni1 < 0)
return false;
1484 while (i <(
int)p2cons.size())
1486 std::vector<Con2dP2,Eigen::aligned_allocator<Con2dP2> >::iterator iter = p2cons.begin() + i;
1487 if (iter->ndr == ni0 && iter->nd1 == ni1)