PCL - MLS代碼研讀（八）- MovingLeastSquares
-
- 前言
- pcl::MovingLeastSquares
-
- UpsamplingMethod
- MLSVoxelGrid
- typedef
- using
- protected成員變量
- public成員函數
-
- constructor和destructor
- getter和setter
- process
- protected成員函數
- private成員
前言
本篇介紹MLS的第二個類別
pcl::MovingLeastSquares
的大致結構。
pcl::MovingLeastSquares
先來大致看以下它的代碼結構：
namespace pcl
{
template <typename PointInT, typename PointOutT>
class MovingLeastSquares : public CloudSurfaceProcessing<PointInT, PointOutT>
{
public:
// 省略了typedef, using
enum UpsamplingMethod
{
// 省略了內容
};
// 省略了函數
protected:
// 省略了變數
/** \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
{
// 省略了內容
};
// 省略了變數
// 省略了函數
private:
// 省略了變數
};
}
可以看到它定義了一個enum及一個class，其餘部分是它的成員變量及函數。
UpsamplingMethod
首先來看
MovingLeastSquares
中定義的enum
UpsamplingMethod
，它表示上採樣的方法。
算上
NONE
，上採樣方法共有五種。其中
NONE
,
SAMPLE_LOCAL_PLANE
,
RANDOM_UNIFORM_DENSITY
三種是在
computeMLSPointNormal
成員函數中就會處理好；其餘兩種
DISTINCT_CLOUD
及
VOXEL_GRID_DILATION
則是留到
performUpsampling
成員函數中處理。
enum UpsamplingMethod
{
NONE, /**< \brief No upsampling will be done, only the input points will be projected
to their own MLS surfaces. */
DISTINCT_CLOUD, /**< \brief Project the points of the distinct cloud to the MLS surface. */
SAMPLE_LOCAL_PLANE, /**< \brief The local plane of each input point will be sampled in a circular fashion
using the \ref upsampling_radius_ and the \ref upsampling_step_ parameters. */
RANDOM_UNIFORM_DENSITY, /**< \brief The local plane of each input point will be sampled using an uniform random
distribution such that the density of points is constant throughout the
cloud - given by the \ref desired_num_points_in_radius_ parameter. */
//洞會被填滿?
VOXEL_GRID_DILATION /**< \brief The input cloud will be inserted into a voxel grid with voxels of
size \ref voxel_size_; this voxel grid will be dilated \ref dilation_iteration_num_
times and the resulting points will be projected to the MLS surface
of the closest point in the input cloud; the result is a point cloud
with filled holes and a constant point density. */
};
MLSVoxelGrid
這是為
VOXEL_GRID_DILATION
上採樣方法特別定義的類別，留到
VOXEL_GRID_DILATION
章節時再一併做介紹。
/** \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:
struct Leaf { Leaf () : valid (true) {} bool valid; };
MLSVoxelGrid (PointCloudInConstPtr& cloud,
IndicesPtr &indices,
float voxel_size);
void
dilate ();
//索引三維轉一維
inline void
getIndexIn1D (const Eigen::Vector3i &index, std::uint64_t &index_1d) const
{
index_1d = index[0] * data_size_ * data_size_ +
index[1] * data_size_ + index[2];
}
//索引一維轉三維
inline void
getIndexIn3D (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);
}
//查看點p是屬於哪一個voxel
inline void
getCellIndex (const Eigen::Vector3f &p, Eigen::Vector3i& index) const
{
for (int i = 0; i < 3; ++i)
index[i] = static_cast<Eigen::Vector3i::Scalar> ((p[i] - bounding_min_ (i)) / voxel_size_);
}
//由voxel的索引得到位置
inline void
getPosition (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[i] = static_cast<Eigen::Vector3f::Scalar> (index_3d[i]) * voxel_size_ + bounding_min_[i];
}
typedef std::map<std::uint64_t, Leaf> HashMap;
HashMap voxel_grid_;
Eigen::Vector4f bounding_min_, bounding_max_;
std::uint64_t data_size_;
float voxel_size_;
PCL_MAKE_ALIGNED_OPERATOR_NEW
};
typedef
這兩個似乎沒用到？
typedef shared_ptr<MovingLeastSquares<PointInT, PointOutT> > Ptr;
typedef shared_ptr<const MovingLeastSquares<PointInT, PointOutT> > ConstPtr;
using
雖然這部分的代碼不長，但是背後學問還挺大的。
using PCLBase<PointInT>::input_;
using PCLBase<PointInT>::indices_;
using PCLBase<PointInT>::fake_indices_;
using PCLBase<PointInT>::initCompute;
using PCLBase<PointInT>::deinitCompute;
這裡的
using
有兩個作用，詳見C++ using - 繼承共同行為 & 改變成員的訪問權限。
using KdTree = pcl::search::Search<PointInT>;
//typename specifies that it is the name of a type
using KdTreePtr = typename KdTree::Ptr;
// 模板中的參數已經給定,是個具體的類型,所以不用加typename告知編譯器它是一個類別
using NormalCloud = pcl::PointCloud<pcl::Normal>;
using NormalCloudPtr = NormalCloud::Ptr;
using PointCloudOut = pcl::PointCloud<PointOutT>;
using PointCloudOutPtr = typename PointCloudOut::Ptr;
using PointCloudOutConstPtr = typename PointCloudOut::ConstPtr;
using PointCloudIn = pcl::PointCloud<PointInT>;
using PointCloudInPtr = typename PointCloudIn::Ptr;
using PointCloudInConstPtr = typename PointCloudIn::ConstPtr;
using SearchMethod = std::function<int (pcl::index_t, double, pcl::Indices &, std::vector<float> &)>;
觀察
KdTree
和
KdTreePtr
，可以發現一個沒加，另一個則有加
typename
，他們之間的差別詳見C++ typename使用時機。
protected成員變量
以下幾個成員變量是不論採用任何上採樣方法都會用到的：
-
normals_
-
search_method_
-
tree_
search_method_
-
order_
-
search_radius_
sqr_gauss_param
-
sqr_gauss_param_
-
compute_normals_
-
upsample_method_
-
cache_mls_results_
-
mls_results_
-
projection_method_
-
threads_
-
nr_coeff_
-
corresponding_input_indices_
以下是只有上採樣方法為
DISTINCT_CLOUD
才會用到的：
-
distinct_cloud_
以下是只有上採樣方法為
SAMPLE_LOCAL_PLANE
才會用到的：
-
upsampling_radius_
-
upsampling_step_
以下是只有上採樣方法為
RANDOM_UNIFORM_DENSITY
才會用到的：
-
desired_num_points_in_radius_
以下是只有上採樣方法為
VOXEL_GRID_DILATION
才會用到的：
-
voxel_size_
-
dilation_iteration_num_
/** \brief The point cloud that will hold the estimated normals, if set. */
NormalCloudPtr normals_;
//distinct cloud是啥?
/** \brief The distinct point cloud that will be projected to the MLS surface. */
PointCloudInConstPtr distinct_cloud_;
/** \brief The search method template for indices. */
SearchMethod search_method_;
/** \brief A pointer to the spatial search object. */
KdTreePtr tree_;
/** \brief The order of the polynomial to be fit. */
int order_;
/** \brief The nearest neighbors search radius for each point. */
double search_radius_;
//用於計算權重
/** \brief Parameter for distance based weighting of neighbors (search_radius_ * search_radius_ works fine) */
double sqr_gauss_param_;
/** \brief Parameter that specifies whether the normals should be computed for the input cloud or not */
bool compute_normals_;
/** \brief Parameter that specifies the upsampling method to be used */
UpsamplingMethod upsample_method_;
/** \brief Radius of the circle in the local point plane that will be sampled
* \note Used only in the case of SAMPLE_LOCAL_PLANE upsampling
*/
double upsampling_radius_;
/** \brief Step size for the local plane sampling
* \note Used only in the case of SAMPLE_LOCAL_PLANE upsampling
*/
double upsampling_step_;
/** \brief Parameter that specifies the desired number of points within the search radius
* \note Used only in the case of RANDOM_UNIFORM_DENSITY upsampling
*/
int desired_num_points_in_radius_;
/** \brief True if the mls results for the input cloud should be stored
* \note This is forced to be true when using upsampling methods VOXEL_GRID_DILATION or DISTINCT_CLOUD.
*/
bool cache_mls_results_;
/** \brief Stores the MLS result for each point in the input cloud
* \note Used only in the case of VOXEL_GRID_DILATION or DISTINCT_CLOUD upsampling
*/
std::vector<MLSResult> mls_results_;
/** \brief Parameter that specifies the projection method to be used. */
MLSResult::ProjectionMethod projection_method_;
/** \brief The maximum number of threads the scheduler should use. */
unsigned int threads_;
/** \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
{
//...
};
/** \brief Voxel size for the VOXEL_GRID_DILATION upsampling method */
float voxel_size_;
/** \brief Number of dilation steps for the VOXEL_GRID_DILATION upsampling method */
int dilation_iteration_num_;
//係數的數量,由曲面的階數決定
/** \brief Number of coefficients, to be computed from the requested order.*/
int nr_coeff_;
//記錄projected_points/output中第i個點是對應到input中的哪一個點
/** \brief Collects for each point in output the corrseponding point in the input. */
PointIndicesPtr corresponding_input_indices_;
public成員函數
constructor和destructor
/** \brief Empty constructor. */
MovingLeastSquares () : CloudSurfaceProcessing<PointInT, PointOutT> (),
distinct_cloud_ (),
tree_ (),
order_ (2),
search_radius_ (0.0),
sqr_gauss_param_ (0.0),
compute_normals_ (false),
upsample_method_ (NONE),
upsampling_radius_ (0.0),
upsampling_step_ (0.0),
desired_num_points_in_radius_ (0),
cache_mls_results_ (true),
projection_method_ (MLSResult::SIMPLE),
threads_ (1),
voxel_size_ (1.0),
dilation_iteration_num_ (0),
nr_coeff_ (),
rng_uniform_distribution_ ()
{};
/** \brief Empty destructor */
~MovingLeastSquares () {}
getter和setter
以下這些函數用於對成員變量進行存取或賦值。
/** \brief Set whether the algorithm should also store the normals computed
* \note This is optional, but need a proper output cloud type
*/
inline void
setComputeNormals (bool compute_normals) { compute_normals_ = compute_normals; }
/** \brief Provide a pointer to the search object.
* \param[in] tree a pointer to the spatial search object.
*/
inline void
setSearchMethod (const KdTreePtr &tree)
{
tree_ = tree;
//一個本質是函數的成員變數
// Declare the search locator definition
search_method_ = [this] (pcl::index_t index, double radius, pcl::Indices& k_indices, std::vector<float>& k_sqr_distances)
{
return tree_->radiusSearch (index, radius, k_indices, k_sqr_distances, 0);
};
}
/** \brief Get a pointer to the search method used. */
inline KdTreePtr
getSearchMethod () const { return (tree_); }
/** \brief Set the order of the polynomial to be fit.
* \param[in] order the order of the polynomial
* \note Setting order > 1 indicates using a polynomial fit.
*/
inline void
setPolynomialOrder (int order) { order_ = order; }
/** \brief Get the order of the polynomial to be fit. */
inline int
getPolynomialOrder () const { return (order_); }
//sqr_gauss_param_是跟著search_radius_變動的
/** \brief Set the sphere radius that is to be used for determining the k-nearest neighbors used for fitting.
* \param[in] radius the sphere radius that is to contain all k-nearest neighbors
* \note Calling this method resets the squared Gaussian parameter to radius * radius !
*/
inline void
setSearchRadius (double radius) { search_radius_ = radius; sqr_gauss_param_ = search_radius_ * search_radius_; }
/** \brief Get the sphere radius used for determining the k-nearest neighbors. */
inline double
getSearchRadius () const { return (search_radius_); }
//實際上sqr_gauss_param_也可以自由設定
/** \brief Set the parameter used for distance based weighting of neighbors (the square of the search radius works
* best in general).
* \param[in] sqr_gauss_param the squared Gaussian parameter
*/
inline void
setSqrGaussParam (double sqr_gauss_param) { sqr_gauss_param_ = sqr_gauss_param; }
/** \brief Get the parameter for distance based weighting of neighbors. */
inline double
getSqrGaussParam () const { return (sqr_gauss_param_); }
/** \brief Set the upsampling method to be used
* \param method
*/
inline void
setUpsamplingMethod (UpsamplingMethod method) { upsample_method_ = method; }
/** \brief Set the distinct cloud used for the DISTINCT_CLOUD upsampling method. */
inline void
setDistinctCloud (PointCloudInConstPtr distinct_cloud) { distinct_cloud_ = distinct_cloud; }
/** \brief Get the distinct cloud used for the DISTINCT_CLOUD upsampling method. */
inline PointCloudInConstPtr
getDistinctCloud () const { return (distinct_cloud_); }
//這個circle說的是論文裡文氏圖的circle?
/** \brief Set the radius of the circle in the local point plane that will be sampled
* \note Used only in the case of SAMPLE_LOCAL_PLANE upsampling
* \param[in] radius the radius of the circle
*/
inline void
setUpsamplingRadius (double radius) { upsampling_radius_ = radius; }
/** \brief Get the radius of the circle in the local point plane that will be sampled
* \note Used only in the case of SAMPLE_LOCAL_PLANE upsampling
*/
inline double
getUpsamplingRadius () const { return (upsampling_radius_); }
/** \brief Set the step size for the local plane sampling
* \note Used only in the case of SAMPLE_LOCAL_PLANE upsampling
* \param[in] step_size the step size
*/
inline void
setUpsamplingStepSize (double step_size) { upsampling_step_ = step_size; }
/** \brief Get the step size for the local plane sampling
* \note Used only in the case of SAMPLE_LOCAL_PLANE upsampling
*/
inline double
getUpsamplingStepSize () const { return (upsampling_step_); }
/** \brief Set the parameter that specifies the desired number of points within the search radius
* \note Used only in the case of RANDOM_UNIFORM_DENSITY upsampling
* \param[in] desired_num_points_in_radius the desired number of points in the output cloud in a sphere of
* radius \ref search_radius_ around each point
*/
inline void
setPointDensity (int desired_num_points_in_radius) { desired_num_points_in_radius_ = desired_num_points_in_radius; }
/** \brief Get the parameter that specifies the desired number of points within the search radius
* \note Used only in the case of RANDOM_UNIFORM_DENSITY upsampling
*/
inline int
getPointDensity () const { return (desired_num_points_in_radius_); }
/** \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_); }
/** \brief Set whether the mls results should be stored for each point in the input cloud
* \param[in] cache_mls_results True if the mls results should be stored, otherwise false.
* \note The cache_mls_results_ is forced to be true when using upsampling method VOXEL_GRID_DILATION or DISTINCT_CLOUD.
* \note If memory consumption is a concern, then set it to false when not using upsampling method VOXEL_GRID_DILATION or DISTINCT_CLOUD.
*/
inline void
setCacheMLSResults (bool cache_mls_results) { cache_mls_results_ = cache_mls_results; }
/** \brief Get the cache_mls_results_ value (True if the mls results should be stored, otherwise false). */
inline bool
getCacheMLSResults () const { return (cache_mls_results_); }
/** \brief Set the method to be used when projection the point on to the MLS surface.
* \param method
* \note This is only used when polynomial fit is enabled.
*/
inline void
setProjectionMethod (MLSResult::ProjectionMethod method) { projection_method_ = method; }
/** \brief Get the current projection method being used. */
inline MLSResult::ProjectionMethod
getProjectionMethod () const { return (projection_method_); }
/** \brief Get the MLSResults for input cloud
* \note The results are only stored if setCacheMLSResults(true) was called or when using the upsampling method DISTINCT_CLOUD or VOXEL_GRID_DILATION.
* \note This vector is aligned with the input cloud indices, so use getCorrespondingIndices to get the correct results when using output cloud indices.
*/
inline const std::vector<MLSResult>&
getMLSResults () const { return (mls_results_); }
/** \brief Set the maximum number of threads to use
* \param threads the maximum number of hardware threads to use (0 sets the value to 1)
*/
inline void
setNumberOfThreads (unsigned int threads = 1)
{
threads_ = threads;
}
//記錄projected_points/output中第i個點是對應到input中的哪一個點
/** \brief Get the set of indices with each point in output having the
* corresponding point in input */
inline PointIndicesPtr
getCorrespondingIndices () const { return (corresponding_input_indices_); }
process
除了constructor，destructor及各成員函數的getter和setter外，只有
process
函數是public的，這也反映了
process
函數是MLS模塊主要入口的事實。
/** \brief Base method for surface reconstruction for all points given in <setInputCloud (), setIndices ()>
* \param[out] output the resultant reconstructed surface model
*/
void
process (PointCloudOut &output) override;
protected成員函數
searchForNeighbors
這個函數就只是把
search_method_
包裝起來，變成一個可以直接調用的函數而已。
/** \brief Search for the nearest neighbors of a given point using a radius search
* \param[in] index the index of the query point
* \param[out] indices the resultant vector of indices representing the neighbors within search_radius_
* \param[out] sqr_distances the resultant squared distances from the query point to the neighbors within search_radius_
*/
inline int
searchForNeighbors (pcl::index_t index, pcl::Indices &indices, std::vector<float> &sqr_distances) const
{
return (search_method_ (index, search_radius_, indices, sqr_distances));
}
下面幾個函數用於尋找點雲的擬合曲面，或進行上採樣：
//計算點雲裡第index個點的法向量?
//對第index個點的鄰居都做投影,得到projected_points和projected_points_normals?
/** \brief Smooth a given point and its neighborghood using Moving Least Squares.
* \param[in] index the index of the query point in the input cloud
* \param[in] nn_indices the set of nearest neighbors indices for pt
* \param[out] projected_points the set of projected points around the query point
* (in the case of upsampling method NONE, only the query point projected to its own fitted surface will be returned,
* in the case of the other upsampling methods, multiple points will be returned)
* \param[out] projected_points_normals the normals corresponding to the projected points
* \param[out] corresponding_input_indices the set of indices with each point in output having the corresponding point in input
* \param[out] mls_result stores the MLS result for each point in the input cloud
* (used only in the case of VOXEL_GRID_DILATION or DISTINCT_CLOUD upsampling)
*/
void
computeMLSPointNormal (pcl::index_t index,
const pcl::Indices &nn_indices,
PointCloudOut &projected_points,
NormalCloud &projected_points_normals,
PointIndices &corresponding_input_indices,
MLSResult &mls_result) const;
//在建構輸出點雲的時候會用到?
/** \brief This is a helper function for adding projected points
* \param[in] index the index of the query point in the input cloud
* \param[in] point the projected point to be added
* \param[in] normal the projected point's normal to be added
* \param[in] curvature the projected point's curvature
* \param[out] projected_points the set of projected points around the query point
* \param[out] projected_points_normals the normals corresponding to the projected points
* \param[out] corresponding_input_indices the set of indices with each point in output having the corresponding point in input
*/
void
addProjectedPointNormal (pcl::index_t index,
const Eigen::Vector3d &point,
const Eigen::Vector3d &normal,
double curvature,
PointCloudOut &projected_points,
NormalCloud &projected_points_normals,
PointIndices &corresponding_input_indices) const;
//沒介紹?
void
copyMissingFields (const PointInT &point_in,
PointOutT &point_out) const;
/** \brief Abstract surface reconstruction method.
* \param[out] output the result of the reconstruction
*/
void
performProcessing (PointCloudOut &output) override;
/** \brief Perform upsampling for the distinct-cloud and voxel-grid methods
* \param[out] output the result of the reconstruction
*/
void
performUpsampling (PointCloudOut &output);
private成員
以下這兩個成員變數都是
RANDOM_UNIFORM_DENSITY
上採樣方法才會用到的，這部分將留到
RANDOM_UNIFORM_DENSITY
章節時再一併做介紹。
/** \brief Random number generator algorithm. */
mutable std::mt19937 rng_;
/** \brief Random number generator using an uniform distribution of floats
* \note Used only in the case of RANDOM_UNIFORM_DENSITY upsampling
*/
std::unique_ptr<std::uniform_real_distribution<>> rng_uniform_distribution_;
用於獲取此類別的字串名稱。
/** \brief Abstract class get name method. */
std::string
getClassName () const { return ("MovingLeastSquares"); }
![《4-Points Congruent Sets for Robust Pairwise Surface Registration》论文研读[图]](data:image/gif;base64,R0lGODlhAQABAIAAAP///wAAACwAAAAAAQABAAACAkQBADs=)