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


sparse_bundle_adjustment
Author(s):
autogenerated on Fri Apr 3 2020 03:30:53