#include <factor2Poses2d.hpp>
Public Member Functions | |
| Factor2Poses2d (const Mat31 &observation, std::shared_ptr< Node > &nodeOrigin, std::shared_ptr< Node > &nodeTarget, const Mat3 &obsInf, bool updateNodeTarget=false, Factor::robustFactorType robust_type=Factor::robustFactorType::QUADRATIC) | |
| void | evaluate_residuals () override |
| void | evaluate_jacobians () override |
| void | evaluate_chi2 () override |
| 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 |
| void | print () 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 | |
| Mat31 | obs_ |
| Mat31 | r_ |
| Mat3 | W_ |
| Mat< 3, 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
Factor2Poses2d if a factor relating a 2 2dimensional poses through a direct observation such that: observation = h(nodeOrigin,nodeTarget) = x_target - x_origin or residual = x_origin + observation - x_target
With this arrangement, the linearized factor substracts the residual (r) to the first order term of the nonlinear observation function: || J dx - r ||
This convention will be followed by all factors in this library, otherwise the optimization will not work properly.
It is possible to update the target node if it has not been initialized. By default is set to FALSE, so we must specify it to true in case we want to update (for instance, for displacement odometry factors)
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 Jacobians, this also creates a new linearization point. This function MOST likely needs to evaluate residuals first
Implements mrob::Factor.
Reimplemented in mrob::Factor2Poses2dOdom.
◆ evaluate_residuals()
|
overridevirtual |
Residuals are evaluated with respect to the current solution
Implements mrob::Factor.
Reimplemented in mrob::Factor2Poses2dOdom.
◆ 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()
|
overridevirtual |
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/factor2Poses2d.hpp
- src/FGraph/factors/factor2Poses2d.cpp
