CubicInterpolation.hpp
Go to the documentation of this file.
1 /*
2  * CubicInterpolation.hpp
3  *
4  * Created on: Jan 21, 2020
5  * Author: Edo Jelavic
6  * Institute: ETH Zurich, Robotic Systems Lab
7  */
8 
9 #pragma once
10 
11 #include <Eigen/Core>
12 #include <vector>
13 #include <map>
15 
16 /*
17  * For difference between bicubic convolution interpolation (piecewise cubic)
18  * and bicubic interpolation see:
19  *
20  * https://en.wikipedia.org/wiki/Bicubic_interpolation
21  *
22  * R. Keys (1981). "Cubic convolution interpolation for digital image processing".
23  * IEEE Transactions on Acoustics, Speech, and Signal Processing. 29 (6): 1153–1160.
24  *
25 * https://web.archive.org/web/20051024202307/
26 * http://www.geovista.psu.edu/sites/geocomp99/Gc99/082/gc_082.htm
27  */
28 
29 namespace grid_map {
30 
31 class GridMap;
32 
33 /*
34  * Data structure (matrix) that contains data
35  * necessary for interpolation. These are either 16
36  * function values in the case of bicubic convolution interpolation
37  * or function values and their derivatives for the case
38  * of standard bicubic interpolation.
39  */
40 using FunctionValueMatrix = Eigen::Matrix4d;
41 
49 unsigned int bindIndexToRange(unsigned int idReq, unsigned int nElem);
50 
51 
62 double getLayerValue(const Matrix &layerMat, int rowReq, int colReq);
63 
64 namespace bicubic_conv {
65 
70 static const Eigen::Matrix4d cubicInterpolationConvolutionMatrix {
71  (Eigen::Matrix4d() << 0.0, 2.0, 0.0, 0.0,
72  -1.0, 0.0, 1.0, 0.0,
73  2.0, -5.0, 4.0, -1.0,
74  -1.0, 3.0, -3.0, 1.0).finished() };
75 
76 /*
77  * Index of the middle knot for bicubic interpolation. This is
78  * the function value with subscripts (0,0), i.e. f00 in
79  * https://en.wikipedia.org/wiki/Bicubic_interpolation
80  * In the grid map it corresponds to the grid map point closest to the
81  * queried point (in terms of Euclidean distance). Queried point has
82  * coordinates (x,y) for at which the interpolation is requested.
83  * @param[in] gridMap - grid map with the data
84  * @param[in] queriedPosition - position for which the interpolated data is requested
85  * @param[out] index - indices of the middle knot for the interpolation
86  * @return - true if success
87  */
88 bool getIndicesOfMiddleKnot(const GridMap &gridMap, const Position &queriedPosition, Index *index);
89 
90 /*
91  * Coordinates used for interpolation need to be shifted and scaled,
92  * since the original algorithm operates around the origin and with unit
93  * resolution
94  * @param[in] gridMap - grid map with the data
95  * @param[in] queriedPosition - position for which the interpolation is requested
96  * @param[out] position - normalized coordinates of the point for which the interpolation is requested
97  * @return - true if success
98  */
99 bool getNormalizedCoordinates(const GridMap &gridMap, const Position &queriedPosition,
100  Position *position);
101 
102 /*
103  * Queries the grid map for function values at the coordinates which are necessary for
104  * performing the interpolation. The right function values are then assembled
105  * in a matrix.
106  * @param[in] gridMap - grid map with the data
107  * @param[in] layer - name of the layer that we are interpolating
108  * @param[in] queriedPosition - position for which the interpolation is requested
109  * @param[out] data - 4x4 matrix with 16 function values used for interpolation, see
110  * R. Keys (1981). "Cubic convolution interpolation for digital image processing".
111  * IEEE Transactions on Acoustics, Speech, and Signal Processing. 29 (6): 1153–1160.
112  * for the details.
113  * @return - true if success
114  */
115 bool assembleFunctionValueMatrix(const GridMap &gridMap, const std::string &layer,
116  const Position &queriedPosition, FunctionValueMatrix *data);
117 
118 /*
119  * Performs convolution in 1D. the function requires 4 function values
120  * to compute the convolution. The result is interpolated data in 1D.
121  * @param[in] t - normalized coordinate (x or y)
122  * @param[in] functionValues - vector of 4 function values necessary to perform
123  * interpolation in 1 dimension.
124  * @return - interpolated value at normalized coordinate t
125  */
126 double convolve1D(double t, const Eigen::Vector4d &functionValues);
127 
128 /*
129  * Performs convolution in 1D. the function requires 4 function values
130  * to compute the convolution. The result is interpolated data in 1D.
131  * @param[in] gridMap - grid map with discrete function values
132  * @param[in] layer - name of the layer for which we want to perform interpolation
133  * @param[in] queriedPosition - position for which the interpolation is requested
134  * @param[out] interpolatedValue - interpolated value at queried point
135  * @return - true if success
136  */
137 bool evaluateBicubicConvolutionInterpolation(const GridMap &gridMap, const std::string &layer,
138  const Position &queriedPosition,
139  double *interpolatedValue);
140 
141 } /* namespace bicubic_conv */
142 
143 namespace bicubic {
144 
145 /*
146  * Enum for the derivatives direction
147  * to perform interpolation one needs
148  * derivatives w.r.t. to x and y dimension.
149  */
150 enum class Dim2D: int {
151  X,
152  Y
153 };
154 
159 static const Eigen::Matrix4d bicubicInterpolationMatrix {
160  (Eigen::Matrix4d() << 1.0, 0.0, 0.0, 0.0,
161  0.0, 0.0, 1.0, 0.0,
162  -3.0, 3.0, -2.0, -1.0,
163  2.0, -2.0, 1.0, 1.0).finished() };
164 
165 /*
166  * Data matrix that can hold function values
167  * these can be either function values at requested
168  * positions or their derivatives.
169  */
171 {
172  double topLeft_ = 0.0;
173  double topRight_ = 0.0;
174  double bottomLeft_ = 0.0;
175  double bottomRight_ = 0.0;
176 };
177 
178 /*
179  * Interpolation is performed on a unit square.
180  * Hence, we need to compute 4 corners of that unit square,
181  * and find their indices in the grid map. IndicesMatrix
182  * is a container that stores those indices. Each index
183  * contains two numbers (row number, column number) in the
184  * grid map.
185  */
187 {
188  Index topLeft_ { 0, 0 };
189  Index topRight_ { 0, 0 };
190  Index bottomLeft_ { 0, 0 };
191  Index bottomRight_ { 0, 0 };
192 };
193 
194 /*
195  * Makes sure that all indices in side the
196  * data structure IndicesMatrix are within the
197  * range of the grid map.
198  * @param[in] gridMap - input grid map with discrete function values
199  * @param[in/out] indices - indices that are bound to range, i.e.
200  * rows and columns are with ranges
201  */
202 void bindIndicesToRange(const GridMap &gridMap, IndicesMatrix *indices);
203 
204 /*
205  * Performs bicubic interpolation at requested position.
206  * @param[in] gridMap - grid map with discrete function values
207  * @param[in] layer - name of the layer for which we want to perform interpolation
208  * @param[in] queriedPosition - position for which the interpolation is requested
209  * @param[out] interpolatedValue - interpolated value at queried point
210  * @return - true if success
211  */
212 bool evaluateBicubicInterpolation(const GridMap &gridMap, const std::string &layer,
213  const Position &queriedPosition, double *interpolatedValue);
214 
215 /*
216  * Deduces which points in the grid map close a unit square around the
217  * queried point and returns their indices (row and column number)
218  * @param[in] gridMap - grid map with discrete function values
219  * @param[in] queriedPosition - position for which the interpolation is requested
220  * @param[out] indicesMatrix - data structure with indices forming a unit square
221  * around the queried point
222  * @return - true if success
223  */
224 bool getUnitSquareCornerIndices(const GridMap &gridMap, const Position &queriedPosition,
225  IndicesMatrix *indicesMatrix);
226 
227 /*
228  * Get index (row and column number) of a point in grid map, which
229  * is closest to the queried position.
230  * @param[in] gridMap - grid map with discrete function values
231  * @param[in] queriedPosition - position for which the interpolation is requested
232  * @param[out] index - indices of the closest point in grid_map
233  * @return - true if success
234  */
235 bool getClosestPointIndices(const GridMap &gridMap, const Position &queriedPosition, Index *index);
236 
237 /*
238  * Retrieve function values from the grid map at requested indices.
239  * @param[in] layerData - layer of a grid map with function values
240  * @param[in] indices - indices (row and column numbers) for which function values are requested
241  * @param[out] data - requested function values
242  * @return - true if success
243  */
244 bool getFunctionValues(const Matrix &layerData, const IndicesMatrix &indices, DataMatrix *data);
245 
246 /*
247  * Retrieve function derivative values from the grid map at requested indices. Function
248  * derivatives are approximated using central difference.
249  * @param[in] layerData - layer of a grid map with function values
250  * @param[in] indices - indices (row and column numbers) for which function derivative
251  * values are requested
252  * @param[in] dim - dimension along which we want to evaluate partial derivatives (X or Y)
253  * @param[in] resolution - resolution of the grid map
254  * @param[out] derivatives - values of derivatives at requested indices
255  * @return - true if success
256  */
257 bool getFirstOrderDerivatives(const Matrix &layerData, const IndicesMatrix &indices, Dim2D dim,
258  double resolution, DataMatrix *derivatives);
259 
260 /*
261  * Retrieve second order function derivative values from the grid map at requested indices.
262  * Function derivatives are approximated using central difference. We compute partial derivative
263  * w.r.t to one coordinate and then the other. Note that the order of differentiation
264  * does not matter.
265  * @param[in] layerData - layer of a grid map with function values
266  * @param[in] indices - indices (row and column numbers) for which function derivative
267  * values are requested
268  * @param[in] resolution - resolution of the grid map
269  * @param[out] derivatives - values of second order mixed derivatives at requested indices
270  * @return - true if success
271  */
272 bool getMixedSecondOrderDerivatives(const Matrix &layerData, const IndicesMatrix &indices,
273  double resolution, DataMatrix *derivatives);
274 
275 /*
276  * First order derivative for a specific point determined by index.
277  * Approximated by central difference.
278  * See https://www.mathematik.uni-dortmund.de/~kuzmin/cfdintro/lecture4.pdf
279  * for details
280  * @param[in] layerData - layer of a grid map with function values
281  * @param[in] index - index (row and column number) for which function derivative
282  * value is requested
283  * @param[in] dim - dimension along which we want to evaluate partial derivative (X or Y)
284  * @param[in] resolution - resolution of the grid map
285  * @return - value of the derivative at requested index
286  */
287 double firstOrderDerivativeAt(const Matrix &layerData, const Index &index, Dim2D dim,
288  double resolution);
289 
290 /*
291  * Second order mixed derivative for a specific point determined by index.
292  * See https://www.mathematik.uni-dortmund.de/~kuzmin/cfdintro/lecture4.pdf
293  * for details
294  * @param[in] layerData - layer of a grid map with function values
295  * @param[in] index - index (row and column number) for which function derivative
296  * value is requested
297  * @param[in] resolution - resolution of the grid map
298  * @return - value of the second order mixed derivative at requested index
299  */
300 double mixedSecondOrderDerivativeAt(const Matrix &layerData, const Index &index, double resolution);
301 
302 /*
303  * Evaluate polynomial at requested coordinates. the function will compute the polynomial
304  * coefficients and then evaluate it. See
305  * https://en.wikipedia.org/wiki/Bicubic_interpolation
306  * for details.
307  * @param[in] functionValues - function values and derivatives required to
308  * compute polynomial coefficients
309  * @param[in] tx - normalized x coordinate for which the interpolation should be computed
310  * @param[in] ty - normalized y coordinate for which the interpolation should be computed
311  * @return - interpolated value at requested normalized coordinates.
312  */
313 double evaluatePolynomial(const FunctionValueMatrix &functionValues, double tx, double ty);
314 
315 /*
316  * Assemble function value matrix from small sub-matrices containing function values
317  * or derivative values at the corners of the unit square.
318  * See https://en.wikipedia.org/wiki/Bicubic_interpolation for details.
319  *
320  * @param[in] f - Function values at the corners of the unit square
321  * @param[in] dfx - Partial derivative w.r.t to x at the corners of the unit square
322  * @param[in] dfy - Partial derivative w.r.t to y at the corners of the unit square
323  * @param[in] ddfxy - Second order partial derivative w.r.t to x and y at the corners of the unit square
324  * @param[out] functionValues - function values and derivatives required to
325  * compute polynomial coefficients
326  */
327 void assembleFunctionValueMatrix(const DataMatrix &f, const DataMatrix &dfx, const DataMatrix &dfy,
328  const DataMatrix &ddfxy, FunctionValueMatrix *functionValues);
329 
330 /*
331  * Coordinates used for interpolation need to be shifter and scaled,
332  * since the original algorithm operates on a unit square around the origin.
333  * @param[in] gridMap - grid map with the data
334  * @param[in] originIndex - index of a bottom left corner if the unit square in the grid map
335  * this corner is the origin for the normalized coordinates.
336  * @param[in] queriedPosition - position for which the interpolation is requested
337  * @param[out] position - normalized coordinates of the point for which the interpolation is requested
338  * @return - true if success
339  */
340 bool computeNormalizedCoordinates(const GridMap &gridMap, const Index &originIndex,
341  const Position &queriedPosition, Position *normalizedCoordinates);
342 
343 } /* namespace bicubic */
344 
345 } /* namespace grid_map*/
bool getFirstOrderDerivatives(const Matrix &layerData, const IndicesMatrix &indices, Dim2D dim, double resolution, DataMatrix *derivatives)
Eigen::Matrix4d FunctionValueMatrix
void bindIndicesToRange(const GridMap &gridMap, IndicesMatrix *indices)
unsigned int bindIndexToRange(unsigned int idReq, unsigned int nElem)
bool getIndicesOfMiddleKnot(const GridMap &gridMap, const Position &queriedPosition, Index *index)
bool computeNormalizedCoordinates(const GridMap &gridMap, const Index &originIndex, const Position &queriedPosition, Position *normalizedCoordinates)
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)
Eigen::Vector2d Position
Definition: TypeDefs.hpp:18
double mixedSecondOrderDerivativeAt(const Matrix &layerData, const Index &index, double resolution)
bool getNormalizedCoordinates(const GridMap &gridMap, const Position &queriedPosition, Position *position)
Eigen::Array2i Index
Definition: TypeDefs.hpp:22
Eigen::MatrixXf Matrix
Definition: TypeDefs.hpp:16
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)
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 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)


grid_map_core
Author(s): Péter Fankhauser
autogenerated on Wed Jul 5 2023 02:23:35