Bundle-Adjustment methods

// global functions

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::containers::yaml& extra_params = {},
    const mrpt::vision::TBundleAdjustmentFeedbackFunctor user_feedback = mrpt::vision::TBundleAdjustmentFeedbackFunctor()
    );

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
    );

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::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
    );

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
    );

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
    );

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
    );

Global Functions

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::containers::yaml& extra_params = {},
    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.

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.

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

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.

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_inverse

If set to true, global camera poses are \(\ominus F\) instead of \(F\), for each F in frame_poses.

Returns:

Overall squared reprojection error.

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
    )

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

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.

Returns:

Overall squared reprojection error.

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_vals

Used just for sanity check, must be equal to “frame_poses.size() * 6”

 
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_vals

Used just for sanity check, must be equal to “landmark_points.size() * 3”