Main MRPT website > C++ reference for MRPT 1.9.9
chessboard_stereo_camera_calib.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "vision-precomp.h" // Precompiled headers
11 
12 #include <mrpt/system/filesystem.h>
14 
17 #include <mrpt/vision/pinhole.h>
18 #include <mrpt/poses/CPose3DQuat.h>
20 #include <mrpt/math/wrap2pi.h>
21 #include <algorithm> // reverse()
22 
24 
25 //#define USE_NUMERIC_JACOBIANS
26 //#define COMPARE_NUMERIC_JACOBIANS
27 
28 #ifdef USE_NUMERIC_JACOBIANS
29 #include <mrpt/math/num_jacobian.h>
30 #endif
31 
32 using namespace mrpt;
33 using namespace mrpt::vision;
34 using namespace mrpt::img;
35 using namespace mrpt::poses;
36 using namespace mrpt::math;
37 using namespace std;
38 
39 /* -------------------------------------------------------
40  checkerBoardStereoCalibration
41  ------------------------------------------------------- */
45 {
46  try
47  {
48  ASSERT_(p.check_size_x > 2);
49  ASSERT_(p.check_size_y > 2);
50  ASSERT_(p.check_squares_length_X_meters > 0);
51  ASSERT_(p.check_squares_length_Y_meters > 0);
52  const bool user_wants_use_robust = p.use_robust_kernel;
53 
54  if (images.size() < 1)
55  {
56  std::cout << "ERROR: No input images." << std::endl;
57  return false;
58  }
59 
60  const unsigned CORNERS_COUNT = p.check_size_x * p.check_size_y;
61  const TImageSize check_size =
62  TImageSize(p.check_size_x, p.check_size_y);
64 
65  // For each image, find checkerboard corners:
66  // -----------------------------------------------
67  // Left/Right sizes (may be different)
68  TImageSize imgSize[2] = {TImageSize(0, 0), TImageSize(0, 0)};
69 
70  vector<size_t> valid_image_pair_indices; // Indices in images[] which
71  // are valid pairs to be used
72  // in the optimization
73 
74  for (size_t i = 0; i < images.size(); i++)
75  {
76  // Do loop for each left/right image:
77  TImageCalibData* dats[2] = {&images[i].left, &images[i].right};
78  bool corners_found[2] = {false, false};
79 
80  for (int lr = 0; lr < 2; lr++)
81  {
82  TImageCalibData& dat = *dats[lr];
83  dat.detected_corners.clear();
84 
85  // Make grayscale version:
86  const CImage img_gray(
88 
89  if (!i)
90  {
91  imgSize[lr] = img_gray.getSize();
92  if (lr == 0)
93  {
94  out.cam_params.leftCamera.ncols = imgSize[lr].x;
95  out.cam_params.leftCamera.nrows = imgSize[lr].y;
96  }
97  else
98  {
99  out.cam_params.rightCamera.ncols = imgSize[lr].x;
100  out.cam_params.rightCamera.nrows = imgSize[lr].y;
101  }
102  }
103  else
104  {
105  if (imgSize[lr].y != (int)img_gray.getHeight() ||
106  imgSize[lr].x != (int)img_gray.getWidth())
107  {
108  std::cout << "ERROR: All the images in each left/right "
109  "channel must have the same size."
110  << std::endl;
111  return false;
112  }
113  }
114 
115  // Do detection (this includes the "refine corners" with
116  // cvFindCornerSubPix):
117  corners_found[lr] = mrpt::vision::findChessboardCorners(
118  img_gray, dat.detected_corners, p.check_size_x,
119  p.check_size_y,
120  p.normalize_image // normalize_image
121  );
122 
123  if (corners_found[lr] &&
124  dat.detected_corners.size() != CORNERS_COUNT)
125  corners_found[lr] = false;
126 
127  if (p.verbose)
128  cout << format(
129  "%s img #%u: %s\n", lr == 0 ? "LEFT" : "RIGHT",
130  static_cast<unsigned int>(i),
131  corners_found[lr] ? "DETECTED" : "NOT DETECTED");
132  // User Callback?
133  if (p.callback)
134  {
135  cbPars.calibRound = -1; // Detecting corners
136  cbPars.current_iter = 0;
137  cbPars.current_rmse = 0;
138  cbPars.nImgsProcessed = i * 2 + lr + 1;
139  cbPars.nImgsToProcess = images.size() * 2;
140  (*p.callback)(cbPars, p.callback_user_param);
141  }
142 
143  if (corners_found[lr])
144  {
145  // Draw the checkerboard in the corresponding image:
146  if (!dat.img_original.isExternallyStored())
147  {
148  // Checkboad as color image:
151  dat.detected_corners, check_size.x, check_size.y);
152  }
153  }
154 
155  } // end for lr
156 
157  // We just finished detecting corners in a left/right pair.
158  // Only if corners were detected perfectly in BOTH images, add this
159  // image pair as a good one for optimization:
160  if (corners_found[0] && corners_found[1])
161  {
162  valid_image_pair_indices.push_back(i);
163 
164  // Consistency between left/right pair: the corners MUST BE
165  // ORDERED so they match to each other,
166  // and the criterion must be the same in ALL pairs to avoid
167  // rubbish optimization:
168 
169  // Key idea: Generate a representative vector that goes along
170  // rows and columns.
171  // Check the angle of those director vectors between L/R images.
172  // That can be done
173  // via the dot product. Swap rows/columns order as needed.
174  bool has_to_redraw_corners = false;
175 
176  const TPixelCoordf pt_l0 = images[i].left.detected_corners[0],
177  pt_l1 = images[i].left.detected_corners[1],
178  pt_r0 = images[i].right.detected_corners[0],
179  pt_r1 = images[i].right.detected_corners[1];
180  const auto Al =
181  TPoint2D(pt_l1.x, pt_l1.y) - TPoint2D(pt_l0.x, pt_l0.y);
182  const auto Ar =
183  TPoint2D(pt_r1.x, pt_r1.y) - TPoint2D(pt_r0.x, pt_r0.y);
184 
185  // If the dot product is negative, we have INVERTED order of
186  // corners:
187  if (Al.x * Ar.x + Al.y * Ar.y < 0)
188  {
189  has_to_redraw_corners = true;
190  // Invert all corners:
191  std::reverse(
192  images[i].right.detected_corners.begin(),
193  images[i].right.detected_corners.end());
194  }
195 
196  if (has_to_redraw_corners)
197  {
198  // Checkboad as color image:
199  images[i].right.img_original.colorImage(
200  images[i].right.img_checkboard);
201  images[i].right.img_checkboard.drawChessboardCorners(
202  images[i].right.detected_corners, check_size.x,
203  check_size.y);
204  }
205  }
206 
207  } // end find corners
208 
209  if (p.verbose)
210  std::cout << valid_image_pair_indices.size()
211  << " valid image pairs." << std::endl;
212  if (valid_image_pair_indices.empty())
213  {
214  std::cout << "ERROR: No valid images. Perhaps the checkerboard "
215  "size is incorrect?"
216  << std::endl;
217  return false;
218  }
219 
220  // Create reference coordinates of the detected corners, in "global"
221  // coordinates:
222  // -------------------------------------------------------------------------------
223  vector<TPoint3D> obj_points;
224  obj_points.reserve(CORNERS_COUNT);
225  for (unsigned int y = 0; y < p.check_size_y; y++)
226  for (unsigned int x = 0; x < p.check_size_x; x++)
227  obj_points.push_back(
228  TPoint3D(
229  p.check_squares_length_X_meters * x,
230  p.check_squares_length_Y_meters * y, 0));
231 
232  // ----------------------------------------------------------------------------------
233  // Levenberg-Marquardt algorithm
234  //
235  // State space (poses are actually on-manifold increments):
236  // N*left_cam_pose + right2left_pose + left_cam_params +
237  // right_cam_params
238  // N * 6 + 6 + 9 + 9
239  // = 6N+24
240  // ----------------------------------------------------------------------------------
241  const size_t N = valid_image_pair_indices.size();
242  const size_t nObs = 2 * N * CORNERS_COUNT; // total number of valid
243  // observations (px,py)
244  // taken into account in the
245  // optimization
246 
247  const double tau = 0.3;
248  const double t1 = 1e-8;
249  const double t2 = 1e-8;
250  const double MAX_LAMBDA = 1e20;
251 
252  // Initial state:
253  // Within lm_stat: CArrayDouble<9> left_cam_params, right_cam_params; //
254  // [fx fy cx cy k1 k2 k3 t1 t2]
255  lm_stat_t lm_stat(images, valid_image_pair_indices, obj_points);
256 
257  lm_stat.left_cam_params[0] = imgSize[0].x * 0.9;
258  lm_stat.left_cam_params[1] = imgSize[0].y * 0.9;
259  lm_stat.left_cam_params[2] = imgSize[0].x * 0.5;
260  lm_stat.left_cam_params[3] = imgSize[0].y * 0.5;
261  lm_stat.left_cam_params[4] = lm_stat.left_cam_params[5] =
262  lm_stat.left_cam_params[6] = lm_stat.left_cam_params[7] =
263  lm_stat.left_cam_params[8] = 0;
264 
265  lm_stat.right_cam_params[0] = imgSize[1].x * 0.9;
266  lm_stat.right_cam_params[1] = imgSize[1].y * 0.9;
267  lm_stat.right_cam_params[2] = imgSize[1].x * 0.5;
268  lm_stat.right_cam_params[3] = imgSize[1].y * 0.5;
269  lm_stat.right_cam_params[4] = lm_stat.right_cam_params[5] =
270  lm_stat.right_cam_params[6] = lm_stat.right_cam_params[7] =
271  lm_stat.right_cam_params[8] = 0;
272 
273  // ===============================================================================
274  // Run stereo calibration in two stages:
275  // (0) Estimate all parameters except distortion
276  // (1) Estimate all parameters, using as starting point the guess from
277  // (0)
278  // ===============================================================================
279  size_t nUnknownsCamParams = 0;
280  size_t iter = 0;
281  double err = 0;
282  std::vector<size_t> vars_to_optimize;
283  Eigen::MatrixXd H; // Hessian matrix (Declared here so it's accessible
284  // as the final uncertainty measure)
285 
286  for (int calibRound = 0; calibRound < 2; calibRound++)
287  {
288  cbPars.calibRound = calibRound;
289  if (p.verbose)
290  cout
291  << ((calibRound == 0) ? "LM calibration round #0: WITHOUT "
292  "distortion ----------\n"
293  : "LM calibration round #1: ALL "
294  "parameters --------------\n");
295 
296  // Select which cam. parameters to optimize (for each camera!) and
297  // which to leave fixed:
298  // Indices: [0 1 2 3 4 5 6 7 8]
299  // Variables:[fx fy cx cy k1 k2 k3 t1 t2]
300  vars_to_optimize.clear();
301  vars_to_optimize.push_back(0);
302  vars_to_optimize.push_back(1);
303  vars_to_optimize.push_back(2);
304  vars_to_optimize.push_back(3);
305  if (calibRound == 1)
306  {
307  if (p.optimize_k1) vars_to_optimize.push_back(4);
308  if (p.optimize_k2) vars_to_optimize.push_back(5);
309  if (p.optimize_t1) vars_to_optimize.push_back(6);
310  if (p.optimize_t2) vars_to_optimize.push_back(7);
311  if (p.optimize_k3) vars_to_optimize.push_back(8);
312  }
313 
314  nUnknownsCamParams = vars_to_optimize.size();
315 
316  // * N x Manifold Epsilon of left camera pose (6)
317  // * Manifold Epsilon of right2left pose (6)
318  // * Left-cam-params (<=9)
319  // * Right-cam-params (<=9)
320  const size_t nUnknowns = N * 6 + 6 + 2 * nUnknownsCamParams;
321 
322  // Residuals & Jacobians:
323  TResidualJacobianList res_jacob;
325  lm_stat, res_jacob, false /* no robust */,
326  p.robust_kernel_param);
327 
328  // Build linear system:
329  Eigen::VectorXd minus_g; // minus gradient
330  build_linear_system(res_jacob, vars_to_optimize, minus_g, H);
331 
332  ASSERT_EQUAL_(nUnknowns, (size_t)H.cols());
333  // Lev-Marq. parameters:
334  double nu = 2;
335  double lambda = tau * H.diagonal().array().maxCoeff();
336  bool done = (minus_g.array().abs().maxCoeff() < t1);
337  int numItersImproving = 0;
338  bool use_robust = false;
339 
340  // Lev-Marq. main loop:
341  iter = 0;
342  while (iter < p.maxIters && !done)
343  {
344  // User Callback?
345  if (p.callback)
346  {
347  cbPars.current_iter = iter;
348  cbPars.current_rmse = std::sqrt(err / nObs);
349  (*p.callback)(cbPars, p.callback_user_param);
350  }
351 
352  // Solve for increment: (H + \lambda I) eps = -gradient
353  Eigen::MatrixXd HH = H;
354  for (size_t i = 0; i < nUnknowns; i++) HH(i, i) += lambda;
355  // HH(i,i)*= (1.0 + lambda);
356 
357  Eigen::LLT<Eigen::MatrixXd> llt(
358  HH.selfadjointView<Eigen::Lower>());
359  if (llt.info() != Eigen::Success)
360  {
361  lambda *= nu;
362  nu *= 2;
363  done = (lambda > MAX_LAMBDA);
364 
365  if (p.verbose && !done)
366  cout << "LM iter#" << iter
367  << ": Couldn't solve LLt, retrying with larger "
368  "lambda="
369  << lambda << endl;
370  continue;
371  }
372  const Eigen::VectorXd eps = llt.solve(minus_g);
373 
374  const double eps_norm = eps.norm();
375  if (eps_norm < t2 * (eps_norm + t2))
376  {
377  if (p.verbose)
378  cout << "Termination criterion: eps_norm < "
379  "t2*(eps_norm+t2): "
380  << eps_norm << " < " << t2 * (eps_norm + t2)
381  << endl;
382  done = true;
383  break;
384  }
385 
386  // Tentative new state:
387  lm_stat_t new_lm_stat(
388  lm_stat); // images,valid_image_pair_indices,obj_points);
389  add_lm_increment(eps, vars_to_optimize, new_lm_stat);
390 
391  TResidualJacobianList new_res_jacob;
392 
393  // discriminant:
394  double err_new = recompute_errors_and_Jacobians(
395  new_lm_stat, new_res_jacob, use_robust,
396  p.robust_kernel_param);
397 
398  if (err_new < err)
399  {
400  // const double l = (err-err_new)/ (eps.dot( lambda*eps +
401  // minus_g) );
402 
403  if (numItersImproving++ > 5)
404  use_robust = user_wants_use_robust;
405 
406  // Good: Accept new values
407  if (p.verbose)
408  cout << "LM iter#" << iter
409  << ": Avr.err(px):" << std::sqrt(err / nObs)
410  << "->" << std::sqrt(err_new / nObs)
411  << " lambda:" << lambda << endl;
412 
413  // swap: faster than "lm_stat <- new_lm_stat"
414  lm_stat.swap(new_lm_stat);
415  res_jacob.swap(new_res_jacob);
416 
417  err = err_new;
419  res_jacob, vars_to_optimize, minus_g, H);
420 
421  // Too small gradient?
422  done = (minus_g.array().abs().maxCoeff() < t1);
423  // lambda *= max(1.0/3.0, 1-std::pow(2*l-1,3.0) );
424  lambda *= 0.6;
425  lambda = std::max(lambda, 1e-100);
426  nu = 2.0;
427 
428  iter++;
429  }
430  else
431  {
432  lambda *= nu;
433  if (p.verbose)
434  cout << "LM iter#" << iter << ": No update: err=" << err
435  << " -> err_new=" << err_new
436  << " retrying with larger lambda=" << lambda
437  << endl;
438  nu *= 2.0;
439  done = (lambda > MAX_LAMBDA);
440  }
441 
442  } // end while LevMarq.
443 
444  } // end for each calibRound = [0,1]
445  // -------------------------------------------------------------------------------
446  // End of Levenberg-Marquardt
447  // -------------------------------------------------------------------------------
448 
449  // Save final optimum values to the output structure
450  out.final_rmse = std::sqrt(err / nObs);
451  out.final_iters = iter;
453 
454  // [fx fy cx cy k1 k2 k3 t1 t2]
455  out.cam_params.leftCamera.fx(lm_stat.left_cam_params[0]);
456  out.cam_params.leftCamera.fy(lm_stat.left_cam_params[1]);
457  out.cam_params.leftCamera.cx(lm_stat.left_cam_params[2]);
458  out.cam_params.leftCamera.cy(lm_stat.left_cam_params[3]);
459  out.cam_params.leftCamera.k1(lm_stat.left_cam_params[4]);
460  out.cam_params.leftCamera.k2(lm_stat.left_cam_params[5]);
461  out.cam_params.leftCamera.k3(lm_stat.left_cam_params[6]);
462  out.cam_params.leftCamera.p1(lm_stat.left_cam_params[7]);
463  out.cam_params.leftCamera.p2(lm_stat.left_cam_params[8]);
464 
465  // [fx fy cx cy k1 k2 k3 t1 t2]
466  out.cam_params.rightCamera.fx(lm_stat.right_cam_params[0]);
467  out.cam_params.rightCamera.fy(lm_stat.right_cam_params[1]);
468  out.cam_params.rightCamera.cx(lm_stat.right_cam_params[2]);
469  out.cam_params.rightCamera.cy(lm_stat.right_cam_params[3]);
470  out.cam_params.rightCamera.k1(lm_stat.right_cam_params[4]);
471  out.cam_params.rightCamera.k2(lm_stat.right_cam_params[5]);
472  out.cam_params.rightCamera.k3(lm_stat.right_cam_params[6]);
473  out.cam_params.rightCamera.p1(lm_stat.right_cam_params[7]);
474  out.cam_params.rightCamera.p2(lm_stat.right_cam_params[8]);
475 
476  // R2L pose:
478 
479  // All the estimated camera poses:
480  out.left_cam_poses = lm_stat.left_cam_poses;
481 
482  out.image_pair_was_used.assign(images.size(), false);
483  for (size_t i = 0; i < valid_image_pair_indices.size(); i++)
484  out.image_pair_was_used[valid_image_pair_indices[i]] = true;
485 
486  // Uncertainties ---------------------
487  // The order of inv. variances in the diagonal of the Hessian is:
488  // * N x Manifold Epsilon of left camera pose (6)
489  // * Manifold Epsilon of right2left pose (6)
490  // * Left-cam-params (<=9)
491  // * Right-cam-params (<=9)
492  out.left_params_inv_variance.setConstant(0);
493  out.right_params_inv_variance.setConstant(0);
494  const size_t base_idx_H_CPs = H.cols() - 2 * nUnknownsCamParams;
495  for (size_t i = 0; i < nUnknownsCamParams; i++)
496  {
497  out.left_params_inv_variance[vars_to_optimize[i]] =
498  H(base_idx_H_CPs + i, base_idx_H_CPs + i);
499  out.right_params_inv_variance[vars_to_optimize[i]] =
500  H(base_idx_H_CPs + nUnknownsCamParams + i,
501  base_idx_H_CPs + nUnknownsCamParams + i);
502  }
503 
504  // Draw projected points
505  for (size_t i = 0; i < valid_image_pair_indices.size(); i++)
506  {
507  // mrpt::poses::CPose3D reconstructed_camera_pose; //!< At
508  // output, the reconstructed pose of the camera.
509  // std::vector<TPixelCoordf> projectedPoints_distorted; //!<
510  // At
511  // output, only will have an empty vector if the checkerboard was
512  // not found in this image, or the predicted (reprojected) corners,
513  // which were used to estimate the average square error.
514  // std::vector<TPixelCoordf> projectedPoints_undistorted;
515  // //!<
516  // At
517  // output, like projectedPoints_distorted but for the undistorted
518  // image.
519  const size_t idx = valid_image_pair_indices[i];
520 
521  TImageCalibData& dat_l = images[idx].left;
522  TImageCalibData& dat_r = images[idx].right;
523 
524  // Rectify image.
527 
528  // Camera poses:
529  dat_l.reconstructed_camera_pose = -lm_stat.left_cam_poses[idx];
531  -(lm_stat.right2left_pose + lm_stat.left_cam_poses[idx]);
532 
533  // Project distorted images:
535  obj_points, out.cam_params.leftCamera,
537  dat_l.projectedPoints_distorted, true);
539  obj_points, out.cam_params.rightCamera,
541  dat_r.projectedPoints_distorted, true);
542 
543  // Draw corners:
545  dat_l.projectedPoints_distorted, check_size.x, check_size.y);
547  dat_r.projectedPoints_distorted, check_size.x, check_size.y);
548  }
549 
550  return true;
551  }
552  catch (std::exception& e)
553  {
554  std::cout << e.what() << std::endl;
555  return false;
556  }
557 }
558 
559 /*
560 Jacobian:
561 
562  d b( [px py pz] )
563 ------------------ (5x3) =
564  d{ px py pz }
565 
566 \left(\begin{array}{ccc} \frac{1}{\mathrm{pz}} & 0 &
567 -\frac{\mathrm{px}}{{\mathrm{pz}}^2}\\ 0 & \frac{1}{\mathrm{pz}} &
568 -\frac{\mathrm{py}}{{\mathrm{pz}}^2} \end{array}\right)
569 
570 */
571 
573  const TPoint3D& p, // 3D coordinates wrt the camera
574  Eigen::Matrix<double, 2, 3>& G)
575 {
576  const double pz_ = 1 / p.z;
577  const double pz_2 = pz_ * pz_;
578 
579  G(0, 0) = pz_;
580  G(0, 1) = 0;
581  G(0, 2) = -p.x * pz_2;
582 
583  G(1, 0) = 0;
584  G(1, 1) = pz_;
585  G(1, 2) = -p.y * pz_2;
586 }
587 
588 /*
589 Jacobian:
590 
591  dh( b,c )
592 ----------- = Hb | 2x2 (b=[x=px/pz y=py/pz])
593  d{ b }
594 
595 \left(\begin{array}{cc} \mathrm{fx}\, \left(\mathrm{k2}\, {\left(x^2 +
596 y^2\right)}^2 + \mathrm{k3}\, {\left(x^2 + y^2\right)}^3 + 6\, \mathrm{t2}\, x +
597 2\, \mathrm{t1}\, y + x\, \left(2\, \mathrm{k1}\, x + 4\, \mathrm{k2}\, x\,
598 \left(x^2 + y^2\right) + 6\, \mathrm{k3}\, x\, {\left(x^2 + y^2\right)}^2\right)
599 + \mathrm{k1}\, \left(x^2 + y^2\right) + 1\right) & \mathrm{fx}\, \left(2\,
600 \mathrm{t1}\, x + 2\, \mathrm{t2}\, y + x\, \left(2\, \mathrm{k1}\, y + 4\,
601 \mathrm{k2}\, y\, \left(x^2 + y^2\right) + 6\, \mathrm{k3}\, y\, {\left(x^2 +
602 y^2\right)}^2\right)\right)\\ \mathrm{fy}\, \left(2\, \mathrm{t1}\, x + 2\,
603 \mathrm{t2}\, y + y\, \left(2\, \mathrm{k1}\, x + 4\, \mathrm{k2}\, x\,
604 \left(x^2 + y^2\right) + 6\, \mathrm{k3}\, x\, {\left(x^2 +
605 y^2\right)}^2\right)\right) & \mathrm{fy}\, \left(\mathrm{k2}\, {\left(x^2 +
606 y^2\right)}^2 + \mathrm{k3}\, {\left(x^2 + y^2\right)}^3 + 2\, \mathrm{t2}\, x +
607 6\, \mathrm{t1}\, y + y\, \left(2\, \mathrm{k1}\, y + 4\, \mathrm{k2}\, y\,
608 \left(x^2 + y^2\right) + 6\, \mathrm{k3}\, y\, {\left(x^2 + y^2\right)}^2\right)
609 + \mathrm{k1}\, \left(x^2 + y^2\right) + 1\right) \end{array}\right)
610 
611  dh( b,c )
612 ----------- = Hc | 2x9
613  d{ c }
614 
615 \left(\begin{array}{ccccccccc} \mathrm{t2}\, \left(3\, x^2 + y^2\right) + x\,
616 \left(\mathrm{k2}\, {\left(x^2 + y^2\right)}^2 + \mathrm{k3}\, {\left(x^2 +
617 y^2\right)}^3 + \mathrm{k1}\, \left(x^2 + y^2\right) + 1\right) + 2\,
618 \mathrm{t1}\, x\, y & 0 & 1 & 0 & \mathrm{fx}\, x\, \left(x^2 + y^2\right) &
619 \mathrm{fx}\, x\, {\left(x^2 + y^2\right)}^2 & \mathrm{fx}\, x\, {\left(x^2 +
620 y^2\right)}^3 & 2\, \mathrm{fx}\, x\, y & \mathrm{fx}\, \left(3\, x^2 +
621 y^2\right)\\ 0 & \mathrm{t1}\, \left(x^2 + 3\, y^2\right) + y\,
622 \left(\mathrm{k2}\, {\left(x^2 + y^2\right)}^2 + \mathrm{k3}\, {\left(x^2 +
623 y^2\right)}^3 + \mathrm{k1}\, \left(x^2 + y^2\right) + 1\right) + 2\,
624 \mathrm{t2}\, x\, y & 0 & 1 & \mathrm{fy}\, y\, \left(x^2 + y^2\right) &
625 \mathrm{fy}\, y\, {\left(x^2 + y^2\right)}^2 & \mathrm{fy}\, y\, {\left(x^2 +
626 y^2\right)}^3 & \mathrm{fy}\, \left(x^2 + 3\, y^2\right) & 2\, \mathrm{fy}\, x\,
627 y \end{array}\right)
628 
629 */
630 
632  const TPoint3D& nP, // Point in relative coords wrt the camera
633  const Eigen::Matrix<double, 9, 1>& c, // camera parameters
634  Eigen::Matrix<double, 2, 2>& Hb, Eigen::Matrix<double, 2, 9>& Hc)
635 {
636  const double x = nP.x / nP.z;
637  const double y = nP.y / nP.z;
638 
639  const double r2 = x * x + y * y;
640  const double r = std::sqrt(r2);
641  const double r6 = r2 * r2 * r2; // (x^2+y^2)^3 = r^6
642 
643  // c=[fx fy cx cy k1 k2 k3 t1 t2]
644  const double fx = c[0], fy = c[1];
645  // const double cx=c[2], cy=c[3]; // Un-unused
646  const double k1 = c[4], k2 = c[5], k3 = c[6];
647  const double t1 = c[7], t2 = c[8];
648 
649  // Hb = dh(b,c)/db (2x2)
650  Hb(0, 0) =
651  fx * (k2 * r2 + k3 * r6 + 6 * t2 * x + 2 * t1 * y +
652  x * (2 * k1 * x + 4 * k2 * x * r + 6 * k3 * x * r2) + k1 * r + 1);
653  Hb(0, 1) = fx * (2 * t1 * x + 2 * t2 * y +
654  x * (2 * k1 * y + 4 * k2 * y * r + 6 * k3 * y * r2));
655 
656  Hb(1, 0) = fy * (2 * t1 * x + 2 * t2 * y +
657  y * (2 * k1 * x + 4 * k2 * x * r + 6 * k3 * x * r2));
658  Hb(1, 1) =
659  fy * (k2 * r2 + k3 * r6 + 2 * t2 * x + 6 * t1 * y +
660  y * (2 * k1 * y + 4 * k2 * y * r + 6 * k3 * y * r2) + k1 * r + 1);
661 
662  // Hc = dh(b,c)/dc (2x9)
663  Hc(0, 0) = t2 * (3 * x * x + y * y) + x * (k2 * r2 + k3 * r6 + k1 * r + 1) +
664  2 * t1 * x * y;
665  Hc(0, 1) = 0;
666  Hc(0, 2) = 1;
667  Hc(0, 3) = 0;
668  Hc(0, 4) = fx * x * r;
669  Hc(0, 5) = fx * x * r2;
670  Hc(0, 6) = fx * x * r6;
671  Hc(0, 7) = 2 * fx * x * y;
672  Hc(0, 8) = fx * (3 * x * x + y * y);
673 
674  Hc(1, 0) = 0;
675  Hc(1, 1) = t1 * (x * x + 3 * y * y) + y * (k2 * r2 + k3 * r6 + k1 * r + 1) +
676  2 * t2 * x * y;
677  Hc(1, 2) = 0;
678  Hc(1, 3) = 1;
679  Hc(1, 4) = fy * y * r;
680  Hc(1, 5) = fy * y * r2;
681  Hc(1, 6) = fy * y * r6;
682  Hc(1, 7) = fy * (x * x + 3 * y * y);
683  Hc(1, 8) = 2 * fy * x * y;
684 }
685 
687  const TPoint3D& p_D, // D (+) p
688  Eigen::Matrix<double, 3, 6>& dpl_del)
689 {
690  // Jacobian 10.3.4 in technical report "A tutorial on SE(3) transformation
691  // parameterizations and on-manifold optimization"
692  dpl_del.block<3, 3>(0, 0).setIdentity();
693  dpl_del.block<3, 3>(0, 3) = mrpt::math::skew_symmetric3_neg(p_D);
694 }
695 
697  const CPose3D& A, const CPose3D& D, const TPoint3D& p,
698  Eigen::Matrix<double, 3, 6>& dp_deps)
699 {
700  // Jacobian 10.3.7 in technical report "A tutorial on SE(3) transformation
701  // parameterizations and on-manifold optimization"
702  const Eigen::Matrix<double, 1, 3> P(p.x, p.y, p.z);
703  const Eigen::Matrix<double, 1, 3> dr1 =
704  D.getRotationMatrix().block<1, 3>(0, 0);
705  const Eigen::Matrix<double, 1, 3> dr2 =
706  D.getRotationMatrix().block<1, 3>(1, 0);
707  const Eigen::Matrix<double, 1, 3> dr3 =
708  D.getRotationMatrix().block<1, 3>(2, 0);
709 
710  const Eigen::Matrix<double, 1, 3> v(
711  P.dot(dr1) + D.x(), P.dot(dr2) + D.y(), P.dot(dr3) + D.z());
712 
713  Eigen::Matrix<double, 3, 6> H;
714  H.block<3, 3>(0, 0).setIdentity();
715  H.block<3, 3>(0, 3) = mrpt::math::skew_symmetric3_neg(v);
716 
717  dp_deps.noalias() = A.getRotationMatrix() * H;
718 }
719 
722  const CPose3D& cameraPose, mrpt::img::TPixelCoordf& px)
723 {
724  // Change the reference system to that wrt the camera
725  TPoint3D nP;
726  cameraPose.composePoint(P.x, P.y, P.z, nP.x, nP.y, nP.z);
727 
728  // Pinhole model:
729  const double x = nP.x / nP.z;
730  const double y = nP.y / nP.z;
731 
732  // Radial distortion:
733  const double r2 = square(x) + square(y);
734  const double r4 = square(r2);
735  const double r6 = r2 * r4;
736  const double A =
737  1 + params.dist[0] * r2 + params.dist[1] * r4 + params.dist[4] * r6;
738  const double B = 2 * x * y;
739 
740  px.x = params.cx() +
741  params.fx() * (x * A + params.dist[2] * B +
742  params.dist[3] * (r2 + 2 * square(x)));
743  px.y = params.cy() +
744  params.fy() * (y * A + params.dist[3] * B +
745  params.dist[2] * (r2 + 2 * square(y)));
746 }
747 
748 // Build the "-gradient" and the Hessian matrix:
749 // Variables are in these order:
750 // * N x Manifold Epsilon of left camera pose (6)
751 // * Manifold Epsilon of right2left pose (6)
752 // * Left-cam-params (<=9)
753 // * Right-cam-params (<=9)
755  const TResidualJacobianList& res_jac, const std::vector<size_t>& var_indxs,
756  Eigen::VectorXd& minus_g, Eigen::MatrixXd& H)
757 {
758  const size_t N = res_jac.size(); // Number of stereo image pairs
759  const size_t nMaxUnknowns = N * 6 + 6 + 9 + 9;
760 
761  // Reset to zeros:
762  Eigen::VectorXd minus_g_tot = Eigen::VectorXd::Zero(nMaxUnknowns);
763  Eigen::MatrixXd H_tot = Eigen::MatrixXd::Zero(nMaxUnknowns, nMaxUnknowns);
764 
765  // Sum the contribution from each observation:
766  for (size_t i = 0; i < N; i++)
767  {
768  const size_t nObs = res_jac[i].size();
769  for (size_t j = 0; j < nObs; j++)
770  {
771  const TResidJacobElement& rje = res_jac[i][j];
772  // Sub-Hessian & sub-gradient are variables in this order:
773  // eps_left_cam, eps_right2left_cam, left_cam_params,
774  // right_cam_params
775  const Eigen::Matrix<double, 30, 30> Hij = rje.J.transpose() * rje.J;
776  const Eigen::Matrix<double, 30, 1> gij =
777  rje.J.transpose() * rje.residual;
778 
779  // Assemble in their place:
780  minus_g_tot.block<6, 1>(i * 6, 0) -= gij.block<6, 1>(0, 0);
781  minus_g_tot.block<6 + 9 + 9, 1>(N * 6, 0) -=
782  gij.block<6 + 9 + 9, 1>(6, 0);
783 
784  H_tot.block<6, 6>(i * 6, i * 6) += Hij.block<6, 6>(0, 0);
785 
786  H_tot.block<6 + 9 + 9, 6 + 9 + 9>(N * 6, N * 6) +=
787  Hij.block<6 + 9 + 9, 6 + 9 + 9>(6, 6);
788  H_tot.block<6, 6 + 9 + 9>(i * 6, N * 6) +=
789  Hij.block<6, 6 + 9 + 9>(0, 6);
790  H_tot.block<6 + 9 + 9, 6>(N * 6, i * 6) +=
791  Hij.block<6 + 9 + 9, 6>(6, 0);
792  }
793  }
794 
795 #if 0
796  // Also include additional cost terms to stabilize estimation:
797  // -----------------------------------------------------------------
798  // In the left->right pose increment: Add cost if we know that both cameras are almost parallel:
799  const double cost_lr_angular = 1e10;
800  H_tot.block<3,3>(N*6+3,N*6+3) += Eigen::Matrix<double,3,3>::Identity() * cost_lr_angular;
801 #endif
802 
803  // Ignore those derivatives of "fixed" (non-optimizable) variables in camera
804  // parameters
805  // --------------------------------------------------------------------------------------
806  const size_t N_Cs = var_indxs.size(); // [0-8]
807  const size_t nUnknowns = N * 6 + 6 + 2 * N_Cs;
808  const size_t nUnkPoses = N * 6 + 6;
809 
810  minus_g.setZero(nUnknowns);
811  H.setZero(nUnknowns, nUnknowns);
812  // Copy unmodified all but the cam. params:
813  minus_g.block(0, 0, nUnkPoses, 1) = minus_g_tot.block(0, 0, nUnkPoses, 1);
814  H.block(0, 0, nUnkPoses, nUnkPoses) =
815  H_tot.block(0, 0, nUnkPoses, nUnkPoses);
816 
817  // Selective copy cam. params parts:
818  for (size_t i = 0; i < N_Cs; i++)
819  {
820  minus_g[nUnkPoses + i] = minus_g_tot[nUnkPoses + var_indxs[i]];
821  minus_g[nUnkPoses + N_Cs + i] =
822  minus_g_tot[nUnkPoses + 9 + var_indxs[i]];
823  }
824 
825  for (size_t k = 0; k < nUnkPoses; k++)
826  {
827  for (size_t i = 0; i < N_Cs; i++)
828  {
829  H(nUnkPoses + i, k) = H(k, nUnkPoses + i) =
830  H_tot(k, nUnkPoses + var_indxs[i]);
831  H(nUnkPoses + i + N_Cs, k) = H(k, nUnkPoses + i + N_Cs) =
832  H_tot(k, nUnkPoses + 9 + var_indxs[i]);
833  }
834  }
835 
836  for (size_t i = 0; i < N_Cs; i++)
837  {
838  for (size_t j = 0; j < N_Cs; j++)
839  {
840  H(nUnkPoses + i, nUnkPoses + j) =
841  H_tot(nUnkPoses + var_indxs[i], nUnkPoses + var_indxs[j]);
842  H(nUnkPoses + N_Cs + i, nUnkPoses + N_Cs + j) = H_tot(
843  nUnkPoses + 9 + var_indxs[i], nUnkPoses + 9 + var_indxs[j]);
844  }
845  }
846 
847 #if 0
848  {
849  CMatrixDouble M1 = H_tot;
850  CMatrixDouble g1 = minus_g_tot;
851  M1.saveToTextFile("H1.txt");
852  g1.saveToTextFile("g1.txt");
853 
854  CMatrixDouble M2 = H;
855  CMatrixDouble g2 = minus_g;
856  M2.saveToTextFile("H2.txt");
857  g2.saveToTextFile("g2.txt");
859  }
860 #endif
861 
862 } // end of build_linear_system
863 
864 // * N x Manifold Epsilon of left camera pose (6)
865 // * Manifold Epsilon of right2left pose (6)
866 // * Left-cam-params (<=9)
867 // * Right-cam-params (<=9)
869  const Eigen::VectorXd& eps, const std::vector<size_t>& var_indxs,
870  lm_stat_t& lm_stat)
871 {
872  // Increment of the N cam poses
873  const size_t N = lm_stat.valid_image_pair_indices.size();
874  for (size_t i = 0; i < N; i++)
875  {
876  CPose3D& cam_pose =
877  lm_stat.left_cam_poses[lm_stat.valid_image_pair_indices[i]];
878 
879  // Use the Lie Algebra methods for the increment:
880  const CArrayDouble<6> incr(&eps[6 * i]);
881  const CPose3D incrPose = CPose3D::exp(incr);
882 
883  // new_pose = old_pose [+] delta
884  // = exp(delta) (+) old_pose
885  cam_pose.composeFrom(incrPose, cam_pose);
886  }
887 
888  // Increment of the right-left pose:
889  {
890  // Use the Lie Algebra methods for the increment:
891  const CArrayDouble<6> incr(&eps[6 * N]);
892  const CPose3D incrPose = CPose3D::exp(incr);
893 
894  // new_pose = old_pose [+] delta
895  // = exp(delta) (+) old_pose
896  lm_stat.right2left_pose.composeFrom(incrPose, lm_stat.right2left_pose);
897  }
898 
899  // Increment of the camera params:
900  const size_t idx = 6 * N + 6;
901  const size_t nPC =
902  var_indxs.size(); // Number of camera parameters being optimized
903  for (size_t i = 0; i < nPC; i++)
904  {
905  lm_stat.left_cam_params[var_indxs[i]] += eps[idx + i];
906  lm_stat.right_cam_params[var_indxs[i]] += eps[idx + nPC + i];
907  }
908 
909 } // end of add_lm_increment
910 
911 #ifdef USE_NUMERIC_JACOBIANS
912 // ---------------------------------------------------------------
913 // Aux. function, only if we evaluate Jacobians numerically:
914 // ---------------------------------------------------------------
915 struct TNumJacobData
916 {
917  const lm_stat_t& lm_stat;
918  const TPoint3D& obj_point;
919  const CPose3D& left_cam;
920  const CPose3D& right2left;
921  const Eigen::Matrix<double, 4, 1>& real_obs;
922 
923  TNumJacobData(
924  const lm_stat_t& _lm_stat, const TPoint3D& _obj_point,
925  const CPose3D& _left_cam, const CPose3D& _right2left,
926  const Eigen::Matrix<double, 4, 1>& _real_obs)
927  : lm_stat(_lm_stat),
928  obj_point(_obj_point),
929  left_cam(_left_cam),
930  right2left(_right2left),
931  real_obs(_real_obs)
932  {
933  }
934 };
935 
936 void numeric_jacob_eval_function(
937  const CArrayDouble<30>& x, const TNumJacobData& dat, CArrayDouble<4>& out)
938 {
939  // Recover the state out from "x":
940  const CArrayDouble<6> incr_l(&x[0]);
941  const CPose3D incrPose_l = CPose3D::exp(incr_l);
942  const CArrayDouble<6> incr_rl(&x[6]);
943  const CPose3D incrPose_rl = CPose3D::exp(incr_rl);
944 
945  // [fx fy cx cy k1 k2 k3 t1 t2]
946  TStereoCamera cam_params;
947  CArrayDouble<9> left_cam_params = x.segment<9>(6 + 6);
948  cam_params.leftCamera.fx(left_cam_params[0]);
949  cam_params.leftCamera.fy(left_cam_params[1]);
950  cam_params.leftCamera.cx(left_cam_params[2]);
951  cam_params.leftCamera.cy(left_cam_params[3]);
952  cam_params.leftCamera.k1(left_cam_params[4]);
953  cam_params.leftCamera.k2(left_cam_params[5]);
954  cam_params.leftCamera.k3(left_cam_params[6]);
955  cam_params.leftCamera.p1(left_cam_params[7]);
956  cam_params.leftCamera.p2(left_cam_params[8]);
957 
958  // [fx fy cx cy k1 k2 k3 t1 t2]
959  CArrayDouble<9> right_cam_params = x.segment<9>(6 + 6 + 9);
960  cam_params.rightCamera.fx(right_cam_params[0]);
961  cam_params.rightCamera.fy(right_cam_params[1]);
962  cam_params.rightCamera.cx(right_cam_params[2]);
963  cam_params.rightCamera.cy(right_cam_params[3]);
964  cam_params.rightCamera.k1(right_cam_params[4]);
965  cam_params.rightCamera.k2(right_cam_params[5]);
966  cam_params.rightCamera.k3(right_cam_params[6]);
967  cam_params.rightCamera.p1(right_cam_params[7]);
968  cam_params.rightCamera.p2(right_cam_params[8]);
969 
970  TPixelCoordf px_l;
972  dat.obj_point, cam_params.leftCamera, incrPose_l + dat.left_cam, px_l);
973  TPixelCoordf px_r;
975  dat.obj_point, cam_params.rightCamera,
976  incrPose_rl + dat.right2left + incrPose_l + dat.left_cam, px_r);
977 
978  const Eigen::Matrix<double, 4, 1> predicted_obs(
979  px_l.x, px_l.y, px_r.x, px_r.y);
980 
981  // Residual:
982  out = predicted_obs; // - dat.real_obs;
983 }
984 
985 void eval_h_b(
986  const CArrayDouble<2>& X, const TCamera& params, CArrayDouble<2>& out)
987 {
988  // Radial distortion:
989  const double x = X[0], y = X[1];
990  const double r2 = square(x) + square(y);
991  const double r4 = square(r2);
992  const double r6 = r2 * r4;
993  const double A =
994  1 + params.dist[0] * r2 + params.dist[1] * r4 + params.dist[4] * r6;
995  const double B = 2 * x * y;
996 
997  out[0] = params.cx() +
998  params.fx() * (x * A + params.dist[2] * B +
999  params.dist[3] * (r2 + 2 * square(x)));
1000  out[1] = params.cy() +
1001  params.fy() * (y * A + params.dist[3] * B +
1002  params.dist[2] * (r2 + 2 * square(y)));
1003 }
1004 
1005 void eval_b_p(const CArrayDouble<3>& P, const int& dummy, CArrayDouble<2>& b)
1006 {
1007  // Radial distortion:
1008  b[0] = P[0] / P[2];
1009  b[1] = P[1] / P[2];
1010 }
1011 
1012 void eval_deps_D_p(
1013  const CArrayDouble<6>& eps, const TPoint3D& D_p, CArrayDouble<3>& out)
1014 {
1015  const CArrayDouble<6> incr(&eps[0]);
1016  const CPose3D incrPose = CPose3D::exp(incr);
1017  TPoint3D D_p_out;
1018  incrPose.composePoint(D_p, D_p_out);
1019  for (int i = 0; i < 3; i++) out[i] = D_p_out[i];
1020 }
1021 
1022 struct TEvalData_A_eps_D_p
1023 {
1024  CPose3D A, D;
1025  TPoint3D p;
1026 };
1027 
1028 void eval_dA_eps_D_p(
1029  const CArrayDouble<6>& eps, const TEvalData_A_eps_D_p& dat,
1030  CArrayDouble<3>& out)
1031 {
1032  const CArrayDouble<6> incr(&eps[0]);
1033  const CPose3D incrPose = CPose3D::exp(incr);
1034  const CPose3D A_eps_D = dat.A + (incrPose + dat.D);
1035  TPoint3D pt;
1036  A_eps_D.composePoint(dat.p, pt);
1037  for (int i = 0; i < 3; i++) out[i] = pt[i];
1038 }
1039 // ---------------------------------------------------------------
1040 // End of aux. function, only if we evaluate Jacobians numerically:
1041 // ---------------------------------------------------------------
1042 #endif
1043 
1044 // the 4x1 prediction are the (u,v) pixel coordinates for the left / right
1045 // cameras:
1047  const lm_stat_t& lm_stat, TResidualJacobianList& res_jac,
1048  bool use_robust_kernel, double kernel_param)
1049 {
1050  double total_err = 0;
1051  const size_t N = lm_stat.valid_image_pair_indices.size();
1052  res_jac.resize(N);
1053 
1054  // Parse lm_stat data: CArrayDouble<9> left_cam_params, right_cam_params; //
1055  // [fx fy cx cy k1 k2 k3 t1 t2]
1056  TCamera camparam_l;
1057  camparam_l.fx(lm_stat.left_cam_params[0]);
1058  camparam_l.fy(lm_stat.left_cam_params[1]);
1059  camparam_l.cx(lm_stat.left_cam_params[2]);
1060  camparam_l.cy(lm_stat.left_cam_params[3]);
1061  camparam_l.k1(lm_stat.left_cam_params[4]);
1062  camparam_l.k2(lm_stat.left_cam_params[5]);
1063  camparam_l.k3(lm_stat.left_cam_params[6]);
1064  camparam_l.p1(lm_stat.left_cam_params[7]);
1065  camparam_l.p2(lm_stat.left_cam_params[8]);
1066  TCamera camparam_r;
1067  camparam_r.fx(lm_stat.right_cam_params[0]);
1068  camparam_r.fy(lm_stat.right_cam_params[1]);
1069  camparam_r.cx(lm_stat.right_cam_params[2]);
1070  camparam_r.cy(lm_stat.right_cam_params[3]);
1071  camparam_r.k1(lm_stat.right_cam_params[4]);
1072  camparam_r.k2(lm_stat.right_cam_params[5]);
1073  camparam_r.k3(lm_stat.right_cam_params[6]);
1074  camparam_r.p1(lm_stat.right_cam_params[7]);
1075  camparam_r.p2(lm_stat.right_cam_params[8]);
1076 
1077  // process all points from all N image pairs:
1078  for (size_t k = 0; k < N; k++)
1079  {
1080  const size_t k_idx = lm_stat.valid_image_pair_indices[k];
1081  const size_t nPts = lm_stat.obj_points.size();
1082  res_jac[k].resize(nPts);
1083  // For each point in the pattern:
1084  for (size_t i = 0; i < nPts; i++)
1085  {
1086  // Observed corners:
1087  const TPixelCoordf& px_obs_l =
1088  lm_stat.images[k_idx].left.detected_corners[i];
1089  const TPixelCoordf& px_obs_r =
1090  lm_stat.images[k_idx].right.detected_corners[i];
1091  const Eigen::Matrix<double, 4, 1> obs(
1092  px_obs_l.x, px_obs_l.y, px_obs_r.x, px_obs_r.y);
1093 
1094  TResidJacobElement& rje = res_jac[k][i];
1095 
1096  // Predict i'th corner on left & right cameras:
1097  // ---------------------------------------------
1098  TPixelCoordf px_l, px_r;
1099  project_point(
1100  lm_stat.obj_points[i], camparam_l,
1101  lm_stat.left_cam_poses[k_idx], px_l);
1102  project_point(
1103  lm_stat.obj_points[i], camparam_r,
1104  lm_stat.right2left_pose + lm_stat.left_cam_poses[k_idx], px_r);
1105  rje.predicted_obs =
1106  Eigen::Matrix<double, 4, 1>(px_l.x, px_l.y, px_r.x, px_r.y);
1107 
1108  // Residual:
1109  rje.residual = rje.predicted_obs - obs;
1110 
1111  // Accum. total squared error:
1112  const double err_sqr_norm = rje.residual.squaredNorm();
1113  if (use_robust_kernel)
1114  {
1116  rk.param_sq = kernel_param;
1117 
1118  double kernel_1st_deriv, kernel_2nd_deriv;
1119  const double scaled_err =
1120  rk.eval(err_sqr_norm, kernel_1st_deriv, kernel_2nd_deriv);
1121 
1122  rje.residual *= kernel_1st_deriv;
1123  total_err += scaled_err;
1124  }
1125  else
1126  total_err += err_sqr_norm;
1127 
1128 // ---------------------------------------------------------------------------------
1129 // Jacobian: (4x30)
1130 // See technical report cited in the headers for the theory behind these
1131 // formulas.
1132 // ---------------------------------------------------------------------------------
1133 #if !defined(USE_NUMERIC_JACOBIANS) || defined(COMPARE_NUMERIC_JACOBIANS)
1134  // ----- Theoretical Jacobians -----
1135  Eigen::Matrix<double, 2, 6> dhl_del, dhr_del, dhr_der;
1136  Eigen::Matrix<double, 2, 9> dhl_dcl, dhr_dcr;
1137 
1138  // 3D coordinates of the corner point wrt both cameras:
1139  TPoint3D pt_wrt_left, pt_wrt_right;
1140  lm_stat.left_cam_poses[k_idx].composePoint(
1141  lm_stat.obj_points[i], pt_wrt_left);
1142  (lm_stat.right2left_pose + lm_stat.left_cam_poses[k_idx])
1143  .composePoint(lm_stat.obj_points[i], pt_wrt_right);
1144 
1145  // Build partial Jacobians:
1146  Eigen::Matrix<double, 2, 2> dhl_dbl, dhr_dbr;
1148  pt_wrt_left, lm_stat.left_cam_params, dhl_dbl, dhl_dcl);
1150  pt_wrt_right, lm_stat.right_cam_params, dhr_dbr, dhr_dcr);
1151 
1152 #if 0
1153  // Almost exact....
1154  {
1155  CArrayDouble<2> x0;
1156  TPoint3D nP = pt_wrt_left;
1157  x0[0] = nP.x/nP.z;
1158  x0[1] = nP.y/nP.z;
1159 
1160  CArrayDouble<2> x_incrs;
1161  x_incrs.setConstant(1e-6);
1162 
1163  Eigen::Matrix<double,2,2> num_dhl_dbl, num_dhr_dbr;
1164  mrpt::math::estimateJacobian(x0, &eval_h_b, x_incrs, camparam_l, num_dhl_dbl );
1165 
1166  nP = pt_wrt_right;
1167  x0[0] = nP.x/nP.z;
1168  x0[1] = nP.y/nP.z;
1169  mrpt::math::estimateJacobian(x0, &eval_h_b, x_incrs, camparam_r, num_dhr_dbr );
1170 
1171  cout << "num_dhl_dbl:\n" << num_dhl_dbl << "\ndiff dhl_dbl:\n" << dhl_dbl-num_dhl_dbl << endl << endl;
1172  cout << "num_dhr_dbr:\n" << num_dhr_dbr << "\ndiff dhr_dbr:\n" << dhr_dbr-num_dhr_dbr << endl << endl;
1173 
1174  }
1175 #endif
1176 
1177  Eigen::Matrix<double, 2, 3> dbl_dpl, dbr_dpr;
1178  jacob_db_dp(pt_wrt_left, dbl_dpl);
1179  jacob_db_dp(pt_wrt_right, dbr_dpr);
1180 
1181 #if 0
1182  // OK! 100% exact.
1183  {
1184  CArrayDouble<3> x0;
1185  x0[0]=pt_wrt_left.x;
1186  x0[1]=pt_wrt_left.y;
1187  x0[2]=pt_wrt_left.z;
1188 
1189  CArrayDouble<3> x_incrs;
1190  x_incrs.setConstant(1e-8);
1191 
1192  Eigen::Matrix<double,2,3> num_dbl_dpl, num_dbr_dpr;
1193  const int dumm=0;
1194  mrpt::math::estimateJacobian(x0, &eval_b_p, x_incrs, dumm, num_dbl_dpl );
1195 
1196  x0[0]=pt_wrt_right.x;
1197  x0[1]=pt_wrt_right.y;
1198  x0[2]=pt_wrt_right.z;
1199  mrpt::math::estimateJacobian(x0, &eval_b_p, x_incrs, dumm, num_dbr_dpr );
1200 
1201  cout << "num_dbl_dpl:\n" << num_dbl_dpl << "\ndbl_dpl:\n" << dbl_dpl << endl << endl;
1202  cout << "num_dbr_dpr:\n" << num_dbr_dpr << "\ndbr_dpr:\n" << dbr_dpr << endl << endl;
1203 
1204  }
1205 #endif
1206 
1207  // p_l = exp(epsilon_l) (+) pose_left (+) point_ij
1208  // p_l = [exp(epsilon_r) (+) pose_right2left] (+) [exp(epsilon_l)
1209  // (+) pose_left] (+) point_ij
1210  Eigen::Matrix<double, 3, 6> dpl_del, dpr_del, dpr_der;
1211  jacob_deps_D_p_deps(pt_wrt_left, dpl_del);
1212  jacob_deps_D_p_deps(pt_wrt_right, dpr_der);
1214  lm_stat.right2left_pose, lm_stat.left_cam_poses[k_idx],
1215  lm_stat.obj_points[i], dpr_del);
1216 
1217 #if 0
1218  // 100% Exact.
1219  {
1220  // Test jacob_deps_D_p_deps:
1221  CArrayDouble<6> x0;
1222  x0.setConstant(0);
1223 
1224  CArrayDouble<6> x_incrs;
1225  x_incrs.setConstant(1e-8);
1226 
1227  Eigen::Matrix<double,3,6> num_dpl_del, num_dpr_der;
1228  mrpt::math::estimateJacobian(x0, &eval_deps_D_p, x_incrs, pt_wrt_left , num_dpl_del );
1229  mrpt::math::estimateJacobian(x0, &eval_deps_D_p, x_incrs, pt_wrt_right, num_dpr_der );
1230 
1231  cout << "num_dpl_del:\n" << num_dpl_del << "\ndiff dpl_del:\n" << dpl_del-num_dpl_del << endl << endl;
1232  cout << "num_dpr_der:\n" << num_dpr_der << "\ndiff dpr_der:\n" << dpr_der-num_dpr_der << endl << endl;
1233  }
1234 #endif
1235 
1236 #if 0
1237  // 100% Exact.
1238  {
1239  // Test jacob_dA_eps_D_p_deps:
1240  CArrayDouble<6> x0;
1241  x0.setConstant(0);
1242 
1243  CArrayDouble<6> x_incrs;
1244  x_incrs.setConstant(1e-8);
1245 
1246  TEvalData_A_eps_D_p dat;
1247  dat.A = lm_stat.right2left_pose;
1248  dat.D = lm_stat.left_cam_poses[k_idx];
1249  dat.p = lm_stat.obj_points[i];
1250 
1251  Eigen::Matrix<double,3,6> num_dpr_del;
1252  mrpt::math::estimateJacobian(x0, &eval_dA_eps_D_p, x_incrs,dat , num_dpr_del );
1253 
1254  cout << "num_dpr_del:\n" << num_dpr_del << "\ndiff dpr_del:\n" << num_dpr_del-dpr_del << endl << endl;
1255  }
1256 #endif
1257 
1258  // Jacobian chain rule:
1259  dhl_del = dhl_dbl * dbl_dpl * dpl_del;
1260  dhr_del = dhr_dbr * dbr_dpr * dpr_del;
1261  dhr_der = dhr_dbr * dbr_dpr * dpr_der;
1262 
1263  rje.J.setZero(4, 30);
1264  rje.J.block<2, 6>(0, 0) = dhl_del;
1265  rje.J.block<2, 6>(2, 0) = dhr_del;
1266  rje.J.block<2, 6>(2, 6) = dhr_der;
1267  rje.J.block<2, 9>(0, 12) = dhl_dcl;
1268  rje.J.block<2, 9>(2, 21) = dhr_dcr;
1269 
1270 #if defined(COMPARE_NUMERIC_JACOBIANS)
1271  const Eigen::Matrix<double, 4, 30> J_theor = rje.J;
1272 #endif
1273 // ---- end of theoretical Jacobians ----
1274 #endif
1275 
1276 #if defined(USE_NUMERIC_JACOBIANS) || defined(COMPARE_NUMERIC_JACOBIANS)
1277  // ----- Numeric Jacobians ----
1278 
1279  CArrayDouble<30> x0; // eps_l (6) + eps_lr (6) + l_camparams (9) +
1280  // r_camparams (9)
1281  x0.setZero();
1282  x0.segment<9>(6 + 6) = lm_stat.left_cam_params;
1283  x0.segment<9>(6 + 6 + 9) = lm_stat.right_cam_params;
1284 
1285  const double x_incrs_val[30] = {
1286  1e-6, 1e-6, 1e-6, 1e-7, 1e-7, 1e-7, // eps_l
1287  1e-6, 1e-6, 1e-6, 1e-7, 1e-7, 1e-7, // eps_r
1288  1e-3, 1e-3, 1e-3, 1e-3, 1e-8, 1e-8, 1e-8, 1e-8, 1e-4, // cam_l
1289  1e-3, 1e-3, 1e-3, 1e-3, 1e-8, 1e-8, 1e-8, 1e-8, 1e-4 // cam_rl
1290  };
1291  const CArrayDouble<30> x_incrs(x_incrs_val);
1292  TNumJacobData dat(
1293  lm_stat, lm_stat.obj_points[i], lm_stat.left_cam_poses[k_idx],
1294  lm_stat.right2left_pose, obs);
1295 
1297  x0, &numeric_jacob_eval_function, x_incrs, dat, rje.J);
1298 
1299 #if defined(COMPARE_NUMERIC_JACOBIANS)
1300  const Eigen::Matrix<double, 4, 30> J_num = rje.J;
1301 #endif
1302 #endif // ---- end of numeric Jacobians ----
1303 
1304 // Only for debugging:
1305 #if defined(COMPARE_NUMERIC_JACOBIANS)
1306  // if ( (J_num-J_theor).array().abs().maxCoeff()>1e-2)
1307  {
1308  ofstream f;
1309  f.open("dbg.txt", ios_base::out | ios_base::app);
1310  f << "J_num:\n"
1311  << J_num << endl
1312  << "J_theor:\n"
1313  << J_theor << endl
1314  << "diff:\n"
1315  << J_num - J_theor << endl
1316  << "diff (ratio):\n"
1317  << (J_num - J_theor).cwiseQuotient(J_num) << endl
1318  << endl;
1319  }
1320 #endif
1321  } // for i
1322  } // for k
1323 
1324  return total_err;
1325 } // end of recompute_errors_and_Jacobians
1326 
1327 // Ctor:
1329  : check_size_x(7),
1330  check_size_y(9),
1331  check_squares_length_X_meters(0.02),
1332  check_squares_length_Y_meters(0.02),
1333  normalize_image(true),
1334  skipDrawDetectedImgs(false),
1335  verbose(true),
1336  maxIters(2000),
1337  optimize_k1(true),
1338  optimize_k2(true),
1339  optimize_k3(false),
1340  optimize_t1(false),
1341  optimize_t2(false),
1342  use_robust_kernel(false),
1343  robust_kernel_param(10),
1344  callback(nullptr),
1345  callback_user_param(nullptr)
1346 {
1347 }
1348 
1350  : final_rmse(0), final_iters(0), final_number_good_image_pairs(0)
1351 {
1352 }
double k3() const
Get the value of the k3 distortion parameter.
Definition: TCamera.h:185
double final_rmse
Final reprojection square Root Mean Square Error (in pixels).
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:140
uint32_t nrows
Definition: TCamera.h:41
mrpt::aligned_std_vector< mrpt::poses::CPose3D > left_cam_poses
Poses of the origin of coordinates of the pattern wrt the left camera (i.e.
bool drawChessboardCorners(std::vector< TPixelCoordf > &cornerCoords, unsigned int check_size_x, unsigned int check_size_y, unsigned int lines_width=1, unsigned int circles_radius=4)
Draw onto this image the detected corners of a chessboard.
Definition: CImage.cpp:2327
double fx() const
Get the value of the focal length x-value (in pixels).
Definition: TCamera.h:165
std::vector< bool > image_pair_was_used
true if a checkerboard was correctly detected in both left/right images.
Eigen::Array< double, 9, 1 > left_params_inv_variance
The inverse variance (information/precision) of each of the 9 left/right camera parameters [fx fy cx ...
const double G
size_t final_number_good_image_pairs
Number of image pairs in which valid checkerboards were correctly detected.
mrpt::aligned_std_vector< mrpt::poses::CPose3D > left_cam_poses
double fy() const
Get the value of the focal length y-value (in pixels).
Definition: TCamera.h:167
CArrayNumeric is an array for numeric types supporting several mathematical operations (actually...
Definition: CArrayNumeric.h:25
size_t getHeight() const override
Returns the height of the image in pixels.
Definition: CImage.cpp:892
size_t final_iters
Final number of optimization iterations executed.
A pair (x,y) of pixel coordinates (subpixel resolution).
Definition: TPixelCoord.h:20
STL namespace.
void jacob_dh_db_and_dh_dc(const TPoint3D &nP, const Eigen::Matrix< double, 9, 1 > &c, Eigen::Matrix< double, 2, 2 > &Hb, Eigen::Matrix< double, 2, 9 > &Hc)
Eigen::Matrix< double, 4, 1 > predicted_obs
[u_l v_l u_r v_r]: left/right camera pixels
bool isExternallyStored() const noexcept
See setExternalStorage().
Definition: img/CImage.h:793
const TCalibrationStereoImageList & images
Data associated to each observation in the Lev-Marq.
void build_linear_system(const TResidualJacobianList &res_jac, const std::vector< size_t > &var_indxs, Eigen::VectorXd &minus_g, Eigen::MatrixXd &H)
void getSize(TImageSize &s) const
Return the size of the image.
Definition: CImage.cpp:851
mrpt::img::TStereoCamera cam_params
Recovered parameters of the stereo camera.
double k2() const
Get the value of the k2 distortion parameter.
Definition: TCamera.h:179
T square(const T x)
Inline function for the square of a number.
void estimateJacobian(const VECTORLIKE &x, const std::function< void(const VECTORLIKE &x, const USERPARAM &y, VECTORLIKE3 &out)> &functor, const VECTORLIKE2 &increments, const USERPARAM &userParam, MATRIXLIKE &out_Jacobian)
Estimate the Jacobian of a multi-dimensional function around a point "x", using finite differences of...
Definition: num_jacobian.h:31
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
void skew_symmetric3_neg(const VECTOR &v, MATRIX &M)
Computes the negative version of a 3x3 skew symmetric matrix from a 3-vector or 3-array: ...
Definition: geometry.h:879
mrpt::img::CImage img_checkboard
At output, this will contain the detected checkerboard overprinted to the image.
Data associated to each image in the calibration process mrpt::vision::checkerBoardCameraCalibration ...
This base provides a set of functions for maths stuff.
size_t getWidth() const override
Returns the width of the image in pixels.
Definition: CImage.cpp:864
double cy() const
Get the value of the principal point y-coordinate (in pixels).
Definition: TCamera.h:163
Input parameters for mrpt::vision::checkerBoardStereoCalibration.
Params of the optional callback provided by the user.
void composeFrom(const CPose3D &A, const CPose3D &B)
Makes "this = A (+) B"; this method is slightly more efficient than "this= A + B;" since it avoids th...
Definition: CPose3D.cpp:565
#define ASSERT_EQUAL_(__A, __B)
Assert comparing two values, reporting their actual values upon failure.
Definition: exceptions.h:153
const GLubyte * c
Definition: glext.h:6313
bool findChessboardCorners(const mrpt::img::CImage &img, std::vector< mrpt::img::TPixelCoordf > &cornerCoords, unsigned int check_size_x, unsigned int check_size_y, bool normalize_image=true, bool useScaramuzzaMethod=false)
Look for the corners of a chessboard in the image using one of two different methods.
A pair (x,y) of pixel coordinates (integer resolution).
Definition: TPixelCoord.h:39
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
GLubyte GLubyte b
Definition: glext.h:6279
const double eps
std::vector< mrpt::img::TPixelCoordf > detected_corners
At output, the detected corners (x,y) in pixel units.
Classes for computer vision, detectors, features, etc.
Definition: CCamModel.h:20
Structure to hold the parameters of a pinhole camera model.
Definition: TCamera.h:29
const std::vector< mrpt::math::TPoint3D > & obj_points
double x
X,Y,Z coordinates.
std::vector< mrpt::aligned_std_vector< TResidJacobElement > > TResidualJacobianList
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,qz).
Definition: CPose3DQuat.h:48
void jacob_dA_eps_D_p_deps(const CPose3D &A, const CPose3D &D, const TPoint3D &p, Eigen::Matrix< double, 3, 6 > &dp_deps)
void jacob_deps_D_p_deps(const TPoint3D &p_D, Eigen::Matrix< double, 3, 6 > &dpl_del)
double p1() const
Get the value of the p1 distortion parameter.
Definition: TCamera.h:181
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
double recompute_errors_and_Jacobians(const lm_stat_t &lm_stat, TResidualJacobianList &res_jac, bool use_robust_kernel, double kernel_param)
const std::vector< size_t > & valid_image_pair_indices
void colorImage(CImage &ret) const
Returns a RGB version of the grayscale image, or itself if it is already a RGB image.
Definition: CImage.cpp:2391
void projectPoints_with_distortion(const std::vector< mrpt::math::TPoint3D > &in_points_3D, const mrpt::poses::CPose3D &cameraPose, const mrpt::math::CMatrixDouble33 &intrinsicParams, const std::vector< double > &distortionParams, std::vector< mrpt::img::TPixelCoordf > &projectedPoints, bool accept_points_behind=false)
Project a set of 3D points into a camera at an arbitrary 6D pose using its calibration matrix and dis...
Definition: pinhole.cpp:51
void pause(const std::string &msg=std::string("Press any key to continue...")) noexcept
Shows the message "Press any key to continue" (or other custom message) to the current standard outpu...
Definition: os.cpp:428
Eigen::Matrix< double, 4, 1 > residual
= predicted_obs - observations
Eigen::Matrix< double, 4, 30 > J
Jacobian.
double cx() const
Get the value of the principal point x-coordinate (in pixels).
Definition: TCamera.h:161
bool checkerBoardStereoCalibration(TCalibrationStereoImageList &images, const TStereoCalibParams &params, TStereoCalibResults &out_results)
Optimize the calibration parameters of a stereo camera or a RGB+D (Kinect) camera.
const GLdouble * v
Definition: glext.h:3678
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
void jacob_db_dp(const TPoint3D &p, Eigen::Matrix< double, 2, 3 > &G)
double current_rmse
Current root-mean square reprojection error (in pixels)
GLdouble GLdouble GLdouble r
Definition: glext.h:3705
unsigned int nImgsProcessed
Info for calibRound==-1.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:88
mrpt::math::CArrayDouble< 9 > right_cam_params
void composePoint(double lx, double ly, double lz, double &gx, double &gy, double &gz, mrpt::math::CMatrixFixedNumeric< double, 3, 3 > *out_jacobian_df_dpoint=nullptr, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dpose=nullptr, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dse3=nullptr, bool use_small_rot_approx=false) const
An alternative, slightly more efficient way of doing with G and L being 3D points and P this 6D pose...
Definition: CPose3D.cpp:379
TPixelCoord TImageSize
A type for image sizes.
Definition: TPixelCoord.h:53
TCamera leftCamera
Intrinsic and distortion parameters of the left and right cameras.
Definition: TStereoCamera.h:30
mrpt::poses::CPose3D right2left_camera_pose
The pose of the left camera as seen from the right camera.
mrpt::math::CArrayDouble< 9 > left_cam_params
GLenum GLint GLint y
Definition: glext.h:3538
double p2() const
Get the value of the p2 distortion parameter.
Definition: TCamera.h:183
double k1() const
Get the value of the k1 distortion parameter.
Definition: TCamera.h:177
void project_point(const mrpt::math::TPoint3D &P, const mrpt::img::TCamera &params, const CPose3D &cameraPose, mrpt::img::TPixelCoordf &px)
GLenum GLint x
Definition: glext.h:3538
Output results for mrpt::vision::checkerBoardStereoCalibration.
void add_lm_increment(const Eigen::VectorXd &eps, const std::vector< size_t > &var_indxs, lm_stat_t &new_lm_stat)
void getRotationMatrix(mrpt::math::CMatrixDouble33 &ROT) const
Get the 3x3 rotation matrix.
Definition: CPose3D.h:232
Lightweight 3D point.
Lightweight 2D point.
mrpt::poses::CPose3D reconstructed_camera_pose
At output, the reconstructed pose of the camera.
int calibRound
=-1:Processing images; =0: Initial calib without distortion, =1: Calib of all parameters ...
std::vector< mrpt::img::TPixelCoordf > projectedPoints_distorted
At output, only will have an empty vector if the checkerboard was not found in this image...
Eigen::Array< double, 9, 1 > right_params_inv_variance
GLfloat GLfloat p
Definition: glext.h:6305
GLenum const GLfloat * params
Definition: glext.h:3534
uint32_t ncols
Camera resolution.
Definition: TCamera.h:41
mrpt::img::CImage img_original
This image will be automatically loaded from the file name passed to checkerBoardCameraCalibration.
Structure to hold the parameters of a pinhole stereo camera model.
Definition: TStereoCamera.h:25
std::vector< TImageStereoCalibData > TCalibrationStereoImageList
A list of images, used in checkerBoardStereoCalibration.
mrpt::img::CImage img_rectified
At output, this will be the rectified image.
A class for storing images as grayscale or RGB bitmaps.
Definition: img/CImage.h:130



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019