node.cpp
Go to the documentation of this file.
2 
3 using namespace std;
4 using namespace Eigen;
5 
6 namespace sba
7 {
8  void Node::setTransform()
9  { transformW2F(w2n,trans,qrot); }
10 
11  //
12  // sets incremental angle derivatives
13  //
14  void Node::setDri()
15  {
16  setDr(true);
17  }
18 
19  // constant derivative matrices
20  // these are the derivatives of the *inverse* rotation
21  Eigen::Matrix3d Node::dRidx, Node::dRidy, Node::dRidz;
22 
23  void Node::initDr()
24  {
25  dRidx << 0.0,0.0,0.0,
26  0.0,0.0,2.0,
27  0.0,-2.0,0.0;
28  dRidy << 0.0,0.0,-2.0,
29  0.0,0.0,0.0,
30  2.0,0.0,0.0;
31  dRidz << 0.0,2.0,0.0,
32  -2.0,0.0,0.0,
33  0.0,0.0,0.0;
34  }
35 
36 
37  void Node::normRot()
38  {
39  // std::cout << "[NormRot] qrot start = " << qrot.transpose() << std::endl;
40  if (qrot.w() < 0) qrot.coeffs().head<3>() = -qrot.coeffs().head<3>();
41  double sn = qrot.coeffs().head<3>().squaredNorm();
42  if (sn >= 0.9999) // too close to high derivatives
43  qrot.coeffs().head<3>() *= -1.0/(sqrt(sn)*1.0001); // switch sides; 1e-4 seems to work well
44  qrot.w() = sqrt(1.0 - qrot.coeffs().head<3>().squaredNorm());
45  if (isnan(qrot.x()) || isnan(qrot.y()) || isnan(qrot.z()) || isnan(qrot.w()) )
46  {
47  printf("[NormRot] Bad quaternion: %f %f %f %f\n", qrot.x(), qrot.y(), qrot.z(), qrot.w());
48  exit(1);
49  }
50  // std::cout << "[NormRot] qrot end = " << qrot.transpose() << std::endl;
51  }
52 
53  //
54  // sets angle derivatives
55  //
56  void Node::setDr(bool local)
57  {
58  // for dS'*R', with dS the incremental change
59  if (local)
60  {
61 #if 0
62  dRdx = w2n.block<3,3>(0,0) * dRidx;
63  dRdy = w2n.block<3,3>(0,0) * dRidy;
64  dRdz = w2n.block<3,3>(0,0) * dRidz;
65 #endif
66  dRdx = dRidx * w2n.block<3,3>(0,0);
67  dRdy = dRidy * w2n.block<3,3>(0,0);
68  dRdz = dRidz * w2n.block<3,3>(0,0);
69 
70  }
71  else
72  {
73  double x2 = qrot.x() * 2.0;
74  double y2 = qrot.y() * 2.0;
75  double z2 = qrot.z() * 2.0;
76  double w2 = qrot.w() * 2.0;
77  double xw = qrot.x()/qrot.w(); // these are problematic for w near zero
78  double yw = qrot.y()/qrot.w();
79  double zw = qrot.z()/qrot.w();
80 
81  // dR/dx
82  dRdx(0,0) = 0.0;
83  dRdx(0,1) = y2-zw*x2;
84  dRdx(0,2) = z2+yw*x2;
85 
86  dRdx(1,0) = y2+zw*x2;
87  dRdx(1,1) = -2.0*x2;
88  dRdx(1,2) = w2-xw*x2;
89 
90  dRdx(2,0) = z2-yw*x2;
91  dRdx(2,1) = -dRdx(1,2);
92  dRdx(2,2) = dRdx(1,1);
93 
94  // dR/dy
95  dRdy(0,0) = -2.0*y2;
96  dRdy(0,1) = x2-zw*y2;
97  dRdy(0,2) = (-w2)+yw*y2;
98 
99  dRdy(1,0) = x2+zw*y2;
100  dRdy(1,1) = 0.0;
101  dRdy(1,2) = dRdx(2,0);
102 
103  dRdy(2,0) = -dRdy(0,2);
104  dRdy(2,1) = dRdx(0,2);
105  dRdy(2,2) = dRdy(0,0);
106 
107  // dR/dz
108  dRdz(0,0) = -2.0*z2;
109  dRdz(0,1) = w2-zw*z2;
110  dRdz(0,2) = dRdy(1,0);
111 
112  dRdz(1,0) = -dRdz(0,1);
113  dRdz(1,1) = dRdz(0,0);
114  dRdz(1,2) = dRdx(0,1);
115 
116  dRdz(2,0) = dRdy(0,1);
117  dRdz(2,1) = dRdx(1,0);
118  dRdz(2,2) = 0.0;
119  }
120  }
121 
122  void Node::normRotLocal()
123  {
124  qrot.normalize();
125  if (qrot.w() < 0) qrot.coeffs().head<3>() = -qrot.coeffs().head<3>();
126  if (isnan(qrot.x()) || isnan(qrot.y()) || isnan(qrot.z()) || isnan(qrot.w()) )
127  {
128  printf("[NormRot] Bad quaternion in normRotLocal(): %f %f %f %f\n", qrot.x(), qrot.y(), qrot.z(), qrot.w());
129  exit(1);
130  }
131  // std::cout << "[NormRot] qrot end = " << qrot.transpose() << std::endl;
132  }
133 
134  void Node::projectMono(const Point& point, Eigen::Vector3d& proj)
135  {
136  Vector2d proj2d;
137  project2im(proj2d, point);
138 
139  proj.head<2>() = proj2d;
140  }
141 
142  void Node::projectStereo(const Point& point, Eigen::Vector3d& proj)
143  {
144  Vector2d proj2d;
145  Vector3d pc, baseline_vect;
146  project2im(proj2d, point);
147 
148  // Camera coords for right camera
149  baseline_vect << baseline, 0, 0;
150  pc = Kcam * (w2n*point - baseline_vect);
151  proj.head<2>() = proj2d;
152  proj(2) = pc(0)/pc(2);
153  }
154 
155 
156  // transforms
157  void transformW2F(Eigen::Matrix<double,3,4> &m,
158  const Eigen::Matrix<double,4,1> &trans,
159  const Eigen::Quaternion<double> &qrot)
160  {
161  m.block<3,3>(0,0) = qrot.toRotationMatrix().transpose();
162  m.col(3).setZero(); // make sure there's no translation
163  m.col(3) = -m*trans;
164  };
165 
166  void transformF2W(Eigen::Matrix<double,3,4> &m,
167  const Eigen::Matrix<double,4,1> &trans,
168  const Eigen::Quaternion<double> &qrot)
169  {
170  m.block<3,3>(0,0) = qrot.toRotationMatrix();
171  m.col(3) = trans.head(3);
172  };
173 
174 
175  // returns the local R,t in nd0 that produces nd1
176  // NOTE: returns a postfix rotation; should return a prefix
177  void transformN2N(Eigen::Matrix<double,4,1> &trans,
178  Eigen::Quaternion<double> &qr,
179  Node &nd0, Node &nd1)
180  {
181  Matrix<double,3,4> tfm;
182  Quaterniond q0,q1;
183  q0 = nd0.qrot;
184  transformW2F(tfm,nd0.trans,q0);
185  trans.head(3) = tfm*nd1.trans;
186  trans(3) = 1.0;
187  q1 = nd1.qrot;
188  qr = q0.inverse()*q1;
189  qr.normalize();
190  if (qr.w() < 0)
191  qr.coeffs() = -qr.coeffs();
192  }
193 
194 
195 } // namespace sba
196 
197 
void transformW2F(Eigen::Matrix< double, 3, 4 > &m, const Eigen::Matrix< double, 4, 1 > &trans, const Eigen::Quaternion< double > &qrot)
Definition: node.cpp:157
void transformF2W(Eigen::Matrix< double, 3, 4 > &m, const Eigen::Matrix< double, 4, 1 > &trans, const Eigen::Quaternion< double > &qrot)
Definition: node.cpp:166
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
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_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


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