PCL - MLS代碼研讀（一）- MLS測試

前言

PCL的MLS模塊用於對點雲做平滑處理（或說曲面重建）及上採樣，其中MLS的理論基礎可以參考PCL MLS論文Computing and Rendering Point Set Surfaces研讀筆記這篇文章。本系列文將記錄筆者研讀PCL MLS模塊代碼的筆記。

想要了解PCL的MLS模組，可以先從它的測試程序入手。MLS的測試程序位於test/surface/test_moving_least_squares.cpp。

PCL的各測試程序中都用到了Googletest，可以到Quickstart: Building with CMake及Googletest Primer查看Googletest的簡要的介紹。

test_moving_least_squares.cpp的測試函數TEST (PCL, MovingLeastSquares)有OPENMP版本和非OPENMP版本，此處只關注非OPENMP版本。

測試曲面重建

下面的測試用例調用MLS模塊的process函數做曲面重建。注意此處節錄的代碼是包含在test_moving_least_squares.cpp的TEST (PCL, MovingLeastSquares)函數裡面的。

首先設定好輸入點雲（cloud），是否要計算法向量，曲面多項式的階數，用於最近鄰查找的kdtree（tree）以及搜尋半徑。

  // Init objects
  PointCloud<PointXYZ> mls_points;
  PointCloud<PointNormal>::Ptr mls_normals (new PointCloud<PointNormal> ());
  MovingLeastSquares<PointXYZ, PointNormal> mls;

  // Set parameters
  mls.setInputCloud (cloud);
  mls.setComputeNormals (true);
  mls.setPolynomialOrder (2);
  mls.setSearchMethod (tree);
  mls.setSearchRadius (0.03);

然後調用MLS模塊的process函數，可以把它視為曲面重建的入口函數，得到平滑後的點雲（mls_normals）。

  // Reconstruct
  // 曲面重建的入口函數
  mls.process (*mls_normals);

process函數的結果是一個帶有法向量的點雲（mls_normals），程序的最後一段就是在檢查輸出點雲的第零個點的坐標，法向量及曲率是否符合預期。

  EXPECT_NEAR ((*mls_normals)[0].x, 0.005417, 1e-3);
  EXPECT_NEAR ((*mls_normals)[0].y, 0.113463, 1e-3);
  EXPECT_NEAR ((*mls_normals)[0].z, 0.040715, 1e-3);
  EXPECT_NEAR (std::abs ((*mls_normals)[0].normal[0]), 0.111894, 1e-3);
  EXPECT_NEAR (std::abs ((*mls_normals)[0].normal[1]), 0.594906, 1e-3);
  EXPECT_NEAR (std::abs ((*mls_normals)[0].normal[2]), 0.795969, 1e-3);
  EXPECT_NEAR ((*mls_normals)[0].curvature, 0.012019, 1e-3);

測試SAMPLE_LOCAL_PLANE上採樣方法

使用SAMPLE_LOCAL_PLANE上採樣方法之前，一樣要先設定好輸入點雲（cloud），是否要計算法向量，曲面多項式的階數，用於最近鄰查找的kdtree（tree）以及搜尋半徑。

  // Testing upsampling
  MovingLeastSquares<PointXYZ, PointNormal> mls_upsampling;
  // Set parameters
  mls_upsampling.setInputCloud (cloud);
  mls_upsampling.setComputeNormals (true);
  mls_upsampling.setPolynomialOrder (2);
  mls_upsampling.setSearchMethod (tree);
  mls_upsampling.setSearchRadius (0.03);

另外還需要設定上採樣方法（此處為SAMPLE_LOCAL_PLANE），還有兩個跟着這個上採樣方法的參數：上採樣半徑及上採樣步長。

  mls_upsampling.setUpsamplingMethod (MovingLeastSquares<PointXYZ, PointNormal>::SAMPLE_LOCAL_PLANE);
  mls_upsampling.setUpsamplingRadius (0.025);
  mls_upsampling.setUpsamplingStepSize (0.01);

然後一樣調用process這個入口函數，得到上採樣後的點雲（mls_normals）。

  mls_normals->clear ();
  mls_upsampling.process (*mls_normals);

最後一樣是檢查輸出點雲的坐標，法向量及曲率是否符合預期。

  EXPECT_NEAR ((*mls_normals)[10].x, -0.000538, 1e-3);
  EXPECT_NEAR ((*mls_normals)[10].y, 0.110080, 1e-3);
  EXPECT_NEAR ((*mls_normals)[10].z, 0.043602, 1e-3);
  EXPECT_NEAR (std::abs ((*mls_normals)[10].normal[0]), 0.022678, 1e-3);
  EXPECT_NEAR (std::abs ((*mls_normals)[10].normal[1]), 0.554978, 1e-3);
  EXPECT_NEAR (std::abs ((*mls_normals)[10].normal[2]), 0.831556, 1e-3);
  EXPECT_NEAR ((*mls_normals)[10].curvature, 0.012019, 1e-3);
  EXPECT_EQ (mls_normals->size (), 6352);

測試VOXEL_GRID_DILATION上採樣方法

VOXEL_GRID_DILATION上採樣方法的測試程序與SAMPLE_LOCAL_PLANE上採樣方法共用了以下代碼：

  MovingLeastSquares<PointXYZ, PointNormal> mls_upsampling;
  // Set parameters
  mls_upsampling.setInputCloud (cloud);
  mls_upsampling.setComputeNormals (true);
  mls_upsampling.setPolynomialOrder (2);
  mls_upsampling.setSearchMethod (tree);
  mls_upsampling.setSearchRadius (0.03);

以下是VOXEL_GRID_DILATION上採樣方法需要另外做的設定，這裡額外設定了dilation次數及voxel的大小：

  mls_upsampling.setUpsamplingMethod (MovingLeastSquares<PointXYZ, PointNormal>::VOXEL_GRID_DILATION);
  mls_upsampling.setDilationIterations (5);
  mls_upsampling.setDilationVoxelSize (0.005f);

最後一樣將處理結果存到mls_normals這個點雲中，然後檢查法向量及曲率：

  mls_normals->clear ();
  mls_upsampling.process (*mls_normals);
  EXPECT_NEAR ((*mls_normals)[10].x, -0.070005938410758972, 2e-3);
  EXPECT_NEAR ((*mls_normals)[10].y, 0.028887597844004631, 2e-3);
  EXPECT_NEAR ((*mls_normals)[10].z, 0.01788550429046154, 2e-3);
  EXPECT_NEAR ((*mls_normals)[10].curvature, 0.107273, 1e-1);
  EXPECT_NEAR (double (mls_normals->size ()), 29394, 2);

主程序

int
main (int argc, char** argv)
{
  if (argc < 2)
  {
    std::cerr << "No test file given. Please download `bun0.pcd` and pass its path to the test." << std::endl;
    return (-1);
  }

  // Load file
  pcl::PCLPointCloud2 cloud_blob;
  loadPCDFile (argv[1], cloud_blob);
  fromPCLPointCloud2 (cloud_blob, *cloud);

  // Create search tree
  tree.reset (new search::KdTree<PointXYZ> (false));
  tree->setInputCloud (cloud);

  // Normal estimation
  NormalEstimation<PointXYZ, Normal> n;
  PointCloud<Normal>::Ptr normals (new PointCloud<Normal> ());
  n.setInputCloud (cloud);
  //n.setIndices (indices[B);
  n.setSearchMethod (tree);
  n.setKSearch (20);
  n.compute (*normals);

  // Concatenate XYZ and normal information
  pcl::concatenateFields (*cloud, *normals, *cloud_with_normals);
      
  // Create search tree
  tree2.reset (new search::KdTree<PointNormal>);
  tree2->setInputCloud (cloud_with_normals);

  // Process for update cloud
  if(argc == 3){
    pcl::PCLPointCloud2 cloud_blob1;
    loadPCDFile (argv[2], cloud_blob1);
    fromPCLPointCloud2 (cloud_blob1, *cloud1);
        // Create search tree
    tree3.reset (new search::KdTree<PointXYZ> (false));
    tree3->setInputCloud (cloud1);

    // Normal estimation
    NormalEstimation<PointXYZ, Normal> n1;
    PointCloud<Normal>::Ptr normals1 (new PointCloud<Normal> ());
    n1.setInputCloud (cloud1);

    n1.setSearchMethod (tree3);
    n1.setKSearch (20);
    n1.compute (*normals1);

    // Concatenate XYZ and normal information
    pcl::concatenateFields (*cloud1, *normals1, *cloud_with_normals1);
    // Create search tree
    tree4.reset (new search::KdTree<PointNormal>);
    tree4->setInputCloud (cloud_with_normals1);
  }

  // Testing
  testing::InitGoogleTest (&argc, argv);
  return (RUN_ALL_TESTS ());
}

這段程序較長，但實際有用處的只有這段：

  // Load file
  pcl::PCLPointCloud2 cloud_blob;
  loadPCDFile (argv[1], cloud_blob);
  fromPCLPointCloud2 (cloud_blob, *cloud);

  // Create search tree
  tree.reset (new search::KdTree<PointXYZ> (false));
  tree->setInputCloud (cloud);

用於將點雲載入cloud這個變量當中，以及設定好search::KdTree物件tree。

還有最下面兩行：

  testing::InitGoogleTest (&argc, argv);
  return (RUN_ALL_TESTS ());

是Googletest要求的寫法。

使用以下指令運行程序：

./test_moving_least_squares bun0.pcd

其中bun0.pcd是從otherlab/pcl下載的點雲檔案。

運行結果如下：

[==========] Running 2 tests from 1 test suite.
[----------] Global test environment set-up.
[----------] 2 tests from PCL
[ RUN      ] PCL.MovingLeastSquares
[       OK ] PCL.MovingLeastSquares (198 ms)
[ RUN      ] PCL.MovingLeastSquaresOMP
[       OK ] PCL.MovingLeastSquaresOMP (4 ms)
[----------] 2 tests from PCL (202 ms total)

[----------] Global test environment tear-down
[==========] 2 tests from 1 test suite ran. (202 ms total)
[  PASSED  ] 2 tests.

MLS module結構

從上面的測試程序中，我們看到了MLS模塊的入口函數是process。這裡來理一下MLS模塊中個函數的調用關係。

├── process
│   ├── performProcessing
│      ├── computeMLSPointNormal
│         ├── computeMLSSurface
│         ├── projectQueryPoint
│            ├── getMLSCoordinates
│            ├── projectPointOrthogonalToPolynomialSurface
│               ├── getPolynomialPartialDerivative
│         ├── projectPointSimpleToPolynomialSurface
│         ├── projectPointToMLSPlane
│         ├── addProjectedPointNormal
│      ├── performUpsampling

後續的文章將按照各函數被調用的順序依次介紹。

注：以上只涉及了曲面重建的部分，上採樣的部分暫不展開。