#include <plane_registration.hpp>

Inheritance diagram for mrob::PlaneRegistration:

Collaboration diagram for mrob::PlaneRegistration:

[legend]

Public Types
enum	TrajectoryMode { SEQUENCE =0, INTERPOLATION }

enum	SolveMode { INITIALIZE =0, GRADIENT, GRADIENT_BENGIOS_NAG, GRADIENT_ALL_POSES, GN_HESSIAN, GN_CLAMPED_HESSIAN, LM_SPHER, LM_ELLIP }

Public Types inherited from mrob::Optimizer
enum	optimMethod { NEWTON_RAPHSON =0, LEVENBERG_MARQUARDT_SPHER, LEVENBERG_MARQUARDT_ELLIP }

Public Member Functions
virtual matData_t	calculate_error () override

virtual void	calculate_gradient_hessian () override

virtual void	update_state () override

virtual void	bookkeep_state () override

virtual void	update_state_from_bookkeep () override

void	set_number_planes_and_poses (uint_t numPlanes, uint_t numPoses)

uint_t	get_number_planes () const

uint_t	get_number_poses () const

uint_t	solve (SolveMode mode, bool singleIteration=false)

uint_t	solve_interpolate_gradient (bool singleIteration=false)

uint_t	solve_gradient_all_poses (bool singleIteration=false)

uint_t	solve_initialize ()

uint_t	solve_quaternion_plane ()

void	reset_solution ()

double	get_current_error () const

SE3	get_pose (uint_t time)

std::vector< SE3 > &	get_trajectory ()

SE3	get_last_pose ()

void	set_last_pose (SE3 &last)

void	add_plane (uint_t id, std::shared_ptr< Plane > &plane)

void	add_new_plane (uint_t id)

void	plane_push_back_point (uint_t id, uint_t t, Mat31 &point)

uint_t	calculate_total_number_points ()

std::shared_ptr< Plane > &	get_plane (uint_t id)

std::unordered_map< uint_t, std::shared_ptr< Plane > > &	get_all_planes ()

void	set_alpha_parameter (double alpha)

void	set_beta_parameter (double beta)

double	calculate_poses_rmse (std::vector< SE3 > &groundTruth) const

void	print (bool plotPlanes=true) const

std::vector< double >	print_evaluate ()

void	add_point_cloud_planes (uint_t time, std::vector< Mat31 > &points, std::vector< uint_t > &point_ids)

std::vector< Mat31 >	get_point_cloud (uint_t time)

std::vector< Mat31 >	get_point_plane_ids (uint_t time)

Public Member Functions inherited from mrob::OptimizerDense
	OptimizerDense (matData_t solutionTolerance=1e-4, matData_t lambda=1e-5)

Public Member Functions inherited from mrob::Optimizer
	Optimizer (matData_t solutionTolerance=1e-4, matData_t lambda=1e-5)

uint_t	solve (optimMethod method, uint_t max_iters=1e2, double lambda=1e-5)

Protected Attributes
uint_t	numberPlanes_

uint_t	numberPoses_

uint_t	numberPoints_

uint_t	isSolved_

PlaneRegistration::TrajectoryMode	trajMode_ {}

uint_t	time_ {}

std::unordered_map< uint_t, std::shared_ptr< Plane > >	planes_

std::shared_ptr< std::vector< SE3 > >	trajectory_

std::vector< SE3 >	temporary_trajectory_

SE3	bookept_trajectory_

double	tau_ {}

uint_t	solveIters_ {}

PlaneRegistration::SolveMode	solveMode_

std::vector< Mat61 >	previousState_

double	c1_

double	c2_

double	alpha_

double	beta_

Mat61	gradient__

Mat6	hessian__

TimeProfiling	time_profiles_

double	initial_error_ {}

Protected Attributes inherited from mrob::OptimizerDense
MatX	hessian_

Protected Attributes inherited from mrob::Optimizer
optimMethod	optimization_method_ {}

matData_t	solutionTolerance_

uint_t	max_iters_

MatX1	gradient_

MatX1	dx_

matData_t	lambda_

Additional Inherited Members
Protected Member Functions inherited from mrob::OptimizerDense
uint_t	optimize_newton_raphson_one_iteration (bool useLambda=false)

matData_t	calculate_model_fidelity (matData_t diff_error)

Protected Member Functions inherited from mrob::Optimizer
uint_t	optimize_newton_raphson ()

uint_t	optimize_levenberg_marquardt ()

Detailed Description

class PlaneRegistration introduced a class for the alignment of planes.

TO BE DEPRECATED

Member Function Documentation

◆ add_new_plane()

void PlaneRegistration::add_new_plane ( uint_t id )

add new plane is an ALTERNATIVE (for py) to the above function which creates a new plane inside the class and then points are added one by one

◆ add_plane()

void PlaneRegistration::add_plane	(	uint_t	id,
		std::shared_ptr< Plane > &	plane
	)

add_plane adds a plane structure already initialized and filled with data

◆ add_point_cloud_planes()

void mrob::PlaneRegistration::add_point_cloud_planes	(	uint_t	time,
		std::vector< Mat31 > &	points,
		std::vector< uint_t > &	point_ids
	)

add point_cloud requires a complete set of points observed at a given time stamp (XXX now only an integer) and fills in the registration structure.

◆ bookkeep_state()

void PlaneRegistration::bookkeep_state ( )

overridevirtual

For Levenberg-Marquard This function bookkeeps the current state values This is in case the optimization step does not improve

Implements mrob::Optimizer.

◆ calculate_error()

matData_t PlaneRegistration::calculate_error ( )

overridevirtual

General abstract functions to implement: Calculate error calculates the current error function

Implements mrob::Optimizer.

◆ calculate_gradient_hessian()

void PlaneRegistration::calculate_gradient_hessian ( )

overridevirtual

Gradient calculates the gradient and Hessian This function may be called after calculate_error() or not (some cases of LM). For a general purpose it is required that this function is either: 1) self-contained. No assumptions made and recalculated (redundantly) residuals 2) Most of the times will be called after calculate_error. No recalculations are requires except inside the update_bookkeep_state which will have invalid residuals and need update (less prefered option)

Implements mrob::Optimizer.

◆ get_point_cloud()

std::vector< Mat31 > PlaneRegistration::get_point_cloud ( uint_t time )

get_point_cloud gets all raw point, according to the current time index from trajectory. It does not distinguish between planes.

◆ get_pose()

SE3 PlaneRegistration::get_pose ( uint_t time )

Get pose from trajectory, a SE3 transformations,

◆ get_trajectory()

std::vector< SE3 > & PlaneRegistration::get_trajectory ( )

Get trajectory returns a SE3 transformations,

◆ plane_push_back_point()

void PlaneRegistration::plane_push_back_point	(	uint_t	id,
		uint_t	t,
		Mat31 &	point
	)

add a point to the new plane. An ALTERNATIVE (for py) to add points into the sover class (instead of adding the point fully as in add_plane())

◆ print_evaluate()

std::vector< double > PlaneRegistration::print_evaluate ( )

print evaluate looks for degenerate cases, such as planes normal vectors, Hessian rank, det of all normals, etc. Basically this function tries to answer if the problem is ill-conditioned

Returns: 0) current error 1) number of iters, 2) determinant 3) number of negative eigenvalues 4) conditioning number

◆ reset_solution()

void PlaneRegistration::reset_solution ( )

reset_solution, resets the current calculated solution while maintainting all data (planes) This function is intended for comparing different solvers without replicating data

◆ set_last_pose()

void PlaneRegistration::set_last_pose ( SE3 & last )

Sets the trajectory (current solution) by addint the last pose

◆ solve()

uint_t PlaneRegistration::solve	(	SolveMode	mode,
		bool	singleIteration = `false`
	)

solve() calculates the poses on trajectory such that the minimization objective is met: J = sum (lamda_min_plane)

◆ solve_initialize()

uint_t PlaneRegistration::solve_initialize ( )

solve_interpolate_hessian() calculates the poses on trajectory such that the minimization objective is met: J = sum (lamda_min_plane), and the trajectory is described as an interpolation from I to T_f using second order methods with Hessian. Very similar to solve_interpolate Initialization_solve give a first guess on all poses by using classical point-point methods SVD-based to calculate an initial condition closer to the true solution

◆ solve_interpolate_gradient()

uint_t PlaneRegistration::solve_interpolate_gradient ( bool singleIteration = false )

solve_interpolate() calculates the poses on trajectory such that the minimization objective is met: J = sum (lamda_min_plane), and the trajectory is described as an interpolation from I to T_f

◆ solve_quaternion_plane()

uint_t PlaneRegistration::solve_quaternion_plane ( )

Solve quaternion plane uses a paramteric representation for each plane, a quaternion, and optimizes both the plane parameters and the trajectory variables

◆ update_state()

void PlaneRegistration::update_state ( )

overridevirtual

Updates the current solution

Implements mrob::Optimizer.

◆ update_state_from_bookkeep()

void PlaneRegistration::update_state_from_bookkeep ( )

overridevirtual

For Levenberg-Marquard Undoes an incorrect update

Implements mrob::Optimizer.

The documentation for this class was generated from the following files:

src/plane-surfaces/mrob/plane_registration.hpp
src/plane-surfaces/plane_registration.cpp

Public Types

Public Member Functions

Protected Attributes

Additional Inherited Members

Detailed Description

Member Function Documentation

◆ add_new_plane()

◆ add_plane()

◆ add_point_cloud_planes()

◆ bookkeep_state()

◆ calculate_error()

◆ calculate_gradient_hessian()

◆ get_point_cloud()

◆ get_pose()

◆ get_trajectory()

◆ plane_push_back_point()

◆ print_evaluate()

◆ reset_solution()

◆ set_last_pose()

◆ solve()

◆ solve_initialize()

◆ solve_interpolate_gradient()

◆ solve_quaternion_plane()

◆ update_state()

◆ update_state_from_bookkeep()