PCL均匀采样的一点修改

pcl中均匀采样代码中保留的点，并不是距网格中心最近的点，而是距离网格左下角坐标最近的点。
下面是源码：

    Eigen::Vector4i ijk = Eigen::Vector4i::Zero ();
    ijk[0] = static_cast<int> (floor (input_->points[(*indices_)[cp]].x * inverse_leaf_size_[0]));
    ijk[1] = static_cast<int> (floor (input_->points[(*indices_)[cp]].y * inverse_leaf_size_[1]));
    ijk[2] = static_cast<int> (floor (input_->points[(*indices_)[cp]].z * inverse_leaf_size_[2]));

以上表示的是(*indices_)[cp]点的网格坐标，也就是网格的左下角坐标，floor为向下取整。

// Check to see if this point is closer to the leaf center than the previous one we saved
    float diff_cur   = (input_->points[(*indices_)[cp]].getVector4fMap () - ijk.cast<float> ()).squaredNorm ();
    float diff_prev  = (input_->points[leaf.idx].getVector4fMap ()        - ijk.cast<float> ()).squaredNorm ();
// If current point is closer, copy its index instead
    if (diff_cur < diff_prev)
      leaf.idx = (*indices_)[cp];

上面为计算当前网格叶子节点和上一个网格叶子节点与ijk网格坐标的距离，选择距离小的点作为叶子节点的保存值。
如上面所说，ijk表示的是网格左下角的坐标，这样计算距离显然是有问题的，为了验证上述猜想，做以下试验：

	cloud->push_back(pcl::PointXYZ(0.4, 0.4, 0.4));
	cloud->push_back(pcl::PointXYZ(0.3, 0.3, 0.3));
	cloud->push_back(pcl::PointXYZ(0.1, 0.1, 0.1));
	cloud->push_back(pcl::PointXYZ(0.6, 0.6, 0.6));
	cloud->push_back(pcl::PointXYZ(0.5, 0.5, 0.5));
	cloud->push_back(pcl::PointXYZ(0.8, 0.8, 0.8));

将数据集设置为以上形式，点数据分布在0-1之间，均匀采样参数设置如下：

	pcl::UniformSampling<pcl::PointXYZ> US;
	US.setInputCloud(cloud);
	US.setRadiusSearch(1.f);// 设置网格大小
	US.filter(*cloud_filtered);

将网格大小设置为1.0，网格划分情况为（0，0，0）-（1，1，1）为一个网格，则网格中心应为（0.5，0.5，0.5），上述数据距离网格中心最近的点的坐标应当为（0.5，0.5，0.5），以下为测试结果：

采样后点数：1
坐标：（0.1，0.1，0.1）

以上结果表明，均匀采样并不是保存的距网格中心（0.5，0.5，0.5）最近的点，而是保留的距网格左下角（0，0，0）最近的点。
修改源码如下：

		// Check to see if this point is closer to the leaf center than the previous one we saved
		float diff_cur = (input_->points[(*indices_)[cp]].getVector4fMap() - ijk.cast<float>()*search_radius_- leaf_size_/2).squaredNorm();
		float diff_prev = (input_->points[leaf.idx].getVector4fMap() - ijk.cast<float>()*search_radius_- leaf_size_/2).squaredNorm();

将左下角坐标平移至网格中心点。
上述代码运行结果如下：

采样后点数：1
坐标：（0.5，0.5，0.5）

兔子点云滤波结果对比图，网格大小为0.01

pcl自带类

修改后的类

pcl
0.01863 0.24982999 0.20953
0.0094699999 0.2462 0.20942
-0.02331 0.23986 0.19967
0.01887 0.25999001 0.20316
0.0094100004 0.2552 0.20457
0.019130001 0.26072001 0.20178001
0.028589999 0.26563001 0.20034
0.01347 0.25977999 0.19944
-0.02008 0.24180999 0.19879
0.0095100002 0.25843999 0.19881999
0.0090699997 0.26001 0.19586
0.019680001 0.26251 0.19897
0.046149999 0.26989999 0.19915

改动后
0.01297 0.24903999 0.20716999
0.0040799999 0.24652 0.20568
-0.02699 0.23971 0.19765
0.01239 0.25630999 0.20502
0.0054500001 0.25386 0.20395
0.01716 0.26016 0.20146
0.025350001 0.26379001 0.20128
0.01248 0.25944999 0.19927999
-0.024219999 0.24324 0.19385999
0.0038399999 0.25738999 0.1965
0.0066399998 0.26102999 0.19256
0.01555 0.26370001 0.19397999
0.043400001 0.26747999 0.19752

两者的差距还是存在的，以上有不对的地方请在评论区讨论。