proj.h
Go to the documentation of this file.
1 #ifndef _PROJ_H_
2 #define _PROJ_H_
3 
4 #ifndef EIGEN_USE_NEW_STDVECTOR
5 #define EIGEN_USE_NEW_STDVECTOR
6 #endif // EIGEN_USE_NEW_STDVECTOR
7 
8 #include <Eigen/Core>
9 #include <Eigen/Geometry>
10 #include <Eigen/LU>
11 #include <Eigen/StdVector>
12 #include <map>
13 
15 
16 namespace sba
17 {
18  class JacobProds
19  {
20  public:
21  EIGEN_MAKE_ALIGNED_OPERATOR_NEW // needed for 16B alignment
22 
24  {
25  Hpp.setZero();
26  Hpc.setZero();
27  Hcc.setZero();
28  Bp.setZero();
29  JcTE.setZero();
30  }
31 
33  Eigen::Matrix<double,3,3> Hpp;
34 
36  Eigen::Matrix<double,3,6> Hpc;
37 
39  Eigen::Matrix<double,6,6> Hcc;
40 
42  Eigen::Matrix<double,3,1> Bp;
43 
45  Eigen::Matrix<double,6,1> JcTE;
46  };
47 
48  class Proj; // Forward reference.
49 
52  typedef std::map<const int, Proj, std::less<int>, Eigen::aligned_allocator<Proj> > ProjMap;
53 
57  class Proj
58  {
59  public:
60 
61  EIGEN_MAKE_ALIGNED_OPERATOR_NEW // needed for 16B alignment
62 
66  Proj(int ci, Eigen::Vector3d &q, bool stereo = true);
67 
71  Proj(int ci, Eigen::Vector2d &q);
72 
75  Proj();
76 
78  int ndi;
79 
81  Eigen::Vector3d kp;
82 
84  Eigen::Vector3d err;
85 
87  bool stereo;
88 
90  double calcErr(const Node &nd, const Point &pt, double huber = 0.0);
91 
94  double getErrSquaredNorm();
95 
98  double getErrNorm();
99 
101 
117  void setJacobians(const Node &nd, const Point &pt, JacobProds *jpp);
118 
121 
124  Eigen::Matrix<double,6,3> Tpc;
125 
127  bool isValid;
128 
131  static constexpr double qScale = 1.0;
132 
134  bool useCovar;
135 
137  Eigen::Matrix<double,3,3> covarmat;
138 
141 
143  Eigen::Vector3d plane_normal;
144 
146  Eigen::Vector3d plane_point;
147 
150 
153 
155  Eigen::Vector3d plane_local_normal;
156 
162  void setCovariance(const Eigen::Matrix3d &covar);
163 
165  void clearCovariance();
166 
167 
168  protected:
170  void setJacobiansMono_(const Node &nd, const Point &pt, JacobProds *jpp);
171 
173  void setJacobiansStereo_(const Node &nd, const Point &pt, JacobProds *jpp);
174 
176  double calcErrMono_(const Node &nd, const Point &pt, double huber);
177 
179  double calcErrStereo_(const Node &nd, const Point &pt, double huber);
180  };
181 
182  class Track
183  {
184  public:
186  Track(Point p);
187 
189  Track();
190 
193  ProjMap projections;
194 
198  };
199 
200 
201 } // sba
202 
203 #endif // _PROJ_H
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
std::map< const int, Proj, std::less< int >, Eigen::aligned_allocator< Proj > > ProjMap
Definition: proj.h:48
Point point
An Eigen 4-vector containing the <x, y, z, w> coordinates of the point associated with the track...
Definition: proj.h:197
Eigen::Matrix< double, 6, 6 > Hcc
Camera-to-camera Hessian (JcT*Jc)
Definition: proj.h:39
bool useCovar
Use a covariance matrix?
Definition: proj.h:134
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
Eigen::Matrix< double, 3, 1 > Bp
The B matrix with respect to points (JpT*Err)
Definition: proj.h:42
EIGEN_MAKE_ALIGNED_OPERATOR_NEW JacobProds()
Definition: proj.h:23
int plane_point_index
Point-plane match point index in SBA.
Definition: proj.h:149
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
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
Definition: bpcg.h:60
ProjMap projections
A map of all the projections of the point with camera index as key, based off an STL map...
Definition: proj.h:193
Proj holds a projection measurement of a point onto a frame. They are a repository for the link betwe...
Definition: proj.h:57
int ndi
Node index, the camera node for this projection.
Definition: proj.h:78
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::Vector3d kp
Keypoint, u,v,u-d vector.
Definition: proj.h:81
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
Eigen::Matrix< double, 3, 3 > covarmat
Covariance matrix for cost calculation.
Definition: proj.h:137
Eigen::Vector3d plane_normal
Normal for point-plane projections.
Definition: proj.h:143


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