椭球曲面拟合算法实现，matlab/C++
Posted Naruto_Q
tags:
篇首语：本文由小常识网(cha138.com)小编为大家整理，主要介绍了椭球曲面拟合算法实现，matlab/C++相关的知识，希望对你有一定的参考价值。
椭球曲面的标准表达式：(x-x0)^2/A^2+(Y-Y0)^2/B^2+(Z-Z0)^2/C^2=R^2，
一般形式可以写为：x^2+ay^2+bz^2+cxy+dxz+eyz+f=0，
模型参数估计中最基本的方法就是最小二乘法，先估计参数a,b,c,d,e,f，然后间接的得到参数x0, y0, z0, A, B, C。基于最小二乘的拟合方法公式推导可以参见：http://blog.csdn.net/hj199404182515/article/details/59480954 ，这里直接复制下博文里的matlab代码：
%% 生成随机椭球球面数据
clear all;close allclc;
A = 300; % x方向上的轴半径
B = 400; % y方向上的轴半径
C = 500; % z方向上的轴半径
x0 = -120; %椭球球心x坐标
y0 = -67; %椭球球心y坐标
z0 = 406; %椭球球心z坐标
SNR = 30; %信噪比
num_alfa = 100;
num_sita = 50;
alfa = (0:num_alfa-1)*1*pi/num_alfa;
sita = (0:num_sita-1)*2*pi/num_sita;
x = zeros(num_alfa,num_sita);
y = zeros(num_alfa,num_sita);
z = zeros(num_alfa,num_sita);
for i = 1:num_alfa
for j = 1:num_sita
x(i,j) = x0 + A*sin(alfa(i))*cos(sita(j));
y(i,j) = y0 + B*sin(alfa(i))*sin(sita(j));
z(i,j) = z0 + C*cos(alfa(i));
end
end
x = reshape(x,num_alfa*num_sita,1); %转换成一维的数组便于后续的处理
y = reshape(y,num_alfa*num_sita,1);
z = reshape(z,num_alfa*num_sita,1);
figure;
plot3(x,y,z,'*');
title('生成的没有噪声的椭球面数据');
%加入均值为0的高斯分布噪声
amp = 10^(-SNR/20)*A;
x = x + amp*rand(num_alfa*num_sita,1);
y = y + amp*B/A*rand(num_alfa*num_sita,1);
z = z + amp*C/A*rand(num_alfa*num_sita,1);
figure;
plot3(x,y,z,'*');
string = ['加入噪声的椭球面数据，SNR=',num2str(SNR),'dB'];
title(string);
%% 空间二次曲面拟合算法
num_points = length(x);
%一次项统计平均
x_avr = sum(x)/num_points;
y_avr = sum(y)/num_points;
z_avr = sum(z)/num_points;
%二次项统计平均
xx_avr = sum(x.*x)/num_points;
yy_avr = sum(y.*y)/num_points;
zz_avr = sum(z.*z)/num_points;
xy_avr = sum(x.*y)/num_points;
xz_avr = sum(x.*z)/num_points;
yz_avr = sum(y.*z)/num_points;
%三次项统计平均
xxx_avr = sum(x.*x.*x)/num_points;
xxy_avr = sum(x.*x.*y)/num_points;
xxz_avr = sum(x.*x.*z)/num_points;
xyy_avr = sum(x.*y.*y)/num_points;
xzz_avr = sum(x.*z.*z)/num_points;
yyy_avr = sum(y.*y.*y)/num_points;
yyz_avr = sum(y.*y.*z)/num_points;
yzz_avr = sum(y.*z.*z)/num_points;
zzz_avr = sum(z.*z.*z)/num_points;
%四次项统计平均
yyyy_avr = sum(y.*y.*y.*y)/num_points;
zzzz_avr = sum(z.*z.*z.*z)/num_points;
xxyy_avr = sum(x.*x.*y.*y)/num_points;
xxzz_avr = sum(x.*x.*z.*z)/num_points;
yyzz_avr = sum(y.*y.*z.*z)/num_points;
%计算求解线性方程的系数矩阵
A0 = [yyyy_avr yyzz_avr xyy_avr yyy_avr yyz_avr yy_avr;
yyzz_avr zzzz_avr xzz_avr yzz_avr zzz_avr zz_avr;
xyy_avr xzz_avr xx_avr xy_avr xz_avr x_avr;
yyy_avr yzz_avr xy_avr yy_avr yz_avr y_avr;
yyz_avr zzz_avr xz_avr yz_avr zz_avr z_avr;
yy_avr zz_avr x_avr y_avr z_avr 1;];
%计算非齐次项
b = [-xxyy_avr;
-xxzz_avr;
-xxx_avr;
-xxy_avr;
-xxz_avr;
-xx_avr];
resoult = inv(A0)*b;
%resoult = solution_equations_n_yuan(A0,b);
x00 = -resoult(3)/2; %拟合出的x坐标
y00 = -resoult(4)/(2*resoult(1)); %拟合出的y坐标
z00 = -resoult(5)/(2*resoult(2)); %拟合出的z坐标
AA = sqrt(x00*x00 + resoult(1)*y00*y00 + resoult(2)*z00*z00 - resoult(6)); % 拟合出的x方向上的轴半径
BB = AA/sqrt(resoult(1)); % 拟合出的y方向上的轴半径
CC = AA/sqrt(resoult(2)); % 拟合出的z方向上的轴半径
fPRintf('拟合结果\\n');
fprintf('a = %f, b = %f, c = %f, d = %f, e = %f, f = %f\\n',resoult);
fprintf('x0 = %f, 相对误差%f\\n',x00,abs((x00-x0)/x0));
fprintf('y0 = %f, 相对误差%f\\n',y00,abs((y00-y0)/y0));
fprintf('z0 = %f, 相对误差%f\\n',z00,abs((z00-z0)/z0));
fprintf('A = %f, 相对误差%f\\n',AA,abs((A-AA)/A));
fprintf('B = %f, 相对误差%f\\n',BB,abs((B-BB)/B));
fprintf('C = %f, 相对误差%f\\n',CC,abs((C-CC)/C));运行该程序得到如下结果：
根据上述公式推导，我进行了C++尝试，使用arma矩阵库进行矩阵方面的计算，直接po下代码：
//椭球拟合类头文件
#ifndef __CURVESURFACE__
#define __CURVESURFACE__
#include <iostream>
#include <armadillo>
#include<cmath>
#define INVEX -100
class CurveSurface
public:
double A, B, C, X0, Y0, Z0;
CurveSurface();
~CurveSurface();
void calcCurveSurface(arma::mat, arma::mat, arma::mat);
private:
int cols =M ;
int rows = N;
double average1(double* x0, int Num);
double average2(double* x0, double* y0, int Num);
double average3(double* x0, double* y0, double* z0, int Num);
double average4(double* x0, double* y0, double* z0, double* e0, int Num);
;
#endif
//椭圆类函数实现
CurveSurface::CurveSurface()
A = B = C = X0 = Y0 = Z0 = INVEX;
void CurveSurface::calcCurveSurface(arma::mat X, arma::mat Y, arma::mat Z)
double *ValidX = new double[M* N];
double *ValidY = new double[M* N];
double *ValidZ = new double[M* N];
int validNum = 0;
for (int i = 0; i < X.n_cols; ++i)
for (int j = 0; j < X.n_rows; ++j)
ValidX[validNum] = X(j, i);
ValidY[validNum] = Y(j, i);
ValidZ[validNum] = Z(j, i);
validNum++;
arma::mat matA(6, 6, arma::fill::zeros);
arma::vec matB(6, 1, arma::fill::zeros);
arma::vec result(6, 1, arma::fill::zeros);
matA(0, 0) = average4(ValidY, ValidY, ValidY, ValidY, validNum);
matA(0, 1) = average4(ValidY, ValidY, ValidZ, ValidZ, validNum);
matA(0, 2) = average3(ValidX, ValidY, ValidY, validNum);
matA(0, 3) = average3(ValidY, ValidY, ValidY, validNum);
matA(0, 4) = average3(ValidY, ValidY, ValidZ, validNum);
matA(0, 5) = average2(ValidY, ValidY, validNum);
matA(1, 0) = average4(ValidY, ValidY, ValidZ, ValidZ, validNum);
matA(1, 1) = average4(ValidZ, ValidZ, ValidZ, ValidZ, validNum);
matA(1, 2) = average3(ValidX, ValidZ, ValidZ, validNum);
matA(1, 3) = average3(ValidY, ValidZ, ValidZ, validNum);
matA(1, 4) = average3(ValidZ, ValidZ, ValidZ, validNum);
matA(1, 5) = average2(ValidZ, ValidZ, validNum);
matA(2, 0) = average3(ValidX, ValidY, ValidY, validNum);
matA(2, 1) = average3(ValidX, ValidZ, ValidZ, validNum);
matA(2, 2) = average2(ValidX, ValidX, validNum);
matA(2, 3) = average2(ValidX, ValidY, validNum);
matA(2, 4) = average2(ValidX, ValidZ, validNum);
matA(2, 5) = average1(ValidX, validNum);
matA(3, 0) = average3(ValidY, ValidY, ValidY, validNum);
matA(3, 1) = average3(ValidY, ValidZ, ValidZ, validNum);
matA(3, 2) = average2(ValidX, ValidY, validNum);
matA(3, 3) = average2(ValidY, ValidY, validNum);
matA(3, 4) = average2(ValidY, ValidZ, validNum);
matA(3, 5) = average1(ValidY, validNum);
matA(4, 0) = average3(ValidY, ValidY, ValidZ, validNum);
matA(4, 1) = average3(ValidZ, ValidZ, ValidZ, validNum);
matA(4, 2) = average2(ValidX, ValidZ, validNum);
matA(4, 3) = average2(ValidY, ValidZ, validNum);
matA(4, 4) = average2(ValidZ, ValidZ, validNum);
matA(4, 5) = average1(ValidZ, validNum);
matA(5, 0) = average2(ValidY, ValidY, validNum);
matA(5, 1) = average2(ValidZ, ValidZ, validNum);
matA(5, 2) = average1(ValidX, validNum);
matA(5, 3) = average1(ValidY, validNum);
matA(5, 4) = average1(ValidZ, validNum);
matA(5, 5) = 1;
matB(0) = -average4(ValidX, ValidX, ValidY, ValidY, validNum);
matB(1) = -average4(ValidX, ValidX, ValidZ, ValidZ, validNum);
matB(2) = -average3(ValidX, ValidX, ValidX, validNum);
matB(3) = -average3(ValidX, ValidX, ValidY, validNum);
matB(4) = -average3(ValidX, ValidX, ValidZ, validNum);
matB(5) = -average2(ValidX, ValidX, validNum);
result = inv(matA)*matB;
X0 = -result(2) / 2;
Y0 = -result(3) / (2 * result(0));
Z0 = -result(4) / (2 * result(1));
A = sqrt(X0*X0 + result(0)*Y0*Y0 + result(1)*Z0*Z0 - result(5));
B = A / sqrt(result(0));
C = A / sqrt(result(1));
以上是关于椭球曲面拟合算法实现，matlab/C++的主要内容，如果未能解决你的问题，请参考以下文章