MRPT  1.9.9
chessboard_stereo_camera_calib_internal.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, 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 #pragma once
11 
13 #include <mrpt/math/geometry.h>
14 #include <mrpt/poses/CPose3D.h>
16 #include <Eigen/Dense>
17 
18 namespace mrpt::vision
19 {
20 // State of the Lev-Marq optimization:
21 struct lm_stat_t
22 {
24  const std::vector<size_t>& valid_image_pair_indices;
25  const std::vector<mrpt::math::TPoint3D>& obj_points;
26 
27  // State being optimized:
28  // N*left_cam_pose + right2left_pose + left_cam_params + right_cam_params
29  // Poses of the origin of coordinates of the pattern wrt the left camera
30  std::vector<mrpt::math::TPose3D> left_cam_poses;
32  /** [fx fy cx cy k1 k2 k3 t1 t2] */
34 
35  // Ctor
37  const TCalibrationStereoImageList& _images,
38  const std::vector<size_t>& _valid_image_pair_indices,
39  const std::vector<mrpt::math::TPoint3D>& _obj_points)
40  : images(_images),
41  valid_image_pair_indices(_valid_image_pair_indices),
42  obj_points(_obj_points)
43  {
44  // Initial
45  left_cam_poses.assign(
46  images.size(), mrpt::math::TPose3D(0, 0, 1, 0, 0, 0));
47  }
48 
49  // Swap:
50  void swap(lm_stat_t& o)
51  {
53  std::swap(right2left_pose, o.right2left_pose);
54  std::swap(left_cam_params, o.left_cam_params);
55  std::swap(right_cam_params, o.right_cam_params);
56  }
57 };
58 
59 /** Data associated to *each observation* in the Lev-Marq. model */
61 {
62  /** [u_l v_l u_r v_r]: left/right camera pixels */
64  /** = predicted_obs - observations */
66  /** Jacobian. 4=the two predicted pixels; 30=Read below for the meaning of
67  * these 30 variables */
69 };
70 
72  std::vector<mrpt::aligned_std_vector<TResidJacobElement>>;
73 
74 // Auxiliary functions for the Lev-Marq algorithm:
76  const lm_stat_t& lm_stat, TResidualJacobianList& res_jac,
77  bool use_robust_kernel, double kernel_param);
79  const TResidualJacobianList& res_jac, const std::vector<size_t>& var_indxs,
81 void add_lm_increment(
83  const std::vector<size_t>& var_indxs, lm_stat_t& new_lm_stat);
84 } // namespace mrpt::vision
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
Eigen::Matrix< double, 4, 1 > predicted_obs
[u_l v_l u_r v_r]: left/right camera pixels
const TCalibrationStereoImageList & images
Data associated to each observation in the Lev-Marq.
void add_lm_increment(const mrpt::math::CVectorDynamic< double > &eps, const std::vector< size_t > &var_indxs, lm_stat_t &new_lm_stat)
const double eps
Classes for computer vision, detectors, features, etc.
Definition: CDifodo.h:17
const std::vector< mrpt::math::TPoint3D > & obj_points
std::vector< mrpt::aligned_std_vector< TResidJacobElement > > TResidualJacobianList
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
std::vector< mrpt::math::TPose3D > left_cam_poses
Eigen::Matrix< double, 4, 1 > residual
= predicted_obs - observations
Eigen::Matrix< double, 4, 30 > J
Jacobian.
mrpt::math::CVectorFixedDouble< 9 > right_cam_params
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Definition: TPose3D.h:24
lm_stat_t(const TCalibrationStereoImageList &_images, const std::vector< size_t > &_valid_image_pair_indices, const std::vector< mrpt::math::TPoint3D > &_obj_points)
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]
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: c7a3bec24 Sun Mar 29 18:33:13 2020 +0200 at dom mar 29 18:50:38 CEST 2020