Conjugate gradient trajectory smoothing

Pictures .png

Pictures .png

At the same time optimizing, the larger the weight heavy, the smoother, the smaller the weight, the closer to the original point.

Pictures .png

/**
 * @description: 平滑生成的曲线 
 * @param {type} 
 * @return: 
 */
void RolloutGenerator::smoothPath(std::vector<UtilityNS::WayPoint>& path, double weight_data,
    double weight_smooth, double tolerance)
{
    if (path.size() <= 2)
        return;

    const std::vector<UtilityNS::WayPoint>& path_in = path;
    std::vector<UtilityNS::WayPoint> smoothPath_out = path_in;

    double change = tolerance;
    double xtemp, ytemp;
    int nIterations = 0;

    int size = path_in.size();

    while (change >= tolerance) {
        change = 0.0;
        for (int i = 1; i < size - 1; i++) {
            xtemp = smoothPath_out[i].pos.x;
            ytemp = smoothPath_out[i].pos.y;

            smoothPath_out[i].pos.x += weight_data * (path_in[i].pos.x - smoothPath_out[i].pos.x);
            smoothPath_out[i].pos.y += weight_data * (path_in[i].pos.y - smoothPath_out[i].pos.y);

            smoothPath_out[i].pos.x += weight_smooth * (smoothPath_out[i - 1].pos.x + smoothPath_out[i + 1].pos.x - (2.0 * smoothPath_out[i].pos.x));
            smoothPath_out[i].pos.y += weight_smooth * (smoothPath_out[i - 1].pos.y + smoothPath_out[i + 1].pos.y - (2.0 * smoothPath_out[i].pos.y));

            change += fabs(xtemp - smoothPath_out[i].pos.x);
            change += fabs(ytemp - smoothPath_out[i].pos.y);
        }
        nIterations++;
    }
    path = smoothPath_out;
}

Pictures .png