21 if (static_cast<unsigned int>(idReq) >= nElem) {
22 return static_cast<unsigned int>(nElem - 1);
24 return static_cast<unsigned int>(idReq);
29 const auto numCol = layerMat.cols();
30 const auto numRow = layerMat.rows();
33 return layerMat(iBoundToRange, jBoundToRange);
46 namespace bicubic_conv {
50 double *interpolatedValue)
62 const double tx = normalizedCoordinate.x();
63 const double ty = normalizedCoordinate.y();
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);
76 double convolve1D(
double t,
const Eigen::Vector4d &functionValues)
78 const Eigen::Vector4d tVector(1.0, t, t * t, t * t * t);
81 const double retVal = 0.5 * tVector.transpose() * temp;
89 Index middleKnotIndex;
94 const Matrix &layerMatrix = gridMap.
get(layer);
95 auto f = [&layerMatrix](
int rowReq,
int colReq) {
100 const unsigned int i = middleKnotIndex.x();
101 const unsigned int j = middleKnotIndex.y();
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);
130 position->x() = (queriedPosition.x() - middleKnot.x()) / gridMap.
getResolution();
131 position->y() = (queriedPosition.y() - middleKnot.y()) / gridMap.
getResolution();
139 if (!gridMap.
getIndex(queriedPosition, *index)) {
159 const Position &queriedPosition,
double *interpolatedValue)
162 const Matrix& layerMat = gridMap.
get(layer);
201 &normalizedCoordinates)) {
207 normalizedCoordinates.y());
216 Index closestPointId;
222 if (!gridMap.
getPosition(closestPointId, closestPoint)) {
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();
268 if (!gridMap.
getIndex(queriedPosition, *index)) {
283 normalizedCoordinates->x() = (queriedPosition.x() - origin.x()) / gridMap.
getResolution();
284 normalizedCoordinates->y() = (queriedPosition.y() - origin.y()) / gridMap.
getResolution();
301 const int numCol = gridMap.
getSize().y();
302 const int numRow = gridMap.
getSize().x();
349 const int numCol = layerData.cols();
350 const int numRow = layerData.rows();
365 throw std::runtime_error(
"Unknown derivative direction");
369 const double perturbation = resolution;
373 return (right - left) / (2.0 * perturbation) * resolution;
384 const int numCol = layerData.cols();
385 const int numRow = layerData.rows();
396 const double perturbation = resolution;
401 return (f11 - f1m1 - fm11 + fm1m1) / (4.0 * perturbation * perturbation) * resolution * resolution;
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
424 const Eigen::Vector4d tempVec = polynomialCoeffMatrix * yVector;
425 return xVector.transpose() * tempVec;
431 auto toEigenMatrix = [](
const DataMatrix &d)-> Eigen::Matrix2d {
433 e(0,0) = d.bottomLeft_;
434 e(1,0) = d.bottomRight_;
436 e(1,1) = d.topRight_;
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);
bool getFirstOrderDerivatives(const Matrix &layerData, const IndicesMatrix &indices, Dim2D dim, double resolution, DataMatrix *derivatives)
Eigen::Matrix4d FunctionValueMatrix
void bindIndicesToRange(const GridMap &gridMap, IndicesMatrix *indices)
bool getPosition(const Index &index, Position &position) const
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)
double getResolution() const
double mixedSecondOrderDerivativeAt(const Matrix &layerData, const Index &index, double resolution)
bool getNormalizedCoordinates(const GridMap &gridMap, const Position &queriedPosition, Position *position)
const Matrix & get(const std::string &layer) const
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
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