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



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019