#include <EigenFactorPlaneCenter.hpp>

Inheritance diagram for mrob::EigenFactorPlaneCenter:

[legend]

Collaboration diagram for mrob::EigenFactorPlaneCenter:

Public Member Functions
	EigenFactorPlaneCenter (Factor::robustFactorType robust_type=Factor::robustFactorType::QUADRATIC)

void	evaluate_residuals () override

void	evaluate_jacobians () override

void	evaluate_chi2 () override

Public Member Functions inherited from mrob::EigenFactorPlane
	EigenFactorPlane (Factor::robustFactorType robust_type=Factor::robustFactorType::QUADRATIC)

void	print () const

MatRefConst	get_obs () const

VectRefConst	get_residual () const

MatRefConst	get_information_matrix () const

MatRefConst	get_jacobian ([[maybe_unused]] mrob::factor_id_t id=0) const override

MatRefConst	get_hessian (mrob::factor_id_t id=0) const override

VectRefConst	get_state (void) const override

void	add_point (const Mat31 &p, std::shared_ptr< Node > &node, matData_t &W) override

void	add_points_array (const MatX &P, std::shared_ptr< Node > &node, mrob::matData_t &W) override

void	add_points_S_matrix (const Mat4 &S, std::shared_ptr< Node > &node, mrob::matData_t &W) override

Mat31	get_mean_point (factor_id_t id)

Public Member Functions inherited from mrob::EigenFactor
	EigenFactor (robustFactorType factor_type=QUADRATIC, uint_t potNumberNodes=5)

Public Member Functions inherited from mrob::Factor
	Factor (uint_t dim, uint_t allNodesDim, robustFactorType factor_type=QUADRATIC, uint_t potNumberNodes=5)

matData_t	get_chi2 () const

factor_id_t	get_id () const

void	set_id (factor_id_t id)

uint_t	get_dim_obs () const

void	set_dim_obs (uint_t dim)

uint_t	get_all_nodes_dim () const

void	set_all_nodes_dim (uint_t dim)

const std::vector< std::shared_ptr< Node > > *	get_neighbour_nodes (void) const

matData_t	evaluate_robust_weight (matData_t u, matData_t params=0.0)

Protected Member Functions
void	estimate_plane () override

Protected Member Functions inherited from mrob::EigenFactorPlane
void	calculate_all_matrices_S ()

void	calculate_all_matrices_Q ()

Protected Attributes
Mat4	accumulatedCenterQ_

Mat41	planeEstimationUnit_

Mat4	Tcenter_

Protected Attributes inherited from mrob::EigenFactorPlane
std::deque< factor_id_t >	nodeIds_

std::unordered_map< factor_id_t, uint_t >	reverseNodeIds_

std::deque< Mat61, Eigen::aligned_allocator< Mat61 > >	J_

std::deque< Mat6, Eigen::aligned_allocator< Mat6 > >	H_

std::deque< Mat4, Eigen::aligned_allocator< Mat4 > >	S_

std::deque< Mat4, Eigen::aligned_allocator< Mat4 > >	Q_

Mat4	accumulatedQ_

Mat41	planeEstimation_

Mat41	planeEstimationCenter_

std::deque< std::deque< Mat31, Eigen::aligned_allocator< Mat31 > > >	allPlanePoints_

std::deque< std::deque< matData_t > >	allPointsInformation_

uint_t	numberPoints_

matData_t	planeError_

Mat4	Tcenter_

Protected Attributes inherited from mrob::Factor
factor_id_t	id_

std::vector< std::shared_ptr< Node > >	neighbourNodes_

uint_t	dim_

uint_t	allNodesDim_

matData_t	chi2_

robustFactorType	robust_type_

matData_t	robust_weight_

Additional Inherited Members
Public Types inherited from mrob::Factor
enum	robustFactorType { QUADRATIC = 0, HUBER, CAUCHY, MCCLURE, RANSAC }

Detailed Description

This is a copy of EF Plane for comparison Eigen factor Plane is a vertex that complies with the Fgraph standards and inherits from base EigenFactor.hpp

The Plane factor connects different poses that have observed the same geometric entity. It is not required an explicit parametrization of the plane, so the resultant topology is N nodes connecting to the plane factor.

NOTE: due to its nature, multiple observation can be added to the same EF, meaning we need to create a constructor PLUS an additional method

add_point()

In order to build the problem we would follow the interface specifications by FGraph but we need extra methods and variables to keep track of the neighbours

This class assumes that matrices S = sum p*p' are calculated before since they are directly inputs XXX should we store all points?

Constructor & Destructor Documentation

◆ EigenFactorPlaneCenter()

EigenFactorPlaneCenter::EigenFactorPlaneCenter ( Factor::robustFactorType robust_type = Factor::robustFactorType::QUADRATIC )

Creates an Eigen Factor plane. The minimum requirements are 1 pose, which is not required at this stage, but will be introduced when we add points/Q matrix.

Member Function Documentation

◆ estimate_plane()

void EigenFactorPlaneCenter::estimate_plane ( )

overrideprotectedvirtual

Estimates the plane parameters: v = [n', d]'_{4x1}, where v is unit, (due to the Eigen solution) although for standard plane estimation we could enforce unit on the normal vector n.

Reimplemented from mrob::EigenFactorPlane.