使用RANSAC拟合点云平面
- 1、C++实现
- 2、效果图
普通的点云平面拟合方式在一般情况下可以得到较好的平面拟合效果，但是容易出现平面拟合错误或是拟合的平面不是最优的情况。此时就需要根据自己的实际使用情况，调整平面拟合的迭代次数以及收敛条件。
使用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
//点云的单位是mm
void 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-1
for (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-1
int 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 empty
if (cloudsource->points.size() < 3)
return (false);
// Get the values at the two points
pcl::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=0
plane[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、效果图
