隧道中线提取的优化方法

目录一、算法原理1、优化方法2、参考文献二、代码实现一、算法原理1、优化方法2、参考文献[105]二、代码实现#includeiostream#includevector#includealgorithm#includepcl/point_types.h#includepcl/point_cloud.h#includeEigen/Dense#includeEigen/GeometrystructQuadraticCoeffs{doublea,b,c;// 二次多项式系数: 对于XOY: x a*y^2 b*y c; 对于YOZ: z a*y^2 b*y cQuadraticCoeffs():a(0),b(0),c(0){}QuadraticCoeffs(doublea_,doubleb_,doublec_):a(a_),b(b_),c(c_){}doubleevaluate(doubley)const{returna*y*yb*yc;}doublederivative(doubley)const{return2.0*a*yb;}};/** * brief计算法线与目标曲线的交点 */std::pairbool,Eigen::Vector2dfindIntersectionWithNormal(doubley0,doubleval0,// 当前点在曲线1上的坐标constQuadraticCoeffscurve1_coeffs,// 当前点所在曲线constQuadraticCoeffscurve2_coeffs)// 目标曲线{// 计算曲线1在当前点的导数doublef_prime_y0curve1_coeffs.derivative(y0);// 法线斜率 -1/f(y0)doubleslope_normal-1.0/f_prime_y0;}}