Main MRPT website > C++ reference for MRPT 1.5.7
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-2017, 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>
15 #include <mrpt/utils/TParameters.h>
17 #include <mrpt/utils/CArray.h>
18 
19 // The methods declared in this file are implemented in separate files in: vision/src/ba_*.cpp
20 namespace 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  */
75  const mrpt::vision::TSequenceFeatureObservations & observations,
76  const mrpt::utils::TCamera & camera_params,
77  mrpt::vision::TFramePosesVec & frame_poses,
78  mrpt::vision::TLandmarkLocationsVec & landmark_points,
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  */
95  const mrpt::vision::TSequenceFeatureObservations & observations,
96  const mrpt::utils::TCamera & camera_params,
97  mrpt::vision::TFramePosesVec & frame_poses,
98  mrpt::vision::TLandmarkLocationsVec & landmark_points );
99 
100  //! \overload
102  const mrpt::vision::TSequenceFeatureObservations & observations,
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  */
116  const mrpt::vision::TSequenceFeatureObservations & observations,
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::utils::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
129  const mrpt::vision::TSequenceFeatureObservations & observations,
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::utils::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 
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::utils::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...
Definition: ba_common.cpp:188
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:35
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, with the "delta" given in the se(3) Lie algebra:
Definition: ba_common.cpp:314
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...
Definition: ba_full.cpp:51
A complete sequence of observations of features from different camera frames (poses).
mrpt::aligned_containers< mrpt::poses::CPose3D >::vector_t TFramePosesVec
A list of camera frames (6D poses), which assumes indexes are unique, consecutive IDs...
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, with the "delta" given in the se(3) Lie algebra:
Definition: ba_common.cpp:354
A STL container (as wrapper) for arrays of constant size defined at compile time. ...
Definition: CArray.h:53
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.
std::vector< mrpt::math::TPoint3D > TLandmarkLocationsVec
A list of landmarks (3D points), which assumes indexes are unique, consecutive IDs.
std::map< TLandmarkID, mrpt::math::TPoint3D > TLandmarkLocationsMap
A list of landmarks (3D points) indexed by unique IDs.
typedef void(APIENTRYP PFNGLBLENDCOLORPROC)(GLclampf red
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...
Definition: ba_common.cpp:58
mrpt::aligned_containers< TCameraPoseID, mrpt::poses::CPose3D >::map_t TFramePosesMap
A list of camera frames (6D poses) indexed by unique IDs.
Structure to hold the parameters of a pinhole camera model.
Definition: TCamera.h:31



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019