#include <factor2Poses3d.hpp>
Public Member Functions | |
| Factor2Poses3d (const Mat4 &observation, std::shared_ptr< Node > &nodeOrigin, std::shared_ptr< Node > &nodeTarget, const Mat6 &obsInf, bool updateNodeTarget=false, Factor::robustFactorType robust_type=Factor::robustFactorType::QUADRATIC) | |
| Factor2Poses3d (const SE3 &observation, std::shared_ptr< Node > &nodeOrigin, std::shared_ptr< Node > &nodeTarget, const Mat6 &obsInf, bool updateNodeTarget=false, Factor::robustFactorType robust_type=Factor::robustFactorType::QUADRATIC) | |
| 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]]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 |
| virtual MatRefConst | get_jacobian ([[maybe_unused]] mrob::factor_id_t id=0) const =0 |
| 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 | |
| SE3 | Tobs_ |
| Mat61 | r_ |
| SE3 | Tr_ |
| Mat6 | W_ |
| Mat< 6, 12 > | 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
The Factor2Poses3d is a vertex representing the distribution between two nodePose3d, that is, it is expressing a Rigid Body Transformation between two poses.
The state is an observer RBT, and as we have commented, we need to specify the two Nodes that the factor is connecting, which are provided by their shared_ptr's. We provide the node's Id to get the correspondent Jacobian
The convention in the library r = f(x) - z.
In this particular factor, we will follow a similar convention as in odometry 2d, where we 'observe' the last pose, and thus, the relation between the transformation of poses is: T_o * T_obs = T_t
T_o is the transformation encoded by the 3D pose 'origin'. Also note that the transformation from a pose (Exp(x_o) transforms point in the local 'origin' frame to the world reference. T_t target transformation from pose x_t T_obs observation transformation from pose obs (observed from origin)
and the residual is thus: r = Ln ( T_o * T_obs * T_t^-1 )
(equivalent to odometry 2d x_origin + observation - x_target)
(it could also be formulated as T_o^-1*T_t*Tob^-1, but the former way is more intuitive
Constructor functions will be overloaded to include the pointers of the nodes, The convention is from node origin, we observe node destination, such that: Factor2Poses3d(nodeOrigin, nodeTarget, ...
The observations relate a pair of nodes. The order matters, since this will affect the order on the Jacobian block matrix
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
Implements mrob::Factor.
◆ evaluate_residuals()
|
overridevirtual |
Jacobians are not evaluated, just the residuals
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/FGraph/mrob/factors/factor2Poses3d.hpp
- src/FGraph/factors/factor2Poses3d.cpp
