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.2 for MRPT 0.9.4 SVN: at Mon Jan 10 22:30:30 UTC 2011 |