Curves
A library for curves generation.
LinearInterpolationVectorSpaceCurve-inl.hpp
Go to the documentation of this file.
1 /*
2 
3  * @file LinearInterpolationVectorSpaceCurve.hpp
4  * @date Aug 17, 2014
5  * @author Paul Furgale, Abel Gawel, Renaud Dube
6 
7 
8 #include <curves/LinearInterpolationVectorSpaceCurve.hpp>
9 #include <iostream>
10 #include <boost/bind.hpp>
11 #include <boost/function.hpp>
12 
13 namespace curves {
14 
15 template<int N>
16 LinearInterpolationVectorSpaceCurve<N>::LinearInterpolationVectorSpaceCurve() : VectorSpaceCurve<N>() {}
17 
18 template<int N>
19 LinearInterpolationVectorSpaceCurve<N>::~LinearInterpolationVectorSpaceCurve() {}
20 
21 template<int N>
22 void LinearInterpolationVectorSpaceCurve<N>::print(const std::string& str) const {
23  std::cout << "=========================================" << std::endl;
24  std::cout << "=======LINEAR INTERPOLATION CURVE========" << std::endl;
25  std::cout << str << std::endl;
26  std::cout << "number of coefficients: " << manager_.size() << std::endl;
27  std::cout << "dimension: " << N << std::endl;
28  std::stringstream ss;
29  std::vector<Key> keys;
30  std::vector<Time> times;
31  manager_.getTimes(&times);
32  manager_.getKeys(&keys);
33  std::cout << "curve defined between times: " << manager_.getMinTime() <<
34  " and " << manager_.getMaxTime() <<std::endl;
35  std::cout <<"=========================================" <<std::endl;
36  for (size_t i = 0; i < manager_.size(); i++) {
37  ss << "coefficient " << keys[i] << ": ";
38  std::cout << ss.str() << manager_.getCoefficientByKey(keys[i]).transpose() << std::endl;
39  std::cout << " | time: " << times[i];
40  std::cout << std::endl;
41  ss.str("");
42  }
43  std::cout <<"=========================================" <<std::endl;
44 }
45 
46 template<int N>
47 Time LinearInterpolationVectorSpaceCurve<N>::getMaxTime() const {
48  return manager_.getMaxTime();
49 }
50 
51 template<int N>
52 Time LinearInterpolationVectorSpaceCurve<N>::getMinTime() const {
53  return manager_.getMinTime();
54 }
55 
56 template<int N>
57 void LinearInterpolationVectorSpaceCurve<N>::fitCurve(const std::vector<Time>& times,
58  const std::vector<ValueType>& values,
59  std::vector<Key>* outKeys) {
60  CHECK_EQ(times.size(), values.size());
61 
62  if(times.size() > 0) {
63  manager_.clear();
64  if (outKeys != NULL) {
65  outKeys->clear();
66  outKeys->reserve(times.size());
67  }
68  std::vector<Coefficient> coefficients;
69  coefficients.reserve(times.size());
70  size_t vsize = values[0].size();
71  for(size_t i = 0; i < values.size(); ++i) {
72  CHECK_EQ(vsize, values[i].size()) << "The vectors must be uniform length.";
73  coefficients.push_back(Coefficient(values[i]));
74  }
75  manager_.insertCoefficients(times,coefficients,outKeys);
76  }
77 }
78 
79 template<int N>
80 void LinearInterpolationVectorSpaceCurve<N>::extend(const std::vector<Time>& times,
81  const std::vector<ValueType>& values,
82  std::vector<Key>* outKeys) {
83 
84  CHECK_EQ(times.size(), values.size()) << "number of times and number of coefficients don't match";
85  std::vector<Coefficient> coefficients(values.size());
86  for (size_t i = 0; i < values.size(); ++i) {
87  coefficients[i] = Coefficient(values[i]);
88  }
89  manager_.insertCoefficients(times, coefficients, outKeys);
90 }
91 
92 template<int N>
93 typename LinearInterpolationVectorSpaceCurve<N>::ValueType
94 LinearInterpolationVectorSpaceCurve<N>::evaluate(Time time) const {
95  CoefficientIter rval0, rval1;
96  bool success = manager_.getCoefficientsAt(time, &rval0, &rval1);
97  CHECK(success) << "Unable to get the coefficients at time " << time;
98 
99  Time dt = rval1->first - rval0->first;
100  Time t = rval1->first - time;
101  // Alpha goes from zero to one.
102  double alpha = double(t)/double(dt);
103 
104  return alpha * rval0->second.coefficient + (1.0 - alpha) * rval1->second.coefficient;
105 }
106 
107 template<int N>
108 typename LinearInterpolationVectorSpaceCurve<N>::DerivativeType
109 LinearInterpolationVectorSpaceCurve<N>::evaluateDerivative(Time time,
110  unsigned derivativeOrder) const {
111 
112  // time is out of bound --> error
113  CHECK_GE(time, this->getMinTime()) << "Time out of bounds";
114  CHECK_LT(time, this->getMaxTime()) << "Time out of bounds";
115  CHECK_GT(derivativeOrder, 0) << "DerivativeOrder must be greater than 0";
116 
117  typename LinearInterpolationVectorSpaceCurve<N>::DerivativeType dCoeff;
118  Time dt;
119  CoefficientIter rval0, rval1;
120  bool success = manager_.getCoefficientsAt(time, &rval0, &rval1);
121  CHECK(success) << "Unable to get the coefficients at time " << time;
122  // first derivative
123  if (derivativeOrder == 1) {
124  dCoeff = rval1->second.coefficient - rval0->second.coefficient;
125  dt = rval1->first - rval0->first;
126  return dCoeff/dt;
127  } else { // order of derivative > 1 returns vector of zeros
128  dCoeff.Zero();
129  return dCoeff;
130  }
131 }
132 
133 // Evaluation function in functional form. To be passed to the expression
134 template<int N>
135 Eigen::Matrix<double,N,1> linearInterpolation(Eigen::Matrix<double,N,1> v1,
136  Eigen::Matrix<double,N,1> v2, double alpha,
137  gtsam::OptionalJacobian<N,N> H1,
138  gtsam::OptionalJacobian<N,N> H2) {
139  if (H1) { *H1 = Eigen::Matrix<double,N,N>::Identity()*(1-alpha); }
140  if (H2) { *H2 = Eigen::Matrix<double,N,N>::Identity()*alpha; }
141 
142  return v1*(1-alpha) + v2*alpha;
143 }
144 
145 template<int N>
146 gtsam::Expression<typename LinearInterpolationVectorSpaceCurve<N>::ValueType>
147 LinearInterpolationVectorSpaceCurve<N>::getValueExpression(const Time& time) const {
148  typedef typename LinearInterpolationVectorSpaceCurve<N>::ValueType ValueType;
149  using namespace gtsam;
150  CoefficientIter rval0, rval1;
151  bool success = manager_.getCoefficientsAt(time, &rval0, &rval1);
152  CHECK(success) << "Unable to get the coefficients at time " << time;
153 
154  Expression<ValueType> leaf1(rval0->second.key);
155  Expression<ValueType> leaf2(rval1->second.key);
156 
157  double alpha = double(time - rval0->first)/double(rval1->first - rval0->first);
158 
159  Expression<ValueType> rval(boost::bind(&linearInterpolation<N>, _1, _2, alpha, _3, _4),
160  leaf1, leaf2);
161 
162  return rval;
163 }
164 
165 template<int N>
166 gtsam::Expression<typename LinearInterpolationVectorSpaceCurve<N>::DerivativeType>
167 LinearInterpolationVectorSpaceCurve<N>::getDerivativeExpression(const Time& time, unsigned derivativeOrder) const {
168  // \todo Abel and Renaud
169  CHECK(false) << "Not implemented";
170 }
171 
172 template<int N>
173 void LinearInterpolationVectorSpaceCurve<N>::initializeGTSAMValues(gtsam::KeySet keys, gtsam::Values* values) const {
174  manager_.initializeGTSAMValues(keys, values);
175 }
176 
177 template<int N>
178 void LinearInterpolationVectorSpaceCurve<N>::initializeGTSAMValues(gtsam::Values* values) const {
179  manager_.initializeGTSAMValues(values);
180 }
181 
182 template<int N>
183 void LinearInterpolationVectorSpaceCurve<N>::updateFromGTSAMValues(const gtsam::Values& values) {
184  manager_.updateFromGTSAMValues(values);
185 }
186 
187 template<int N>
188 void LinearInterpolationVectorSpaceCurve<N>::clear() {
189  manager_.clear();
190 }
191 
192 template<int N>
193 void LinearInterpolationVectorSpaceCurve<N>::addPriorFactors(gtsam::NonlinearFactorGraph* graph, Time priorTime) const {
194  gtsam::noiseModel::Constrained::shared_ptr priorNoise = gtsam::noiseModel::Constrained::All(gtsam::traits<Coefficient>::dimension);
195 
196  // Constraint the coefficients which influence the curve value at priorTime
197  CoefficientIter rVal0, rVal1;
198  manager_.getCoefficientsAt(priorTime, &rVal0, &rVal1);
199 
200  gtsam::ExpressionFactor<Coefficient> factor0(priorNoise,
201  rVal0->second.coefficient,
202  gtsam::Expression<Coefficient>(rVal0->second.key));
203  gtsam::ExpressionFactor<Coefficient> factor1(priorNoise,
204  rVal1->second.coefficient,
205  gtsam::Expression<Coefficient>(rVal1->second.key));
206  graph->push_back(factor0);
207  graph->push_back(factor1);
208 }
209 
210 template<int N>
211 void LinearInterpolationVectorSpaceCurve<N>::transformCurve(const ValueType T) {
212  //todo
213 }
214 
215 template<int N>
216 Time LinearInterpolationVectorSpaceCurve<N>::getTimeAtKey(gtsam::Key key) const {
217  return manager_.getCoefficientTimeByKey(key);
218 }
219 
220 } // namespace curves
221 */