#include <factor.hpp>
Public Types | |
| enum | robustFactorType { QUADRATIC = 0, HUBER, CAUCHY, MCCLURE, RANSAC } |
Public Member Functions | |
| Factor (uint_t dim, uint_t allNodesDim, robustFactorType factor_type=QUADRATIC, uint_t potNumberNodes=5) | |
| virtual void | evaluate_residuals ()=0 |
| virtual void | evaluate_jacobians ()=0 |
| virtual void | evaluate_chi2 ()=0 |
| matData_t | get_chi2 () const |
| virtual void | print () const |
| virtual MatRefConst | get_obs () const =0 |
| virtual VectRefConst | get_residual () const =0 |
| virtual MatRefConst | get_information_matrix () const =0 |
| 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 | |
| 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_ |
Detailed Description
Factor class is a base pure abstract class defining factors, the second type of vertexes on factor graphs (bipartite). Factors keep track of all their neighbour nodes they are connected to.
By convention, the residuals r_i are ALWAYS formulated as follows:
| r(x) = h(x) - z |
otherwise the optimization will not work properly.
Because the number of Nodes they point to is fixed, we only allow to indicate its node neighbours at the object declaration. On the abstract class constructor they are not indicated, but should be on any child class.
Constructor functions will be overloaded to include the pointers of the nodes, The convention is from node 1, to node 2, and etc. such that: myfactor2poses(n1, n2, ...)
Conventions (transparent for users, but good to know):
- The connecting nodes are stores ordered on a vector. The user does not have to provide such ordering, but at creation each factor such comply this.
- Jacobian is a block matrix corresponding to the Jacobian of the first node J1, then second node J2, etc, such that J = [J1, J2, ..., Jn],
Robust Factors TODO All factors also allow for robust factors, they just have to be specified at the constructor. By the default the robust factor is quadratic (no effect) Currently supports robust factors: Influence function robust weight
- Quadratic: p(u) = 1/2u^2 w(u) = 1
- Huber: p(u) = 1/2u^2 if u < d w(u) = 1 d(u-1/2d) 1/u
- Cauchy: p(u) = 1/2 ln(1+u^2) w(u) = 1/(1+u^2)
- McClure: p(u) = 1/2 u^2 / (1+u^2) w(u) = 1/(1+u^2)^2
- Ransac: p(u) = 1/2u^2 if u < d w(u) = 1 d 0
Member Enumeration Documentation
◆ robustFactorType
On the derived class constructor we will specify the (ordered) nodes that the factor is connected to.
Member Function Documentation
◆ evaluate_chi2()
|
pure virtual |
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.
Implemented in mrob::Factor2Poses3d, mrob::Factor2Poses3d2obs, mrob::Factor1Pose1Landmark3d, mrob::FactorCameraProj3dPoint, mrob::EigenFactorPlaneCenter, mrob::Factor1Pose1Landmark2d, mrob::EigenFactorPlane, mrob::EigenFactorPoint, mrob::FactorCameraProj3dLine, mrob::EigenFactorPlaneRaw, mrob::Factor1PosePoint2Plane, mrob::Factor1PosePoint2Point, mrob::EigenFactorPlaneCoordinatesAlign, mrob::Factor2Poses2d, mrob::PiFactorPlane, mrob::Factor1Pose3d, mrob::Factor1Pose1Plane4d, and mrob::Factor1Pose2d.
◆ evaluate_jacobians()
|
pure virtual |
Evaluates Jacobians, this also creates a new linearization point. This function MOST likely needs to evaluate residuals first
Implemented in mrob::Factor2Poses2dOdom, mrob::Factor2Poses3d, mrob::Factor2Poses3d2obs, mrob::Factor1Pose1Landmark3d, mrob::FactorCameraProj3dPoint, mrob::Factor1Pose1Landmark2d, mrob::FactorCameraProj3dLine, mrob::EigenFactorPlaneCenter, mrob::Factor1PosePoint2Plane, mrob::EigenFactorPlane, mrob::EigenFactorPoint, mrob::EigenFactorPlaneRaw, mrob::Factor1PosePoint2Point, mrob::Factor2Poses2d, mrob::EigenFactorPlaneCoordinatesAlign, mrob::PiFactorPlane, mrob::Factor1Pose3d, mrob::Factor1Pose1Plane4d, and mrob::Factor1Pose2d.
◆ evaluate_residuals()
|
pure virtual |
Residuals are evaluated with respect to the current solution
Implemented in mrob::Factor2Poses2dOdom, mrob::Factor2Poses3d, mrob::Factor2Poses3d2obs, mrob::Factor1Pose1Landmark3d, mrob::FactorCameraProj3dPoint, mrob::Factor1Pose1Landmark2d, mrob::FactorCameraProj3dLine, mrob::EigenFactorPlaneCenter, mrob::Factor1PosePoint2Plane, mrob::EigenFactorPlane, mrob::EigenFactorPoint, mrob::EigenFactorPlaneRaw, mrob::Factor1PosePoint2Point, mrob::Factor2Poses2d, mrob::PiFactorPlane, mrob::EigenFactorPlaneCoordinatesAlign, mrob::Factor1Pose3d, mrob::Factor1Pose2d, and mrob::Factor1Pose1Plane4d.
◆ evaluate_robust_weight()
| matData_t Factor::evaluate_robust_weight | ( | matData_t | u, |
| matData_t | params = 0.0 |
||
| ) |
Robust functions, given the current distance u = sqrt(r' W r) It can be calculated in the base class for most of the robust functions given that we provide the following inputs:
- u = sqrt(r'Wr)
- param: may be used to pass some information for some losses
Output:
- dp/du 1/u or influence by the inverse of the distance u. The output is directly multiplied in the formation of the matrix W when building the problem.
◆ get_chi2()
|
inline |
get chi2 returns the value in the variable chi2_. This value will be updated every time there is a calculation of residuals.
◆ get_jacobian()
|
pure virtual |
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
Implemented in mrob::EigenFactorPlane, mrob::EigenFactorPlaneRaw, mrob::Factor1Pose1Landmark3d, mrob::FactorCameraProj3dPoint, mrob::Factor1Pose1Landmark2d, mrob::FactorCameraProj3dLine, mrob::Factor1PosePoint2Plane, mrob::Factor1PosePoint2Point, mrob::PiFactorPlane, mrob::Factor1Pose3d, mrob::Factor1Pose1Plane4d, and mrob::Factor1Pose2d.
◆ get_obs()
|
pure virtual |
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.
Implemented in mrob::Factor2Poses3d, mrob::Factor2Poses3d2obs, mrob::Factor1Pose1Landmark3d, mrob::FactorCameraProj3dPoint, mrob::Factor1Pose1Landmark2d, mrob::EigenFactorPlane, mrob::FactorCameraProj3dLine, mrob::EigenFactorPlaneRaw, mrob::Factor1PosePoint2Plane, mrob::Factor1PosePoint2Point, mrob::PiFactorPlane, mrob::Factor2Poses2d, mrob::Factor1Pose3d, mrob::Factor1Pose1Plane4d, and mrob::Factor1Pose2d.
◆ get_residual()
|
pure virtual |
Residual will always be a block vector
Implemented in mrob::Factor2Poses3d, mrob::Factor2Poses3d2obs, mrob::Factor1Pose1Landmark3d, mrob::FactorCameraProj3dPoint, mrob::EigenFactorPlane, mrob::Factor1Pose1Landmark2d, mrob::EigenFactorPlaneRaw, mrob::FactorCameraProj3dLine, mrob::Factor1PosePoint2Plane, mrob::Factor1PosePoint2Point, mrob::PiFactorPlane, mrob::Factor2Poses2d, mrob::Factor1Pose3d, mrob::Factor1Pose1Plane4d, and mrob::Factor1Pose2d.
◆ print()
|
inlinevirtual |
The print utility could be re-implemented on child classes if there are special needs
Reimplemented in mrob::Factor2Poses3d, mrob::Factor2Poses3d2obs, mrob::Factor1Pose1Landmark3d, mrob::FactorCameraProj3dPoint, mrob::Factor1Pose1Landmark2d, mrob::EigenFactorPlane, mrob::FactorCameraProj3dLine, mrob::EigenFactorPlaneRaw, mrob::Factor1PosePoint2Plane, mrob::Factor1PosePoint2Point, mrob::Factor2Poses2d, mrob::PiFactorPlane, mrob::Factor1Pose3d, mrob::Factor1Pose1Plane4d, and mrob::Factor1Pose2d.
Member Data Documentation
◆ neighbourNodes_
|
protected |
This is a sorted list, so at the constructor we should check of the order based on increasing ids (See examples)
The documentation for this class was generated from the following files:
- src/FGraph/mrob/factor.hpp
- src/FGraph/factor.cpp
