PCL - ICP代碼研讀(十二 ) - CorrespondenceEstimationBase架構
前言
CorrespondenceEstimationBase
是一個抽象類別,提供了如CorrespondenceEstimation
,CorrespondenceEstimationNormalShooting
等用於估計兩點雲間配對的類別的代碼框架。
本篇主要對應到correspondence_estimation.h
這個檔案。
CorrespondenceEstimationBase
#pragma once
#include <pcl/common/io.h> // for getFields
#include <pcl/registration/correspondence_types.h>
#include <pcl/search/kdtree.h>
#include <pcl/memory.h>
#include <pcl/pcl_base.h>
#include <pcl/pcl_macros.h>
#include <string>
namespace pcl {
namespace registration {
/** \brief Abstract @b CorrespondenceEstimationBase class.
* All correspondence estimation methods should inherit from this.
* \author Radu B. Rusu
* \ingroup registration
*/
template <typename PointSource, typename PointTarget, typename Scalar = float>
class CorrespondenceEstimationBase : public PCLBase<PointSource> {
public:
using
定義名稱,方便後續使用:
using Ptr =
shared_ptr<CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>>;
using ConstPtr =
shared_ptr<const CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>>;
因為PCLBase
已經定義了deinitCompute
,input_
,indices_
,setIndices
,這裡用了using
表示沿用PCLBase
中的定義。
參考改變成員的訪問權限,此處using
的另外一個作用為:將原來是PCLBase
的protected成員的導入CorrespondenceEstimationBase
的public權限區塊。
// 有自己override一個initCompute
// using PCLBase<PointSource>::initCompute;
using PCLBase<PointSource>::deinitCompute;
using PCLBase<PointSource>::input_;
using PCLBase<PointSource>::indices_;
using PCLBase<PointSource>::setIndices;
其餘皆是用using
來定義名稱,方便後續使用。
using KdTree = pcl::search::KdTree<PointTarget>;
using KdTreePtr = typename KdTree::Ptr;
using KdTreeReciprocal = pcl::search::KdTree<PointSource>;
using KdTreeReciprocalPtr = typename KdTree::Ptr;
using PointCloudSource = pcl::PointCloud<PointSource>;
using PointCloudSourcePtr = typename PointCloudSource::Ptr;
using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr;
using PointCloudTarget = pcl::PointCloud<PointTarget>;
using PointCloudTargetPtr = typename PointCloudTarget::Ptr;
using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr;
using PointRepresentationConstPtr = typename KdTree::PointRepresentationConstPtr;
constructor和destructor
/** \brief Empty constructor. */
CorrespondenceEstimationBase()
: corr_name_("CorrespondenceEstimationBase")
, tree_(new pcl::search::KdTree<PointTarget>)
, tree_reciprocal_(new pcl::search::KdTree<PointSource>)
, target_()
, point_representation_()
, input_transformed_()
, target_cloud_updated_(true)
, source_cloud_updated_(true)
, force_no_recompute_(false)
, force_no_recompute_reciprocal_(false)
{}
/** \brief Empty destructor */
~CorrespondenceEstimationBase() {}
點雲的setter和getter
與Registration::setInputSource的代碼對照,可以發現這個函數的內容與Registration::setInputSource
幾乎一致,只差在少了對輸入點雲cloud
是否為空的檢查,並多了input_fields_
相關的代碼。
/** \brief Provide a pointer to the input source
* (e.g., the point cloud that we want to align to the target)
*
* \param[in] cloud the input point cloud source
*/
inline void
setInputSource(const PointCloudSourceConstPtr& cloud)
{
// 表示需要再算一次tree_reciprocal_
source_cloud_updated_ = true;
PCLBase<PointSource>::setInputCloud(cloud);
input_fields_ = pcl::getFields<PointSource>();
}
getInputSource
直接返回input_
點雲:
/** \brief Get a pointer to the input point cloud dataset target. */
inline PointCloudSourceConstPtr const
getInputSource()
{
return (input_);
}
setInputTarget
的定義位於correspondence_estimation.hpp
這份檔案:
/** \brief Provide a pointer to the input target
* (e.g., the point cloud that we want to align the input source to)
* \param[in] cloud the input point cloud target
*/
// 不需要類似target_cloud_updated_ = true;的語句? -> 在hpp裡有
// 它的定義在hpp裡面
inline void
setInputTarget(const PointCloudTargetConstPtr& cloud);
getInputTarget
直接返回target_
點雲:
/** \brief Get a pointer to the input point cloud dataset target. */
inline PointCloudTargetConstPtr const
getInputTarget()
{
return (target_);
}
法向量相關函數
在預設情況下,點對估計器是不需要點雲有法向量的。這也是此處requiresSourceNormals
和requiresTargetNormals
函數回傳false的原因。
注意這些函數都被宣告為virtual
的,如此一來,CorrespondenceEstimationBase
的各個子類別便可以自行決定是否需要法向量。
/** \brief See if this rejector requires source normals */
virtual bool
requiresSourceNormals() const
{
return (false);
}
/** \brief Abstract method for setting the source normals */
virtual void setSourceNormals(pcl::PCLPointCloud2::ConstPtr /*cloud2*/)
{
PCL_WARN("[pcl::registration::%s::setSourceNormals] This class does not require "
"input source normals\n",
getClassName().c_str());
}
/** \brief See if this rejector requires target normals */
virtual bool
requiresTargetNormals() const
{
return (false);
}
/** \brief Abstract method for setting the target normals */
virtual void setTargetNormals(pcl::PCLPointCloud2::ConstPtr /*cloud2*/)
{
PCL_WARN("[pcl::registration::%s::setTargetNormals] This class does not require "
"input target normals\n",
getClassName().c_str());
}
索引的setter和getter
setIndicesSource
函數調用了pcl::PCLBase::setIndices
,詳見# PCL - PCLBase代碼研讀(二)- PCLBase實現 - pcl::PCLBase::setIndices。(這裡不需要source_cloud_updated_ = true;
?)
/** \brief Provide a pointer to the vector of indices that represent the
* input source point cloud.
* \param[in] indices a pointer to the vector of indices
*/
// 表示輸入點雲的indices向量?
inline void
setIndicesSource(const IndicesPtr& indices)
{
// 設定好indices_這個成員變數
setIndices(indices);
}
其餘三個函數的意義顯而易見:
/** \brief Get a pointer to the vector of indices used for the source dataset. */
inline IndicesPtr const
getIndicesSource()
{
return (indices_);
}
/** \brief Provide a pointer to the vector of indices that represent the input target
* point cloud. \param[in] indices a pointer to the vector of indices
*/
// 表示target點雲的indices向量?
inline void
setIndicesTarget(const IndicesPtr& indices)
{
// 這裡有target_cloud_updated_ = true, setInputTarget函數的定義裡也有
target_cloud_updated_ = true;
target_indices_ = indices;
}
/** \brief Get a pointer to the vector of indices used for the target dataset. */
inline IndicesPtr const
getIndicesTarget()
{
return (target_indices_);
}
最近鄰查找相關函數
這四個函數的功用也顯而易見,要注意的是,在setSearchMethodTarget
(setSearchMethodSource
)中,需要這一行:target_cloud_updated_ = true;
(source_cloud_updated_ = true;
)。這表示在initCompute
(initComputeReciprocal
)函數中,還得更新tree_
(tree_reciprocal_
)。
/** \brief Provide a pointer to the search object used to find correspondences in
* the target cloud.
* \param[in] tree a pointer to the spatial search object.
* \param[in] force_no_recompute If set to true, this tree will NEVER be
* recomputed, regardless of calls to setInputTarget. Only use if you are
* confident that the tree will be set correctly.
*/
inline void
setSearchMethodTarget(const KdTreePtr& tree, bool force_no_recompute = false)
{
tree_ = tree;
force_no_recompute_ = force_no_recompute;
// Since we just set a new tree, we need to check for updates
// 需要再調用一次initCompute,執行tree_->setInputCloud,target_cloud_updated_才會被設回false
target_cloud_updated_ = true;
}
/** \brief Get a pointer to the search method used to find correspondences in the
* target cloud. */
inline KdTreePtr
getSearchMethodTarget() const
{
return (tree_);
}
/** \brief Provide a pointer to the search object used to find correspondences in
* the source cloud (usually used by reciprocal correspondence finding).
* \param[in] tree a pointer to the spatial search object.
* \param[in] force_no_recompute If set to true, this tree will NEVER be
* recomputed, regardless of calls to setInputSource. Only use if you are
* extremely confident that the tree will be set correctly.
*/
inline void
setSearchMethodSource(const KdTreeReciprocalPtr& tree,
bool force_no_recompute = false)
{
tree_reciprocal_ = tree;
force_no_recompute_reciprocal_ = force_no_recompute;
// Since we just set a new tree, we need to check for updates
// 需要再調用一次initComputeReciprocal,執行tree_reciprocal_->setInputCloud,source_cloud_updated_才會被設回false
source_cloud_updated_ = true;
}
/** \brief Get a pointer to the search method used to find correspondences in the
* source cloud. */
inline KdTreeReciprocalPtr
getSearchMethodSource() const
{
return (tree_reciprocal_);
}
估計點對的函數
這兩個函數是CorrespondenceEstimationBase
的核心所在,用於估計兩點雲間的點對。
/** \brief Determine the correspondences between input and target cloud.
* \param[out] correspondences the found correspondences (index of query point, index
* of target point, distance) \param[in] max_distance maximum allowed distance between
* correspondences
*/
virtual void
determineCorrespondences(
pcl::Correspondences& correspondences,
double max_distance = std::numeric_limits<double>::max()) = 0;
/** \brief Determine the reciprocal correspondences between input and target cloud.
* A correspondence is considered reciprocal if both Src_i has Tgt_i as a
* correspondence, and Tgt_i has Src_i as one.
*
* \param[out] correspondences the found correspondences (index of query and target
* point, distance) \param[in] max_distance maximum allowed distance between
* correspondences
*/
virtual void
determineReciprocalCorrespondences(
pcl::Correspondences& correspondences,
double max_distance = std::numeric_limits<double>::max()) = 0;
point_representation_的setter
與point_representation_
相關的代碼皆待研究。
/** \brief Provide a boost shared pointer to the PointRepresentation to be used
* when searching for nearest neighbors.
*
* \param[in] point_representation the PointRepresentation to be used by the
* k-D tree for nearest neighbor search
*/
// 這個的作用是?
inline void
setPointRepresentation(const PointRepresentationConstPtr& point_representation)
{
point_representation_ = point_representation;
}
clone函數
返回一份自己的克隆:
/** \brief Clone and cast to CorrespondenceEstimationBase */
virtual typename CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::Ptr
clone() const = 0;
protected成員變數
點雲相關:
target_
:target點雲target_indices_
:target點雲索引
最近鄰搜索相關(此處與Registration - 最近鄰搜索相關protected成員變數一致):
tree_
tree_reciprocal_
target_cloud_updated_
source_cloud_updated_
force_no_recompute_
force_no_recompute_reciprocal_
其它:
corr_name_
:點對估計算法的名稱point_representation_
:待研究input_transformed_
:目前尚未看到它在哪裡被使用input_fields_
:儲存source點雲點的各欄位資訊(pcl::PCLPointField
)的向量
protected:
/** \brief The correspondence estimation method name. */
std::string corr_name_;
/** \brief A pointer to the spatial search object used for the target dataset. */
KdTreePtr tree_;
/** \brief A pointer to the spatial search object used for the source dataset. */
KdTreeReciprocalPtr tree_reciprocal_;
// source也有對應的input_
/** \brief The input point cloud dataset target. */
PointCloudTargetConstPtr target_;
// source也有對應的indices_
/** \brief The target point cloud dataset indices. */
IndicesPtr target_indices_;
// 作用為何?
/** \brief The point representation used (internal). */
PointRepresentationConstPtr point_representation_;
// 作用為何?
/** \brief The transformed input source point cloud dataset. */
PointCloudTargetPtr input_transformed_;
// 作用為何?
/** \brief The types of input point fields available. */
std::vector<pcl::PCLPointField> input_fields_;
/** \brief Variable that stores whether we have a new target cloud, meaning we need to
* pre-process it again. This way, we avoid rebuilding the kd-tree for the target
* cloud every time the determineCorrespondences () method is called. */
// 如果是true,代表需要重新計算tree_
bool target_cloud_updated_;
/** \brief Variable that stores whether we have a new source cloud, meaning we need to
* pre-process it again. This way, we avoid rebuilding the reciprocal kd-tree for the
* source cloud every time the determineCorrespondences () method is called. */
// 如果是true,代表需要重新計算tree_reciprocal_
bool source_cloud_updated_;
/** \brief A flag which, if set, means the tree operating on the target cloud
* will never be recomputed*/
bool force_no_recompute_;
/** \brief A flag which, if set, means the tree operating on the source cloud
* will never be recomputed*/
bool force_no_recompute_reciprocal_;
};
protected成員函數
getClassName
回傳corr_name_
,即配對估計器的名稱:
/** \brief Abstract class get name method. */
inline const std::string&
getClassName() const
{
return (corr_name_);
}
記得在using那部分的代碼中,using PCLBase<PointSource>::initCompute;
這一行是被注釋掉的,這是因為CorrespondenceEstimationBase
有自己定義的initCompute
函數。
/** \brief Internal computation initialization. */
bool
initCompute();
source點雲版本的initCompute
函數:
/** \brief Internal computation initialization for reciprocal correspondences. */
bool
initComputeReciprocal();