Smoothing.cpp

Go to the documentation of this file.

#include "Smoothing.h"
 
#include <cmath>
#include <iomanip>
//#include <ios>
#include <vector>
 
#include <Eigen/Geometry>
 
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/util/StringHelpers.h>
 
#include <armarx/navigation/algorithms/orientation_aware/Costmap3D.h>
#include <armarx/navigation/algorithms/orientation_aware/TrajectoryChecker.h>
#include <armarx/navigation/algorithms/orientation_aware/smoothing/Costmap3DWrapper.h>
#include <armarx/navigation/algorithms/orientation_aware/smoothing/io.h>
#include <armarx/navigation/algorithms/orientation_aware/smoothing/residuals.h>
#include <armarx/navigation/conversions/eigen.h>
#include <armarx/navigation/core/Trajectory.h>
 
namespace armarx::navigation::algorithms::orientation_aware::smoothing
{
    Smoothing::Smoothing(const core::GlobalTrajectory& trajectory,
                         const Costmap3D& costmap3d,
                         const Params& params) :
        trajectory(trajectory), costmap3d(costmap3d), params(params)
    {
    }
 
    // convert back to global trajectory
    armarx::navigation::core::GlobalTrajectory
    toGlobalTrajectory(const std::vector<CenterPoint>& traj)
    {
        armarx::navigation::core::GlobalTrajectory gtraj;
        for (const auto& p : traj)
        {
            armarx::navigation::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;
    }
 
    // convert global trajectory to trajectory for smoothing
    std::vector<CenterPoint>
    toCenterTrajectory(const armarx::navigation::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;
    }
 
    static double
    queryDistance(const Costmap3DWrapper& wrapper, double x, double y, double theta)
    {
        double out;
        wrapper(&x, &y, &theta, &out);
        return out;
    }
 
    // Pre-process: push interior waypoints into the safe zone by hill-climbing
    // on the signed-distance field.  The SDF's local maxima are at corridor
    // centres, so this naturally places waypoints in the middle of free space.
    static void
    pushWaypointsToSafeZone(std::vector<CenterPoint>& traj,
                            const Costmap3DWrapper& wrapper,
                            const Costmap3D& costmap,
                            double clearance,
                            double stepSize,
                            int maxIterations)
    {
        // 8-neighbour offsets plus zero (stay in place)
        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},
        };
 
        auto isInsideBounds = [&costmap](double x, double y) -> bool
        {
            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();
        };
 
        for (int idx = 1; idx + 1 < static_cast<int>(traj.size()); ++idx)
        {
            CenterPoint& p = traj[idx];
            double bestDist = queryDistance(wrapper, p.x, p.y, p.theta);
 
            if (bestDist >= clearance)
                continue; // already safe
 
            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))
                        continue; // reject out-of-bounds candidates
                    double candDist = queryDistance(wrapper, candX, candY, p.theta);
                    if (candDist > bestDist)
                    {
                        bestDist = candDist;
                        bestX = candX;
                        bestY = candY;
                        improved = true;
                    }
                }
 
                if (!improved)
                {
                    // Local maximum (narrow corridor).  Stay at best available.
                    break;
                }
 
                p.x = bestX;
                p.y = bestY;
 
                if (bestDist >= clearance)
                    break; // reached safe zone
            }
        }
    }
 
    Smoothing::OptimizationResult
    Smoothing::optimize()
    {
        Costmap3DWrapper costmapWrapper{costmap3d};
 
        std::vector<CenterPoint> traj;
        {
            traj = toCenterTrajectory(trajectory);
        }
 
        // Stage 1: Pre-process waypoints into the safe zone.
        // The SDF's local maxima are at corridor centres, so hill-climbing
        // naturally places waypoints in the middle of free space.
        pushWaypointsToSafeZone(traj,
                                costmapWrapper,
                                costmap3d,
                                params.clearance,
                                params.hill_climb_step_size,
                                params.hill_climb_max_iterations);
 
        // Stage 2: Ceres smoothing with the safe positions as tracking targets.
        // Tracking now cooperates with obstacle avoidance because it pulls
        // toward an already-safe path.
        std::vector<CenterPoint> traj_targets = traj;
        optimizeTrajectoryCeres(traj, costmapWrapper, traj_targets, params);
 
        TrajectoryChecker checker(costmap3d);
        auto checkResult = checker.check(toGlobalTrajectory(traj));
        bool collisionFree = checkResult.isCollisionFree();
        if (collisionFree)
        {
            ARMARX_INFO << "Smoothed trajectory is collision-free.";
        }
        else
        {
            ARMARX_WARNING << "Smoothed trajectory still has collision after pre-processing.";
        }
 
        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;
        }
 
        return {toGlobalTrajectory(traj), toGlobalTrajectory(traj_targets), collisionFree};
    }
 
} // namespace armarx::navigation::algorithms::orientation_aware::smoothing