MRPT  1.9.9
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-2019, 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  const TCamera& camera_params, TFramePosesMap& frame_poses,
50  TLandmarkLocationsMap& landmark_points)
51 {
52  MRPT_UNUSED_PARAM(camera_params);
54  // VERY CRUDE APPROACH: All camera poses at the origin, all points at
55  // (0,0,1)
56  // TODO: Improve with the data in "observations"...
57 
58  // Clear previous contents:
59  frame_poses.clear();
60  landmark_points.clear();
61 
62  // Go thru the obs list and insert poses:
63  for (TSequenceFeatureObservations::const_iterator it = observations.begin();
64  it != observations.end(); ++it)
65  {
66  const TFeatureID feat_ID = it->id_feature;
67  const TCameraPoseID frame_ID = it->id_frame;
68 
69  frame_poses[frame_ID] = CPose3D(0, 0, 0, 0, 0, 0);
70  landmark_points[feat_ID] = TPoint3D(0, 0, 1);
71  }
72  MRPT_END
73 }
74 
76  const TSequenceFeatureObservations& observations,
77  const TCamera& camera_params, TFramePosesVec& frame_poses,
78  TLandmarkLocationsVec& landmark_points)
79 {
80  MRPT_UNUSED_PARAM(camera_params);
82  // VERY CRUDE APPROACH: All camera poses at the origin, all points at
83  // (0,0,1)
84  // TODO: Improve with the data in "observations"...
85 
86  // Clear previous contents:
87  frame_poses.clear();
88  landmark_points.clear();
89 
90  if (observations.empty()) return;
91 
92  // Go thru the obs list and insert poses:
93 
94  TFeatureID max_pt_id = 0;
95  TCameraPoseID max_fr_id = 0;
96 
97  for (TSequenceFeatureObservations::const_iterator it = observations.begin();
98  it != observations.end(); ++it)
99  {
100  const TFeatureID feat_ID = it->id_feature;
101  const TCameraPoseID frame_ID = it->id_frame;
102  keep_max(max_fr_id, frame_ID);
103  keep_max(max_pt_id, feat_ID);
104  }
105 
106  // Insert N copies of the same values:
107  frame_poses.assign(max_fr_id + 1, CPose3D(0, 0, 0, 0, 0, 0));
108  landmark_points.assign(max_pt_id + 1, TPoint3D(0, 0, 1));
109 
110  MRPT_END
111 }
112 
113 // This function is what to do for each feature in the reprojection loops below.
114 // -> residual: the raw residual, even if using robust kernel
115 // -> sum+= scaled squared norm of the residual (which != squared norm if using
116 // robust kernel)
117 template <bool POSES_INVERSE>
119  const TCamera& camera_params, const TFeatureObservation& OBS,
120  std::array<double, 2>& out_residual,
121  const TFramePosesVec::value_type& frame,
122  const TLandmarkLocationsVec::value_type& point, double& sum,
123  const bool use_robust_kernel, const double kernel_param,
124  double* out_kernel_1st_deriv)
125 {
126  const TPixelCoordf z_pred =
127  mrpt::vision::pinhole::projectPoint_no_distortion<POSES_INVERSE>(
128  camera_params, frame, point);
129  const TPixelCoordf& z_meas = OBS.px;
130 
131  out_residual[0] = z_meas.x - z_pred.x;
132  out_residual[1] = z_meas.y - z_pred.y;
133 
134  const double sum_2 = square(out_residual[0]) + square(out_residual[1]);
135 
136  if (use_robust_kernel)
137  {
139  kernel.param_sq = square(kernel_param);
140  double kernel_1st_deriv, kernel_2nd_deriv;
141 
142  sum += kernel.eval(sum_2, kernel_1st_deriv, kernel_2nd_deriv);
143  if (out_kernel_1st_deriv) *out_kernel_1st_deriv = kernel_1st_deriv;
144  }
145  else
146  {
147  sum += sum_2;
148  }
149 }
150 
151 /** Compute reprojection error vector (used from within Bundle Adjustment
152  * methods, but can be used in general)
153  * See mrpt::vision::bundle_adj_full for a description of most parameters.
154  *
155  * \return Overall squared reprojection error.
156  */
158  const TSequenceFeatureObservations& observations,
159  const TCamera& camera_params, const TFramePosesMap& frame_poses,
160  const TLandmarkLocationsMap& landmark_points,
161  std::vector<std::array<double, 2>>& out_residuals,
162  const bool frame_poses_are_inverse, const bool use_robust_kernel,
163  const double kernel_param, std::vector<double>* out_kernel_1st_deriv)
164 {
165  MRPT_START
166 
167  double sum = 0;
168 
169  const size_t N = observations.size();
170  out_residuals.resize(N);
171  if (out_kernel_1st_deriv) out_kernel_1st_deriv->resize(N);
172 
173  for (size_t i = 0; i < N; i++)
174  {
175  const TFeatureObservation& OBS = observations[i];
176 
177  const TFeatureID i_p = OBS.id_feature;
178  const TCameraPoseID i_f = OBS.id_frame;
179 
180  TFramePosesMap::const_iterator itF = frame_poses.find(i_f);
181  TLandmarkLocationsMap::const_iterator itP = landmark_points.find(i_p);
182  ASSERTMSG_(itF != frame_poses.end(), "Frame ID is not in list!");
183  ASSERTMSG_(itP != landmark_points.end(), "Landmark ID is not in list!");
184 
185  const TFramePosesMap::mapped_type& frame = itF->second;
186  const TLandmarkLocationsMap::mapped_type& point = itP->second;
187 
188  double* ptr_1st_deriv =
189  out_kernel_1st_deriv ? &((*out_kernel_1st_deriv)[i]) : nullptr;
190 
191  if (frame_poses_are_inverse)
192  reprojectionResidualsElement<true>(
193  camera_params, OBS, out_residuals[i], frame, point, sum,
194  use_robust_kernel, kernel_param, ptr_1st_deriv);
195  else
196  reprojectionResidualsElement<false>(
197  camera_params, OBS, out_residuals[i], frame, point, sum,
198  use_robust_kernel, kernel_param, ptr_1st_deriv);
199  }
200 
201  return sum;
202 
203  MRPT_END
204 }
205 
207  const TSequenceFeatureObservations& observations,
208  const TCamera& camera_params, const TFramePosesVec& frame_poses,
209  const TLandmarkLocationsVec& landmark_points,
210  std::vector<std::array<double, 2>>& out_residuals,
211  const bool frame_poses_are_inverse, const bool use_robust_kernel,
212  const double kernel_param, std::vector<double>* out_kernel_1st_deriv)
213 {
214  MRPT_START
215 
216  double sum = 0;
217 
218  const size_t N = observations.size();
219  out_residuals.resize(N);
220  if (out_kernel_1st_deriv) out_kernel_1st_deriv->resize(N);
221 
222  for (size_t i = 0; i < N; i++)
223  {
224  const TFeatureObservation& OBS = observations[i];
225 
226  const TFeatureID i_p = OBS.id_feature;
227  const TCameraPoseID i_f = OBS.id_frame;
228 
229  ASSERT_BELOW_(i_p, landmark_points.size());
230  ASSERT_BELOW_(i_f, frame_poses.size());
231  const TFramePosesVec::value_type& frame = frame_poses[i_f];
232  const TLandmarkLocationsVec::value_type& point = landmark_points[i_p];
233 
234  double* ptr_1st_deriv =
235  out_kernel_1st_deriv ? &((*out_kernel_1st_deriv)[i]) : nullptr;
236 
237  if (frame_poses_are_inverse)
238  reprojectionResidualsElement<true>(
239  camera_params, OBS, out_residuals[i], frame, point, sum,
240  use_robust_kernel, kernel_param, ptr_1st_deriv);
241  else
242  reprojectionResidualsElement<false>(
243  camera_params, OBS, out_residuals[i], frame, point, sum,
244  use_robust_kernel, kernel_param, ptr_1st_deriv);
245  }
246 
247  return sum;
248  MRPT_END
249 }
250 
251 // Compute gradients & Hessian blocks
253  const TSequenceFeatureObservations& observations,
254  const vector<std::array<double, 2>>& residual_vec,
255  const std::vector<JacData<6, 3, 2>>& jac_data_vec,
256  std::vector<CMatrixFixed<double, 6, 6>>& U,
257  std::vector<CVectorFixedDouble<6>>& eps_frame,
258  std::vector<CMatrixFixed<double, 3, 3>>& V,
259  std::vector<CVectorFixedDouble<3>>& eps_point, const size_t num_fix_frames,
260  const size_t num_fix_points, const vector<double>* kernel_1st_deriv)
261 {
262  MRPT_START
263 
264  const size_t N = observations.size();
265  const bool use_robust_kernel = (kernel_1st_deriv != nullptr);
266 
267  for (size_t i = 0; i < N; i++)
268  {
269  const TFeatureObservation& OBS = observations[i];
270 
271  const TFeatureID i_p = OBS.id_feature;
272  const TCameraPoseID i_f = OBS.id_frame;
273 
274  const Eigen::Matrix<double, 2, 1> RESID(&residual_vec[i][0]);
275  const JacData<6, 3, 2>& JACOB = jac_data_vec[i];
276  ASSERTDEB_(JACOB.frame_id == i_f && JACOB.point_id == i_p);
277 
278  if (i_f >= num_fix_frames)
279  {
280  const size_t frame_id = i_f - num_fix_frames;
281  ASSERTDEB_(JACOB.J_frame_valid);
282  ASSERT_BELOW_(frame_id, U.size());
284  JtJ.matProductOf_AtA(JACOB.J_frame);
285 
286  CVectorFixedDouble<6> eps_delta;
287  // eps_delta = J^t * RESID
288  eps_delta = JACOB.J_frame.transpose() * RESID;
289  if (!use_robust_kernel)
290  {
291  eps_frame[frame_id] += eps_delta;
292  }
293  else
294  {
295  const double rho_1st_der = (*kernel_1st_deriv)[i];
296  auto scaled_eps = eps_delta;
297  scaled_eps *= rho_1st_der;
298  eps_frame[frame_id] += scaled_eps;
299  }
300  U[frame_id] += JtJ;
301  }
302  if (i_p >= num_fix_points)
303  {
304  const size_t point_id = i_p - num_fix_points;
305  ASSERTDEB_(JACOB.J_point_valid);
306  ASSERT_BELOW_(point_id, V.size());
308  JtJ.matProductOf_AtA(JACOB.J_point);
309 
310  CVectorFixedDouble<3> eps_delta;
311  // eps_delta = J^t * RESID
312  eps_delta = JACOB.J_point.transpose() * RESID;
313  if (!use_robust_kernel)
314  {
315  eps_point[point_id] += eps_delta;
316  }
317  else
318  {
319  const double rho_1st_der = (*kernel_1st_deriv)[i];
320  auto scaled_eps = eps_delta;
321  scaled_eps *= rho_1st_der;
322  eps_point[point_id] += scaled_eps;
323  }
324 
325  V[point_id] += JtJ;
326  }
327  }
328 
329  MRPT_END
330 }
331 
333  const TFramePosesVec& frame_poses, const CVectorDouble& delta,
334  const size_t delta_first_idx, const size_t delta_num_vals,
335  TFramePosesVec& new_frame_poses, const size_t num_fix_frames)
336 {
337  MRPT_START
338 
339  new_frame_poses.resize(frame_poses.size());
340 
341  for (size_t i = 0; i < num_fix_frames; ++i)
342  new_frame_poses[i] = frame_poses[i];
343 
344  size_t delta_used_vals = 0;
345  const double* delta_val = &delta[delta_first_idx];
346 
347  for (size_t i = num_fix_frames; i < frame_poses.size(); i++)
348  {
349  const CPose3D& old_pose = frame_poses[i];
350  CPose3D& new_pose = new_frame_poses[i];
351 
352  // Use the Lie Algebra methods for the increment:
353  const CPose3D incrPose =
355 
356  // new_pose = old_pose [+] delta
357  // = exp(delta) (+) old_pose
358  new_pose.composeFrom(incrPose, old_pose);
359 
360  // Move to the next entry in delta:
361  delta_val += 6;
362  delta_used_vals += 6;
363  }
364 
365  ASSERT_(delta_used_vals == delta_num_vals);
366  MRPT_END
367 }
369  const TLandmarkLocationsVec& landmark_points, const CVectorDouble& delta,
370  const size_t delta_first_idx, const size_t delta_num_vals,
371  TLandmarkLocationsVec& new_landmark_points, const size_t num_fix_points)
372 {
373  MRPT_START
374 
375  new_landmark_points.resize(landmark_points.size());
376 
377  for (size_t i = 0; i < num_fix_points; ++i)
378  new_landmark_points[i] = landmark_points[i];
379 
380  size_t delta_used_vals = 0;
381  const double* delta_val = &delta[delta_first_idx];
382 
383  for (size_t i = num_fix_points; i < landmark_points.size(); i++)
384  {
385  const TPoint3D& old_point = landmark_points[i];
386  TPoint3D& new_point = new_landmark_points[i];
387 
388  for (size_t j = 0; j < 3; j++)
389  new_point[j] = old_point[j] + delta_val[j];
390 
391  // Move to the next entry in delta:
392  delta_val += 3;
393  delta_used_vals += 3;
394  }
395 
396  ASSERT_(delta_used_vals == delta_num_vals);
397  MRPT_END
398 }
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:118
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).
T square(const T x)
Inline function for the square of a number.
#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 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...
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:563
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:368
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
Structure to hold the parameters of a pinhole camera model.
Definition: TCamera.h:25
#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.
Traits for SE(n), rigid-body transformations in R^n space.
Definition: SE.h:30
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:84
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.
Lightweight 3D point.
Definition: TPoint3D.h:90
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
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 8fe78517f Sun Jul 14 19:43:28 2019 +0200 at lun oct 28 02:10:00 CET 2019