residuals.h

Go to the documentation of this file.

#pragma once
 
#include <algorithm>
#include <cmath>
#include <iomanip>
#include <iostream>
#include <istream>
#include <vector>
 
#include <ArmarXCore/core/logging/Logging.h>
 
#include <armarx/navigation/algorithms/orientation_aware/smoothing/Costmap3DWrapper.h>
#include <armarx/navigation/algorithms/orientation_aware/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::orientation_aware::smoothing
{
    // Used ChatGPT for initial draft
    // Full CHOMP-like smoother using Ceres with spacing residual and correct angle handling.
    //
    // NOTE: This is a compact but complete example. Adapt the centerToRobot transform, SDF grid source,
    // weights, and solver options to your codebase.
 
 
    // -----------------------------
    // Data structures
    // -----------------------------
    struct CenterPoint
    {
        double x, y, theta, v;
    };
 
    // -----------------------------
    // Utilities: angle diff + normalize
    // -----------------------------
    template <typename T>
    T
    angleDiff(const T& a, const T& b)
    {
        // returns normalized a - b in [-pi, pi]
        T diff = a - b;
        return ceres::atan2(ceres::sin(diff), ceres::cos(diff));
    }
 
    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);
    }
 
    inline double
    smoothstep(double edge0, double edge1, double x)
    {
        if (x <= edge0)
            return 0.0;
        if (x >= edge1)
            return 1.0;
        double t = (x - edge0) / (edge1 - edge0);
        return t * t * (3.0 - 2.0 * t);
    }
 
    inline double
    trackingScaleFromClearance(double clearance,
                               double fade_start_clearance,
                               double fade_end_clearance)
    {
        // 1.0 in tight space (low clearance), 0.0 in open space (high clearance)
        return 1.0 - smoothstep(fade_start_clearance, fade_end_clearance, clearance);
    }
 
    namespace detail
    {
        template <typename T>
        inline T
        expForSmoothMin(const T& x)
        {
            if constexpr (std::is_arithmetic_v<T>)
                return std::exp(x);
            else
                return ceres::exp(x);
        }
 
        template <typename T>
        inline T
        logForSmoothMin(const T& x)
        {
            if constexpr (std::is_arithmetic_v<T>)
                return std::log(x);
            else
                return ceres::log(x);
        }
    } // namespace detail
 
    // Smooth minimum: differentiable approximation of min().
    // alpha → ∞  → exact min;  alpha → 0  → average.
    // Uses a min-pivot for numerical stability: all exponents are <= 0,
    // so overflow is impossible even when one sample is deeply negative.
    template <typename T>
    T
    smoothMin(const std::vector<T>& values, double alpha)
    {
        if (values.empty())
            return T(0);
        if (values.size() == 1)
            return values[0];
 
        T min_val = values[0];
        for (const auto& v : values)
            if (v < min_val)
                min_val = v;
 
        T sum = T(0);
        for (const auto& v : values)
        {
            T arg = -alpha * (v - min_val); // arg <= 0, never overflows
            sum += detail::expForSmoothMin(arg);
        }
 
        return min_val - detail::logForSmoothMin(sum) / alpha;
    }
 
    // -----------------------------
    // Discrete orientation helpers for fuzzy obstacle queries
    // -----------------------------
    static std::pair<int, int>
    getDiscreteOrientationBracket(double theta_rad, int num_orientations)
    {
        double deg = theta_rad * 180.0 / M_PI;
        // wrap to [0, 360)
        while (deg < 0.0)
            deg += 360.0;
        while (deg >= 360.0)
            deg -= 360.0;
 
        double deg_per = 360.0 / num_orientations;
        int lower = static_cast<int>(std::floor(deg / deg_per));
        int upper = lower + 1;
        if (lower < 0)
            lower += num_orientations;
        if (upper >= num_orientations)
            upper -= num_orientations;
        return {lower, upper};
    }
 
    // -----------------------------
    // Residuals
    // -----------------------------
 
    // Pose second-difference smoothness residual (x,y,theta)
    struct PoseSmoothResidual
    {
        PoseSmoothResidual(double pos_weight = 1.0, double orientation_weight = 1.0) :
            pos_w(pos_weight), ori_w(orientation_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(pos_w) * (c_prev[0] - T(2.0) * c_i[0] + c_next[0]); // x
            residual[1] = T(pos_w) * (c_prev[1] - T(2.0) * c_i[1] + c_next[1]); // y
            // theta second-diff using angleDiff to handle wrap
            T dprev = angleDiff(c_prev[2], c_i[2]);
            T dnext = angleDiff(c_i[2], c_next[2]);
            residual[2] = T(ori_w) * (dprev - dnext);
            return true;
        }
 
        double pos_w;
        double ori_w;
    };
 
    // Residual to enforce smooth orientation transition at start and end of trajectory
    struct StartRotationResidual
    {
        StartRotationResidual(double w) : w_(w)
        {
        }
 
        template <typename T>
        bool
        operator()(const T* const theta0, const T* const theta1, T* residuals) const
        {
 
            T d = angleDiff(theta1[2], theta0[2]);
            residuals[0] = T(w_) * d;
            return true;
        }
 
        double w_;
    };
 
    // Residual to pull theta toward the pre-processed (hill-climbed) orientation.
    // This replaces the old theta-blending inside ObstacleResidual with an
    // explicit, tunable cost that keeps theta from jumping wildly without
    // hiding the true collision gradient.
    struct ThetaTrackingResidual
    {
        ThetaTrackingResidual(double w, double theta_target) : w_(w), theta_target_(theta_target) {}
 
        template <typename T>
        bool operator()(const T* const node, T* residual) const
        {
            residual[0] = T(w_) * angleDiff(node[2], T(theta_target_));
            return true;
        }
 
        double w_;
        double theta_target_;
    };
 
    // Accel/decel limit residual: symmetric per-segment rate limit.
    // Penalizes only if the velocity change across a segment exceeds the bounds.
    struct AccelDecelLimitResidual
    {
        AccelDecelLimitResidual(double max_accel, double max_decel, double weight) :
            max_accel_(max_accel), max_decel_(max_decel), w(weight)
        {
        }
 
        template <typename T>
        bool
        operator()(const T* const c_i, const T* const c_ip1, T* residual) const
        {
            T dv = c_ip1[3] - c_i[3];
            if (dv > T(max_accel_))
                residual[0] = T(w) * (dv - T(max_accel_));
            else if (dv < -T(max_decel_))
                residual[0] = T(w) * (-dv - T(max_decel_));
            else
                residual[0] = T(0.0);
            return true;
        }
 
        double max_accel_;
        double max_decel_;
        double w;
    };
 
    // Nominal velocity residual: pulls v_i toward the cruise speed.
    struct NominalVelocityResidual
    {
        NominalVelocityResidual(double v_nominal, double weight) :
            v_nominal_(v_nominal), w(weight)
        {
        }
 
        template <typename T>
        bool
        operator()(const T* const c_i, T* residual) const
        {
            residual[0] = T(w) * (c_i[3] - T(v_nominal_));
            return true;
        }
 
        double v_nominal_;
        double w;
    };
 
    // Velocity proximity residual: penalizes high speed near obstacles.
    // danger is precomputed in [0,1] from the obstacle distance.
    // Only speeds above min_velocity are penalized.
    struct VelocityProximityResidual
    {
        VelocityProximityResidual(double danger, double min_velocity, double weight) :
            danger_(danger), min_velocity_(min_velocity), w(weight)
        {
        }
 
        template <typename T>
        bool
        operator()(const T* const c_i, T* residual) const
        {
            T v_above_min = c_i[3] - T(min_velocity_);
            if (v_above_min < T(0))
                v_above_min = T(0);
            residual[0] = T(w) * v_above_min * T(danger_);
            return true;
        }
 
        double danger_;
        double min_velocity_;
        double w;
    };
 
    // Jerk residual (optional) for tighter smoothness on pose (uses 4 consecutive centers)
    struct PoseJerkResidual
    {
        PoseJerkResidual(double weight = 1.0) : w(weight)
        {
        }
 
        template <typename T>
        bool
        operator()(const T* const c_m2,
                   const T* const c_m1,
                   const T* const c_i,
                   const T* const c_p1,
                   T* residual) const
        {
            residual[0] = T(w) * (c_m2[0] - T(3.0) * c_m1[0] + T(3.0) * c_i[0] - c_p1[0]);
            residual[1] = T(w) * (c_m2[1] - T(3.0) * c_m1[1] + T(3.0) * c_i[1] - c_p1[1]);
            // approximate angle jerk (small-angle assumption)
            T a_m2 = c_m2[2], a_m1 = c_m1[2], a_i = c_i[2], a_p1 = c_p1[2];
            T r = angleDiff(a_m2, a_i) - T(3.0) * angleDiff(a_m1, a_i) +
                  T(3.0) * angleDiff(a_i, a_i) - angleDiff(a_p1, a_i);
            residual[2] = T(w) * r;
            // T d1 = angleDiff(a_m1, a_i);
            // T d2 = angleDiff(a_i,   a_p1);
            // T d3 = angleDiff(a_p1,  a_p2);
            //
            // T jerk = d1 - T(2.0)*d2 + d3;
            // residual[2] = T(w) * jerk;
 
            return true;
        }
 
        double w;
    };
 
    // obstacle residual using Costmap3DWrapper; weighted
    struct ObstacleResidual
    {
        ObstacleResidual(const Costmap3DWrapper& wrapper,
                          double weight,
                          double max_dist,
                          double clearance,
                          double barrier_eps,
                          bool use_fuzzy_orientation = true,
                          int num_orientations = 72,
                          int fuzzy_window_bins = 2,
                          double fuzzy_alpha = 5.0) :
            wrapper_(wrapper),
            w_(weight),
            max_dist_(max_dist),
            clearance_(clearance),
            barrier_eps_(barrier_eps),
            use_fuzzy_orientation_(use_fuzzy_orientation),
            num_orientations_(num_orientations),
            fuzzy_window_bins_(fuzzy_window_bins),
            fuzzy_alpha_(fuzzy_alpha)
        {
        }
 
        template <typename T>
        bool
        operator()(const T* const node, T* residual) const
        {
            // Evaluate obstacle cost at the current Ceres theta so the optimizer
            // sees the true collision landscape.  A separate ThetaTrackingResidual
            // keeps theta from jumping wildly without hiding the gradient.
            T theta_query = node[2];
            T obs_dist_raw;
 
            if (use_fuzzy_orientation_)
            {
                // Sample orientations in a window around theta_query and take the
                // smooth minimum. Each sample orientation is a differentiable
                // function of theta, so the obstacle residual propagates a strong
                // theta gradient that pushes the robot away from colliding orientations.
                const double bin_size = 2.0 * M_PI / static_cast<double>(num_orientations_);
                std::vector<T> samples;
                samples.reserve(2 * fuzzy_window_bins_ + 1);
                for (int k = -fuzzy_window_bins_; k <= fuzzy_window_bins_; ++k)
                {
                    T theta_sample = theta_query + T(static_cast<double>(k) * bin_size);
                    T d_sample;
                    wrapper_(&node[0], &node[1], &theta_sample, &d_sample);
                    samples.push_back(d_sample);
                }
                obs_dist_raw = smoothMin(samples, fuzzy_alpha_);
            }
            else
            {
                wrapper_(&node[0], &node[1], &theta_query, &obs_dist_raw);
            }
 
            // Defensive guard: if the wrapper ever returns non-finite, treat as deep
            // obstacle so the residual stays finite and Ceres does not abort.
            if (!ceres::isfinite(obs_dist_raw))
            {
                residual[0] = T(w_) * T(100.0);
                return true;
            }
 
            // Keep deeply negative values bounded for numerical stability.
            const T min_allowed = T(-3.0) * T(max_dist_);
            if (obs_dist_raw < min_allowed)
                obs_dist_raw = min_allowed;
 
            // Shift the distance field so the optimizer treats 'clearance' mm away
            // from the true safety margin as the new zero boundary.
            T obs_dist = obs_dist_raw - T(clearance_);
 
            if (obs_dist >= T(0.0))
            {
                // Outside the clearance margin: no obstacle cost.
                residual[0] = T(0.0);
            }
            else
            {
                // Inside the clearance margin: smooth one-sided barrier.
                // Zero at the boundary, ~linear deep inside, capped to keep the
                // residual finite for very deep penetrations.
                T penetration = -obs_dist;
                const T max_pen = T(3.0) * T(max_dist_);
                if (penetration > max_pen)
                    penetration = max_pen;
 
                const T eps = T(barrier_eps_);
                // Smooth hinge: r(p) = 0.5 * (sqrt(p^2 + eps^2) + p) - eps/2
                // r(0) = 0, r'(0) = 0.5, r(p) -> p for p >> eps.
                T r = T(0.5) * (ceres::sqrt(penetration * penetration + eps * eps) + penetration)
                      - T(0.5) * eps;
                residual[0] = T(w_) * r;
            }
            return true;
        }
 
        const Costmap3DWrapper& wrapper_;
        double w_;
        double max_dist_;
        double clearance_;
        double barrier_eps_;
        bool use_fuzzy_orientation_;
        int num_orientations_;
        int fuzzy_window_bins_;
        double fuzzy_alpha_;
    };
 
    // Midpoint obstacle residual: evaluates obstacle cost at the midpoint
    // between two consecutive waypoints.  This is essential because the
    // straight segment between two free waypoints can cut through an
    // obstacle that neither endpoint touches.
    struct MidpointObstacleResidual
    {
        MidpointObstacleResidual(const Costmap3DWrapper& wrapper,
                                  double weight,
                                  double max_dist,
                                  double clearance,
                                  double barrier_eps,
                                  bool use_fuzzy_orientation = true,
                                  int num_orientations = 72,
                                  int fuzzy_window_bins = 2,
                                  double fuzzy_alpha = 5.0) :
            wrapper_(wrapper),
            w_(weight),
            max_dist_(max_dist),
            clearance_(clearance),
            barrier_eps_(barrier_eps),
            use_fuzzy_orientation_(use_fuzzy_orientation),
            num_orientations_(num_orientations),
            fuzzy_window_bins_(fuzzy_window_bins),
            fuzzy_alpha_(fuzzy_alpha)
        {
        }
 
        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 mtheta = c_i[2] + angleDiff(c_ip1[2], c_i[2]) / T(2.0);
 
            // Evaluate obstacle cost at the current Ceres midpoint theta so the
            // optimizer sees the true collision landscape.
            T theta_query = mtheta;
            T obs_dist_raw;
 
            if (use_fuzzy_orientation_)
            {
                // Sample orientations in a window around the midpoint theta and
                // take the smooth minimum. This propagates a strong theta gradient
                // so the segment can rotate away from collisions.
                const double bin_size = 2.0 * M_PI / static_cast<double>(num_orientations_);
                std::vector<T> samples;
                samples.reserve(2 * fuzzy_window_bins_ + 1);
                for (int k = -fuzzy_window_bins_; k <= fuzzy_window_bins_; ++k)
                {
                    T theta_sample = theta_query + T(static_cast<double>(k) * bin_size);
                    T d_sample;
                    wrapper_(&mx, &my, &theta_sample, &d_sample);
                    samples.push_back(d_sample);
                }
                obs_dist_raw = smoothMin(samples, fuzzy_alpha_);
            }
            else
            {
                wrapper_(&mx, &my, &theta_query, &obs_dist_raw);
            }
 
            // Defensive guard: if the wrapper ever returns non-finite, treat as deep
            // obstacle so the residual stays finite and Ceres does not abort.
            if (!ceres::isfinite(obs_dist_raw))
            {
                residual[0] = T(w_) * T(100.0);
                return true;
            }
 
            // Keep deeply negative values bounded for numerical stability.
            const T min_allowed = T(-3.0) * T(max_dist_);
            if (obs_dist_raw < min_allowed)
                obs_dist_raw = min_allowed;
 
            T obs_dist = obs_dist_raw - T(clearance_);
 
            if (obs_dist >= T(0.0))
            {
                // Outside the clearance margin: no obstacle cost.
                residual[0] = T(0.0);
            }
            else
            {
                // Inside the clearance margin: smooth one-sided barrier.
                T penetration = -obs_dist;
                const T max_pen = T(3.0) * T(max_dist_);
                if (penetration > max_pen)
                    penetration = max_pen;
 
                const T eps = T(barrier_eps_);
                T r = T(0.5) * (ceres::sqrt(penetration * penetration + eps * eps) + penetration)
                      - T(0.5) * eps;
                residual[0] = T(w_) * r;
            }
            return true;
        }
 
        const Costmap3DWrapper& wrapper_;
        double w_;
        double max_dist_;
        double clearance_;
        double barrier_eps_;
        bool use_fuzzy_orientation_;
        int num_orientations_;
        int fuzzy_window_bins_;
        double fuzzy_alpha_;
    };
 
    // Tracking residual to original center traj: (x,y,theta,v)
    struct TrackingResidual
    {
        TrackingResidual(double xr,
                         double yr,
                         double thetar,
                         double vr,
                         double wx,
                         double wy,
                         double wth,
                         double wv) :
            xr_(xr), yr_(yr), thetar_(thetar), vr_(vr), wx_(wx), wy_(wy), wth_(wth), wv_(wv)
        {
        }
 
        template <typename T>
        bool
        operator()(const T* const c_i, T* residual) const
        {
            residual[0] = T(wx_) * (c_i[0] - T(xr_));
            residual[1] = T(wy_) * (c_i[1] - T(yr_));
            residual[2] = T(wth_) * angleDiff(c_i[2], T(thetar_));
            residual[3] = T(wv_) * (c_i[3] - T(vr_));
            return true;
        }
 
        double xr_, yr_, thetar_, vr_;
        double wx_, wy_, wth_, wv_;
    };
 
    // Velocity limit hinge residual: penalize v > vmax
    struct VelLimitResidual
    {
        VelLimitResidual(double vmax, double weight = 1.0) : vmax_(vmax), w(weight)
        {
        }
 
        template <typename T>
        bool
        operator()(const T* const c_i, T* residual) const
        {
            T v = c_i[3];
            T diff = v - T(vmax_);
            if (diff > T(0))
                residual[0] = T(w) * diff;
            else
                residual[0] = T(0.0);
            return true;
        }
 
        double vmax_;
        double w;
    };
 
    // Robot smoothness: second-diff applied to robot pose derived from center.
    // The mapping center->robot is templated for AutoDiff.
    struct RobotSmoothResidual
    {
        RobotSmoothResidual(double weight = 1.0, double dx = -0.5, double dy = 0.0) :
            w(weight), dx_(dx), dy_(dy)
        {
        }
 
        template <typename T>
        inline void
        centerToRobotTemplated(const T* const c, T& rx, T& ry, T& rtheta) const
        {
            T cx = c[0], cy = c[1], cth = c[2];
            T ccos = ceres::cos(cth), csin = ceres::sin(cth);
            rx = cx + ccos * T(dx_) - csin * T(dy_);
            ry = cy + csin * T(dx_) + ccos * T(dy_);
            rtheta = cth; // robot orientation equals center orientation here; adapt if needed
        }
 
        template <typename T>
        bool
        operator()(const T* const c_prev,
                   const T* const c_i,
                   const T* const c_next,
                   T* residual) const
        {
            T rpx, rpy, rpth, rix, riy, rith, rnx, rny, rnth;
            centerToRobotTemplated(c_prev, rpx, rpy, rpth);
            centerToRobotTemplated(c_i, rix, riy, rith);
            centerToRobotTemplated(c_next, rnx, rny, rnth);
            residual[0] = T(w) * (rpx - T(2.0) * rix + rnx);
            residual[1] = T(w) * (rpy - T(2.0) * riy + rny);
            // Proper theta second-diff via angleDiff:
            residual[2] = T(w) * (angleDiff(rpth, rith) - angleDiff(rnth, rith));
            return true;
        }
 
        double w;
        double dx_, dy_;
    };
 
    // Robot-pose proximity to a reference robot base pose.
    // Smooth one-sided hinge: negligible cost inside the deadband,
    // linearly-growing cost outside.  The center->robot transform matches
    // RobotSmoothResidual (dx = -0.5, dy = 0.0).
    struct RobotPoseProximityResidual
    {
        RobotPoseProximityResidual(double rx0,
                                   double ry0,
                                   double threshold_mm,
                                   double weight,
                                   double eps_mm) :
            rx0_(rx0), ry0_(ry0), threshold_(threshold_mm), w_(weight), eps_(eps_mm)
        {
        }
 
        template <typename T>
        inline void
        centerToRobot(const T* const c, T& rx, T& ry) const
        {
            T cx = c[0], cy = c[1], cth = c[2];
            T ccos = ceres::cos(cth), csin = ceres::sin(cth);
            rx = cx + ccos * T(-0.5) - csin * T(0.0);
            ry = cy + csin * T(-0.5) + ccos * T(0.0);
        }
 
        template <typename T>
        bool
        operator()(const T* const c_i, T* residual) const
        {
            T rx, ry;
            centerToRobot(c_i, rx, ry);
 
            T dx = rx - T(rx0_);
            T dy = ry - T(ry0_);
            // Regularize the distance to avoid sqrt(0) and the resulting
            // division-by-zero in its derivative at the initial trajectory.
            const T dist_eps = T(1e-6);
            T dist = ceres::sqrt(dx * dx + dy * dy + dist_eps * dist_eps);
 
            T y = dist - T(threshold_);
            residual[0] = T(w_) * T(0.5)
                          * (ceres::sqrt(y * y + T(eps_ * eps_)) + y);
            return true;
        }
 
        double rx0_, ry0_;
        double threshold_, w_, eps_;
    };
 
    // Spacing residual: keeps |p_{i+1} - p_i| close to average spacing d_avg
    // Uses squared distance so the Ceres cost is (dist^2 - d_avg^2)^2,
    // which is a true quadratic in (x,y) and avoids the sqrt(0) singularity.
    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;
    };
 
    // Soft spacing residual: linear penalty on relative deviation up to a
    // configurable threshold, then an additional quadratic penalty beyond it.
    // This lets the optimizer redistribute segment lengths moderately (e.g. at
    // corridor exits) without the explosive stiffness of (dist^2 - d_avg^2)^2.
    struct SoftSpacingResidual
    {
        SoftSpacingResidual(double d_avg,
                            double linear_weight,
                            double quadratic_weight,
                            double relative_deviation_threshold) :
            d_avg_(d_avg),
            linear_w_(linear_weight),
            quadratic_w_(quadratic_weight),
            threshold_(relative_deviation_threshold)
        {
        }
 
        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;
            // Regularise sqrt to avoid a singular derivative at dist == 0.
            T dist = ceres::sqrt(dist_sq + T(1e-12));
            T rel_dev = ceres::abs(dist / T(d_avg_) - T(1.0));
 
            T r = T(linear_w_) * rel_dev;
            T excess = rel_dev - T(threshold_);
            if (excess > T(0.0))
            {
                r += T(quadratic_w_) * excess * excess;
            }
            residual[0] = r;
            return true;
        }
 
        double d_avg_;
        double linear_w_;
        double quadratic_w_;
        double threshold_;
    };
 
    // -----------------------------
    // Options & 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);
    }
 
    // -----------------------------
    // Diagnostic: log per-residual cost breakdown
    // -----------------------------
    static void
    logResidualBreakdown(const std::vector<CenterPoint>& traj,
                         const Costmap3DWrapper& wrapper,
                         const std::vector<CenterPoint>& targets,
                         const std::vector<double>& tracking_scales,
                         const io::SmoothingParams& opts)
    {
        const int N = static_cast<int>(traj.size());
        double c_obs = 0.0, c_mid_obs = 0.0, c_pose_smooth = 0.0, c_start_rot = 0.0,
               c_vel_smooth = 0.0, c_jerk = 0.0, c_robot_smooth = 0.0, c_spacing = 0.0,
               c_tracking = 0.0, c_robot_pose_proximity = 0.0;
 
        auto evalObsAtTheta = [&](double x, double y, double th) -> double
        {
            double d_raw;
            wrapper(&x, &y, &th, &d_raw);
            double d = d_raw - opts.clearance;
            if (d < 0.0)
            {
                double pen = -d;
                double max_pen = 3.0 * opts.obs_max_distance;
                if (pen > max_pen)
                    pen = max_pen;
                const double eps = opts.obs_barrier_eps_mm;
                // Smooth one-sided barrier: zero at the boundary, ~linear deep inside.
                const double r = 0.5 * (std::sqrt(pen * pen + eps * eps) + pen) - 0.5 * eps;
                return opts.w_obs * r;
            }
            return 0.0;
        };
 
        auto evalObsFuzzy = [&](double x, double y, double theta_query) -> double
        {
            if (!opts.use_fuzzy_orientation)
                return evalObsAtTheta(x, y, theta_query);
 
            double d_raw;
            if (opts.fuzzy_orientation_window_bins > 0)
            {
                // Sample orientations in a window around theta_query and take the
                // smooth minimum, matching the behaviour of ObstacleResidual.
                const double bin_size = 2.0 * M_PI / 72.0;
                std::vector<double> samples;
                samples.reserve(2 * opts.fuzzy_orientation_window_bins + 1);
                for (int k = -opts.fuzzy_orientation_window_bins;
                     k <= opts.fuzzy_orientation_window_bins;
                     ++k)
                {
                    double theta_sample = theta_query + static_cast<double>(k) * bin_size;
                    double d_sample;
                    wrapper(&x, &y, &theta_sample, &d_sample);
                    samples.push_back(d_sample);
                }
                d_raw = smoothMin(samples, opts.fuzzy_orientation_alpha);
            }
            else
            {
                wrapper(&x, &y, &theta_query, &d_raw);
            }
 
            // Deep-penetration cap for stability.
            const double min_allowed = -3.0 * opts.obs_max_distance;
            if (d_raw < min_allowed)
                d_raw = min_allowed;
 
            double d = d_raw - opts.clearance;
            if (d < 0.0)
            {
                double pen = -d;
                double max_pen = 3.0 * opts.obs_max_distance;
                if (pen > max_pen)
                    pen = max_pen;
                const double eps = opts.obs_barrier_eps_mm;
                // Smooth one-sided barrier: zero at the boundary, ~linear deep inside.
                const double r = 0.5 * (std::sqrt(pen * pen + eps * eps) + pen) - 0.5 * eps;
                return opts.w_obs * r;
            }
            return 0.0;
        };
 
        // Obstacle (waypoint)
        if (opts.use_obs)
        {
            for (int i = 0; i < N; ++i)
            {
                double r = evalObsFuzzy(traj[i].x, traj[i].y, traj[i].theta);
                c_obs += r * r;
            }
            for (int i = 0; i < N - 1; ++i)
            {
                double midTheta =
                    normalizeAngle(traj[i].theta
                                   + angleDiff(traj[i + 1].theta, traj[i].theta) / 2.0);
                double mx = (traj[i].x + traj[i + 1].x) / 2.0;
                double my = (traj[i].y + traj[i + 1].y) / 2.0;
                double r = evalObsFuzzy(mx, my, midTheta);
                c_mid_obs += r * r;
            }
        }
 
        // Pose smoothness
        for (int i = 1; i <= N - 2; ++i)
        {
            double rx = traj[i - 1].x - 2.0 * traj[i].x + traj[i + 1].x;
            double ry = traj[i - 1].y - 2.0 * traj[i].y + traj[i + 1].y;
            double dprev = normalizeAngle(traj[i - 1].theta - traj[i].theta);
            double dnext = normalizeAngle(traj[i].theta - traj[i + 1].theta);
            double rth = dprev - dnext;
            c_pose_smooth += (opts.w_pose_smooth * rx) * (opts.w_pose_smooth * rx)
                            + (opts.w_pose_smooth * ry) * (opts.w_pose_smooth * ry)
                            + (opts.w_orientation_smooth * rth) * (opts.w_orientation_smooth * rth);
        }
 
        // Start/end rotation
        if (N >= 2)
        {
            double d = normalizeAngle(traj[1].theta - traj[0].theta);
            c_start_rot += (opts.w_boundary * d) * (opts.w_boundary * d);
            d = normalizeAngle(traj[N - 1].theta - traj[N - 2].theta);
            c_start_rot += (opts.w_boundary * d) * (opts.w_boundary * d);
        }
 
        // Accel/decel limits
        for (int i = 0; i < N - 1; ++i)
        {
            double dv = traj[i + 1].v - traj[i].v;
            if (dv > opts.max_accel_per_segment)
                c_vel_smooth += (opts.w_accel_decel_limit * (dv - opts.max_accel_per_segment))
                                * (opts.w_accel_decel_limit * (dv - opts.max_accel_per_segment));
            else if (dv < -opts.max_decel_per_segment)
                c_vel_smooth += (opts.w_accel_decel_limit * (-dv - opts.max_decel_per_segment))
                                * (opts.w_accel_decel_limit * (-dv - opts.max_decel_per_segment));
        }
 
        // Nominal velocity
        double v_nominal = (N >= 3) ? traj[1].v : traj.front().v;
        for (int i = 1; i < N - 1; ++i)
        {
            double rv = traj[i].v - v_nominal;
            c_vel_smooth += (opts.w_nominal_velocity * rv) * (opts.w_nominal_velocity * rv);
        }
 
        // Velocity proximity
        for (int i = 1; i < N - 1; ++i)
        {
            double d = 0.0;
            wrapper(&traj[i].x, &traj[i].y, &traj[i].theta, &d);
            double danger = std::clamp((opts.slow_down_distance - d) / opts.slow_down_distance,
                                       0.0,
                                       1.0);
            double v_above_min = std::max(traj[i].v - opts.min_velocity_near_obstacle, 0.0);
            c_vel_smooth +=
                (opts.w_velocity_proximity * v_above_min * danger)
                * (opts.w_velocity_proximity * v_above_min * danger);
        }
 
        // Pose jerk
        if (opts.use_jerk && N >= 4)
        {
            for (int i = 2; i <= N - 2; ++i)
            {
                double rx = traj[i - 2].x - 3.0 * traj[i - 1].x + 3.0 * traj[i].x - traj[i + 1].x;
                double ry = traj[i - 2].y - 3.0 * traj[i - 1].y + 3.0 * traj[i].y - traj[i + 1].y;
                double r = normalizeAngle(traj[i - 2].theta - traj[i].theta)
                           - 3.0 * normalizeAngle(traj[i - 1].theta - traj[i].theta)
                           + 3.0 * normalizeAngle(traj[i].theta - traj[i].theta)
                           - normalizeAngle(traj[i + 1].theta - traj[i].theta);
                c_jerk += (opts.w_pose_jerk * rx) * (opts.w_pose_jerk * rx)
                        + (opts.w_pose_jerk * ry) * (opts.w_pose_jerk * ry)
                        + (opts.w_pose_jerk * r) * (opts.w_pose_jerk * r);
            }
        }
 
        // Robot smoothness
        if (opts.use_robot_smooth)
        {
            for (int i = 1; i <= N - 2; ++i)
            {
                auto c2r = [](const CenterPoint& c, double& rx, double& ry, double& rth)
                {
                    double ccos = std::cos(c.theta), csin = std::sin(c.theta);
                    rx = c.x + ccos * (-0.5) - csin * (0.0);
                    ry = c.y + csin * (-0.5) + ccos * (0.0);
                    rth = c.theta;
                };
                double rpx, rpy, rpth, rix, riy, rith, rnx, rny, rnth;
                c2r(traj[i - 1], rpx, rpy, rpth);
                c2r(traj[i], rix, riy, rith);
                c2r(traj[i + 1], rnx, rny, rnth);
                double rx = opts.w_robot_smooth * (rpx - 2.0 * rix + rnx);
                double ry = opts.w_robot_smooth * (rpy - 2.0 * riy + rny);
                double rth = opts.w_robot_smooth
                             * (normalizeAngle(rpth - rith) - normalizeAngle(rnth - rith));
                c_robot_smooth += rx * rx + ry * ry + rth * rth;
            }
        }
 
        // Spacing
        double d_avg = 0.0;
        for (int i = 0; i + 1 < N; ++i)
            d_avg += std::hypot(traj[i + 1].x - traj[i].x, traj[i + 1].y - traj[i].y);
        if (N > 1)
            d_avg /= (N - 1);
        if (d_avg <= 1e-9)
            d_avg = 0.1;
        for (int i = 0; i + 1 < N; ++i)
        {
            double dist = std::hypot(traj[i + 1].x - traj[i].x, traj[i + 1].y - traj[i].y);
            if (opts.use_soft_spacing)
            {
                double rel_dev = std::abs(dist / d_avg - 1.0);
                double r = opts.w_spacing_linear * rel_dev;
                double excess = rel_dev - opts.spacing_relative_deviation_threshold;
                if (excess > 0.0)
                {
                    r += opts.w_spacing_quadratic * excess * excess;
                }
                c_spacing += r * r;
            }
            else
            {
                double r = opts.w_spacing * (dist * dist - d_avg * d_avg);
                c_spacing += r * r;
            }
        }
 
        // Tracking
        if (opts.use_tracking && static_cast<int>(targets.size()) == N)
        {
            for (int i = 0; i < N; ++i)
            {
                double s = tracking_scales[i];
                double wx = opts.w_track * s, wy = opts.w_track * s,
                       wth = opts.w_track * 300.0 * s, wv = opts.w_track * 0.2 * s;
                double rx = wx * (traj[i].x - targets[i].x);
                double ry = wy * (traj[i].y - targets[i].y);
                double rth = wth * normalizeAngle(traj[i].theta - targets[i].theta);
                double rv = wv * (traj[i].v - targets[i].v);
                c_tracking += rx * rx + ry * ry + rth * rth + rv * rv;
            }
        }
 
        // Robot-pose proximity to pre-processed trajectory
        if (opts.use_robot_pose_proximity && static_cast<int>(targets.size()) == N)
        {
            auto c2r = [](const CenterPoint& c, double& rx, double& ry)
            {
                double ccos = std::cos(c.theta), csin = std::sin(c.theta);
                rx = c.x + ccos * (-0.5) - csin * (0.0);
                ry = c.y + csin * (-0.5) + ccos * (0.0);
            };
            const double eps_sq = opts.robot_pose_proximity_eps_mm
                                  * opts.robot_pose_proximity_eps_mm;
            for (int i = 1; i <= N - 2; ++i)
            {
                double rx0, ry0, rx, ry;
                c2r(targets[i], rx0, ry0);
                c2r(traj[i], rx, ry);
                const double dx = rx - rx0;
                const double dy = ry - ry0;
                const double dist = std::hypot(dx, dy);
                const double y = dist - opts.robot_pose_proximity_threshold_mm;
                const double r = opts.w_robot_pose_proximity * tracking_scales[i] * 0.5
                                 * (std::sqrt(y * y + eps_sq) + y);
                c_robot_pose_proximity += r * r;
            }
        }
 
        double total = c_obs + c_mid_obs + c_pose_smooth + c_start_rot + c_vel_smooth + c_jerk
                     + c_robot_smooth + c_spacing + c_tracking + c_robot_pose_proximity;
 
        ARMARX_INFO << "=== Residual cost breakdown ===";
        auto pct = [&](double c) -> double { return total > 0.0 ? c / total * 100.0 : 0.0; };
        ARMARX_INFO << "Obstacle (waypoint):  " << std::scientific << std::setprecision(3)
                    << c_obs << " (" << std::fixed << std::setprecision(1) << pct(c_obs) << "%)";
        ARMARX_INFO << "Obstacle (midpoint):  " << std::scientific << std::setprecision(3)
                    << c_mid_obs << " (" << std::fixed << std::setprecision(1) << pct(c_mid_obs)
                    << "%)";
        ARMARX_INFO << "Pose smoothness:      " << std::scientific << std::setprecision(3)
                    << c_pose_smooth << " (" << std::fixed << std::setprecision(1)
                    << pct(c_pose_smooth) << "%)";
        ARMARX_INFO << "Start/end rotation:   " << std::scientific << std::setprecision(3)
                    << c_start_rot << " (" << std::fixed << std::setprecision(1)
                    << pct(c_start_rot) << "%)";
        ARMARX_INFO << "Velocity smoothness:  " << std::scientific << std::setprecision(3)
                    << c_vel_smooth << " (" << std::fixed << std::setprecision(1)
                    << pct(c_vel_smooth) << "%)";
        ARMARX_INFO << "Pose jerk:            " << std::scientific << std::setprecision(3)
                    << c_jerk << " (" << std::fixed << std::setprecision(1) << pct(c_jerk)
                    << "%)";
        ARMARX_INFO << "Robot smoothness:     " << std::scientific << std::setprecision(3)
                    << c_robot_smooth << " (" << std::fixed << std::setprecision(1)
                    << pct(c_robot_smooth) << "%)";
        ARMARX_INFO << "Spacing:              " << std::scientific << std::setprecision(3)
                    << c_spacing << " (" << std::fixed << std::setprecision(1) << pct(c_spacing)
                    << "%)";
        ARMARX_INFO << "Tracking:             " << std::scientific << std::setprecision(3)
                    << c_tracking << " (" << std::fixed << std::setprecision(1)
                    << pct(c_tracking) << "%)";
        ARMARX_INFO << "Robot-pose proximity: " << std::scientific << std::setprecision(3)
                    << c_robot_pose_proximity << " (" << std::fixed << std::setprecision(1)
                    << pct(c_robot_pose_proximity) << "%)";
        ARMARX_INFO << "TOTAL:                " << std::scientific << std::setprecision(3)
                    << total;
    }
 
    // -----------------------------
    // Main optimizer
    // -----------------------------
    void
    optimizeTrajectoryCeres(std::vector<CenterPoint>& traj,
                            const Costmap3DWrapper& costmap_wrapper,
                            const std::vector<CenterPoint>& traj_targets,
                            const io::SmoothingParams& opts)
    {
        const int N = static_cast<int>(traj.size());
        if (N < 4)
        {
            ARMARX_INFO << "[optimizeTrajectoryCeres] need at least 4 points\n";
            return;
        }
 
        normalizeTrajectoryAngles(traj);
 
        // Deduplicate: if two consecutive waypoints (start or end excluded)
        // are closer than 1 mm, nudge the later one by 1 mm to avoid a
        // zero-length segment and the associated sqrt(0) singularity.
        for (int i = 1; i < N - 1; ++i)
        {
            double dx = traj[i].x - traj[i - 1].x;
            double dy = traj[i].y - traj[i - 1].y;
            if (std::hypot(dx, dy) < 1.0)
            {
                double ndx = (i + 1 < N) ? (traj[i + 1].x - traj[i].x) : 1.0;
                double ndy = (i + 1 < N) ? (traj[i + 1].y - traj[i].y) : 0.0;
                double nlen = std::hypot(ndx, ndy);
                if (nlen < 1e-6)
                {
                    ndx = 1.0;
                    ndy = 0.0;
                    nlen = 1.0;
                }
                traj[i].x += 1.0 * ndx / nlen;
                traj[i].y += 1.0 * ndy / nlen;
            }
        }
 
        // Per-waypoint tracking weight scale based on pre-processed target clearance.
        // Strong tracking in tight corridors, fading to zero in open space so the
        // smoothness residual can straighten the trajectory without fighting the
        // pre-processed path.
        std::vector<double> tracking_scales(N, 1.0);
        if (opts.use_tracking_clearance_adaptive &&
            opts.tracking_fade_end_clearance > opts.tracking_fade_start_clearance)
        {
            for (int i = 0; i < N; ++i)
            {
                double d = 0.0;
                costmap_wrapper(&traj_targets[i].x,
                                &traj_targets[i].y,
                                &traj_targets[i].theta,
                                &d);
                tracking_scales[i] = trackingScaleFromClearance(
                    d,
                    opts.tracking_fade_start_clearance,
                    opts.tracking_fade_end_clearance);
            }
        }
 
        // Freeze interior waypoints whose pre-processed clearance is below the
        // threshold. These points are already in a safe corridor centre and
        // should not be moved by the smoother.
        std::vector<bool> freeze_pose(N, false);
        if (opts.use_obstacle_freeze)
        {
            for (int i = 1; i <= N - 2; ++i)
            {
                double d = 0.0;
                costmap_wrapper(&traj_targets[i].x,
                                &traj_targets[i].y,
                                &traj_targets[i].theta,
                                &d);
                freeze_pose[i] = (d <= opts.obstacle_freeze_threshold);
            }
 
            std::string frozen_indices;
            for (int i = 1; i <= N - 2; ++i)
            {
                if (freeze_pose[i])
                {
                    if (!frozen_indices.empty())
                        frozen_indices += ", ";
                    frozen_indices += std::to_string(i);
                }
            }
            ARMARX_INFO << "[Pass 1] Frozen waypoints (clearance <= "
                        << opts.obstacle_freeze_threshold << " mm): ["
                        << (frozen_indices.empty() ? "none" : frozen_indices) << "]";
        }
 
        // ==================== PASS 1: Optimize poses (x, y, theta) ====================
        {
            ceres::Problem problem;
            for (int i = 0; i < N; ++i)
                problem.AddParameterBlock(&traj[i].x, 4);
 
            // Hold start and end fully constant.
            problem.SetParameterBlockConstant(&traj.front().x);
            problem.SetParameterBlockConstant(&traj.back().x);
 
            // In Pass 1 only x, y, theta are optimized; keep v constant on interior waypoints.
            // Waypoints close to obstacles are held fully constant.
            for (int i = 1; i < N - 1; ++i)
            {
                if (freeze_pose[i])
                {
                    problem.SetParameterBlockConstant(&traj[i].x);
                }
                else
                {
                    std::vector<int> constant_v = {3};
                    SET_CONSTRAINT(problem, &traj[i].x, new SubsetConstraint(4, constant_v));
                }
            }
 
            // 1) Pose smoothness
            for (int i = 1; i <= N - 2; ++i)
            {
                problem.AddResidualBlock(
                    new ceres::AutoDiffCostFunction<PoseSmoothResidual, 3, 4, 4, 4>(
                        new PoseSmoothResidual(opts.w_pose_smooth, opts.w_orientation_smooth)),
                    nullptr,
                    &traj[i - 1].x,
                    &traj[i].x,
                    &traj[i + 1].x);
            }
 
            // Start and end rotation smoothness
            problem.AddResidualBlock(
                new ceres::AutoDiffCostFunction<StartRotationResidual, 1, 4, 4>(
                    new StartRotationResidual(opts.w_boundary)),
                nullptr,
                &traj[0].x,
                &traj[1].x);
            problem.AddResidualBlock(
                new ceres::AutoDiffCostFunction<StartRotationResidual, 1, 4, 4>(
                    new StartRotationResidual(opts.w_boundary)),
                nullptr,
                &traj[N - 2].x,
                &traj[N - 1].x);
 
            // 2) Optional jerk residuals
            if (opts.use_jerk && N >= 4)
            {
                for (int i = 2; i <= N - 2; ++i)
                {
                    problem.AddResidualBlock(
                        new ceres::AutoDiffCostFunction<PoseJerkResidual, 3, 4, 4, 4, 4>(
                            new PoseJerkResidual(opts.w_pose_jerk)),
                        nullptr,
                        &traj[i - 2].x,
                        &traj[i - 1].x,
                        &traj[i].x,
                        &traj[i + 1].x);
                }
            }
 
            // 3) Robot smoothness residuals (optional)
            if (opts.use_robot_smooth)
            {
                for (int i = 1; i <= N - 2; ++i)
                {
                    problem.AddResidualBlock(
                        new ceres::AutoDiffCostFunction<RobotSmoothResidual, 3, 4, 4, 4>(
                            new RobotSmoothResidual(opts.w_robot_smooth, -0.5, 0.0)),
                        nullptr,
                        &traj[i - 1].x,
                        &traj[i].x,
                        &traj[i + 1].x);
                }
            }
 
            // 4) Spacing residuals
            double d_avg = computeAverageSpacing(traj);
            if (d_avg <= 1e-9)
                d_avg = 0.1;
            for (int i = 0; i < N - 1; ++i)
            {
                if (opts.use_soft_spacing)
                {
                    problem.AddResidualBlock(
                        new ceres::AutoDiffCostFunction<SoftSpacingResidual, 1, 4, 4>(
                            new SoftSpacingResidual(d_avg,
                                                    opts.w_spacing_linear,
                                                    opts.w_spacing_quadratic,
                                                    opts.spacing_relative_deviation_threshold)),
                        nullptr,
                        &traj[i].x,
                        &traj[i + 1].x);
                }
                else
                {
                    problem.AddResidualBlock(
                        new ceres::AutoDiffCostFunction<SpacingResidual, 1, 4, 4>(
                            new SpacingResidual(d_avg, opts.w_spacing)),
                        nullptr,
                        &traj[i].x,
                        &traj[i + 1].x);
                }
            }
 
            // 5) Obstacle residuals (waypoint + midpoint)
            if (opts.use_obs)
            {
                for (int i = 0; i < N; ++i)
                {
                    problem.AddResidualBlock(
                        new ceres::AutoDiffCostFunction<ObstacleResidual, 1, 4>(
                            new ObstacleResidual(costmap_wrapper,
                                                 opts.w_obs,
                                                 opts.obs_max_distance,
                                                 opts.clearance,
                                                 opts.obs_barrier_eps_mm,
                                                 opts.use_fuzzy_orientation,
                                                 72,
                                                 opts.fuzzy_orientation_window_bins,
                                                 opts.fuzzy_orientation_alpha)),
                        nullptr,
                        &traj[i].x);
                }
                for (int i = 0; i < N - 1; ++i)
                {
                    problem.AddResidualBlock(
                        new ceres::AutoDiffCostFunction<MidpointObstacleResidual, 1, 4, 4>(
                            new MidpointObstacleResidual(costmap_wrapper,
                                                           opts.w_obs,
                                                           opts.obs_max_distance,
                                                           opts.clearance,
                                                           opts.obs_barrier_eps_mm,
                                                           opts.use_fuzzy_orientation,
                                                           72,
                                                           opts.fuzzy_orientation_window_bins,
                                                           opts.fuzzy_orientation_alpha)),
                        nullptr,
                        &traj[i].x,
                        &traj[i + 1].x);
                }
            }
 
            // 5b) Theta tracking: pull interior waypoints toward pre-processed theta
            for (int i = 1; i <= N - 2; ++i)
            {
                problem.AddResidualBlock(
                    new ceres::AutoDiffCostFunction<ThetaTrackingResidual, 1, 4>(
                        new ThetaTrackingResidual(opts.w_theta_track * tracking_scales[i],
                                                  traj_targets[i].theta)),
                    nullptr,
                    &traj[i].x);
            }
 
            // 6) Tracking residuals to original (optional)
            if (opts.use_tracking)
            {
                if (static_cast<int>(traj_targets.size()) != N)
                {
                    ARMARX_INFO << "[Pass 1] traj_targets size mismatch; skipping tracking.\n";
                }
                else
                {
                    for (int i = 0; i < N; ++i)
                    {
                        double wx = opts.w_track * tracking_scales[i];
                        double wy = opts.w_track * tracking_scales[i];
                        double wth = opts.w_track * 300.0 * tracking_scales[i];
                        double wv = opts.w_track * 0.2 * tracking_scales[i];
                        problem.AddResidualBlock(
                            new ceres::AutoDiffCostFunction<TrackingResidual, 4, 4>(
                                new TrackingResidual(traj_targets[i].x,
                                                     traj_targets[i].y,
                                                     traj_targets[i].theta,
                                                     traj_targets[i].v,
                                                     wx,
                                                     wy,
                                                     wth,
                                                     wv)),
                            nullptr,
                            &traj[i].x);
                    }
                }
            }
 
            // 7) Robot-pose proximity to pre-processed trajectory (optional)
            if (opts.use_robot_pose_proximity)
            {
                if (static_cast<int>(traj_targets.size()) != N)
                {
                    ARMARX_INFO << "[Pass 1] traj_targets size mismatch; skipping robot-pose proximity.\n";
                }
                else
                {
                    for (int i = 1; i <= N - 2; ++i)
                    {
                        const double cth = traj_targets[i].theta;
                        const double rx0 = traj_targets[i].x + std::cos(cth) * (-0.5);
                        const double ry0 = traj_targets[i].y + std::sin(cth) * (-0.5);
 
                        problem.AddResidualBlock(
                            new ceres::AutoDiffCostFunction<RobotPoseProximityResidual, 1, 4>(
                                new RobotPoseProximityResidual(rx0,
                                                               ry0,
                                                               opts.robot_pose_proximity_threshold_mm,
                                                               opts.w_robot_pose_proximity
                                                                   * tracking_scales[i],
                                                               opts.robot_pose_proximity_eps_mm)),
                            nullptr,
                            &traj[i].x);
                    }
                }
            }
 
            // Solver options
            ceres::Solver::Options options;
            options.max_num_iterations = opts.max_iterations;
            options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
            options.minimizer_progress_to_stdout = false;
            options.num_threads = opts.num_threads;
            options.function_tolerance = 1e-6;
            options.parameter_tolerance = 1e-8;
            options.trust_region_strategy_type = ceres::LEVENBERG_MARQUARDT;
            options.initial_trust_region_radius = opts.initial_trust_region_radius;
            options.max_trust_region_radius = opts.max_trust_region_radius;
            options.min_trust_region_radius = opts.min_trust_region_radius;
            options.min_relative_decrease = 1e-3;
 
            // Pre-solve validation
            bool valid = true;
            for (int i = 0; i < N; ++i)
            {
                if (!std::isfinite(traj[i].x) || !std::isfinite(traj[i].y) ||
                    !std::isfinite(traj[i].theta) || !std::isfinite(traj[i].v))
                {
                    ARMARX_WARNING << "[Pass 1] NaN/Inf in traj[" << i
                                   << "] before solve: x=" << traj[i].x << " y=" << traj[i].y
                                   << " theta=" << traj[i].theta << " v=" << traj[i].v;
                    valid = false;
                }
            }
            if (!valid)
            {
                ARMARX_WARNING << "[Pass 1] Pre-solve validation FAILED. Aborting.";
                return;
            }
 
            ARMARX_INFO << "[Pass 1] Pre-solve residual breakdown:";
            logResidualBreakdown(traj, costmap_wrapper, traj_targets, tracking_scales, opts);
 
            ceres::Solver::Summary summary;
            ceres::Solve(options, &problem, &summary);
            normalizeTrajectoryAngles(traj);
 
            ARMARX_INFO << "[Pass 1] Post-solve residual breakdown:";
            logResidualBreakdown(traj, costmap_wrapper, traj_targets, tracking_scales, opts);
 
            bool postSolveValid = true;
            for (int i = 0; i < N; ++i)
            {
                if (!std::isfinite(traj[i].x) || !std::isfinite(traj[i].y) ||
                    !std::isfinite(traj[i].theta) || !std::isfinite(traj[i].v))
                {
                    ARMARX_WARNING << "[Pass 1] NaN/Inf in traj[" << i
                                   << "] AFTER solve. Reverting to pre-processed trajectory.";
                    postSolveValid = false;
                }
            }
            if (!postSolveValid)
            {
                for (int i = 0; i < N; ++i)
                    traj[i] = traj_targets[i];
                ARMARX_WARNING << "[Pass 1] Trajectory reverted to pre-processed safe path.";
                return;
            }
 
            ARMARX_INFO << summary.FullReport();
            ARMARX_INFO << "[Pass 1] Done. Final cost: " << summary.final_cost
                        << " iterations: " << summary.num_successful_steps << "\n";
        }
 
        // ==================== PASS 2: Optimize velocities (v) ====================
        {
            // Auto-detect nominal cruise speed from the first interior waypoint
            double v_nominal = (N >= 3) ? traj[1].v : traj.front().v;
 
            // Set start velocity boundary
            traj.front().v = opts.start_velocity;
 
            // Pre-query obstacle danger at each optimized pose
            std::vector<double> dangers(N);
            for (int i = 0; i < N; ++i)
            {
                double d = 0.0;
                costmap_wrapper(&traj[i].x, &traj[i].y, &traj[i].theta, &d);
                dangers[i] = std::clamp(
                    (opts.slow_down_distance - d) / opts.slow_down_distance, 0.0, 1.0);
            }
 
            ceres::Problem problem;
            for (int i = 0; i < N; ++i)
            {
                problem.AddParameterBlock(&traj[i].x, 4);
                // Hold x, y, theta constant; only v (index 3) is free
                std::vector<int> constant_indices = {0, 1, 2};
                SET_CONSTRAINT(
                    problem, &traj[i].x, new SubsetConstraint(4, constant_indices));
            }
 
            // Hold start and end fully constant (including v)
            problem.SetParameterBlockConstant(&traj.front().x);
            problem.SetParameterBlockConstant(&traj.back().x);
 
            // 1) Accel/decel limit residuals (per-segment)
            for (int i = 0; i < N - 1; ++i)
            {
                problem.AddResidualBlock(
                    new ceres::AutoDiffCostFunction<AccelDecelLimitResidual, 1, 4, 4>(
                        new AccelDecelLimitResidual(opts.max_accel_per_segment,
                                                    opts.max_decel_per_segment,
                                                    opts.w_accel_decel_limit)),
                    nullptr,
                    &traj[i].x,
                    &traj[i + 1].x);
            }
 
            // 2) Nominal velocity residual (interior waypoints)
            for (int i = 1; i < N - 1; ++i)
            {
                problem.AddResidualBlock(
                    new ceres::AutoDiffCostFunction<NominalVelocityResidual, 1, 4>(
                        new NominalVelocityResidual(v_nominal, opts.w_nominal_velocity)),
                    nullptr,
                    &traj[i].x);
            }
 
            // 3) Velocity proximity residual (interior waypoints)
            for (int i = 1; i < N - 1; ++i)
            {
                problem.AddResidualBlock(
                    new ceres::AutoDiffCostFunction<VelocityProximityResidual, 1, 4>(
                        new VelocityProximityResidual(dangers[i],
                                                      opts.min_velocity_near_obstacle,
                                                      opts.w_velocity_proximity)),
                    nullptr,
                    &traj[i].x);
            }
 
            // Solver options (velocity pass)
            ceres::Solver::Options options;
            options.max_num_iterations = opts.max_iterations;
            options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
            options.minimizer_progress_to_stdout = false;
            options.num_threads = opts.num_threads;
            options.function_tolerance = 1e-6;
            options.parameter_tolerance = 1e-8;
            options.trust_region_strategy_type = ceres::LEVENBERG_MARQUARDT;
            options.initial_trust_region_radius = opts.initial_trust_region_radius;
            options.max_trust_region_radius = opts.max_trust_region_radius;
            options.min_trust_region_radius = opts.min_trust_region_radius;
            options.min_relative_decrease = 1e-3;
 
            ceres::Solver::Summary summary;
            ceres::Solve(options, &problem, &summary);
 
            ARMARX_INFO << summary.FullReport();
            ARMARX_INFO << "[Pass 2] Done. Final cost: " << summary.final_cost
                        << " iterations: " << summary.num_successful_steps << "\n";
        }
 
        normalizeTrajectoryAngles(traj);
    }
 
} // namespace armarx::navigation::algorithms::orientation_aware::smoothing