#include <factor1PosePoint2Plane.hpp>
Public Member Functions | |
| Factor1PosePoint2Plane (const Mat31 &z_point_x, const Mat31 &z_point_y, const Mat31 &z_normal_y, std::shared_ptr< Node > &node, const Mat1 &obsInf, Factor::robustFactorType robust_type=Factor::robustFactorType::QUADRATIC) | |
| virtual void | evaluate_residuals () override |
| virtual void | evaluate_jacobians () override |
| virtual void | evaluate_chi2 () override |
| virtual void | print () const |
| MatRefConst | get_obs () const override |
| VectRefConst | get_residual () const override |
| MatRefConst | get_information_matrix () const override |
| MatRefConst | get_jacobian ([[maybe_unused]] mrob::factor_id_t id=0) const override |
 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 Attributes | |
| Mat31 | z_point_x_ |
| Mat31 | z_point_y_ |
| Mat31 | Tx_ |
| Mat31 | z_normal_y_ |
| Mat1 | r_ |
| Mat1 | W_ |
| Mat< 1, 6 > | J_ |
| 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
Factor1PoseToPlaneAndPoint is a vertex in the Fgraph library that models the distribution of observations of a 3D pose. The purpose of this factors is to estimate the relative transformation from two different sets of point clouds, given some planar constraaints. Mainly, it will include a pointcloud from images (chessboard) after solving the PnP (external) and any sensor providing depth.
Observations
- Point x
- Point y, normal n_y, from reference y
State to estimate is 3D pose y^T_x.
The residual, as a convention in the library is: r = f(x) = n_y' * (Tx - y) -> 0 if the relative point is in the plane.
The Jacobian is then dr = n_y' [-(Tp)^, I]
TODO: correctly characterize the covariance, since this is a contribution from 2 rv
Member Function Documentation
◆ evaluate_chi2()
|
overridevirtual |
Evaluates chi2 of the current problem, with the given residuals. It may be required to evaluate_residuals() to obtain the new chi2 values This function MOST likely needs to evaluate residuals first, but evaluate_residuals does not necessarily requires to calculate chi2, that is why there are 2 functions.
Implements mrob::Factor.
◆ evaluate_jacobians()
|
overridevirtual |
Evaluates residuals and Jacobians J = dr/dxi = pi'*[-Tp^. I]
Implements mrob::Factor.
◆ evaluate_residuals()
|
overridevirtual |
Jacobians are not evaluated, just the residuals r = <pi, T p>
The transformation T is, thus, the transformation from the point reference to the plane reference
Implements mrob::Factor.
◆ get_jacobian()
|
inlineoverridevirtual |
get_jacobian returns a block matrices stacking all the Jacobians on the factor. The convention is that Jacobians corresponding to.
The input value is in case that Jacobian supports accessing a particular Jacobian of a node. For most factors (include 1-2 nodes) this option is not available Mostly only for EigenFactors whose number of connected nodes is unbounded
Implements mrob::Factor.
◆ get_obs()
|
inlineoverridevirtual |
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()
|
inlineoverridevirtual |
Residual will always be a block vector
Implements mrob::Factor.
◆ print()
|
virtual |
The print utility could be re-implemented on child classes if there are special needs
Reimplemented from mrob::Factor.
The documentation for this class was generated from the following files:
- src/PCRegistration/mrob/factors/factor1PosePoint2Plane.hpp
- src/PCRegistration/factors/factor1PosePoint2Plane.cpp
