sba.h
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 // Sparse bundle/pose adjustment classes and functions
37 //
38 
39 #ifndef _SBA_H_
40 #define _SBA_H_
41 
42 #ifndef EIGEN_USE_NEW_STDVECTOR
43 #define EIGEN_USE_NEW_STDVECTOR
44 #endif // EIGEN_USE_NEW_STDVECTOR
45 
46 #include <stdio.h>
47 #include <iostream>
48 #include <Eigen/Core>
49 #include <Eigen/Geometry>
50 #include <Eigen/LU>
51 #include <Eigen/StdVector>
52 #include <vector>
53 #include <algorithm>
54 #include <Eigen/Cholesky>
55 
58 
59 
60 // sparse Cholesky
62 // block jacobian pcg
64 
65 // Defines for methods to use with doSBA().
66 #define SBA_DENSE_CHOLESKY 0
67 #define SBA_SPARSE_CHOLESKY 1
68 #define SBA_GRADIENT 2
69 #define SBA_BLOCK_JACOBIAN_PCG 3
70 
71 namespace sba
72 {
74 
75  class SysSBA
76  {
77  public:
78  EIGEN_MAKE_ALIGNED_OPERATOR_NEW // needed for 16B alignment
79 
81  int verbose;
82  long long t0, t1, t2, t3, t4; // save timing
83 
86  verbose = 1; huber = 0.0; }
87 
89  std::vector<Node, Eigen::aligned_allocator<Node> > nodes;
90 
92  int nFixed;
93 
95  std::vector<Track, Eigen::aligned_allocator<Track> > tracks;
96 
98  int countProjs();
99 
103  double calcCost();
104 
110  double calcCost(double dist);
111 
113  double huber;
114 
120  double calcRMSCost(double dist = 10000.0);
121 
124  double calcAvgError();
125 
127  int numBadPoints();
128 
130  int countBad(double dist);
131 
134  int removeBad(double dist);
135 
138  int reduceTracks();
139 
141  void printStats();
142 
145 
148  double lambda; // save for continuation
149  void setupSys(double sLambda);
150  void setupSparseSys(double sLambda, int iter, int sparseType);
151 
158  int doSBA(int niter, double lambda = 1.0e-4, int useCSparse = 0, double initTol = 1.0e-8,
159  int maxCGiters = 100);
160 
162  double sqMinDelta;
163 
171  int addNode(Eigen::Matrix<double,4,1> &trans,
172  Eigen::Quaternion<double> &qrot,
173  const fc::CamParams &cp,
174  bool isFixed = false);
175 
179  int addPoint(Point p);
180 
192  bool addProj(int ci, int pi, Eigen::Vector3d &q, bool stereo=true);
193 
203  bool addMonoProj(int ci, int pi, Eigen::Vector2d &q);
204 
214  bool addStereoProj(int ci, int pi, Eigen::Vector3d &q);
215 
222  void setProjCovariance(int ci, int pi, Eigen::Matrix3d &covar);
223 
234  void addPointPlaneMatch(int ci0, int pi0, Eigen::Vector3d normal0, int ci1, int pi1, Eigen::Vector3d normal1);
235 
237  void updateNormals();
238 
240  Eigen::MatrixXd A;
241  Eigen::VectorXd B;
242 
246  std::vector<std::vector<bool> > connMat;
247 
250  void setConnMat(int minpts);
252  void setConnMatReduced(int maxconns);
254  int remExcessTracks(int minpts);
255 
257  int reduceLongTracks(double pct);
258 
260  void mergeTracks(std::vector<std::pair<int,int> > &prs);
262  int mergeTracksSt(int tr0, int tr1);
263 
265  void useCholmod(bool yes)
266  { csp.useCholmod = yes; }
267 
271 
272  // Private helper functions
273  protected:
276  vector<map<int,int> > generateConns_();
277 
279  void tsplit(int tri, int len);
280 
282  std::vector<Point, Eigen::aligned_allocator<Point> > oldpoints;
283 
285  std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > tps;
286 
288  std::vector<JacobProds, Eigen::aligned_allocator<JacobProds> > jps;
289 
290  };
291 
292 
296 
297  class ConP2
298  {
299  public:
300  EIGEN_MAKE_ALIGNED_OPERATOR_NEW // needed for 16B alignment
301 
303  int ndr;
304 
306  int nd1;
307 
309  Eigen::Vector3d tmean;
310  Eigen::Quaternion<double> qpmean;
311  Eigen::Matrix<double,6,6> prec;
312 
313 
315  Eigen::Matrix<double,6,1> err;
317  inline double calcErr(const Node &nd0, const Node &nd1);
318 
320  double calcErrDist(const Node &nd0, const Node &nd1);
321 
322 
324  Eigen::Matrix<double,6,6> J0,J0t,J1,J1t;
325 
328  const static double qScale = 1.0;
329 
332 
335 
339  void setJacobians(std::vector<Node,Eigen::aligned_allocator<Node> > &nodes);
340 
342  bool isValid;
343  };
344 
345 
348 
349  class ConScale
350  {
351  public:
352  EIGEN_MAKE_ALIGNED_OPERATOR_NEW // needed for 16B alignment
353 
355  int nd0;
356 
358  int nd1;
359 
361  int sv;
362 
364  double ks;
365 
367  double w;
368 
370  double err;
372  inline double calcErr(const Node &nd0, const Node &nd1, double alpha);
373 
376  Eigen::Vector3d J0;
377 
380  Eigen::Vector3d J1;
381 
383  void setJacobians(std::vector<Node,Eigen::aligned_allocator<Node> > &nodes);
384 
386  bool isValid;
387  };
388 
389 
393 
394  class ConP3P
395  {
396  public:
397  EIGEN_MAKE_ALIGNED_OPERATOR_NEW // needed for 16B alignment
398 
400  int ndr;
401 
403  int nd1, nd2;
404 
406  Eigen::Matrix<double,12,1> mean;
407  Eigen::Matrix<double,12,12> prec;
408 
410  Eigen::Matrix<double,12,1> err;
412  Eigen::Matrix<double,12,1> calcErr(const Node &nd, const Point &pt);
413 
415  Eigen::Matrix<double,6,6> J10, J11, J20, J22;
416 
419 
422 
426  void setJacobians(std::vector<Node,Eigen::aligned_allocator<Node> > nodes);
427 
429  Eigen::Matrix<double,3,6> Hpc;
430  Eigen::Matrix<double,6,3> Tpc;
431 
433  bool isValid;
434  };
435 
436 
437 
439 
440  class SysSPA
441  {
442  public:
443  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
444 
446  SysSPA() { nFixed = 1; useLocalAngles = true; Node::initDr(); lambda = 1.0e-4;
447  verbose = false;}
448 
450  bool verbose;
451 
457  int addNode(Eigen::Matrix<double,4,1> &trans,
458  Eigen::Quaternion<double> &qrot,
459  bool isFixed = false);
460 
468  bool addConstraint(int nd0, int nd1,
469  Eigen::Vector3d &tmean,
470  Eigen::Quaterniond &qpmean,
471  Eigen::Matrix<double,6,6> &prec);
472 
474  std::vector<Node,Eigen::aligned_allocator<Node> > nodes;
475 
477  std::vector<double> scales;
478 
480  int nFixed;
481 
483  std::vector<ConP2,Eigen::aligned_allocator<ConP2> > p2cons;
484 
486  std::vector<ConScale,Eigen::aligned_allocator<ConScale> > scons;
487 
490 
493  double calcCost(bool tcost = false);
495  double calcCost(double dist);
496 
497 
499  void printStats();
500  void writeSparseA(char *fname, bool useCSparse = false); // save precision matrix in CSPARSE format
501 
504  double lambda; // save for continuation
505  void setupSys(double sLambda);
506  void setupSparseSys(double sLambda, int iter, int sparseType);
507 
511  int doSPA(int niter, double sLambda = 1.0e-4, int useCSparse = SBA_SPARSE_CHOLESKY,
512  double initTol = 1.0e-8, int CGiters = 50);
513 
515  double sqMinDelta;
516 
518  void spanningTree(int node=0);
519 
522  void addConstraint(int pr, int p0, int p1, Eigen::Matrix<double,12,1> &mean, Eigen::Matrix<double,12,12> &prec);
523 
525  Eigen::MatrixXd A;
526  Eigen::VectorXd B;
527 
530 
533  Eigen::Matrix<double,4,1> oldtrans; // homogeneous coordinates, last element is 1.0
534  Eigen::Matrix<double,4,1> oldqrot; // this is the quaternion as coefficients, note xyzw order
535 
536  };
537 
539 
541  bool readP2File(char *fname, SysSPA spa);
542 
543 
544 } // ends namespace sba
545 
546 #endif // _SBA_H_
547 
Eigen::VectorXd B
Definition: sba.h:526
int nFixed
Number of fixed nodes (nodes with known poses) from the first node.
Definition: sba.h:92
#define SBA_SPARSE_CHOLESKY
Definition: sba.h:67
EIGEN_MAKE_ALIGNED_OPERATOR_NEW int verbose
How much information to print to console.
Definition: sba.h:81
SysSPA holds a set of nodes and constraints for sparse pose adjustment.
Definition: sba.h:440
bool readP2File(char *fname, SysSPA spa)
constraint files
Definition: spa.cpp:78
void setupSparseSys(double sLambda, int iter, int sparseType)
Definition: sba.cpp:1165
void setConnMat(int minpts)
Definition: sba.cpp:720
bool isValid
valid or not (could be out of bounds)
Definition: sba.h:386
int removeBad(double dist)
Remove projections with too high of an error.
Definition: sba.cpp:446
Eigen::Matrix< double, 12, 1 > err
error
Definition: sba.h:410
EIGEN_MAKE_ALIGNED_OPERATOR_NEW SysSPA()
constructor
Definition: sba.h:446
Eigen::MatrixXd A
linear system matrix and vector
Definition: sba.h:240
int numBadPoints()
Find number of points with Z < 0 (projected behind the camera).
Definition: sba.cpp:389
long long t4
Definition: sba.h:82
SysSBA holds a set of nodes and points for sparse bundle adjustment.
Definition: sba.h:75
int nd2
Definition: sba.h:403
long long t0
Definition: sba.h:82
Eigen::Matrix< double, 6, 6 > prec
Definition: sba.h:311
vector< map< int, int > > generateConns_()
Definition: sba.cpp:659
Eigen::Matrix< double, 6, 6 > J1t
Definition: sba.h:324
void useCholmod(bool yes)
use CHOLMOD or CSparse
Definition: sba.h:265
bool isValid
valid or not (could be out of bounds)
Definition: sba.h:433
std::vector< JacobProds, Eigen::aligned_allocator< JacobProds > > jps
storage for Jacobian products
Definition: sba.h:288
int nd1
Node index for the second node.
Definition: sba.h:306
double sqMinDelta
Convergence bound (square of minimum acceptable delta change)
Definition: sba.h:515
int countBad(double dist)
Find number of projections with errors over a certain value.
Definition: sba.cpp:417
void updateNormals()
Update normals in point-plane matches, if any.
Definition: sba.cpp:239
Eigen::Matrix< double, 4, 1 > oldqrot
Definition: sba.h:534
CSparse csp
Definition: sba.h:270
Eigen::Vector3d tmean
Mean vector, quaternion (inverse) and precision matrix for this constraint.
Definition: sba.h:309
int nd1
Node index for the second node.
Definition: sba.h:358
double lambda
Definition: sba.h:148
double calcCost()
Calculate the total cost of the system by adding the squared error over all projections.
Definition: sba.cpp:289
Eigen::Matrix< double, 12, 1 > mean
Mean vector and precision matrix for this constraint.
Definition: sba.h:406
std::vector< double > scales
set of scale for SPA system, indexed by position;
Definition: sba.h:477
double err
error
Definition: sba.h:370
EIGEN_MAKE_ALIGNED_OPERATOR_NEW int ndr
Reference pose index.
Definition: sba.h:400
int reduceLongTracks(double pct)
get rid of long tracks
Definition: sba.cpp:867
double w
Weight for this constraint.
Definition: sba.h:367
int mergeTracksSt(int tr0, int tr1)
merge two tracks if possible (no conflicts)
Definition: sba.cpp:1660
bool addMonoProj(int ci, int pi, Eigen::Vector2d &q)
Add a projection between point and camera, in setting up the system.
Definition: sba.cpp:145
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
std::vector< Track, Eigen::aligned_allocator< Track > > tracks
Set of tracks for each point P (projections onto camera frames).
Definition: sba.h:95
static void initDr()
Sets up constant matrices for derivatives.
Definition: node.cpp:23
Eigen::Vector3d J0
Definition: sba.h:376
int doSBA(int niter, double lambda=1.0e-4, int useCSparse=0, double initTol=1.0e-8, int maxCGiters=100)
Definition: sba.cpp:1312
std::vector< Point, Eigen::aligned_allocator< Point > > oldpoints
Storage for old points, for checking LM step and reverting.
Definition: sba.h:282
std::vector< Node, Eigen::aligned_allocator< Node > > nodes
Set of nodes (camera frames) for SBA system, indexed by node number.
Definition: sba.h:89
double calcRMSCost(double dist=10000.0)
Calculates total RMS cost of the system, not counting projections with errors higher than <dist>...
Definition: sba.cpp:336
SysSBA()
Default constructor.
Definition: sba.h:85
double lambda
Definition: sba.h:504
double calcAvgError()
Calculates average cost of the system.
Definition: sba.cpp:365
bool verbose
print info
Definition: sba.h:450
Definition: bpcg.h:60
long long t3
Definition: sba.h:82
std::vector< std::vector< bool > > connMat
Definition: sba.h:246
CSparse csp
sparse matrix object
Definition: sba.h:529
bool useLocalAngles
local or global angle coordinates
Definition: sba.h:144
int addNode(Eigen::Matrix< double, 4, 1 > &trans, Eigen::Quaternion< double > &qrot, const fc::CamParams &cp, bool isFixed=false)
Adds a node to the system.
Definition: sba.cpp:83
void writeSparseA(const char *fname, SysSBA &sba)
Eigen::Matrix< double, 6, 6 > J22
Definition: sba.h:415
Eigen::Matrix< double, 6, 3 > Tpc
Definition: sba.h:430
Eigen::VectorXd B
Definition: sba.h:241
int remExcessTracks(int minpts)
removes tracks that aren&#39;t needed
Definition: sba.cpp:950
Eigen::Vector3d J1
Definition: sba.h:380
int nFixed
Number of fixed nodes.
Definition: sba.h:480
bool isValid
valid or not (could be out of bounds)
Definition: sba.h:342
std::vector< Eigen::Vector3d, Eigen::aligned_allocator< Eigen::Vector3d > > tps
variables for each track
Definition: sba.h:285
long long t2
Definition: sba.h:82
bool addStereoProj(int ci, int pi, Eigen::Vector3d &q)
Add a projection between point and camera, in setting up the system.
Definition: sba.cpp:160
bool addProj(int ci, int pi, Eigen::Vector3d &q, bool stereo=true)
Add a projection between point and camera, in setting up the system.
Definition: sba.cpp:115
long long t1
Definition: sba.h:82
bool useCholmod
Definition: csparse.h:124
int sv
Scale variable index.
Definition: sba.h:361
std::vector< Node, Eigen::aligned_allocator< Node > > nodes
set of nodes (camera frames) for SPA system, indexed by position;
Definition: sba.h:474
Eigen::MatrixXd A
linear system matrix and vector
Definition: sba.h:525
double huber
Huber parameter; greater than 0.0 for Huber weighting of cost.
Definition: sba.h:113
int addPoint(Point p)
Adds a point to the system.
Definition: sba.cpp:104
double sqMinDelta
Convergence bound (square of minimum acceptable delta change)
Definition: sba.h:162
void printStats()
Print some system stats.
Definition: sba.cpp:505
Eigen::Matrix< double, 4, 1 > oldtrans
Definition: sba.h:533
Eigen::Vector4d Point
Point holds 3D points using in world coordinates. Currently we just represent these as 4-vectors...
Definition: node.h:43
Eigen::Matrix< double, 6, 1 > err
error
Definition: sba.h:315
Eigen::Matrix< double, 12, 12 > prec
Definition: sba.h:407
int countProjs()
Return total number of projections.
Definition: sba.cpp:275
void setupSys(double sLambda)
Definition: sba.cpp:1051
void tsplit(int tri, int len)
Split a track into random tracks. (What is len?)
Definition: sba.cpp:815
EIGEN_MAKE_ALIGNED_OPERATOR_NEW int nd0
Reference pose index.
Definition: sba.h:355
void mergeTracks(std::vector< std::pair< int, int > > &prs)
merge tracks based on identity pairs
Definition: sba.cpp:1592
Eigen::Matrix< double, 3, 6 > Hpc
temp storage for Hpc, Tpc matrices in SBA
Definition: sba.h:429
Eigen::Quaternion< double > qpmean
Definition: sba.h:310
std::vector< ConP2, Eigen::aligned_allocator< ConP2 > > p2cons
Set of P2 constraints.
Definition: sba.h:483
bool useLocalAngles
local or global angle coordinates
Definition: sba.h:489
void addPointPlaneMatch(int ci0, int pi0, Eigen::Vector3d normal0, int ci1, int pi1, Eigen::Vector3d normal1)
Adds a pair of point-plane projection matches. Assumes the points have already been added to the syst...
Definition: sba.cpp:191
int reduceTracks()
Reduce tracks by eliminating single tracks and invalid projections.
Definition: sba.cpp:472
double ks
Scale factor for this constraint.
Definition: sba.h:364
void setProjCovariance(int ci, int pi, Eigen::Matrix3d &covar)
Sets the covariance matrix of a projection.
Definition: sba.cpp:184
EIGEN_MAKE_ALIGNED_OPERATOR_NEW int ndr
Reference pose index.
Definition: sba.h:303
std::vector< ConScale, Eigen::aligned_allocator< ConScale > > scons
Set of scale constraints.
Definition: sba.h:486
void setConnMatReduced(int maxconns)
sets up connectivity matrix by greedy spanning tree
Definition: sba.cpp:768


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