You can do-it-yourself from this:
- use a 3x3 array
A
and a 3x1 vector
B
and accumulate the normal least-squares equations, using the formulas in the paper (this is an easy task);
- solve the 3x3 system using this function (this is the Cholesky method for a 3x3 symmetric positive defined matrix):
bool Cholesky3x3(double* A, double* B)
{
#define A(i,j) A[i + j * 3]
double Sum;
double Diagonal[3];
Sum= A(0,0);
if (Sum <= 0.f) return false;
Diagonal[0]= Sqrt(Sum);
Sum= A(0,1);
A(1,0)= Sum / Diagonal[0];
Sum= A(0,2);
A(2,0)= Sum / Diagonal[0];
Sum= A(1,1) - A(1,0) * A(1,0);
if (Sum <= 0.f) return false;
Diagonal[1]= Sqrt(Sum);
Sum= A(1,2) - A(1,0) * A(2,0);
A(2,1)= Sum / Diagonal[1];
Sum= A(2,2) - A(2,1) * A(2,1) - A(2,0) * A(2,0);
if (Sum <= 0.f) return false;
Diagonal[2]= Sqrt(Sum);
Sum= B[0];
B[0]= Sum / Diagonal[0];
Sum= B[1] - A(1,0) * B[0];
B[1]= Sum / Diagonal[1];
Sum= B[2] - A(2,1) * B[1] - A(2,0) * B[0];
B[2]= Sum / Diagonal[2];
Sum= B[2];
B[2]= Sum / Diagonal[2];
Sum= B[1] - A(2,1) * B[2];
B[1]= Sum / Diagonal[1];
Sum= B[0] - A(1,0) * B[1] - A(2,0) * B[2];
B[0]= Sum / Diagonal[0];
return true;
}
Solutions in
B
. Compute
Xc
,
Yc
,
R
, you are done. Requires no library.