Costmap2DWrapper.h

Go to the documentation of this file.

/**
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 */
 
#pragma once
 
#include <algorithm>
#include <array>
#include <cmath>
#include <limits>
 
#include <Eigen/Core>
#include <Eigen/Geometry>
 
#include <ArmarXCore/core/logging/Logging.h>
 
#include <armarx/navigation/algorithms/Costmap.h>
 
#include <ceres/ceres.h>
 
namespace armarx::navigation::algorithms::spfa::smoothing
{
    /**
     * Differentiable wrapper around the standard 2-D distance-to-obstacle costmap.
     *
     * Provides bilinear interpolation in (x, y) for Ceres AutoDiff, with a
     * resolution-aware conservative correction:
     *   - the interpolated distance is reduced by cellSize/2 to account for the
     *     fact that the true obstacle boundary can be up to half a cell away
     *     from the stored cell-center distance;
     *   - if any of the four contributing cells is a collision cell (value <= 0),
     *     the result is capped at -cellSize/2 so the optimizer never sees a
     *     falsely positive clearance.
     */
    class Costmap2DWrapper
    {
    public:
        explicit Costmap2DWrapper(const armarx::navigation::algorithms::Costmap& map) : map_(map)
        {
            cell_size_ = map_.params().cellSize;
        }
 
        template <typename T>
        bool operator()(const T* const x_ptr, const T* const y_ptr, T* out_cost) const
        {
            // World -> local
            const Eigen::Transform<T, 2, 1> tfT = convertTransform<T>(map_.origin());
            const Eigen::Matrix<T, 2, 1> localPosition = tfT.inverse() * Eigen::Matrix<T, 2, 1>{*x_ptr, *y_ptr};
 
            const T cell = T(cell_size_);
            const T half_cell = cell / T(2.0);
 
            // Continuous cell coordinates. Cell centers are at integer values.
            const T vX = (localPosition.x() - half_cell - T(map_.getLocalSceneBounds().min.x())) / cell;
            const T vY = (localPosition.y() - half_cell - T(map_.getLocalSceneBounds().min.y())) / cell;
 
            // Degeneracy guard: shift slightly so floor/ceil behave predictably
            // when vX/vY are exactly integers (i.e. on cell centers).
            const T vX_shift = vX - T(0.01);
            const T vY_shift = vY - T(0.01);
 
            int iXlow = static_cast<int>(toFloat(ceres::floor(vX_shift)));
            int iXhigh = static_cast<int>(toFloat(ceres::ceil(vX_shift)));
            int iYlow = static_cast<int>(toFloat(ceres::floor(vY_shift)));
            int iYhigh = static_cast<int>(toFloat(ceres::ceil(vY_shift)));
 
            // Clamp to map bounds
            const int maxX = static_cast<int>(map_.getGrid().rows()) - 1;
            const int maxY = static_cast<int>(map_.getGrid().cols()) - 1;
            iXlow = std::clamp(iXlow, 0, maxX);
            iXhigh = std::clamp(iXhigh, 0, maxX);
            iYlow = std::clamp(iYlow, 0, maxY);
            iYhigh = std::clamp(iYhigh, 0, maxY);
 
            // Retrieve four neighbour values. Treat invalid/masked cells as collision.
            const double d_ll = queryCell(iXlow, iYlow);
            const double d_hl = queryCell(iXhigh, iYlow);
            const double d_lh = queryCell(iXlow, iYhigh);
            const double d_hh = queryCell(iXhigh, iYhigh);
 
            // Bilinear weights using the unshifted coordinates.
            // Note: we intentionally use the unclamped indices for the weights
            // so that w00+w10+w01+w11 == 1 even at boundaries.
            const T w00 = (T(iXhigh) - vX) * (T(iYhigh) - vY); // (low, low)
            const T w10 = (vX - T(iXlow)) * (T(iYhigh) - vY);  // (high, low)
            const T w01 = (T(iXhigh) - vX) * (vY - T(iYlow));  // (low, high)
            const T w11 = (vX - T(iXlow)) * (vY - T(iYlow));   // (high, high)
 
            T d_bilinear = w00 * T(d_ll) + w10 * T(d_hl) + w01 * T(d_lh) + w11 * T(d_hh);
 
            // Degeneracy guard: if the query point lies exactly on a grid line,
            // the weights sum to 0. Fall back to nearest-neighbour.
            const T sumW = w00 + w10 + w01 + w11;
            if (ceres::abs(sumW) < T(1e-6))
            {
                const std::array<std::pair<int, int>, 4> idxs = {{{iXlow, iYlow},
                                                                  {iXhigh, iYlow},
                                                                  {iXlow, iYhigh},
                                                                  {iXhigh, iYhigh}}};
                const std::array<double, 4> vals = {d_ll, d_hl, d_lh, d_hh};
                double bestVal = vals[0];
                double bestDist = std::numeric_limits<double>::max();
                for (int k = 0; k < 4; ++k)
                {
                    const double dx = static_cast<double>(idxs[k].first) - toFloat(vX);
                    const double dy = static_cast<double>(idxs[k].second) - toFloat(vY);
                    const double dist = dx * dx + dy * dy;
                    if (dist < bestDist)
                    {
                        bestDist = dist;
                        bestVal = vals[k];
                    }
                }
                d_bilinear = T(bestVal);
            }
 
            // Conservative correction based on grid resolution.
            T d_conservative = d_bilinear - half_cell;
 
            // If any contributing cell is in collision, never report a positive clearance.
            const double d_min = std::min({d_ll, d_hl, d_lh, d_hh});
            if (d_min <= 0.0 && d_conservative > -half_cell)
            {
                d_conservative = -half_cell;
            }
 
            out_cost[0] = d_conservative;
            return true;
        }
 
        float cellSize() const noexcept
        {
            return cell_size_;
        }
 
    private:
        armarx::navigation::algorithms::Costmap map_;
        float cell_size_;
 
        double queryCell(int ix, int iy) const
        {
            const armarx::navigation::algorithms::Costmap::Index idx{ix, iy};
            if (!map_.isValid(idx))
            {
                // Invalid/masked/out-of-bounds cell -> treat as deep obstacle.
                return -100.0;
            }
            const auto val = map_.value(idx);
            if (!val.has_value())
            {
                return -100.0;
            }
            return static_cast<double>(val.value());
        }
 
        template <typename T>
        static Eigen::Transform<T, 2, 1> convertTransform(const Eigen::Transform<float, 2, 1>& tf)
        {
            Eigen::Transform<T, 2, 1> out;
            out.matrix() = tf.matrix().template cast<T>();
            return out;
        }
 
        template <typename T>
        static float toFloat(const T& val)
        {
            if constexpr (std::is_same_v<T, double> || std::is_same_v<T, float>)
            {
                return static_cast<float>(val);
            }
            else
            {
                return static_cast<float>(val.a); // Jet
            }
        }
    };
} // namespace armarx::navigation::algorithms::spfa::smoothing