main/src/georegression/fitting/curves/FitCurve_F64.java
public static boolean fit(double[] data, int offset , int length , PolynomialQuadratic2D_F64 output ) {

		final int N = length/3;
		if( N < 6 )
			throw new IllegalArgumentException("Need at least 6 points and not "+N);

		// Unrolled pseudo inverse
		// coef = inv(A^T*A)*A^T*y
		double sx1=0, sy1=0, sx1y1=0, sx2=0, sy2=0, sx2y1=0, sx1y2=0, sx2y2=0, sx3=0, sy3=0, sx3y1=0, sx1y3=0, sx4=0, sy4=0;

		double b0=0,b1=0,b2=0,b3=0,b4=0,b5=0;

		int end = offset+length;
		for (int i = offset; i < end; i += 3 ) {
			double x = data[i];
			double y = data[i+1];
			double z = data[i+2];

			double x2 = x*x;
			double x3 = x2*x;
			double x4 = x2*x2;
			double y2 = y*y;
			double y3 = y2*y;
			double y4 = y2*y2;

			sx1 += x; sx2 += x2; sx3 += x3;sx4 += x4;
			sy1 += y; sy2 += y2; sy3 += y3;sy4 += y4;
			sx1y1 += x*y; sx2y1 += x2*y; sx1y2 += x*y2; sx2y2 +=x2*y2;
			sx3y1 += x3*y; sx1y3 += x*y3;

			b0 += z;
			b1 += x*z;
			b2 += y*z;
			b3 += x*y*z;
			b4 += x2*z;
			b5 += y2*z;
		}

		// using a fixed size matrix because the notation is much nicer
		DMatrix6x6 A = new DMatrix6x6();
		A.set(  N    ,sx1,  sy1  ,sx1y1,sx2  ,sy2  ,
				sx1  ,sx2,  sx1y1,sx2y1,sx3  ,sx1y2,
				sy1  ,sx1y1,sy2  ,sx1y2,sx2y1,sy3  ,
				sx1y1,sx2y1,sx1y2,sx2y2,sx3y1,sx1y3,
				sx2  ,sx3  ,sx2y1,sx3y1,sx4  ,sx2y2,
				sy2  ,sx1y2,sy3  ,sx1y3,sx2y2,sy4   );

		DMatrixRMaj _A = new DMatrixRMaj(6,6);
		ConvertDMatrixStruct.convert(A,_A);

		// pseudo inverse is required to handle degenerate matrices, e.g. lines
		LinearSolverDense<DMatrixRMaj> solver = LinearSolverFactory_DDRM.pseudoInverse(true);
		if( !solver.setA(_A) )
			return false;
		solver.invert(_A);

		DMatrixRMaj B = new DMatrixRMaj(6,1,true,b0,b1,b2,b3,b4,b5);
		DMatrixRMaj Y = new DMatrixRMaj(6,1);
		CommonOps_DDRM.mult(_A,B,Y);

		// output = inv(A)*B      Unrolled here for speed
		output.a = Y.data[0];
		output.b = Y.data[1];
		output.c = Y.data[2];
		output.d = Y.data[3];
		output.e = Y.data[4];
		output.f = Y.data[5];

		return true;
	}
public static boolean fit(double[] data, int offset , int length , PolynomialQuadratic2D_F64 output ) {

		final int N = length/3;
		if( N < 6 )
			throw new IllegalArgumentException("Need at least 6 points and not "+N);

		// Unrolled pseudo inverse
		// coef = inv(A^T*A)*A^T*y
		double sx1=0, sy1=0, sx1y1=0, sx2=0, sy2=0, sx2y1=0, sx1y2=0, sx2y2=0, sx3=0, sy3=0, sx3y1=0, sx1y3=0, sx4=0, sy4=0;

		double b0=0,b1=0,b2=0,b3=0,b4=0,b5=0;

		int end = offset+length;
		for (int i = offset; i < end; i += 3 ) {
			double x = data[i];
			double y = data[i+1];
			double z = data[i+2];

			double x2 = x*x;
			double x3 = x2*x;
			double x4 = x2*x2;
			double y2 = y*y;
			double y3 = y2*y;
			double y4 = y2*y2;

			sx1 += x; sx2 += x2; sx3 += x3;sx4 += x4;
			sy1 += y; sy2 += y2; sy3 += y3;sy4 += y4;
			sx1y1 += x*y; sx2y1 += x2*y; sx1y2 += x*y2; sx2y2 +=x2*y2;
			sx3y1 += x3*y; sx1y3 += x*y3;

			b0 += z;
			b1 += x*z;
			b2 += y*z;
			b3 += x*y*z;
			b4 += x2*z;
			b5 += y2*z;
		}

		// using a fixed size matrix because the notation is much nicer
		DMatrix6x6 A = new DMatrix6x6();
		A.set(  N    ,sx1,  sy1  ,sx1y1,sx2  ,sy2  ,
				sx1  ,sx2,  sx1y1,sx2y1,sx3  ,sx1y2,
				sy1  ,sx1y1,sy2  ,sx1y2,sx2y1,sy3  ,
				sx1y1,sx2y1,sx1y2,sx2y2,sx3y1,sx1y3,
				sx2  ,sx3  ,sx2y1,sx3y1,sx4  ,sx2y2,
				sy2  ,sx1y2,sy3  ,sx1y3,sx2y2,sy4   );

		DMatrixRMaj _A = new DMatrixRMaj(6,6);
		ConvertDMatrixStruct.convert(A,_A);

		// pseudo inverse is required to handle degenerate matrices, e.g. lines
		LinearSolverDense<DMatrixRMaj> solver = LinearSolverFactory_DDRM.pseudoInverse(true);
		if( !solver.setA(_A) )
			return false;
		solver.invert(_A);

		DMatrixRMaj B = new DMatrixRMaj(6,1,true,b0,b1,b2,b3,b4,b5);
		DMatrixRMaj Y = new DMatrixRMaj(6,1);
		CommonOps_DDRM.mult(_A,B,Y);

		// output = inv(A)*B      Unrolled here for speed
		output.a = Y.data[0];
		output.b = Y.data[1];
		output.c = Y.data[2];
		output.d = Y.data[3];
		output.e = Y.data[4];
		output.f = Y.data[5];

		return true;
	}
<p>Fits points to a 2D quadratic polynomial. There are two inputs and one output for each data point.
It's recommended that you apply a linear transform to the points. </p>

@param data Interleaved data [inputA[0], inputB[0], output[0], ....
@param offset first index in data
@param length number of elements in data that are to be read. must be divisible by 2
@param output (Optional) storage for the curve
@return The fitted curve