float* globalICPRegistration( float* model, int modelSize,
Vector3f minModelBounds, Vector3f maxModelBounds,
float *data, int dataSize,
int maxIterations)
{
float mdlSntrMass[4], dataSntrMass[4];
float *bestRegistration, lowestError = MAX_FLOAT_VALUE; //stores best attempt to registrate
int iterations;
icpStruct *localICP;
//Allocate memmory for IcpStruct
localICP = createICPInstance(modelSize, dataSize);
//Structure model as an OctTree and attach it to IcpStruct
localICP->model = createKD_Tree(model, modelSize);
//save
sntrMass(mdlSntrMass, model, modelSize);
sntrMass(dataSntrMass, data, dataSize);
memcpy(localICP->modelSntrMass, mdlSntrMass, sizeof(float) * 4);
bestRegistration = (float*)malloc(sizeof(float) * 16);
seedRandomGen();
for(iterations = 0; iterations < maxIterations; iterations++){
memcpy(localICP->data, data, sizeof(float) * dataSize);
memcpy(localICP->dataSntrMass, dataSntrMass, sizeof(float) * 4);
localICP->errorMeasure = MAX_FLOAT_VALUE;
toIdentity(localICP->registrationMatrix, 4);
//fill registration Translation vector with random trans values
matrix4Access(localICP->registrationMatrix, 0, 3) = randomInLimitf(minModelBounds.x, maxModelBounds.x);
matrix4Access(localICP->registrationMatrix, 1, 3) = randomInLimitf(minModelBounds.y, maxModelBounds.y);
matrix4Access(localICP->registrationMatrix, 2, 3) = randomInLimitf(minModelBounds.z, maxModelBounds.z);
applyInitialRegistration(localICP);
localICPRegistration(localICP, THRESHOLD, DELTA_THRESHOLD, MAX_LOCAL_ICP_ITERATIONS);
if( localICP->errorMeasure < lowestError ){
memcpy(bestRegistration, localICP->registrationMatrix, sizeof(float) * 16);
lowestError = localICP->errorMeasure;
if(lowestError < THRESHOLD)
break;
}
}
deleteICPInstance(localICP);
return bestRegistration;
}
ccGLMatrix::ccGLMatrix(const CCLib::SquareMatrix& R, const CCVector3& T)
{
toIdentity();
if (R.size()==3)
{
//we copy each column
float* mat = m_mat;
for (unsigned j=0;j<3;++j)
{
*mat++ = (float)R.m_values[0][j];
*mat++ = (float)R.m_values[1][j];
*mat++ = (float)R.m_values[2][j];
mat++;
}
}
*this += T;
}
static float* rotationMtrxFromQuaternion( float *dst, float *q )
{
float q00 = q[0]*q[0];
float q11 = q[1]*q[1];
float q22 = q[2]*q[2];
float q33 = q[3]*q[3];
float q03 = q[0]*q[3];
float q13 = q[1]*q[3];
float q23 = q[2]*q[3];
float q02 = q[0]*q[2];
float q12 = q[1]*q[2];
float q01 = q[0]*q[1];
toIdentity(dst, 4);
matrixAccess(dst, 0, 0, 4) = (q00 + q11 - q22 - q33);
matrixAccess(dst, 1, 1, 4) = (q00 - q11 + q22 - q33);
matrixAccess(dst, 2, 2, 4) = (q00 - q11 - q22 + q33);
matrixAccess(dst, 0, 1, 4) = (2.0f*(q12-q03));
matrixAccess(dst, 1, 0, 4) = (2.0f*(q12+q03));
matrixAccess(dst, 0, 2, 4) = (2.0f*(q13+q02));
matrixAccess(dst, 2, 0, 4) = (2.0f*(q13-q02));
matrixAccess(dst, 1, 2, 4) = (2.0f*(q23-q01));
matrixAccess(dst, 2, 1, 4) = (2.0f*(q23+q01));
return dst;
}
ccGLMatrix::ccGLMatrix()
{
toIdentity();
}
Matrix4::~Matrix4(void) {
toIdentity();
}
/* computes egenvectors and eigenvalues using Jacobian method
* returns egenValues on succses and NULL of failiar
* eigenVectors on succses will be filled with egenVectors corresponding to eigenvalues
* NB n can be up to a maximum of 10 for speed increace (static alloc vs dynamic)
*/
static float* computeJacobianEigenValuesAndVectors(float* matrix, float** eigenVectors, int n)
{
int j,iq,ip,i,nrot;
float tresh,theta,tau,t,sm,s,h,g,c,b[10],z[10],*d;
float *eigenValues;
*eigenVectors = (float*)malloc(sizeof(float) * n*n);
toIdentity(*eigenVectors, n);
d = eigenValues = (float*)malloc(sizeof(float) * n );
for (ip=0;ip<n;ip++)
{
b[ip]=d[ip]=matrix4Access(matrix, ip, ip); //Initialize b and d to the diagonal of a.
z[ip]=0.0; //This vector will accumulate terms of the form tapq as in equation (11.1.14)
}
nrot=0;
for (i=1;i<=50;i++)
{
sm=0.0;
for (ip=0;ip<n-1;ip++) //Sum off-diagonal elements.
{
for (iq=ip+1;iq<n;iq++)
sm += (float)fabs(matrix4Access(matrix, ip, iq));
}
if (sm == 0.0) //The normal return, which relies on quadratic convergence to machine underflow.
{
//we only need the absolute values of eigenvalues
for (ip=0;ip<n;ip++)
d[ip]=(float)fabs(d[ip]);
return eigenValues;
}
if (i < 4)
tresh = 0.2f * sm/(float)(n*n); //...on the first three sweeps.
else
tresh = 0.0f; //...thereafter.
for (ip=0;ip<n-1;ip++)
{
for (iq=ip+1;iq<n;iq++)
{
g=100.0f * (float)fabs(matrix4Access(matrix, ip, iq));
//After four sweeps, skip the rotation if the off-diagonal element is small.
if (i > 4 && (float)(fabs(d[ip])+g) == (float)fabs(d[ip])
&& (float)(fabs(d[iq])+g) == (float)fabs(d[iq]))
{
matrix4Access(matrix, ip, iq)=0.0f;
}
else if (fabs(matrix4Access(matrix, ip, iq)) > tresh)
{
h=d[iq]-d[ip];
if ((float)(fabs(h)+g) == (float)fabs(h))
t = matrix4Access(matrix, ip, iq )/h; //t = 1/(2¦theta)
else
{
theta=0.5f * h/matrix4Access(matrix, ip, iq); //Equation (11.1.10).
t=1.0f/(float)(fabs(theta)+sqrt(1.0f+theta*theta));
if (theta < 0.0)
t = -t;
}
c=1.0f/(float)sqrt(1.0f+t*t);
s=t*c;
tau=s/(1.0f+c);
h=t*matrix4Access(matrix, ip, iq);
z[ip] -= h;
z[iq] += h;
d[ip] -= h;
d[iq] += h;
matrix4Access(matrix, ip, iq)=0.0;
for (j=0;j<=ip-1;j++) //Case of rotations 1 <= j < p.
{
ROTATE(matrix,j,ip,j,iq);
}
for (j=ip+1;j<=iq-1;j++) //Case of rotations p < j < q.
{
ROTATE(matrix,ip,j,j,iq);
}
for (j=iq+1;j<n;j++) //Case of rotations q < j <= n.
{
ROTATE(matrix,ip,j,iq,j);
}
for (j=0;j<n;j++)
{
ROTATE((*eigenVectors),j,ip,j,iq);
}
++nrot;
}
}
}
for (ip=0;ip<n;ip++)
{
b[ip]+=z[ip];
d[ip]=b[ip]; //Update d with the sum of tapq,
z[ip]=0.0; //and reinitialize z.
}
}
//Too many iterations in routine jacobi!
free(eigenValues);
free(*eigenVectors);
return NULL;
}
Matrix3::Matrix3(void) {
toIdentity();
}
