MRPT  2.0.4
bundle_adjustment.h
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 #pragma once
25 
26 #include <mrpt/img/TCamera.h>
29 #include <mrpt/vision/types.h>
30 #include <array>
31 #include <functional>
32 
33 // The methods declared in this file are implemented in separate files in:
34 // vision/src/ba_*.cpp
35 namespace mrpt::vision
36 {
37 /** \defgroup bundle_adj Bundle-Adjustment methods
38  * \ingroup mrpt_vision_grp
39  */
40 
41 /** @name Bundle-Adjustment methods
42  @{ */
43 
44 /** A functor type for BA methods \sa bundle_adj_full */
45 using TBundleAdjustmentFeedbackFunctor = std::function<void(
46  const size_t cur_iter, const double cur_total_sq_error,
47  const size_t max_iters,
48  const mrpt::vision::TSequenceFeatureObservations& input_observations,
49  const mrpt::vision::TFramePosesVec& current_frame_estimate,
50  const mrpt::vision::TLandmarkLocationsVec& current_landmark_estimate)>;
51 
52 /** Sparse Levenberg-Marquart solution to bundle adjustment - optimizes all the
53  *camera frames & the landmark locations.
54  * At input a gross estimation of the frame poses & the landmark points must be
55  *supplied. If you don't have such a
56  * starting point, use mrpt::vision::ba_initial_estimate() to compute it.
57  *
58  * At output the best found solution will be returned in the variables.
59  *Optionally, a functor can be passed for having
60  * feedback on the progress at each iteration (you can use it to refresh a
61  *GUI, display a progress bar, etc...).
62  *
63  * This implementation is almost entirely an adapted version from RobotVision
64  *(at OpenSLAM.org) (C) by Hauke Strasdat (Imperial College London), licensed
65  *under GNU LGPL.
66  * See the related paper: H. Strasdat, J.M.M. Montiel, A.J. Davison: "Scale
67  *Drift-Aware Large Scale Monocular SLAM", RSS2010,
68  *http://www.roboticsproceedings.org/rss06/p10.html
69  *
70  * List of optional parameters in "extra_params":
71  * - "verbose" : Verbose output (default=0)
72  * - "max_iterations": Maximum number of iterations to run (default=50)
73  * - "robust_kernel": If !=0, use a robust kernel against outliers
74  *(default=1)
75  * - "kernel_param": The pseudo-huber kernel parameter (default=3)
76  * - "mu": Initial mu for LevMarq (default=-1 -> autoguess)
77  * - "num_fix_frames": Number of first frame poses to don't optimize (keep
78  *unmodified as they come in) (default=1: the first pose is the reference and
79  *is not modified)
80  * - "num_fix_points": Idem, for the landmarks positions (default=0:
81  *optimize
82  *all)
83  * - "profiler": If !=0, displays profiling information to the console at
84  *return.
85  *
86  * \note In this function, all coordinates are absolute. Camera frames are such
87  *that +Z points forward from the focal point (see the figure in
88  *mrpt::obs::CObservationImage).
89  * \note The first frame pose will be not updated since at least one frame must
90  *remain fixed.
91  *
92  * \param observations [IN] All the feature observations (WITHOUT distortion),
93  *indexed by feature ID as lists of <frame_ID, (x,y)>. See
94  *TSequenceFeatureObservations.
95  * \param camera_params [IN] The camera parameters, mainly used for the
96  *intrinsic 3x3 matrix. Distortion params are ignored since it's assumed that
97  *\a observations are already undistorted pixels.
98  * \param frame_poses [IN/OUT] Input: Gross estimation of each frame poses.
99  *Output: The found optimal solution.
100  * \param landmark_points [IN/OUT] Input: Gross estimation of each landmark
101  *point. Output: The found optimal solution.
102  * \param extra_params [IN] Optional extra parameters. Read above.
103  * \param user_feedback [IN] If provided, this functor will be called at each
104  *iteration to provide a feedback to the user.
105  *
106  * \return The final overall squared error.
107  * \ingroup bundle_adj
108  */
109 double bundle_adj_full(
110  const mrpt::vision::TSequenceFeatureObservations& observations,
111  const mrpt::img::TCamera& camera_params,
112  mrpt::vision::TFramePosesVec& frame_poses,
113  mrpt::vision::TLandmarkLocationsVec& landmark_points,
114  const mrpt::system::TParametersDouble& extra_params =
118 
119 /** @} */
120 
121 /** @name Bundle-Adjustment Auxiliary methods
122  @{ */
123 
124 /** Fills the frames & landmark points maps with an initial gross estimate from
125  * the sequence \a observations, so they can be fed to bundle adjustment
126  * methods.
127  * \sa bundle_adj_full
128  * \ingroup bundle_adj
129  */
131  const mrpt::vision::TSequenceFeatureObservations& observations,
132  const mrpt::img::TCamera& camera_params,
133  mrpt::vision::TFramePosesVec& frame_poses,
134  mrpt::vision::TLandmarkLocationsVec& landmark_points);
135 
136 //! \overload
138  const mrpt::vision::TSequenceFeatureObservations& observations,
139  const mrpt::img::TCamera& camera_params,
140  mrpt::vision::TFramePosesMap& frame_poses,
141  mrpt::vision::TLandmarkLocationsMap& landmark_points);
142 
143 /** Compute reprojection error vector (used from within Bundle Adjustment
144  * methods, but can be used in general)
145  * See mrpt::vision::bundle_adj_full for a description of most parameters.
146  * \param frame_poses_are_inverse If set to true, global camera poses are \f$
147  * \ominus F \f$ instead of \f$ F \f$, for each F in frame_poses.
148  *
149  * \return Overall squared reprojection error.
150  * \ingroup bundle_adj
151  */
152 double reprojectionResiduals(
153  const mrpt::vision::TSequenceFeatureObservations& observations,
154  const mrpt::img::TCamera& camera_params,
155  const mrpt::vision::TFramePosesVec& frame_poses,
156  const mrpt::vision::TLandmarkLocationsVec& landmark_points,
157  std::vector<std::array<double, 2>>& out_residuals,
158  const bool frame_poses_are_inverse, const bool use_robust_kernel = true,
159  const double kernel_param = 3.0,
160  std::vector<double>* out_kernel_1st_deriv = nullptr);
161 
162 //! \overload
163 double reprojectionResiduals(
164  const mrpt::vision::TSequenceFeatureObservations& observations,
165  const mrpt::img::TCamera& camera_params,
166  const mrpt::vision::TFramePosesMap& frame_poses,
167  const mrpt::vision::TLandmarkLocationsMap& landmark_points,
168  std::vector<std::array<double, 2>>& out_residuals,
169  const bool frame_poses_are_inverse, const bool use_robust_kernel = true,
170  const double kernel_param = 3.0,
171  std::vector<double>* out_kernel_1st_deriv = nullptr);
172 
173 /** For each pose in the vector \a frame_poses, adds a "delta" increment to the
174  * manifold, with the "delta" given in the se(3) Lie algebra:
175  *
176  * new_frame_poses[i] = frame_poses[i] [+]
177  * delta[(first_idx+6*i):(first_idx+6*i+5)] , for every pose in \a
178  * frame_poses
179  *
180  * With the left-multiplication convention of the manifold exp(delta)
181  * operator, that is:
182  *
183  * p <-- p [+] delta ==> p <-- exp(delta) * p
184  *
185  * \param delta_num_vals Used just for sanity check, must be equal to
186  * "frame_poses.size() * 6"
187  * \ingroup bundle_adj
188  */
190  const mrpt::vision::TFramePosesVec& frame_poses,
191  const mrpt::math::CVectorDouble& delta, const size_t delta_first_idx,
192  const size_t delta_num_vals, mrpt::vision::TFramePosesVec& new_frame_poses,
193  const size_t num_fix_frames);
194 
195 /** For each pose in the vector \a frame_poses, adds a "delta" increment to the
196  * manifold, with the "delta" given in the se(3) Lie algebra:
197  *
198  * new_landmark_points[i] = landmark_points[i] +
199  * delta[(first_idx+3*i):(first_idx+3*i+2)] , for every pose in \a
200  * landmark_points
201  *
202  * \param delta_num_vals Used just for sanity check, must be equal to
203  * "landmark_points.size() * 3"
204  * \ingroup bundle_adj
205  */
207  const mrpt::vision::TLandmarkLocationsVec& landmark_points,
208  const mrpt::math::CVectorDouble& delta, const size_t delta_first_idx,
209  const size_t delta_num_vals,
210  mrpt::vision::TLandmarkLocationsVec& new_landmark_points,
211  const size_t num_fix_points);
212 
213 /** @} */
214 } // namespace mrpt::vision
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:
A complete sequence of observations of features from different camera frames (poses).
std::vector< mrpt::poses::CPose3D > TFramePosesVec
A list of camera frames (6D poses), which assumes indexes are unique, consecutive IDs...
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.
Classes for computer vision, detectors, features, etc.
Definition: CDifodo.h:17
Parameters for the Brown-Conrady camera lens distortion model.
Definition: TCamera.h:26
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...
std::map< TCameraPoseID, mrpt::poses::CPose3D > TFramePosesMap
A list of camera frames (6D poses) indexed by unique IDs.
double bundle_adj_full(const mrpt::vision::TSequenceFeatureObservations &observations, const mrpt::img::TCamera &camera_params, mrpt::vision::TFramePosesVec &frame_poses, mrpt::vision::TLandmarkLocationsVec &landmark_points, const mrpt::system::TParametersDouble &extra_params=mrpt::system::TParametersDouble(), const mrpt::vision::TBundleAdjustmentFeedbackFunctor user_feedback=mrpt::vision::TBundleAdjustmentFeedbackFunctor())
Sparse Levenberg-Marquart solution to bundle adjustment - optimizes all the camera frames & the landm...
std::function< void(const size_t cur_iter, const double cur_total_sq_error, const size_t max_iters, const mrpt::vision::TSequenceFeatureObservations &input_observations, const mrpt::vision::TFramePosesVec &current_frame_estimate, const mrpt::vision::TLandmarkLocationsVec &current_landmark_estimate)> TBundleAdjustmentFeedbackFunctor
A functor type for BA methods.
std::vector< mrpt::math::TPoint3D > TLandmarkLocationsVec
A list of landmarks (3D points), which assumes indexes are unique, consecutive IDs.



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