proj.cpp
Go to the documentation of this file.
1 #include <sparse_bundle_adjustment/proj.h>
2 
3 namespace sba
4 {
5  Proj::Proj(int ci, Eigen::Vector3d &q, bool stereo)
6  : ndi(ci), kp(q), stereo(stereo),
7  isValid(true), useCovar(false), pointPlane(false) {}
8 
9  Proj::Proj(int ci, Eigen::Vector2d &q)
10  : ndi(ci), kp(q(0), q(1), 0),
11  stereo(false), isValid(true), useCovar(false), pointPlane(false) {}
12 
13  Proj::Proj()
14  : ndi(0), kp(0, 0, 0),
15  stereo(false), isValid(false), useCovar(false), pointPlane(false) {}
16 
17  void Proj::setJacobians(const Node &nd, const Point &pt, JacobProds *jpp)
18  {
19  if (stereo)
20  setJacobiansStereo_(nd, pt, jpp);
21  else
22  setJacobiansMono_(nd, pt, jpp);
23  }
24 
25  double Proj::calcErr(const Node &nd, const Point &pt, const double huber)
26  {
27  if (stereo)
28  return calcErrStereo_(nd, pt, huber);
29  else
30  return calcErrMono_(nd, pt, huber);
31  }
32 
33  double Proj::getErrNorm()
34  {
35  if (stereo)
36  return err.norm();
37  else
38  return err.head<2>().norm();
39  }
40 
41  double Proj::getErrSquaredNorm()
42  {
43  if (stereo)
44  return err.squaredNorm();
45  else
46  return err.head<2>().squaredNorm();
47  }
48 
49  void Proj::setCovariance(const Eigen::Matrix3d &covar)
50  {
51  useCovar = true;
52  covarmat = covar;
53  }
54 
55  void Proj::clearCovariance()
56  {
57  useCovar = false;
58  }
59 
60  void Proj::setJacobiansMono_(const Node &nd, const Point &pt, JacobProds *jpp)
61  {
62  // first get the world point in camera coords
63  Eigen::Matrix<double,3,1> pc = nd.w2n * pt;
64 
66  Eigen::Matrix<double,2,6> jacc;
67 
69  Eigen::Matrix<double,2,3> jacp;
70 
71  // Jacobians wrt camera parameters
72  // set d(quat-x) values [ pz*dpx/dx - px*dpz/dx ] / pz^2
73  double px = pc(0);
74  double py = pc(1);
75  double pz = pc(2);
76  double ipz2 = 1.0/(pz*pz);
77  if (std::isnan(ipz2) ) { printf("[SetJac] infinite jac\n"); *(int *)0x0 = 0; }
78 
79  double ipz2fx = ipz2*nd.Kcam(0,0); // Fx
80  double ipz2fy = ipz2*nd.Kcam(1,1); // Fy
81  // scale quaternion derivative to match the translational ones
82  double ipz2fxq = qScale*ipz2fx;
83  double ipz2fyq = qScale*ipz2fy;
84  Eigen::Matrix<double,3,1> pwt;
85 
86  // check for local vars
87  pwt = (pt-nd.trans).head<3>(); // transform translations, use differential rotation
88 
89  // dx
90  Eigen::Matrix<double,3,1> dp = nd.dRdx * pwt; // dR'/dq * [pw - t]
91  jacc(0,3) = (pz*dp(0) - px*dp(2))*ipz2fxq;
92  jacc(1,3) = (pz*dp(1) - py*dp(2))*ipz2fyq;
93  // dy
94  dp = nd.dRdy * pwt; // dR'/dq * [pw - t]
95  jacc(0,4) = (pz*dp(0) - px*dp(2))*ipz2fxq;
96  jacc(1,4) = (pz*dp(1) - py*dp(2))*ipz2fyq;
97  // dz
98  dp = nd.dRdz * pwt; // dR'/dq * [pw - t]
99  jacc(0,5) = (pz*dp(0) - px*dp(2))*ipz2fxq;
100  jacc(1,5) = (pz*dp(1) - py*dp(2))*ipz2fyq;
101 
102  // set d(t) values [ pz*dpx/dx - px*dpz/dx ] / pz^2
103  dp = -nd.w2n.col(0); // dpc / dx
104  jacc(0,0) = (pz*dp(0) - px*dp(2))*ipz2fx;
105  jacc(1,0) = (pz*dp(1) - py*dp(2))*ipz2fy;
106  dp = -nd.w2n.col(1); // dpc / dy
107  jacc(0,1) = (pz*dp(0) - px*dp(2))*ipz2fx;
108  jacc(1,1) = (pz*dp(1) - py*dp(2))*ipz2fy;
109  dp = -nd.w2n.col(2); // dpc / dz
110  jacc(0,2) = (pz*dp(0) - px*dp(2))*ipz2fx;
111  jacc(1,2) = (pz*dp(1) - py*dp(2))*ipz2fy;
112 
113  // Jacobians wrt point parameters
114  // set d(t) values [ pz*dpx/dx - px*dpz/dx ] / pz^2
115  dp = nd.w2n.col(0); // dpc / dx
116  jacp(0,0) = (pz*dp(0) - px*dp(2))*ipz2fx;
117  jacp(1,0) = (pz*dp(1) - py*dp(2))*ipz2fy;
118  dp = nd.w2n.col(1); // dpc / dy
119  jacp(0,1) = (pz*dp(0) - px*dp(2))*ipz2fx;
120  jacp(1,1) = (pz*dp(1) - py*dp(2))*ipz2fy;
121  dp = nd.w2n.col(2); // dpc / dz
122  jacp(0,2) = (pz*dp(0) - px*dp(2))*ipz2fx;
123  jacp(1,2) = (pz*dp(1) - py*dp(2))*ipz2fy;
124 
125 #ifdef DEBUG
126  for (int i=0; i<2; i++)
127  for (int j=0; j<6; j++)
128  if (std::isnan(jacc(i,j)) ) { printf("[SetJac] NaN in jacc(%d,%d)\n", i, j); *(int *)0x0 = 0; }
129 #endif
130 
131  // Set Hessians + extras.
132  jpp->Hpp = jacp.transpose() * jacp;
133  jpp->Hcc = jacc.transpose() * jacc;
134  jpp->Hpc = jacp.transpose() * jacc;
135  jpp->JcTE = jacc.transpose() * err.head<2>();
136  jpp->Bp = jacp.transpose() * err.head<2>();
137 
138  jp = jpp;
139  }
140 
141  // calculate error of a projection
142  // we should do something about negative Z
143  double Proj::calcErrMono_(const Node &nd, const Point &pt, double huber)
144  {
145  Eigen::Vector3d p1 = nd.w2i * pt;
146  err(2) = 0.0;
147  if (p1(2) <= 0.0)
148  {
149 #ifdef DEBUG
150  printf("[CalcErr] negative Z! Node %d \n",ndi);
151  if (std::isnan(err[0]) || std::isnan(err[1]) ) printf("[CalcErr] NaN!\n");
152 #endif
153  err = Eigen::Vector3d(0.0,0.0,0.0);
154  return 0.0;
155  }
156  else
157  err.head<2>() = p1.head<2>()/p1(2);
158 
159  err -= kp;
160 
161  // pseudo-Huber weighting
162  // C(e) = 2*s^2*[sqrt(1+(e/s)^2)-1]
163  // w = sqrt(C(norm(e)))/norm(e)
164 
165  if (huber > 0)
166  {
167  double b2 = huber*huber; // kernel width
168  double e2 = err.head<2>().squaredNorm();
169  if (e2 > b2)
170  {
171  double c = 2.0*huber*sqrt(e2) - b2;
172  double w = sqrt(c/e2);
173  err.head<2>() *= w; // weight the error
174  // std::cout << "Huber weight: " << w << " Err sq: " << e2 << std::endl;
175  }
176 
177 
178 // double b2 = HUBER*HUBER; // kernel width
179 // double e2 = err.squaredNorm();
180 // e2 = std::max(e2,1e-22); // can't have a zero here
181 // double w = 2*b2*(sqrt(1+e2/b2)-1);
182 // w = sqrt(w/e2);
183 // err.head<2>() *= w; // weight the error
184  }
185 
186  return err.head<2>().squaredNorm();
187  }
188 
189 
190  void Proj::setJacobiansStereo_(const Node &nd, const Point &pt, JacobProds *jpp)
191  {
192  // first get the world point in camera coords
193  Eigen::Matrix<double,3,1> pc = nd.w2n * pt;
194 
196  Eigen::Matrix<double,3,3> jacp;
197 
199  Eigen::Matrix<double,3,6> jacc;
200 
201  // Jacobians wrt camera parameters
202  // set d(quat-x) values [ pz*dpx/dx - px*dpz/dx ] / pz^2
203  double px = pc(0);
204  double py = pc(1);
205  double pz = pc(2);
206  double ipz2 = 1.0/(pz*pz);
207  if (std::isnan(ipz2) ) { printf("[SetJac] infinite jac\n"); *(int *)0x0 = 0; }
208 
209  double ipz2fx = ipz2*nd.Kcam(0,0); // Fx
210  double ipz2fy = ipz2*nd.Kcam(1,1); // Fy
211  double b = nd.baseline; // stereo baseline
212  // scale quaternion derivative to match the translational ones
213  double ipz2fxq = qScale*ipz2fx;
214  double ipz2fyq = qScale*ipz2fy;
215  Eigen::Matrix<double,3,1> pwt;
216 
217  // check for local vars
218  pwt = (pt-nd.trans).head(3); // transform translations, use differential rotation
219 
220  // dx
221  Eigen::Matrix<double,3,1> dp = nd.dRdx * pwt; // dR'/dq * [pw - t]
222  jacc(0,3) = (pz*dp(0) - px*dp(2))*ipz2fxq;
223  jacc(1,3) = (pz*dp(1) - py*dp(2))*ipz2fyq;
224  jacc(2,3) = (pz*dp(0) - (px-b)*dp(2))*ipz2fxq; // right image px
225  // dy
226  dp = nd.dRdy * pwt; // dR'/dq * [pw - t]
227  jacc(0,4) = (pz*dp(0) - px*dp(2))*ipz2fxq;
228  jacc(1,4) = (pz*dp(1) - py*dp(2))*ipz2fyq;
229  jacc(2,4) = (pz*dp(0) - (px-b)*dp(2))*ipz2fxq; // right image px
230  // dz
231  dp = nd.dRdz * pwt; // dR'/dq * [pw - t]
232  jacc(0,5) = (pz*dp(0) - px*dp(2))*ipz2fxq;
233  jacc(1,5) = (pz*dp(1) - py*dp(2))*ipz2fyq;
234  jacc(2,5) = (pz*dp(0) - (px-b)*dp(2))*ipz2fxq; // right image px
235 
236  // set d(t) values [ pz*dpx/dx - px*dpz/dx ] / pz^2
237  dp = -nd.w2n.col(0); // dpc / dx
238  jacc(0,0) = (pz*dp(0) - px*dp(2))*ipz2fxq;
239  jacc(1,0) = (pz*dp(1) - py*dp(2))*ipz2fy;
240  jacc(2,0) = (pz*dp(0) - (px-b)*dp(2))*ipz2fxq; // right image px
241  dp = -nd.w2n.col(1); // dpc / dy
242  jacc(0,1) = (pz*dp(0) - px*dp(2))*ipz2fxq;
243  jacc(1,1) = (pz*dp(1) - py*dp(2))*ipz2fy;
244  jacc(2,1) = (pz*dp(0) - (px-b)*dp(2))*ipz2fxq; // right image px
245  dp = -nd.w2n.col(2); // dpc / dz
246  jacc(0,2) = (pz*dp(0) - px*dp(2))*ipz2fxq;
247  jacc(1,2) = (pz*dp(1) - py*dp(2))*ipz2fy;
248  jacc(2,2) = (pz*dp(0) - (px-b)*dp(2))*ipz2fxq; // right image px
249 
250  // Jacobians wrt point parameters
251  // set d(t) values [ pz*dpx/dx - px*dpz/dx ] / pz^2
252  dp = nd.w2n.col(0); // dpc / dx
253  jacp(0,0) = (pz*dp(0) - px*dp(2))*ipz2fxq;
254  jacp(1,0) = (pz*dp(1) - py*dp(2))*ipz2fy;
255  jacp(2,0) = (pz*dp(0) - (px-b)*dp(2))*ipz2fxq; // right image px
256  dp = nd.w2n.col(1); // dpc / dy
257  jacp(0,1) = (pz*dp(0) - px*dp(2))*ipz2fxq;
258  jacp(1,1) = (pz*dp(1) - py*dp(2))*ipz2fy;
259  jacp(2,1) = (pz*dp(0) - (px-b)*dp(2))*ipz2fxq; // right image px
260  dp = nd.w2n.col(2); // dpc / dz
261  jacp(0,2) = (pz*dp(0) - px*dp(2))*ipz2fxq;
262  jacp(1,2) = (pz*dp(1) - py*dp(2))*ipz2fy;
263  jacp(2,2) = (pz*dp(0) - (px-b)*dp(2))*ipz2fxq; // right image px
264 
265 #ifdef DEBUG
266  for (int i=0; i<2; i++)
267  for (int j=0; j<6; j++)
268  if (std::isnan(jacc(i,j)) ) { printf("[SetJac] NaN in jacc(%d,%d)\n", i, j); *(int *)0x0 = 0; }
269 #endif
270  if (useCovar)
271  {
272  jacc = covarmat * jacc;
273  jacp = covarmat * jacp;
274  }
275 
276  // Set Hessians + extras.
277  jpp->Hpp = jacp.transpose() * jacp;
278  jpp->Hcc = jacc.transpose() * jacc;
279  jpp->Hpc = jacp.transpose() * jacc;
280  jpp->JcTE = jacc.transpose() * err;
281  jpp->Bp = jacp.transpose() * err;
282 
283  jp = jpp;
284  }
285 
286  // calculate error of a projection
287  // we should do something about negative Z
288  double Proj::calcErrStereo_(const Node &nd, const Point &pt, double huber)
289  {
290  Eigen::Vector3d p1 = nd.w2i * pt;
291  Eigen::Vector3d p2 = nd.w2n * pt;
292  Eigen::Vector3d pb(nd.baseline,0,0);
293 
294  // TODO: Clean this up a bit.
295  if (pointPlane)
296  {
297  // Project point onto plane.
298  Eigen::Vector3d w = pt.head<3>()-plane_point;
299 
300  //printf("w: %f %f %f\n", w.x(), w.y(), w.z());
301  //Eigen::Vector3d projpt = pt.head<3>()+(w.dot(plane_normal))*plane_normal;
302  Eigen::Vector3d projpt = plane_point+(w.dot(plane_normal))*plane_normal;
303  // Eigen::Vector3d projpt = pt.head<3>()+(w.dot(plane_normal))*plane_normal;
304  //printf("[Proj] Distance to plane: %f\n", w.dot(plane_normal));
305  p1 = nd.w2i*Eigen::Vector4d(projpt.x(), projpt.y(), projpt.z(), 1.0);
306  p2 = nd.w2n*Eigen::Vector4d(projpt.x(), projpt.y(), projpt.z(), 1.0);
307  }
308 
309  double invp1 = 1.0/p1(2);
310 
311  err.head<2>() = p1.head<2>()*invp1;
312  // right camera px
313  p2 = nd.Kcam*(p2-pb);
314 
315  err(2) = p2(0)/p2(2);
316  if (p1(2) <= 0.0)
317  {
318 #ifdef DEBUG
319  printf("[CalcErr] negative Z! Node %d\n",ndi);
320  if (std::isnan(err[0]) || std::isnan(err[1]) ) printf("[CalcErr] NaN!\n");
321 #endif
322  err.setZero();
323 
324  return 0.0;
325  }
326  err -= kp;
327 
328  if (abs(err(0)) > 1e6 || abs(err(1)) > 1e6 || abs(err(2)) > 1e6)
329  {
330  printf("\n\n[CalcErr] Excessive error.\n");
331 
332  isValid = false;
333  }
334 
335  if (useCovar)
336  err = covarmat*err;
337 
338  // Huber kernel weighting
339  if (huber > 0.0)
340  {
341  double b2 = huber*huber; // kernel width
342  double e2 = err.squaredNorm();
343  if (e2 > b2)
344  {
345  double c = 2.0*huber*sqrt(e2) - b2;
346  double w = sqrt(c/e2);
347  err *= w;
348  // std::cout << "Huber weight: " << w << " Err sq: " << e2 << std::endl;
349  }
350  }
351 
352  return err.squaredNorm();
353  }
354 
355  // Constructors for track.
356  Track::Track() : point() { }
357  Track::Track(Point p) : point(p) { }
358 
359 } // sba
360 
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::JacobProds::Hpc
Eigen::Matrix< double, 3, 6 > Hpc
Point-to-camera Hessian (JpT*Jc)
Definition: proj.h:36
sba::Node::w2i
Eigen::Matrix< double, 3, 4 > w2i
The transform from world to image coordinates.
Definition: node.h:111
sba::Proj::setJacobiansMono_
void setJacobiansMono_(const Node &nd, const Point &pt, JacobProds *jpp)
Set monocular jacobians/hessians.
Definition: proj.cpp:60
sba::Proj::calcErr
double calcErr(const Node &nd, const Point &pt, double huber=0.0)
Calculates re-projection error and stores it in err.
Definition: proj.cpp:25
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::Track::point
Point point
An Eigen 4-vector containing the <x, y, z, w> coordinates of the point associated with the track...
Definition: proj.h:197
sba::Proj::qScale
static constexpr double qScale
Definition: proj.h:131
sba::JacobProds::Hcc
Eigen::Matrix< double, 6, 6 > Hcc
Camera-to-camera Hessian (JcT*Jc)
Definition: proj.h:39
sba::Proj::setJacobians
void setJacobians(const Node &nd, const Point &pt, JacobProds *jpp)
Sets the jacobians and hessians for the projection to use for SBA.
Definition: proj.cpp:17
sba::Node::dRdy
Eigen::Matrix< double, 3, 3 > dRdy
Definition: node.h:126
sba::Proj::clearCovariance
void clearCovariance()
Clear the covariance matrix and no longer use it.
Definition: proj.cpp:55
sba::Proj::useCovar
bool useCovar
Use a covariance matrix?
Definition: proj.h:134
sba::Proj::Proj
Proj()
Default constructor. Initializes to default values, kp = <0 0 0> and ndi = <0>. Also sets the project...
Definition: proj.cpp:13
sba::JacobProds::Hpp
Eigen::Matrix< double, 3, 3 > Hpp
Point-to-point Hessian (JpT*Jp).
Definition: proj.h:33
sba::Proj::setCovariance
void setCovariance(const Eigen::Matrix3d &covar)
Set the covariance matrix to use for cost calculation. Without the covariance matrix, cost is calculated by: cost = ||err|| With a covariance matrix, the cost is calculated by: cost = (err)T*covar*(err)
Definition: proj.cpp:49
sba::Proj::err
Eigen::Vector3d err
Reprojection error.
Definition: proj.h:84
sba::JacobProds::Bp
Eigen::Matrix< double, 3, 1 > Bp
The B matrix with respect to points (JpT*Err)
Definition: proj.h:42
sba::Proj::setJacobiansStereo_
void setJacobiansStereo_(const Node &nd, const Point &pt, JacobProds *jpp)
Set stereo jacobians/hessians.
Definition: proj.cpp:190
sba::Node::baseline
double baseline
baseline for stereo
Definition: node.h:93
sba::Node
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
sba::JacobProds::JcTE
Eigen::Matrix< double, 6, 1 > JcTE
Another matrix with respect to cameras (JcT*Err)
Definition: proj.h:45
sba::Proj::stereo
bool stereo
Whether the projection is Stereo (True) or Monocular (False).
Definition: proj.h:87
sba::Node::w2n
Eigen::Matrix< double, 3, 4 > w2n
Resultant transform from world to node coordinates.
Definition: node.h:80
sba::Proj::getErrNorm
double getErrNorm()
Get the correct norm of the error, depending on whether the projection is monocular or stereo...
Definition: proj.cpp:33
sba
Definition: bpcg.h:60
sba::Proj::calcErrMono_
double calcErrMono_(const Node &nd, const Point &pt, double huber)
Calculate error function for stereo.
Definition: proj.cpp:143
sba::Track::Track
Track()
Default constructor for Track.
Definition: proj.cpp:356
sba::Proj::ndi
int ndi
Node index, the camera node for this projection.
Definition: proj.h:78
sba::Proj::plane_point
Eigen::Vector3d plane_point
Point for point-plane projections.
Definition: proj.h:146
sba::Proj::isValid
bool isValid
valid or not (could be out of bounds)
Definition: proj.h:127
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::Proj::kp
Eigen::Vector3d kp
Keypoint, u,v,u-d vector.
Definition: proj.h:81
sba::Proj::getErrSquaredNorm
double getErrSquaredNorm()
Get the correct squared norm of the error, depending on whether the projection is monocular or stereo...
Definition: proj.cpp:41
sba::Proj::jp
JacobProds * jp
Jacobian products.
Definition: proj.h:120
sba::Proj::pointPlane
bool pointPlane
Whether this is a point-plane match (true) or a point-point match (false).
Definition: proj.h:140
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
sba::Proj::calcErrStereo_
double calcErrStereo_(const Node &nd, const Point &pt, double huber)
Calculate error function for stereo.
Definition: proj.cpp:288
sba::Node::dRdz
Eigen::Matrix< double, 3, 3 > dRdz
Definition: node.h:126
sba::Proj::covarmat
Eigen::Matrix< double, 3, 3 > covarmat
Covariance matrix for cost calculation.
Definition: proj.h:137
sba::Proj::plane_normal
Eigen::Vector3d plane_normal
Normal for point-plane projections.
Definition: proj.h:143

sparse_bundle_adjustment
Author(s):
autogenerated on Mon Feb 28 2022 23:48:02