PCL - MLS代碼研讀（十五）- VOXEL_GRID

PCL - MLS代碼研讀（十五）- VOXEL_GRID_DILATION上採樣方法

前言
成員變數
MLSVoxelGrid
MLSVoxelGrid建構子
dilate函數
getter & setter
process函數
performUpsampling函數
上採樣結果

前言

PCL - MLS代碼研讀（十一）- computeMLSPointNormal函數，PCL - MLS代碼研讀（十三）- RANDOM_UNIFORM_DENSITY上採樣方法及PCL - MLS代碼研讀（十四）- DISTINCT_CLOUD上採樣方法介紹了五種上採樣方法中的四種，本篇介紹的是最後一種上採樣方法VOXEL_GRID_DILATION。

成員變數

以下是與VOXEL_GRID_DILATION上採樣方法相關的成員變數：

/** \brief Voxel size for the VOXEL_GRID_DILATION upsampling method */
//在performUpsampling中會把這個成員變數當作MLSVoxelGrid建構子的參數,用它來設定MLSVoxelGrid裡面的voxel_size_成員變數
float voxel_size_;/** \brief Number of dilation steps for the VOXEL_GRID_DILATION upsampling method */
//dilate次數
int dilation_iteration_num_;

其中voxel_size_代表一個voxel grid的寬度，dilation_iteration_num_代表要做幾次dilation（向外擴張）。

MLSVoxelGrid

MLSVoxelGrid為一代表voxel grids的數據結構。

/** \brief A minimalistic implementation of a voxel grid, necessary for the point cloud upsampling* \note Used only in the case of VOXEL_GRID_DILATION upsampling*/
class MLSVoxelGrid
{public:

voxel grids的葉子節點（有包含點雲中的點的節點）：

    //擁有一個"valid"成員變數的structstruct Leaf { Leaf () : valid (true) {} bool valid; };

MLSVoxelGrid的建構子及dilate函數，這兩個函數將留到後面介紹。

    MLSVoxelGrid (PointCloudInConstPtr& cloud,IndicesPtr &indices,float voxel_size);voiddilate ();

索引有一維和三維兩種表示法，以下的兩個函數用於在他們之間做轉換。注意data_size_必須大於等於每個邊的voxel grid數量才能保證這兩個函數的計算結果正確。

    //索引三維轉一維,data_size_必須大於每個邊的voxel grid數量inline voidgetIndexIn1D (const Eigen::Vector3i &index, std::uint64_t &index_1d) const{index_1d = index[0] * data_size_ * data_size_ +index[1] * data_size_ + index[2];}//索引一維轉三維,data_size_必須大於每個邊的voxel grid數量inline voidgetIndexIn3D (std::uint64_t index_1d, Eigen::Vector3i& index_3d) const{index_3d[0] = static_cast<Eigen::Vector3i::Scalar> (index_1d / (data_size_ * data_size_));index_1d -= index_3d[0] * data_size_ * data_size_;index_3d[1] = static_cast<Eigen::Vector3i::Scalar> (index_1d / data_size_);index_1d -= index_3d[1] * data_size_;index_3d[2] = static_cast<Eigen::Vector3i::Scalar> (index_1d);}

用於在三維點坐標和voxel grid索引之間做轉換的函數：

    //查看點p是屬於哪一個voxelinline voidgetCellIndex (const Eigen::Vector3f &p, Eigen::Vector3i& index) const{//計算點p三個維度上各自落在第幾個voxelfor (int i = 0; i < 3; ++i)index[i] = static_cast<Eigen::Vector3i::Scalar> ((p[i] - bounding_min_ (i)) / voxel_size_);}//由voxel的索引得到voxel grid左上角點的位置inline voidgetPosition (const std::uint64_t &index_1d, Eigen::Vector3f &point) const{//先將一維的索引轉成三維Eigen::Vector3i index_3d;getIndexIn3D (index_1d, index_3d);for (int i = 0; i < 3; ++i)//point為index_1d所代表的voxel grid的左上(三個維度中較小的那一側)角點point[i] = static_cast<Eigen::Vector3f::Scalar> (index_3d[i]) * voxel_size_ + bounding_min_[i];}

MLSVoxelGrid的成員變數：

    typedef std::map<std::uint64_t, Leaf> HashMap;HashMap voxel_grid_;//所有voxel grids的邊界Eigen::Vector4f bounding_min_, bounding_max_;//最長邊有多少個voxel gridstd::uint64_t data_size_;//一個voxel grid的寬度float voxel_size_;PCL_MAKE_ALIGNED_OPERATOR_NEW
};

MLSVoxelGrid建構子

在initialization list中設定voxel_size_成員變數：

template <typename PointInT, typename PointOutT>
pcl::MovingLeastSquares<PointInT, PointOutT>::MLSVoxelGrid::MLSVoxelGrid (PointCloudInConstPtr& cloud,IndicesPtr &indices,float voxel_size) :voxel_grid_ (), data_size_ (), voxel_size_ (voxel_size)
{

計算輸入點雲cloud和索引indices的邊界，然後計算得到點雲包圍框的最長邊的邊長，再取data_size_為static_cast<std::uint64_t> (1.5 * max_size / voxel_size_)。可以把它想成voxel grids每個維度的grid數量上限。在MLSVoxelGrid中用來做索引一維/三維表達方式轉換的函數（getIndexIn1D及getIndexIn3D）中，就用到了data_size_。

  //輸入點雲的邊界為bounding_min_及bounding_max_pcl::getMinMax3D (*cloud, *indices, bounding_min_, bounding_max_);//輸入點雲在三個維度上的長度Eigen::Vector4f bounding_box_size = bounding_max_ - bounding_min_;//輸入點雲包圍框的最長邊const double max_size = (std::max) ((std::max)(bounding_box_size.x (), bounding_box_size.y ()), bounding_box_size.z ());// Put initial cloud in voxel grid//最長的邊有多少個grid,注意是乘上1.5?data_size_ = static_cast<std::uint64_t> (1.5 * max_size / voxel_size_);

遍歷點雲中的每一個點，在該處建立一個能把它包住的Leaf，然後把它加入voxel_grid_這個HashMap中。

  //遍歷點雲中的每一個點,在該處建立一個Leaf,把他們加入voxel_grid_這個HashMap中//注意是有點的地方才有leaf,所以voxel grids可能不是連續的for (std::size_t i = 0; i < indices->size (); ++i)if (std::isfinite ((*cloud)[(*indices)[i]].x)){Eigen::Vector3i pos;//查看點(*cloud)[(*indices)[i]]是屬於哪一個voxelgetCellIndex ((*cloud)[(*indices)[i]].getVector3fMap (), pos);std::uint64_t index_1d;//三維索引轉一維getIndexIn1D (pos, index_1d);Leaf leaf;//為對應的voxel grid加上Leafvoxel_grid_[index_1d] = leaf;}
}

dilate函數

對每一個有點的voxel grid，都往前後左右上下幾個方向擴張。

template <typename PointInT, typename PointOutT> void
pcl::MovingLeastSquares<PointInT, PointOutT>::MLSVoxelGrid::dilate ()
{//對每一個有點的voxel grid,都往前後左右上下共3*3*3-1=26個鄰居擴張.//因為voxel grids可能不是連續的,所以這裡讓存在的grid往外擴張,希望能把洞補起來?//HashMap:由voxel index對應到Leaf的數據結構HashMap new_voxel_grid = voxel_grid_;//遍歷voxel_grid_這個HashMap中存在的每一個格子/遍歷有點的每一個格子for (typename MLSVoxelGrid::HashMap::iterator m_it = voxel_grid_.begin (); m_it != voxel_grid_.end (); ++m_it){//三維的voxel grid索引Eigen::Vector3i index;getIndexIn3D (m_it->first, index);// Now dilate all of its voxels//在三個方向上往左右兩邊新增voxel gridfor (int x = -1; x <= 1; ++x)for (int y = -1; y <= 1; ++y)for (int z = -1; z <= 1; ++z)if (x != 0 || y != 0 || z != 0){//排除(x,y,z)=(0,0,0)的點Eigen::Vector3i new_index;//new_index:index周圍立方體上的點,應該不一定是新的,有可能會找到之前就存在的點?new_index = index + Eigen::Vector3i (x, y, z);std::uint64_t index_1d;getIndexIn1D (new_index, index_1d);Leaf leaf;new_voxel_grid[index_1d] = leaf;}}voxel_grid_ = new_voxel_grid;
}

getter & setter

這幾個MovingLeastSquares類別的public成員函數是voxel_size_和dilation_iteration_num_的getter和setter。

/** \brief Set the voxel size for the voxel grid* \note Used only in the VOXEL_GRID_DILATION upsampling method* \param[in] voxel_size the edge length of a cubic voxel in the voxel grid*/
inline void
setDilationVoxelSize (float voxel_size) { voxel_size_ = voxel_size; }/** \brief Get the voxel size for the voxel grid* \note Used only in the VOXEL_GRID_DILATION upsampling method*/
inline float
getDilationVoxelSize () const { return (voxel_size_); }/** \brief Set the number of dilation steps of the voxel grid* \note Used only in the VOXEL_GRID_DILATION upsampling method* \param[in] iterations the number of dilation iterations*/
inline void
setDilationIterations (int iterations) { dilation_iteration_num_ = iterations; }/** \brief Get the number of dilation steps of the voxel grid* \note Used only in the VOXEL_GRID_DILATION upsampling method*/
inline int
getDilationIterations () const { return (dilation_iteration_num_); }

process函數

在process函數中，僅將cache_mls_results_設為true後，就調用performProcessing (output)進行上採樣。

template <typename PointInT, typename PointOutT> void
pcl::MovingLeastSquares<PointInT, PointOutT>::process (PointCloudOut &output)
{//...switch (upsample_method_){// Initialize random number generator if necessarycase (RANDOM_UNIFORM_DENSITY):{//...}case (VOXEL_GRID_DILATION):case (DISTINCT_CLOUD):{if (!cache_mls_results_)PCL_WARN ("The cache mls results is forced when using upsampling method VOXEL_GRID_DILATION or DISTINCT_CLOUD.\n");cache_mls_results_ = true;break;}default:break;}//...// Perform the actual surface reconstructionperformProcessing (output);//...
}

performUpsampling函數

在performUpsampling函數中實際進行上採樣。

template <typename PointInT, typename PointOutT> void
pcl::MovingLeastSquares<PointInT, PointOutT>::performUpsampling (PointCloudOut &output)
{//...// For the voxel grid upsampling method, generate the voxel grid and dilate it// Then, project the newly obtained points to the MLS surfaceif (upsample_method_ == VOXEL_GRID_DILATION){

corresponding_input_indices_是輸出時會用到的數據結構，在這裡進行reset：

    corresponding_input_indices_.reset (new PointIndices);

初始化MLSVoxelGrid物件並進行dilation_iteration_num_次dilation：

    MLSVoxelGrid voxel_grid (input_, indices_, voxel_size_);for (int iteration = 0; iteration < dilation_iteration_num_; ++iteration)voxel_grid.dilate ();

對每一個有包含點的voxel grid都做以下事情：

    for (typename MLSVoxelGrid::HashMap::iterator m_it = voxel_grid.voxel_grid_.begin (); m_it != voxel_grid.voxel_grid_.end (); ++m_it){

計算得到該voxel grid左上角點的位置：

      // Get 3D position of pointEigen::Vector3f pos;//由voxel的索引得到voxel grid左上角點位置voxel_grid.getPosition (m_it->first, pos);PointInT p;p.x = pos[0];p.y = pos[1];p.z = pos[2];

在輸入點雲中尋找距離該點最近的點，其索引為input_index：

      pcl::Indices nn_indices;std::vector<float> nn_dists;tree_->nearestKSearch (p, 1, nn_indices, nn_dists);const auto input_index = nn_indices.front ();

如果該點的投影結果是無效的，則跳過：

      // If the closest point did not have a valid MLS fitting result// OR if it is too far away from the sampled pointif (mls_results_[input_index].valid == false)continue;

獲取該最近鄰的坐標，對它做投影，並加入output，normals_，corresponding_input_indices_等用於輸出的數據結構中：

      Eigen::Vector3d add_point = p.getVector3fMap ().template cast<double> ();MLSResult::MLSProjectionResults proj = mls_results_[input_index].projectPoint (add_point, projection_method_,  5 * nr_coeff_);addProjectedPointNormal (input_index, proj.point, proj.normal, mls_results_[input_index].curvature, output, *normals_, *corresponding_input_indices_);}}
}

上採樣結果

輸入點雲：

dilate次數為0，使用VOXEL_GRID_DILATION方法上採樣的結果：

dilate次數為1，使用VOXEL_GRID_DILATION方法上採樣的結果。相較於上面的結果，可以看到點變得密集些了。但是周圍卻多了一些預期之外的點（可能是bug？）

在更大的尺度下看同一個點雲：上面我們所看到的部分只是整個點雲的一小部分，下面這張圖還未將整個點雲展示完全，因為點雲的尺度是在1e+7這個數量級，相較之下，原始點雲只在1~10這個數量級（這是什麼原因造成的？）。