【基础函数-线性插值】

线性插值是一种简单的插值方法，它连接两个已知数据点之间的线段，并根据需要在这两点之间生成新的数据点。线性插值基于这样的假设：两个已知点之间的数据变化是线性的。在泊车规划领域经常会碰到已知两个点（起点和终点）生成中间离散点的需求，这时候就需要线性插值得到中间点的坐标，插值时也分是等间隔距离插值还是按照轨迹点的数量插值。

线性插值基本理论

线性插值是指插值函数为一次多项式的插值方式，已知坐标 (x0​,y0​),(x1​,y1​)，求区间 [x0​,x1​]中间一个x位置在直线上的值，使用线性插值的方法如下，两点式直线方程可写为：



这个比较好理解，但是如果我们按照此类方式插值，那么如果是按照固定轨迹点数量的方式插值这样直接插值是没有问题的，但实际上我们用的比较多的是按照固定间隔的插值方式，这样x值的间隔怎么确定才能保证轨迹点的间隔是一样的呢？不太好确定，所以我采用相似三角形的原理，先将两个点之间的距离计算出来，按照要求的轨迹点的间隔值确定每一段的s，利用每一段的s计算当前的x和y，这样就避免了先确定x的问题，这个和一个点沿着直线或者线段移动一定距离得到另一个点有点像。

   void CalLinePath(PathPoint init_coord, PathPoint end_coord, int count, std::vector<PathPoint> &Points)
    {
        PathPoint Point;
        Points.clear();
        double dx = (end_coord.x - init_coord.x) / (count - 1);
        double dy = (end_coord.y - init_coord.y) / (count - 1);
        for (size_t i = 0; i < count; i++)
        {
            Point.x = init_coord.x + dx * i;
            Point.y = init_coord.y + dy * i;
            Point.yaw = init_coord.yaw;
            Point.k = 0;
            //  Point.dir = -1;
            Points.emplace_back(Point);
        }
    }

    void CalLinePath(PathPoint init_coord, PathPoint end_coord, double dis, std::vector<PathPoint> &Points)
    {
        PathPoint Point;
        Points.clear();
        double AB_x = end_coord.x - init_coord.x;
        double AB_y = end_coord.y - init_coord.y;
        double AB = std::sqrt(AB_x * AB_x + AB_y * AB_y);
        int count = AB / dis;
        double dx = (end_coord.x - init_coord.x) / (count - 1);
        double dy = (end_coord.y - init_coord.y) / (count - 1);
        for (size_t i = 0; i <= count; i++)
        {
            Point = interpolate(init_coord, end_coord, i * dis);
            Point.yaw = init_coord.yaw;
            Point.k = 0;
            //  Point.dir = -1;
            Points.emplace_back(Point);
        }
    }

    PathPoint interpolate(PathPoint A, PathPoint B, double dis)
    {

        double AB_x = B.x - A.x;

        double AB_y = B.y - A.y;

        double AB = std::sqrt(AB_x * AB_x + AB_y * AB_y);

        double C_x = A.x + (dis / AB) * AB_x;

        double C_y = A.y + (dis / AB) * AB_y;

        return {C_x, C_y};
    }

在文章基础函数-固定间隔采样轨迹点 也是用了线性插值的思想得到间隔均匀的点，这个函数存在的意义就在于如果前端生成的轨迹不均匀（例如按照固定轨迹点数量的生成方式），需要使用下面的函数进行处理。

/*用线性插值相同间隔的轨迹点*/
    std::vector<PathPoint> interpolateTrajectory(const std::vector<PathPoint> &originalTrajectory, float spacing)
    {
        std::vector<PathPoint> interpolatedTrajectory;
        int numPoints = originalTrajectory.size();

        // Calculate the total length of the original trajectory
        float totalLength = 0.0f;
        for (int i = 0; i < numPoints - 1; ++i)
        {
            float dx = originalTrajectory[i + 1].x - originalTrajectory[i].x;
            float dy = originalTrajectory[i + 1].y - originalTrajectory[i].y;
            totalLength += std::sqrt(dx * dx + dy * dy);
        }

        // Calculate the number of interpolated points based on the desired spacing
        int numInterpolatedPoints = static_cast<int>(totalLength / spacing);

        // Interpolate the trajectory
        for (int i = 0; i < numPoints - 1; ++i)
        {
            float dx = originalTrajectory[i + 1].x - originalTrajectory[i].x;
            float dy = originalTrajectory[i + 1].y - originalTrajectory[i].y;
            float segmentLength = std::sqrt(dx * dx + dy * dy);
            int numSegments = static_cast<int>(segmentLength / spacing);

            for (int j = 0; j < numSegments; ++j)
            {
                float t = static_cast<float>(j) / numSegments;
                float interpolatedX = originalTrajectory[i].x + t * dx;
                float interpolatedY = originalTrajectory[i].y + t * dy;
                interpolatedTrajectory.push_back({interpolatedX, interpolatedY});
            }
        }

        // Add the last point of the original trajectory
        interpolatedTrajectory.push_back(originalTrajectory[numPoints - 1]);

        return interpolatedTrajectory;
    }