| 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) |
no test coverage detected