MRPT  1.9.9
Bundle-Adjustment methods

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 365 of file ba_common.cpp.

References ASSERT_, MRPT_END, and MRPT_START.

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

Here is the caller graph for this function:

◆ 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 329 of file ba_common.cpp.

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

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

Here is the call graph for this function:
Here is the caller graph for this function:

◆ 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 75 of file ba_common.cpp.

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

Here is the call graph for this function:

◆ 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 47 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.

Here is the call graph for this function:

◆ 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 206 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().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ 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 157 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().

Here is the call graph for this function:



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 58f856185 Mon May 20 11:43:57 2019 +0200 at lun may 20 11:50:17 CEST 2019