MRPT  1.9.9
Bundle-Adjustment methods
[mrpt-vision]

Detailed Description

Collaboration diagram for Bundle-Adjustment methods:

Bundle-Adjustment methods

double mrpt::vision::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 landmark locations. More...
 

Bundle-Adjustment Auxiliary methods

void mrpt::vision::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, so they can be fed to bundle adjustment methods. More...
 
double mrpt::vision::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 general) See mrpt::vision::bundle_adj_full for a description of most parameters. More...
 
void mrpt::vision::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: More...
 
void mrpt::vision::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: More...
 
void mrpt::vision::ba_initial_estimate (const mrpt::vision::TSequenceFeatureObservations &observations, const mrpt::img::TCamera &camera_params, mrpt::vision::TFramePosesMap &frame_poses, mrpt::vision::TLandmarkLocationsMap &landmark_points)
 
double mrpt::vision::reprojectionResiduals (const mrpt::vision::TSequenceFeatureObservations &observations, const mrpt::img::TCamera &camera_params, const mrpt::vision::TFramePosesMap &frame_poses, const mrpt::vision::TLandmarkLocationsMap &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 general) See mrpt::vision::bundle_adj_full for a description of most parameters. More...
 

Function Documentation

◆ add_3d_deltas_to_points()

void mrpt::vision::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:

new_landmark_points[i] = landmark_points[i] + delta[(first_idx+3*i):(first_idx+3*i+2)] , for every pose in landmark_points

Parameters
delta_num_valsUsed just for sanity check, must be equal to "landmark_points.size() * 3"

Definition at line 364 of file ba_common.cpp.

References ASSERT_, MRPT_END, and MRPT_START.

Referenced by mrpt::vision::bundle_adj_full().

◆ add_se3_deltas_to_frames()

void mrpt::vision::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:

new_frame_poses[i] = frame_poses[i] [+] delta[(first_idx+6*i):(first_idx+6*i+5)] , for every pose in frame_poses

With the left-multiplication convention of the manifold exp(delta) operator, that is: 

 p <-- p [+] delta ==>  p <-- exp(delta) * p
Parameters
delta_num_valsUsed just for sanity check, must be equal to "frame_poses.size() * 6"

Definition at line 328 of file ba_common.cpp.

References ASSERT_, mrpt::poses::CPose3D::composeFrom(), MRPT_END, and MRPT_START.

Referenced by mrpt::vision::bundle_adj_full().

◆ ba_initial_estimate() [1/2]

void mrpt::vision::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, so they can be fed to bundle adjustment methods.

See also
bundle_adj_full

Definition at line 74 of file ba_common.cpp.

References mrpt::keep_max(), MRPT_END, MRPT_START, and MRPT_UNUSED_PARAM.

◆ ba_initial_estimate() [2/2]

void mrpt::vision::ba_initial_estimate ( const mrpt::vision::TSequenceFeatureObservations &  observations,
const mrpt::img::TCamera &  camera_params,
mrpt::vision::TFramePosesMap &  frame_poses,
mrpt::vision::TLandmarkLocationsMap &  landmark_points 
)

This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts.

Definition at line 46 of file ba_common.cpp.

References MRPT_END, MRPT_START, and MRPT_UNUSED_PARAM.

◆ bundle_adj_full()

double mrpt::vision::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 landmark locations.

At input a gross estimation of the frame poses & the landmark points must be supplied. If you don't have such a starting point, use mrpt::vision::ba_initial_estimate() to compute it.

At output the best found solution will be returned in the variables. Optionally, a functor can be passed for having feedback on the progress at each iteration (you can use it to refresh a GUI, display a progress bar, etc...).

This implementation is almost entirely an adapted version from RobotVision (at OpenSLAM.org) (C) by Hauke Strasdat (Imperial College London), licensed under GNU LGPL. 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

List of optional parameters in "extra_params":

  • "verbose" : Verbose output (default=0)
  • "max_iterations": Maximum number of iterations to run (default=50)
  • "robust_kernel": If !=0, use a robust kernel against outliers (default=1)
  • "kernel_param": The pseudo-huber kernel parameter (default=3)
  • "mu": Initial mu for LevMarq (default=-1 -> autoguess)
  • "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)
  • "num_fix_points": Idem, for the landmarks positions (default=0: optimize all)
  • "profiler": If !=0, displays profiling information to the console at return.
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).
The first frame pose will be not updated since at least one frame must remain fixed.
Parameters
observations[IN] All the feature observations (WITHOUT distortion), indexed by feature ID as lists of <frame_ID, (x,y)>. See TSequenceFeatureObservations.
camera_params[IN] The camera parameters, mainly used for the intrinsic 3x3 matrix. Distortion params are ignored since it's assumed that observations are already undistorted pixels.
frame_poses[IN/OUT] Input: Gross estimation of each frame poses. Output: The found optimal solution.
landmark_points[IN/OUT] Input: Gross estimation of each landmark point. Output: The found optimal solution.
extra_params[IN] Optional extra parameters. Read above.
user_feedback[IN] If provided, this functor will be called at each iteration to provide a feedback to the user.
Returns
The final overall squared error.

Definition at line 51 of file ba_full.cpp.

References mrpt::vision::add_3d_deltas_to_points(), mrpt::vision::add_se3_deltas_to_frames(), ASSERT_, ASSERT_ABOVE_, ASSERT_ABOVEEQ_, mrpt::vision::ba_build_gradient_Hessians(), mrpt::math::CSparseMatrix::compressFromTriplet(), mrpt::system::CTimeLogger::enter(), eps, mrpt::system::TParameters< T >::getWithDefaultVal(), mrpt::math::CSparseMatrix::insert_submatrix(), INV_POSES_BOOL, mrpt::pbmap::inverse(), mrpt::keep_max(), mrpt::system::CTimeLogger::leave(), mrpt::system::os::memcpy(), MRPT_CHECK_NORMAL_NUMBER, MRPT_END, MRPT_START, norm_inf(), mrpt::vision::reprojectionResiduals(), mrpt::math::UNINITIALIZED_MATRIX, and VERBOSE_COUT.

◆ reprojectionResiduals() [1/2]

double mrpt::vision::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 general) See mrpt::vision::bundle_adj_full for a description of most parameters.

Parameters
frame_poses_are_inverseIf set to true, global camera poses are $ \ominus F $ instead of $ F $, for each F in frame_poses.
Returns
Overall squared reprojection error.

Definition at line 205 of file ba_common.cpp.

References ASSERT_BELOW_, mrpt::vision::TFeatureObservation::id_feature, mrpt::vision::TFeatureObservation::id_frame, MRPT_END, MRPT_START, and mrpt::math::sum().

Referenced by mrpt::vision::bundle_adj_full().

◆ reprojectionResiduals() [2/2]

double mrpt::vision::reprojectionResiduals ( const mrpt::vision::TSequenceFeatureObservations &  observations,
const mrpt::img::TCamera &  camera_params,
const mrpt::vision::TFramePosesMap &  frame_poses,
const mrpt::vision::TLandmarkLocationsMap &  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 general) See mrpt::vision::bundle_adj_full for a description of most parameters.

This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts.

Returns
Overall squared reprojection error.

Definition at line 156 of file ba_common.cpp.

References ASSERTMSG_, mrpt::vision::TFeatureObservation::id_feature, mrpt::vision::TFeatureObservation::id_frame, MRPT_END, MRPT_START, and mrpt::math::sum().



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020