void
DistanceTransformFInv(vector<float>& D,
const vector<unsigned char>& L,
int mode,
int ndim,
const int* dims) {
vector<unsigned char> invL(L.size());
int i;
for(i=0; i<L.size(); ++i) {
if(L[i]==0)
invL[i]=1;
else
invL[i]=0;
}
DistanceTransformF(D,invL,mode,ndim,dims);
}
bool EllipseFitting(POINT* rgPoints, int nPoints, double &a, double &b, double &c, double &d, double &e, double &f)
{
int i, j;
int np = nPoints;
int nrot=0;
Matrix<double> D(np+1,7); //Design matrix
Matrix<double> S(7,7); //Scatter matrix
Matrix<double> Const(7,7); //Constraint matrix
Matrix<double> temp(7,7);
Matrix<double> L(7,7); //The lower triangular matrix L * L' = S
Matrix<double> C(7,7);
Matrix<double> invL(7,7); //Inverse matrix of L
Matrix<double> V(7,7); //The eigen vectors
Matrix<double> sol(7,7); //The GVE solution
double ev[7]; //The eigen value
double pvec[7];
if (np < 6)
{
return false;
}
//Build design matrix D
for (i = 0; i < np; i++)
{
D[i][1] = rgPoints[i].x * rgPoints[i].x;
D[i][2] = rgPoints[i].x * rgPoints[i].y;
D[i][3] = rgPoints[i].y * rgPoints[i].y;
D[i][4] = rgPoints[i].x;
D[i][5] = rgPoints[i].y;
D[i][6] = 1.0;
}
//Build Scatter matrix S
A_TperB(D, D, S, np, 6, np, 6);
//Build 6*6 constraint matrix
Const[1][3] = -2;
Const[2][2] = 1;
Const[3][1] = -2;
//Sovle generalized eigen system
choldc(S, 6, L);
if (!inverse(L, invL, 6))
{
return false;
}
AperB_T(Const, invL, temp, 6, 6, 6, 6);
AperB(invL, temp, C, 6, 6, 6, 6);
jacobi(C, 6, ev, V, nrot);
A_TperB(invL, V, sol, 6, 6, 6, 6);
//Normalize the solution
for (j = 1; j <= 6; j++)
{
double mod = 0.0;
for (i = 1; i <= 6; i++)
{
mod += sol[i][j] * sol[i][j];
}
mod = sqrt(mod);
for (i = 1 ; i <= 6; i++)
{
sol[i][j] /= mod;
}
}
double zero = 10e-20;
int solind = 0;
//Find the only negative eigen value
for (i = 1; i <= 6; i++)
{
if ((ev[i] < 0) && (fabs(ev[i]) > zero))
{
solind = i;
}
}
//Get the fitted parameters
for (j = 1; j <= 6; j++)
{
pvec[j] = sol[j][solind];
}
a = pvec[1];
b = pvec[2];
c = pvec[3];
d = pvec[4];
e = pvec[5];
f = pvec[6];
return true;
}
ThinPlateSpline(std::vector<boost::array<WorkingType, PosDim> > positions,
std::vector<boost::array<WorkingType, ValDim> > values)
{
boost::numeric::ublas::matrix<WorkingType> L;
refPositions = positions;
const int numPoints((int)refPositions.size());
const int WaLength(numPoints + PosDim + 1);
L = boost::numeric::ublas::matrix<WorkingType> (WaLength, WaLength);
// Calculate K and store in L
for(int i(0); i < numPoints; i++)
{
L(i,i) = 0.0;
// K is symmetrical so no point in calculating things twice
int j(i + 1);
for(; j < numPoints; ++j)
{
L(i,j) = L(j,i) = radialbasis<WorkingType, WorkingType, PosDim>(refPositions[i], refPositions[j]);
}
// construct P and store in K
L(j,i) = L(i,j) = 1.0;
++j;
for(int posElm(0); j < WaLength; ++posElm, ++j)
L(j,i) = L(i,j) = positions[i][posElm];
}
// O
for(int i(numPoints); i < WaLength; i++)
for(int j(numPoints); j < WaLength; j++)
L(i,j) = 0.0;
// Solve L^-1 Y = W^T
typedef boost::numeric::ublas::permutation_matrix<std::size_t> pmatrix;
boost::numeric::ublas::matrix<WorkingType> A(L);
pmatrix pm(A.size1());
int res = (int)lu_factorize(A, pm);
if(res != 0)
{
;//TODO catch this error
}
boost::numeric::ublas::matrix<WorkingType> invL(boost::numeric::ublas::identity_matrix<WorkingType>(A.size1()));
lu_substitute(A, pm, invL);
Wa = boost::numeric::ublas::matrix<WorkingType>(WaLength, ValDim );
boost::numeric::ublas::matrix<WorkingType> Y(WaLength, ValDim);
int i(0);
for(; i < numPoints; i++)
for(int j(0); j < ValDim; ++j)
Y(i, j) = values[i][j];
for(; i < WaLength; i++)
for(int j(0); j < ValDim; ++j)
Y(i, j) = 0.0;
Wa = prod(invL,Y);
}
