NorMIT-nav  18.04
An IGT application
quadratic_spline_fitter.hpp
Go to the documentation of this file.
1 #ifndef SPLINE_FITTER_HPP
2 #define SPLINE_FITTER_HPP
3 #include <vector>
4 #include <Eigen/SparseLU>
5 
12 template<typename T>
14 {
15 public:
16 
21  {
22  };
23 
28  QuadraticSplineFitter(std::vector<T>& points){
29  setPoints(points);
30  }
31 
36  inline void
37  setPoints(std::vector<T>& points)
38  {
39  m_points = points;
40  }
41 
42 
47  std::vector<T>
49  {
50  // Here:
51  // Build a NxN matrix such that
52  // matrix[row][row] = matrix[row][row+1] = 1/2
53  // rest = 0
54  // build B such that B = [ep_start points ep_stop]
55  // Solve Ax = B
56  // x are the knots
57  int n = m_points.size()+2;
58  Eigen::SparseMatrix<T> A(n,n);
59 
60  Eigen::Matrix<T, Eigen::Dynamic, 1> b(n);
61  std::vector<Eigen::Triplet<T> > triplets;
62  triplets.reserve((n)*2);
63 
64  // Initialize coefficients for the points to reach
65 
66  for(int i = 1; i < n-1; i++)
67  {
68  triplets.push_back(Eigen::Triplet<T>(i,i-1,0.125));
69  triplets.push_back(Eigen::Triplet<T>(i,i,0.75));
70  triplets.push_back(Eigen::Triplet<T>(i,i+1,0.125));
71  b(i) = m_points[i-1];
72  }
73 
74  __insertStartpointCoeffs(triplets,b,0);
75  __insertEndpointCoeffs(triplets,b,n-1);
76  A.setFromTriplets(triplets.begin(), triplets.end());
77 
78  // Now solve the linear system using linear least squares
79  Eigen::Matrix<T, Eigen::Dynamic, 1> x(n);
80  Eigen::SparseLU<Eigen::SparseMatrix<T> > solver;
81 
82  solver.compute(A);
83  x = solver.solve(b);
84 
85  // printVector(x,n);
86  // Copy back to std::vector
87  std::vector<T> ret;
88 
89  ret.reserve(n);
90  for(int i = 0; i < n; i++)
91  {
92  ret.push_back(x(i));
93  }
94  return ret;
95 
96  }
97 
98 
99 
100 private:
101  inline void
102  __insertStartpointCoeffs( std::vector<Eigen::Triplet<T> >& triplets,
103  Eigen::Matrix<T, Eigen::Dynamic, 1>& b,
104  const int i) const
105  {
106  triplets.push_back(Eigen::Triplet<T>(i,i,-1));
107  triplets.push_back(Eigen::Triplet<T>(i,i+1,1));
108  b(i) = 0.0;
109  }
110 
111  inline void
112  __insertEndpointCoeffs( std::vector<Eigen::Triplet<T> >& triplets,
113  Eigen::Matrix<T, Eigen::Dynamic, 1>& b,
114  const int i) const
115  {
116  triplets.push_back(Eigen::Triplet<T>(i,i-1,-1));
117  triplets.push_back(Eigen::Triplet<T>(i,i,1));
118  b(i) = 0.0;
119  }
120 
121  std::vector<T> m_points;
122 
123 };
124 
125 #endif
std::vector< T > compute_control_points() const
QuadraticSplineFitter(std::vector< T > &points)
void setPoints(std::vector< T > &points)