CubicInterpolation.cpp
Go to the documentation of this file.
1 /*
2  * CubicInterpolation.cpp
3  *
4  * Created on: Jan 21, 2020
5  * Author: Edo Jelavic
6  * Institute: ETH Zurich, Robotic Systems Lab
7  */
8 
10 
13 
14 namespace grid_map {
15 
16 unsigned int bindIndexToRange(int idReq, unsigned int nElem)
17 {
18  if (idReq < 0) {
19  return 0;
20  }
21  if (idReq >= nElem) {
22  return static_cast<unsigned int>(nElem - 1);
23  }
24  return static_cast<unsigned int>(idReq);
25 }
26 
27 double getLayerValue(const Matrix &layerMat, int rowReq, int colReq)
28 {
29  const auto numCol = layerMat.cols();
30  const auto numRow = layerMat.rows();
31  const unsigned int iBoundToRange = bindIndexToRange(rowReq, numRow);
32  const unsigned int jBoundToRange = bindIndexToRange(colReq, numCol);
33  return layerMat(iBoundToRange, jBoundToRange);
34 }
35 
36 
46 namespace bicubic_conv {
47 
48 bool evaluateBicubicConvolutionInterpolation(const GridMap &gridMap, const std::string &layer,
49  const Position &queriedPosition,
50  double *interpolatedValue)
51 {
52  FunctionValueMatrix functionValues;
53  if (!assembleFunctionValueMatrix(gridMap, layer, queriedPosition, &functionValues)) {
54  return false;
55  }
56 
57  Position normalizedCoordinate;
58  if (!getNormalizedCoordinates(gridMap, queriedPosition, &normalizedCoordinate)) {
59  return false;
60  }
61 
62  const double tx = normalizedCoordinate.x();
63  const double ty = normalizedCoordinate.y();
64 
65  //bm1 stands for b minus one, i.e. index decreased by one
66  //b2 stands for b plus 2, i.e. index increased by two
67  const double bm1 = convolve1D(tx, functionValues.row(0));
68  const double b0 = convolve1D(tx, functionValues.row(1));
69  const double b1 = convolve1D(tx, functionValues.row(2));
70  const double b2 = convolve1D(tx, functionValues.row(3));
71  const Eigen::Vector4d vectorBs(bm1, b0, b1, b2);
72  *interpolatedValue = convolve1D(ty, vectorBs);
73  return true;
74 }
75 
76 double convolve1D(double t, const Eigen::Vector4d &functionValues)
77 {
78  const Eigen::Vector4d tVector(1.0, t, t * t, t * t * t);
79  const Eigen::Vector4d temp = cubicInterpolationConvolutionMatrix
80  * functionValues;
81  const double retVal = 0.5 * tVector.transpose() * temp;
82  return retVal;
83 }
84 
85 bool assembleFunctionValueMatrix(const GridMap &gridMap, const std::string &layer,
86  const Position &queriedPosition, FunctionValueMatrix *data)
87 {
88 
89  Index middleKnotIndex;
90  if (!getIndicesOfMiddleKnot(gridMap, queriedPosition, &middleKnotIndex)) {
91  return false;
92  }
93 
94  const Matrix &layerMatrix = gridMap.get(layer);
95  auto f = [&layerMatrix](int rowReq, int colReq) {
96  double retVal = getLayerValue(layerMatrix, rowReq, colReq);
97  return retVal;
98  };
99 
100  const unsigned int i = middleKnotIndex.x();
101  const unsigned int j = middleKnotIndex.y();
102  /*
103  * Notation taken from: https://en.wikipedia.org/wiki/Bicubic_interpolation
104  * increasing f's indices is flipped w.r.t. to the above since in the article
105  * they use a coordinate frame centered around (i,j). Therefore:
106  * f(i+1,j-1) in their notation corresponds to f(i-1,j+1) in ours. This is
107  * because our coordinate frame sits in the top left corner, see
108  * https://github.com/ANYbotics/grid_map
109  */
110  *data << f(i + 1, j + 1), f(i, j + 1), f(i - 1, j + 1), f(i - 2, j + 1), f(i + 1, j), f(i, j), f(
111  i - 1, j), f(i - 2, j), f(i + 1, j - 1), f(i, j - 1), f(i - 1, j - 1), f(i - 2, j - 1), f(
112  i + 1, j - 2), f(i, j - 2), f(i - 1, j - 2), f(i - 2, j - 2);
113 
114  return true;
115 }
116 
117 bool getNormalizedCoordinates(const GridMap &gridMap, const Position &queriedPosition,
118  Position *position)
119 {
120  Index index;
121  if (!getIndicesOfMiddleKnot(gridMap, queriedPosition, &index)) {
122  return false;
123  }
124 
125  Position middleKnot;
126  if (!gridMap.getPosition(index, middleKnot)) {
127  return false;
128  }
129 
130  position->x() = (queriedPosition.x() - middleKnot.x()) / gridMap.getResolution();
131  position->y() = (queriedPosition.y() - middleKnot.y()) / gridMap.getResolution();
132 
133  return true;
134 }
135 
136 bool getIndicesOfMiddleKnot(const GridMap &gridMap, const Position &queriedPosition, Index *index)
137 {
138 
139  if (!gridMap.getIndex(queriedPosition, *index)) {
140  return false;
141  }
142  return true;
143 }
144 
145 } /* namespace bicubic_conv */
146 
156 namespace bicubic {
157 
158 bool evaluateBicubicInterpolation(const GridMap &gridMap, const std::string &layer,
159  const Position &queriedPosition, double *interpolatedValue)
160 {
161 
162  const Matrix& layerMat = gridMap.get(layer);
163  const double resolution = gridMap.getResolution();
164 
165  // get indices of data points needed for interpolation
166  IndicesMatrix unitSquareCornerIndices;
167  if (!getUnitSquareCornerIndices(gridMap, queriedPosition, &unitSquareCornerIndices)) {
168  return false;
169  }
170 
171  // get function values
172  DataMatrix f;
173  if (!getFunctionValues(layerMat, unitSquareCornerIndices, &f)) {
174  return false;
175  }
176 
177  // get the first derivatives in x
178  DataMatrix dfx;
179  if (!getFirstOrderDerivatives(layerMat, unitSquareCornerIndices, Dim2D::X, resolution, &dfx)) {
180  return false;
181  }
182 
183  // the first derivatives in y
184  DataMatrix dfy;
185  if (!getFirstOrderDerivatives(layerMat, unitSquareCornerIndices, Dim2D::Y, resolution, &dfy)) {
186  return false;
187  }
188  // mixed derivatives in x y
189  DataMatrix ddfxy;
190  if (!getMixedSecondOrderDerivatives(layerMat, unitSquareCornerIndices, resolution, &ddfxy)) {
191  return false;
192  }
193 
194  // assemble function value matrix matrix
195  FunctionValueMatrix functionValues;
196  assembleFunctionValueMatrix(f, dfx, dfy, ddfxy, &functionValues);
197 
198  // get normalized coordiantes
199  Position normalizedCoordinates;
200  if (!computeNormalizedCoordinates(gridMap, unitSquareCornerIndices.bottomLeft_, queriedPosition,
201  &normalizedCoordinates)) {
202  return false;
203  }
204 
205  // evaluate polynomial
206  *interpolatedValue = evaluatePolynomial(functionValues, normalizedCoordinates.x(),
207  normalizedCoordinates.y());
208 
209  return true;
210 }
211 
212 bool getUnitSquareCornerIndices(const GridMap &gridMap, const Position &queriedPosition,
213  IndicesMatrix *indicesMatrix)
214 {
215 
216  Index closestPointId;
217  if (!getClosestPointIndices(gridMap, queriedPosition, &closestPointId)) {
218  return false;
219  }
220 
221  Position closestPoint;
222  if (!gridMap.getPosition(closestPointId, closestPoint)) {
223  return false;
224  }
225 
226  const int idx0 = closestPointId.x();
227  const int idy0 = closestPointId.y();
228  const double x0 = closestPoint.x();
229  const double y0 = closestPoint.y();
230  const double x = queriedPosition.x();
231  const double y = queriedPosition.y();
232 
233  if (x > x0) { //first or fourth quadrant
234  if (y > y0) { //first quadrant
235  indicesMatrix->topLeft_ = Index(idx0, idy0 - 1);
236  indicesMatrix->topRight_ = Index(idx0 - 1, idy0 - 1);
237  indicesMatrix->bottomLeft_ = Index(idx0, idy0);
238  indicesMatrix->bottomRight_ = Index(idx0 - 1, idy0);
239  } else { // fourth quadrant
240  indicesMatrix->topLeft_ = Index(idx0, idy0);
241  indicesMatrix->topRight_ = Index(idx0 - 1, idy0);
242  indicesMatrix->bottomLeft_ = Index(idx0, idy0 + 1);
243  indicesMatrix->bottomRight_ = Index(idx0 - 1, idy0 + 1);
244  }
245  } else { // second or third quadrant
246  if (y > y0) { //second quadrant
247  indicesMatrix->topLeft_ = Index(idx0 + 1, idy0 - 1);
248  indicesMatrix->topRight_ = Index(idx0, idy0 - 1);
249  indicesMatrix->bottomLeft_ = Index(idx0 + 1, idy0);
250  indicesMatrix->bottomRight_ = Index(idx0, idy0);
251  } else { // third quadrant
252  indicesMatrix->topLeft_ = Index(idx0 + 1, idy0);
253  indicesMatrix->topRight_ = Index(idx0, idy0);
254  indicesMatrix->bottomLeft_ = Index(idx0 + 1, idy0 + 1);
255  indicesMatrix->bottomRight_ = Index(idx0, idy0 + 1);
256  }
257  }
258 
259  bindIndicesToRange(gridMap, indicesMatrix);
260 
261  return true;
262 
263 }
264 
265 bool getClosestPointIndices(const GridMap &gridMap, const Position &queriedPosition, Index *index)
266 {
267 
268  if (!gridMap.getIndex(queriedPosition, *index)) {
269  return false;
270  }
271  return true;
272 }
273 
274 bool computeNormalizedCoordinates(const GridMap &gridMap, const Index &originIndex,
275  const Position &queriedPosition, Position *normalizedCoordinates)
276 {
277 
278  Position origin;
279  if (!gridMap.getPosition(originIndex, origin)) {
280  return false;
281  }
282 
283  normalizedCoordinates->x() = (queriedPosition.x() - origin.x()) / gridMap.getResolution();
284  normalizedCoordinates->y() = (queriedPosition.y() - origin.y()) / gridMap.getResolution();
285 
286  return true;
287 
288 }
289 
290 bool getFunctionValues(const Matrix &layerData, const IndicesMatrix &indices, DataMatrix *data)
291 {
292  data->topLeft_ = layerData(indices.topLeft_.x(), indices.topLeft_.y());
293  data->topRight_ = layerData(indices.topRight_.x(), indices.topRight_.y());
294  data->bottomLeft_ = layerData(indices.bottomLeft_.x(), indices.bottomLeft_.y());
295  data->bottomRight_ = layerData(indices.bottomRight_.x(), indices.bottomRight_.y());
296  return true;
297 }
298 
299 void bindIndicesToRange(const GridMap &gridMap, IndicesMatrix *indices)
300 {
301  const int numCol = gridMap.getSize().y();
302  const int numRow = gridMap.getSize().x();
303 
304  //top left
305  {
306  const unsigned int iBoundToRange = bindIndexToRange(indices->topLeft_.x(), numRow);
307  const unsigned int jBoundToRange = bindIndexToRange(indices->topLeft_.y(), numCol);
308  indices->topLeft_ = Index(iBoundToRange, jBoundToRange);
309  }
310 
311  //top right
312  {
313  const unsigned int iBoundToRange = bindIndexToRange(indices->topRight_.x(), numRow);
314  const unsigned int jBoundToRange = bindIndexToRange(indices->topRight_.y(), numCol);
315  indices->topRight_ = Index(iBoundToRange, jBoundToRange);
316  }
317 
318  //bottom left
319  {
320  const unsigned int iBoundToRange = bindIndexToRange(indices->bottomLeft_.x(), numRow);
321  const unsigned int jBoundToRange = bindIndexToRange(indices->bottomLeft_.y(), numCol);
322  indices->bottomLeft_ = Index(iBoundToRange, jBoundToRange);
323  }
324 
325  //bottom right
326  {
327  const unsigned int iBoundToRange = bindIndexToRange(indices->bottomRight_.x(), numRow);
328  const unsigned int jBoundToRange = bindIndexToRange(indices->bottomRight_.y(), numCol);
329  indices->bottomRight_ = Index(iBoundToRange, jBoundToRange);
330  }
331 
332 }
333 
334 bool getFirstOrderDerivatives(const Matrix &layerData, const IndicesMatrix &indices, Dim2D dim,
335  double resolution, DataMatrix *derivatives)
336 {
337  derivatives->topLeft_ = firstOrderDerivativeAt(layerData, indices.topLeft_, dim, resolution);
338  derivatives->topRight_ = firstOrderDerivativeAt(layerData, indices.topRight_, dim, resolution);
339  derivatives->bottomLeft_ = firstOrderDerivativeAt(layerData, indices.bottomLeft_, dim,
340  resolution);
341  derivatives->bottomRight_ = firstOrderDerivativeAt(layerData, indices.bottomRight_, dim,
342  resolution);
343  return true;
344 }
345 
346 double firstOrderDerivativeAt(const Matrix &layerData, const Index &index, Dim2D dim,
347  double resolution)
348 {
349  const int numCol = layerData.cols();
350  const int numRow = layerData.rows();
351 
352  double left, right;
353  switch (dim) {
354  case Dim2D::X: {
355  left = layerData(bindIndexToRange(index.x() + 1, numRow), index.y());
356  right = layerData(bindIndexToRange(index.x() - 1, numRow), index.y());
357  break;
358  }
359  case Dim2D::Y: {
360  left = layerData(index.x(), bindIndexToRange(index.y() + 1, numCol));
361  right = layerData(index.x(), bindIndexToRange(index.y() - 1, numCol));
362  break;
363  }
364  default: {
365  throw std::runtime_error("Unknown derivative direction");
366  }
367  }
368 
369  const double perturbation = resolution;
370  // central difference approximation
371  // we need to multiply with resolution since we are
372  // operating in scaled coordinates
373  return (right - left) / (2.0 * perturbation) * resolution;
374 }
375 
376 double mixedSecondOrderDerivativeAt(const Matrix &layerData, const Index &index, double resolution)
377 {
378  /*
379  * no need for dimensions since the we have to differentiate w.r.t. x and y
380  * the order doesn't matter. Derivative values are the same.
381  * Taken from https://www.mathematik.uni-dortmund.de/~kuzmin/cfdintro/lecture4.pdf
382  */
383 
384  const int numCol = layerData.cols();
385  const int numRow = layerData.rows();
386 
387  const double f11 = layerData(bindIndexToRange(index.x() - 1, numRow),
388  bindIndexToRange(index.y() - 1, numCol));
389  const double f1m1 = layerData(bindIndexToRange(index.x() - 1, numRow),
390  bindIndexToRange(index.y() + 1, numCol));
391  const double fm11 = layerData(bindIndexToRange(index.x() + 1, numRow),
392  bindIndexToRange(index.y() - 1, numCol));
393  const double fm1m1 = layerData(bindIndexToRange(index.x() + 1, numRow),
394  bindIndexToRange(index.y() + 1, numCol));
395 
396  const double perturbation = resolution;
397  // central difference approximation
398  // we need to multiply with resolution^2 since we are
399  // operating in scaled coordinates. Second derivative scales
400  // with the square of the resolution
401  return (f11 - f1m1 - fm11 + fm1m1) / (4.0 * perturbation * perturbation) * resolution * resolution;
402 
403 }
404 
405 bool getMixedSecondOrderDerivatives(const Matrix &layerData, const IndicesMatrix &indices,
406  double resolution, DataMatrix *derivatives)
407 {
408  derivatives->topLeft_ = mixedSecondOrderDerivativeAt(layerData, indices.topLeft_, resolution);
409  derivatives->topRight_ = mixedSecondOrderDerivativeAt(layerData, indices.topRight_, resolution);
410  derivatives->bottomLeft_ = mixedSecondOrderDerivativeAt(layerData, indices.bottomLeft_,
411  resolution);
412  derivatives->bottomRight_ = mixedSecondOrderDerivativeAt(layerData, indices.bottomRight_,
413  resolution);
414  return true;
415 }
416 
417 double evaluatePolynomial(const FunctionValueMatrix &functionValues, double tx, double ty)
418 {
419  const Eigen::Vector4d xVector(1, tx, tx * tx, tx * tx * tx);
420  const Eigen::Vector4d yVector(1, ty, ty * ty, ty * ty * ty);
421  const Eigen::Matrix4d tempMat = functionValues
422  * bicubicInterpolationMatrix.transpose();
423  const Eigen::Matrix4d polynomialCoeffMatrix = bicubicInterpolationMatrix * tempMat;
424  const Eigen::Vector4d tempVec = polynomialCoeffMatrix * yVector;
425  return xVector.transpose() * tempVec;
426 }
427 
428 void assembleFunctionValueMatrix(const DataMatrix &f, const DataMatrix &dfx, const DataMatrix &dfy,
429  const DataMatrix &ddfxy, FunctionValueMatrix *functionValues)
430 {
431  auto toEigenMatrix = [](const DataMatrix &d)-> Eigen::Matrix2d {
432  Eigen::Matrix2d e;
433  e(0,0) = d.bottomLeft_;
434  e(1,0) = d.bottomRight_;
435  e(0,1) = d.topLeft_;
436  e(1,1) = d.topRight_;
437  return e;
438  };
439 
440  functionValues->block<2, 2>(0, 0) = toEigenMatrix(f);
441  functionValues->block<2, 2>(2, 2) = toEigenMatrix(ddfxy);
442  functionValues->block<2, 2>(0, 2) = toEigenMatrix(dfy);
443  functionValues->block<2, 2>(2, 0) = toEigenMatrix(dfx);
444 }
445 
446 } /* namespace bicubic*/
447 
448 } /* namespace grid_map */
bool getFirstOrderDerivatives(const Matrix &layerData, const IndicesMatrix &indices, Dim2D dim, double resolution, DataMatrix *derivatives)
Eigen::Array2i Index
Definition: TypeDefs.hpp:22
Eigen::Matrix4d FunctionValueMatrix
void bindIndicesToRange(const GridMap &gridMap, IndicesMatrix *indices)
bool getPosition(const Index &index, Position &position) const
Definition: GridMap.cpp:233
bool getIndicesOfMiddleKnot(const GridMap &gridMap, const Position &queriedPosition, Index *index)
bool computeNormalizedCoordinates(const GridMap &gridMap, const Index &originIndex, const Position &queriedPosition, Position *normalizedCoordinates)
Eigen::MatrixXf Matrix
Definition: TypeDefs.hpp:16
static const Eigen::Matrix4d bicubicInterpolationMatrix
double convolve1D(double t, const Eigen::Vector4d &functionValues)
static const Eigen::Matrix4d cubicInterpolationConvolutionMatrix
double firstOrderDerivativeAt(const Matrix &layerData, const Index &index, Dim2D dim, double resolution)
double getResolution() const
Definition: GridMap.cpp:648
double mixedSecondOrderDerivativeAt(const Matrix &layerData, const Index &index, double resolution)
Eigen::Vector2d Position
Definition: TypeDefs.hpp:18
bool getNormalizedCoordinates(const GridMap &gridMap, const Position &queriedPosition, Position *position)
const Matrix & get(const std::string &layer) const
Definition: GridMap.cpp:111
bool getFunctionValues(const Matrix &layerData, const IndicesMatrix &indices, DataMatrix *data)
bool getMixedSecondOrderDerivatives(const Matrix &layerData, const IndicesMatrix &indices, double resolution, DataMatrix *derivatives)
double evaluatePolynomial(const FunctionValueMatrix &functionValues, double tx, double ty)
double getLayerValue(const Matrix &layerMat, int rowReq, int colReq)
unsigned int bindIndexToRange(int idReq, unsigned int nElem)
bool evaluateBicubicConvolutionInterpolation(const GridMap &gridMap, const std::string &layer, const Position &queriedPosition, double *interpolatedValue)
bool getClosestPointIndices(const GridMap &gridMap, const Position &queriedPosition, Index *index)
bool getUnitSquareCornerIndices(const GridMap &gridMap, const Position &queriedPosition, IndicesMatrix *indicesMatrix)
bool getIndex(const Position &position, Index &index) const
Definition: GridMap.cpp:229
bool evaluateBicubicInterpolation(const GridMap &gridMap, const std::string &layer, const Position &queriedPosition, double *interpolatedValue)
bool assembleFunctionValueMatrix(const GridMap &gridMap, const std::string &layer, const Position &queriedPosition, FunctionValueMatrix *data)
const Size & getSize() const
Definition: GridMap.cpp:652


grid_map_core
Author(s): P├ęter Fankhauser
autogenerated on Sat Nov 28 2020 03:16:50