网站建设的步骤及方法,网站建设合伙合同,中新生态城建设局门户网站,网站系统建设的主要意义使用RANSAC拟合点云平面 1、C实现2、效果图 普通的点云平面拟合方式在一般情况下可以得到较好的平面拟合效果#xff0c;但是容易出现平面拟合错误或是拟合的平面不是最优的情况。此时就需要根据自己的实际使用情况#xff0c;调整平面拟合的迭代次数以及收敛条件。
使用RAN… 使用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.VoxelSize3.0 mParams.MaxModelFitIterations2000//mParams.SegDistanceThreshold0.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::vectorstd::vectorsize_t mvSets std::vectorstd::vectorsize_t(mParams.MaxModelFitIterations, std::vectorsize_t(PointsNum, 0));//点的对数const int N cloudDownSampling-points.size();//新建一个容器vAllIndices存储点云索引并预分配空间std::vectorsize_t vAllIndices;vAllIndices.reserve(N);//在RANSAC的某次迭代中还可以被抽取来作为数据样本的索引所以这里起的名字叫做可用的索引std::vectorsize_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::vectorint 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::ExtractIndicespcl::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::vectorcv::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::VoxelGridPointT 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];}//计算拟合误差//AxByCzD Aplane[0],Bplane[1],Cplane[2],Dplane[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();//归一化平面向量并将方程修改为AxByCzD0plane[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、效果图
