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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 3316834cb Wed Aug 21 11:42:56 2019 +0200 at miƩ ago 21 11:50:11 CEST 2019