精华内容
下载资源
问答
  • 使用最小二乘法拟合平面,借助pcl点云库中的估计法矢的类来得到模型中点云曲面法矢估计。 是一个较为简单,常用的代码。txt文件
  • 最小二乘法拟合平面 -- 代码实现(C++, Eigen3)说明作者[ 哈哈kls ]的推导C++实现 说明 本文基于博主 哈哈kls 的博文:最小二乘法拟合平面 的公式推导,用C++进行实现。 文章出处:...

    最小二乘法拟合平面 -- 代码实现(C++, Eigen3)

    说明

    本文基于博主 哈哈kls 的博文:最小二乘法拟合平面 的公式推导,用C++进行实现。
    文章出处:https://blog.csdn.net/konglingshneg/article/details/82585868

    作者[ 哈哈kls ]的推导

    Alt
    注:上图最后一句应该是: z = a 0 x + a 1 y + a 2 z=a_{0}x+a_{1}y+a{2} z=a0x+a1y+a2

    C++实现

    #include <iostream>
    #include <vector>
    #include <Eigen/Dense>
    #include <Eigen/Cholesky>
    #include <Eigen/LU>
    #include <Eigen/QR>
    #include <Eigen/SVD>
    
    using namespace std;
    using namespace Eigen;
    
    /*
    * From: https://blog.csdn.net/konglingshneg/article/details/82585868
    * Author: JARK006(JARK006@QQ.COM)
    * z = a0x + a1y + a2
    * a0*x + a1*y - z + a2 = 0
    * 入参:points --待拟合平面的点集
    * 返回:拟合平面法向量 { a0, a1, -1 }
    */
    vector<double> FittingPlaneEigen(vector<vector<double>>& points) {
        vector<vector<double>> mat3x4(3, vector<double>(4, 0));
        for (const auto& p : points) {
            // 3x3 方程组参数   p[0]:x, p[1]:y, p[2]:z
            // 对角线对称,只需处理5个
            mat3x4[0][0] += p[0] * p[0];
            mat3x4[0][1] += p[0] * p[1];
            mat3x4[0][2] += p[0];
            mat3x4[1][1] += p[1] * p[1];
            mat3x4[1][2] += p[1];
    
            // 3x1 方程组值
            mat3x4[0][3] += p[0] * p[2];
            mat3x4[1][3] += p[1] * p[2];
            mat3x4[2][3] += p[2];
        }
    
        MatrixXd A(3, 3);
        A <<
            mat3x4[0][0], mat3x4[0][1], mat3x4[0][2],
            mat3x4[0][1], mat3x4[1][1], mat3x4[1][2],
            mat3x4[0][2], mat3x4[1][2], (double)points.size();
        MatrixXd B(3, 1);
        B << mat3x4[0][3], mat3x4[1][3], mat3x4[2][3];
    
        //以下三个方式前14位有效数字基本一致,任选一个
        //来源:https://eigen.tuxfamily.org/dox/group__LeastSquares.html
        MatrixXd a = A.bdcSvd(ComputeThinU | ComputeThinV).solve(B);
        //MatrixXd a = A.colPivHouseholderQr().solve(B);
        //MatrixXd a = A.lu().solve(B);
    
        // a0*x + a1*y - z + a2 = 0 法向量为:a(0), a(1), -1
        return { a(0), a(1), -1.0 };
        
        // 若需 A*x + B*y + C*z + D = 0 形式,则A,B,C,D为{ a(0), a(1), -1.0, a(2) }
        // return { a(0), a(1), -1.0, a(2) };
    }
    
    展开全文
  • void FitPlaneByLeastSquares(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud) { if (cloud->points.size() < 3) return; double meanX = 0, meanY = 0, meanZ = 0; double meanXX = 0, meanYY = ...

    原理:
    在这里插入图片描述
    实现:

    void FitPlaneByLeastSquares(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud)
    {
    	if (cloud->points.size() < 3)	return;
    
    	double meanX = 0, meanY = 0, meanZ = 0;
    	double meanXX = 0, meanYY = 0, meanZZ = 0;
    	double meanXY = 0, meanXZ = 0, meanYZ = 0;
    	for (int i = 0; i < cloud->points.size(); i++)
    	{
    		meanX += cloud->points[i].x;
    		meanY += cloud->points[i].y;
    		meanZ += cloud->points[i].z;
    
    		meanXX += cloud->points[i].x * cloud->points[i].x;
    		meanYY += cloud->points[i].y * cloud->points[i].y;
    		meanZZ += cloud->points[i].z * cloud->points[i].z;
    
    		meanXY += cloud->points[i].x * cloud->points[i].y;
    		meanXZ += cloud->points[i].x * cloud->points[i].z;
    		meanYZ += cloud->points[i].y * cloud->points[i].z;
    	}
    	meanX /= cloud->points.size();
    	meanY /= cloud->points.size();
    	meanZ /= cloud->points.size();
    	meanXX /= cloud->points.size();
    	meanYY /= cloud->points.size();
    	meanZZ /= cloud->points.size();
    	meanXY /= cloud->points.size();
    	meanXZ /= cloud->points.size();
    	meanYZ /= cloud->points.size();
    
    	/* eigenvector */
    	Matrix3d m;
    	m(0, 0) = meanXX - meanX * meanX; m(0, 1) = meanXY - meanX * meanY; m(0, 2) = meanXZ - meanX * meanZ;
    	m(1, 0) = meanXY - meanX * meanY; m(1, 1) = meanYY - meanY * meanY; m(1, 2) = meanYZ - meanY * meanZ;
    	m(2, 0) = meanXZ - meanX * meanZ; m(2, 1) = meanYZ - meanY * meanZ; m(2, 2) = meanZZ - meanZ * meanZ;
    	Eigen::EigenSolver<Eigen::Matrix3d> PlMat(m * cloud->points.size());
    	Matrix3d eigenvalue = PlMat.pseudoEigenvalueMatrix();
    	Matrix3d eigenvector = PlMat.pseudoEigenvectors();
    
    	/* the eigenvector corresponding to the minimum eigenvalue */
    	double v1 = eigenvalue(0, 0), v2 = eigenvalue(1, 1), v3 = eigenvalue(2, 2);
    	int minNumber = 0;
    	if ((abs(v2) <= abs(v1)) && (abs(v2) <= abs(v3)))	minNumber = 1;
    	if ((abs(v3) <= abs(v1)) && (abs(v3) <= abs(v2)))	minNumber = 2;
    	double a = eigenvector(0, minNumber), b = eigenvector(1, minNumber), c = eigenvector(2, minNumber), d = -(a * meanX + b * meanY + c * meanZ);
    
    	if (c < 0)
    	{
    		a *= -1.0;
    		b *= -1.0;
    		c *= -1.0;
    		d *= -1.0;
    	}
    	cout << a << " " << b << " " << c << " " << d << endl;
    }
    
    
    展开全文
  • 平面公式为:Ax+By+Cz=D 代码: ...//对应的方程:Ax+By+Cz=D 其中 A = plane12[0], B = plane12[1], C = plane12[2], D = plane12[3],这是要注意的方程的表示 //float plane12[4] = { 0 };//定义用来储存平面参数...拟合

    本文主要验证了博客上的最小二乘法拟合平面的。与 用matlab拟合出来的平面计算的点到直线的距离是一样的,而且系数也是一样的。说明了本方法的可行性。
    matlab中公式为z = c + ax +by
    oepncv中公式为Ax+By+Cz=D 将opencv中公式换算成matlab的公式,系数是一样的。

    平面公式为:Ax+By+Cz=D
    代码:
    来自于:https://blog.csdn.net/laobai1015/article/details/73603327?utm_source=blogxgwz4

    //对应的方程:Ax+By+Cz=D 其中 A = plane12[0], B = plane12[1], C = plane12[2], D = plane12[3],这是要注意的方程的表示
    //float plane12[4] = { 0 };//定义用来储存平面参数的数组,分别对应ABCD

    拟合平面

    void cvFitPlane(vector<float>dx, vector<float>dy, vector<float>dz, float* plane) {
    
    	//直线方程为:
    	//构建点集cvmat
    	CvMat* points = cvCreateMat(dx.size(), 3, CV_32FC1);
    	int nnum = 96;
    	for (int i = 0; i < dx.size(); ++i)
    	{
    		points->data.fl[i * 3 + 0] = dx[i];//矩阵的值进行初始化   X的坐标值  
    		points->data.fl[i * 3 + 1] = dy[i];//  Y的坐标值  
    		points->data.fl[i * 3 + 2] = dz[i]; //  Z的坐标值
    
    	}
    
    	 Estimate geometric centroid.  
    	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];
    	}
    	// Release allocated resources.  
    	cvReleaseMat(&points);
    	cvReleaseMat(&centroid);
    	cvReleaseMat(&points2);
    	cvReleaseMat(&A);
    	cvReleaseMat(&W);
    	cvReleaseMat(&V);
    }
    

    计算点到平面的距离

    //计算点到平面的距离
    //Ax+By+Cz=D
    //|点(a,b,c) 到平面bai Ax+By+Cz=D的距离du
    
    //= | A * a + B * b + C * c - D| /√(A ^ 2 + B ^ 2 + C ^ 2)
    void calculateDist(vector<float>dx, vector<float>dy, vector<float>dz, float* plane, vector<float> &dist)
    {
    	for (int i = 0; i < dx.size(); i++)
    	{
    		float ds = fabs(plane[0] * dx[i] + plane[1] * dy[i] + plane[2] * dz[i] - plane[3]);
    		float dfen = sqrt(plane[0] * plane[0] + plane[1] * plane[1] + plane[2] * plane[2]);
    		if (!(dfen > -0.00001 && dfen < -0.00001))
    		{
    			float ddist = ds / dfen;
    			dist.push_back(ddist);
    		}		
    	}
    }
    

    测试流程

    从文件中读取数据,然后计算拟合平面,计算点到平面的距离,并输出到csv文件中
    数据格式为:
    一行中代表xyz

    -53.883533,55.133049,895.801941
    -40.928612,32.402653,897.237793
    -21.391739,50.161041,899.748901
    2.107507,62.850151,902.479065
    3.594930,37.490810,902.427490
    

    具体代码为:

    fstream fs;
    	fs.open("E:\\wokspace\\PROJECT\\ThirdTrailInspection\\matlab\\dResult.txt");
    	if (!fs.is_open())
    	{
    		return;
    	}
    	vector<float> dx;
    	vector<float> dy;
    	vector<float> dz;
    	int i = 0;
    	string buff;
    	while (getline(fs, buff))//是否到文件结bai尾
    	{
    		int nfist = buff.find_first_of(',');
    		int nLast = buff.find_last_of(',');
    		string st1 = buff.substr(0, nfist);
    		string st2 = buff.substr(nfist + 1, nLast - nfist - 1);
    		string st3 =(buff.substr(nLast + 1));
    		dx.push_back(stof(buff.substr(0, nfist)));
    		dy.push_back(stof(buff.substr(nfist + 1, nLast - nfist - 1)));
    		dz.push_back(stof(buff.substr(nLast + 1)));		
    	}
    	fs.close();
    
    
    	//代入最小二乘算法中
    	float plane[4] = { 0 };
    	vector<float> dx1;
    	vector<float> dy1;
    	vector<float> dz1;
    	dx1.assign(dx.begin(), dx.begin() + 96);
    	dy1.assign(dy.begin(), dy.begin() + 96);
    	dz1.assign(dz.begin(), dz.begin() + 96);
    	cvFitPlane(dx1, dy1, dz1,  plane);
    	vector<float> dist;
    	calculateDist(dx, dy, dz, plane, dist);
    	fstream fws("e://de.csv", fstream::in | fstream::out | fstream::trunc);
    	for (int i = 0; i < dist.size(); i++)
    	{
    		fws << dist[i] <<"\r";
    	}
    	fws.close();
    

    对应的matlab代码

    clc;
    close all;
    clear all;
    %https://www.ilovematlab.cn/thread-220252-1-1.html
    
    data = importdata('E:\wokspace\PROJECT\ThirdTrailInspection\matlab\dResult.txt');
    x = data(1:96, 1);
    y = data(1:96, 2);
    z = data(1:96, 3);
    % x = data(113:192, 1);
    % y = data(113:192, 2);
    % z = data(113:192, 3);
    scatter3(x, y,z, 'r');%画点  散点图
    hold on;
    X = [ones(length(x),1) x y];
    [b,bint,r,rint,stats] = regress(z,X,95);
    
    % 图形绘制
    xfit = min(x):0.1:max(x);
    yfit = min(y):0.1:max(y);
    [XFIT,YFIT]= meshgrid (xfit,yfit);%用于生成网格采样点
    ZFIT = b(1) + b(2) * XFIT + b(3) * YFIT;
    mesh(XFIT,YFIT,ZFIT);
    
    %%测试结果
    %data = importdata('C:\Users\apr_z\Desktop\dResult.txt');
    xx = data(:, 1);
    yy = data(:, 2);
    zz = data(:, 3);
    [row, col] = size(xx);%求矩阵的行数和列数
    dist = ones(row, 1);
    for i = 1: row
        dist(i) = abs(b(2) * xx(i) + b(3) * yy(i)  - zz(i) + b(1)) / sqrt(b(2)* b(2) + b(3)*b(3) + 1 );
    end
    xlswrite('C:\Users\apr_z\Desktop\AnalyzeResult.xlsx', dist);
    

    小结:

    vector 复制某一些数据时: dx1.assign(dx.begin(), dx.begin() + 96);

    vector容器 追加其他容器的内容,使用insert
    pt3DList.insert(pt3DList.end(), vc.begin(), vc.end());

    用此平面拟合的计算 点到直线的距离 与 用matlab计算出来的点到直线的距离是一模一样的。

    展开全文
  • opencv最小二乘法拟合平面

    千次阅读 2016-07-30 08:18:28
    ... points2->data.fl[ncols*r + c] =...我们拟合出来的方程:Ax+By+Cz=D 其中 A=plane12[0], B=plane12[1], C=plane12[2], D=plane12[3], 这是要注意的方程的表示
     
    
    1. //Ax+by+cz=D  
    2. void cvFitPlane(const CvMat* points, float* plane){  
    3.     // Estimate geometric centroid.  
    4.     int nrows = points->rows;  
    5.     int ncols = points->cols;  
    6.     int type = points->type;  
    7.     CvMat* centroid = cvCreateMat(1, ncols, type);  
    8.     cvSet(centroid, cvScalar(0));  
    9.     for (int c = 0; c<ncols; c++){  
    10.         for (int r = 0; r < nrows; r++)  
    11.         {  
    12.             centroid->data.fl[c] += points->data.fl[ncols*r + c];  
    13.         }  
    14.         centroid->data.fl[c] /= nrows;  
    15.     }  
    16.     // Subtract geometric centroid from each point.  
    17.     CvMat* points2 = cvCreateMat(nrows, ncols, type);  
    18.     for (int r = 0; r<nrows; r++)  
    19.         for (int c = 0; c<ncols; c++)  
    20.             points2->data.fl[ncols*r + c] = points->data.fl[ncols*r + c] - centroid->data.fl[c];  
    21.     // Evaluate SVD of covariance matrix.  
    22.     CvMat* A = cvCreateMat(ncols, ncols, type);  
    23.     CvMat* W = cvCreateMat(ncols, ncols, type);  
    24.     CvMat* V = cvCreateMat(ncols, ncols, type);  
    25.     cvGEMM(points2, points, 1, NULL, 0, A, CV_GEMM_A_T);  
    26.     cvSVD(A, W, NULL, V, CV_SVD_V_T);  
    27.     // Assign plane coefficients by singular vector corresponding to smallest singular value.  
    28.     plane[ncols] = 0;  
    29.     for (int c = 0; c<ncols; c++){  
    30.         plane[c] = V->data.fl[ncols*(ncols - 1) + c];  
    31.         plane[ncols] += plane[c] * centroid->data.fl[c];  
    32.     }  
    33.     // Release allocated resources.  
    34.     cvReleaseMat(¢roid);  
    35.     cvReleaseMat(&points2);  
    36.     cvReleaseMat(&A);  
    37.     cvReleaseMat(&W);  
    38.     cvReleaseMat(&V);  
    39. }  

    调用的方式:

    [cpp]  view plain  copy
     print ?
    1. CvMat*points_mat = cvCreateMat(X_vector.size(), 3, CV_32FC1);//定义用来存储需要拟合点的矩阵   
    2.         for (int i=0;i < X_vector.size(); ++i)  
    3.         {  
    4.             points_mat->data.fl[i*3+0] = X_vector[i];//矩阵的值进行初始化   X的坐标值  
    5.             points_mat->data.fl[i * 3 + 1] = Y_vector[i];//  Y的坐标值  
    6.             points_mat->data.fl[i * 3 + 2] = Z_vector[i];<span style="font-family: Arial, Helvetica, sans-serif;">//  Z的坐标值</span>  
    7.   
    8.         }  
    9.         float plane12[4] = { 0 };//定义用来储存平面参数的数组   
    10.         cvFitPlane(points_mat, plane12);//调用方程   
    我们拟合出来的方程:Ax+By+Cz=D

    其中 A=plane12[0],    B=plane12[1],   C=plane12[2],   D=plane12[3],

    这是要注意的方程的表示

    展开全文
  • //fitting.h ...pcl/point_types.h> #include <vector> #include <Eigen/dense> #include <vtkPolyLine.h> #include <pcl/visualization/pcl_visualizer.h> #include <pcl/...
  • PCL_最小二乘法点云平滑

    千次阅读 2020-03-17 11:31:07
    不规则数据直接进行曲面重建会造成曲面不光滑或者漏洞,在不能进行额外扫描的情况下,可以通过数据重采样解决这一问题,重采样算法通过对...pcl/point_types.h> #include <pcl/io/pcd_io.h> #include <p...
  • PCL 最小二乘平面拟合(SVD法)

    万次阅读 2021-02-27 09:09:33
    最小二乘法拟合平面
  • 估计某个点的法向量,可以类似于点云的曲面法向量估计,将该点附近K近邻的点近似在一个局部平面上,之后就通过最小二乘法拟合平面方程.本代码是基于PCL库,往库中添加新的法向量估计。
  • PCL 利用RANSAC算法拟合平面并旋转

    千次阅读 2019-10-22 21:26:23
    说明: 最近的项目用到了PCL里的旋转平面,然后又需要按一定的角度旋转,因此对于给定一个平面 ...拟合平面有两种方法,最小二乘法,和RANSAC算法。PCL库中SACSegmentation类中用的是RANSAC的算法来拟合平面的。...
  • 在使用过程中遇到离散点不在一个平面是拟合出来的平面方程和最小二乘法拟合的结果方程差异和大(你所给的实例中的点云拟合的平面却又和最小二乘法拟合平面方程一致),请问我直接使用是否可以,不可以的话问题是出...
  • 估计某个点的法向量,可以类似于点云的曲面法向量估计,将该点附近K近邻的点近似在一个局部平面上,之后就通过最小二乘法拟合平面方程,通过高等数学空间解析几何知识可知,若平面方程为A*x + B*y + C*z + D
  • 说明 最近的项目用到了PCL里的旋转平面,然后又需要按一定的角度旋转,因此对于给定一个平面的数据集,需要利用...拟合平面有两种方法,最小二乘法,和RANSAC算法。PCL库中SACSegmentation类中用的是RANSAC的算法
  • matlab中对三维点云利用最小二乘法进行平面拟合,该程序本人自己写的一个子程序
  • 使用两种思路进行直线拟合: 1.利用逆矩阵思想 --------------进行下列公式的推导需要理解逆矩阵(求A矩阵的逆矩阵,则A矩阵必须是方阵)的知识: (1)为什么要引入逆矩阵呢? 逆矩阵可以类比成数字的倒数,...
  • 最小二乘法与投影

    千次阅读 2018-01-25 11:31:14
    作者:阿狸 ... 来源:知乎 著作权归作者所有。...最小二乘法(Least Squares Method,简记为LSE)是一个比较古老的方法,源于天文学和测地学上的应用需要。在早期数理统计方法的发展中,这两门科学起了很大的
  • 目录 导言 最小二乘法 使用投影的方法来求解 使用求偏导的方法来求解 加权最小二乘法 使用mls平滑 PCL中upsampling的实现 RANDOM_UNIFORM_DENSITY SAM...
  • halcon拓展系列—平面拟合算子fit_plane

    千次阅读 热门讨论 2020-05-01 21:48:07
    一、算子说明 fit_plane(imageReal,rectangleFit:imageSurface:rateLowRemove,rateHighRemove,operatorType: ) ** 功能:最小二乘法拟合平面 ** 输入 ** image 点云图像 ** rectangleFit ...
  • 本节做下面一个工作:通过直通滤波过滤一小片平面区域的点云(标定版),通过最小二乘法拟合,并把参数化的平面绘制在原图中。 待修正:拟合平面时离散点的处理。和拟合效果的判别 效果: 原始点云的文件这这里:(16...
  • Point Cloud Library 0.0 documentationhttps://pcl.readthedocs.io/projects/tutorials/en/latest/resampling.html#moving-least-squares 在本篇教程中,将使用移动最小二乘法(MLS)表面重建的方式对噪点数据进行...
  • 在二维空间将离散点拟合直线使用最小二乘法的应用非常广泛,方法也比较简单。与此对应的是三维空间离散点拟合为平面也是很有用的方法,比如一些特定图像分析。本文所介绍的就是三维空间离散点拟合平面的方法,也是...
  • SciPy Cookbook中的RANSAC样例,简单易懂,直观理解RANSAC拟合直线的过程。1. 选取部分数据点 2. 最小二乘法拟合直线 3. 判定inliners 4. 终止条件
  • pcl normal estimator

    千次阅读 2019-06-27 15:28:49
    常常在被使用在很多计算机视觉的应用里面,比如可以用来推出光源的位置,通过阴影与其他视觉影响,表面法线的问题可以近似化解为切面的问题,这个切面的问题又会变成最小二乘法拟合平面的问题 解决表面法线估计的...
  • PCL法线估计

    2017-03-21 16:28:00
    常常在被使用在很多计算机视觉的应用里面,比如可以用来推出光源的位置,通过阴影与其他视觉影响,表面法线的问题可以近似化解为切面的问题,这个切面的问题又会变成最小二乘法拟合平面的问题 解决表面法线估计的问....
  • PCL学习一 法线估计

    2018-12-07 15:26:50
    常常在被使用在很多计算机视觉的应用里面,比如可以用来推出光源的位置,通过阴影与其他视觉影响,表面法线的问题可以近似化解为切面的问题,这个切面的问题又会变成最小二乘法拟合平面的问题 解决表面法线估计的...
  • (1)从样本集N中,随机选n个点作为初始子集(但是所选取的这n个点点必须能够表达待拟合的模型,比如拟合直线的话最少需要2个点,所以n=2(这是正常求解方式),但是基于最小二乘的思想的化最少需要大于2个点,所以n...

空空如也

空空如也

1 2 3 4 5 ... 11
收藏数 207
精华内容 82
关键字:

pcl最小二乘法拟合平面