PCL - ICP代碼研讀(十五 ) - CorrespondenceEstimation實現
前言
接續PCL - ICP代碼研讀(十四 ) - CorrespondenceEstimation架構,本篇主要介紹correspondence_estimation.h
中宣告但未給出實現的determineCorrespondences
和determineReciprocalCorrespondences
函數。
本篇對應到correspondence_estimation.hpp
這份檔案。
CorrespondenceEstimation
determineCorrespondences函數
determineCorrespondences
函數尋找兩點雲中距離小於max_distance
者當作點對,並填入correspondences
這個容器裡。
template <typename PointSource, typename PointTarget, typename Scalar>
void
CorrespondenceEstimation<PointSource, PointTarget, Scalar>::determineCorrespondences(
pcl::Correspondences& correspondences, double max_distance)
{
首先調用CorrespondenceEstimationBase::initCompute函數對搜索方法tree_
初始化:
if (!initCompute())
return;
距離平方超過max_dist_sqr
的點對會被排除:
// max_distance預設值是std::numeric_limits<double>::max(),這樣不會overflow?
double max_dist_sqr = max_distance * max_distance;
一開始先將correspondences
resize成點雲索引indices_
的大小:
// 一開始先resize成輸入點雲索引的大小
correspondences.resize(indices_->size());
與最近鄰搜索相關的變數:
pcl::Indices index(1);
std::vector<float> distance(1);
pcl::Correspondence corr;
unsigned int nr_valid_correspondences = 0;
如果source點雲和target點雲的點型別相同,則執行if分支:
// Check if the template types are the same. If true, avoid a copy.
// Both point types MUST be registered using the POINT_CLOUD_REGISTER_POINT_STRUCT
// macro!
if (isSamePointType<PointSource, PointTarget>()) {
// Iterate over the input set of source indices
遍歷輸入點雲的索引:
// 遍歷輸入點雲的索引
for (const auto& idx : (*indices_)) {
尋找目標點雲中離(*input_)[idx]
最近的點:
// 尋找目標點雲中離input_[idx]最近的點
tree_->nearestKSearch((*input_)[idx], 1, index, distance);
排除距離平方超過max_dist_sqr
(即距離大於max_distance
)的點:
// 排除距離大於max_distance的點
if (distance[0] > max_dist_sqr)
continue;
由source點雲中的idx
對應到target點雲的index[0]
點:
// 由source點雲中的idx對應到target點雲的index[0]點
corr.index_query = idx;
corr.index_match = index[0];
// 距離平方?
corr.distance = distance[0];
填入correspondences
這個容器:
// 儲存到correspondences數據結構中
correspondences[nr_valid_correspondences++] = corr;
}
}
如果source點雲和target點雲的點型別不同,則執行else分支。與if分支相比,這裡需要先定義一個類型為PointTarget
的點,然後將source點雲中的點(*input_)[idx]
複製給pt
。
else {
PointTarget pt;
// Iterate over the input set of source indices
for (const auto& idx : (*indices_)) {
// Copy the source data to a target PointTarget format so we can search in the
// tree
// 先把原始點雲中的點複製到目標點雲類型的點pt中,然後再進行搜索
copyPoint((*input_)[idx], pt);
tree_->nearestKSearch(pt, 1, index, distance);
if (distance[0] > max_dist_sqr)
continue;
corr.index_query = idx;
corr.index_match = index[0];
corr.distance = distance[0];
correspondences[nr_valid_correspondences++] = corr;
}
}
將correspondences
容器縮小為有效點對的個數nr_valid_correspondences
:
// 知道實際有多少配對後再resize成配對數:nr_valid_correspondences
correspondences.resize(nr_valid_correspondences);
deinitCompute();
}
determineReciprocalCorrespondences函數
與determineCorrespondences
函數類似,determineReciprocalCorrespondences
函數也是用於尋找點對。但是這裡的方式較為嚴格,要求配對的兩點互為最近鄰,也就是說:除了(*target_)[target_idx]
必須是離(*input_)[idx]
最近的鄰居外,還要求(*input_)[idx]
必須是(*target_)[target_idx]
在source點雲中的最近鄰。
template <typename PointSource, typename PointTarget, typename Scalar>
void
CorrespondenceEstimation<PointSource, PointTarget, Scalar>::
determineReciprocalCorrespondences(pcl::Correspondences& correspondences,
double max_distance)
{
if (!initCompute())
return;
// setup tree for reciprocal search
// Set the internal point representation of choice
if (!initComputeReciprocal())
return;
double max_dist_sqr = max_distance * max_distance;
correspondences.resize(indices_->size());
pcl::Indices index(1);
std::vector<float> distance(1);
pcl::Indices index_reciprocal(1);
std::vector<float> distance_reciprocal(1);
pcl::Correspondence corr;
unsigned int nr_valid_correspondences = 0;
int target_idx = 0;
// Check if the template types are the same. If true, avoid a copy.
// Both point types MUST be registered using the POINT_CLOUD_REGISTER_POINT_STRUCT
// macro!
if (isSamePointType<PointSource, PointTarget>()) {
// Iterate over the input set of source indices
for (const auto& idx : (*indices_)) {
tree_->nearestKSearch((*input_)[idx], 1, index, distance);
if (distance[0] > max_dist_sqr)
continue;
// target點雲中離source點雲的第idx個點最近的點是的target_idx
target_idx = index[0];
tree_reciprocal_->nearestKSearch(
(*target_)[target_idx], 1, index_reciprocal, distance_reciprocal);
if (distance_reciprocal[0] > max_dist_sqr || idx != index_reciprocal[0])
continue;
// source點雲中離target點雲的第target_idx個點最近的點是的index_reciprocal[0],也就是idx
corr.index_query = idx;
corr.index_match = index[0];
corr.distance = distance[0];
correspondences[nr_valid_correspondences++] = corr;
}
}
else {
PointTarget pt_src;
PointSource pt_tgt;
// Iterate over the input set of source indices
for (const auto& idx : (*indices_)) {
// Copy the source data to a target PointTarget format so we can search in the
// tree
copyPoint((*input_)[idx], pt_src);
tree_->nearestKSearch(pt_src, 1, index, distance);
if (distance[0] > max_dist_sqr)
continue;
target_idx = index[0];
// Copy the target data to a target PointSource format so we can search in the
// tree_reciprocal
copyPoint((*target_)[target_idx], pt_tgt);
tree_reciprocal_->nearestKSearch(
pt_tgt, 1, index_reciprocal, distance_reciprocal);
if (distance_reciprocal[0] > max_dist_sqr || idx != index_reciprocal[0])
continue;
corr.index_query = idx;
corr.index_match = index[0];
corr.distance = distance[0];
correspondences[nr_valid_correspondences++] = corr;
}
}
correspondences.resize(nr_valid_correspondences);
deinitCompute();
}
} // namespace registration
} // namespace pcl
//#define PCL_INSTANTIATE_CorrespondenceEstimation(T,U) template class PCL_EXPORTS
// pcl::registration::CorrespondenceEstimation<T,U>;
#endif /* PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_H_ */