基于Kinect v2+PCL的模型奶牛重建（下）——点云融合

这一节将介绍如何进行点云融合。

首先是加载文件，点云过滤什么的，这里不详讲了。

融合过程如下图所示：

首先，把0当作是Target，1和2当作是Source。1和2分别转60度之后，分别与0进行配准（旋转时计算这3帧的重心，然后绕其重心进行旋转）。配准后，0的点云是不变的，变的分别是1和2的点云。配准采用的是向量ICP 方法，下面仅以前面3帧为例给出代码，后面3帧处理过程是一样的，把3当作是Target，4和5当作是Source。

 

Eigen::Vector3f totalMass1 = Eigen::Vector3f::Zero();//记录所有点云的质心
int totalNumberOfPoints1 = 0;//记录所有点云的点个数
for (int i = 0; i < 3; ++i)
{
	totalNumberOfPoints1 += data[i].cloudWithNormal->size();
	totalMass1 += data[i].mass * data[i].cloudWithNormal->size();
}
totalMass1 /= totalNumberOfPoints1;

Eigen::Vector3f upVector(0, 1.0, 0);
Eigen::Matrix4f rotationMatrix = rot_mat(totalMass1, upVector, M_PI / 3);
pcl::transformPointCloudWithNormals(*data[1].cloudWithNormal, *data[1].cloudWithNormal, rotationMatrix);
UpdatePCDMass(data[1]);

rotationMatrix = rot_mat(totalMass1, upVector, -M_PI / 3);
pcl::transformPointCloudWithNormals(*data[2].cloudWithNormal, *data[2].cloudWithNormal, rotationMatrix);
UpdatePCDMass(data[2]);

pcl::IterativeClosestPointWithNormals<pcl::PointNormal, pcl::PointNormal> icpWithNormals;
icpWithNormals.setMaxCorrespondenceDistance(0.5);
icpWithNormals.setMaximumIterations(100);
icpWithNormals.setTransformationEpsilon(1e-10);
icpWithNormals.setEuclideanFitnessEpsilon(0.01);

icpWithNormals.setInputCloud(data[1].cloudWithNormal);
icpWithNormals.setInputTarget(data[0].cloudWithNormal);
icpWithNormals.align(*data[1].cloudWithNormal);

icpWithNormals.setInputCloud(data[2].cloudWithNormal);
icpWithNormals.setInputTarget(data[0].cloudWithNormal);
icpWithNormals.align(*data[2].cloudWithNormal);

pcl::PointCloud<pcl::PointNormal>::Ptr Front(new pcl::PointCloud<pcl::PointNormal>);

*Front += *data[0].cloudWithNormal;
*Front += *data[1].cloudWithNormal;
*Front += *data[2].cloudWithNormal;
UpdatePCDMass(data[1]);
UpdatePCDMass(data[2]);

经过上述处理后，可以得Front和Back这两个分别代表前面与后面的点云。接下来就是要把前面与后面的点云进行融合。

 

然而直接融合是一件非常困难的事情，因为其能找到的Corresponding Points不仅少，而且不准确。于是我们改为对奶牛底下的盒子进行前后配准。

我们把Front点云当作Target，把Back点云当作Source。先把Back点云旋转180度：

 

Eigen::Vector3f mass = Eigen::Vector3f::Zero();
for (int i = 0; i < Back->points.size(); ++i){
	mass(0) += Back->points[i].x;
	mass(1) += Back->points[i].y;
	mass(2) += Back->points[i].z;
}
mass /= Back->points.size();
rotationMatrix = rot_mat(mass, upVector, M_PI);
pcl::transformPointCloudWithNormals(*Back, *Back, rotationMatrix);

然后我们使用AABB（Axially Aligned Bounding Box）对前后点云进行初始对齐，处理过程为把Back点云向Front点云进行移动：

 

 

AABB BackAABB = computerAABB(Back);
AABB FrontAABB = computerAABB(Front);
Eigen::Vector3f diff = FrontAABB.center - BackAABB.center;
Eigen::Matrix4f translationMatrix = Eigen::Matrix4f::Identity();
translationMatrix(0, 3) = diff(0);
translationMatrix(1, 3) = diff(1);
translationMatrix(2, 3) = FrontAABB.max(2) - BackAABB.min(2) - (FrontAABB.max(2) - FrontAABB.min(2))*0.9;
pcl::transformPointCloudWithNormals(*Back, *Back, translationMatrix);

上面代码倒数第二行中，理论上应该赋值为diff（2）。然而这样赋值之后前后配准得到的点云有偏移，不太准确，之后我再讲讲为什么会这么赋值。

 

接下来就是要前后配准了。我们的策略是对盒子进行前后配准。判断该点云是否是盒子的点云，我们可以通过上一节得到的yMax2来判断。Y值小于yMax2的点即为盒子点云的 点。通过AABB初始对齐后，先对盒子的左右面进行配准。思想挺简单：每次迭代时，分别提取Front点云和Back点云中盒子点云的法向量朝左右两边的点。分别定义朝向左右两边的法向量为plane_left(-1, 0, 0)和plane_right(1, 0, 0)，并设置一个阈值，如果其法向量与这两个法向量其中之一的夹角小于10度，我们就认为是我们想要的点。然后我们还是使用法向量进行配准，具体代码如下：

 

const float yMax = -0.218759;

Eigen::Vector3f plane_left(-1, 0, 0);
Eigen::Vector3f plane_right(1, 0, 0);
float cos_angle = cos(M_PI * 10 / 180);//角度阈值

cout << "左右面配准" << endl;
int iteration = 100;
for (int iter = 0; iter < iteration; ++iter)
{
	pcl::IndicesPtr source_indices(new std::vector<int>());
	for (int i = 0; i < Back->points.size(); ++i){
		if (Back->points[i].y>yMax)//如果其Y值大于yMax，那么它不是盒子的点云，下同
			continue;
		Eigen::Vector3f n = Back->points[i].getNormalVector3fMap();
		n.normalize();
		if (n.transpose() * plane_left > cos_angle){
			source_indices->push_back(i);
			continue;
		}
		if (n.transpose() * plane_right > cos_angle){
			source_indices->push_back(i);
		}
	}

	pcl::IndicesPtr target_indices(new std::vector<int>());
	for (int i = 0; i < Front->points.size(); ++i){
		if (Front->points[i].y>yMax)
			continue;
		Eigen::Vector3f n = Front->points[i].getNormalVector3fMap();
		n.normalize();
		if (n.transpose() * plane_left > cos_angle){
			target_indices->push_back(i);
			continue;
		}
		if (n.transpose() * plane_right > cos_angle){
			target_indices->push_back(i);
		}
	}

	pcl::registration::CorrespondenceEstimationNormalShooting<pcl::PointNormal, pcl::PointNormal, pcl::PointNormal> correst;
	correst.setInputCloud(Back);
	correst.setSourceNormals(Back);
	correst.setInputTarget(Front);
	correst.setIndicesSource(source_indices);
	correst.setIndicesTarget(target_indices);
	correst.setKSearch(15);
	pcl::CorrespondencesPtr all_correspondences(new pcl::Correspondences);
	correst.determineReciprocalCorrespondences(*all_correspondences);

	pcl::registration::CorrespondenceRejectorSurfaceNormal rejector;
	rejector.initializeDataContainer<pcl::PointNormal, pcl::PointNormal>();
	rejector.setInputSource<pcl::PointNormal>(Back);
	rejector.setInputTarget<pcl::PointNormal>(Front);
	rejector.setInputNormals<pcl::PointNormal, pcl::PointNormal>(Back);
	rejector.setTargetNormals<pcl::PointNormal, pcl::PointNormal>(Front);
	rejector.setInputCorrespondences(all_correspondences);
	rejector.setThreshold(M_PI * 10 / 180);
	pcl::CorrespondencesPtr correspondences_after_rejector(new pcl::Correspondences);
	rejecto