梯度下降拟合曲面，修复曲面

方程 f(x,y)-z =a*x*x+b*y*y+c*x*y+d*x+e*y+1 

使用函数 

move_least_squares_point_repair2

#include "PC.h"
double calculate_Lost(vector<double> W, vector<vector<double>> P, vector<double> A,vector<double> Z) {
	double Lost = 0;
	for (int i = 0; i < W.size(); i++) {
		double temp = 0;
		for (int j = 0; j < P[i].size(); j++) {
			temp += P[i][j] * A[j];
		}
		temp -= Z[i];    ///f(x,y)-z
		Lost = Lost + W[i] * (temp )*(temp );
	}
	return Lost / (2.0*W.size());
}
double calculate_gradient(vector<double> W,int index,vector<vector<double>> P,vector<double> A,vector<double> Z) {
	 double gradient = 0;
	 for (int i = 0; i < W.size(); i++) {
		  double temp = 0;
		  for (int j = 0; j < P[i].size(); j++) {
			 temp += P[i][j] * A[j];
		   }
		  temp = Z[i] - temp;
		  gradient = gradient + W[i] *  P[i][index] * temp;
	 }
	 return gradient/(W.size());
}
vector<double> calculate_Z(double x, double y, vector<double> param, double zmin, double zmax) {
	vector<double> result;
	result.clear();
	double A = param[2];
	double B = param[4] * x + param[5] * y + param[8];
	double C = param[0] * x*x + param[1] * y*y + param[3] * x*y + param[6] * x + param[7] * y + param[9];
	if (B*B - 4 * A*C < 0) return result;
	else  if(B*B - 4 * A*C == 0){
		result.push_back(-1.0*B / (2 * A));
	}
	else {
		double dert = sqrt(B*B - 4 * A*C);
		double z1 = (-1.0*B - dert) / (2 * A);
		double z2 = (-1.0*B + dert) / (2 * A);
		//if(z1>=zmin&&z1<=zmax)
		if(z1>-10)
		     result.push_back(z1);
		//if (z2 >= zmin && z2 <= zmax)
		if (z2 > -10)
			result.push_back(z2);
	}
	return result;
}
vector<vector<double>>  move_least_squares_point_repair2(vector<vector<double>> data, double D,int itertion) {
	//采用梯度下降计算其中六个参数
	vector<double> A(6);
	vector<double> X;
	vector<double> Y;
	vector<double> Z;
	vector<double> W;
	vector<vector<double>> P;
	// for (int i = 0; i < 6; i++) A[i] = 1;
	for (int i = 0; i < data.size(); i ++) {
		X.push_back(data[i][0]);
		Y.push_back(data[i][1]);
		Z.push_back(data[i][2]);
	}
	for (int i = 0; i < X.size(); i++) {
		
		W.push_back(1.0);
	}
	int n = X.size();
	for (int i = 0; i < n; i++) {
		vector<double> temp;
		temp.push_back(X[i] * X[i]);
		temp.push_back(Y[i] * Y[i]);
		//temp.push_back(Z[i] * Z[i]);
		temp.push_back(X[i] * Y[i]);
		///temp.push_back(X[i] * Z[i]);
		//temp.push_back(Y[i] * Z[i]);
		temp.push_back(X[i]);
		temp.push_back(Y[i]);
		//temp.push_back(Z[i]);
		temp.push_back(1);
	
		P.push_back(temp);
	}
	double alpha = 0.01;
	int iter = 0;
	double Lost = calculate_Lost(W, P, A,Z);
	cout<<"损失函数初始值：" << Lost << endl;
	while (iter < itertion) {

		
		//梯度下降
		for (int i = 0; i < A.size(); i++) {
			A[i] = A[i] + alpha * calculate_gradient(W, i, P,A, Z);
		}
	
		iter++;
		double lost = calculate_Lost(W, P, A,Z);
		if (fabs(lost - Lost) / Lost < 1e-8) break;
		Lost = lost;
		//cout << iter <<" : " <<lost<< endl;
		//for (int i = 0; i < 6; i++) cout << A[i] << endl;
	}
	cout << "共计迭代 " << iter << " 次，损失为：" << Lost << endl;
	double xmax, xmin, ymax, ymin,zmin,zmax;
	xmax = data[0][0]; xmin = data[0][0]; ymax = data[0][1]; ymin = data[0][1]; zmax = data[0][2]; zmin = data[0][2];
	for (int i = 3; i < data.size(); i ++) {
		xmax = max(xmax, data[i][0]);
		xmin = min(xmin, data[i][0]);
		ymax = max(ymax, data[i][1]);
		ymin = min(ymin, data[i][1]);
	}
	vector<double> dataX, dataY;
	for (double i =xmin; i <= xmax; i += D) dataX.push_back(i);
	for (double i =ymin; i <= ymax; i += D) dataY.push_back(i);
	// 生成数据
	vector<vector<double>> data2;

	
	for (int i = 0; i < dataX.size(); i++) {
		for (int j = 0; j < dataY.size(); j++) {
			vector<double> point(3);
			point[0] = dataX[i];
			point[1] = dataY[j];
			point[2] = A[0] * dataX[i] * dataX[i] + A[1] * dataY[j] * dataY[j] + A[2] * dataX[i] * dataY[j] + A[3] * dataX[i] + A[4] * dataY[j] + A[5];
			data2.push_back(point);

		}
	}
	return data2;
}
vector<vector<double>>  least_squares(vector<vector<double>> data,double D) {
	vector<double> x;
	vector<double> y;
	vector<double> z;
	int N = data.size();
	for (int i = 0; i < data.size(); i++) {
		x.push_back(data[i][0]);
		y.push_back(data[i][1]);
		z.push_back(data[i][2]);
	}
	// 构造二次曲面拟合方程Ax=b中的A和b矩阵
	double A[9][9] = { 0 };
	double b[9] = { 0 };
	for (int i = 0; i < N; ++i) {
		double xi = x[i], yi = y[i], zi = z[i];
		A[0][0] += xi * xi; A[0][1] += xi * yi; A[0][2] += xi * zi;
		A[1][0] += xi * yi; A[1][1] += yi * yi; A[1][2] += yi * zi;
		A[2][0] += xi * zi; A[2][1] += yi * zi; A[2][2] += zi * zi;
		A[3][0] += 2 * xi;  A[3][1] += yi;     A[3][2] += zi;
		A[4][0] += xi;      A[4][1] += 2 * yi;  A[4][2] += zi;
		A[5][0] += xi;      A[5][1] += yi;     A[5][2] += 2 * zi;
		A[6][0] += 1;       A[6][1] += 0;      A[6][2] += 0;   A[6][6] += 0;
		A[7][0] += 0;       A[7][1] += 1;      A[7][2] += 0;   A[7][7] += 0;
		A[8][0] += 0;       A[8][1] += 0;      A[8][2] += 1;   A[8][8] += 0;
		b[0] -= xi * xi; b[1] -= yi * yi; b[2] -= zi * zi;
		b[3] -= 2 * xi;  b[4] -= 2 * yi;  b[5] -= 2 * zi;
	}

	// 最小二乘求解未知系数向量x
	double L[9][9] = { 0 };
	for (int i = 0; i < 9; ++i) {
		for (int j = 0; j <= i; ++j) {
			if (j == i) {
				double sum = 0.0;
				for (int k = 0; k < j; ++k) {
					sum += L[j][k] * L[j][k];
				}
				L[j][j] = sqrt(A[j][j] - sum);
			}
			else {
				double sum = 0.0;
				for (int k = 0; k < j; ++k) {
					sum += L[i][k] * L[j][k];
				}
				L[i][j] = (A[i][j] - sum) / L[j][j];
			}
		}
	}

	double yparam[9] = { 0 };
	for (int i = 0; i < 9; ++i) {
		double sum = 0.0;
		for (int k = 0; k < i; ++k) {
			sum += L[i][k] * yparam[k];
		}
		yparam[i] = (b[i] - sum) / L[i][i];
	}
	for (int i = 0; i < 9; i++) cout << yparam[i] << endl;
	vector<double> param(10);
	for (int i = 8; i >= 0; --i) 
	{ 
		double sum = 0.0; 
		for (int k = i + 1; k < 9; ++k) 
		{ 
			sum += L[k][i] * param[k];
		} 
		param[i] = (yparam[i] - sum) / L[i][i];
	} 
	param[9] = 1.0;
	cout << "二次曲面方程为:\n";
	printf("%lfx^2 + %lfy^2 + %lfz^2 + %lfxy + %lfxz + %lfyz + %lfx + %lfy + %lfz + %lf = 0", param[0], param[1], param[2], param[3], param[4], param[5], param[6], param[7], param[8], param[9]);
	double xmax, xmin, ymax, ymin, zmin, zmax;
	xmax = data[0][0]; xmin = data[0][0]; ymax = data[0][1]; ymin = data[0][1]; zmax = data[0][2]; zmin = data[0][2];
	for (int i = 3; i < data.size(); i++) {
		xmax = max(xmax, data[i][0]);
		xmin = min(xmin, data[i][0]);
		ymax = max(ymax, data[i][1]);
		ymin = min(ymin, data[i][1]);
	}
	vector<double> dataX, dataY;
	for (double i = xmin; i <= xmax; i += D) dataX.push_back(i);
	for (double i = ymin; i <= ymax; i += D) dataY.push_back(i);
	// 生成数据
	vector<vector<double>> data2;


	for (int i = 0; i < dataX.size(); i++) {
		for (int j = 0; j < dataY.size(); j++) {
			vector<double> point(3);
			point[0] = dataX[i];
			point[1] = dataY[j];
			vector<double> Z_solve = calculate_Z(point[0], point[1], param, zmin, zmax);
			// A[0] * 1 + A[1] * dataX[i] + A[2] * dataY[j] + A[3] * dataX[i] * dataY[j] + A[4] * dataX[i] * dataX[i] + A[5] * dataY[j] * dataY[j];
		//cout << point[2] << endl;
			for (int k = 0; k < Z_solve.size(); k++)
			{
				point[2] = Z_solve[k];
				data2.push_back(point);
			}

		}
	}
	return data2;
}