MRPT  2.0.4
ba_common.cpp
Go to the documentation of this file.
1 /**
2  * Copyright (C) 2010 Hauke Strasdat
3  * Imperial College London
4  * Copyright (c) 2005-2020, Individual contributors, see AUTHORS file
5  * See: https://www.mrpt.org/Authors - All rights reserved.
6  *
7  * bundle_adjuster.h is part of RobotVision.
8  *
9  * RobotVision is free software: you can redistribute it and/or modify
10  * it under the terms of the GNU Lesser General Public License as
11  * published by the Free Software Foundation, either version 3 of the
12  * License, or any later version.
13  *
14  * RobotVision is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17  * GNU Lesser General Public License for more details.
18  *
19  * You should have received a copy of the GNU General Public License
20  * and the GNU Lesser General Public License along with this program.
21  * If not, see <http://www.gnu.org/licenses/>.
22  */
23 
24 #include "vision-lgpl-precomp.h" // Precompiled headers
25 
27 #include <mrpt/poses/Lie/SE.h>
29 #include <mrpt/vision/pinhole.h>
30 #include "ba_internals.h"
31 
32 using namespace std;
33 using namespace mrpt;
34 using namespace mrpt::vision;
35 using namespace mrpt::img;
36 using namespace mrpt::math;
37 using namespace mrpt::poses;
38 
39 /* -------------------------------------------------------------------------------------
40  ba_initial_estimate
41 
42  Fills the frames & landmark points maps with an initial gross estimate
43  from the sequence \a observations, so they can be fed to bundle adjustment
44  methods.
45  ------------------------------------------------------------------------------------
46  */
48  const TSequenceFeatureObservations& observations,
49  [[maybe_unused]] const TCamera& camera_params, TFramePosesMap& frame_poses,
50  TLandmarkLocationsMap& landmark_points)
51 {
53  // VERY CRUDE APPROACH: All camera poses at the origin, all points at
54  // (0,0,1)
55  // TODO: Improve with the data in "observations"...
56 
57  // Clear previous contents:
58  frame_poses.clear();
59  landmark_points.clear();
60 
61  // Go thru the obs list and insert poses:
62  for (TSequenceFeatureObservations::const_iterator it = observations.begin();
63  it != observations.end(); ++it)
64  {
65  const TFeatureID feat_ID = it->id_feature;
66  const TCameraPoseID frame_ID = it->id_frame;
67 
68  frame_poses[frame_ID] = CPose3D(0, 0, 0, 0, 0, 0);
69  landmark_points[feat_ID] = TPoint3D(0, 0, 1);
70  }
71  MRPT_END
72 }
73 
75  const TSequenceFeatureObservations& observations,
76  [[maybe_unused]] const TCamera& camera_params, TFramePosesVec& frame_poses,
77  TLandmarkLocationsVec& landmark_points)
78 {
80  // VERY CRUDE APPROACH: All camera poses at the origin, all points at
81  // (0,0,1)
82  // TODO: Improve with the data in "observations"...
83 
84  // Clear previous contents:
85  frame_poses.clear();
86  landmark_points.clear();
87 
88  if (observations.empty()) return;
89 
90  // Go thru the obs list and insert poses:
91 
92  TFeatureID max_pt_id = 0;
93  TCameraPoseID max_fr_id = 0;
94 
95  for (TSequenceFeatureObservations::const_iterator it = observations.begin();
96  it != observations.end(); ++it)
97  {
98  const TFeatureID feat_ID = it->id_feature;
99  const TCameraPoseID frame_ID = it->id_frame;
100  keep_max(max_fr_id, frame_ID);
101  keep_max(max_pt_id, feat_ID);
102  }
103 
104  // Insert N copies of the same values:
105  frame_poses.assign(max_fr_id + 1, CPose3D(0, 0, 0, 0, 0, 0));
106  landmark_points.assign(max_pt_id + 1, TPoint3D(0, 0, 1));
107 
108  MRPT_END
109 }
110 
111 // This function is what to do for each feature in the reprojection loops below.
112 // -> residual: the raw residual, even if using robust kernel
113 // -> sum+= scaled squared norm of the residual (which != squared norm if using
114 // robust kernel)
115 template <bool POSES_INVERSE>
117  const TCamera& camera_params, const TFeatureObservation& OBS,
118  std::array<double, 2>& out_residual,
119  const TFramePosesVec::value_type& frame,
120  const TLandmarkLocationsVec::value_type& point, double& sum,
121  const bool use_robust_kernel, const double kernel_param,
122  double* out_kernel_1st_deriv)
123 {
124  const TPixelCoordf z_pred =
125  mrpt::vision::pinhole::projectPoint_no_distortion<POSES_INVERSE>(
126  camera_params, frame, point);
127  const TPixelCoordf& z_meas = OBS.px;
128 
129  out_residual[0] = z_meas.x - z_pred.x;
130  out_residual[1] = z_meas.y - z_pred.y;
131 
132  const double sum_2 = square(out_residual[0]) + square(out_residual[1]);
133 
134  if (use_robust_kernel)
135  {
137  kernel.param_sq = square(kernel_param);
138  double kernel_1st_deriv, kernel_2nd_deriv;
139 
140  sum += kernel.eval(sum_2, kernel_1st_deriv, kernel_2nd_deriv);
141  if (out_kernel_1st_deriv) *out_kernel_1st_deriv = kernel_1st_deriv;
142  }
143  else
144  {
145  sum += sum_2;
146  }
147 }
148 
149 /** Compute reprojection error vector (used from within Bundle Adjustment
150  * methods, but can be used in general)
151  * See mrpt::vision::bundle_adj_full for a description of most parameters.
152  *
153  * \return Overall squared reprojection error.
154  */
156  const TSequenceFeatureObservations& observations,
157  const TCamera& camera_params, const TFramePosesMap& frame_poses,
158  const TLandmarkLocationsMap& landmark_points,
159  std::vector<std::array<double, 2>>& out_residuals,
160  const bool frame_poses_are_inverse, const bool use_robust_kernel,
161  const double kernel_param, std::vector<double>* out_kernel_1st_deriv)
162 {
163  MRPT_START
164 
165  double sum = 0;
166 
167  const size_t N = observations.size();
168  out_residuals.resize(N);
169  if (out_kernel_1st_deriv) out_kernel_1st_deriv->resize(N);
170 
171  for (size_t i = 0; i < N; i++)
172  {
173  const TFeatureObservation& OBS = observations[i];
174 
175  const TFeatureID i_p = OBS.id_feature;
176  const TCameraPoseID i_f = OBS.id_frame;
177 
178  TFramePosesMap::const_iterator itF = frame_poses.find(i_f);
179  TLandmarkLocationsMap::const_iterator itP = landmark_points.find(i_p);
180  ASSERTMSG_(itF != frame_poses.end(), "Frame ID is not in list!");
181  ASSERTMSG_(itP != landmark_points.end(), "Landmark ID is not in list!");
182 
183  const TFramePosesMap::mapped_type& frame = itF->second;
184  const TLandmarkLocationsMap::mapped_type& point = itP->second;
185 
186  double* ptr_1st_deriv =
187  out_kernel_1st_deriv ? &((*out_kernel_1st_deriv)[i]) : nullptr;
188 
189  if (frame_poses_are_inverse)
190  reprojectionResidualsElement<true>(
191  camera_params, OBS, out_residuals[i], frame, point, sum,
192  use_robust_kernel, kernel_param, ptr_1st_deriv);
193  else
194  reprojectionResidualsElement<false>(
195  camera_params, OBS, out_residuals[i], frame, point, sum,
196  use_robust_kernel, kernel_param, ptr_1st_deriv);
197  }
198 
199  return sum;
200 
201  MRPT_END
202 }
203 
205  const TSequenceFeatureObservations& observations,
206  const TCamera& camera_params, const TFramePosesVec& frame_poses,
207  const TLandmarkLocationsVec& landmark_points,
208  std::vector<std::array<double, 2>>& out_residuals,
209  const bool frame_poses_are_inverse, const bool use_robust_kernel,
210  const double kernel_param, std::vector<double>* out_kernel_1st_deriv)
211 {
212  MRPT_START
213 
214  double sum = 0;
215 
216  const size_t N = observations.size();
217  out_residuals.resize(N);
218  if (out_kernel_1st_deriv) out_kernel_1st_deriv->resize(N);
219 
220  for (size_t i = 0; i < N; i++)
221  {
222  const TFeatureObservation& OBS = observations[i];
223 
224  const TFeatureID i_p = OBS.id_feature;
225  const TCameraPoseID i_f = OBS.id_frame;
226 
227  ASSERT_BELOW_(i_p, landmark_points.size());
228  ASSERT_BELOW_(i_f, frame_poses.size());
229  const TFramePosesVec::value_type& frame = frame_poses[i_f];
230  const TLandmarkLocationsVec::value_type& point = landmark_points[i_p];
231 
232  double* ptr_1st_deriv =
233  out_kernel_1st_deriv ? &((*out_kernel_1st_deriv)[i]) : nullptr;
234 
235  if (frame_poses_are_inverse)
236  reprojectionResidualsElement<true>(
237  camera_params, OBS, out_residuals[i], frame, point, sum,
238  use_robust_kernel, kernel_param, ptr_1st_deriv);
239  else
240  reprojectionResidualsElement<false>(
241  camera_params, OBS, out_residuals[i], frame, point, sum,
242  use_robust_kernel, kernel_param, ptr_1st_deriv);
243  }
244 
245  return sum;
246  MRPT_END
247 }
248 
249 // Compute gradients & Hessian blocks
251  const TSequenceFeatureObservations& observations,
252  const vector<std::array<double, 2>>& residual_vec,
253  const std::vector<JacData<6, 3, 2>>& jac_data_vec,
254  std::vector<CMatrixFixed<double, 6, 6>>& U,
255  std::vector<CVectorFixedDouble<6>>& eps_frame,
256  std::vector<CMatrixFixed<double, 3, 3>>& V,
257  std::vector<CVectorFixedDouble<3>>& eps_point, const size_t num_fix_frames,
258  const size_t num_fix_points, const vector<double>* kernel_1st_deriv)
259 {
260  MRPT_START
261 
262  const size_t N = observations.size();
263  const bool use_robust_kernel = (kernel_1st_deriv != nullptr);
264 
265  for (size_t i = 0; i < N; i++)
266  {
267  const TFeatureObservation& OBS = observations[i];
268 
269  const TFeatureID i_p = OBS.id_feature;
270  const TCameraPoseID i_f = OBS.id_frame;
271 
272  const Eigen::Matrix<double, 2, 1> RESID(&residual_vec[i][0]);
273  const JacData<6, 3, 2>& JACOB = jac_data_vec[i];
274  ASSERTDEB_(JACOB.frame_id == i_f && JACOB.point_id == i_p);
275 
276  if (i_f >= num_fix_frames)
277  {
278  const size_t frame_id = i_f - num_fix_frames;
279  ASSERTDEB_(JACOB.J_frame_valid);
280  ASSERT_BELOW_(frame_id, U.size());
282  JtJ.matProductOf_AtA(JACOB.J_frame);
283 
284  CVectorFixedDouble<6> eps_delta;
285  // eps_delta = J^t * RESID
286  eps_delta = JACOB.J_frame.transpose() * RESID;
287  if (!use_robust_kernel)
288  {
289  eps_frame[frame_id] += eps_delta;
290  }
291  else
292  {
293  const double rho_1st_der = (*kernel_1st_deriv)[i];
294  auto scaled_eps = eps_delta;
295  scaled_eps *= rho_1st_der;
296  eps_frame[frame_id] += scaled_eps;
297  }
298  U[frame_id] += JtJ;
299  }
300  if (i_p >= num_fix_points)
301  {
302  const size_t point_id = i_p - num_fix_points;
303  ASSERTDEB_(JACOB.J_point_valid);
304  ASSERT_BELOW_(point_id, V.size());
306  JtJ.matProductOf_AtA(JACOB.J_point);
307 
308  CVectorFixedDouble<3> eps_delta;
309  // eps_delta = J^t * RESID
310  eps_delta = JACOB.J_point.transpose() * RESID;
311  if (!use_robust_kernel)
312  {
313  eps_point[point_id] += eps_delta;
314  }
315  else
316  {
317  const double rho_1st_der = (*kernel_1st_deriv)[i];
318  auto scaled_eps = eps_delta;
319  scaled_eps *= rho_1st_der;
320  eps_point[point_id] += scaled_eps;
321  }
322 
323  V[point_id] += JtJ;
324  }
325  }
326 
327  MRPT_END
328 }
329 
331  const TFramePosesVec& frame_poses, const CVectorDouble& delta,
332  const size_t delta_first_idx, const size_t delta_num_vals,
333  TFramePosesVec& new_frame_poses, const size_t num_fix_frames)
334 {
335  MRPT_START
336 
337  new_frame_poses.resize(frame_poses.size());
338 
339  for (size_t i = 0; i < num_fix_frames; ++i)
340  new_frame_poses[i] = frame_poses[i];
341 
342  size_t delta_used_vals = 0;
343  const double* delta_val = &delta[delta_first_idx];
344 
345  for (size_t i = num_fix_frames; i < frame_poses.size(); i++)
346  {
347  const CPose3D& old_pose = frame_poses[i];
348  CPose3D& new_pose = new_frame_poses[i];
349 
350  // Use the Lie Algebra methods for the increment:
351  const CPose3D incrPose =
353 
354  // new_pose = old_pose [+] delta
355  // = exp(delta) (+) old_pose
356  new_pose.composeFrom(incrPose, old_pose);
357 
358  // Move to the next entry in delta:
359  delta_val += 6;
360  delta_used_vals += 6;
361  }
362 
363  ASSERT_(delta_used_vals == delta_num_vals);
364  MRPT_END
365 }
367  const TLandmarkLocationsVec& landmark_points, const CVectorDouble& delta,
368  const size_t delta_first_idx, const size_t delta_num_vals,
369  TLandmarkLocationsVec& new_landmark_points, const size_t num_fix_points)
370 {
371  MRPT_START
372 
373  new_landmark_points.resize(landmark_points.size());
374 
375  for (size_t i = 0; i < num_fix_points; ++i)
376  new_landmark_points[i] = landmark_points[i];
377 
378  size_t delta_used_vals = 0;
379  const double* delta_val = &delta[delta_first_idx];
380 
381  for (size_t i = num_fix_points; i < landmark_points.size(); i++)
382  {
383  const TPoint3D& old_point = landmark_points[i];
384  TPoint3D& new_point = new_landmark_points[i];
385 
386  for (size_t j = 0; j < 3; j++)
387  new_point[j] = old_point[j] + delta_val[j];
388 
389  // Move to the next entry in delta:
390  delta_val += 3;
391  delta_used_vals += 3;
392  }
393 
394  ASSERT_(delta_used_vals == delta_num_vals);
395  MRPT_END
396 }
TLandmarkID id_feature
A unique ID of this feature.
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
void reprojectionResidualsElement(const TCamera &camera_params, const TFeatureObservation &OBS, std::array< double, 2 > &out_residual, const TFramePosesVec::value_type &frame, const TLandmarkLocationsVec::value_type &point, double &sum, const bool use_robust_kernel, const double kernel_param, double *out_kernel_1st_deriv)
Definition: ba_common.cpp:116
uint64_t TCameraPoseID
Unique IDs for camera frames (poses)
#define MRPT_START
Definition: exceptions.h:241
uint64_t TFeatureID
Definition of a feature ID.
mrpt::math::CMatrixFixed< double, ObsDim, PointDof > J_point
Definition: ba_internals.h:65
#define ASSERT_BELOW_(__A, __B)
Definition: exceptions.h:149
One feature observation entry, used within sequences with TSequenceFeatureObservations.
A pair (x,y) of pixel coordinates (subpixel resolution).
Definition: TPixelCoord.h:18
STL namespace.
void ba_initial_estimate(const mrpt::vision::TSequenceFeatureObservations &observations, const mrpt::img::TCamera &camera_params, mrpt::vision::TFramePosesVec &frame_poses, mrpt::vision::TLandmarkLocationsVec &landmark_points)
Fills the frames & landmark points maps with an initial gross estimate from the sequence observations...
void add_se3_deltas_to_frames(const mrpt::vision::TFramePosesVec &frame_poses, const mrpt::math::CVectorDouble &delta, const size_t delta_first_idx, const size_t delta_num_vals, mrpt::vision::TFramePosesVec &new_frame_poses, const size_t num_fix_frames)
For each pose in the vector frame_poses, adds a "delta" increment to the manifold, with the "delta" given in the se(3) Lie algebra:
TCameraPoseID frame_id
Definition: ba_internals.h:60
A complete sequence of observations of features from different camera frames (poses).
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
std::vector< mrpt::poses::CPose3D > TFramePosesVec
A list of camera frames (6D poses), which assumes indexes are unique, consecutive IDs...
This base provides a set of functions for maths stuff.
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:556
void add_3d_deltas_to_points(const mrpt::vision::TLandmarkLocationsVec &landmark_points, const mrpt::math::CVectorDouble &delta, const size_t delta_first_idx, const size_t delta_num_vals, mrpt::vision::TLandmarkLocationsVec &new_landmark_points, const size_t num_fix_points)
For each pose in the vector frame_poses, adds a "delta" increment to the manifold, with the "delta" given in the se(3) Lie algebra:
Definition: ba_common.cpp:366
std::map< TLandmarkID, mrpt::math::TPoint3D > TLandmarkLocationsMap
A list of landmarks (3D points) indexed by unique IDs.
CONTAINER::Scalar sum(const CONTAINER &v)
Computes the sum of all the elements.
Classes for computer vision, detectors, features, etc.
Definition: CDifodo.h:17
Parameters for the Brown-Conrady camera lens distortion model.
Definition: TCamera.h:26
TPoint3D_< double > TPoint3D
Lightweight 3D point.
Definition: TPoint3D.h:268
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:108
double reprojectionResiduals(const mrpt::vision::TSequenceFeatureObservations &observations, const mrpt::img::TCamera &camera_params, const mrpt::vision::TFramePosesVec &frame_poses, const mrpt::vision::TLandmarkLocationsVec &landmark_points, std::vector< std::array< double, 2 >> &out_residuals, const bool frame_poses_are_inverse, const bool use_robust_kernel=true, const double kernel_param=3.0, std::vector< double > *out_kernel_1st_deriv=nullptr)
Compute reprojection error vector (used from within Bundle Adjustment methods, but can be used in gen...
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
mrpt::img::TPixelCoordf px
The pixel coordinates of the observed feature.
void keep_max(T &var, const K test_val)
If the second argument is above the first one, set the first argument to this higher value...
Traits for SE(n), rigid-body transformations in R^n space.
Definition: SE.h:30
return_t square(const num_t x)
Inline function for the square of a number.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
std::map< TCameraPoseID, mrpt::poses::CPose3D > TFramePosesMap
A list of camera frames (6D poses) indexed by unique IDs.
#define ASSERTDEB_(f)
Defines an assertion mechanism - only when compiled in debug.
Definition: exceptions.h:190
#define MRPT_END
Definition: exceptions.h:245
void ba_build_gradient_Hessians(const TSequenceFeatureObservations &observations, const std::vector< std::array< double, 2 >> &residual_vec, const std::vector< JacData< 6, 3, 2 >> &jac_data_vec, std::vector< mrpt::math::CMatrixFixed< double, 6, 6 >> &U, std::vector< CVectorFixedDouble< 6 >> &eps_frame, std::vector< mrpt::math::CMatrixFixed< double, 3, 3 >> &V, std::vector< CVectorFixedDouble< 3 >> &eps_point, const size_t num_fix_frames, const size_t num_fix_points, const vector< double > *kernel_1st_deriv)
Construct the BA linear system.
mrpt::math::CMatrixFixed< double, ObsDim, FrameDof > J_frame
Definition: ba_internals.h:64
TCameraPoseID id_frame
A unique ID of a "frame" (camera position) from where the feature was observed.
std::vector< mrpt::math::TPoint3D > TLandmarkLocationsVec
A list of landmarks (3D points), which assumes indexes are unique, consecutive IDs.
TLandmarkID point_id
Definition: ba_internals.h:61



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020