前言
由于有2个点云需要合并, 坐标相同, 主要是要将某些属性合并到一起。
合并前需要按照坐标对其进行排序, 使2个点云中对应点的index也对应。
一些尝试
按x坐标排序, 代码就一行
std::sort(cloud_a->begin(), cloud_a->end(), [](PointA pt1, PointA pt2) {return pt1.x < pt2.x; });
跑完后发现有很多点并不对应, 检查后发现是因为有相同x坐标点所引起的。
尝试些简单粗暴的方法
std::sort(cloud_b->begin(), cloud_b->end(), [](PointB pt1, PointB pt2) {return pt1.z < pt2.z; });
std::sort(cloud_b->begin(), cloud_b->end(), [](PointB pt1, PointB pt2) {return pt1.y < pt2.y; });
std::sort(cloud_b->begin(), cloud_b->end(), [](PointB pt1, PointB pt2) {return pt1.x < pt2.x; });
发现不行, 还是会有同样的问题.
解决方案
先定义两个点的比对方法, 由于用了类别模板, 所以没有重载运算符。
template <typename PointT>
inline bool point_comepare(const PointT& pt1, const PointT& pt2)
{
// 排序优先级:x轴 > y轴 > z轴
if (pt1.x < pt2.x) return true;
else if (pt1.x > pt2.x) return false;
else if (pt1.y < pt2.y) return true;
else if (pt1.y > pt2.y) return false;
else if (pt1.z < pt2.z) return true;
else return false;
}
执行排序, 调用上述比对方法。
std::sort(cloud_a->begin(), cloud_a->end(), [](PointA pt1, PointA pt2) {return point_comepare<PointA>(pt1, pt2); });
std::sort(cloud_b->begin(), cloud_b->end(), [](PointB pt1, PointB pt2) {return point_comepare<PointB>(pt1, pt2); });
运行后检查得到预期结果。
后续
后来发现我这里的其中一个点云进行过多种变换操作, 由于舍入误差, 该点云与源点云对应的某些点坐标产生了细微偏差(点云中最小点间距0.1m, 该偏差不大于0.001m)。
该原因导致排序法无法使2个点云中对应点的index相对应。
最后使用了基于KDTree的逐点匹配法, 虽然效率低下, 但是暂时没找到别的解决方法。
//构建临近搜索环境
pcl::KdTreeFLANN<PointB>kdtree;
kdtree.setInputCloud(cloud_b);
PointB searchPoint;
std::vector<int>pointIdxNKNSearch(1);
std::vector<float>pointNKNSquaredDistance(1);
//逐点搜索对应点
for (size_t i = 0; i < cloud_a->points.size(); i++) {
kdtree.nearestKSearch(searchPoint, 1, pointIdxNKNSearch, pointNKNSquaredDistance);
cout << i << "-" << pointIdxNKNSearch[0] << endl;
}