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-2018, Individual contributors, see AUTHORS file
5  * See: http://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/vision/pinhole.h>
29 #include "ba_internals.h"
30 
31 using namespace std;
32 using namespace mrpt;
33 using namespace mrpt::vision;
34 using namespace mrpt::img;
35 using namespace mrpt::math;
36 using namespace mrpt::poses;
37 
38 /* -------------------------------------------------------------------------------------
39  ba_initial_estimate
40 
41  Fills the frames & landmark points maps with an initial gross estimate
42  from the sequence \a observations, so they can be fed to bundle adjustment
43  methods.
44  ------------------------------------------------------------------------------------
45  */
47  const TSequenceFeatureObservations& observations,
48  const TCamera& camera_params, TFramePosesMap& frame_poses,
49  TLandmarkLocationsMap& landmark_points)
50 {
51  MRPT_UNUSED_PARAM(camera_params);
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  const TCamera& camera_params, TFramePosesVec& frame_poses,
77  TLandmarkLocationsVec& landmark_points)
78 {
79  MRPT_UNUSED_PARAM(camera_params);
81  // VERY CRUDE APPROACH: All camera poses at the origin, all points at
82  // (0,0,1)
83  // TODO: Improve with the data in "observations"...
84 
85  // Clear previous contents:
86  frame_poses.clear();
87  landmark_points.clear();
88 
89  if (observations.empty()) return;
90 
91  // Go thru the obs list and insert poses:
92 
93  TFeatureID max_pt_id = 0;
94  TCameraPoseID max_fr_id = 0;
95 
96  for (TSequenceFeatureObservations::const_iterator it = observations.begin();
97  it != observations.end(); ++it)
98  {
99  const TFeatureID feat_ID = it->id_feature;
100  const TCameraPoseID frame_ID = it->id_frame;
101  keep_max(max_fr_id, frame_ID);
102  keep_max(max_pt_id, feat_ID);
103  }
104 
105  // Insert N copies of the same values:
106  frame_poses.assign(max_fr_id + 1, CPose3D(0, 0, 0, 0, 0, 0));
107  landmark_points.assign(max_pt_id + 1, TPoint3D(0, 0, 1));
108 
109  MRPT_END
110 }
111 
112 // This function is what to do for each feature in the reprojection loops below.
113 // -> residual: the raw residual, even if using robust kernel
114 // -> sum+= scaled squared norm of the residual (which != squared norm if using
115 // robust kernel)
116 template <bool POSES_INVERSE>
118  const TCamera& camera_params, const TFeatureObservation& OBS,
119  std::array<double, 2>& out_residual,
120  const TFramePosesVec::value_type& frame,
121  const TLandmarkLocationsVec::value_type& point, double& sum,
122  const bool use_robust_kernel, const double kernel_param,
123  double* out_kernel_1st_deriv)
124 {
125  const TPixelCoordf z_pred =
126  mrpt::vision::pinhole::projectPoint_no_distortion<POSES_INVERSE>(
127  camera_params, frame, point);
128  const TPixelCoordf& z_meas = OBS.px;
129 
130  out_residual[0] = z_meas.x - z_pred.x;
131  out_residual[1] = z_meas.y - z_pred.y;
132 
133  const double sum_2 = square(out_residual[0]) + square(out_residual[1]);
134 
135  if (use_robust_kernel)
136  {
138  kernel.param_sq = square(kernel_param);
139  double kernel_1st_deriv, kernel_2nd_deriv;
140 
141  sum += kernel.eval(sum_2, kernel_1st_deriv, kernel_2nd_deriv);
142  if (out_kernel_1st_deriv) *out_kernel_1st_deriv = kernel_1st_deriv;
143  }
144  else
145  {
146  sum += sum_2;
147  }
148 }
149 
150 /** Compute reprojection error vector (used from within Bundle Adjustment
151  * methods, but can be used in general)
152  * See mrpt::vision::bundle_adj_full for a description of most parameters.
153  *
154  * \return Overall squared reprojection error.
155  */
157  const TSequenceFeatureObservations& observations,
158  const TCamera& camera_params, const TFramePosesMap& frame_poses,
159  const TLandmarkLocationsMap& landmark_points,
160  std::vector<std::array<double, 2>>& out_residuals,
161  const bool frame_poses_are_inverse, const bool use_robust_kernel,
162  const double kernel_param, std::vector<double>* out_kernel_1st_deriv)
163 {
164  MRPT_START
165 
166  double sum = 0;
167 
168  const size_t N = observations.size();
169  out_residuals.resize(N);
170  if (out_kernel_1st_deriv) out_kernel_1st_deriv->resize(N);
171 
172  for (size_t i = 0; i < N; i++)
173  {
174  const TFeatureObservation& OBS = observations[i];
175 
176  const TFeatureID i_p = OBS.id_feature;
177  const TCameraPoseID i_f = OBS.id_frame;
178 
179  TFramePosesMap::const_iterator itF = frame_poses.find(i_f);
180  TLandmarkLocationsMap::const_iterator itP = landmark_points.find(i_p);
181  ASSERTMSG_(itF != frame_poses.end(), "Frame ID is not in list!");
182  ASSERTMSG_(itP != landmark_points.end(), "Landmark ID is not in list!");
183 
184  const TFramePosesMap::mapped_type& frame = itF->second;
185  const TLandmarkLocationsMap::mapped_type& point = itP->second;
186 
187  double* ptr_1st_deriv =
188  out_kernel_1st_deriv ? &((*out_kernel_1st_deriv)[i]) : nullptr;
189 
190  if (frame_poses_are_inverse)
191  reprojectionResidualsElement<true>(
192  camera_params, OBS, out_residuals[i], frame, point, sum,
193  use_robust_kernel, kernel_param, ptr_1st_deriv);
194  else
195  reprojectionResidualsElement<false>(
196  camera_params, OBS, out_residuals[i], frame, point, sum,
197  use_robust_kernel, kernel_param, ptr_1st_deriv);
198  }
199 
200  return sum;
201 
202  MRPT_END
203 }
204 
206  const TSequenceFeatureObservations& observations,
207  const TCamera& camera_params, const TFramePosesVec& frame_poses,
208  const TLandmarkLocationsVec& landmark_points,
209  std::vector<std::array<double, 2>>& out_residuals,
210  const bool frame_poses_are_inverse, const bool use_robust_kernel,
211  const double kernel_param, std::vector<double>* out_kernel_1st_deriv)
212 {
213  MRPT_START
214 
215  double sum = 0;
216 
217  const size_t N = observations.size();
218  out_residuals.resize(N);
219  if (out_kernel_1st_deriv) out_kernel_1st_deriv->resize(N);
220 
221  for (size_t i = 0; i < N; i++)
222  {
223  const TFeatureObservation& OBS = observations[i];
224 
225  const TFeatureID i_p = OBS.id_feature;
226  const TCameraPoseID i_f = OBS.id_frame;
227 
228  ASSERT_BELOW_(i_p, landmark_points.size());
229  ASSERT_BELOW_(i_f, frame_poses.size());
230  const TFramePosesVec::value_type& frame = frame_poses[i_f];
231  const TLandmarkLocationsVec::value_type& point = landmark_points[i_p];
232 
233  double* ptr_1st_deriv =
234  out_kernel_1st_deriv ? &((*out_kernel_1st_deriv)[i]) : nullptr;
235 
236  if (frame_poses_are_inverse)
237  reprojectionResidualsElement<true>(
238  camera_params, OBS, out_residuals[i], frame, point, sum,
239  use_robust_kernel, kernel_param, ptr_1st_deriv);
240  else
241  reprojectionResidualsElement<false>(
242  camera_params, OBS, out_residuals[i], frame, point, sum,
243  use_robust_kernel, kernel_param, ptr_1st_deriv);
244  }
245 
246  return sum;
247  MRPT_END
248 }
249 
250 // Compute gradients & Hessian blocks
252  const TSequenceFeatureObservations& observations,
253  const vector<std::array<double, 2>>& residual_vec,
254  const mrpt::aligned_std_vector<JacData<6, 3, 2>>& jac_data_vec,
259  const size_t num_fix_frames, const size_t num_fix_points,
260  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.multiply_AtA(JACOB.J_frame);
285 
286  CArrayDouble<6> eps_delta;
287  JACOB.J_frame.multiply_Atb(
288  RESID, eps_delta); // eps_delta = J^t * 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  eps_frame[frame_id] += eps_delta * rho_1st_der;
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.multiply_AtA(JACOB.J_point);
307 
308  CArrayDouble<3> eps_delta;
309  JACOB.J_point.multiply_Atb(
310  RESID, eps_delta); // eps_delta = J^t * 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  eps_point[point_id] += eps_delta * rho_1st_der;
319  }
320 
321  V[point_id] += JtJ;
322  }
323  }
324 
325  MRPT_END
326 }
327 
329  const TFramePosesVec& frame_poses, const CVectorDouble& delta,
330  const size_t delta_first_idx, const size_t delta_num_vals,
331  TFramePosesVec& new_frame_poses, const size_t num_fix_frames)
332 {
333  MRPT_START
334 
335  new_frame_poses.resize(frame_poses.size());
336 
337  for (size_t i = 0; i < num_fix_frames; ++i)
338  new_frame_poses[i] = frame_poses[i];
339 
340  size_t delta_used_vals = 0;
341  const double* delta_val = &delta[delta_first_idx];
342 
343  for (size_t i = num_fix_frames; i < frame_poses.size(); i++)
344  {
345  const CPose3D& old_pose = frame_poses[i];
346  CPose3D& new_pose = new_frame_poses[i];
347 
348  // Use the Lie Algebra methods for the increment:
349  const CArrayDouble<6> incr(delta_val);
350  const CPose3D incrPose = CPose3D::exp(incr);
351 
352  // new_pose = old_pose [+] delta
353  // = exp(delta) (+) old_pose
354  new_pose.composeFrom(incrPose, old_pose);
355 
356  // Move to the next entry in delta:
357  delta_val += 6;
358  delta_used_vals += 6;
359  }
360 
361  ASSERT_(delta_used_vals == delta_num_vals);
362  MRPT_END
363 }
365  const TLandmarkLocationsVec& landmark_points, const CVectorDouble& delta,
366  const size_t delta_first_idx, const size_t delta_num_vals,
367  TLandmarkLocationsVec& new_landmark_points, const size_t num_fix_points)
368 {
369  MRPT_START
370 
371  new_landmark_points.resize(landmark_points.size());
372 
373  for (size_t i = 0; i < num_fix_points; ++i)
374  new_landmark_points[i] = landmark_points[i];
375 
376  size_t delta_used_vals = 0;
377  const double* delta_val = &delta[delta_first_idx];
378 
379  for (size_t i = num_fix_points; i < landmark_points.size(); i++)
380  {
381  const TPoint3D& old_point = landmark_points[i];
382  TPoint3D& new_point = new_landmark_points[i];
383 
384  for (size_t j = 0; j < 3; j++)
385  new_point[j] = old_point[j] + delta_val[j];
386 
387  // Move to the next entry in delta:
388  delta_val += 3;
389  delta_used_vals += 3;
390  }
391 
392  ASSERT_(delta_used_vals == delta_num_vals);
393  MRPT_END
394 }
mrpt::math::CMatrixFixedNumeric< double, ObsDim, PointDof > J_point
Definition: ba_internals.h:65
TLandmarkID id_feature
A unique ID of this feature.
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:117
uint64_t TCameraPoseID
Unique IDs for camera frames (poses)
#define MRPT_START
Definition: exceptions.h:262
uint64_t TFeatureID
Definition of a feature ID.
#define ASSERT_BELOW_(__A, __B)
Definition: exceptions.h:165
CArrayNumeric is an array for numeric types supporting several mathematical operations (actually...
Definition: CArrayNumeric.h:25
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:44
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...
Definition: ba_common.cpp:74
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:
Definition: ba_common.cpp:328
std::vector< T, mrpt::aligned_allocator_cpp11< T > > aligned_std_vector
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:113
A numeric matrix of compile-time fixed size.
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:565
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:364
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: CCamModel.h:18
Structure to hold the parameters of a pinhole camera model.
Definition: TCamera.h:27
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:101
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...
Definition: ba_common.cpp:205
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.
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:86
void ba_build_gradient_Hessians(const TSequenceFeatureObservations &observations, const std::vector< std::array< double, 2 >> &residual_vec, const mrpt::aligned_std_vector< JacData< 6, 3, 2 >> &jac_data_vec, mrpt::aligned_std_vector< mrpt::math::CMatrixFixedNumeric< double, 6, 6 >> &U, mrpt::aligned_std_vector< CArrayDouble< 6 >> &eps_frame, mrpt::aligned_std_vector< mrpt::math::CMatrixFixedNumeric< double, 3, 3 >> &V, mrpt::aligned_std_vector< CArrayDouble< 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.
#define ASSERTDEB_(f)
Defines an assertion mechanism - only when compiled in debug.
Definition: exceptions.h:205
#define MRPT_END
Definition: exceptions.h:266
mrpt::aligned_std_vector< mrpt::poses::CPose3D > TFramePosesVec
A list of camera frames (6D poses), which assumes indexes are unique, consecutive IDs...
mrpt::math::CMatrixFixedNumeric< 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.
mrpt::aligned_std_map< TCameraPoseID, mrpt::poses::CPose3D > TFramePosesMap
A list of camera frames (6D poses) indexed by unique IDs.
Lightweight 3D point.
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
const Scalar * const_iterator
Definition: eigen_plugins.h:27
#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: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020