SPFASmoothing.cpp

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/>.
 */
 
#include "SPFASmoothing.h"
 
#include <array>
#include <cmath>
#include <iomanip>
#include <string>
#include <vector>
 
#include <Eigen/Geometry>
 
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/util/StringHelpers.h>
 
#include <armarx/navigation/algorithms/Costmap.h>
#include <armarx/navigation/algorithms/spfa/smoothing/Costmap2DWrapper.h>
#include <armarx/navigation/algorithms/spfa/smoothing/io.h>
#include <armarx/navigation/algorithms/spfa/smoothing/residuals.h>
#include <armarx/navigation/algorithms/spfa/smoothing/TrajectoryChecker2D.h>
#include <armarx/navigation/conversions/eigen.h>
#include <armarx/navigation/core/Trajectory.h>
 
namespace armarx::navigation::algorithms::spfa::smoothing
{
    namespace
    {
        using Params = io::SmoothingParams;
 
        core::GlobalTrajectory toGlobalTrajectory(const std::vector<CenterPoint>& traj)
        {
            core::GlobalTrajectory gtraj;
            for (const auto& p : traj)
            {
                core::GlobalTrajectoryPoint gp;
                gp.waypoint.pose =
                    Eigen::Translation3f{Eigen::Vector3f(static_cast<float>(p.x),
                                                         static_cast<float>(p.y),
                                                         0.0f)} *
                    Eigen::AngleAxisf{static_cast<float>(p.theta), Eigen::Vector3f::UnitZ()};
                gp.velocity = static_cast<float>(p.v);
                gtraj.mutablePoints().push_back(gp);
            }
            return gtraj;
        }
 
        std::vector<CenterPoint> toCenterTrajectory(const core::GlobalTrajectory& gtraj)
        {
            std::vector<CenterPoint> traj;
            for (const auto& gp : gtraj.points())
            {
                CenterPoint p;
                p.x = gp.waypoint.pose.translation().x();
                p.y = gp.waypoint.pose.translation().y();
                const auto& R = gp.waypoint.pose.rotation();
                p.theta = std::atan2(R(1, 0), R(0, 0));
                p.v = gp.velocity;
                traj.push_back(p);
            }
            return traj;
        }
 
        double queryDistance(const Costmap2DWrapper& wrapper, double x, double y)
        {
            double out;
            wrapper(&x, &y, &out);
            return out;
        }
 
        bool isInsideBounds(double x, double y, const algorithms::Costmap& costmap)
        {
            const auto local = costmap.origin().inverse() * Eigen::Vector2f{static_cast<float>(x),
                                                                            static_cast<float>(y)};
            const auto& bounds = costmap.getLocalSceneBounds();
            return local.x() >= bounds.min.x() && local.x() <= bounds.max.x() &&
                   local.y() >= bounds.min.y() && local.y() <= bounds.max.y();
        }
 
        void pushWaypointsToSafeZone(std::vector<CenterPoint>& traj,
                                     const Costmap2DWrapper& wrapper,
                                     const algorithms::Costmap& costmap,
                                     const std::vector<CenterPoint>& originalTargets,
                                     double clearance,
                                     double stepSize,
                                     int maxIterations,
                                     double maxDeviation)
        {
            const std::vector<std::pair<double, double>> neighbours = {
                {0.0, 0.0},
                {stepSize, 0.0},
                {-stepSize, 0.0},
                {0.0, stepSize},
                {0.0, -stepSize},
                {stepSize, stepSize},
                {stepSize, -stepSize},
                {-stepSize, stepSize},
                {-stepSize, -stepSize},
            };
 
            for (int idx = 1; idx + 1 < static_cast<int>(traj.size()); ++idx)
            {
                CenterPoint& p = traj[idx];
                const double origX = originalTargets[idx].x;
                const double origY = originalTargets[idx].y;
 
                auto withinDeviation = [&](double x, double y)
                {
                    return std::hypot(x - origX, y - origY) <= maxDeviation + 1e-6;
                };
 
                double bestDist = queryDistance(wrapper, p.x, p.y);
 
                if (bestDist >= clearance)
                    continue;
 
                for (int iter = 0; iter < maxIterations; ++iter)
                {
                    double bestX = p.x;
                    double bestY = p.y;
                    bool improved = false;
 
                    for (const auto& [dx, dy] : neighbours)
                    {
                        double candX = p.x + dx;
                        double candY = p.y + dy;
                        if (!isInsideBounds(candX, candY, costmap))
                            continue;
                        if (!withinDeviation(candX, candY))
                            continue;
                        double candDist = queryDistance(wrapper, candX, candY);
                        if (candDist > bestDist)
                        {
                            bestDist = candDist;
                            bestX = candX;
                            bestY = candY;
                            improved = true;
                        }
                    }
 
                    if (!improved)
                        break;
 
                    p.x = bestX;
                    p.y = bestY;
 
                    if (bestDist >= clearance)
                        break;
                }
 
                // Final safety projection: if still in collision, snap to the closest
                // collision-free cell within the allowed deviation.
                if (queryDistance(wrapper, p.x, p.y) <= 0.0)
                {
                    const auto vertex = costmap.findClosestCollisionFreeVertex(
                        Eigen::Vector2f{static_cast<float>(origX), static_cast<float>(origY)},
                        static_cast<float>(maxDeviation));
 
                    if (vertex && withinDeviation(vertex->position.x(), vertex->position.y()))
                    {
                        p.x = vertex->position.x();
                        p.y = vertex->position.y();
                    }
                }
            }
        }
 
        bool validateTrajectory(const std::vector<CenterPoint>& traj, const std::string& label)
        {
            bool valid = true;
            for (int i = 0; i < static_cast<int>(traj.size()); ++i)
            {
                if (!std::isfinite(traj[i].x) || !std::isfinite(traj[i].y) ||
                    !std::isfinite(traj[i].theta) || !std::isfinite(traj[i].v))
                {
                    ARMARX_WARNING << "[" << label << "] NaN/Inf in traj[" << i
                                   << "] : x=" << traj[i].x << " y=" << traj[i].y
                                   << " theta=" << traj[i].theta << " v=" << traj[i].v;
                    valid = false;
                }
            }
            return valid;
        }
 
        ceres::Solver::Options makeSolverOptions(const Params& params, int maxIterations)
        {
            ceres::Solver::Options options;
            options.max_num_iterations = maxIterations;
            options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
            options.minimizer_progress_to_stdout = false;
            options.num_threads = params.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 = params.initial_trust_region_radius;
            options.max_trust_region_radius = params.max_trust_region_radius;
            options.min_trust_region_radius = params.min_trust_region_radius;
            options.min_relative_decrease = 1e-4;
            options.use_nonmonotonic_steps = true;
            return options;
        }
 
        struct PassWeights
        {
            double w_obs;
            double w_pose_smooth;
            double w_spacing;
            double w_tracking;
            double w_safe_tracking;
        };
 
        double evalObstacleCost(const Costmap2DWrapper& wrapper,
                                double x,
                                double y,
                                double max_dist,
                                double clearance)
        {
            double d_raw;
            wrapper(&x, &y, &d_raw);
            if (!std::isfinite(d_raw))
                return 100.0;
 
            double obs_dist = d_raw - clearance;
            if (obs_dist > max_dist)
                return 0.0;
            if (obs_dist < 0.0)
            {
                double penetration = -obs_dist;
                double max_pen = 3.0 * max_dist;
                if (penetration > max_pen)
                    penetration = max_pen;
                double ratio = penetration / max_dist;
                return std::exp(2.0 * ratio) - 1.0 + 0.1 * ratio;
            }
            return (max_dist - obs_dist) / max_dist;
        }
 
        void logResidualBreakdown(const std::vector<CenterPoint>& traj,
                                  const Costmap2DWrapper& wrapper,
                                  const std::vector<CenterPoint>& originalTargets,
                                  const std::vector<CenterPoint>& safeTargets,
                                  const Params& params,
                                  const std::string& label)
        {
            const int N = static_cast<int>(traj.size());
            if (N < 4)
                return;
 
            double c_obs_wp = 0.0;
            double c_obs_seg = 0.0;
            double c_smooth = 0.0;
            double c_tracking = 0.0;
            double c_safe_tracking = 0.0;
            double c_spacing = 0.0;
 
            // Obstacle (waypoints)
            for (int i = 0; i < N; ++i)
            {
                double r = evalObstacleCost(wrapper, traj[i].x, traj[i].y, params.obs_max_distance, params.clearance);
                c_obs_wp += r * r;
            }
 
            // Obstacle (segment samples)
            for (int i = 0; i < N - 1; ++i)
            {
                for (int s = 1; s <= params.segment_obstacle_samples; ++s)
                {
                    double t = static_cast<double>(s) / (params.segment_obstacle_samples + 1);
                    double mx = (1.0 - t) * traj[i].x + t * traj[i + 1].x;
                    double my = (1.0 - t) * traj[i].y + t * traj[i + 1].y;
                    double r = evalObstacleCost(wrapper, mx, my, params.obs_max_distance, params.clearance);
                    c_obs_seg += r * r;
                }
            }
 
            // Position 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;
                c_smooth += rx * rx + ry * ry;
            }
 
            // Bounded tracking to safe path
            for (int i = 1; i <= N - 2; ++i)
            {
                double dx = traj[i].x - safeTargets[i].x;
                double dy = traj[i].y - safeTargets[i].y;
                double d = std::hypot(dx, dy);
                double excess = d - params.tracking_deadzone;
                if (excess > 0)
                {
                    if (excess <= params.tracking_max_deviation - params.tracking_deadzone)
                    {
                        double r = params.w_tracking_in_bounds * excess;
                        c_tracking += r * r;
                    }
                    else
                    {
                        double over = excess - (params.tracking_max_deviation - params.tracking_deadzone);
                        double r = params.w_tracking_in_bounds * (params.tracking_max_deviation - params.tracking_deadzone)
                                   + params.w_tracking_hard * over;
                        c_tracking += r * r;
                    }
                }
            }
 
            // Soft tracking to original path
            for (int i = 1; i <= N - 2; ++i)
            {
                double rx = traj[i].x - originalTargets[i].x;
                double ry = traj[i].y - originalTargets[i].y;
                // Use pass3 weight for breakdown (representative)
                double w = params.pass3_w_tracking;
                c_safe_tracking += (w * rx) * (w * rx) + (w * ry) * (w * ry);
            }
 
            // Spacing
            double d_avg = computeAverageSpacing(traj);
            if (d_avg <= 1e-9)
                d_avg = 0.1;
            for (int i = 0; i < N - 1; ++i)
            {
                double dx = traj[i + 1].x - traj[i].x;
                double dy = traj[i + 1].y - traj[i].y;
                double dist_sq = dx * dx + dy * dy;
                double r = params.pass3_w_spacing * (dist_sq - d_avg * d_avg);
                c_spacing += r * r;
            }
 
            double total = c_obs_wp + c_obs_seg + c_smooth + c_tracking + c_safe_tracking + c_spacing;
 
            ARMARX_INFO << "=== Residual breakdown [" << label << "] ===";
            ARMARX_INFO << "Obstacle (waypoints): " << std::scientific << std::setprecision(3) << c_obs_wp;
            ARMARX_INFO << "Obstacle (segments):  " << std::scientific << std::setprecision(3) << c_obs_seg;
            ARMARX_INFO << "Position smoothness:  " << std::scientific << std::setprecision(3) << c_smooth;
            ARMARX_INFO << "Bounded tracking:     " << std::scientific << std::setprecision(3) << c_tracking;
            ARMARX_INFO << "Soft tracking:        " << std::scientific << std::setprecision(3) << c_safe_tracking;
            ARMARX_INFO << "Spacing:              " << std::scientific << std::setprecision(3) << c_spacing;
            ARMARX_INFO << "TOTAL:                " << std::scientific << std::setprecision(3) << total;
        }
 
        void runPositionPass(std::vector<CenterPoint>& traj,
                             const Costmap2DWrapper& wrapper,
                             const std::vector<CenterPoint>& originalTargets,
                             const std::vector<CenterPoint>& safeTargets,
                             const PassWeights& w,
                             const Params& params,
                             int maxIterations)
        {
            const int N = static_cast<int>(traj.size());
            if (N < 4)
            {
                ARMARX_INFO << "[SPFA smoothing] need at least 4 points for position pass";
                return;
            }
 
            normalizeTrajectoryAngles(traj);
 
            // Avoid zero-length segments.
            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;
                }
            }
 
            ceres::Problem problem;
            for (int i = 0; i < N; ++i)
                problem.AddParameterBlock(&traj[i].x, 4);
 
            problem.SetParameterBlockConstant(&traj.front().x);
            problem.SetParameterBlockConstant(&traj.back().x);
 
            // Position pass: only x, y are free.
            for (int i = 1; i < N - 1; ++i)
            {
                std::vector<int> constant_indices = {2, 3}; // theta, v
                SET_CONSTRAINT(problem, &traj[i].x, new SubsetConstraint(4, constant_indices));
            }
 
            // 1) Position smoothness
            for (int i = 1; i <= N - 2; ++i)
            {
                problem.AddResidualBlock(
                    new ceres::AutoDiffCostFunction<PositionSmoothResidual, 2, 4, 4, 4>(
                        new PositionSmoothResidual(w.w_pose_smooth)),
                    nullptr,
                    &traj[i - 1].x,
                    &traj[i].x,
                    &traj[i + 1].x);
            }
 
            // 2) Spacing
            double d_avg = computeAverageSpacing(traj);
            if (d_avg <= 1e-9)
                d_avg = 0.1;
            for (int i = 0; i < N - 1; ++i)
            {
                problem.AddResidualBlock(
                    new ceres::AutoDiffCostFunction<SpacingResidual, 1, 4, 4>(
                        new SpacingResidual(d_avg, w.w_spacing)),
                    nullptr,
                    &traj[i].x,
                    &traj[i + 1].x);
            }
 
            // 3) Obstacle residuals (waypoints)
            for (int i = 0; i < N; ++i)
            {
                problem.AddResidualBlock(
                    new ceres::AutoDiffCostFunction<ObstacleResidual2D, 1, 4>(
                        new ObstacleResidual2D(wrapper, w.w_obs, params.obs_max_distance, params.clearance)),
                    nullptr,
                    &traj[i].x);
            }
 
            // 4) Obstacle residuals along segments (interior samples)
            for (int i = 0; i < N - 1; ++i)
            {
                for (int s = 1; s <= params.segment_obstacle_samples; ++s)
                {
                    double t = static_cast<double>(s) / (params.segment_obstacle_samples + 1);
                    problem.AddResidualBlock(
                        new ceres::AutoDiffCostFunction<SegmentObstacleResidual2D, 1, 4, 4>(
                            new SegmentObstacleResidual2D(wrapper, w.w_obs, params.obs_max_distance, params.clearance, t)),
                        nullptr,
                        &traj[i].x,
                        &traj[i + 1].x);
                }
            }
 
            // 5) Bounded tracking to the safe (hill-climbed) path
            for (int i = 1; i <= N - 2; ++i)
            {
                problem.AddResidualBlock(
                    new ceres::AutoDiffCostFunction<BoundedTrackingResidual, 1, 4>(
                        new BoundedTrackingResidual(safeTargets[i].x,
                                                    safeTargets[i].y,
                                                    params.tracking_deadzone,
                                                    params.tracking_max_deviation,
                                                    params.w_tracking_in_bounds,
                                                    params.w_tracking_hard)),
                    nullptr,
                    &traj[i].x);
            }
 
            // 6) Soft tracking to the original SPFA path (weak, keeps deviation small)
            if (w.w_tracking > 0.0)
            {
                for (int i = 1; i <= N - 2; ++i)
                {
                    problem.AddResidualBlock(
                        new ceres::AutoDiffCostFunction<SoftTrackingResidual, 2, 4>(
                            new SoftTrackingResidual(originalTargets[i].x, originalTargets[i].y, w.w_tracking)),
                        nullptr,
                        &traj[i].x);
                }
            }
 
            ceres::Solver::Options options = makeSolverOptions(params, maxIterations);
            ceres::Solver::Summary summary;
            ceres::Solve(options, &problem, &summary);
 
            normalizeTrajectoryAngles(traj);
 
            ARMARX_INFO << summary.FullReport();
            ARMARX_INFO << "[Position pass] Final cost: " << summary.final_cost
                        << " iterations: " << summary.num_successful_steps
                        << " termination: " << summary.termination_type;
        }
 
        bool repairWaypoints(std::vector<CenterPoint>& traj,
                             const algorithms::Costmap& costmap,
                             const Costmap2DWrapper& wrapper,
                             const std::vector<CenterPoint>& originalTargets,
                             const Params& params)
        {
            const int N = static_cast<int>(traj.size());
 
            for (int i = 1; i < N - 1; ++i)
            {
                double d = queryDistance(wrapper, traj[i].x, traj[i].y);
                if (d > params.clearance)
                    continue;
 
                const auto vertex = costmap.findClosestCollisionFreeVertex(
                    Eigen::Vector2f{static_cast<float>(originalTargets[i].x),
                                    static_cast<float>(originalTargets[i].y)},
                    static_cast<float>(params.tracking_max_deviation));
 
                if (!vertex)
                    return false;
 
                const double dx = vertex->position.x() - originalTargets[i].x;
                const double dy = vertex->position.y() - originalTargets[i].y;
                if (std::hypot(dx, dy) > params.tracking_max_deviation + 1e-3)
                    return false;
 
                traj[i].x = vertex->position.x();
                traj[i].y = vertex->position.y();
            }
 
            return true;
        }
 
        double computeMaxDeviation(const std::vector<CenterPoint>& traj,
                                   const std::vector<CenterPoint>& originalTargets)
        {
            double maxDev = 0.0;
            for (std::size_t i = 0; i < traj.size(); ++i)
            {
                double dx = traj[i].x - originalTargets[i].x;
                double dy = traj[i].y - originalTargets[i].y;
                maxDev = std::max(maxDev, std::hypot(dx, dy));
            }
            return maxDev;
        }
    } // namespace
 
    SPFASmoothing::SPFASmoothing(const core::GlobalTrajectory& trajectory,
                                 const algorithms::Costmap& costmap,
                                 const Params& params) :
        trajectory(trajectory),
        costmap(costmap),
        params(params)
    {
    }
 
    SPFASmoothing::OptimizationResult SPFASmoothing::optimize()
    {
        Costmap2DWrapper wrapper{costmap};
 
        std::vector<CenterPoint> traj = toCenterTrajectory(trajectory);
        const std::vector<CenterPoint> originalTargets = traj;
 
        if (!validateTrajectory(traj, "pre-hill-climb"))
        {
            ARMARX_WARNING << "[SPFA smoothing] Invalid trajectory before pre-processing. Aborting.";
            return {std::nullopt, std::nullopt, false};
        }
 
        pushWaypointsToSafeZone(traj,
                                wrapper,
                                costmap,
                                originalTargets,
                                params.clearance,
                                params.hill_climb_step_size,
                                params.hill_climb_max_iterations,
                                params.tracking_max_deviation);
 
        const std::vector<CenterPoint> safeTargets = traj;
 
        if (!validateTrajectory(traj, "post-hill-climb"))
        {
            ARMARX_WARNING << "[SPFA smoothing] Invalid trajectory after pre-processing. Aborting.";
            return {std::nullopt, std::nullopt, false};
        }
 
        logResidualBreakdown(traj, wrapper, originalTargets, safeTargets, params, "after pre-processing");
 
        // Multi-pass position optimization.
        const std::array<PassWeights, 3> passes = {
            PassWeights{params.pass1_w_obs,
                        params.pass1_w_pose_smooth,
                        params.pass1_w_spacing,
                        params.pass1_w_tracking,
                        params.pass1_w_safe_tracking},
            PassWeights{params.pass2_w_obs,
                        params.pass2_w_pose_smooth,
                        params.pass2_w_spacing,
                        params.pass2_w_tracking,
                        params.pass2_w_safe_tracking},
            PassWeights{params.pass3_w_obs,
                        params.pass3_w_pose_smooth,
                        params.pass3_w_spacing,
                        params.pass3_w_tracking,
                        params.pass3_w_safe_tracking}};
 
        for (size_t pass = 0; pass < passes.size(); ++pass)
        {
            ARMARX_INFO << "[SPFA smoothing] Starting position pass " << (pass + 1);
            runPositionPass(traj, wrapper, originalTargets, safeTargets, passes[pass], params, params.max_iterations);
            logResidualBreakdown(traj, wrapper, originalTargets, safeTargets, params,
                                 "after position pass " + std::to_string(pass + 1));
 
            if (!validateTrajectory(traj, "post-position-pass-" + std::to_string(pass + 1)))
            {
                ARMARX_WARNING << "[SPFA smoothing] NaN/Inf after position pass " << (pass + 1)
                               << ". Reverting to pre-processed safe path.";
                traj = safeTargets;
                break;
            }
        }
 
        normalizeTrajectoryAngles(traj);
 
        // Collision repair if needed.
        TrajectoryChecker2D checker(costmap, params.clearance);
        core::GlobalTrajectory smoothedTrajectory = toGlobalTrajectory(traj);
        bool collisionFree = checker.check(smoothedTrajectory);
 
        if (!collisionFree)
        {
            ARMARX_WARNING << "[SPFA smoothing] Initial smoothing has collisions. Running repair pass.";
 
            if (!repairWaypoints(traj, costmap, wrapper, originalTargets, params))
            {
                ARMARX_ERROR << "[SPFA smoothing] Repair failed: could not project colliding waypoints "
                                "to collision-free positions within max deviation.";
            }
            else
            {
                PassWeights repairWeights{params.repair_w_obs,
                                          params.repair_w_pose_smooth,
                                          params.repair_w_spacing,
                                          params.repair_w_tracking,
                                          params.repair_w_safe_tracking};
                runPositionPass(traj, wrapper, originalTargets, safeTargets, repairWeights, params, params.repair_max_iterations);
                logResidualBreakdown(traj, wrapper, originalTargets, safeTargets, params, "after repair pass");
 
                smoothedTrajectory = toGlobalTrajectory(traj);
                collisionFree = checker.check(smoothedTrajectory);
            }
        }
 
        const double maxDeviation = computeMaxDeviation(traj, originalTargets);
        const bool deviationOk = maxDeviation <= params.tracking_max_deviation + 1e-3;
 
        if (collisionFree && deviationOk)
        {
            ARMARX_INFO << "[SPFA smoothing] Smoothed trajectory is collision-free. Max deviation: "
                        << maxDeviation << " mm";
        }
        else
        {
            if (!collisionFree)
                ARMARX_WARNING << "[SPFA smoothing] Smoothed trajectory still has collisions after repair.";
            if (!deviationOk)
                ARMARX_WARNING << "[SPFA smoothing] Max deviation too large: " << maxDeviation
                               << " mm (limit: " << params.tracking_max_deviation << " mm).";
        }
 
        for (int i = 0; i < static_cast<int>(traj.size()); ++i)
        {
            ARMARX_DEBUG << std::fixed << std::setprecision(3) << "i=" << i << " : x=" << traj[i].x
                         << " y=" << traj[i].y << " theta=" << traj[i].theta << " v=" << traj[i].v;
        }
 
        const core::GlobalTrajectory preprocessedTrajectory = toGlobalTrajectory(safeTargets);
        const bool success = collisionFree && deviationOk;
        return {smoothedTrajectory, preprocessedTrajectory, success};
    }
 
} // namespace armarx::navigation::algorithms::spfa::smoothing