#include <factorCameraProj3dLine.hpp>
Public Member Functions | |
| FactorCameraProj3dLine (const Mat21 &obsPoint1, const Mat21 &obsPoint2, std::shared_ptr< Node > &nodePose, std::shared_ptr< Node > &nodePoint1, std::shared_ptr< Node > &nodePoint2, const Mat41 &camera_k, const Mat2 &obsInf=Mat2::Identity(), 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 |
 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 | |
| Mat31 | project_point_homog (const Mat31 &point) |
| Mat31 | calculate_image_line (const Mat21 &p1, const Mat21 &p2) |
Protected Attributes | |
| Mat31 | line_obs_ |
| Mat21 | r_ |
| Mat31 | point1_ |
| Mat31 | point2_ |
| Mat31 | local_point1_ |
| Mat31 | local_point2_ |
| Mat41 | camera_k_ |
| SE3 | Tinv_ |
| Mat2 | W_ |
| Mat< 2, 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 FactorCameraProj3dLine is a vertex representing the distribution between a Rigid Body Transformation encoding a 3D pose and a 3D line expressed as 2 3D points, projected to the image plane in 2D (Vakhitov, ECCV 2016)
The observation is a 2D line, in the local frame of the current 3D pose, the sensor reference frame The two Nodes that the factor is connecting, which are provided by their shared_ptr's, are:
- 2 observation (1,2) pixel coordinates in the image with start-end segments of the line
- 1 Pose in 3d (Camera)
- 2 Points in 3D (map coordinates) expressing the line end-start position
- (TODO) Camera intrinsics K We provide the node's Id to get the correspondent Jacobian
z is a 2d line coordinates in the camera plane, in P^2 T is the transformation encoded by the 3D pose, the local frame (sensor) p1, p2 are the 3d point encoding the point positions in the map coordinate frame
Member Function Documentation
◆ calculate_image_line()
|
protected |
calculate_image_line: given two points in the image coordiantes (pixels) outputs the line in the plane that interesects both
◆ 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
This function is calculated by using the chain rule. Here, our convention of left-hand-side retraction of poses makes the derivatives more involved than comparing with taking a rhs convetion. dr / dT = line' * d proj_k / d p' * d p' / d T J_project = d proj_k / d p' = [fx/z 0 -fx/z^2 x] [0 fy/z -fy/z^2 y] Jr = d p' / d T = d ( T-1 Exp(-dx) l ) / dl = T-1 [l^ -I]
and
dr / d p = line' * d proj_k / d p' * d p' / d l d p' / d l = d (T^-1 l ) d l = R'
Implements mrob::Factor.
◆ evaluate_residuals()
|
overridevirtual |
Jacobians are not evaluated, just the residuals
Implements mrob::Factor.
◆ get_jacobian()
|
inlinevirtual |
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()
|
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()
|
inlinevirtual |
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.
◆ project_point_homog()
|
protected |
project_point in 3D to P^2 by the camera parameters in this class
The documentation for this class was generated from the following files:
- src/visual/mrob/factors/factorCameraProj3dLine.hpp
- src/visual/factors/factorCameraProj3dLine.cpp
