42 using namespace Eigen;
50 auto duration = std::chrono::system_clock::now().time_since_epoch();
51 return std::chrono::duration_cast<std::chrono::microseconds>(duration).count();
60 #define F77_FUNC(func) func ## _
62 extern int F77_FUNC(
dpotf2)(
char *uplo,
int *n,
double *a,
int *lda,
int *info);
63 extern int F77_FUNC(
dpotrf)(
char *uplo,
int *n,
double *a,
int *lda,
int *info);
64 extern int F77_FUNC(
dpotrs)(
char *uplo,
int *n,
int *nrhs,
double *a,
int *lda,
double *b,
int *ldb,
int *info);
65 extern int F77_FUNC(
dpotri)(
char *uplo,
int *n,
double *a,
int *lda,
int *info);
79 int SysSBA::addNode(Eigen::Matrix<double,4,1> &trans,
80 Eigen::Quaternion<double> &qrot,
95 return nodes.size()-1;
102 tracks.push_back(
Track(p));
103 return tracks.size()-1;
111 bool SysSBA::addProj(
int ci,
int pi, Eigen::Vector3d &q,
bool stereo)
116 if (tracks[pi].projections.count(ci) > 0)
118 if (tracks[pi].projections[ci].kp == q)
122 tracks[pi].projections[ci] =
Proj(ci, q, stereo);
125 Eigen::Matrix3d covar;
131 tracks[pi].projections[ci].setCovariance(covar);
141 bool SysSBA::addMonoProj(
int ci,
int pi, Eigen::Vector2d &q)
143 if (tracks[pi].projections.count(ci) > 0)
145 if (tracks[pi].projections[ci].kp.head(2) == q)
149 tracks[pi].projections[ci] =
Proj(ci, q);
156 bool SysSBA::addStereoProj(
int ci,
int pi, Eigen::Vector3d &q)
158 if (tracks[pi].projections.count(ci) > 0)
160 if (tracks[pi].projections[ci].kp == q)
164 tracks[pi].projections[ci] =
Proj(ci, q,
true);
167 Eigen::Matrix3d covar;
173 tracks[pi].projections[ci].setCovariance(covar);
180 void SysSBA::setProjCovariance(
int ci,
int pi, Eigen::Matrix3d &covar)
183 tracks[pi].projections[ci].setCovariance(covar);
187 void SysSBA::addPointPlaneMatch(
int ci0,
int pi0, Eigen::Vector3d normal0,
int ci1,
int pi1, Eigen::Vector3d normal1)
189 Point pt0 = tracks[pi0].point;
190 Point pt1 = tracks[pi1].point;
195 Vector3d proj_forward;
196 proj_forward = tracks[pi1].projections[ci1].kp;
198 addStereoProj(ci1, pi0, proj_forward);
200 Proj &forward_proj = tracks[pi0].projections[ci1];
212 proj_fake = tracks[pi0].projections[ci0].kp;
213 addStereoProj(ci0, pi1, proj_fake);
214 Proj &fake_proj = tracks[pi1].projections[ci0];
220 Vector3d proj_backward;
222 proj_backward = tracks[pi0].projections[ci0].kp;
223 addStereoProj(ci0, pi1, proj_backward);
225 Proj &backward_proj = tracks[pi1].projections[ci0];
235 void SysSBA::updateNormals()
237 for(
size_t i=0; i<tracks.size(); i++)
239 ProjMap &prjs = tracks[i].projections;
240 if (prjs.size() == 0)
continue;
242 for(ProjMap::iterator itr = prjs.begin(); itr != prjs.end(); itr++)
244 Proj &prj = itr->second;
250 Quaterniond qrot = nodes[prj.
ndi].qrot;
260 Point &pt0 = tracks[i].point;
263 Vector3d w = pt0.head<3>()-plane_point;
264 Vector3d projpt = pt0.head<3>() - (w.dot(plane_normal))*plane_normal;
271 int SysSBA::countProjs()
274 for(
size_t i=0; i<tracks.size(); i++)
276 ProjMap &prjs = tracks[i].projections;
285 double SysSBA::calcCost()
288 for(
size_t i=0; i<tracks.size(); i++)
290 ProjMap &prjs = tracks[i].projections;
291 if (prjs.size() == 0)
continue;
292 for(ProjMap::iterator itr = prjs.begin(); itr != prjs.end(); itr++)
294 Proj &prj = itr->second;
296 double err = prj.
calcErr(nodes[prj.
ndi],tracks[i].point,huber);
306 double SysSBA::calcCost(
double dist)
311 for(
size_t i=0; i<tracks.size(); i++)
313 ProjMap &prjs = tracks[i].projections;
314 if (prjs.size() == 0)
continue;
315 for(ProjMap::iterator itr = prjs.begin(); itr != prjs.end(); itr++)
317 Proj &prj = itr->second;
319 double err = prj.
calcErr(nodes[prj.
ndi],tracks[i].point);
332 double SysSBA::calcRMSCost(
double dist)
338 for(
size_t i=0; i<tracks.size(); i++)
340 ProjMap &prjs = tracks[i].projections;
341 if (prjs.size() == 0)
continue;
342 for(ProjMap::iterator itr = prjs.begin(); itr != prjs.end(); itr++)
344 Proj &prj = itr->second;
346 double err = prj.
calcErr(nodes[prj.
ndi],tracks[i].point,huber);
355 return sqrt(cost/(
double)nprjs);
361 double SysSBA::calcAvgError()
366 for(
size_t i=0; i<tracks.size(); i++)
368 ProjMap &prjs = tracks[i].projections;
369 if (prjs.size() == 0)
continue;
370 for(ProjMap::iterator itr = prjs.begin(); itr != prjs.end(); itr++)
372 Proj &prj = itr->second;
374 prj.
calcErr(nodes[prj.
ndi],tracks[i].point,huber);
380 return cost/(double)nprjs;
385 int SysSBA::numBadPoints()
389 for(
size_t i=0; i<tracks.size(); i++)
391 ProjMap &prjs = tracks[i].projections;
392 if (prjs.size() == 0)
continue;
393 for(ProjMap::iterator itr = prjs.begin(); itr != prjs.end(); itr++)
395 Proj &prj = itr->second;
398 if (prj.
err[0] == 0.0 && prj.
err[1] == 0.0 && prj.
err[2] == 0.0)
413 int SysSBA::countBad(
double dist)
418 double b2 = HUBER*HUBER;
419 dist = 2*b2*(sqrt(1+dist/b2)-1);
422 for (
int i=0; i<(int)tracks.size(); i++)
424 ProjMap &prjs = tracks[i].projections;
425 if (prjs.size() == 0)
continue;
426 for(ProjMap::iterator itr = prjs.begin(); itr != prjs.end(); itr++)
428 Proj &prj = itr->second;
442 int SysSBA::removeBad(
double dist)
447 for (
int i=0; i<(int)tracks.size(); i++)
449 ProjMap &prjs = tracks[i].projections;
450 if (prjs.size() == 0)
continue;
451 for(ProjMap::iterator itr = prjs.begin(); itr != prjs.end(); itr++)
453 Proj &prj = itr->second;
468 int SysSBA::reduceTracks()
471 for (
int i=0; i<(int)tracks.size(); i++)
473 ProjMap &prjs = tracks[i].projections;
477 for(ProjMap::iterator itr = prjs.begin(); itr != prjs.end(); )
479 Proj &prj = itr->second;
501 void SysSBA::printStats()
503 int ncams = nodes.size();
504 vector<map<int,int>, Eigen::aligned_allocator<map<int,int> > > conns;
506 VectorXi dcnt(ncams);
513 int npts = tracks.size();
518 if (tracks.size() > 0)
520 for (
int i=0; i<npts; i++)
522 int s = (int)tracks[i].projections.size();
527 cout <<
"[SBAsys] Cameras: " << ncams <<
" Points: " << npts
528 <<
" Projections: " << nprjs <<
" Hessian elements: " << nhs << endl;
529 cout <<
"[SBAsys] Average " << (double)nprjs/(
double)npts
530 <<
" projections per track and " << (double)nprjs/(
double)ncams
531 <<
" projections per camera" << endl;
533 cout <<
"[SBAsys] Checking camera order in projections..." << flush;
534 for (
int i=0; i<npts; i++)
537 ProjMap &prjs = tracks[i].projections;
541 for(ProjMap::iterator itr = prjs.begin(); itr != prjs.end(); itr++)
544 Proj &prj = itr->second;
559 if (j < (
int)prjs.size()-1)
561 map<int,int> &pm = conns[cami];
562 map<int,int>::iterator it;
563 for(ProjMap::iterator itr2 = itr; itr2 != prjs.end(); itr2++)
565 Proj &prj2 = itr2->second;
566 int cami2 = prj2.
ndi;
567 map<int,int> &pm2 = conns[cami2];
577 pm2.find(cami)->second++;
593 cout <<
"ok" << endl;
596 cout <<
"[SBAsys] Number of invalid projections: " << nbad << endl;
597 cout <<
"[SBAsys] Number of null tracks: " << n0 <<
" Number of single tracks: " << n1 <<
" Number of double tracks: " << n2 << endl;
602 for (
int i=0; i<ncams; i++)
604 if (dcnt(i) == 0) dnz++;
605 if (dcnt(i) < 5) dn5++;
607 cout <<
"[SBAsys] Number of disconnected cameras: " << dnz << endl;
608 cout <<
"[SBAsys] Number of cameras with <5 projs: " << dn5 << endl;
623 for (
int i=0; i<ncams; i++)
625 int nc = conns[i].size();
629 map<int,int>::iterator it;
630 for (it = conns[i].begin(); it != conns[i].end(); it++)
632 int np = (*it).second;
634 if (np < nmin) nmin = np;
635 if (np > nmax) nmax = np;
643 cout <<
"[SBAsys] Number of camera connections: " << nconns/2 <<
" which is "
644 << (double)nconns/(
double)ncams <<
" connections per camera and "
645 << (nconns+ncams)*100.0/(ncams*ncams) <<
" percent matrix fill" << endl;
646 cout <<
"[SBAsys] Min connection points: " << nmin <<
" Max connection points: "
647 << nmax <<
" Average connection points: " << (double)ntot/(
double)nconns << endl;
648 cout <<
"[SBAsys] Total connection projections: " << ntot << endl;
649 cout <<
"[SBAsys] Connections with less than 5 points: " << nc5/2 <<
" 10 pts: " << nc10/2
650 <<
" 20 pts: " << nc20/2 <<
" 50 pts: " << nc50/2 << endl;
655 vector<map<int,int> > SysSBA::generateConns_()
657 int ncams = nodes.size();
658 vector<map<int,int> > conns;
662 connMat.resize(ncams);
663 for (
int i=0; i<ncams; i++)
665 vector<bool> &bm = connMat[i];
666 bm.assign(ncams,
false);
670 for (
int i=0; i<(int)tracks.size(); i++)
673 ProjMap &prjs = tracks[i].projections;
674 for(ProjMap::iterator itr = prjs.begin(); itr != prjs.end(); itr++)
676 Proj &prj = itr->second;
685 if (j < (
int)prjs.size()-1)
687 map<int,int> &pm = conns[cami];
688 map<int,int>::iterator it;
689 for(ProjMap::iterator itr2 = itr; itr2 != prjs.end(); itr2++)
691 Proj &prj2 = itr2->second;
692 int cami2 = prj2.
ndi;
693 map<int,int> &pm2 = conns[cami2];
703 pm2.find(cami)->second++;
716 void SysSBA::setConnMat(
int minpts)
719 int ncams = nodes.size();
721 vector<map<int,int> > conns = generateConns_();
724 multimap<int,pair<int,int> > weakcs;
725 for (
int i=0; i<ncams; i++)
727 map<int,int> cs = conns[i];
728 map<int,int>::iterator it;
729 for (it = cs.begin(); it != cs.end(); it++)
731 if (it->second < minpts && it->first > i)
732 weakcs.insert(pair<
int,pair<int,int> >(it->second, pair<int,int>(i,it->first)));
736 cout <<
"[SetConnMat] Found " << weakcs.size() <<
" connections with < "
737 << minpts <<
" points" << endl;
741 multimap<int,pair<int,int> >::iterator it;
742 for (it = weakcs.begin(); it != weakcs.end(); it++)
744 int c0 = it->second.first;
745 int c1 = it->second.second;
746 if (conns[c0].size() > 1 && conns[c1].size() > 1)
749 conns[c0].erase(conns[c0].find(c1));
750 conns[c1].erase(conns[c1].find(c0));
752 connMat[c0][c1] =
true;
753 connMat[c1][c0] =
true;
757 cout <<
"[SetConnMat] Erased " << n <<
" connections" << endl;
764 void SysSBA::setConnMatReduced(
int maxconns)
767 int ncams = nodes.size();
769 vector<map<int,int> > conns = generateConns_();
772 multimap<int,pair<int,int> > weakcs;
773 for (
int i=0; i<ncams; i++)
775 map<int,int> cs = conns[i];
776 map<int,int>::iterator it;
777 for (it = cs.begin(); it != cs.end(); it++)
780 weakcs.insert(pair<
int,pair<int,int> >(-it->second, pair<int,int>(i,it->first)));
787 found.assign(ncams,0);
788 multimap<int,pair<int,int> >::iterator it;
789 for (it = weakcs.begin(); it != weakcs.end(); it++)
791 int c0 = it->second.first;
792 int c1 = it->second.second;
793 if (found[c0] < maxconns || found[c1] < maxconns)
799 connMat[c0][c1] =
false;
800 connMat[c1][c0] =
false;
804 cout <<
"[SetConnMat] Found " << n <<
" connections in spanning tree" << endl;
811 SysSBA::tsplit(
int tri,
int len)
813 ProjMap prjs = tracks[tri].projections;
814 tracks[tri].projections.clear();
818 if ((
int)prjs.size() == len+1) len = len+1;
822 while (prjs.size() > 0 && i < len)
825 ProjMap::iterator randomitr = prjs.begin();
826 std::advance( randomitr, rand() % prjs.size());
827 Proj &prj = randomitr->second;
831 prjs.erase(randomitr);
836 int pti = tracks.size();
837 while (prjs.size() > 0)
840 if ((
int)prjs.size() == len+1) len = len+1;
841 while (prjs.size() > 0 && i < len)
844 ProjMap::iterator randomitr = prjs.begin();
845 std::advance( randomitr, rand() % prjs.size());
846 Proj &prj = randomitr->second;
851 prjs.erase(randomitr);
854 tracks[pti].point = tracks[tri].point;
863 int SysSBA::reduceLongTracks(
double len)
867 int npts = tracks.size();
869 #if 1 // this algorithm splits tracks
874 for (
int i=0; i<npts; i++)
876 if ((
int)tracks[i].projections.size() > ilen)
880 int ts = tracks[i].projections.size()+1;
888 #else // this throws them out
892 multimap<int,int> ordps;
895 for (
int i=0; i<npts; i++)
896 if ((
int)tracks[i].size() > ilen)
897 ordps.insert(pair<int,int>(-(
int)tracks[i].size(), i));
901 int nn = ordps.size();
902 map<int,int>::iterator it = ordps.begin();
904 for (
int i=0; i<nn; i++, it++)
905 rem.push_back(it->second);
906 sort(rem.begin(),rem.end());
907 cout <<
"Finished finding " << rem.size() <<
" tracks" << endl;
909 std::vector<Point, Eigen::aligned_allocator<Point> > pts;
910 std::vector< std::vector<MonoProj, Eigen::aligned_allocator<MonoProj> >,
911 Eigen::aligned_allocator<std::vector<MonoProj, Eigen::aligned_allocator<MonoProj> > > > trs;
916 for (
int i=0; i<npts; i++)
918 if (rem.size()>n && i == rem[n])
923 pts.push_back(points[i]);
924 vector<MonoProj, Eigen::aligned_allocator<MonoProj> > &prjs = tracks[i];
927 trs.push_back(tracks[i]);
932 points.resize(pts.size());
933 tracks.resize(pts.size());
946 int SysSBA::remExcessTracks(
int minpts)
950 int npts = tracks.size();
952 vector<map<int,int> > conns = generateConns_();
955 multimap<int,int> ordtrs;
956 for (
int i=0; i<npts; i++)
957 ordtrs.insert(pair<int,int>((
int)tracks[i].projections.size(),i));
961 multimap<int,int>::reverse_iterator it;
962 for (it = ordtrs.rbegin(); it != ordtrs.rend(); it++)
964 int tri = it->second;
965 ProjMap &prjs = tracks[tri].projections;
967 for(ProjMap::iterator itr = prjs.begin(); itr != prjs.end(); itr++)
969 Proj &prj = itr->second;
972 for(ProjMap::iterator itr2 = itr; itr2 != prjs.end(); itr2++)
974 Proj &prj2 = itr2->second;
976 if (conns[c0].find(c1)->second <= minpts)
987 for(ProjMap::iterator itr = prjs.begin(); itr != prjs.end(); itr++)
989 Proj &prj = itr->second;
991 for(ProjMap::iterator itr2 = itr; itr2 != prjs.end(); itr2++)
993 Proj &prj2 = itr2->second;
995 conns[c0].find(c1)->second--;
996 conns[c1].find(c0)->second--;
999 remtrs.push_back(tri);
1004 for (
int i=0; i<(int)remtrs.size(); i++)
1006 int s = tracks[remtrs[i]].projections.size();
1010 cout <<
"[RemExcessTracks] Can erase " << remtrs.size() <<
" tracks with " << hms <<
" H entries" << endl;
1012 std::sort(remtrs.begin(),remtrs.end());
1014 std::vector<Track, Eigen::aligned_allocator<Track> > trs;
1019 for (
int i=0; i<npts; i++)
1021 if ((
int)remtrs.size()>n && i == remtrs[n])
1029 trs.push_back(tracks[i]);
1033 cout <<
"[RemExcessTracks] Erased " << n <<
" tracks" << endl;
1036 tracks.resize(trs.size());
1039 return remtrs.size();
1047 void SysSBA::setupSys(
double sLambda)
1050 int nFree = nodes.size() - nFixed;
1051 A.setZero(6*nFree,6*nFree);
1053 VectorXi dcnt(nFree);
1054 dcnt.setZero(nFree);
1057 double lam = 1.0 + sLambda;
1060 for(
size_t pi=0; pi<tracks.size(); pi++)
1062 ProjMap &prjs = tracks[pi].projections;
1063 if (prjs.size() < 1)
continue;
1066 if (prjs.size() > jps.size())
1067 jps.resize(prjs.size());
1078 for(ProjMap::iterator itr = prjs.begin(); itr != prjs.end(); itr++, ii++)
1080 Proj &prj = itr->second;
1082 int ci = (prj.
ndi - nFixed) * 6;
1088 if (!nodes[prj.
ndi].isFixed)
1090 dcnt(prj.
ndi - nFixed)++;
1092 A.block<6,6>(ci,ci) += prj.
jp->
Hcc;
1093 B.block<6,1>(ci,0) -= prj.
jp->
JcTE;
1100 Hpp.diagonal() *= lam;
1101 Matrix3d Hppi = Hpp.inverse();
1102 Vector3d &tp = tps[pi];
1106 for(ProjMap::iterator itr = prjs.begin(); itr != prjs.end(); itr++)
1108 Proj &prj = itr->second;
1110 if (nodes[prj.
ndi].isFixed)
continue;
1111 int ci = (prj.
ndi - nFixed) * 6;
1113 B.block<6,1>(ci,0) -= prj.
jp->
Hpc.transpose() * tp;
1114 prj.
Tpc = prj.
jp->
Hpc.transpose() * Hppi;
1117 for(ProjMap::iterator itr2 = itr; itr2 != prjs.end(); itr2++)
1119 Proj &prj2 = itr2->second;
1121 if (nodes[prj2.
ndi].isFixed)
continue;
1122 int ci2 = (prj2.
ndi - nFixed) * 6;
1125 A.block<6,6>(ci,ci2) -= prj.
Tpc * prj2.
jp->
Hpc;
1128 A.block<6,6>(ci2,ci) = A.block<6,6>(ci,ci2).transpose();
1135 A.diagonal() *= lam;
1138 for (
int i=0; i<6*nFree; i++)
1139 for (
int j=0; j<6*nFree; j++)
1140 if (std::isnan(A(i,j)) ) { printf(
"[SetupSys] NaN in A\n"); *(
int *)0x0 = 0; }
1142 for (
int j=0; j<6*nFree; j++)
1143 if (std::isnan(B[j]) ) { printf(
"[SetupSys] NaN in B\n"); *(
int *)0x0 = 0; }
1146 for (
int i=0; i<nFree; i++)
1147 if (dcnt(i) == 0) ndc++;
1150 cout <<
"[SetupSys] " << ndc <<
" disconnected nodes" << endl;
1161 void SysSBA::setupSparseSys(
double sLambda,
int iter,
int sparseType)
1164 int nFree = nodes.size() - nFixed;
1165 if (nFree < 0) nFree = 0;
1167 long long t0, t1, t2, t3;
1170 csp.setupBlockStructure(nFree);
1172 csp.setupBlockStructure(0);
1176 VectorXi dcnt(nFree);
1177 dcnt.setZero(nFree);
1180 double lam = 1.0 + sLambda;
1183 bool useConnMat = connMat.size() > 0;
1187 for(
size_t pi=0; pi<tracks.size(); pi++)
1189 ProjMap &prjs = tracks[pi].projections;
1190 if (prjs.size() < 1)
continue;
1193 if (prjs.size() > jps.size())
1194 jps.resize(prjs.size());
1205 for(ProjMap::iterator itr = prjs.begin(); itr != prjs.end(); itr++, ii++)
1207 Proj &prj = itr->second;
1209 int ni = prj.
ndi - nFixed;
1216 if (!nodes[prj.
ndi].isFixed)
1218 dcnt(prj.
ndi - nFixed)++;
1221 csp.addDiagBlock(prj.
jp->
Hcc,ni);
1222 csp.B.block<6,1>(ci,0) -= prj.
jp->
JcTE;
1229 Hpp.diagonal() *= lam;
1230 Matrix3d Hppi = Hpp.inverse();
1231 Vector3d &tp = tps[pi];
1235 for(ProjMap::iterator itr = prjs.begin(); itr != prjs.end(); itr++)
1237 Proj &prj = itr->second;
1239 if (nodes[prj.
ndi].isFixed)
continue;
1240 int ni = prj.
ndi - nFixed;
1243 csp.B.block<6,1>(ci,0) -= prj.
jp->
Hpc.transpose() * tp;
1244 prj.
Tpc = prj.
jp->
Hpc.transpose() * Hppi;
1248 for(ProjMap::iterator itr2 = itr; itr2 != prjs.end(); itr2++)
1250 Proj &prj2 = itr2->second;
1252 if (nodes[prj2.
ndi].isFixed)
continue;
1253 int ni2 = prj2.
ndi - nFixed;
1254 if (useConnMat && connMat[prj.
ndi][prj2.
ndi])
1259 Matrix<double,6,6> m = -prj.
Tpc * prj2.
jp->
Hpc;
1261 csp.addDiagBlock(m,ni);
1263 csp.addOffdiagBlock(m,ni,ni2);
1267 Matrix<double,6,6> m = -prj.
Tpc * prj.
jp->
Hpc;
1268 csp.addDiagBlock(m,ni);
1281 csp.incDiagBlocks(lam);
1283 csp.setupCSstructure(lam,iter==0);
1288 printf(
"\n[SetupSparseSys] Block: %0.1f Cons: %0.1f CS: %0.1f\n",
1289 (t1-t0)*.001, (t2-t1)*.001, (t3-t2)*.001);
1292 for (
int i=0; i<nFree; i++)
1293 if (dcnt(i) == 0) ndc++;
1296 cout <<
"[SetupSys] " << ndc <<
" disconnected nodes" << endl;
1308 int SysSBA::doSBA(
int niter,
double sLambda,
int useCSparse,
double initTol,
int maxCGiter)
1312 oldpoints.resize(tracks.size());
1316 int npts = tracks.size();
1317 int ncams = nodes.size();
1322 for(
size_t i=0; i<tracks.size(); i++)
1324 oldpoints[i] = tracks[i].point;
1325 nprjs += tracks[i].projections.size();
1328 if (nprjs == 0 || npts == 0 || ncams == 0)
1340 cout <<
"[doSBA] No fixed frames" << endl;
1343 for (
int i=0; i<ncams; i++)
1345 Node &nd = nodes[i];
1352 nd.
setDr(useLocalAngles);
1356 double laminc = 2.0;
1357 double lamdec = 0.5;
1359 sqMinDelta = 1e-8 * 1e-8;
1361 double cost = calcCost();
1365 cout << iter <<
" Initial squared cost: " << cost <<
" which is "
1366 << sqrt(cost/nprjs) <<
" rms pixel error and "
1367 << calcAvgError() <<
" average reproj error; "
1368 << numBadPoints() <<
" bad points" << endl;
1371 for (; iter<niter; iter++)
1382 setupSparseSys(lambda,iter,useCSparse);
1393 sprintf(fn,
"A%d.txt",xs);
1394 printf(
"Writing file %s\n",fn);
1395 FILE *fd = fopen(fn,
"w");
1396 fprintf(fd,
"%d %d\n",xs,xs);
1397 for (
int ii=0; ii<xs; ii++)
1398 for (
int jj=0; jj<xs; jj++)
1399 fprintf(fd,
"%.16g\n",A(ii,jj));
1402 sprintf(fn,
"B%d.txt",xs);
1404 fprintf(fd,
"%d\n",xs);
1405 for (
int ii=0; ii<xs; ii++)
1406 fprintf(fd,
"%.16g\n",B(ii));
1415 if (csp.B.rows() != 0)
1417 int iters = csp.doBPCG(maxCGiter,initTol,iter);
1418 cout <<
"[Block PCG] " << iters <<
" iterations" << endl;
1421 else if (useCSparse > 0)
1423 if (csp.B.rows() != 0)
1425 bool ok = csp.doChol();
1427 cout <<
"[DoSBA] Sparse Cholesky failed!" << endl;
1433 A.llt().solveInPlace(B);
1435 printf(
"\nDoing dpotrf/dpotrs\n");
1436 double *a = A.data();
1437 int info, m = B.size();
1438 double *x = B.data();
1441 F77_FUNC(
dpotrs)(
"U", (
int *)&m, (
int *)&nrhs, a, (
int *)&m, x, (
int *)&m, &info);
1450 VectorXd &BB = useCSparse ? csp.B : B;
1454 double sqDiff = BB.squaredNorm();
1455 if (sqDiff < sqMinDelta)
1458 cout <<
"Converged with delta: " << sqrt(sqDiff) << endl;
1464 for(vector<
Node, Eigen::aligned_allocator<Node> >::iterator itr = nodes.begin(); itr != nodes.end(); itr++)
1470 nd.
trans.head<3>() += BB.segment<3>(ci);
1474 Quaternion<double> qr;
1475 qr.vec() = BB.segment<3>(ci+3);
1479 qr.w() = sqrt(1.0 - qr.vec().squaredNorm());
1486 nd.
qrot.coeffs().head<3>() += BB.segment<3>(ci+3);
1492 nd.
setDr(useLocalAngles);
1499 for(vector<
Track, Eigen::aligned_allocator<Track> >::iterator itr = tracks.begin();
1500 itr != tracks.end(); itr++, pi++)
1502 ProjMap &prjs = itr->projections;
1503 if (prjs.size() < 1)
continue;
1504 Vector3d tp = tps[pi];
1506 for(ProjMap::iterator pitr = prjs.begin(); pitr != prjs.end(); pitr++)
1508 Proj &prj = pitr->second;
1510 if (nodes[prj.
ndi].isFixed)
continue;
1511 int ci = (prj.
ndi - nFixed) * 6;
1513 tp -= prj.
Tpc.transpose() * BB.segment<6>(ci);
1516 oldpoints[pi] = tracks[pi].point;
1517 tracks[pi].point.head(3) += tp;
1524 double newcost = calcCost();
1529 cout << iter <<
" Updated squared cost: " << newcost <<
" which is "
1530 << sqrt(newcost/(
double)nprjs) <<
" rms pixel error and "
1531 << calcAvgError() <<
" average reproj error; "
1532 << numBadPoints() <<
" bad points" << endl;
1547 for(
int i=0; i < (int)tracks.size(); i++)
1549 tracks[i].point = oldpoints[i];
1552 for(
int i=0; i < (int)nodes.size(); i++)
1554 Node &nd = nodes[i];
1560 nd.
setDr(useLocalAngles);
1566 cout << iter <<
" Downdated cost: " << cost << endl;
1571 if (iter == 0 && verbose > 0)
1572 printf(
"\n[SBA] Cost: %0.2f ms Setup: %0.2f ms Solve: %0.2f ms Update: %0.2f ms Total: %0.2f ms\n\n",
1573 0.001*(
double)(t4-t3),
1574 0.001*(
double)(t1-t0),
1575 0.001*(
double)(t2-t1),
1576 0.001*(
double)(t3-t2),
1577 0.001*(
double)(t4-t0));
1588 void SysSBA::mergeTracks(std::vector<std::pair<int,int> > &prs)
1591 int ntrs = tracks.size();
1595 for (
int i=0; i<ntrs; i++)
1599 for (
int i=0; i<(int)prs.size(); i++)
1601 pair<int,int> &pr = prs[i];
1602 int p0 = min(pr.first,pr.second);
1603 int p1 = max(pr.first,pr.second);
1608 for (
int i=0; i<ntrs; i++)
1609 tris[i] = tris[tris[i]];
1612 if (tracks.size() > 0)
1614 for (
int i=0; i<(int)tracks.size(); i++)
1616 if (tris[i] == i)
continue;
1618 ProjMap &tr0 = tracks[tris[i]].projections;
1619 ProjMap &tr1 = tracks[i].projections;
1621 for(ProjMap::iterator itr1 = tr1.begin(); itr1 != tr1.end(); itr1++)
1623 Proj &prj = itr1->second;
1634 for (
int i=0; i<(int)tracks.size(); i++)
1636 if (tris[i] != i)
continue;
1637 if (n == i)
continue;
1638 tracks[n] = tracks[i];
1639 tracks[n].point = tracks[i].point;
1656 int SysSBA::mergeTracksSt(
int tri0,
int tri1)
1659 ProjMap tr0 = tracks[tri0].projections;
1660 ProjMap &tr1 = tracks[tri1].projections;
1662 for(ProjMap::iterator itr = tr1.begin(); itr != tr1.end(); itr++)
1664 Proj &prj = itr->second;
1668 tracks[tri0].projections = tr0;