residuals.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 <cmath>
#include <vector>
 
#include <ArmarXCore/core/logging/Logging.h>
 
#include <armarx/navigation/algorithms/spfa/smoothing/Costmap2DWrapper.h>
#include <armarx/navigation/algorithms/spfa/smoothing/io.h>
 
#include <ceres/ceres.h>
 
#if CERES_VERSION_MAJOR >= 2 && CERES_VERSION_MINOR >= 1
#  include <ceres/manifold.h>
    using SubsetConstraint = ceres::SubsetManifold;
#  define SET_CONSTRAINT(problem, param, constraint) \
        (problem).SetManifold((param), (constraint))
#else
#  include <ceres/local_parameterization.h>
    using SubsetConstraint = ceres::SubsetParameterization;
#  define SET_CONSTRAINT(problem, param, constraint) \
        (problem).SetParameterization((param), (constraint))
#endif
 
namespace armarx::navigation::algorithms::spfa::smoothing
{
    struct CenterPoint
    {
        double x, y, theta, v;
    };
 
    inline double normalizeAngle(double a)
    {
        return std::atan2(std::sin(a), std::cos(a));
    }
 
    static void normalizeTrajectoryAngles(std::vector<CenterPoint>& traj)
    {
        for (auto& p : traj)
            p.theta = normalizeAngle(p.theta);
    }
 
    // -----------------------------
    // Position-only smoothness residual
    // -----------------------------
    struct PositionSmoothResidual
    {
        explicit PositionSmoothResidual(double weight = 1.0) : w(weight) {}
 
        template <typename T>
        bool operator()(const T* const c_prev,
                        const T* const c_i,
                        const T* const c_next,
                        T* residual) const
        {
            residual[0] = T(w) * (c_prev[0] - T(2.0) * c_i[0] + c_next[0]); // x
            residual[1] = T(w) * (c_prev[1] - T(2.0) * c_i[1] + c_next[1]); // y
            return true;
        }
 
        double w;
    };
 
    // -----------------------------
    // Obstacle residuals (2-D)
    // -----------------------------
    struct ObstacleResidual2D
    {
        ObstacleResidual2D(const Costmap2DWrapper& wrapper,
                           double weight,
                           double max_dist,
                           double clearance) :
            wrapper_(wrapper),
            w_(weight),
            max_dist_(max_dist),
            clearance_(clearance)
        {
        }
 
        template <typename T>
        bool operator()(const T* const node, T* residual) const
        {
            T d_raw;
            wrapper_(&node[0], &node[1], &d_raw);
 
            if (!ceres::isfinite(d_raw))
            {
                residual[0] = T(w_) * T(100.0);
                return true;
            }
 
            T obs_dist = d_raw - T(clearance_);
 
            if (obs_dist > T(max_dist_))
            {
                residual[0] = T(0.0);
            }
            else if (obs_dist < T(0.0))
            {
                T penetration = -obs_dist;
                const T max_pen = T(3.0) * T(max_dist_);
                if (penetration > max_pen)
                    penetration = max_pen;
                T ratio = penetration / T(max_dist_);
                residual[0] = T(w_) * (ceres::exp(T(2.0) * ratio) - T(1.0) + T(0.1) * ratio);
            }
            else
            {
                residual[0] = T(w_) * (T(max_dist_) - obs_dist) / T(max_dist_);
            }
            return true;
        }
 
        const Costmap2DWrapper& wrapper_;
        double w_;
        double max_dist_;
        double clearance_;
    };
 
    struct MidpointObstacleResidual2D
    {
        MidpointObstacleResidual2D(const Costmap2DWrapper& wrapper,
                                   double weight,
                                   double max_dist,
                                   double clearance) :
            wrapper_(wrapper),
            w_(weight),
            max_dist_(max_dist),
            clearance_(clearance)
        {
        }
 
        template <typename T>
        bool operator()(const T* const c_i, const T* const c_ip1, T* residual) const
        {
            T mx = (c_i[0] + c_ip1[0]) / T(2.0);
            T my = (c_i[1] + c_ip1[1]) / T(2.0);
 
            T d_raw;
            wrapper_(&mx, &my, &d_raw);
 
            if (!ceres::isfinite(d_raw))
            {
                residual[0] = T(w_) * T(100.0);
                return true;
            }
 
            T obs_dist = d_raw - T(clearance_);
 
            if (obs_dist > T(max_dist_))
            {
                residual[0] = T(0.0);
            }
            else if (obs_dist < T(0.0))
            {
                T penetration = -obs_dist;
                const T max_pen = T(3.0) * T(max_dist_);
                if (penetration > max_pen)
                    penetration = max_pen;
                T ratio = penetration / T(max_dist_);
                residual[0] = T(w_) * (ceres::exp(T(2.0) * ratio) - T(1.0) + T(0.1) * ratio);
            }
            else
            {
                residual[0] = T(w_) * (T(max_dist_) - obs_dist) / T(max_dist_);
            }
            return true;
        }
 
        const Costmap2DWrapper& wrapper_;
        double w_;
        double max_dist_;
        double clearance_;
    };
 
    // Obstacle residual sampled at an arbitrary position along the segment.
    // t=0 evaluates at c_i, t=1 at c_ip1; midpoints use t=0.5.
    struct SegmentObstacleResidual2D
    {
        SegmentObstacleResidual2D(const Costmap2DWrapper& wrapper,
                                  double weight,
                                  double max_dist,
                                  double clearance,
                                  double t) :
            wrapper_(wrapper),
            w_(weight),
            max_dist_(max_dist),
            clearance_(clearance),
            t_(t)
        {
        }
 
        template <typename T>
        bool operator()(const T* const c_i, const T* const c_ip1, T* residual) const
        {
            T sx = (T(1.0) - T(t_)) * c_i[0] + T(t_) * c_ip1[0];
            T sy = (T(1.0) - T(t_)) * c_i[1] + T(t_) * c_ip1[1];
 
            T d_raw;
            wrapper_(&sx, &sy, &d_raw);
 
            if (!ceres::isfinite(d_raw))
            {
                residual[0] = T(w_) * T(100.0);
                return true;
            }
 
            T obs_dist = d_raw - T(clearance_);
 
            if (obs_dist > T(max_dist_))
            {
                residual[0] = T(0.0);
            }
            else if (obs_dist < T(0.0))
            {
                T penetration = -obs_dist;
                const T max_pen = T(3.0) * T(max_dist_);
                if (penetration > max_pen)
                    penetration = max_pen;
                T ratio = penetration / T(max_dist_);
                residual[0] = T(w_) * (ceres::exp(T(2.0) * ratio) - T(1.0) + T(0.1) * ratio);
            }
            else
            {
                residual[0] = T(w_) * (T(max_dist_) - obs_dist) / T(max_dist_);
            }
            return true;
        }
 
        const Costmap2DWrapper& wrapper_;
        double w_;
        double max_dist_;
        double clearance_;
        double t_;
    };
 
    // -----------------------------
    // Tracking residuals
    // -----------------------------
 
    /**
     * Bounded tracking residual.
     *
     * - Deviation <= deadzone: 0 cost.
     * - deadzone < deviation <= max_deviation: quadratic growth.
     * - deviation > max_deviation: very steep quadratic growth (hard limit).
     */
    struct BoundedTrackingResidual
    {
        BoundedTrackingResidual(double xr,
                                double yr,
                                double deadzone,
                                double max_deviation,
                                double w_in_bounds,
                                double w_hard) :
            xr_(xr),
            yr_(yr),
            deadzone_(deadzone),
            max_deviation_(max_deviation),
            w_in_bounds_(w_in_bounds),
            w_hard_(w_hard)
        {
        }
 
        template <typename T>
        bool operator()(const T* const c_i, T* residual) const
        {
            T dx = c_i[0] - T(xr_);
            T dy = c_i[1] - T(yr_);
            T d = ceres::sqrt(dx * dx + dy * dy + T(1e-6));
 
            T excess = d - T(deadzone_);
            if (excess <= T(0))
            {
                residual[0] = T(0);
            }
            else if (excess <= T(max_deviation_ - deadzone_))
            {
                residual[0] = T(w_in_bounds_) * excess;
            }
            else
            {
                T over = excess - T(max_deviation_ - deadzone_);
                residual[0] = T(w_in_bounds_) * T(max_deviation_ - deadzone_) + T(w_hard_) * over;
            }
            return true;
        }
 
        double xr_, yr_;
        double deadzone_;
        double max_deviation_;
        double w_in_bounds_;
        double w_hard_;
    };
 
    // Soft tracking to the hill-climbed safe path (weak weight).
    struct SoftTrackingResidual
    {
        SoftTrackingResidual(double xr, double yr, double weight) :
            xr_(xr), yr_(yr), w_(weight)
        {
        }
 
        template <typename T>
        bool operator()(const T* const c_i, T* residual) const
        {
            residual[0] = T(w_) * (c_i[0] - T(xr_));
            residual[1] = T(w_) * (c_i[1] - T(yr_));
            return true;
        }
 
        double xr_, yr_;
        double w_;
    };
 
    // -----------------------------
    // Spacing residual
    // -----------------------------
    struct SpacingResidual
    {
        SpacingResidual(double d_avg, double weight) : d_avg_(d_avg), w(weight) {}
 
        template <typename T>
        bool operator()(const T* const c_i, const T* const c_ip1, T* residual) const
        {
            T dx = c_ip1[0] - c_i[0];
            T dy = c_ip1[1] - c_i[1];
            T dist_sq = dx * dx + dy * dy;
            T target_sq = T(d_avg_ * d_avg_);
            residual[0] = T(w) * (dist_sq - target_sq);
            return true;
        }
 
        double d_avg_;
        double w;
    };
 
    // -----------------------------
    // Helpers
    // -----------------------------
    static double segment_length(const CenterPoint& a, const CenterPoint& b)
    {
        return std::hypot(b.x - a.x, b.y - a.y);
    }
 
    static double computeAverageSpacing(const std::vector<CenterPoint>& traj)
    {
        if (traj.size() < 2)
            return 0.0;
        double sum = 0.0;
        for (size_t i = 0; i + 1 < traj.size(); ++i)
            sum += segment_length(traj[i], traj[i + 1]);
        return sum / double(traj.size() - 1);
    }
 
} // namespace armarx::navigation::algorithms::spfa::smoothing