node.h
Go to the documentation of this file.
1 #ifndef _NODE_H_
2 #define _NODE_H_
3 
4 // Functions for handling nodes (cameras) within the sba system.
5 
6 #include <stdio.h>
7 #include <iostream>
8 #include <Eigen/Core>
9 #include <Eigen/Geometry>
10 
11 // needed for camera params
12 #ifndef _CAMPARAMS_H_
13 #define _CAMPARAMS_H_
14 
15 namespace frame_common
16 {
17 
21  struct CamParams
22  {
23  double fx, fy, cx, cy, tx;
24  };
25 } // end namespace
26 
27 #endif // _CAMPARMS_H_
28 
29 namespace fc = frame_common;
30 
31 // put things into a namespace
32 namespace sba
33 {
34 
37  typedef Eigen::Vector3d Keypoint;
38 
39 
43  typedef Eigen::Vector4d Point;
44 
45 
63  class Node
64  {
65  public:
66  EIGEN_MAKE_ALIGNED_OPERATOR_NEW // needed for 16B alignment
67 
68  // 6DOF pose as a unit quaternion and translation vector
69  Eigen::Matrix<double,4,1> trans;
70  Eigen::Quaternion<double> qrot;
71 
74  void normRot();
75 
77  void normRotLocal();
78 
80  Eigen::Matrix<double,3,4> w2n;
81 
83  void setTransform();
84 
85  // Covariance matrix, 6x6. Variables are [trans,rot], with the
86  // rotational part being the xyz parameters of the unit
87  // quaternion
88  // Eigen::Matrix<double,6,6> covar;
89 
92  Eigen::Matrix<double,3,3> Kcam;
93  double baseline;
94 
95 
98  void setKcam(const fc::CamParams &cp)
99  {
100  Kcam.setZero();
101  Kcam(0,0) = cp.fx;
102  Kcam(1,1) = cp.fy;
103  Kcam(0,2) = cp.cx;
104  Kcam(1,2) = cp.cy;
105  Kcam(2,2) = 1.0;
106  baseline = cp.tx;
107  setProjection();
108  }
109 
111  Eigen::Matrix<double,3,4> w2i;
112 
116  void project2im(Eigen::Vector2d &pi, const Point &p) const
117  { Eigen::Vector3d p1 = w2i * p; pi = p1.head(2)/p1(2); }
118 
122  { w2i = Kcam * w2n; }
123 
126  Eigen::Matrix<double,3,3> dRdx, dRdy, dRdz;
127 
129  static Eigen::Matrix3d dRidx, dRidy, dRidz; // these are constant
130 
132  static void initDr(); // call this to set up constant matrices
133 
135  void setDr(bool local = false);
136 
138  void setDri();
139 
141  bool isFixed;
142 
144  Eigen::Matrix<double,4,1> oldtrans;
145 
147  Eigen::Quaternion<double> oldqrot;
148 
150  void projectMono(const Point& pt, Eigen::Vector3d& proj);
151 
153  void projectStereo(const Point& pt, Eigen::Vector3d& proj);
154  };
155 
156 
157  // Functions to create transformations
158 
164  void transformW2F(Eigen::Matrix<double,3,4> &m,
165  const Eigen::Matrix<double,4,1> &trans,
166  const Eigen::Quaternion<double> &qrot);
167 
168 
171  void transformF2W(Eigen::Matrix<double,3,4> &m,
172  const Eigen::Matrix<double,4,1> &trans,
173  const Eigen::Quaternion<double> &qrot);
174 
175 
178  void transformN2N(Eigen::Matrix<double,4,1> &trans,
179  Eigen::Quaternion<double> &qrot,
180  Node &nd0, Node &nd1);
181 
182 } // sba namespace
183 
184 #endif // _NODE_H_
void setKcam(const fc::CamParams &cp)
Sets the Kcam and baseline matrices from frame_common::CamParams.
Definition: node.h:98
Eigen::Matrix< double, 3, 4 > w2i
The transform from world to image coordinates.
Definition: node.h:111
Eigen::Matrix< double, 3, 3 > Kcam
Projection to frame image coordinates. pre-multiply by the frame camera matrix.
Definition: node.h:92
Eigen::Quaternion< double > oldqrot
Previous quaternion rotation, saved for downdating within LM.
Definition: node.h:147
void transformW2F(Eigen::Matrix< double, 3, 4 > &m, const Eigen::Matrix< double, 4, 1 > &trans, const Eigen::Quaternion< double > &qrot)
Definition: node.cpp:157
bool isFixed
For SBA, is this camera fixed or free?
Definition: node.h:141
void transformF2W(Eigen::Matrix< double, 3, 4 > &m, const Eigen::Matrix< double, 4, 1 > &trans, const Eigen::Quaternion< double > &qrot)
Definition: node.cpp:166
Eigen::Matrix< double, 4, 1 > oldtrans
Previous translation, saved for downdating within LM.
Definition: node.h:144
void setProjection()
Definition: node.h:121
double baseline
baseline for stereo
Definition: node.h:93
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
static Eigen::Matrix3d dRidz
Definition: node.h:129
Eigen::Matrix< double, 3, 4 > w2n
Resultant transform from world to node coordinates.
Definition: node.h:80
Definition: bpcg.h:60
void transformN2N(Eigen::Matrix< double, 4, 1 > &trans, Eigen::Quaternion< double > &qrot, Node &nd0, Node &nd1)
Definition: node.cpp:177
Eigen::Quaternion< double > qrot
Rotation of the node expressed as a Quaternion.
Definition: node.h:70
Eigen::Vector3d Keypoint
Keypoints - subpixel using floats. u,v are pixel coordinates, d is disparity (if stereo) ...
Definition: node.h:37
EIGEN_MAKE_ALIGNED_OPERATOR_NEW Eigen::Matrix< double, 4, 1 > trans
Translation in homogeneous coordinates, last element is 1.0.
Definition: node.h:69
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 > dRdz
Definition: node.h:126
void project2im(Eigen::Vector2d &pi, const Point &p) const
Definition: node.h:116


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