隧道中线提取的优化方法

目录

• 一、算法原理
• • 1、优化方法
  • 2、参考文献
• 二、代码实现

一、算法原理

1、优化方法

2、参考文献

105

二、代码实现

#include <iostream>
#include <vector>
#include <algorithm>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <Eigen/Dense>
#include <Eigen/Geometry>



struct QuadraticCoeffs 
{
    double a, b, c;  // 二次多项式系数: 对于XOY: x = a*y^2 + b*y + c; 对于YOZ: z = a*y^2 + b*y + c

    QuadraticCoeffs() : a(0), b(0), c(0) {}
    QuadraticCoeffs(double a_, double b_, double c_) : a(a_), b(b_), c(c_) {}

    double evaluate(double y) const 
    {
        return a * y * y + b * y + c;
    }

    double derivative(double y) const 
    {
        return 2.0 * a * y + b;
    }
};


/**
 * @brief计算法线与目标曲线的交点
 */
std::pair<bool, Eigen::Vector2d> findIntersectionWithNormal(
    double y0, double val0,  // 当前点在曲线1上的坐标
    const QuadraticCoeffs& curve1_coeffs,  // 当前点所在曲线
    const QuadraticCoeffs& curve2_coeffs)  // 目标曲线
{
    // 计算曲线1在当前点的导数
    double f_prime_y0 = curve1_coeffs.derivative(y0);
    // 法线斜率 = -1/f'(y0)
    double slope_normal = -1.0 / f_prime_y0;

    }
}