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_
sba::Node::setKcam
void setKcam(const fc::CamParams &cp)
Sets the Kcam and baseline matrices from frame_common::CamParams.
Definition: node.h:98
sba::Node::setDr
void setDr(bool local=false)
Set angle derivates.
Definition: node.cpp:56
sba::transformN2N
void transformN2N(Eigen::Matrix< double, 4, 1 > &trans, Eigen::Quaternion< double > &qrot, Node &nd0, Node &nd1)
Definition: node.cpp:177
sba::Point
Eigen::Vector4d Point
Point holds 3D points using in world coordinates. Currently we just represent these as 4-vectors,...
Definition: node.h:43
frame_common::CamParams
Definition: node.h:21
sba::Node::oldqrot
Eigen::Quaternion< double > oldqrot
Previous quaternion rotation, saved for downdating within LM.
Definition: node.h:147
sba::Node::normRot
void normRot()
Normalize quaternion to unit. Problem with global derivatives near w=0, solved by a hack for now.
Definition: node.cpp:37
sba::Node::setProjection
void setProjection()
Definition: node.h:121
sba::Node::dRdy
Eigen::Matrix< double, 3, 3 > dRdy
Definition: node.h:126
frame_common
Definition: node.h:15
sba::Node::Kcam
Eigen::Matrix< double, 3, 3 > Kcam
Projection to frame image coordinates. pre-multiply by the frame camera matrix.
Definition: node.h:92
sba::Node::baseline
double baseline
baseline for stereo
Definition: node.h:93
sba::Node::dRidz
static Eigen::Matrix3d dRidz
Definition: node.h:129
sba::Node::initDr
static void initDr()
Sets up constant matrices for derivatives.
Definition: node.cpp:23
frame_common::CamParams::tx
double tx
Definition: node.h:23
sba::Node::isFixed
bool isFixed
For SBA, is this camera fixed or free?
Definition: node.h:141
sba::Node::setDri
void setDri()
Set local angle derivatives.
Definition: node.cpp:14
sba::Keypoint
Eigen::Vector3d Keypoint
Keypoints - subpixel using floats. u,v are pixel coordinates, d is disparity (if stereo)
Definition: node.h:37
sba::Node::project2im
void project2im(Eigen::Vector2d &pi, const Point &p) const
Definition: node.h:116
sba
Definition: bpcg.h:60
sba::Node::w2n
Eigen::Matrix< double, 3, 4 > w2n
Resultant transform from world to node coordinates.
Definition: node.h:80
sba::Node::qrot
Eigen::Quaternion< double > qrot
Rotation of the node expressed as a Quaternion.
Definition: node.h:70
sba::Node::oldtrans
Eigen::Matrix< double, 4, 1 > oldtrans
Previous translation, saved for downdating within LM.
Definition: node.h:144
frame_common::CamParams::fy
double fy
Definition: node.h:23
sba::Node::dRidx
static Eigen::Matrix3d dRidx
Constant matrices for derivatives.
Definition: node.h:129
sba::Node::projectMono
void projectMono(const Point &pt, Eigen::Vector3d &proj)
Project a 3D point into the image frame as a monocular point.
Definition: node.cpp:134
sba::transformW2F
void transformW2F(Eigen::Matrix< double, 3, 4 > &m, const Eigen::Matrix< double, 4, 1 > &trans, const Eigen::Quaternion< double > &qrot)
Definition: node.cpp:157
sba::Node::setTransform
void setTransform()
Sets the transform matrices of the node.
Definition: node.cpp:8
frame_common::CamParams::cy
double cy
Definition: node.h:23
sba::Node::dRidy
static Eigen::Matrix3d dRidy
Definition: node.h:129
sba::Node::normRotLocal
void normRotLocal()
Normalize quaternion to unit, without w=0 considerations.
Definition: node.cpp:122
frame_common::CamParams::fx
double fx
Definition: node.h:23
sba::Node::trans
EIGEN_MAKE_ALIGNED_OPERATOR_NEW Eigen::Matrix< double, 4, 1 > trans
Translation in homogeneous coordinates, last element is 1.0.
Definition: node.h:69
sba::transformF2W
void transformF2W(Eigen::Matrix< double, 3, 4 > &m, const Eigen::Matrix< double, 4, 1 > &trans, const Eigen::Quaternion< double > &qrot)
Definition: node.cpp:166
sba::Node::projectStereo
void projectStereo(const Point &pt, Eigen::Vector3d &proj)
Project a 3D point into the image frame as a stereo point.
Definition: node.cpp:142
sba::Node::dRdz
Eigen::Matrix< double, 3, 3 > dRdz
Definition: node.h:126
sba::Node
NODE holds graph nodes corresponding to frames, for use in sparse bundle adjustment....
Definition: node.h:63
sba::Node::dRdx
Eigen::Matrix< double, 3, 3 > dRdx
Derivatives of the rotation matrix transpose wrt quaternion xyz, used for calculating Jacobian wrt po...
Definition: node.h:126
sba::Node::w2i
Eigen::Matrix< double, 3, 4 > w2i
The transform from world to image coordinates.
Definition: node.h:111
frame_common::CamParams::cx
double cx
Definition: node.h:23


sparse_bundle_adjustment
Author(s):
autogenerated on Wed Mar 2 2022 01:03:04