Main MRPT website > C++ reference for MRPT 1.4.0
bundle_adjustment.h
Go to the documentation of this file.
1/* +---------------------------------------------------------------------------+
2 | Mobile Robot Programming Toolkit (MRPT) |
3 | http://www.mrpt.org/ |
4 | |
5 | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file |
6 | See: http://www.mrpt.org/Authors - All rights reserved. |
7 | Released under BSD License. See details in http://www.mrpt.org/License |
8 +---------------------------------------------------------------------------+ */
9
10#ifndef mrpt_vision_ba_H
11#define mrpt_vision_ba_H
12
13#include <mrpt/vision/types.h>
14#include <mrpt/utils/TCamera.h>
17#include <mrpt/math/CArray.h>
18
19// The methods declared in this file are implemented in separate files in: vision/src/ba_*.cpp
20namespace mrpt
21{
22 namespace vision
23 {
24 /** \defgroup bundle_adj Bundle-Adjustment methods
25 * \ingroup mrpt_vision_grp
26 */
27
28 /** @name Bundle-Adjustment methods
29 @{ */
30
31 /** A functor type for BA methods \sa bundle_adj_full */
33 const size_t cur_iter,
34 const double cur_total_sq_error,
35 const size_t max_iters,
36 const mrpt::vision::TSequenceFeatureObservations & input_observations,
37 const mrpt::vision::TFramePosesVec & current_frame_estimate,
38 const mrpt::vision::TLandmarkLocationsVec & current_landmark_estimate );
39
40
41 /** Sparse Levenberg-Marquart solution to bundle adjustment - optimizes all the camera frames & the landmark locations.
42 * At input a gross estimation of the frame poses & the landmark points must be supplied. If you don't have such a
43 * starting point, use mrpt::vision::ba_initial_estimate() to compute it.
44 *
45 * At output the best found solution will be returned in the variables. Optionally, a functor can be passed for having
46 * feedback on the progress at each iteration (you can use it to refresh a GUI, display a progress bar, etc...).
47 *
48 * This implementation is almost entirely an adapted version from RobotVision (at OpenSLAM.org) (C) by Hauke Strasdat (Imperial College London), licensed under GNU LGPL.
49 * See the related paper: H. Strasdat, J.M.M. Montiel, A.J. Davison: "Scale Drift-Aware Large Scale Monocular SLAM", RSS2010, http://www.roboticsproceedings.org/rss06/p10.html
50 *
51 * List of optional parameters in "extra_params":
52 * - "verbose" : Verbose output (default=0)
53 * - "max_iterations": Maximum number of iterations to run (default=50)
54 * - "robust_kernel": If !=0, use a robust kernel against outliers (default=1)
55 * - "kernel_param": The pseudo-huber kernel parameter (default=3)
56 * - "mu": Initial mu for LevMarq (default=-1 -> autoguess)
57 * - "num_fix_frames": Number of first frame poses to don't optimize (keep unmodified as they come in) (default=1: the first pose is the reference and is not modified)
58 * - "num_fix_points": Idem, for the landmarks positions (default=0: optimize all)
59 * - "profiler": If !=0, displays profiling information to the console at return.
60 *
61 * \note In this function, all coordinates are absolute. Camera frames are such that +Z points forward from the focal point (see the figure in mrpt::obs::CObservationImage).
62 * \note The first frame pose will be not updated since at least one frame must remain fixed.
63 *
64 * \param observations [IN] All the feature observations (WITHOUT distortion), indexed by feature ID as lists of <frame_ID, (x,y)>. See TSequenceFeatureObservations.
65 * \param camera_params [IN] The camera parameters, mainly used for the intrinsic 3x3 matrix. Distortion params are ignored since it's assumed that \a observations are already undistorted pixels.
66 * \param frame_poses [IN/OUT] Input: Gross estimation of each frame poses. Output: The found optimal solution.
67 * \param landmark_points [IN/OUT] Input: Gross estimation of each landmark point. Output: The found optimal solution.
68 * \param extra_params [IN] Optional extra parameters. Read above.
69 * \param user_feedback [IN] If provided, this functor will be called at each iteration to provide a feedback to the user.
70 *
71 * \return The final overall squared error.
72 * \ingroup bundle_adj
73 */
76 const mrpt::utils::TCamera & camera_params,
77 mrpt::vision::TFramePosesVec & frame_poses,
80 const mrpt::vision::TBundleAdjustmentFeedbackFunctor user_feedback = NULL
81 );
82
83
84 /** @} */
85
86
87 /** @name Bundle-Adjustment Auxiliary methods
88 @{ */
89
90 /** Fills the frames & landmark points maps with an initial gross estimate from the sequence \a observations, so they can be fed to bundle adjustment methods.
91 * \sa bundle_adj_full
92 * \ingroup bundle_adj
93 */
96 const mrpt::utils::TCamera & camera_params,
97 mrpt::vision::TFramePosesVec & frame_poses,
98 mrpt::vision::TLandmarkLocationsVec & landmark_points );
99
100 //! \overload
103 const mrpt::utils::TCamera & camera_params,
104 mrpt::vision::TFramePosesMap & frame_poses,
105 mrpt::vision::TLandmarkLocationsMap & landmark_points );
106
107
108 /** Compute reprojection error vector (used from within Bundle Adjustment methods, but can be used in general)
109 * See mrpt::vision::bundle_adj_full for a description of most parameters.
110 * \param frame_poses_are_inverse If set to true, global camera poses are \f$ \ominus F \f$ instead of \f$ F \f$, for each F in frame_poses.
111 *
112 * \return Overall squared reprojection error.
113 * \ingroup bundle_adj
114 */
117 const mrpt::utils::TCamera & camera_params,
118 const mrpt::vision::TFramePosesVec & frame_poses,
119 const mrpt::vision::TLandmarkLocationsVec & landmark_points,
120 std::vector<mrpt::math::CArray<double,2> > & out_residuals,
121 const bool frame_poses_are_inverse,
122 const bool use_robust_kernel = true,
123 const double kernel_param = 3.0,
124 std::vector<double> * out_kernel_1st_deriv = NULL
125 );
126
127 //! \overload
130 const mrpt::utils::TCamera & camera_params,
131 const mrpt::vision::TFramePosesMap & frame_poses,
132 const mrpt::vision::TLandmarkLocationsMap & landmark_points,
133 std::vector<mrpt::math::CArray<double,2> > & out_residuals,
134 const bool frame_poses_are_inverse,
135 const bool use_robust_kernel = true,
136 const double kernel_param = 3.0,
137 std::vector<double> * out_kernel_1st_deriv = NULL
138 );
139
140
141 /** For each pose in the vector \a frame_poses, adds a "delta" increment to the manifold, with the "delta" given in the se(3) Lie algebra:
142 *
143 * new_frame_poses[i] = frame_poses[i] [+] delta[(first_idx+6*i):(first_idx+6*i+5)] , for every pose in \a frame_poses
144 *
145 * With the left-multiplication convention of the manifold exp(delta) operator, that is:
146 *
147 * p <-- p [+] delta ==> p <-- exp(delta) * p
148 *
149 * \param delta_num_vals Used just for sanity check, must be equal to "frame_poses.size() * 6"
150 * \ingroup bundle_adj
151 */
153 const mrpt::vision::TFramePosesVec & frame_poses,
154 const mrpt::math::CVectorDouble &delta,
155 const size_t delta_first_idx,
156 const size_t delta_num_vals,
157 mrpt::vision::TFramePosesVec & new_frame_poses,
158 const size_t num_fix_frames );
159
160 /** For each pose in the vector \a frame_poses, adds a "delta" increment to the manifold, with the "delta" given in the se(3) Lie algebra:
161 *
162 * new_landmark_points[i] = landmark_points[i] + delta[(first_idx+3*i):(first_idx+3*i+2)] , for every pose in \a landmark_points
163 *
164 * \param delta_num_vals Used just for sanity check, must be equal to "landmark_points.size() * 3"
165 * \ingroup bundle_adj
166 */
168 const mrpt::vision::TLandmarkLocationsVec & landmark_points,
169 const mrpt::math::CVectorDouble & delta,
170 const size_t delta_first_idx,
171 const size_t delta_num_vals,
172 mrpt::vision::TLandmarkLocationsVec & new_landmark_points,
173 const size_t num_fix_points );
174
175 /** @} */
176 }
177}
178#endif
179
A STL container (as wrapper) for arrays of constant size defined at compile time - Users will most li...
Definition: CArray.h:55
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction.
Definition: types_math.h:65
Structure to hold the parameters of a pinhole camera model.
Definition: TCamera.h:32
void VISION_IMPEXP ba_initial_estimate(const mrpt::vision::TSequenceFeatureObservations &observations, const mrpt::utils::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 VISION_IMPEXP 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,...
void VISION_IMPEXP 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,...
double VISION_IMPEXP bundle_adj_full(const mrpt::vision::TSequenceFeatureObservations &observations, const mrpt::utils::TCamera &camera_params, mrpt::vision::TFramePosesVec &frame_poses, mrpt::vision::TLandmarkLocationsVec &landmark_points, const mrpt::utils::TParametersDouble &extra_params=mrpt::utils::TParametersDouble(), const mrpt::vision::TBundleAdjustmentFeedbackFunctor user_feedback=NULL)
Sparse Levenberg-Marquart solution to bundle adjustment - optimizes all the camera frames & the landm...
double VISION_IMPEXP reprojectionResiduals(const mrpt::vision::TSequenceFeatureObservations &observations, const mrpt::utils::TCamera &camera_params, const mrpt::vision::TFramePosesVec &frame_poses, const mrpt::vision::TLandmarkLocationsVec &landmark_points, std::vector< mrpt::math::CArray< 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=NULL)
Compute reprojection error vector (used from within Bundle Adjustment methods, but can be used in gen...
std::map< TLandmarkID, mrpt::math::TPoint3D > TLandmarkLocationsMap
A list of landmarks (3D points) indexed by unique IDs.
std::vector< mrpt::math::TPoint3D > TLandmarkLocationsVec
A list of landmarks (3D points), which assumes indexes are unique, consecutive IDs.
mrpt::aligned_containers< mrpt::poses::CPose3D >::vector_t TFramePosesVec
A list of camera frames (6D poses), which assumes indexes are unique, consecutive IDs.
mrpt::aligned_containers< TCameraPoseID, mrpt::poses::CPose3D >::map_t TFramePosesMap
A list of camera frames (6D poses) indexed by unique IDs.
void(* TBundleAdjustmentFeedbackFunctor)(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)
A functor type for BA methods.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
For usage when passing a dynamic number of (numeric) arguments to a function, by name.
Definition: TParameters.h:47
A complete sequence of observations of features from different camera frames (poses).



Page generated by Doxygen 1.9.5 for MRPT 1.4.0 SVN: at Sun Nov 27 02:56:59 UTC 2022