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;
	}
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
<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 >