toys/gps/docs/source/math/compute_st.cpp

#include "basic/basic.h"
#include "geo/private/math/math.h"
#include "geo/private/math/distance.h"
#include "geo/private/util/angle_util.h"
#include "geo/private/math/compute_st.h"
#include "geo/private/math/ns_geom_math.h"

namespace ns
{
namespace geo
{

// 参考网址： https://github.com/udacity/CarND-Path-Planning-Project/blob/master/src/helpers.h
namespace
{

#if 0
// Calculate closest waypoint to current x, y position
int ClosestWaypoint(double x, double y, const std::vector<double> &maps_x, const std::vector<double> &maps_y)
{
    double closestLen = 100000; // large number
    int closestWaypoint = 0;

    for (int i = 0; i < (int)maps_x.size(); ++i)
    {
        double map_x = maps_x[i];
        double map_y = maps_y[i];
        double dist = flat_distance(x, y, map_x, map_y);
        if (dist < closestLen)
        {
            closestLen = dist;
            closestWaypoint = i;
        }
    }

    return closestWaypoint;
}

// Returns next waypoint of the closest waypoint
int NextWaypoint(double x, double y, double theta, const std::vector<double> &maps_x, const std::vector<double> &maps_y)
{
    int closestWaypoint = ClosestWaypoint(x, y, maps_x, maps_y);

    double map_x = maps_x[closestWaypoint];
    double map_y = maps_y[closestWaypoint];

    double heading = atan2((map_y - y), (map_x - x));

    double angle = fabs(theta - heading);
    angle = std::min(2 * c_pi() - angle, angle);

    if (angle > c_pi_2())
    {
        ++closestWaypoint;
        if (closestWaypoint == (int)maps_x.size())
        {
            closestWaypoint = 0;
        }
    }

    return closestWaypoint;
}

// Transform from Cartesian x,y coordinates to Frenet s,d coordinates
std::vector<double> getFrenet(double x, double y, double theta, const std::vector<double> &maps_x, const std::vector<double> &maps_y)
{
    int next_wp = NextWaypoint(x, y, theta, maps_x, maps_y);

    int prev_wp;
    prev_wp = next_wp - 1;
    if (next_wp == 0)
    {
        prev_wp = maps_x.size() - 1;
    }

    double n_x = maps_x[next_wp] - maps_x[prev_wp];
    double n_y = maps_y[next_wp] - maps_y[prev_wp];
    double x_x = x - maps_x[prev_wp];
    double x_y = y - maps_y[prev_wp];
    //LOG_DEBUG("x: {:.8f}, y: {:.8f}", x, y);

    // find the projection of x onto n
    // TODO 这个没有看明白,现象是可能存在
    double proj_norm = (x_x * n_x + x_y * n_y) / (n_x * n_x + n_y * n_y);
    double proj_x = proj_norm * n_x;
    double proj_y = proj_norm * n_y;
    //LOG_DEBUG("project point: {:.8f}, {:.8f}", proj_x, proj_y);


    double frenet_d = flat_distance(x_x, x_y, proj_x, proj_y);

    // // see if d value is positive or negative by comparing it to a center point
    // double center_x = 1000 - maps_x[prev_wp];
    // double center_y = 2000 - maps_y[prev_wp];
    // double centerToPos = flat_distance(center_x, center_y, x_x, x_y);
    // double centerToRef = flat_distance(center_x, center_y, proj_x, proj_y);

    // if (centerToPos <= centerToRef)
    // {
    //     frenet_d *= -1;
    // }

    // calculate s value
    double frenet_s = 0;
    for (int i = 0; i < prev_wp; ++i)
    {
        frenet_s += flat_distance(maps_x[i], maps_y[i], maps_x[i + 1], maps_y[i + 1]);
    }

    frenet_s += flat_distance(0, 0, proj_x, proj_y);

    return {frenet_s, frenet_d};
}
#endif

#if 1

/**
 * @brief 获取指定点到指定polyline上的最近距离以及polyline上对应的点的坐标
 *
 * @param pts 指定的polyline
 * @param pos 指定的坐标点
 * @param foot polyline上距离pos最近的点
 * @param preIndex
 * @param beginIndex 从beginIndex点开始抓路
 * polyline上foot之前point的index,若foot和polyline上某一点重合,preIndex也是重合点之前点的下标
 * @return double
 */
double grabLine(std::vector<PointDST> pts, const PointD& pos, PointD& foot, int32_t& preIndex,
                int32_t beginIndex = 0)
{
    double dis = basic::DOUBLE_MAX;

    if (pts.size() >= 2)
    {
        for (size_t i = beginIndex; i < pts.size() - 1; ++i)
        {
            PointD tmp;
            double d = pointSegmentDistance(pos, pts[i].p, pts[i + 1].p, tmp);
            if (d < dis)
            {
                dis = d;
                foot = tmp;
                preIndex = static_cast<int32_t>(i);
            }
        }
    }

    return dis;
}

// Transform from Cartesian x,y coordinates to Frenet s,d coordinates
// 输入参考线的点的个数不能小于2,否则结果未知, 目前t应该是DOUBLE_MAX
std::vector<double> getFrenet(const PointD& pos, const std::vector<PointDST>& referLine,
                              int32_t& preIndex, int32_t beginIndex = 0)
{
    PointD foot;
    double frenet_d = grabLine(referLine, pos, foot, preIndex, beginIndex);
    if (frenet_d < basic::DOUBLE_MAX)
    {
        int32_t side = JudgeSideness(referLine[preIndex].p, referLine[preIndex + 1].p, pos);
        if (side < 0)
        {
            frenet_d *= -1;
        }

        double frenet_s = 0;
        frenet_s += referLine[preIndex].s;
        frenet_s += referLine[preIndex].p.distance(foot);
        return {frenet_s, frenet_d};
    }

    return {};
}
#endif

} // namespace

// 求某一个点的st值
std::vector<double> computePointST(const PointD& p, const std::vector<PointDST>& referLine)
{
    if (referLine.size() >= 2)
    {
        int32_t preIndex;
        return getFrenet(p, referLine, preIndex);
    }

    return std::vector<double>();
}

// 求一条参考线的所有st值
int computePolylineST(std::vector<PointDST>& polyline, const std::vector<PointDST>& referLine,
                      bool increase)
{
    if (polyline.size() < 2)
    {
        return -1;
    }

    int32_t preIndex = 0;
    for (size_t i = 0; i < polyline.size(); i++)
    {
        int beginIndex = increase ? preIndex : 0;
        std::vector<double> results = getFrenet(polyline.at(i).p, referLine, preIndex, beginIndex);
        if (results.empty())
            return -1;

        polyline.at(i).s = results[0];
        polyline.at(i).t = results[1];
    }

    return 0;
}

// TODO 要考虑越界以及点之间有重合的问题
// Transform from Frenet s,d coordinates to Cartesian x,y
PointD getXY(double s, double d, const std::vector<PointDST>& referLine)
{
    int prev_wp = -1;
    for (size_t i = 0; i < referLine.size() - 1; i++)
    {
        if (s >= referLine[i].s)
            prev_wp = static_cast<int>(i);
    }
    int wp2 = (prev_wp + 1); // 上面已经确保不会越界

    double heading = atan2((referLine[wp2].p.y - referLine[prev_wp].p.y),
                           (referLine[wp2].p.x - referLine[prev_wp].p.x));
    // the x,y,s along the segment
    double seg_s = (s - referLine[prev_wp].s);

    double seg_x = referLine[prev_wp].p.x + seg_s * cos(heading);
    double seg_y = referLine[prev_wp].p.y + seg_s * sin(heading);

    double perp_heading = heading + c_pi_2();

    PointD pos;
    pos.x = seg_x + d * cos(perp_heading);
    pos.y = seg_y + d * sin(perp_heading);
    pos.z = referLine[prev_wp].p.z;

    return pos;
}

} // namespace geo
} // namespace ns