CircularPathSmoothing.cpp

Go to the documentation of this file.

#include "CircularPathSmoothing.h"
 
#include <algorithm>
#include <cmath>
#include <cstddef>
#include <cstdlib>
#include <type_traits>
 
#include <armarx/navigation/conversions/eigen.h>
#include <armarx/navigation/core/Trajectory.h>
 
namespace
{
    template <typename T>
    constexpr T
    deg2rad(T deg)
    {
        static_assert(std::is_floating_point<T>::value);
        constexpr T c = M_PI / 180;
        return deg * c;
    }
} // namespace
 
namespace armarx::navigation::algorithm
{
    namespace
    {
        // TODO move to Simox
        float
        getAngleBetweenVectors(const Eigen::Vector2f& v1, const Eigen::Vector2f& v2)
        {
            // Returns an angle between 0 and 180 degree (the minimum angle between the two vectors)
            const Eigen::Vector2f v1_vec_normed = v1.normalized();
            const Eigen::Vector2f v2_vec_normed = v2.normalized();
            const float dot_product_vec =
                v1_vec_normed(0) * v2_vec_normed(0) + v1_vec_normed(1) * v2_vec_normed(1);
            float yaw = acos(dot_product_vec);
            return yaw;
        }
    } // namespace
 
    CircularPathSmoothing::Points
    CircularPathSmoothing::smooth(const Points& p)
    {
        Points points;
 
        points.push_back(p.front());
 
        // omit start and end
        for (size_t i = 1; i < (p.size() - 1); i++)
        {
            const Eigen::Vector2f& prev = p.at(i - 1);
            const Eigen::Vector2f& at = p.at(i);
            const Eigen::Vector2f& next = p.at(i + 1);
 
            // the largest radius for smoothing
            const float maxRadius = std::min((prev - at).norm(), (next - at).norm());
            const float radius = std::min(maxRadius, 300.F);
 
            const Eigen::Vector2f vIn = (prev - at).normalized();
            const Eigen::Vector2f vOut = (next - at).normalized();
 
            // the angle between the lines
            const float phi = getAngleBetweenVectors(vIn, vOut);
            const float l = radius / std::tan(phi / 2);
 
            points.emplace_back(at + Eigen::Vector2f(prev - at).normalized() * l);
            points.emplace_back(at + Eigen::Vector2f(next - at).normalized() * l);
        }
 
        points.push_back(p.back());
 
        return points;
    }
 
    core::GlobalTrajectory
    CircularPathSmoothing::smooth(const core::GlobalTrajectory& trajectory)
    {
        core::GlobalTrajectoryPoints points;
        const auto& p = trajectory.points();
 
        points.push_back(p.front());
 
        // omit start and end
        for (size_t i = 1; i < (p.size() - 1); i++)
        {
            const Eigen::Vector2f& prev =
                navigation::conv::to2D(p.at(i - 1).waypoint.pose.translation());
            const Eigen::Vector2f& at = navigation::conv::to2D(p.at(i).waypoint.pose.translation());
            const Eigen::Vector2f& next =
                navigation::conv::to2D(p.at(i + 1).waypoint.pose.translation());
 
            // the largest radius for smoothing
            const float maxRadius = std::min((prev - at).norm(), (next - at).norm());
            const float radius = std::min(maxRadius, 300.F);
 
            const Eigen::Vector2f vIn = (prev - at).normalized();
            const Eigen::Vector2f vOut = (next - at).normalized();
 
            // the angle between the lines
            const float phi = getAngleBetweenVectors(vIn, vOut);
 
            // decide whether it is necessary to smooth the path
            if (std::abs(phi) < deg2rad(10.F))
            {
                points.push_back(p.at(i));
            }
            else
            {
                const float l = radius / std::tan(phi / 2);
 
                const Eigen::Vector2f prePos = at + Eigen::Vector2f(prev - at).normalized() * l;
                const Eigen::Vector2f postPos = at + Eigen::Vector2f(next - at).normalized() * l;
 
                const auto tpProj = [&](const Eigen::Vector2f& pos) -> core::GlobalTrajectoryPoint
                {
                    const auto proj =
                        trajectory.getProjection(navigation::conv::to3D(pos),
                                                 core::VelocityInterpolation::LinearInterpolation);
 
                    return proj.projection;
                };
 
                points.push_back(tpProj(prePos));
                points.push_back(tpProj(postPos));
            }
        }
 
        points.push_back(p.back());
 
        return points;
    }
} // namespace armarx::navigation::algorithm