三维空间点进行空间平面拟合原理及MATLAB和C++代码实现

平面拟合原理参考网页：https://blog.csdn.net/duiwangxiaomi/article/details/89246715

MATLAB实现参考网页：https://blog.csdn.net/duiwangxiaomi/article/details/89238969

C++代码实现，包括测试数据，vs2013结合OpenCV2.4.13：

#include <iostream>
#include <highgui.h>using namespace std;
using namespace cv;//AX+BY+CZ+D=0
void cvFitPlane(const CvMat* points, float* plane){// 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.  points2存放的是各个点减去几何重心的值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);Mat A1(A);cout << "A: " << A1<<endl;Mat V1(V);cout << "V: "<<V1<<endl;// 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];}plane[ncols] = -plane[ncols];// Release allocated resources.//cvReleaseMat(?roid);cvReleaseMat(&points2);cvReleaseMat(&A);cvReleaseMat(&W);cvReleaseMat(&V);
}int main()
{//float x0 = 1,L1 = 2,y0 = 1,   L2 = 2, x1,y1,z1;//vector<float> x, y, z;//for (int i = 0; i < 20;++i)//{//    x1 = x0 + rand()*L1;//    y1 = y0 + rand()*L2;//    z1 = 1 + 2 * x1 + 3 * y1;//  x.push_back(x1);//  y.push_back(y1);//  z.push_back(z1);//}Mat point3D = (Mat_<float>(20, 3) << 2.62944737278636, 2.31148139831317, 13.1933389405122, 2.81158387415124, 1.07142335714838, 9.83743781974761,1.25397363258701, 2.69825861173755, 11.6027231003867, 2.82675171227804, 2.86798649551510, 15.2574629111014,2.26471849245082, 2.35747030971555, 12.6018479140483, 1.19508080999882, 2.51548026115667, 10.9366024034676,1.55699643773410, 2.48626493624983, 11.5727876842177, 2.09376303840997, 1.78445403906834, 10.5408881940249,2.91501367086860, 2.31095578035511, 13.7628946828025, 2.92977707039855, 1.34237337562312, 10.8866742676665,1.31522616335510, 2.41209217603922, 10.8667288548278, 2.94118556352123, 1.06366569275484, 10.0733682053070,2.91433389648589, 1.55384596992178, 11.4902057027371, 1.97075129744568, 1.09234278126231, 8.21853093867829,2.60056093777760, 1.19426356247170, 9.78391256297029, 1.28377267725443, 2.64691565665459, 11.5082923244726,1.84352256525255, 2.38965724595163, 11.8560168683600, 2.83147105037813, 1.63419896012172, 11.5655389811214,2.58441465911911, 2.90044409767671, 14.8701616112683, 2.91898485278581, 1.06889216100582, 10.0446461885891);vector<Point3f> points;points = Mat_<Point3f>(point3D);CvMat* points_mat = cvCreateMat(points.size(), 3, CV_32FC1);//定义用来存储需要拟合点的矩阵 for (int i = 0; i < points.size(); ++i){points_mat->data.fl[i * 3 + 0] = points[i].x;//矩阵的值进行初始化   X的坐标值points_mat->data.fl[i * 3 + 1] = points[i].y;//  Y的坐标值points_mat->data.fl[i * 3 + 2] = points[i].z;// < span style = "font-family: Arial, Helvetica, sans-serif;" >//  Z的坐标值</span>}float plane12[4] = { 0 };//定义用来储存平面参数的数组 cvFitPlane(points_mat, plane12);//调用方程
}

对应的MATLAB代码为：

% 随机生成一组（x,y,z),这些点的坐标离一个空间平面比较近
x0=1;
L1=2;
y0=1;
L2=2;
x=x0+rand(20,1)*L1;
y=y0+rand(20,1)*L2;
z=1+2*x+3*y;
scatter3(x,y,z,'filled')
hold on;planeData=[x,y,z];% 协方差矩阵的SVD变换中，最小奇异值对应的奇异向量就是平面的方向
xyz0=mean(planeData,1);
centeredPlane=bsxfun(@minus,planeData,xyz0);
A=centeredPlane'*planeData;
[U,S,V]=svd(A);
%[U,S,V]=svd(centeredPlane);a=V(1,3);
b=V(2,3);
c=V(3,3);
d=-dot([a b c],xyz0);% 图形绘制
xfit = min(x):0.1:max(x);
yfit = min(y):0.1:max(y);
[XFIT,YFIT]= meshgrid (xfit,yfit);
ZFIT = -(d + a * XFIT + b * YFIT)/c;
mesh(XFIT,YFIT,ZFIT);

三维空间点进行空间平面拟合原理及MATLAB和C++代码实现相关推荐

PCL最小二乘法进行平面拟合原理
最小二乘法进行平面拟合原理 1 最小二乘原理 2 最小二乘拟合平面 1 最小二乘原理最小二乘法(又称最小平方法)是一种数学优化技术.它通过最小化误差的平方和寻找数据的最佳函数匹配.利用最小二乘法可以 ...
最小二乘法函数拟合原理及matlab实现—数学笔记
最小二乘法函数拟合原理及matlab实现 --数值分析数学笔记如有纰漏,欢迎指正文章目录最小二乘法函数拟合原理及matlab实现前言一.拟合标准 1.使偏差向量满足 1 1 1 - 范数 2 ...
PCL Lesson5: 直通滤波+空间平面拟合+提供原始点云数据集PCD文件
书接上文.本节做下面一个工作:通过直通滤波过滤一小片平面区域的点云(标定版),通过最小二乘法拟合,并把参数化的平面绘制在原图中. 待修正:拟合平面时离散点的处理.和拟合效果的判别效果: 原始点云的文 ...
【图像处理】双三次插值(Bicubic interpolation)原理及matlab简易版代码
先简单写下. 双线性插值:缩放后图像矩阵(简称TI)像素坐标映射到原图像矩阵(简称OI)中得坐标点P(x,y),P点临近四个坐标点像素值的线性加权求和即P点像素值. "双"指的图像 ...
jacobi方法 matlab,Jacobi（雅可比）迭代原理与matlab代码
Jacobi(雅可比)迭代原理与matlab代码 Jacobi(雅可比)迭代原理与matlab代码 Jacobi(雅可比)迭代原理与matlab,C代码 Jacobi迭代分量形式: x i ( k + ...
Jacobi（雅可比）迭代原理与matlab代码
Jacobi(雅可比)迭代原理与matlab,C代码 Jacobi迭代分量形式: xi(k+1)=1/aii(bi−∑j≠ii=1naijxj(k))\ x_i^{(k+1)}=1/a_{ii}(b_ ...
三维点集拟合：平面拟合、RANSAC、ICP算法
ACM算法分类:http://www.kuqin.com/algorithm/20080229/4071.html;CSDN容易吞图,不过编辑器里面图片还是显示的..... 一. 拟合一个平面 ...
三维空间点的直线方程拟合(最小二乘法)
本文要解决如下问题:给定一些三维空间点,要求用直线拟合这些点,求出直线方程. 一.直线方程的三种表示方法: 参考:空间直线的点向式方程 - 百度文库 (baidu.com) 空间直线方程 - 百度文库 ...
空间离散点拟合成空间平面
1.原理空间中的离散点得到拟合平面,其实这就是一个最优化的过程.即求这些点到某个平面距离和最小的问题.我们知道一个先验消息,那就是该平面一定会过众散点的平均值.接着我们需要做的工作就是求这个平面的法 ...

三维空间点进行空间平面拟合原理及MATLAB和C++代码实现

三维空间点进行空间平面拟合原理及MATLAB和C++代码实现相关推荐

最新文章

热门文章