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_vals

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

Definition at line 366 of file ba_common.cpp.

References ASSERT_, MRPT_END, and MRPT_START.

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_vals

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

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

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.

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.

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_inverse

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

Returns

Overall squared reprojection error.

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 155 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: