#include <EigenFactorPlane.hpp>

Inheritance diagram for mrob::EigenFactorPlane:

[legend]

Collaboration diagram for mrob::EigenFactorPlane:

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

void	evaluate_residuals () override

void	evaluate_jacobians () override

void	evaluate_chi2 () override

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
virtual void	estimate_plane ()

void	calculate_all_matrices_S ()

void	calculate_all_matrices_Q ()

Protected Attributes
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

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

◆ EigenFactorPlane()

EigenFactorPlane::EigenFactorPlane ( 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

◆ add_point()

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

overridevirtual

Add point: raw points are stored into an unoderred map

Later, the S matrix should be calculated

The alternative is adding directly S, but this offers less flexibility.

Implements mrob::EigenFactor.

◆ add_points_array()

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

overridevirtual

Add point array: uses internally add_point in a block operation

Implements mrob::EigenFactor.

◆ add_points_S_matrix()

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

overridevirtual

Add point S matrix: directly stores S matrices as S = sum p_i'*p_i the homogenoues outer product sum This is done when you dont want to store all points, but process them outside

Implements mrob::EigenFactor.

◆ calculate_all_matrices_Q()

void EigenFactorPlane::calculate_all_matrices_Q ( )

protected

calculates the matrix Qi = 1^T_i * Si * (1^T_i)^transp for all planes. Since this is an iterative process on T's, we separate the calculation of the S matrix, and the Q matrix which rotates S

◆ calculate_all_matrices_S()

void EigenFactorPlane::calculate_all_matrices_S ( )

protected

Calculates the matrix S = sum(p*p'), where p = [x,y,z,1] for all planes, as an aggregation of the outer product of all homogeneous points Deprecated flag: If reset = true, clears all information and recalculates S If reset = false (default) only calculates S if there is no calculation yet

◆ estimate_plane()

void EigenFactorPlane::estimate_plane ( )

protectedvirtual

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 in mrob::EigenFactorPlaneCenter.

◆ evaluate_chi2()

void EigenFactorPlane::evaluate_chi2 ( )

overridevirtual

Chi2 is a scaling of the plane error

Implements mrob::Factor.

Reimplemented in mrob::EigenFactorPlaneCenter, mrob::EigenFactorPoint, and mrob::EigenFactorPlaneCoordinatesAlign.

◆ evaluate_jacobians()

void EigenFactorPlane::evaluate_jacobians ( )

overridevirtual

Evaluates Jacobians, given the residual evaluated It also evaluated the Hessians

Implements mrob::Factor.

Reimplemented in mrob::EigenFactorPlaneCenter, mrob::EigenFactorPoint, and mrob::EigenFactorPlaneCoordinatesAlign.

◆ evaluate_residuals()

void EigenFactorPlane::evaluate_residuals ( )

overridevirtual

Jacobians are not evaluated, just the residuals. This function is calculating the current plane estimation

Implements mrob::Factor.

Reimplemented in mrob::EigenFactorPlaneCenter, mrob::EigenFactorPoint, and mrob::EigenFactorPlaneCoordinatesAlign.

◆ get_hessian()

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

overridevirtual

get hessian returns the Hassian corresponding to the given node id.

Returns

Implements mrob::EigenFactor.

◆ get_jacobian()

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

overridevirtual

get jacobian returns the jacobian corresponding to the given node id.

Returns

Implements mrob::Factor.

◆ get_mean_point()

Mat31 EigenFactorPlane::get_mean_point ( factor_id_t id )

get mean point calculates the mean of the pointcloud observed at pose node id, given that S = sum p * p' = sum ([x2 xy xz x yx y2 yz y zx zy z2 z x y z 1] ser we just calculate S and return

◆ get_obs()

MatRefConst mrob::EigenFactorPlane::get_obs ( ) const

inlinevirtual

Return a Ref to a dynamic matrix, while the child matrix should declare all these variables as fixed size matrices, and ref takes care of doing the conversion with minimal temporary artifacts Observation can be a 3d point, a 3d pose (transformation 4x4), etc.

Implements mrob::Factor.

◆ get_residual()

VectRefConst mrob::EigenFactorPlane::get_residual ( ) const

inlinevirtual

Residual will always be a block vector

Implements mrob::Factor.

◆ get_state()

VectRefConst mrob::EigenFactorPlane::get_state ( void ) const

inlineoverridevirtual

get current state of the Eigen Factor, in this case, a plane is returned as the current planeEstimation

Implements mrob::EigenFactor.

◆ print()

void EigenFactorPlane::print ( void ) const

virtual

The print utility could be re-implemented on child classes if there are special needs

Reimplemented from mrob::Factor.

Member Data Documentation

◆ H_

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

protected

Hessian matrix, dense since it connects all poses from where plane was observed. We store the block diagonal terms, according to the indexes of the nodes

◆ J_

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

protected

The Jacobian of the plane error, the poses involved. Stores the map according to the nodes indexes/identifiers.

◆ nodeIds_

std::deque<factor_id_t> mrob::EigenFactorPlane::nodeIds_

protected

A deque storing the ids in the FGraph of each of the poses This is preferred over vector due to the a priori unknown size of elements. The same applies for other structures containing matrices

◆ reverseNodeIds_

std::unordered_map<factor_id_t, uint_t> mrob::EigenFactorPlane::reverseNodeIds_

protected

Mapping from the FG nodes IDs to the local indexes in the current EF. This conversion is necessary to maintain consistency in case of non-subsequent observations

◆ S_

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

protected

According to our notation S = sum p*p' We choose unordered map here since this is a subset of neighbours (small) and we will iterate over them Iterations may be not in strict order, but we don't care much for now (will we?)

Q = T *S *T'

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

src/plane-surfaces/mrob/factors/EigenFactorPlane.hpp
src/plane-surfaces/factors/EigenFactorPlane.cpp

Public Member Functions

Protected Member Functions

Protected Attributes

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

◆ EigenFactorPlane()

Member Function Documentation

◆ add_point()

◆ add_points_array()

◆ add_points_S_matrix()

◆ calculate_all_matrices_Q()

◆ calculate_all_matrices_S()

◆ estimate_plane()

◆ evaluate_chi2()

◆ evaluate_jacobians()

◆ evaluate_residuals()

◆ get_hessian()

◆ get_jacobian()

◆ get_mean_point()

◆ get_obs()

◆ get_residual()

◆ get_state()

◆ print()

Member Data Documentation

◆ H_

◆ J_

◆ nodeIds_

◆ reverseNodeIds_

◆ S_