椭球曲面拟合算法实现，matlab/C++

最新推荐文章于 2024-07-08 13:42:21 发布

Naruto_Q

最新推荐文章于 2024-07-08 13:42:21 发布

阅读量1.3w

点赞数 7

分类专栏： C/C++ Math/Matlab

本文链接：https://blog.csdn.net/piaoxuezhong/article/details/77247827

版权

C/C++ 同时被 2 个专栏收录

30 篇文章 8 订阅

订阅专栏

Math/Matlab

13 篇文章 9 订阅

订阅专栏

椭球曲面的标准表达式：(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));
	
}

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