patchwork++ .cpp文件代码讲解

#include <patchworkpp.h>

#include <iostream>
#include <fstream>
#include <open3d/Open3D.h>

// for list folder
#include <experimental/filesystem>
namespace fs = std::experimental::filesystem;

using namespace open3d;

int filename_length = std::string("demo_visaulize.cpp").length();
std::string file_dir = std::string(__FILE__);
std::string data_dir = file_dir.substr(0, file_dir.size()-filename_length) + "../../data/";

//读取文件
void read_bin(std::string bin_path, Eigen::MatrixXf &cloud)
{
  FILE *file = fopen(bin_path.c_str(), "rb");     //打开文件读取
  if (!file) {                                    //如果没有文件
    std::cerr << "error: failed to load " << bin_path << std::endl;
    return;
  }

  std::vector<float> buffer(1000000);     //创建一个大小为1000000的float类型缓冲区，用于临时存储从文件中读取的数据。使用fread函数从文件中读取数据到缓冲区。这里假设每个点由4个float值表示（例如，x, y, z坐标和一个强度值或类似的数据）。因此，通过将读取的字节数除以4（sizeof(float)）来计算点的数量。
  size_t num_points = fread(reinterpret_cast<char *>(buffer.data()), sizeof(float), buffer.size(), file) / 4;//根据读取到的点的数量num_points和每个点包含4个float值，调整cloud矩阵的大小。每行为4
  cloud.resize(num_points, 4);//通过一个循环，将缓冲区中的数据复制到cloud矩阵的相应行中。每个点的四个值分别对应cloud矩阵中的一列。
  for(int i=0; i<num_points; i++)
  {
    cloud.row(i) << buffer[i*4], buffer[i*4+1], buffer[i*4+2], buffer[i*4+3];       //通过一个循环，将缓冲区中的数据复制到cloud矩阵的相应行中。每个点的四个值分别对应cloud矩阵中的一列。
  }
}

void eigen2geo(Eigen::MatrixXf add, std::shared_ptr<geometry::PointCloud> geo)        //将add的位置坐标转给geo
{
  for ( int i=0; i<add.rows(); i++ ) {
    geo->points_.push_back(Eigen::Vector3d(add.row(i)(0), add.row(i)(1), add.row(i)(2)));
  }
}

void addNormals(Eigen::MatrixXf normals, std::shared_ptr<geometry::PointCloud> geo)        
 //，它提取前三个元素（法线向量的x、y、z分量），并创建一个Eigen::Vector3d对象来存储这些分量。然后，这个Vector3d对象被添加到geo的normals_成员中。
{
  for (int i=0; i<normals.rows(); i++) {
    geo->normals_.push_back(Eigen::Vector3d(normals.row(i)(0), normals.row(i)(1), normals.row(i)(2)));
  }
}


int main(int argc, char* argv[]) {      //主函数入口：

  cout << "Execute" << __FILE__ << endl;
  // Get the dataset
  std::string input_cloud_filepath;       //代码首先检查是否提供了命令行参数（即点云文件的路径）。如果没有提供，则使用默认的测试数据集路径。
  if (argc < 2) {
    // Try out running on the test datasets.
    input_cloud_filepath = data_dir + "000000.bin";
    std::cout << "\033[1;33mNo point cloud file path specified; defaulting to the test directory. \033[0m" << std::endl;
  } else {
    input_cloud_filepath = argv[1];
    std::cout << "\033[1;32mLoading point cloud files from " << input_cloud_filepath << "\033[0m" << std::endl;
  }
  if(!fs::exists(input_cloud_filepath)){        //检查文件是否存在：
  //使用fs::exists函数检查指定的点云文件路径是否存在。如果不存在，则输出错误信息并退出程序。
    std::cout << "\033[1;31mERROR HERE: maybe wrong data file path, please check the path or remove argv to run default one. \033[0m" 
              << "\nThe file path you provide is: " << input_cloud_filepath << std::endl;
    return 0;
  }

  // Patchwork++ initialization初始化Patchwork++的参数，并创建一个PatchWorkpp对象。这里设置了verbose为true，可能意味着在程序运行时输出更详细的日志信息。


  patchwork::Params patchwork_parameters;
  patchwork_parameters.verbose = true;

  patchwork::PatchWorkpp Patchworkpp(patchwork_parameters);

  // Load point cloud   加载点云：

  Eigen::MatrixXf cloud;
  read_bin(input_cloud_filepath, cloud);

  // Estimate Ground    估计地面：
  Patchworkpp.estimateGround(cloud);
  
  // Get Ground and Nonground       获取地面和非地面点：

  Eigen::MatrixX3f ground     = Patchworkpp.getGround();
  Eigen::MatrixX3f nonground  = Patchworkpp.getNonground();
  double time_taken = Patchworkpp.getTimeTaken();

  Eigen::VectorXi ground_idx    = Patchworkpp.getGroundIndices();
  Eigen::VectorXi nonground_idx = Patchworkpp.getNongroundIndices();

  // Get centers and normals for patches
  Eigen::MatrixX3f centers    = Patchworkpp.getCenters();
  Eigen::MatrixX3f normals    = Patchworkpp.getNormals();

  cout << "Origianl Points  #: " << cloud.rows() << endl;
  cout << "Ground Points    #: " << ground.rows() << endl;
  cout << "Nonground Points #: " << nonground.rows() << endl;
  cout << "Time Taken : "<< time_taken / 1000000 << "(sec)" << endl;
  cout << "Press ... \n" << endl;
  cout << "\t H  : help" << endl;
  cout << "\t N  : visualize the surface normals" << endl;
  cout << "\tESC : close the Open3D window" << endl;

  // Visualize
  std::shared_ptr<geometry::PointCloud> geo_ground(new geometry::PointCloud);
  std::shared_ptr<geometry::PointCloud> geo_nonground(new geometry::PointCloud);
  std::shared_ptr<geometry::PointCloud> geo_centers(new geometry::PointCloud);

  eigen2geo(ground,     geo_ground);
  eigen2geo(nonground,  geo_nonground);
  eigen2geo(centers,    geo_centers);
  addNormals(normals,   geo_centers);
  //设置不同部分的颜色
  geo_ground->PaintUniformColor(Eigen::Vector3d(0.0, 1.0, 0.0));      
  geo_nonground->PaintUniformColor(Eigen::Vector3d(1.0, 0.0, 0.0));
  geo_centers->PaintUniformColor(Eigen::Vector3d(1.0, 1.0, 0.0));  
//窗口显示部分
  visualization::Visualizer visualizer;
  visualizer.CreateVisualizerWindow("Open3D", 1600, 900);
  visualizer.AddGeometry(geo_ground);
  visualizer.AddGeometry(geo_nonground);
  visualizer.AddGeometry(geo_centers);
  visualizer.Run();
  visualizer.DestroyVisualizerWindow();
}