C++ CGAL: 使用线性最小二乘法拟合平面并计算点云间距离和角度

这段代码使用 CGAL 库进行点云处理，主要包括以下步骤：

平面拟合: 使用 CGAL::linear_least_squares_fitting_3 函数对点云数据进行线性最小二乘法拟合，得到每个点云的最佳拟合平面。
计算质心: 使用 CGAL::centroid 函数计算每个点云的质心。
计算距离: 使用 CGAL::Squared_distance_3 类计算两个点云中所有点对之间的距离。
计算角度: 计算两个点云中对应平面法向量之间的夹角。

代码示例如下：

std::vector<Point> centroid_lidar;
std::vector<Point> centroid_photo;
std::vector<Kernel::Plane_3 > fp;
std::vector<Kernel::Plane_3 > fp1;
std::vector<Kernel::Vector_3> faxiangliang;
std::vector<Kernel::Vector_3> faxiangliang1;

for (size_t i = 0; i < planes.size(); i++)
{
  Point centroid = CGAL::centroid(planes[i].begin(), planes[i].end());
  centroid_lidar.push_back(centroid);
  std::vector<Point> plane = planes[i];
  Kernel::Plane_3 fittedPlane;
  CGAL::linear_least_squares_fitting_3(plane.begin(), plane.end(), fittedPlane, CGAL::Dimension_tag<0>());
  Kernel::Vector_3 a= fittedPlane.orthogonal_vector();
  faxiangliang.push_back(a);
  fp.push_back(fittedPlane);
}

for (size_t i = 0; i < planes1.size(); i++)
{
  Point centroid = CGAL::centroid(planes1[i].begin(), planes1[i].end());
  centroid_photo.push_back(centroid);
  std::vector<Point> plane = planes1[i];
  Kernel::Plane_3 fittedPlane;
  CGAL::linear_least_squares_fitting_3(plane.begin(), plane.end(), fittedPlane, CGAL::Dimension_tag<0>());
  Kernel::Vector_3 a = fittedPlane.orthogonal_vector();
  faxiangliang1.push_back(a);
  fp1.push_back(fittedPlane);
}

Squared_distance_3 dis;
Squared_distance_3 dis1;
Sqrt sqrtt;
Sqrt sqrttt;

std::vector<std::vector<double>>  dis_centroid_LidarToPhoto;
std::vector<std::vector<double>>  dis_centroid_PhotoToLidar;
std::vector<std::vector<double>>  angle_LidarToPhoto;
std::vector<std::vector<double>>  angle_PhotoToLidar;

dis_centroid_LidarToPhoto.resize(centroid_lidar.size()); // 初始化
dis_centroid_PhotoToLidar.resize(centroid_photo.size()); // 初始化

for (size_t i = 0; i < centroid_lidar.size(); i++)
{
  for (size_t j = 0; j < centroid_photo.size(); j++)
  {
    double a = dis(centroid_lidar[i], centroid_photo[j]);
    dis_centroid_LidarToPhoto[i].push_back(sqrtt(a));
  }
}

for (size_t i = 0; i < centroid_photo.size(); i++)
{
  for (size_t j = 0; j < centroid_lidar.size(); j++)
  {
    double a = dis(centroid_photo[i], centroid_lidar[j]);
    dis_centroid_PhotoToLidar[i].push_back(sqrttt(a));
  }
}

这段代码中，dis_centroid_LidarToPhoto 和 dis_centroid_PhotoToLidar 两个二维向量需要在使用前进行初始化，否则会造成未定义行为。代码中添加了 resize 函数对这两个向量进行初始化，确保它们在使用时已经分配了内存空间。

这段代码示例展示了使用 CGAL 进行点云处理的基本步骤，可以根据实际需求进行扩展和修改。

注意:

代码中使用 CGAL 库进行点云处理，需要安装和配置 CGAL 库。
代码示例中使用的 planes 和 planes1 变量需要提前定义和赋值。
代码示例中使用的 Point 和 Kernel::Plane_3 数据类型需要根据实际需求进行定义。

相关链接: