C++ PCL 点云数据株高计算代码示例

#include <pcl\point_cloud.h> #include <pcl\point_types.h> #include

void computeAngle(const pcl::PointCloudpcl::PointXYZ::Ptr& cloud, double& fai) { // 获取索引号61和43的点坐标 pcl::PointXYZ point5 = cloud->points[61]; pcl::PointXYZ point7 = cloud->points[43];

// 计算中心线的方向向量
Eigen::Vector3f centerLineVec = point7.getVector3fMap() - point5.getVector3fMap();

// 计算参考向量
Eigen::Vector3f referenceVec(0, 1, 0);

// 归一化向量
centerLineVec.normalize();
referenceVec.normalize();

// 计算两个向量之间的夹角
double dotProduct = centerLineVec.dot(referenceVec);
double angleRad = std::acos(dotProduct);
double angleDeg = angleRad * 180.0 / M_PI;

// 将角度保存到 fai 变量中
fai = angleDeg;

}

int main() { pcl::PointCloudpcl::PointXYZ::Ptr cloud(new pcl::PointCloudpcl::PointXYZ); // 假设cloud已经被填充了点云数据

double fai;
computeAngle(cloud, fai);

pcl::PointXYZ point5 = cloud->points[61];
pcl::PointXYZ point7 = cloud->points[43];
double X = point5.y;
double Xsoil = point7.y;

double H = (X - Xsoil) / std::cos(fai);

std::cout << "株高H: " << H << std::endl;

return 0;

}