spa2d.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 pose adjustment, 2d version
37 //
38 
39 #ifndef _SPA2D_H_
40 #define _SPA2D_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 #ifndef EIGEN_USE_NEW_STDVECTOR
52 #define EIGEN_USE_NEW_STDVECTOR
53 #endif
54 #include <Eigen/StdVector>
55 #include <vector>
56 
57 // sparse Cholesky
59 // block jacobian pcg
61 
62 // Defines for methods to use with doSBA().
63 #define SBA_DENSE_CHOLESKY 0
64 #define SBA_SPARSE_CHOLESKY 1
65 #define SBA_GRADIENT 2
66 #define SBA_BLOCK_JACOBIAN_PCG 3
67 
68 
69 // put things into a namespace
70 namespace sba
71 {
72 
75 
93 
94  class Node2d
95  {
96  public:
97  EIGEN_MAKE_ALIGNED_OPERATOR_NEW // needed for 16B alignment
98 
100  int nodeId;
101 
103  Eigen::Matrix<double,3,1> trans; // homogeneous coordinates, last element is 1.0
104  double arot; // angle in radians, normalized to [-pi,+pi]
106  inline void normArot()
107  {
108  if (arot > M_PI) arot -= 2.0*M_PI;
109  if (arot < -M_PI) arot += 2.0*M_PI;
110  }
111 
113  Eigen::Matrix<double,2,3> w2n;
114  void setTransform();
115 
119  // Eigen::Matrix<double,3,3> covar;
120 
123  Eigen::Matrix2d dRdx;
124 
125  void setDr(); // set local angle derivatives
126 
128  bool isFixed;
129 
132  Eigen::Matrix<double,3,1> oldtrans; // homogeneous coordinates, last element is 1.0
133  double oldarot; // angle
134  };
135 
136 
140 
141  class Con2dP2
142  {
143  public:
144  EIGEN_MAKE_ALIGNED_OPERATOR_NEW // needed for 16B alignment
145 
147  int ndr;
148 
150  int nd1;
151 
153  Eigen::Vector2d tmean;
154  double amean;
155  Eigen::Matrix<double,3,3> prec;
156 
158  Eigen::Matrix<double,3,1> err;
160  inline double calcErr(const Node2d &nd0, const Node2d &nd1);
161 
163  double calcErrDist(const Node2d &nd0, const Node2d &nd1);
164 
165 
167  Eigen::Matrix<double,3,3> J0,J0t,J1,J1t;
168 
171  static constexpr double qScale = 1.0;
172 
175 
178 
182  void setJacobians(std::vector<Node2d,Eigen::aligned_allocator<Node2d> > &nodes);
183 
185  bool isValid;
186  };
187 
188 
189 
191 
192  class SysSPA2d
193  {
194  public:
195  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
196 
198  SysSPA2d() { nFixed = 1; verbose = false; lambda = 1.0e-4, print_iros_stats=false; }
199 
202  int addNode(const Vector3d &pos, int id);
203  void removeNode(int id);
204 
205  // add a constraint
206  // <nd0>, <nd1> are node id's
207  // <mean> is x,y,th, with th in radians
208  // <prec> is a 3x3 precision matrix (inverse covariance
209  bool addConstraint(int nd0, int nd1, const Vector3d &mean, const Matrix3d &prec);
210  bool removeConstraint(int ndi0, int ndi1);
211 
212 
214  std::vector<Node2d,Eigen::aligned_allocator<Node2d> > nodes;
215  std::vector<Node2d,Eigen::aligned_allocator<Node2d> > getNodes()
216  { return nodes; }
217 
219  std::vector< std::vector< Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d> > > scans;
220 
222  int nFixed;
223 
225  std::vector<Con2dP2,Eigen::aligned_allocator<Con2dP2> > p2cons;
226 
229  double calcCost(bool tcost = false);
231  double calcCost(double dist);
232  double errcost;
233 
234 
236  void printStats();
237  void writeSparseA(char *fname, bool useCSparse = false); // save precision matrix in CSPARSE format
238 
241  void setupSys(double sLambda);
242  void setupSparseSys(double sLambda, int iter, int sparseType);
243 
248  double lambda;
249  int doSPA(int niter, double sLambda = 1.0e-4, int useCSparse = SBA_SPARSE_CHOLESKY, double initTol = 1.0e-8, int CGiters = 50);
250  int doSPAwindowed(int window, int niter, double sLambda, int useCSparse);
251 
252 
253 #ifdef SBA_DSIF
254  void doDSIF(int newnode);
256  void setupSparseDSIF(int newnode);
257 #endif
258 
260  double sqMinDelta;
261 
263  Eigen::MatrixXd A;
264  Eigen::VectorXd B;
265 
268 
270  void useCholmod(bool yes)
271  { csp.useCholmod = yes; }
272 
274  bool verbose;
276 
279  void getGraph(std::vector<float> &graph);
280  };
281 
282 
283 
284 
286 
288  bool read2dP2File(char *fname, SysSPA2d spa);
289 
290 
291 } // ends namespace sba
292 
293 #endif // _SPA2d_H_
bool print_iros_stats
Definition: spa2d.h:275
Eigen::VectorXd B
Definition: spa2d.h:264
double sqMinDelta
Convergence bound (square of minimum acceptable delta change)
Definition: spa2d.h:260
EIGEN_MAKE_ALIGNED_OPERATOR_NEW SysSPA2d()
constructor
Definition: spa2d.h:198
Eigen::Vector2d tmean
Mean vector, quaternion (inverse) and precision matrix for this constraint.
Definition: spa2d.h:153
std::vector< std::vector< Eigen::Vector2d, Eigen::aligned_allocator< Eigen::Vector2d > > > scans
set of point scans, corresponding to nodes
Definition: spa2d.h:219
std::vector< Node2d, Eigen::aligned_allocator< Node2d > > getNodes()
Definition: spa2d.h:215
bool isFixed
For SPA, is this camera fixed or free?
Definition: spa2d.h:128
void useCholmod(bool yes)
use CHOLMOD or CSparse
Definition: spa2d.h:270
double lambda
Definition: spa2d.h:248
double oldarot
Definition: spa2d.h:133
int nd1
Node2d index for the second node.
Definition: spa2d.h:150
EIGEN_MAKE_ALIGNED_OPERATOR_NEW int nodeId
node id - somewhat redundant, but can be useful, e.g., in KARTO links
Definition: spa2d.h:100
double arot
Definition: spa2d.h:104
Eigen::Matrix< double, 3, 3 > J1t
Definition: spa2d.h:167
Eigen::Matrix< double, 3, 3 > prec
Definition: spa2d.h:155
double errcost
Definition: spa2d.h:232
double amean
Definition: spa2d.h:154
Eigen::Matrix2d dRdx
Definition: spa2d.h:123
#define SBA_SPARSE_CHOLESKY
Definition: spa2d.h:64
void setTransform()
Definition: spa2d.cpp:67
Eigen::Matrix< double, 3, 1 > trans
6DOF pose as a unit quaternion and translation vector
Definition: spa2d.h:103
Definition: bpcg.h:60
Eigen::Matrix< double, 3, 1 > err
error
Definition: spa2d.h:158
void writeSparseA(const char *fname, SysSBA &sba)
void normArot()
Normalize to [-pi,+pi].
Definition: spa2d.h:106
bool verbose
if we want statistics
Definition: spa2d.h:274
Eigen::Matrix< double, 3, 1 > oldtrans
Definition: spa2d.h:132
bool isValid
valid or not (could be out of bounds)
Definition: spa2d.h:185
Eigen::Matrix< double, 2, 3 > w2n
Resultant transform from world to node coordinates;.
Definition: spa2d.h:113
std::vector< Con2dP2, Eigen::aligned_allocator< Con2dP2 > > p2cons
Set of P2 constraints.
Definition: spa2d.h:225
std::vector< Node2d, Eigen::aligned_allocator< Node2d > > nodes
set of nodes (camera frames) for SPA system, indexed by position;
Definition: spa2d.h:214
CSparse2d csp
sparse matrix object
Definition: spa2d.h:267
EIGEN_MAKE_ALIGNED_OPERATOR_NEW int ndr
Reference pose index.
Definition: spa2d.h:147
SysSPA2d holds a set of nodes and constraints for sparse pose adjustment.
Definition: spa2d.h:192
bool useCholmod
Definition: csparse.h:189
void setDr()
Definition: spa2d.cpp:80
bool read2dP2File(char *fname, SysSPA2d spa)
constraint files
Eigen::MatrixXd A
linear system matrix and vector
Definition: spa2d.h:263
int nFixed
Number of fixed nodes.
Definition: spa2d.h:222


sparse_bundle_adjustment
Author(s):
autogenerated on Fri Mar 15 2019 02:41:46