|
MROB
|
#include <plane.hpp>
Public Member Functions | |
| Plane (uint_t timeLength) | |
| void | reserve (uint_t d, uint_t t) |
| Mat41 | get_plane (void) |
| void | push_back_point (Mat31 &point, uint_t t) |
| std::vector< Mat31 > & | get_points (uint_t t) |
| uint_t | get_number_points (uint_t t) |
| uint_t | get_total_number_points () |
| void | clear_points () |
| void | set_trajectory (const std::shared_ptr< const std::vector< SE3 >> &trajectory) |
| double | estimate_plane () |
| double | estimate_plane_incrementally (uint_t t) |
| double | get_error () const |
| double | get_error_incremental (uint_t t) const |
| void | calculate_all_matrices_S (bool reset=false) |
| Mat31 | get_mean_point (uint_t t) |
| void | calculate_all_matrices_Q () |
| Mat61 | calculate_gradient (uint_t t) |
| Mat6 | calculate_hessian (uint_t t) |
| void | reset () |
| void | print () const |
Protected Attributes | |
| uint_t | timeLength_ |
| Mat41 | planeEstimation_ |
| double | lambda_ {} |
| bool | isPlaneEstimated_ |
| std::vector< std::vector< Mat31 > > | allPlanePoints_ |
| uint_t | numberPoints_ |
| std::shared_ptr< const std::vector< SE3 > > | trajectory_ |
| std::vector< Mat4, Eigen::aligned_allocator< Mat4 > > | matrixS_ |
| std::vector< Mat4, Eigen::aligned_allocator< Mat4 > > | matrixQ_ |
| Mat4 | accumulatedQ_ |
| std::vector< Mat4, Eigen::aligned_allocator< Mat4 > > | gradQ_ |
| std::vector< Mat4, Eigen::aligned_allocator< Mat4 > > | lieGenerativeMatrices_ |
class Plane stores a vector of point clouds from different observations and provides the gradients with respect to those poses.
It requires the exact number of poses that it is being considering. This is a design limitation, to be solved soon.
| void Plane::calculate_all_matrices_Q | ( | ) |
calculates the matrix Qi = 1^T_i * Si * (1^T_i)^transp for all planes. Since this is an iterative process on T's, we separate the calculation of the S matrix, and the Q matrix which rotates S
| void Plane::calculate_all_matrices_S | ( | bool | reset = false | ) |
calculates the matrix S = sum(p*p'), where p = [x,y,z,1] for all planes, as an aggregation of the outer product of all homogeneous points If reset = true, clears all information and recalculates S If reset = false (default) only calculates S if there is no calculation yet
| Mat61 Plane::calculate_gradient | ( | uint_t | t | ) |
calculate gradient at time t, returns the Jacobian over the EF from SVD of Q = U D V' as d lamda = v' * dQ * v
| Mat6 Plane::calculate_hessian | ( | uint_t | t | ) |
calculates the Hessian of the eigen factor, as described in the paper. It requires to FIRST calculate gradient. Output is an upper traingular view of the symetric hessian matrix
| double Plane::estimate_plane | ( | ) |
Estimates the plane parameters: v = [n', d]'_{4x1}, where v is unitary, (due to the SVD solution) although for standard plane estimation we could enforce unitary on the normal vector n.
| double Plane::estimate_plane_incrementally | ( | uint_t | t | ) |
Estimates Incremetally the plane parameters: v = [n', d]'_{4x1}, where v is unitary, (due to the SVD solution) although for standard plane estimation we could enforce unitary on the normal vector n.
The difference with the previous estimate_plane() is that we update the matrix Q for the give time stamp and recalculate the solution, on constant time O(1)
|
inline |
get error: returns the error as the min eigenvalue
| double Plane::get_error_incremental | ( | uint_t | t | ) | const |
get error incremental: returns the error as the min eigenvalue only updating the value of Q_t, at time step t. Nothing insie gets updated
| Mat31 Plane::get_mean_point | ( | uint_t | t | ) |
get mean point calculates the mean of the pointcloud observed at time t, given that S = sum p * p' = sum ([x2 xy xz x yx y2 yz y zx zy z2 z x y z 1] ser we just calcualte S and return
| void Plane::push_back_point | ( | Mat31 & | point, |
| uint_t | t | ||
| ) |
This function is intended to add a point that belongs to the plane at time t Internally, we will save a copy of them
1.8.13