54 #include <Eigen/Cholesky>
57 using namespace Eigen;
68 auto duration = std::chrono::system_clock::now().time_since_epoch();
69 return std::chrono::duration_cast<std::chrono::microseconds>(duration).count();
89 cout <<
"Can't open file " << fname << endl;
96 if (!getline(ifs,line) || line !=
"# P2 Constraint File")
98 cout <<
"Bad header" << endl;
101 cout <<
"Found P2 constraint file" << endl;
104 int ncams, nss, nscs, np2s;
105 if (!(ifs >> ncams >> nss >> np2s >> nscs))
107 cout <<
"Bad entity count" << endl;
110 cout <<
"Number of cameras: " << ncams
111 <<
" Number of scalers: " << nss
112 <<
" Number of p2p constraints: " << np2s
113 <<
" Number of scale constraints: " << nscs << endl;
115 cout <<
"Reading in camera data..." << flush;
116 std::vector<Node,Eigen::aligned_allocator<Node> > &nodes = spa.
nodes;
119 for (
int i=0; i<ncams; i++)
122 if (!(ifs >> v1 >> v2 >> v3))
124 cout <<
"Bad node translation params at number " << i << endl;
127 nd.
trans = Vector4d(v1,v2,v3,1.0);
129 if (!(ifs >> v1 >> v2 >> v3))
131 cout <<
"Bad node rotation quaternion at number " << i << endl;
135 nd.
qrot = Quaternion<double>(v1,v2,v2,0.0);
140 cout <<
"done" << endl;
143 cout <<
"Reading in constraint data..." << flush;
144 std::vector<ConP2,Eigen::aligned_allocator<ConP2> > &cons = spa.
p2cons;
147 for (
int i=0; i<np2s; i++)
153 if (!(ifs >> p1 >> p2))
155 cout <<
"Bad node indices at constraint " << i << endl;
159 for (
int i=0; i<6; i++)
163 cout <<
"Bad constraint mean at constraint " << i << endl;
168 for (
int i=0; i<36; i++)
172 cout <<
"Bad constraint precision at constraint " << i << endl;
181 con.
prec = Matrix<double,6,6>(vv);
188 cout <<
"Reading in scale constraint data..." << flush;
189 std::vector<ConScale,Eigen::aligned_allocator<ConScale> > &scons = spa.
scons;
192 for (
int i=0; i<nscs; i++)
197 if (!(ifs >> p1 >> p2 >> sv >> ks >> w))
199 cout <<
"Bad scale constraint at constraint " << i << endl;
209 scons.push_back(scon);
221 void ConP2::setJacobians(std::vector<
Node,Eigen::aligned_allocator<Node> > &nodes)
224 Node &nr = nodes[ndr];
225 Matrix<double,4,1> &tr = nr.
trans;
226 Quaternion<double> &qr = nr.
qrot;
227 Node &n1 = nodes[nd1];
228 Matrix<double,4,1> &t1 = n1.
trans;
229 Quaternion<double> &q1 = n1.
qrot;
232 Eigen::Matrix<double,3,1> pc = nr.
w2n * t1;
238 J0.block<3,3>(0,0) = -nr.
w2n.block<3,3>(0,0);
243 Eigen::Matrix<double,3,1> pwt;
244 pwt = (t1-tr).head(3);
247 Eigen::Matrix<double,3,1> dp = nr.
dRdx * pwt;
248 J0.block<3,1>(0,3) = dp;
251 J0.block<3,1>(0,4) = dp;
254 J0.block<3,1>(0,5) = dp;
257 J0.block<3,3>(3,0).setZero();
263 Eigen::Quaternion<double> qr0, qr1, qrn, qrd;
264 qr1.coeffs() = q1.coeffs();
265 qrn.coeffs() = Vector4d(-qpmean.w(),-qpmean.z(),qpmean.y(),qpmean.x());
266 qr0.coeffs() = Vector4d(-qr.x(),-qr.y(),-qr.z(),qr.w());
273 qrn.vec() = -qrn.vec();
276 J0.block<3,1>(3,3) = qrn.vec();
279 qrn.coeffs() = Vector4d(qpmean.z(),-qpmean.w(),-qpmean.x(),qpmean.y());
284 qrn.vec() = -qrn.vec();
287 J0.block<3,1>(3,4) = qrn.vec();
290 qrn.coeffs() = Vector4d(-qpmean.y(),qpmean.x(),-qpmean.w(),qpmean.z());
295 qrn.vec() = -qrn.vec();
298 J0.block<3,1>(3,5) = qrn.vec();
301 J0t = J0.transpose();
308 J1.block<3,3>(0,0) = nr.
w2n.block<3,3>(0,0);
311 J1.block<3,3>(0,3).setZero();
314 J1.block<3,3>(3,0).setZero();
320 Eigen::Quaternion<double> qrc;
321 qrc.coeffs() = Vector4d(-qr.x(),-qr.y(),-qr.z(),qr.w());
322 qrc = qpmean*qrc*qr1;
328 double wdq = 1.0 - dq*dq;
329 qr1.coeffs() = Vector4d(dq,0,0,wdq);
334 qrn.coeffs() = Vector4d(1,0,0,0);
341 qrn.vec() = -qrn.vec();
344 J1.block<3,1>(3,3) = qrn.vec();
347 qrn.coeffs() = Vector4d(0,1,0,0);
352 qrn.vec() = -qrn.vec();
355 J1.block<3,1>(3,4) = qrn.vec();
358 qrn.coeffs() = Vector4d(0,0,1,0);
363 qrn.vec() = -qrn.vec();
366 J1.block<3,1>(3,5) = qrn.vec();
369 J1t = J1.transpose();
380 void ConScale::setJacobians(std::vector<
Node,Eigen::aligned_allocator<Node> > &nodes)
383 Node &n0 = nodes[nd0];
384 Matrix<double,4,1> &t0 = n0.
trans;
385 Node &n1 = nodes[nd1];
386 Matrix<double,4,1> &t1 = n1.
trans;
388 Eigen::Matrix<double,3,1> td = (t1-t0).head(3);
408 void ConP3P::setJacobians(std::vector<
Node,Eigen::aligned_allocator<Node> > nodes)
411 Node nr = nodes[ndr];
412 Matrix<double,4,1> &tr = nr.
trans;
413 Quaternion<double> &qr = nr.
qrot;
414 Node n1 = nodes[nd1];
415 Matrix<double,4,1> &t1 = n1.
trans;
416 Quaternion<double> &q1 = n1.
qrot;
417 Node n2 = nodes[nd2];
418 Matrix<double,4,1> &t2 = n2.
trans;
419 Quaternion<double> &q2 = n2.
qrot;
427 J10.block<3,3>(0,0) = -nr.
w2n.block<3,3>(0,0);
431 Matrix<double,3,1> pwt = (t1-tr).head(3);
432 Matrix<double,3,1> dp = nr.
dRdx * pwt;
433 J10.block<3,1>(0,3) = dp;
435 J10.block<3,1>(0,4) = dp;
437 J10.block<3,1>(0,5) = dp;
440 J10.block<3,3>(3,0).setZero();
446 double wn = -1.0/qr.w();
447 dp[0] = wn*t1[0]*qr.x() - t1[3];
448 dp[1] = wn*t1[1]*qr.x() + t1[2];
449 dp[2] = wn*t1[2]*qr.x() - t1[1];
450 J10.block<3,1>(3,3) = dp;
453 dp[0] = wn*t1[0]*qr.y() - t1[2];
454 dp[1] = wn*t1[1]*qr.y() - t1[3];
455 dp[2] = wn*t1[2]*qr.y() + t1[0];
456 J10.block<3,1>(3,4) = dp;
459 dp[0] = wn*t1[0]*qr.z() + t1[1];
460 dp[1] = wn*t1[1]*qr.z() - t1[0];
461 dp[2] = wn*t1[2]*qr.z() - t1[3];
462 J10.block<3,1>(3,5) = dp;
469 J20.block<3,3>(0,0) = -nr.
w2n.block<3,3>(0,0);
473 pwt = (t2-tr).head(3);
475 J20.block<3,1>(0,3) = dp;
477 J20.block<3,1>(0,4) = dp;
479 J20.block<3,1>(0,5) = dp;
482 J20.block<3,3>(3,0).setZero();
489 dp[0] = wn*q2.x()*qr.x() - q2.w();
490 dp[1] = wn*q2.y()*qr.x() + q2.z();
491 dp[2] = wn*q2.z()*qr.x() - q2.y();
492 J20.block<3,1>(3,3) = dp;
495 dp[0] = wn*q2.x()*qr.y() - q2.z();
496 dp[1] = wn*q2.y()*qr.y() - q2.w();
497 dp[2] = wn*q2.z()*qr.y() + q2.x();
498 J20.block<3,1>(3,4) = dp;
501 dp[0] = wn*q2.x()*qr.z() + q2.y();
502 dp[1] = wn*q2.y()*qr.z() - q2.x();
503 dp[2] = wn*q2.z()*qr.z() - q2.w();
504 J20.block<3,1>(3,5) = dp;
512 J11.block<3,3>(0,0) = nr.
w2n.block<3,3>(0,0);
515 J11.block<3,3>(0,3).setZero();
518 J11.block<3,3>(3,0).setZero();
525 dp[0] = wn*qr.x()*q1.x() + qr.w();
526 dp[1] = wn*qr.y()*q1.x() - qr.z();
527 dp[2] = wn*qr.z()*q1.x() + qr.y();
528 J11.block<3,1>(3,3) = dp;
531 dp[0] = wn*qr.x()*q1.y() + qr.z();
532 dp[1] = wn*qr.y()*q1.y() + qr.w();
533 dp[2] = wn*qr.z()*q1.y() - qr.x();
534 J11.block<3,1>(3,4) = dp;
537 dp[0] = wn*qr.x()*q1.z() - qr.y();
538 dp[1] = wn*qr.y()*q1.z() + qr.x();
539 dp[2] = wn*qr.z()*q1.z() + qr.w();
540 J11.block<3,1>(3,5) = dp;
550 J22.block<3,3>(0,0) = nr.
w2n.block<3,3>(0,0);
553 J22.block<3,3>(0,3).setZero();
556 J22.block<3,3>(3,0).setZero();
563 dp[0] = wn*qr.x()*q2.x() + qr.w();
564 dp[1] = wn*qr.y()*q2.x() - qr.z();
565 dp[2] = wn*qr.z()*q2.x() + qr.y();
566 J22.block<3,1>(3,3) = dp;
569 dp[0] = wn*qr.x()*q2.y() + qr.z();
570 dp[1] = wn*qr.y()*q2.y() + qr.w();
571 dp[2] = wn*qr.z()*q2.y() - qr.x();
572 J22.block<3,1>(3,4) = dp;
575 dp[0] = wn*qr.x()*q2.z() - qr.y();
576 dp[1] = wn*qr.y()*q2.z() + qr.x();
577 dp[2] = wn*qr.z()*q2.z() + qr.w();
578 J22.block<3,1>(3,5) = dp;
586 inline double ConP2::calcErr(
const Node &nd0,
const Node &nd1)
588 Quaternion<double> q0p,q1;
589 q0p.vec() = -nd0.
qrot.coeffs().head(3);
590 q0p.w() = nd0.
qrot.w();
592 err.block<3,1>(0,0) = nd0.
w2n * nd1.
trans - tmean;
599 q1 = qpmean * q0p * q1;
606 err.block<3,1>(3,0) = -q1.vec();
609 err.block<3,1>(3,0) = q1.vec();
611 return err.dot(prec * err);
616 double ConP2::calcErrDist(
const Node &nd0,
const Node &nd1)
619 Quaternion<double> q0p,q1;
620 q0p.vec() = -nd0.
qrot.vec();
621 q0p.w() = nd0.
qrot.w();
624 return derr.dot(derr);
629 inline double ConScale::calcErr(
const Node &nd0,
const Node &nd1,
double alpha)
640 int SysSPA::addNode(Eigen::Matrix<double,4,1> &trans,
641 Eigen::Quaternion<double> &qrot,
653 return nodes.size()-1;
662 bool SysSPA::addConstraint(
int nd0,
int nd1,
663 Eigen::Vector3d &tmean,
664 Eigen::Quaterniond &qpmean,
665 Eigen::Matrix<double,6,6> &prec)
667 if (nd0 >= (
int)nodes.size() || nd1 >= (
int)nodes.size())
675 Quaternion<double> qr;
678 con.
qpmean = qr.inverse();
681 p2cons.push_back(con);
689 double SysSPA::calcCost(
bool tcost)
696 for(
size_t i=0; i<p2cons.size(); i++)
698 ConP2 &con = p2cons[i];
707 for(
size_t i=0; i<p2cons.size(); i++)
709 ConP2 &con = p2cons[i];
713 if (scons.size() > 0)
714 for(
size_t i=0; i<scons.size(); i++)
717 double err = con.
calcErr(nodes[con.
nd0],nodes[con.
nd1],scales[con.
sv]);
730 void SysSPA::setupSys(
double sLambda)
734 int nFree = nodes.size() - nFixed;
735 int nscales = scales.size();
736 A.setZero(6*nFree+nscales,6*nFree+nscales);
737 B.setZero(6*nFree+nscales);
738 VectorXi dcnt(nFree);
742 double lam = 1.0 + sLambda;
745 for(
size_t pi=0; pi<p2cons.size(); pi++)
747 ConP2 &con = p2cons[pi];
752 int i0 = 6*(con.
ndr-nFixed);
753 int i1 = 6*(con.
nd1-nFixed);
757 A.block<6,6>(i0,i0) += con.
J0t * con.
prec * con.
J0;
758 dcnt(con.
ndr - nFixed)++;
762 dcnt(con.
nd1 - nFixed)++;
763 Matrix<double,6,6> tp = con.
prec * con.
J1;
764 A.block<6,6>(i1,i1) += con.
J1t * tp;
767 A.block<6,6>(i0,i1) += con.
J0t * con.
prec * con.
J1;
768 A.block<6,6>(i1,i0) += con.
J1t * con.
prec * con.
J0;
774 B.block<6,1>(i0,0) -= con.
J0t * con.
prec * con.
err;
776 B.block<6,1>(i1,0) -= con.
J1t * con.
prec * con.
err;
780 if (scons.size() > 0)
781 for(
size_t pi=0; pi<scons.size(); pi++)
787 int i0 = 6*(con.
nd0-nFixed);
788 int i1 = 6*(con.
nd1-nFixed);
792 A.block<3,3>(i0,i0) += con.
w * con.
J0 * con.
J0.transpose();
796 A.block<3,3>(i1,i1) += con.
w * con.
J1 * con.
J1.transpose();
799 A.block<3,3>(i0,i1) += con.
w * con.
J0 * con.
J1.transpose();
800 A.block<3,3>(i1,i0) = A.block<3,3>(i0,i1).transpose();
806 B.block<3,1>(i0,0) -= con.
w * con.
J0 * con.
err;
808 B.block<3,1>(i1,0) -= con.
w * con.
J1 * con.
err;
811 int is = 6*nFree+con.
sv;
812 A(is,is) += con.
w * con.
ks * con.
ks;
815 A.block<3,1>(i0,is) += con.
w * con.
J0 * -con.
ks;
816 A.block<1,3>(is,i0) = A.block<3,1>(i0,is).transpose();
820 A.block<3,1>(i1,is) += con.
w * con.
J1 * -con.
ks;
821 A.block<1,3>(is,i1) = A.block<3,1>(i1,is).transpose();
825 B(is) -= con.
w * (-con.
ks) * con.
err;
834 for (
int i=0; i<6*nFree; i++)
835 for (
int j=0; j<6*nFree; j++)
836 if (std::isnan(A(i,j)) ) { printf(
"[SetupSys] NaN in A\n"); *(
int *)0x0 = 0; }
838 for (
int j=0; j<6*nFree; j++)
839 if (std::isnan(B[j]) ) { printf(
"[SetupSys] NaN in B\n"); *(
int *)0x0 = 0; }
842 for (
int i=0; i<nFree; i++)
843 if (dcnt(i) == 0) ndc++;
846 cout <<
"[SetupSys] " << ndc <<
" disconnected nodes" << endl;
852 void SysSPA::setupSparseSys(
double sLambda,
int iter,
int sparseType)
856 int nFree = nodes.size() - nFixed;
862 csp.setupBlockStructure(nFree);
864 csp.setupBlockStructure(0);
868 VectorXi dcnt(nFree);
872 double lam = 1.0 + sLambda;
875 for(
size_t pi=0; pi<p2cons.size(); pi++)
877 ConP2 &con = p2cons[pi];
881 int i0 = con.
ndr-nFixed;
882 int i1 = con.
nd1-nFixed;
886 Matrix<double,6,6> m = con.
J0t*con.
prec*con.
J0;
887 csp.addDiagBlock(m,i0);
888 dcnt(con.
ndr - nFixed)++;
892 dcnt(con.
nd1 - nFixed)++;
893 Matrix<double,6,6> tp = con.
prec * con.
J1;
894 Matrix<double,6,6> m = con.
J1t * tp;
895 csp.addDiagBlock(m,i1);
898 Matrix<double,6,6> m2 = con.
J0t * tp;
902 csp.addOffdiagBlock(m,i1,i0);
905 csp.addOffdiagBlock(m2,i0,i1);
911 csp.B.block<6,1>(i0*6,0) -= con.
J0t * con.
prec * con.
err;
913 csp.B.block<6,1>(i1*6,0) -= con.
J1t * con.
prec * con.
err;
920 csp.incDiagBlocks(lam);
922 csp.setupCSstructure(lam,iter==0);
930 for (
int i=0; i<nFree; i++)
931 if (dcnt(i) == 0) ndc++;
934 cout <<
"[SetupSparseSys] " << ndc <<
" disconnected nodes" << endl;
947 int SysSPA::doSPA(
int niter,
double sLambda,
int useCSparse,
double initTol,
951 int nFree = nodes.size() - nFixed;
954 int ncams = nodes.size();
956 int nscales = scales.size();
959 vector<double> oldscales;
960 oldscales.resize(nscales);
967 int ncons = p2cons.size();
970 for (
int i=0; i<ncams; i++)
985 sqMinDelta = 1e-8 * 1e-8;
986 double cost = calcCost();
988 cout << iter <<
" Initial squared cost: " << cost <<
" which is "
989 << sqrt(cost/ncons) <<
" rms error" << endl;
992 for (; iter<niter; iter++)
998 long long t0, t1, t2, t3;
1001 setupSparseSys(lambda,iter,useCSparse);
1008 if (csp.B.rows() != 0)
1010 int iters = csp.doBPCG(maxCGiters,initTol,iter);
1012 cout <<
"[Block PCG] " << iters <<
" iterations" << endl;
1015 else if (useCSparse > 0)
1017 bool ok = csp.doChol();
1019 cout <<
"[DoSPA] Sparse Cholesky failed!" << endl;
1022 A.ldlt().solveInPlace(B);
1025 VectorXd &BB = useCSparse ? csp.B : B;
1029 double sqDiff = BB.squaredNorm();
1030 if (sqDiff < sqMinDelta)
1037 for(
int i=0; i < ncams; i++)
1039 Node &nd = nodes[i];
1043 nd.
trans.head<3>() += BB.segment<3>(ci);
1045 Quaternion<double> qr;
1046 qr.vec() = BB.segment<3>(ci+3);
1047 qr.w() = sqrt(1.0 - qr.vec().squaredNorm());
1049 Quaternion<double> qrn,qrx;
1054 nd.
qrot.coeffs() = -qr.coeffs();
1056 nd.
qrot.coeffs() = qr.coeffs();
1066 for(
int i=0; i < nscales; i++)
1068 oldscales[i] = scales[i];
1075 double newcost = calcCost();
1077 cout << iter <<
" Updated squared cost: " << newcost <<
" which is "
1078 << sqrt(newcost/ncons) <<
" rms error" << endl;
1094 for(
int i=0; i<ncams; i++)
1096 Node &nd = nodes[i];
1106 for(
int i=0; i < nscales; i++)
1107 scales[i] = oldscales[i];
1112 cout << iter <<
" Downdated cost: " << cost << endl;
1126 ofstream ofs(fname);
1129 cout <<
"Can't open file " << fname << endl;
1136 setupSparseSys(0.0,0,useCSparse);
1140 double *Ax = csp.A->x;
1142 for (
int i=0; i<csp.csize; i++)
1143 for (
int j=Ap[i]; j<Ap[i+1]; j++)
1145 ofs << Ai[j] <<
" " << i << setprecision(16) <<
" " << Ax[j] << endl;
1149 Eigen::IOFormat pfmt(16);
1151 int nrows = A.rows();
1152 int ncols = A.cols();
1154 for (
int i=0; i<nrows; i++)
1155 for (
int j=i; j<ncols; j++)
1159 ofs << i <<
" " << j << setprecision(16) <<
" " << a << endl;
1168 void SysSPA::spanningTree(
int node)
1170 int nnodes = nodes.size();
1173 vector<vector<int> > cind;
1174 cind.resize(nnodes);
1176 for(
size_t pi=0; pi<p2cons.size(); pi++)
1178 ConP2 &con = p2cons[pi];
1181 cind[i0].push_back(i1);
1182 cind[i1].push_back(i0);
1186 VectorXd dist(nnodes);
1187 dist.setConstant(1e100);
1191 std::multimap<double,int> open;
1192 open.emplace(0.0,node);
1195 while (!open.empty())
1198 int ni = open.begin()->second;
1199 double di = open.begin()->first;
1200 open.erase(open.begin());
1201 if (di > dist[ni])
continue;
1204 Node &nd = nodes[ni];
1205 Matrix<double,3,4> n2w;
1208 vector<int> &nns = cind[ni];
1209 for (
int i=0; i<(int)nns.size(); i++)
1211 ConP2 &con = p2cons[nns[i]];
1212 double dd = con.
tmean.norm();
1217 Node &nd2 = nodes[nn];
1218 Vector3d tmean = con.
tmean;
1219 Quaterniond qpmean = con.
qpmean;
1222 qpmean = qpmean.inverse();
1223 tmean = nd.
qrot.toRotationMatrix().transpose()*nd2.
qrot.toRotationMatrix()*tmean;
1226 if (dist[nn] > di + dd)
1230 open.emplace(di+dd,nn);
1233 trans.head(3) = tmean;
1235 nd2.
trans.head(3) = n2w*trans;