sba.cpp
Go to the documentation of this file.
1  /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2009, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 //
36 // Visual Odometry classes and functions
37 //
38 
40 #include <chrono>
41 
42 using namespace Eigen;
43 using namespace std;
44 
45 //#define DEBUG
46 
47 // elapsed time in microseconds
48 static long long utime()
49 {
50  auto duration = std::chrono::system_clock::now().time_since_epoch();
51  return std::chrono::duration_cast<std::chrono::microseconds>(duration).count();
52 }
53 
54 
55 // some LAPACK Cholesky routines
56 #ifdef __cplusplus
57 extern "C" {
58 #endif
59 
60 #define F77_FUNC(func) func ## _
61 /* Cholesky decomposition, linear system solution and matrix inversion */
62 extern int F77_FUNC(dpotf2)(char *uplo, int *n, double *a, int *lda, int *info); /* unblocked Cholesky */
63 extern int F77_FUNC(dpotrf)(char *uplo, int *n, double *a, int *lda, int *info); /* block version of dpotf2 */
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);
66 
67 #ifdef __cplusplus
68 }
69 #endif
70 
71 
72 
73 namespace sba
74 {
75  // SBA system functions
76 
77  // Adds a node to the system.
78  // \return the index of the node added.
79  int SysSBA::addNode(Eigen::Matrix<double,4,1> &trans,
80  Eigen::Quaternion<double> &qrot,
81  const fc::CamParams &cp,
82  bool isFixed)
83  {
84  Node nd;
85  nd.trans = trans;
86  nd.qrot = qrot;
87  nd.isFixed = isFixed;
88  nd.setKcam(cp); // set up node2image projection
89  nd.setTransform(); // set up world2node transform
90  nd.setDr(true); // set rotational derivatives
91  nd.setProjection();
92  // Should this be local or global?
93  nd.normRot();//Local();
94  nodes.push_back(nd);
95  return nodes.size()-1;
96  }
97 
98  // Adds a point to the system.
99  // \return the index of the point added.
100  int SysSBA::addPoint(Point p)
101  {
102  tracks.push_back(Track(p));
103  return tracks.size()-1;
104  }
105 
106 
107  // Add a projection between point and camera, in setting up the system;
108  // <ci> is camera/node index, <pi> point index, <q> is image coordinates.
109  // Stereo is whether the point is stereo or not (true is stereo, false is
110  // monocular).
111  bool SysSBA::addProj(int ci, int pi, Eigen::Vector3d &q, bool stereo)
112  {
113  // NOTE: should check bounds on ci, pi, and prjs
114  // get track
115 
116  if (tracks[pi].projections.count(ci) > 0)
117  {
118  if (tracks[pi].projections[ci].kp == q)
119  return true;
120  return false;
121  }
122  tracks[pi].projections[ci] = Proj(ci, q, stereo);
123 
124 #if 0
125  Eigen::Matrix3d covar;
127  covar.setIdentity();
128  covar(0,0) = 0.4;
129  covar(1,1) = 0.4;
130  covar(2,2) = 4.0;
131  tracks[pi].projections[ci].setCovariance(covar);
132 #endif
133 
134  return true;
135  }
136 
137  // Add a projection between point and camera, in setting up the system;
138  // <ci> is camera/node index, <pi> point index, <q> is image coordinates.
139  // This function explicitly sets up a monocular projection. If creating
140  // projections from existing projections, use addProj().
141  bool SysSBA::addMonoProj(int ci, int pi, Eigen::Vector2d &q)
142  {
143  if (tracks[pi].projections.count(ci) > 0)
144  {
145  if (tracks[pi].projections[ci].kp.head(2) == q)
146  return true;
147  return false;
148  }
149  tracks[pi].projections[ci] = Proj(ci, q);
150  return true;
151  }
152 
153  // Add a projection between point and camera, in setting up the system;
154  // <ci> is camera/node index, <pi> point index, <q> is image coordinates.
155  // This function explicitly sets up a stereo projection.
156  bool SysSBA::addStereoProj(int ci, int pi, Eigen::Vector3d &q)
157  {
158  if (tracks[pi].projections.count(ci) > 0)
159  {
160  if (tracks[pi].projections[ci].kp == q)
161  return true;
162  return false;
163  }
164  tracks[pi].projections[ci] = Proj(ci, q, true);
165 
166 #if 0
167  Eigen::Matrix3d covar;
169  covar.setIdentity();
170  covar(0,0) = 0.4;
171  covar(1,1) = 0.4;
172  covar(2,2) = 4.0;
173  tracks[pi].projections[ci].setCovariance(covar);
174 #endif
175 
176  return true;
177  }
178 
179  // Sets the covariance matrix of a projection.
180  void SysSBA::setProjCovariance(int ci, int pi, Eigen::Matrix3d &covar)
181  {
182  // TODO Check if the projection exists instead.
183  tracks[pi].projections[ci].setCovariance(covar);
184  }
185 
186  // Add a point-plane match, forward and backward.
187  void SysSBA::addPointPlaneMatch(int ci0, int pi0, Eigen::Vector3d normal0, int ci1, int pi1, Eigen::Vector3d normal1)
188  {
189  Point pt0 = tracks[pi0].point;
190  Point pt1 = tracks[pi1].point;
191 
192  // works best with single constraint
193 #if 1
194  // Forward: point 0 into camera 1.
195  Vector3d proj_forward;
196  proj_forward = tracks[pi1].projections[ci1].kp;
197  //nodes[ci1].projectStereo(pt0, proj_forward);
198  addStereoProj(ci1, pi0, proj_forward);
199 
200  Proj &forward_proj = tracks[pi0].projections[ci1];
201  forward_proj.pointPlane = true;
202  forward_proj.plane_point = pt1.head<3>();
203  forward_proj.plane_local_normal = normal1;
204  forward_proj.plane_point_index = pi1;
205  forward_proj.plane_node_index = ci0;
206 #endif
207 
208 #if 0
209  // Peter: to avoid removeFrame() removing pi1 because it only has a single projection (according to projections of pi1)
210  // Note that if we do point to plane projections both directions this should not be done
211  Vector3d proj_fake;
212  proj_fake = tracks[pi0].projections[ci0].kp;
213  addStereoProj(ci0, pi1, proj_fake);
214  Proj &fake_proj = tracks[pi1].projections[ci0];
215  fake_proj.isValid = false;
216 #endif
217 
218 #if 0
219  // Backward: point 1 into camera 0.
220  Vector3d proj_backward;
221  //nodes[ci0].projectStereo(pt1, proj_backward);
222  proj_backward = tracks[pi0].projections[ci0].kp;
223  addStereoProj(ci0, pi1, proj_backward);
224 
225  Proj &backward_proj = tracks[pi1].projections[ci0];
226  backward_proj.pointPlane = true;
227  backward_proj.plane_point = pt0.head<3>();
228  backward_proj.plane_local_normal = normal0;
229  backward_proj.plane_point_index = pi0;
230  backward_proj.plane_node_index = ci1;
231 #endif
232  }
233 
234  // Update the normals for point-plane matches.
235  void SysSBA::updateNormals()
236  {
237  for(size_t i=0; i<tracks.size(); i++)
238  {
239  ProjMap &prjs = tracks[i].projections;
240  if (prjs.size() == 0) continue;
241 
242  for(ProjMap::iterator itr = prjs.begin(); itr != prjs.end(); itr++)
243  {
244  Proj &prj = itr->second;
245  if (!prj.pointPlane || !prj.isValid) continue;
246 
247  prj.plane_point = tracks[prj.plane_point_index].point.head<3>();
248 
249  // Rotation between nodes into the projection's image plane
250  Quaterniond qrot = nodes[prj.ndi].qrot;
251  // Vector4d trans;
252 
253  // transformN2N(trans, qrot, nodes[prj.plane_node_index], nodes[itr->first]);
254 
255  prj.plane_normal = qrot.toRotationMatrix() * prj.plane_local_normal;
256  //printf("Global normal: %f %f %f\n", prj.plane_normal.x(), prj.plane_normal.y(), prj.plane_normal.z());
257 
258  // Update projections
259  //nodes[prj.ndi].projectStereo(tracks[prj.plane_point_index].point, prj.kp);
260  Point &pt0 = tracks[i].point;
261  Vector3d &plane_point = prj.plane_point;
262  Vector3d &plane_normal = prj.plane_normal;
263  Vector3d w = pt0.head<3>()-plane_point;
264  Vector3d projpt = pt0.head<3>() - (w.dot(plane_normal))*plane_normal;
265 
266  }
267  }
268  }
269 
270  // help function
271  int SysSBA::countProjs()
272  {
273  int tot = 0;
274  for(size_t i=0; i<tracks.size(); i++)
275  {
276  ProjMap &prjs = tracks[i].projections;
277  tot += prjs.size();
278  }
279  return tot;
280  }
281 
282 
283  // error measure, squared
284  // assumes node projection matrices have already been calculated
285  double SysSBA::calcCost()
286  {
287  double cost = 0.0;
288  for(size_t i=0; i<tracks.size(); i++)
289  {
290  ProjMap &prjs = tracks[i].projections;
291  if (prjs.size() == 0) continue;
292  for(ProjMap::iterator itr = prjs.begin(); itr != prjs.end(); itr++)
293  {
294  Proj &prj = itr->second;
295  if (!prj.isValid) continue;
296  double err = prj.calcErr(nodes[prj.ndi],tracks[i].point,huber);
297  cost += err;
298  }
299  }
300  return cost;
301  }
302 
303  // error measure, squared
304  // assumes node projection matrices have already been calculated
305  // doesn't count projections with errors higher than <dist>
306  double SysSBA::calcCost(double dist)
307  {
308  double cost = 0.0;
309  dist = dist*dist; // square it
310 
311  for(size_t i=0; i<tracks.size(); i++)
312  {
313  ProjMap &prjs = tracks[i].projections;
314  if (prjs.size() == 0) continue;
315  for(ProjMap::iterator itr = prjs.begin(); itr != prjs.end(); itr++)
316  {
317  Proj &prj = itr->second;
318  if (!prj.isValid) continue;
319  double err = prj.calcErr(nodes[prj.ndi],tracks[i].point);
320  if (err < dist)
321  cost += err;
322  }
323  }
324 
325  return cost;
326  }
327 
328 
329  // error measure, RMS
330  // assumes node projection matrices have already been calculated
331  // doesn't count projections with errors higher than <dist>
332  double SysSBA::calcRMSCost(double dist)
333  {
334  double cost = 0.0;
335  dist = dist*dist; // square it
336  int nprjs = 0;
337 
338  for(size_t i=0; i<tracks.size(); i++)
339  {
340  ProjMap &prjs = tracks[i].projections;
341  if (prjs.size() == 0) continue;
342  for(ProjMap::iterator itr = prjs.begin(); itr != prjs.end(); itr++)
343  {
344  Proj &prj = itr->second;
345  if (!prj.isValid) continue;
346  double err = prj.calcErr(nodes[prj.ndi],tracks[i].point,huber);
347  if (err < dist)
348  {
349  cost += err;
350  nprjs++;
351  }
352  }
353  }
354 
355  return sqrt(cost/(double)nprjs);
356  }
357 
358 
359  // Average reprojection error
360  // assumes projection errors already calculated
361  double SysSBA::calcAvgError()
362  {
363  double cost = 0.0;
364  int nprjs = 0;
365 
366  for(size_t i=0; i<tracks.size(); i++)
367  {
368  ProjMap &prjs = tracks[i].projections;
369  if (prjs.size() == 0) continue;
370  for(ProjMap::iterator itr = prjs.begin(); itr != prjs.end(); itr++)
371  {
372  Proj &prj = itr->second;
373  if (!prj.isValid) continue;
374  prj.calcErr(nodes[prj.ndi],tracks[i].point,huber);
375  cost += prj.getErrNorm();
376  nprjs++;
377  }
378  }
379 
380  return cost/(double)nprjs;
381  }
382 
383 
384  // number of points that have negative Z
385  int SysSBA::numBadPoints()
386  {
387  int count = 0;
388 
389  for(size_t i=0; i<tracks.size(); i++)
390  {
391  ProjMap &prjs = tracks[i].projections;
392  if (prjs.size() == 0) continue;
393  for(ProjMap::iterator itr = prjs.begin(); itr != prjs.end(); itr++)
394  {
395  Proj &prj = itr->second;
396  if (!prj.isValid) continue;
397  prj.calcErr(nodes[prj.ndi],tracks[i].point);
398  if (prj.err[0] == 0.0 && prj.err[1] == 0.0 && prj.err[2] == 0.0)
399  count++;
400  }
401 
402  /*if (tracks[i].point.z() < 0)
403  count++;*/
404  }
405 
406  return count;
407  }
408 
409 
410  //
411  // find the number of outlying projections
412  //
413  int SysSBA::countBad(double dist)
414  {
415  dist = dist*dist; // square it
416 #ifdef HUBER
417  // convert to Huber distance
418  double b2 = HUBER*HUBER;
419  dist = 2*b2*(sqrt(1+dist/b2)-1);
420 #endif
421  int n=0;
422  for (int i=0; i<(int)tracks.size(); i++)
423  {
424  ProjMap &prjs = tracks[i].projections;
425  if (prjs.size() == 0) continue;
426  for(ProjMap::iterator itr = prjs.begin(); itr != prjs.end(); itr++)
427  {
428  Proj &prj = itr->second;
429  if (!prj.isValid) continue;
430  if (prj.getErrSquaredNorm() >= dist)
431  n++;
432  }
433  }
434 
435  return n;
436  }
437 
438 
439  //
440  // remove outlying projections
441  //
442  int SysSBA::removeBad(double dist)
443  {
444  dist = dist*dist; // square it
445  int nbad = 0;
446 
447  for (int i=0; i<(int)tracks.size(); i++)
448  {
449  ProjMap &prjs = tracks[i].projections;
450  if (prjs.size() == 0) continue;
451  for(ProjMap::iterator itr = prjs.begin(); itr != prjs.end(); itr++)
452  {
453  Proj &prj = itr->second;
454  if (!prj.isValid) continue; // This line was added. Do we care?
455  if (prj.getErrSquaredNorm() >= dist)
456  {
457  prj.isValid = false;
458  nbad++;
459  }
460  }
461  }
462  return nbad;
463  }
464 
465  //
466  // reduce tracks by eliminating bad projections and single tracks
467  //
468  int SysSBA::reduceTracks()
469  {
470  int ret = 0;
471  for (int i=0; i<(int)tracks.size(); i++)
472  {
473  ProjMap &prjs = tracks[i].projections;
474  int ngood = 0;
475  // increment is IN for loop, since after erasing an element, the old iterator is invalid
476  // => incrementing from old iterator results in undefined behgaviour!
477  for(ProjMap::iterator itr = prjs.begin(); itr != prjs.end(); /*no increment here!!!*/)
478  {
479  Proj &prj = itr->second;
480  if (prj.isValid)
481  {
482  ngood++;
483  ++itr;
484  }
485  else
486  {
487  prjs.erase(itr++); // Erase bad projections
488  }
489  }
490  // Clear out tracks with too few good projections.
491  if (ngood < 2)
492  {
493  prjs.clear();
494  ret++;
495  }
496  }
497  return ret; // Returns the number of tracks cleared.
498  }
499 
500  // print some stats about the system
501  void SysSBA::printStats()
502  {
503  int ncams = nodes.size();
504  vector<map<int,int>, Eigen::aligned_allocator<map<int,int> > > conns; // connections between cameras - key is camera, val is count
505  conns.resize(ncams);
506  VectorXi dcnt(ncams);
507  dcnt.setZero(ncams);
508 
509  int nbad = 0;
510  int n2 = 0;
511  int n1 = 0;
512  int n0 = 0;
513  int npts = tracks.size();
514 
515  int nprjs = 0;
516  int nhs = 0; // number of Hessian elements
517 
518  if (tracks.size() > 0) // have a stereo system, maybe
519  {
520  for (int i=0; i<npts; i++)
521  {
522  int s = (int)tracks[i].projections.size();
523  nprjs += s;
524  nhs += s*(s+1)/2;
525  }
526 
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;
532 
533  cout << "[SBAsys] Checking camera order in projections..." << flush;
534  for (int i=0; i<npts; i++)
535  {
536  // track
537  ProjMap &prjs = tracks[i].projections;
538  int n = 0;
539  int j = 0;
540 
541  for(ProjMap::iterator itr = prjs.begin(); itr != prjs.end(); itr++)
542  {
543  j++;
544  Proj &prj = itr->second;
545  // projection
546  int cami = prj.ndi;
547  if (prj.isValid)
548  dcnt(cami)++;
549 
550  // check validity
551  if (!prj.isValid)
552  {
553  nbad++;
554  continue;
555  }
556  n++;
557 
558  // add to camera connection matrix
559  if (j < (int)prjs.size()-1)
560  {
561  map<int,int> &pm = conns[cami];
562  map<int,int>::iterator it;
563  for(ProjMap::iterator itr2 = itr; itr2 != prjs.end(); itr2++)
564  {
565  Proj &prj2 = itr2->second;
566  int cami2 = prj2.ndi;
567  map<int,int> &pm2 = conns[cami2];
568  it = pm.find(cami2);
569  if (it == pm.end()) // not here yet
570  {
571  pm[cami2] = 1; // create entry
572  pm2[cami] = 1;
573  }
574  else
575  {
576  it->second++; // increment
577  pm2.find(cami)->second++;
578  }
579  }
580  }
581  }
582 
583  // stats on tracks
584  if (n == 2)
585  n2++;
586  if (n == 1)
587  n1++;
588  if (n == 0)
589  n0++;
590  }
591  }
592 
593  cout << "ok" << endl;
594 
595  // projection stats
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;
598 
599  // connections
600  int dnz = 0;
601  int dn5 = 0;
602  for (int i=0; i<ncams; i++)
603  {
604  if (dcnt(i) == 0) dnz++;
605  if (dcnt(i) < 5) dn5++;
606  }
607  cout << "[SBAsys] Number of disconnected cameras: " << dnz << endl;
608  cout << "[SBAsys] Number of cameras with <5 projs: " << dn5 << endl;
609 
610 
611 #if 1
612  // connection stats
613  int nconns = 0;
614  int ntot = 0;
615  int nmin = 1000000;
616  int nmax = 0;
617  int nc5 = 0;
618  int nc10 = 0;
619  int nc20 = 0;
620  int nc50 = 0;
621  n0 = 0;
622  n1 = 1;
623  for (int i=0; i<ncams; i++)
624  {
625  int nc = conns[i].size();
626  nconns += nc;
627  if (nc == 0) n0++;
628  if (nc == 1) n1++;
629  map<int,int>::iterator it;
630  for (it = conns[i].begin(); it != conns[i].end(); it++)
631  {
632  int np = (*it).second;
633  ntot += np;
634  if (np < nmin) nmin = np;
635  if (np > nmax) nmax = np;
636  if (np < 5) nc5++;
637  if (np < 10) nc10++;
638  if (np < 20) nc20++;
639  if (np < 50) nc50++;
640  }
641  }
642 
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;
651 #endif
652 
653  }
654 
655  vector<map<int,int> > SysSBA::generateConns_()
656  {
657  int ncams = nodes.size();
658  vector<map<int,int> > conns; // connections between cameras - key is camera, val is count
659  conns.resize(ncams);
660 
661  // reset bit maps
662  connMat.resize(ncams);
663  for (int i=0; i<ncams; i++)
664  {
665  vector<bool> &bm = connMat[i];
666  bm.assign(ncams,false);
667  }
668 
669  // set up sparse connectivity matrix
670  for (int i=0; i<(int)tracks.size(); i++)
671  {
672  int j = 0;
673  ProjMap &prjs = tracks[i].projections;
674  for(ProjMap::iterator itr = prjs.begin(); itr != prjs.end(); itr++)
675  {
676  Proj &prj = itr->second;
677  j++;
678  int cami = prj.ndi;
679 
680  // check validity
681  if (!prj.isValid)
682  continue;
683 
684  // add to camera connection matrix
685  if (j < (int)prjs.size()-1)
686  {
687  map<int,int> &pm = conns[cami];
688  map<int,int>::iterator it;
689  for(ProjMap::iterator itr2 = itr; itr2 != prjs.end(); itr2++)
690  {
691  Proj &prj2 = itr2->second;
692  int cami2 = prj2.ndi;
693  map<int,int> &pm2 = conns[cami2];
694  it = pm.find(cami2);
695  if (it == pm.end()) // not here yet
696  {
697  pm[cami2] = 1; // create entry
698  pm2[cami] = 1;
699  }
700  else
701  {
702  it->second++; // increment
703  pm2.find(cami)->second++;
704  }
705  }
706  }
707  }
708  }
709 
710  return conns;
711  }
712 
713 
714  // set the connectivity matrix based on a minimum number of points
715  // greedy algorithm, heads with lowest and preserves connectivity
716  void SysSBA::setConnMat(int minpts)
717  {
718  // data structures
719  int ncams = nodes.size();
720  // connections between cameras - key is camera, val is count
721  vector<map<int,int> > conns = generateConns_();
722 
723  // get ordered list of connections
724  multimap<int,pair<int,int> > weakcs;
725  for (int i=0; i<ncams; i++)
726  {
727  map<int,int> cs = conns[i];
728  map<int,int>::iterator it;
729  for (it = cs.begin(); it != cs.end(); it++)
730  {
731  if (it->second < minpts && it->first > i) // upper triangle
732  weakcs.insert(pair<int,pair<int,int> >(it->second, pair<int,int>(i,it->first)));
733  }
734  }
735 
736  cout << "[SetConnMat] Found " << weakcs.size() << " connections with < "
737  << minpts << " points" << endl;
738 
739  // greedily delete connections that don't disconnect the graph
740  int n = 0;
741  multimap<int,pair<int,int> >::iterator it;
742  for (it = weakcs.begin(); it != weakcs.end(); it++)
743  {
744  int c0 = it->second.first;
745  int c1 = it->second.second;
746  if (conns[c0].size() > 1 && conns[c1].size() > 1)
747  {
748  n++;
749  conns[c0].erase(conns[c0].find(c1)); // erase entry
750  conns[c1].erase(conns[c1].find(c0)); // erase entry
751  // set correct bits
752  connMat[c0][c1] = true;
753  connMat[c1][c0] = true;
754  }
755  }
756 
757  cout << "[SetConnMat] Erased " << n << " connections" << endl;
758 
759  }
760 
761 
762  // set the connectivity matrix based on a spanning tree
763  // greedy algorithm, strings together best matches first
764  void SysSBA::setConnMatReduced(int maxconns)
765  {
766  // data structures
767  int ncams = nodes.size();
768  // connections between cameras - key is camera, val is count
769  vector<map<int,int> > conns = generateConns_();
770 
771  // get ordered list of connections
772  multimap<int,pair<int,int> > weakcs;
773  for (int i=0; i<ncams; i++)
774  {
775  map<int,int> cs = conns[i];
776  map<int,int>::iterator it;
777  for (it = cs.begin(); it != cs.end(); it++)
778  {
779  if (it->first > i) // upper triangle, order by biggest matches first
780  weakcs.insert(pair<int,pair<int,int> >(-it->second, pair<int,int>(i,it->first)));
781  }
782  }
783 
784  // greedily add connections to build a spanning graph
785  int n = 0;
786  vector<int> found;
787  found.assign(ncams,0);
788  multimap<int,pair<int,int> >::iterator it;
789  for (it = weakcs.begin(); it != weakcs.end(); it++)
790  {
791  int c0 = it->second.first;
792  int c1 = it->second.second;
793  if (found[c0] < maxconns || found[c1] < maxconns)
794  {
795  n++;
796  // set correct bits
797  found[c0]++;
798  found[c1]++;
799  connMat[c0][c1] = false; // assign this connection
800  connMat[c1][c0] = false;
801  }
802  }
803 
804  cout << "[SetConnMat] Found " << n << " connections in spanning tree" << endl;
805  }
806 
807 
808  // helper fn
809  // split into random tracks
810  void
811  SysSBA::tsplit(int tri, int len)
812  {
813  ProjMap prjs = tracks[tri].projections;
814  tracks[tri].projections.clear();
815 
816  // first reset current track
817  int i=0;
818  if ((int)prjs.size() == len+1) len = len+1; // get rid of single tracks
819 
820  // Goes through existing projections and adds them back to the track up
821  // until len elements are added back.
822  while (prjs.size() > 0 && i < len)
823  {
824  // Pick a random projection to add back.
825  ProjMap::iterator randomitr = prjs.begin();
826  std::advance( randomitr, rand() % prjs.size());
827  Proj &prj = randomitr->second;
828 
829  // Add it back to the original track.
830  addProj(prj.ndi, tri, prj.kp, prj.stereo);
831  prjs.erase(randomitr);
832  i++;
833  }
834 
835  // Creates new tracks for remaining elements in the track.
836  int pti = tracks.size();
837  while (prjs.size() > 0)
838  {
839  i = 0;
840  if ((int)prjs.size() == len+1) len = len+1; // get rid of single tracks
841  while (prjs.size() > 0 && i < len)
842  {
843  // Pick a random projection to add to a new track.
844  ProjMap::iterator randomitr = prjs.begin();
845  std::advance( randomitr, rand() % prjs.size());
846  Proj &prj = randomitr->second;
847 
848  // Add it to the new track and erase it from the list of projections
849  // remaining.
850  addProj(prj.ndi, pti, prj.kp, prj.stereo);
851  prjs.erase(randomitr);
852  i++;
853  }
854  tracks[pti].point = tracks[tri].point;
855  pti++;
856  }
857  }
858 
859 
860  // get rid of long tracks
861  // currently not very smart, just orders them by length and gets rid of top ones
862  // or else splits them
863  int SysSBA::reduceLongTracks(double len)
864  {
865  int ilen = len;
866  // data structures
867  int npts = tracks.size();
868 
869 #if 1 // this algorithm splits tracks
870  // algorithm: for a long track, break it into enough pieces to get it under
871  // the required length. Randomzed point picking.
872  srand(time(NULL));
873  int nn = 0;
874  for (int i=0; i<npts; i++)
875  {
876  if ((int)tracks[i].projections.size() > ilen)
877  {
878  // make new set of proj's
879  nn++;
880  int ts = tracks[i].projections.size()+1;
881  int tn = ts/ilen;
882  tsplit(i,ts/tn);
883  }
884  }
885 
886  return nn;
887 
888 #else // this throws them out
889 
890  // Has not been changed from old API. :(
891 
892  multimap<int,int> ordps; // ordered tracks
893 
894  // order them, largest first
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));
898 
899  // remove long tracks
900  // int nn = npts*pct;
901  int nn = ordps.size();
902  map<int,int>::iterator it = ordps.begin();
903  vector<int> rem;
904  for (int i=0; i<nn; i++, it++)
905  rem.push_back(it->second);
906  sort(rem.begin(),rem.end()); // sort into ascending order
907  cout << "Finished finding " << rem.size() << " tracks" << endl;
908 
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;
912 
913  // delete elements into new vectors
914  int n = 0; // index into rem()
915  int ii = 0;
916  for (int i=0; i<npts; i++)
917  {
918  if (rem.size()>n && i == rem[n]) // skip this element
919  {
920  n++;
921  continue;
922  }
923  pts.push_back(points[i]);
924  vector<MonoProj, Eigen::aligned_allocator<MonoProj> > &prjs = tracks[i];
925  /*for (int j=0; j<(int)prjs.size(); j++)
926  prjs[j].pti = ii; */
927  trs.push_back(tracks[i]);
928  ii++;
929  }
930 
931  // transfer vectors
932  points.resize(pts.size());
933  tracks.resize(pts.size());
934  points = pts;
935  tracks = trs;
936 #endif
937 
938 
939  return nn;
940  }
941 
942 
943  // delete any track that doesn't reduce the connection number between two nodes below
944  // a minimum
945  // greedy algorithm, heads with largest (smallest???) tracks
946  int SysSBA::remExcessTracks(int minpts)
947  {
948  // data structures
949  //int ncams = nodes.size();
950  int npts = tracks.size();
951  // connections between cameras - key is camera, val is count
952  vector<map<int,int> > conns = generateConns_();
953 
954  // get ordered list of tracks
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));
958  vector<int> remtrs; // tracks to delete
959 
960  // greedily delete tracks that don't bring connection size below a minimum
961  multimap<int,int>::reverse_iterator it;
962  for (it = ordtrs.rbegin(); it != ordtrs.rend(); it++)
963  {
964  int tri = it->second;
965  ProjMap &prjs = tracks[tri].projections;
966  bool isgood = true;
967  for(ProjMap::iterator itr = prjs.begin(); itr != prjs.end(); itr++)
968  {
969  Proj &prj = itr->second;
970 
971  int c0 = prj.ndi;
972  for(ProjMap::iterator itr2 = itr; itr2 != prjs.end(); itr2++)
973  {
974  Proj &prj2 = itr2->second;
975  int c1 = prj2.ndi;
976  if (conns[c0].find(c1)->second <= minpts) // can't reduce this connection
977  {
978  isgood = false;
979  break;
980  }
981  }
982  if (!isgood) break;
983  }
984 
985  if (isgood) // found a deletable track, change connection values
986  {
987  for(ProjMap::iterator itr = prjs.begin(); itr != prjs.end(); itr++)
988  {
989  Proj &prj = itr->second;
990  int c0 = prj.ndi;
991  for(ProjMap::iterator itr2 = itr; itr2 != prjs.end(); itr2++)
992  {
993  Proj &prj2 = itr2->second;
994  int c1 = prj2.ndi;
995  conns[c0].find(c1)->second--;
996  conns[c1].find(c0)->second--;
997  }
998  }
999  remtrs.push_back(tri); // save for deletion
1000  }
1001  }
1002 
1003  int hms = 0;
1004  for (int i=0; i<(int)remtrs.size(); i++)
1005  {
1006  int s = tracks[remtrs[i]].projections.size();
1007  hms += s*(s+1)/2;
1008  }
1009 
1010  cout << "[RemExcessTracks] Can erase " << remtrs.size() << " tracks with " << hms << " H entries" << endl;
1011 
1012  std::sort(remtrs.begin(),remtrs.end()); // sort into ascending order
1013 
1014  std::vector<Track, Eigen::aligned_allocator<Track> > trs;
1015 
1016  // delete elements into new vectors
1017  int n = 0; // index into rem()
1018  int ii = 0;
1019  for (int i=0; i<npts; i++)
1020  {
1021  if ((int)remtrs.size()>n && i == remtrs[n]) // skip this element
1022  {
1023  n++;
1024  continue;
1025  }
1026  // vector<MonoProj, Eigen::aligned_allocator<MonoProj> > &prjs = tracks[i];
1027  /*for (int j=0; j<(int)prjs.size(); j++)
1028  prjs[j].pti = ii; */
1029  trs.push_back(tracks[i]);
1030  ii++;
1031  }
1032 
1033  cout << "[RemExcessTracks] Erased " << n << " tracks" << endl;
1034 
1035  // transfer vectors
1036  tracks.resize(trs.size());
1037  tracks = trs;
1038 
1039  return remtrs.size();
1040  }
1041 
1042  // Set up linear system, from Engels and Nister 2006, Table 1, steps 3 and 4
1043  // This is a relatively compact version of the algorithm!
1044  // Assumes camera transforms and derivatives have already been computed,
1045  // but not projection Jacobians
1046 
1047 void SysSBA::setupSys(double sLambda)
1048  {
1049  // set matrix sizes and clear (step 3)
1050  int nFree = nodes.size() - nFixed;
1051  A.setZero(6*nFree,6*nFree);
1052  B.setZero(6*nFree);
1053  VectorXi dcnt(nFree);
1054  dcnt.setZero(nFree);
1055 
1056  // lambda augmentation
1057  double lam = 1.0 + sLambda;
1058 
1059  // loop over tracks (step 4)
1060  for(size_t pi=0; pi<tracks.size(); pi++)
1061  {
1062  ProjMap &prjs = tracks[pi].projections;
1063  if (prjs.size() < 1) continue; // this catches some problems with bad tracks
1064 
1065  // Jacobian product storage
1066  if (prjs.size() > jps.size())
1067  jps.resize(prjs.size());
1068 
1069  // local storage
1070  Matrix3d Hpp;
1071  Hpp.setZero(); // zero it out
1072  Vector3d bp;
1073  bp.setZero();
1074 
1075  // "compute derivates" of step 4
1076  // assume error has already been calculated in the cost function
1077  int ii=0;
1078  for(ProjMap::iterator itr = prjs.begin(); itr != prjs.end(); itr++, ii++)
1079  {
1080  Proj &prj = itr->second;
1081  if (!prj.isValid) continue;
1082  int ci = (prj.ndi - nFixed) * 6; // index of camera params (6DOF)
1083  // NOTE: assumes fixed cams are at beginning
1084  prj.setJacobians(nodes[prj.ndi],tracks[pi].point,&jps[ii]); // calculate derivatives
1085  Hpp += prj.jp->Hpp; // add in JpT*Jp
1086  bp -= prj.jp->Bp; // subtract JcT*f from bp; compute transpose twice???
1087 
1088  if (!nodes[prj.ndi].isFixed) // if not a fixed camera, do more
1089  {
1090  dcnt(prj.ndi - nFixed)++;
1091  // NOTE: A is symmetric, only need the upper/lower triangular part
1092  A.block<6,6>(ci,ci) += prj.jp->Hcc; // add JcT*Jc to A; diagonal augmented????
1093  B.block<6,1>(ci,0) -= prj.jp->JcTE;
1094  }
1095  }
1096 
1097  // Augment Hpp, invert it and save Hpp' * bp
1098  // Hmm, shouldn't need this (augmented diagonal at end),
1099  // but seems to work better...
1100  Hpp.diagonal() *= lam;
1101  Matrix3d Hppi = Hpp.inverse(); // Which inverse should we use???? Note Hpp is symmetric
1102  Vector3d &tp = tps[pi];
1103  tp = Hppi * bp;
1104 
1105  // "outer product of track" in Step 4
1106  for(ProjMap::iterator itr = prjs.begin(); itr != prjs.end(); itr++)
1107  {
1108  Proj &prj = itr->second;
1109  if (!prj.isValid) continue;
1110  if (nodes[prj.ndi].isFixed) continue; // skip fixed cameras
1111  int ci = (prj.ndi - nFixed) * 6; // index of camera params (6DOF)
1112  // NOTE: assumes fixed cams are at beginning
1113  B.block<6,1>(ci,0) -= prj.jp->Hpc.transpose() * tp; // Hpc * tp subtracted from B
1114  prj.Tpc = prj.jp->Hpc.transpose() * Hppi;
1115 
1116  // iterate over nodes left on the track, plus yourself
1117  for(ProjMap::iterator itr2 = itr; itr2 != prjs.end(); itr2++)
1118  {
1119  Proj &prj2 = itr2->second;
1120  if (!prj2.isValid) continue;
1121  if (nodes[prj2.ndi].isFixed) continue; // skip fixed cameras
1122  int ci2 = (prj2.ndi - nFixed) * 6; // index of camera params (6DOF)
1123  // NOTE: assumes fixed cams are at beginning
1124  // NOTE: this only does upper triangular part
1125  A.block<6,6>(ci,ci2) -= prj.Tpc * prj2.jp->Hpc; // Tpc * Hpc2 subtracted from A(c,c2)
1126  // lower triangular part - this can be dropped for CSparse, uses ~30% of setup time
1127  if (ci != ci2)
1128  A.block<6,6>(ci2,ci) = A.block<6,6>(ci,ci2).transpose();
1129  }
1130  } // finish outer product
1131 
1132  } // finish track
1133 
1134  // augment diagonal
1135  A.diagonal() *= lam;
1136 
1137  // check the matrix and vector
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; }
1141 
1142  for (int j=0; j<6*nFree; j++)
1143  if (std::isnan(B[j]) ) { printf("[SetupSys] NaN in B\n"); *(int *)0x0 = 0; }
1144 
1145  int ndc = 0;
1146  for (int i=0; i<nFree; i++)
1147  if (dcnt(i) == 0) ndc++;
1148 
1149  if (ndc > 0)
1150  cout << "[SetupSys] " << ndc << " disconnected nodes" << endl;
1151 
1152  }
1153 
1154 
1155  // Set up linear system, from Engels and Nister 2006, Table 1, steps 3 and 4
1156  // This is a relatively compact version of the algorithm!
1157  // Assumes camera transforms and derivatives have already been computed,
1158  // but not projection Jacobians
1159  // CSparse version
1160 
1161  void SysSBA::setupSparseSys(double sLambda, int iter, int sparseType)
1162  {
1163  // set matrix sizes and clear (step 3)
1164  int nFree = nodes.size() - nFixed;
1165  if (nFree < 0) nFree = 0;
1166 
1167  long long t0, t1, t2, t3;
1168  t0 = utime();
1169  if (iter == 0)
1170  csp.setupBlockStructure(nFree); // initialize CSparse structures
1171  else
1172  csp.setupBlockStructure(0); // zero out CSparse structures
1173  t1 = utime();
1174 
1175 
1176  VectorXi dcnt(nFree);
1177  dcnt.setZero(nFree);
1178 
1179  // lambda augmentation
1180  double lam = 1.0 + sLambda;
1181 
1182  // use connection matrix?
1183  bool useConnMat = connMat.size() > 0;
1184  int nskip = 0;
1185 
1186  // loop over tracks (step 4)
1187  for(size_t pi=0; pi<tracks.size(); pi++)
1188  {
1189  ProjMap &prjs = tracks[pi].projections;
1190  if (prjs.size() < 1) continue; // this catches some problems with bad tracks
1191 
1192  // set up vector storage of Jacobian products
1193  if (prjs.size() > jps.size())
1194  jps.resize(prjs.size());
1195 
1196  // local storage
1197  Matrix3d Hpp;
1198  Hpp.setZero(); // zero it out
1199  Vector3d bp;
1200  bp.setZero();
1201 
1202  // "compute derivates" of step 4
1203  // assume error has already been calculated in the cost function
1204  int ii=0;
1205  for(ProjMap::iterator itr = prjs.begin(); itr != prjs.end(); itr++, ii++)
1206  {
1207  Proj &prj = itr->second;
1208  if (!prj.isValid) continue;
1209  int ni = prj.ndi - nFixed;
1210  int ci = ni * 6; // index of camera params (6DOF)
1211  // NOTE: assumes fixed cams are at beginning
1212  prj.setJacobians(nodes[prj.ndi],tracks[pi].point,&jps[ii]); // calculate derivatives
1213  Hpp += prj.jp->Hpp; // add in JpT*Jp
1214  bp -= prj.jp->Bp; // subtract JcT*f from bp; compute transpose twice???
1215 
1216  if (!nodes[prj.ndi].isFixed) // if not a fixed camera, do more
1217  {
1218  dcnt(prj.ndi - nFixed)++;
1219  // NOTE: A is symmetric, only need the upper/lower triangular part
1220  // jctjc.diagonal() *= lam; // now done at end
1221  csp.addDiagBlock(prj.jp->Hcc,ni);
1222  csp.B.block<6,1>(ci,0) -= prj.jp->JcTE;
1223  }
1224  }
1225 
1226  // Augment Hpp, invert it and save Hpp' * bp
1227  // Hmm, shouldn't need this (augmented diagonal at end),
1228  // but seems to work better...
1229  Hpp.diagonal() *= lam;
1230  Matrix3d Hppi = Hpp.inverse(); // Which inverse should we use???? Note Hpp is symmetric; but this is not a bottleneck
1231  Vector3d &tp = tps[pi];
1232  tp = Hppi * bp;
1233 
1234  // "outer product of track" in Step 4
1235  for(ProjMap::iterator itr = prjs.begin(); itr != prjs.end(); itr++)
1236  {
1237  Proj &prj = itr->second;
1238  if (!prj.isValid) continue;
1239  if (nodes[prj.ndi].isFixed) continue; // skip fixed cameras
1240  int ni = prj.ndi - nFixed;
1241  int ci = ni * 6; // index of camera params (6DOF)
1242  // NOTE: assumes fixed cams are at beginning
1243  csp.B.block<6,1>(ci,0) -= prj.jp->Hpc.transpose() * tp; // Hpc * tp subtracted from B
1244  prj.Tpc = prj.jp->Hpc.transpose() * Hppi;
1245 
1246  // iterate over nodes left on the track, plus yourself
1247  if (sparseType != SBA_GRADIENT)
1248  for(ProjMap::iterator itr2 = itr; itr2 != prjs.end(); itr2++)
1249  {
1250  Proj &prj2 = itr2->second;
1251  if (!prj2.isValid) continue;
1252  if (nodes[prj2.ndi].isFixed) continue; // skip fixed cameras
1253  int ni2 = prj2.ndi - nFixed; // NOTE: assumes fixed cams are at beginning
1254  if (useConnMat && connMat[prj.ndi][prj2.ndi]) // check connection matrix filter
1255  {
1256  nskip++;
1257  continue;
1258  }
1259  Matrix<double,6,6> m = -prj.Tpc * prj2.jp->Hpc;
1260  if (ni == ni2)
1261  csp.addDiagBlock(m,ni);
1262  else
1263  csp.addOffdiagBlock(m,ni,ni2);
1264  }
1265  else // gradient calculation
1266  {
1267  Matrix<double,6,6> m = -prj.Tpc * prj.jp->Hpc;
1268  csp.addDiagBlock(m,ni);
1269  }
1270 
1271  } // finish outer product
1272 
1273  } // finish track
1274 
1275  // cout << "[SetupSparseSys] Skipped conns: " << nskip << endl;
1276 
1277  t2 = utime();
1278 
1279  // set up sparse matrix structure from blocks
1280  if (sparseType == SBA_BLOCK_JACOBIAN_PCG)
1281  csp.incDiagBlocks(lam); // increment diagonal block
1282  else
1283  csp.setupCSstructure(lam,iter==0);
1284 
1285 
1286  t3 = utime();
1287  if (verbose)
1288  printf("\n[SetupSparseSys] Block: %0.1f Cons: %0.1f CS: %0.1f\n",
1289  (t1-t0)*.001, (t2-t1)*.001, (t3-t2)*.001);
1290 
1291  int ndc = 0;
1292  for (int i=0; i<nFree; i++)
1293  if (dcnt(i) == 0) ndc++;
1294 
1295  if (ndc > 0)
1296  cout << "[SetupSys] " << ndc << " disconnected nodes" << endl;
1297 
1298  } // finish all tracks
1299 
1300 
1301 
1308  int SysSBA::doSBA(int niter, double sLambda, int useCSparse, double initTol, int maxCGiter)
1309  {
1310  // set aux buffer
1311  oldpoints.clear();
1312  oldpoints.resize(tracks.size());
1313 
1314 
1315  // storage
1316  int npts = tracks.size();
1317  int ncams = nodes.size();
1318  tps.resize(npts);
1319 
1320  // set number of projections
1321  int nprjs = 0;
1322  for(size_t i=0; i<tracks.size(); i++)
1323  {
1324  oldpoints[i] = tracks[i].point;
1325  nprjs += tracks[i].projections.size();
1326  }
1327 
1328  if (nprjs == 0 || npts == 0 || ncams == 0)
1329  {
1330  return -1;
1331  }
1332 
1333  // initialize vars
1334  if (sLambda > 0.0) // do we initialize lambda?
1335  lambda = sLambda;
1336 
1337  // check for fixed frames
1338  if (nFixed <= 0)
1339  {
1340  cout << "[doSBA] No fixed frames" << endl;
1341  return 0;
1342  }
1343  for (int i=0; i<ncams; i++)
1344  {
1345  Node &nd = nodes[i];
1346  if (i >= nFixed)
1347  nd.isFixed = false;
1348  else
1349  nd.isFixed = true;
1350  nd.setTransform(); // set up projection matrix for cost calculation
1351  nd.setProjection();
1352  nd.setDr(useLocalAngles);
1353  }
1354 
1355  // initialize vars
1356  double laminc = 2.0; // how much to increment lambda if we fail
1357  double lamdec = 0.5; // how much to decrement lambda if we succeed
1358  int iter = 0; // iterations
1359  sqMinDelta = 1e-8 * 1e-8;
1360  updateNormals();
1361  double cost = calcCost();
1362 
1363  if (verbose > 0)
1364  {
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;
1369  }
1370 
1371  for (; iter<niter; iter++) // loop at most <niter> times
1372  {
1373  // set up and solve linear system
1374  // NOTE: shouldn't need to redo all calcs in setupSys if we
1375  // got here from a bad update
1376 
1377  // If we have point-plane matches, should update normals here.
1378  updateNormals();
1379 
1380  t0 = utime();
1381  if (useCSparse)
1382  setupSparseSys(lambda,iter,useCSparse); // sparse version
1383  else
1384  setupSys(lambda); // set up linear system
1385 
1386  // if (iter == 0)
1387  // cout << endl << A << endl << endl;
1388  // cout << "[SBA] Solving...";
1389 
1390 #if 0
1391  int xs = B.size();
1392  char fn[2048];
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));
1400  fclose(fd);
1401 
1402  sprintf(fn,"B%d.txt",xs);
1403  fd = fopen(fn,"w");
1404  fprintf(fd,"%d\n",xs);
1405  for (int ii=0; ii<xs; ii++)
1406  fprintf(fd,"%.16g\n",B(ii));
1407  fclose(fd);
1408 #endif
1409 
1410  t1 = utime();
1411 
1412  // use appropriate linear solver
1413  if (useCSparse == SBA_BLOCK_JACOBIAN_PCG)
1414  {
1415  if (csp.B.rows() != 0)
1416  {
1417  int iters = csp.doBPCG(maxCGiter,initTol,iter);
1418  cout << "[Block PCG] " << iters << " iterations" << endl;
1419  }
1420  }
1421  else if (useCSparse > 0)
1422  {
1423  if (csp.B.rows() != 0)
1424  {
1425  bool ok = csp.doChol();
1426  if (!ok)
1427  cout << "[DoSBA] Sparse Cholesky failed!" << endl;
1428  }
1429  }
1430  else
1431  {
1432 #if 1
1433  A.llt().solveInPlace(B); // Cholesky decomposition and solution
1434 #else
1435  printf("\nDoing dpotrf/dpotrs\n");
1436  double *a = A.data();
1437  int info, m = B.size();
1438  double *x = B.data();
1439  int nrhs = 1;
1440  F77_FUNC(dpotrf)("U", (int *)&m, a, (int *)&m, (int *)&info);
1441  F77_FUNC(dpotrs)("U", (int *)&m, (int *)&nrhs, a, (int *)&m, x, (int *)&m, &info);
1442 #endif
1443  }
1444  t2 = utime();
1445  // printf("Matrix size: %d Time: %d\n", B.size(), t2-t1);
1446 
1447  // cout << "solved" << endl;
1448 
1449  // get correct result vector
1450  VectorXd &BB = useCSparse ? csp.B : B;
1451 
1452  // check for convergence
1453  // this is a pretty crummy convergence measure...
1454  double sqDiff = BB.squaredNorm();
1455  if (sqDiff < sqMinDelta) // converged, done...
1456  {
1457  if (verbose > 0)
1458  cout << "Converged with delta: " << sqrt(sqDiff) << endl;
1459  break;
1460  }
1461 
1462  // update the cameras
1463  int ci = 0;
1464  for(vector<Node, Eigen::aligned_allocator<Node> >::iterator itr = nodes.begin(); itr != nodes.end(); itr++)
1465  {
1466  Node &nd = *itr;
1467  if (nd.isFixed) continue; // not to be updated
1468  nd.oldtrans = nd.trans; // save in case we don't improve the cost
1469  nd.oldqrot = nd.qrot;
1470  nd.trans.head<3>() += BB.segment<3>(ci);
1471 
1472  if (useLocalAngles)
1473  {
1474  Quaternion<double> qr;
1475  qr.vec() = BB.segment<3>(ci+3);
1476  // double sn = qr.vec().squaredNorm();
1477  // if (sn > 0.01) // usually indicates something has gone wrong
1478  // qr.vec() = qr.vec() / (sqrt(sn) * 10.0);
1479  qr.w() = sqrt(1.0 - qr.vec().squaredNorm());
1480  qr = nd.qrot*qr; // post-multiply, because we pre-multiply the transpose for Jacobian
1481  qr.normalize();
1482  nd.qrot = qr;
1483  }
1484  else
1485  {
1486  nd.qrot.coeffs().head<3>() += BB.segment<3>(ci+3);
1487  nd.normRot();
1488  }
1489 
1490  nd.setTransform(); // set up projection matrix for cost calculation
1491  nd.setProjection();
1492  nd.setDr(useLocalAngles); // set rotational derivatives
1493  ci += 6;
1494  }
1495 
1496  // update the points (step 7)
1497  // loop over tracks
1498  int pi = 0; // point index
1499  for(vector<Track, Eigen::aligned_allocator<Track> >::iterator itr = tracks.begin();
1500  itr != tracks.end(); itr++, pi++)
1501  {
1502  ProjMap &prjs = itr->projections;
1503  if (prjs.size() < 1) continue;
1504  Vector3d tp = tps[pi]; // copy to preserve the original
1505  // loop over cameras in each track
1506  for(ProjMap::iterator pitr = prjs.begin(); pitr != prjs.end(); pitr++)
1507  {
1508  Proj &prj = pitr->second;
1509  if (!prj.isValid) continue;
1510  if (nodes[prj.ndi].isFixed) continue; // only update with non-fixed cameras
1511  int ci = (prj.ndi - nFixed) * 6; // index of camera params (6DOF)
1512  // NOTE: assumes fixed cams are at beginning
1513  tp -= prj.Tpc.transpose() * BB.segment<6>(ci);
1514  }
1515  // update point
1516  oldpoints[pi] = tracks[pi].point; // save for backing out
1517  tracks[pi].point.head(3) += tp;
1518  }
1519 
1520  t3 = utime();
1521 
1522  // new cost
1523  updateNormals();
1524  double newcost = calcCost();
1525 
1526  // average reprojection error (for Lourakis test)
1527  // write some stats
1528  if (verbose > 0)
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;
1533 
1534 
1535  // check if we did good
1536  if (newcost < cost) // && iter != 0) // NOTE: iter==0 case is for checking
1537  {
1538  cost = newcost;
1539  lambda *= lamdec; // decrease lambda
1540  // laminc = 2.0; // reset bad lambda factor; not sure if this is a good idea...
1541  }
1542  else
1543  {
1544  lambda *= laminc; // increase lambda
1545  laminc *= 2.0; // increase the increment
1546  // reset points
1547  for(int i=0; i < (int)tracks.size(); i++)
1548  {
1549  tracks[i].point = oldpoints[i];
1550  }
1551  // reset cams
1552  for(int i=0; i < (int)nodes.size(); i++)
1553  {
1554  Node &nd = nodes[i];
1555  if (nd.isFixed) continue; // not to be updated
1556  nd.trans = nd.oldtrans;
1557  nd.qrot = nd.oldqrot;
1558  nd.setTransform(); // set up projection matrix for cost calculation
1559  nd.setProjection();
1560  nd.setDr(useLocalAngles);
1561  }
1562 
1563  updateNormals();
1564  cost = calcCost(); // need to reset errors
1565  if (verbose > 0)
1566  cout << iter << " Downdated cost: " << cost << endl;
1567  // NOTE: shouldn't need to redo all calcs in setupSys
1568  }
1569 
1570  t4 = utime();
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));
1578 
1579  }
1580 
1581  // return number of iterations performed
1582  return iter;
1583  }
1584 
1585 
1588  void SysSBA::mergeTracks(std::vector<std::pair<int,int> > &prs)
1589  {
1590  // first form an ordered index of tracks
1591  int ntrs = tracks.size();
1592  vector<int> tris;
1593  tris.resize(ntrs);
1594 
1595  for (int i=0; i<ntrs; i++)
1596  tris[i] = i;
1597 
1598  // go over pairs and insert equalities
1599  for (int i=0; i<(int)prs.size(); i++)
1600  {
1601  pair<int,int> &pr = prs[i];
1602  int p0 = min(pr.first,pr.second);
1603  int p1 = max(pr.first,pr.second);
1604  tris[p1] = p0;
1605  }
1606 
1607  // do transitive closure
1608  for (int i=0; i<ntrs; i++)
1609  tris[i] = tris[tris[i]];
1610 
1611  // fix up tracks, monocular
1612  if (tracks.size() > 0)
1613  {
1614  for (int i=0; i<(int)tracks.size(); i++)
1615  {
1616  if (tris[i] == i) continue;
1617 
1618  ProjMap &tr0 = tracks[tris[i]].projections;
1619  ProjMap &tr1 = tracks[i].projections;
1620 
1621  for(ProjMap::iterator itr1 = tr1.begin(); itr1 != tr1.end(); itr1++)
1622  {
1623  Proj &prj = itr1->second;
1624  int ci = prj.ndi;
1625 
1626  // Insert the projection into the original track
1627  tr0[ci] = prj;
1628  }
1629  tr1.clear();
1630  }
1631 
1632  // fill up holes, reset point indices
1633  int n = 0;
1634  for (int i=0; i<(int)tracks.size(); i++)
1635  {
1636  if (tris[i] != i) continue;
1637  if (n == i) continue;
1638  tracks[n] = tracks[i];
1639  tracks[n].point = tracks[i].point;
1640 
1641  // We don't really do anything here? Should this be uncommented?
1642  // std::ProjMap &tr = tracks[n].projections;
1643  /*for (int j=0; j<(int)tr.size(); j++)
1644  tr[j].pti = n; */
1645  n++;
1646  }
1647  tracks.resize(n);
1648  }
1649  }
1650 
1651 
1656  int SysSBA::mergeTracksSt(int tri0, int tri1)
1657  {
1658  // loop through track tr1, adding projections to tr0 and checking for redundancy
1659  ProjMap tr0 = tracks[tri0].projections; // duplicate this track, for undo
1660  ProjMap &tr1 = tracks[tri1].projections;
1661 
1662  for(ProjMap::iterator itr = tr1.begin(); itr != tr1.end(); itr++)
1663  {
1664  Proj &prj = itr->second;
1665  bool ok = addProj(prj.ndi, tri0, prj.kp, prj.stereo);
1666  if (!ok)
1667  {
1668  tracks[tri0].projections = tr0; // reset to original track
1669  return -1;
1670  }
1671  }
1672 
1673  tr1.clear();
1674  return tri0;
1675  }
1676 
1677 
1678 } // end namespace sba
sba::Proj::plane_normal
Eigen::Vector3d plane_normal
Normal for point-plane projections.
Definition: proj.h:143
Eigen
Definition: sba_file_io.h:18
sba::Node::setKcam
void setKcam(const fc::CamParams &cp)
Sets the Kcam and baseline matrices from frame_common::CamParams.
Definition: node.h:98
sba::Node::setDr
void setDr(bool local=false)
Set angle derivates.
Definition: node.cpp:56
dpotrf
int F77_FUNC() dpotrf(char *uplo, int *n, double *a, int *lda, int *info)
sba::JacobProds::Hpc
Eigen::Matrix< double, 3, 6 > Hpc
Point-to-camera Hessian (JpT*Jc)
Definition: proj.h:36
sba::Proj::plane_local_normal
Eigen::Vector3d plane_local_normal
Original normal in plane_node_index coordinate's frame.
Definition: proj.h:155
sba::Point
Eigen::Vector4d Point
Point holds 3D points using in world coordinates. Currently we just represent these as 4-vectors,...
Definition: node.h:43
frame_common::CamParams
Definition: node.h:21
s
XmlRpcServer s
sba::Node::oldqrot
Eigen::Quaternion< double > oldqrot
Previous quaternion rotation, saved for downdating within LM.
Definition: node.h:147
sba::Node::normRot
void normRot()
Normalize quaternion to unit. Problem with global derivatives near w=0, solved by a hack for now.
Definition: node.cpp:37
sba::Node::setProjection
void setProjection()
Definition: node.h:121
sba::JacobProds::Hpp
Eigen::Matrix< double, 3, 3 > Hpp
Point-to-point Hessian (JpT*Jp).
Definition: proj.h:33
sba::Proj::calcErr
double calcErr(const Node &nd, const Point &pt, double huber=0.0)
Calculates re-projection error and stores it in err.
Definition: proj.cpp:25
sba::Proj::err
Eigen::Vector3d err
Reprojection error.
Definition: proj.h:84
SBA_GRADIENT
#define SBA_GRADIENT
Definition: sba.h:68
sba::Proj::plane_node_index
int plane_node_index
Point-plane node index in SBA.
Definition: proj.h:152
sba::Track
Definition: proj.h:182
sba::JacobProds::Hcc
Eigen::Matrix< double, 6, 6 > Hcc
Camera-to-camera Hessian (JcT*Jc)
Definition: proj.h:39
ok
ROSCPP_DECL bool ok()
sba::Proj::Tpc
Eigen::Matrix< double, 6, 3 > Tpc
Definition: proj.h:124
sba::JacobProds::Bp
Eigen::Matrix< double, 3, 1 > Bp
The B matrix with respect to points (JpT*Err)
Definition: proj.h:42
sba::Node::isFixed
bool isFixed
For SBA, is this camera fixed or free?
Definition: node.h:141
sba::Proj::setJacobians
void setJacobians(const Node &nd, const Point &pt, JacobProds *jpp)
Sets the jacobians and hessians for the projection to use for SBA.
Definition: proj.cpp:17
dpotf2
int F77_FUNC() dpotf2(char *uplo, int *n, double *a, int *lda, int *info)
sba::Proj::getErrNorm
double getErrNorm()
Get the correct norm of the error, depending on whether the projection is monocular or stereo.
Definition: proj.cpp:33
F77_FUNC
#define F77_FUNC(func)
Definition: sba.cpp:60
sba::Proj::plane_point_index
int plane_point_index
Point-plane match point index in SBA.
Definition: proj.h:149
sba
Definition: bpcg.h:60
sba::Proj::stereo
bool stereo
Whether the projection is Stereo (True) or Monocular (False).
Definition: proj.h:87
sba::Node::qrot
Eigen::Quaternion< double > qrot
Rotation of the node expressed as a Quaternion.
Definition: node.h:70
SBA_BLOCK_JACOBIAN_PCG
#define SBA_BLOCK_JACOBIAN_PCG
Definition: sba.h:69
sba::Node::oldtrans
Eigen::Matrix< double, 4, 1 > oldtrans
Previous translation, saved for downdating within LM.
Definition: node.h:144
sba::Proj::plane_point
Eigen::Vector3d plane_point
Point for point-plane projections.
Definition: proj.h:146
utime
static long long utime()
Definition: sba.cpp:48
sba::ProjMap
std::map< const int, Proj, std::less< int >, Eigen::aligned_allocator< Proj > > ProjMap
Definition: proj.h:48
sba::JacobProds::JcTE
Eigen::Matrix< double, 6, 1 > JcTE
Another matrix with respect to cameras (JcT*Err)
Definition: proj.h:45
sba::Proj::ndi
int ndi
Node index, the camera node for this projection.
Definition: proj.h:78
dpotri
int F77_FUNC() dpotri(char *uplo, int *n, double *a, int *lda, int *info)
sba::Node::setTransform
void setTransform()
Sets the transform matrices of the node.
Definition: node.cpp:8
sba::Proj::pointPlane
bool pointPlane
Whether this is a point-plane match (true) or a point-point match (false).
Definition: proj.h:140
std
sba::Proj::isValid
bool isValid
valid or not (could be out of bounds)
Definition: proj.h:127
sba::Node::trans
EIGEN_MAKE_ALIGNED_OPERATOR_NEW Eigen::Matrix< double, 4, 1 > trans
Translation in homogeneous coordinates, last element is 1.0.
Definition: node.h:69
sba::Proj::jp
JacobProds * jp
Jacobian products.
Definition: proj.h:120
sba::Proj::getErrSquaredNorm
double getErrSquaredNorm()
Get the correct squared norm of the error, depending on whether the projection is monocular or stereo...
Definition: proj.cpp:41
dpotrs
int F77_FUNC() dpotrs(char *uplo, int *n, int *nrhs, double *a, int *lda, double *b, int *ldb, int *info)
sba::Proj::kp
Eigen::Vector3d kp
Keypoint, u,v,u-d vector.
Definition: proj.h:81
sba.h
sba::Node
NODE holds graph nodes corresponding to frames, for use in sparse bundle adjustment....
Definition: node.h:63
sba::Proj
Proj holds a projection measurement of a point onto a frame. They are a repository for the link betwe...
Definition: proj.h:57


sparse_bundle_adjustment
Author(s):
autogenerated on Wed Mar 2 2022 01:03:04