Helios++
Helios software for LiDAR simulations
PlaneFitter.h
1 #pragma once
2 
3 #include <vector>
4 #include <armadillo>
5 
6 using std::vector;
7 using arma::Mat;
8 
15 public:
16  // *** STATIC FUNCTIONS *** //
17  // ************************** //
26  template <typename T>
27  static void centerCoordinatesMatrix(Mat<T> & M);
28 
55  template <typename T>
56  static vector<T> bestFittingPlaneOrthoNormal(Mat<T> & M, bool center=true);
57 };
58 
59 // *** IMPLEMENTATION *** //
60 // ************************ //
65 template <typename T>
67  arma::colvec mins = arma::min(M, 1);
68  arma::colvec maxs = arma::max(M, 1);
69  arma::colvec centers = (maxs+mins) / 2.0;
70  M.each_col() -= centers;
71 }
72 
82 template <typename T>
84  Mat<T> & M,
85  bool center
86 ){
87  // Transpose M to originWaypoint
89 
90  // Compute SVD
91  arma::mat U;
92  arma::vec s;
93  arma::mat V;
94  arma::svd(U, s, V, M);
95  return std::vector<T>({U[6], U[7], U[8]});
96 }
Handle plane fitting operations.
Definition: PlaneFitter.h:14
static vector< T > bestFittingPlaneOrthoNormal(Mat< T > &M, bool center=true)
Compute the orthonormal of best fitting plane for given Matrix of coordinates.
static void centerCoordinatesMatrix(Mat< T > &M)
Modify coordinates at matrix M so it is centered at originWaypoint.
Definition: PlaneFitter.h:66