这一节将介绍如何进行点云融合。
首先是加载文件,点云过滤什么的,这里不详讲了。
融合过程如下图所示:
首先,把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