椭球曲面拟合算法实现，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));    endendx = 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 -100class 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));}

几个求均值函数比较简单，就不po了，试了下效果与matlab程序基本一致。