Main MRPT website > C++ reference
MRPT logo

bundle_adjustment.h

Go to the documentation of this file.
00001 /* +---------------------------------------------------------------------------+
00002    |          The Mobile Robot Programming Toolkit (MRPT) C++ library          |
00003    |                                                                           |
00004    |                   http://mrpt.sourceforge.net/                            |
00005    |                                                                           |
00006    |   Copyright (C) 2005-2011  University of Malaga                           |
00007    |                                                                           |
00008    |    This software was written by the Machine Perception and Intelligent    |
00009    |      Robotics Lab, University of Malaga (Spain).                          |
00010    |    Contact: Jose-Luis Blanco  <jlblanco@ctima.uma.es>                     |
00011    |                                                                           |
00012    |  This file is part of the MRPT project.                                   |
00013    |                                                                           |
00014    |     MRPT is free software: you can redistribute it and/or modify          |
00015    |     it under the terms of the GNU General Public License as published by  |
00016    |     the Free Software Foundation, either version 3 of the License, or     |
00017    |     (at your option) any later version.                                   |
00018    |                                                                           |
00019    |   MRPT is distributed in the hope that it will be useful,                 |
00020    |     but WITHOUT ANY WARRANTY; without even the implied warranty of        |
00021    |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         |
00022    |     GNU General Public License for more details.                          |
00023    |                                                                           |
00024    |     You should have received a copy of the GNU General Public License     |
00025    |     along with MRPT.  If not, see <http://www.gnu.org/licenses/>.         |
00026    |                                                                           |
00027    +---------------------------------------------------------------------------+ */
00028 
00029 #ifndef mrpt_vision_ba_H
00030 #define mrpt_vision_ba_H
00031 
00032 #include <mrpt/vision/types.h>
00033 #include <mrpt/utils/TCamera.h>
00034 #include <mrpt/math/lightweight_geom_data.h>
00035 
00036 // The methods declared in this file are implemented in separate files in: vision/src/ba_*.cpp
00037 namespace mrpt
00038 {
00039         namespace vision
00040         {
00041                 using mrpt::math::TPose3D;
00042                 using mrpt::math::TPoint3D;
00043                 using mrpt::utils::TCamera;
00044 
00045                 /** @name Bundle-Adjustment methods
00046                     @{ */
00047 
00048                 /** A functor type for BA methods \sa bundle_adj_full */
00049                 typedef void (*TBundleAdjustmentFeedbackFunctor)(
00050                         const size_t cur_iter,
00051                         const double cur_total_sq_error,
00052                         const size_t max_iters,
00053                         const mrpt::vision::TSequenceFeatureObservations & input_observations,
00054                         const mrpt::vision::TFramePosesVec & current_frame_estimate,
00055                         const mrpt::vision::TLandmarkLocationsVec & current_landmark_estimate );
00056 
00057 
00058                 /** Sparse Levenberg-Marquart solution to bundle adjustment - optimizes all the camera frames & the landmark locations.
00059                   * At input a gross estimation of the frame poses & the landmark points must be supplied. If you don't have such a
00060                   *  starting point, use mrpt::vision::ba_initial_estimate() to compute it.
00061                   *
00062                   * At output the best found solution will be returned in the variables. Optionally, a functor can be passed for having
00063                   *  feedback on the progress at each iteration (you can use it to refresh a GUI, display a progress bar, etc...).
00064                   *
00065                   * This implementation is almost entirely an adapted version from RobotVision (at OpenSLAM.org) (C) by Hauke Strasdat (Imperial College London), licensed under GNU LGPL.
00066                   *  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
00067                   *
00068                   *  List of optional parameters in "extra_params":
00069                   *             - "verbose" : Verbose output (default=0)
00070                   *             - "max_iterations": Maximum number of iterations to run (default=50)
00071                   *             - "robust_kernel": If !=0, use a robust kernel against outliers (default=1)
00072                   *             - "mu": Initial mu for LevMarq (default=-1 -> autoguess)
00073                   *             - "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)
00074                   *             - "num_fix_points": Idem, for the landmarks positions (default=0: optimize all)
00075                   *             - "profiler": If !=0, displays profiling information to the console at return.
00076                   *
00077                   * \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::slam::CObservationImage).
00078                   * \note The first frame pose will be not updated since at least one frame must remain fixed.
00079                   *
00080                   * \param observations [IN] All the feature observations (WITHOUT distortion), indexed by feature ID as lists of <frame_ID, (x,y)>. See TSequenceFeatureObservations.
00081                   * \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.
00082                   * \param frame_poses [IN/OUT] Input: Gross estimation of each frame poses. Output: The found optimal solution.
00083                   * \param landmark_points [IN/OUT] Input: Gross estimation of each landmark point. Output: The found optimal solution.
00084                   * \param extra_params [IN] Optional extra parameters. Read above.
00085                   * \param user_feedback [IN] If provided, this functor will be called at each iteration to provide a feedback to the user.
00086                   *
00087                   * \return The final overall squared error.
00088                   */
00089                 double VISION_IMPEXP bundle_adj_full(
00090                         const mrpt::vision::TSequenceFeatureObservations   & observations,
00091                         const mrpt::utils::TCamera                        & camera_params,
00092                         mrpt::vision::TFramePosesVec                       & frame_poses,
00093                         mrpt::vision::TLandmarkLocationsVec                & landmark_points,
00094                         const mrpt::utils::TParametersDouble & extra_params = mrpt::utils::TParametersDouble(),
00095                         const mrpt::vision::TBundleAdjustmentFeedbackFunctor user_feedback = NULL
00096                         );
00097 
00098 
00099                 /** @} */
00100 
00101 
00102                 /** @name Bundle-Adjustment Auxiliary methods
00103                     @{ */
00104 
00105                 /** 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.
00106                   * \sa bundle_adj_full
00107                   */
00108                 void VISION_IMPEXP ba_initial_estimate(
00109                         const mrpt::vision::TSequenceFeatureObservations   & observations,
00110                         const mrpt::utils::TCamera                        & camera_params,
00111                         mrpt::vision::TFramePosesVec                       & frame_poses,
00112                         mrpt::vision::TLandmarkLocationsVec                & landmark_points );
00113 
00114                 //! \overload
00115                 void VISION_IMPEXP ba_initial_estimate(
00116                         const mrpt::vision::TSequenceFeatureObservations   & observations,
00117                         const mrpt::utils::TCamera                        & camera_params,
00118                         mrpt::vision::TFramePosesMap                       & frame_poses,
00119                         mrpt::vision::TLandmarkLocationsMap                & landmark_points );
00120 
00121 
00122                 /** Compute reprojection error vector (used from within Bundle Adjustment methods, but can be used in general)
00123                   *  See mrpt::vision::bundle_adj_full for a description of most parameters.
00124                   * \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.
00125                   *
00126                   *  \return Overall squared reprojection error.
00127                   */
00128                 double VISION_IMPEXP reprojectionResiduals(
00129                         const mrpt::vision::TSequenceFeatureObservations   & observations,
00130                         const mrpt::utils::TCamera                        & camera_params,
00131                         const mrpt::vision::TFramePosesVec                 & frame_poses,
00132                         const mrpt::vision::TLandmarkLocationsVec          & landmark_points,
00133                         std::vector<CArray<double,2> > & out_residuals,
00134                         const bool  frame_poses_are_inverse,
00135                         const bool  use_robust_kernel = true
00136                         );
00137 
00138                 //! \overload
00139                 double VISION_IMPEXP reprojectionResiduals(
00140                         const mrpt::vision::TSequenceFeatureObservations   & observations,
00141                         const mrpt::utils::TCamera                        & camera_params,
00142                         const mrpt::vision::TFramePosesMap                 & frame_poses,
00143                         const mrpt::vision::TLandmarkLocationsMap          & landmark_points,
00144                         std::vector<CArray<double,2> > & out_residuals,
00145                         const bool  frame_poses_are_inverse,
00146                         const bool  use_robust_kernel = true
00147                         );
00148 
00149 
00150                 /** 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:
00151                   *
00152                   *    new_frame_poses[i] = frame_poses[i] [+] delta[(first_idx+6*i):(first_idx+6*i+5)]    , for every pose in \a frame_poses
00153                   *
00154                   *  With the left-multiplication convention of the manifold exp(delta) operator, that is:
00155                   *
00156                   *     p <-- p [+] delta ==>  p <-- exp(delta) * p
00157                   *
00158                   * \param delta_num_vals Used just for sanity check, must be equal to "frame_poses.size() * 6"
00159                   */
00160                 void VISION_IMPEXP add_se3_deltas_to_frames(
00161                         const mrpt::vision::TFramePosesVec & frame_poses,
00162                         const mrpt::vector_double &delta,
00163                         const size_t         delta_first_idx,
00164                         const size_t         delta_num_vals,
00165                         mrpt::vision::TFramePosesVec       & new_frame_poses,
00166                         const size_t         num_fix_frames );
00167 
00168                 /** 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:
00169                   *
00170                   *    new_landmark_points[i] = landmark_points[i] + delta[(first_idx+3*i):(first_idx+3*i+2)]    , for every pose in \a landmark_points
00171                   *
00172                   * \param delta_num_vals Used just for sanity check, must be equal to "landmark_points.size() * 3"
00173                   */
00174                 void VISION_IMPEXP add_3d_deltas_to_points(
00175                         const mrpt::vision::TLandmarkLocationsVec & landmark_points,
00176                         const mrpt::vector_double       & delta,
00177                         const size_t                delta_first_idx,
00178                         const size_t                delta_num_vals,
00179                         mrpt::vision::TLandmarkLocationsVec        & new_landmark_points,
00180                         const size_t                num_fix_points );
00181 
00182                 /** @} */
00183         }
00184 }
00185 #endif
00186 



Page generated by Doxygen 1.7.3 for MRPT 0.9.4 SVN: at Sat Mar 26 06:40:17 UTC 2011