算法来源:C++/PCL:最小二乘拟合平面直线,平面多项式曲线,空间多项式曲线
以下是我改写的算法,未经验证正确性!!!
// 3D 空间中拟合曲线
void fittingPolynomial3D(const pcl::PointCloud<pcl::PointXYZI>::Ptr &cloud, double &a, double &b, double &c) {
// 获取点的总数
int num_point = cloud->points.size();
Eigen::MatrixXd A_(3, 3), B_(3, 1), A123(3, 1);
double A01(0.0), A02(0.0), A12(0.0), A22(0.0), B00(0.0), B10(0.0), B12(0.0);
for (int i = 0; i < num_point; ++i) {
double x_y = std::sqrt(std::pow(cloud->points[i].x, 2) + std::pow(cloud->points[i].y, 2));
A01 += x_y;
A02 += std::pow(x_y, 2);
A12 += std::pow(x_y, 3);
A22 += std::pow(x_y, 4);
B00 += cloud->points[i].z;
B10 += x_y * cloud->points[i].z;
B12 += std::pow(x_y, 2) * cloud->points[i].z;
}
A_ << num_point, A01, A02,
A01, A02, A12,
A02, A12, A22;
B_ << B00,
B10,
B12;
A123 = A_.inverse() * B_;
a = A123(2, 0);
b = A123(1, 0);
c = A123(0, 0);
}