PCL拟合并绘制平面(二)

普通的点云平面拟合方式在一般情况下可以得到较好的平面拟合效果，但是容易出现平面拟合错误或是拟合的平面不是最优的情况。此时就需要根据自己的实际使用情况，调整平面拟合的迭代次数以及收敛条件。

使用RANSAC迭代的方式，获取所有迭代过程中的最优平面，虽然速度上不如普通的平面拟合方式，但是准确度有一定的提升。下面是具体实现的方式：

1、C++实现

随机处理函数：

	//随机处理static bool m_already_seeded = false;inline void SeedRand(int seed){srand(seed);}inline void SeedRandOnce(int seed){//if (!m_already_seeded)//{SeedRand(seed);m_already_seeded = true;//}}inline int RandomInt(int min, int max){int d = max - min + 1;return int(((double)rand() / ((double)RAND_MAX + 1.0)) * d) + min;}

主要函数实现部分：

	//部分用到的参数的初始值 mParams.VoxelSize=3.0 mParams.MaxModelFitIterations=2000//mParams.SegDistanceThreshold=0.3//点云的单位是mmvoid PlaneFittingRansac(PointCloudT::Ptr cloudsource, PointCloudT::Ptr cloudfiltered, PointCloudT::Ptr cloudseg, pcl::ModelCoefficients::Ptr coefficients){//点云下采样PointCloudT::Ptr cloudDownSampling;cloudDownSampling.reset(new(PointCloudT));if (cloudsource->points.size() > 0 && cloudsource->points.size() < 20000){cloudDownSampling = cloudsource;}else if (cloudsource->points.size() > 20000 && cloudsource->points.size() < 60000){mParams.VoxelSize = 1.0;PointCloudVoxelGrid(cloudsource, cloudDownSampling, mParams.VoxelSize);}else if (cloudsource->points.size() > 60000 && cloudsource->points.size() < 100000){mParams.VoxelSize = 2.0;PointCloudVoxelGrid(cloudsource, cloudDownSampling, mParams.VoxelSize);}else{PointCloudVoxelGrid(cloudsource, cloudDownSampling, mParams.VoxelSize);}int PointsNum = 6;std::vector<std::vector<size_t>> mvSets = std::vector<std::vector<size_t>>(mParams.MaxModelFitIterations, std::vector<size_t>(PointsNum, 0));//点的对数const int N = cloudDownSampling->points.size();//新建一个容器vAllIndices存储点云索引，并预分配空间std::vector<size_t> vAllIndices;vAllIndices.reserve(N);//在RANSAC的某次迭代中，还可以被抽取来作为数据样本的索引，所以这里起的名字叫做可用的索引std::vector<size_t> vAvailableIndices;//初始化所有特征点对的索引，索引值0到N-1for (int i = 0; i < N; i++)vAllIndices.push_back(i);//用于进行随机数据样本采样，设置随机数种子SeedRandOnce(0);//开始每一次的迭代 for (int it = 0; it < mParams.MaxModelFitIterations; it++){//迭代开始的时候，所有的点都是可用的vAvailableIndices = vAllIndices;//选择最小的数据样本集for (size_t j = 0; j < PointsNum; j++){// 随机产生一对点的id,范围从0到N-1int randi = RandomInt(0, vAvailableIndices.size() - 1);// idx表示哪一个索引对应的点对被选中int idx = vAvailableIndices[randi];//将本次迭代这个选中的第j个特征点对的索引添加到mvSets中mvSets[it][j] = idx;// 由于这对点在本次迭代中已经被使用了,所以我们为了避免再次抽到这个点,就在"点的可选列表"中,// 将这个点原来所在的位置用vector最后一个元素的信息覆盖,并且删除尾部的元素// 这样就相当于将这个点的信息从"点的可用列表"中直接删除了vAvailableIndices[randi] = vAvailableIndices.back();vAvailableIndices.pop_back();}//依次提取出6个点}//迭代mMaxIterations次，选取各自迭代时需要用到的最小数据集//某次迭代中，点云的坐标PointCloudT::Ptr cloudIn;cloudIn.reset(new(PointCloudT));//保存最优的平面double fError = 0.0;double plane[4] = { 0 };coefficients->values.resize(4);//最优的匹配int BestMatch = 0;pcl::PointIndices::Ptr inliers;inliers.reset(new pcl::PointIndices());//下面进行每次的RANSAC迭代for (int it = 0; it < mParams.MaxModelFitIterations; it++){//选择6个点对进行迭代for (size_t j = 0; j < PointsNum; j++){//从mvSets中获取当前次迭代的某个特征点对的索引信息int idx = mvSets[it][j];pcl::PointXYZ pointcloud = cloudDownSampling->points[idx];if (!isFinite(pointcloud)) //不是有效点continue;cloudIn->push_back(pointcloud);}if (!isSampleGood(cloudIn)) //不是好的平面点(构成直线了)continue;PlaneFitting(cloudIn, plane, fError);//获得内点的数量和索引std::vector<int> TempInlier;for (int i = 0; i < cloudDownSampling->points.size(); i++){pcl::PointXYZ pointcloud = cloudDownSampling->points[i];if (!isFinite(pointcloud)) //不是有效点continue;//计算误差double det = sqrt(plane[0] * plane[0] + plane[1] * plane[1] + plane[2] * plane[2]);double err = abs(plane[0] * cloudDownSampling->points[i].x + plane[1] * cloudDownSampling->points[i].y + plane[2] * cloudDownSampling->points[i].z + plane[3]) / det;if (err < mParams.SegDistanceThreshold)TempInlier.push_back(i);}//更新最优方程if (TempInlier.size() > BestMatch){BestMatch = TempInlier.size();inliers->indices = TempInlier;coefficients->values[0] = plane[0];coefficients->values[1] = plane[1];coefficients->values[2] = plane[2];coefficients->values[3] = plane[3];}cloudIn.reset(new(PointCloudT));}cloudIn.reset();//创建点云提取对象pcl::ExtractIndices<pcl::PointXYZ> extract;extract.setInputCloud(cloudDownSampling);extract.setIndices(inliers);//提取内点extract.setNegative(false);extract.filter(*cloudseg);//提取外点extract.setNegative(true);extract.filter(*cloudfiltered);inliers.reset();//不优化if (mParams.RefinePlane == 0)return;//平面系数优化(这一步需要ceres库，如果没有，直接屏蔽就好)plane[0] = coefficients->values[0];plane[1] = coefficients->values[1];plane[2] = coefficients->values[2];plane[3] = coefficients->values[3];std::vector<cv::Point3f> v3dpointsPlane;for (int i = 0; i < cloudseg->points.size(); i++){pcl::PointXYZ pointcloud = cloudseg->points[i];if (!isFinite(pointcloud)) //不是有效点continue;v3dpointsPlane.push_back(cv::Point3f(pointcloud.x, pointcloud.y, pointcloud.z));}optimizer.BundleAdjustmentPlane(v3dpointsPlane, plane);//归一化存储double det = sqrt(plane[0] * plane[0] + plane[1] * plane[1] + plane[2] * plane[2]);coefficients->values[0] = plane[0] / det;coefficients->values[1] = plane[1] / det;coefficients->values[2] = plane[2] / det;coefficients->values[3] = plane[3] / det;}

体素滤波：

	void PointCloudVoxelGrid(PointCloudT::Ptr cloudsource, PointCloudT::Ptr cloudfiltered, float voxelsize){pcl::VoxelGrid<PointT> sor;							  //创建滤波对象sor.setInputCloud(cloudsource);                       //设置需要过滤的点云给滤波对象sor.setLeafSize(voxelsize, voxelsize, voxelsize);     //设置滤波时创建的体素体积为1cm的立方体sor.filter(*cloudfiltered);                           //执行滤波处理，存储输出}

判断是否前3个点共线：

	bool isSampleGood(PointCloudT::Ptr cloudsource){// Need an extra check in case the sample selection is emptyif (cloudsource->points.size() < 3)return (false);// Get the values at the two pointspcl::Array4fMap p0 = cloudsource->points[0].getArray4fMap();pcl::Array4fMap p1 = cloudsource->points[1].getArray4fMap();pcl::Array4fMap p2 = cloudsource->points[2].getArray4fMap();Eigen::Array4f dy1dy2 = (p1 - p0) / (p2 - p0);return ((dy1dy2[0] != dy1dy2[1]) || (dy1dy2[2] != dy1dy2[1]));}

平面拟合：

	void PlaneFitting(PointCloudT::Ptr cloudIn, double* plane, double& fError){CvMat* points = cvCreateMat(cloudIn->points.size(), 3, CV_32FC1);for (int i = 0; i < cloudIn->points.size(); ++i){cvmSet(points, i, 0, cloudIn->points[i].x);cvmSet(points, i, 1, cloudIn->points[i].y);cvmSet(points, i, 2, cloudIn->points[i].z);}int nrows = points->rows;int ncols = points->cols;int type = points->type;CvMat* centroid = cvCreateMat(1, ncols, type);cvSet(centroid, cvScalar(0));for (int c = 0; c < ncols; c++){for (int r = 0; r < nrows; r++){centroid->data.fl[c] += points->data.fl[ncols*r + c];}centroid->data.fl[c] /= nrows;}// Subtract geometric centroid from each point.  CvMat* points2 = cvCreateMat(nrows, ncols, type);for (int r = 0; r < nrows; r++)for (int c = 0; c < ncols; c++)points2->data.fl[ncols*r + c] = points->data.fl[ncols*r + c] - centroid->data.fl[c];// Evaluate SVD of covariance matrix.  CvMat* A = cvCreateMat(ncols, ncols, type);CvMat* W = cvCreateMat(ncols, ncols, type);CvMat* V = cvCreateMat(ncols, ncols, type);cvGEMM(points2, points, 1, NULL, 0, A, CV_GEMM_A_T);cvSVD(A, W, NULL, V, CV_SVD_V_T);// Assign plane coefficients by singular vector corresponding to smallest singular value.  plane[ncols] = 0;for (int c = 0; c < ncols; c++){plane[c] = V->data.fl[ncols*(ncols - 1) + c];plane[ncols] += plane[c] * centroid->data.fl[c];}//计算拟合误差//Ax+By+Cz=D A=plane[0],B=plane[1],C=plane[2],D=plane[3]double sum_error = 0;double det = sqrt(plane[0] * plane[0] + plane[1] * plane[1] + plane[2] * plane[2]);for (int i = 0; i < cloudIn->points.size(); ++i){double  a = cloudIn->points[i].x;double  b = cloudIn->points[i].y;double  c = cloudIn->points[i].z;double err = abs(plane[0] * a + plane[1] * b + plane[2] * c - plane[3]) / det;sum_error = sum_error + err;}fError = sum_error / cloudIn->points.size();//归一化平面向量，并将方程修改为Ax+By+Cz+D=0plane[0] = plane[0] / det;plane[1] = plane[1] / det;plane[2] = plane[2] / det;plane[3] = -plane[3] / det;cvReleaseMat(&points);cvReleaseMat(&points2);cvReleaseMat(&A);cvReleaseMat(&W);cvReleaseMat(&V);}

平面拟合的另一种方式：OpenCV最小二乘法拟合空间平面

2、效果图