bool RegistrationTools::RegistrationProcedure(GenericCloud* P,
GenericCloud* X,
PointProjectionTools::Transformation& trans,
ScalarField* weightsP/*=0*/,
ScalarField* weightsX/*=0*/,
PointCoordinateType scale/*=1.0f*/)
{
//resulting transformation (R is invalid on initialization and T is (0,0,0))
trans.R.invalidate();
trans.T = CCVector3(0,0,0);
PointCoordinateType bbMin[3],bbMax[3];
X->getBoundingBox(bbMin,bbMax);
PointCoordinateType dx = bbMax[0]-bbMin[0];
PointCoordinateType dy = bbMax[1]-bbMin[1];
PointCoordinateType dz = bbMax[2]-bbMin[2];
CCVector3 Gp = GeometricalAnalysisTools::computeGravityCenter(P);
CCVector3 Gx = GeometricalAnalysisTools::computeGravityCenter(X);
//if the cloud is equivalent to a single point (for instance
//it's the case when the two clouds are very far away from
//each other in the ICP process) we try to get the two clouds closer
if (fabs(dx)+fabs(dy)+fabs(dz) < ZERO_TOLERANCE)
{
trans.T = Gx - Gp*scale;
return true;
}
//Cross covariance matrix
SquareMatrixd Sigma_px = (weightsP || weightsX) ? GeometricalAnalysisTools::computeWeightedCrossCovarianceMatrix(P,X,Gp.u,Gx.u,weightsP,weightsX) : GeometricalAnalysisTools::computeCrossCovarianceMatrix(P,X,Gp.u,Gx.u);
if (!Sigma_px.isValid())
return false;
SquareMatrixd Sigma_px_t = Sigma_px;
Sigma_px_t.transpose();
SquareMatrixd Aij = Sigma_px-Sigma_px_t;
double trace = Sigma_px.trace();
SquareMatrixd traceI3(3);
traceI3.m_values[0][0]=trace;
traceI3.m_values[1][1]=trace;
traceI3.m_values[2][2]=trace;
SquareMatrixd bottomMat = Sigma_px+Sigma_px_t-traceI3;
//we build up the registration matrix (see ICP algorithm)
SquareMatrixd QSigma(4);
QSigma.m_values[0][0]=trace;
QSigma.m_values[0][1]=QSigma.m_values[1][0]=Aij.m_values[1][2];
QSigma.m_values[0][2]=QSigma.m_values[2][0]=Aij.m_values[2][0];
QSigma.m_values[0][3]=QSigma.m_values[3][0]=Aij.m_values[0][1];
QSigma.m_values[1][1]=bottomMat.m_values[0][0];
QSigma.m_values[1][2]=bottomMat.m_values[0][1];
QSigma.m_values[1][3]=bottomMat.m_values[0][2];
QSigma.m_values[2][1]=bottomMat.m_values[1][0];
QSigma.m_values[2][2]=bottomMat.m_values[1][1];
QSigma.m_values[2][3]=bottomMat.m_values[1][2];
QSigma.m_values[3][1]=bottomMat.m_values[2][0];
QSigma.m_values[3][2]=bottomMat.m_values[2][1];
QSigma.m_values[3][3]=bottomMat.m_values[2][2];
//we compute its eigenvalues and eigenvectors
SquareMatrixd eig = QSigma.computeJacobianEigenValuesAndVectors();
if (!eig.isValid())
return false;
//as Besl says, the best rotation corresponds to the eigenvector associated to the biggest eigenvalue
double qR[4];
eig.getMaxEigenValueAndVector(qR);
//these eigenvalue and eigenvector correspond to a quaternion --> we get the corresponding matrix
trans.R.initFromQuaternion(qR);
//and we deduce the translation
trans.T = Gx - (trans.R*Gp)*scale;
return true;
}
bool Neighbourhood::compute3DQuadric(double quadricEquation[10])
{
if (!m_associatedCloud || !quadricEquation)
{
//invalid (input) parameters
assert(false);
return false;
}
//computes a 3D quadric of the form ax2 +by2 +cz2 + 2exy + 2fyz + 2gzx + 2lx + 2my + 2nz + d = 0
//"THREE-DIMENSIONAL SURFACE CURVATURE ESTIMATION USING QUADRIC SURFACE PATCHES", I. Douros & B. Buxton, University College London
//we get centroid
const CCVector3* G = getGravityCenter();
assert(G);
//we look for the eigen vector associated to the minimum eigen value of a matrix A
//where A=transpose(D)*D, and D=[xi^2 yi^2 zi^2 xiyi yizi xizi xi yi zi 1] (i=1..N)
unsigned count = m_associatedCloud->size();
//we compute M = [x2 y2 z2 xy yz xz x y z 1] for all points
std::vector<PointCoordinateType> M;
{
try
{
M.resize(count*10);
}
catch (std::bad_alloc)
{
return false;
}
PointCoordinateType* _M = &(M[0]);
for (unsigned i=0; i<count; ++i)
{
CCVector3 P = *m_associatedCloud->getPoint(i) - *G;
//we fill the ith line
(*_M++) = P.x * P.x;
(*_M++) = P.y * P.y;
(*_M++) = P.z * P.z;
(*_M++) = P.x * P.y;
(*_M++) = P.y * P.z;
(*_M++) = P.x * P.z;
(*_M++) = P.x;
(*_M++) = P.y;
(*_M++) = P.z;
(*_M++) = 1;
}
}
//D = tM.M
SquareMatrixd D(10);
for (unsigned l=0; l<10; ++l)
{
for (unsigned c=0; c<10; ++c)
{
double sum = 0;
PointCoordinateType* _M = &(M[0]);
for (unsigned i=0; i<count; ++i, _M+=10)
sum += static_cast<double>(_M[l] * _M[c]);
D.m_values[l][c] = sum;
}
}
//we don't need M anymore
M.clear();
//now we compute eigen values and vectors of D
SquareMatrixd eig = D.computeJacobianEigenValuesAndVectors();
//failure?
if (!eig.isValid())
return false;
//we get the eigen vector corresponding to the minimum eigen value
/*double lambdaMin = */eig.getMinEigenValueAndVector(quadricEquation);
return true;
}
bool Neighbourhood::compute3DQuadric()
{
//invalidate previous quadric (if any)
structuresValidity &= (~QUADRIC_3D);
assert(m_associatedCloud);
if (!m_associatedCloud)
return false;
//computes a 3D quadric of the form ax2 +by2 +cz2 + 2exy + 2fyz + 2gzx + 2lx + 2my + 2nz + d = 0
//"THREE-DIMENSIONAL SURFACE CURVATURE ESTIMATION USING QUADRIC SURFACE PATCHES", I. Douros & B. Buxton, University College London
//we get centroid
const CCVector3* G = getGravityCenter();
assert(G);
//we look for the eigen vector associated to the minimum eigen value of a matrix A
//where A=transpose(D)*D, and D=[xi^2 yi^2 zi^2 xiyi yizi xizi xi yi zi 1] (i=1..N)
unsigned i,l,c,count = m_associatedCloud->size();
CCVector3 P;
//we compute M = [x2 y2 z2 xy yz xz x y z 1] for all points
PointCoordinateType* M = new PointCoordinateType[count*10];
if (!M)
return false;
PointCoordinateType* _M = M;
for (i=0;i<count;++i)
{
P = *m_associatedCloud->getPoint(i) - *G;
//we fill the ith line
(*_M++) = P.x * P.x;
(*_M++) = P.y * P.y;
(*_M++) = P.z * P.z;
(*_M++) = P.x * P.y;
(*_M++) = P.y * P.z;
(*_M++) = P.x * P.z;
(*_M++) = P.x;
(*_M++) = P.y;
(*_M++) = P.z;
(*_M++) = 1.0;
}
//D = tM.M
SquareMatrixd D(10);
for (l=0; l<10; ++l)
{
for (c=0; c<10; ++c)
{
double sum=0.0;
_M = M;
for (i=0; i<count; ++i)
{
sum += (double)(_M[l] * _M[c]);
_M+=10;
}
D.m_values[l][c] = sum;
}
}
//we don't need M anymore
delete[] M;
M=0;
//now we compute eigen values and vectors of D
SquareMatrixd eig = D.computeJacobianEigenValuesAndVectors();
//failure?
if (!eig.isValid())
return false;
//we get the eigen vector corresponding to the minimum eigen value
double vec[10];
/*double lambdaMin = */eig.getMinEigenValueAndVector(vec);
//we store result
for (i=0;i<10;++i)
the3DQuadric[i]=vec[i];
structuresValidity |= QUADRIC_3D;
return true;
}
ScalarType Neighbourhood::computeCurvature2(unsigned neighbourIndex, CC_CURVATURE_TYPE cType)
{
//we get 3D quadric parameters
const double* Q = get3DQuadric();
if (!Q)
return NAN_VALUE;
//we get centroid (should have already been computed during Quadric computation)
const CCVector3* Gc = getGravityCenter();
//we compute curvature at the input neighbour position + we recenter it by the way
CCVector3 Pc = *m_associatedCloud->getPoint(neighbourIndex) - *Gc;
double a=Q[0];
const double b=Q[1]/a;
const double c=Q[2]/a;
const double e=Q[3]/a;
const double f=Q[4]/a;
const double g=Q[5]/a;
const double l=Q[6]/a;
const double m=Q[7]/a;
const double n=Q[8]/a;
//const double d=Q[9]/a;
a=1.0;
const double& x=Pc.x;
const double& y=Pc.y;
const double& z=Pc.z;
//first order partial derivatives
const double Fx = 2.*a*x+e*y+g*z+l;
const double Fy = 2.*b*y+e*x+f*z+m;
const double Fz = 2.*c*z+f*y+g*x+n;
const double Fx2 = Fx*Fx;
const double Fy2 = Fy*Fy;
const double Fz2 = Fz*Fz;
//coefficients E,F,G
const double E = 1.+Fx2/Fz2;
const double F = Fx2/Fz2;
const double G = 1.+Fy2/Fz2;
//second order partial derivatives
//const double Fxx = 2.*a;
//const double Fyy = 2.*b;
//const double Fzz = 2.*c;
//const double Fxy = e;
//const double Fyx = e;
//const double Fyz = f;
//const double Fzy = f;
//const double Fxz = g;
//const double Fzx = g;
//gradient norm
const double gradF = sqrt(Fx2+Fy2+Fz2);
//coefficients L,M,N
SquareMatrixd D(3);
D.m_values[0][0] = 2.*a;//Fxx;
D.m_values[0][1] = g;//Fxz;
D.m_values[0][2] = Fx;
D.m_values[1][0] = g;//Fzx;
D.m_values[1][1] = 2.*c;//Fzz;
D.m_values[1][2] = Fz;
D.m_values[2][0] = Fx;
D.m_values[2][1] = Fz;
D.m_values[2][2] = 0.0;
const double L = D.computeDet()/(Fz2*gradF);
D.m_values[0][0] = e;//Fxy;
D.m_values[0][1] = f;//Fyz;
D.m_values[0][2] = Fy;
/*D.m_values[1][0] = Fzx;
D.m_values[1][1] = Fzz;
D.m_values[1][2] = Fz;
D.m_values[2][0] = Fx;
D.m_values[2][1] = Fz;
D.m_values[2][2] = 0.0;
//*/
const double M = D.computeDet()/(Fz2*gradF);
D.m_values[0][0] = 2.*b;//Fyy;
//D.m_values[0][1] = f;//Fyz;
//D.m_values[0][2] = Fy;
D.m_values[1][0] = f;//Fzy;
//D.m_values[1][1] = Fzz;
//D.m_values[1][2] = Fz;
D.m_values[2][0] = Fy;
//D.m_values[2][1] = Fz;
//D.m_values[2][2] = 0.0;
const double N = D.computeDet()/(Fz2*gradF);
//compute curvature
SquareMatrixd A(2);
A.m_values[0][0] = L;
A.m_values[0][1] = M;
A.m_values[1][0] = M;
A.m_values[1][1] = N;
SquareMatrixd B(2);
B.m_values[0][0] = E;
B.m_values[0][1] = F;
B.m_values[1][0] = F;
B.m_values[1][1] = G;
/*FILE* fp = fopen("computeCurvature2_trace.txt","wt");
fprintf(fp,"Fx=%f\n",Fx);
fprintf(fp,"Fy=%f\n",Fx);
fprintf(fp,"Fz=%f\n",Fx);
fprintf(fp,"E=%f\n",E);
fprintf(fp,"F=%f\n",F);
fprintf(fp,"G=%f\n",G);
fprintf(fp,"L=%f\n",L);
fprintf(fp,"M=%f\n",M);
fprintf(fp,"N=%f\n",N);
fprintf(fp,"A:\n%f %f\n%f %f\n",A.m_values[0][0],A.m_values[0][1],A.m_values[1][0],A.m_values[1][1]);
fprintf(fp,"B:\n%f %f\n%f %f\n",B.m_values[0][0],B.m_values[0][1],B.m_values[1][0],B.m_values[1][1]);
//fclose(fp);
//*/
//Gaussian curvature K = det(A)/det(B)
if (cType==GAUSSIAN_CURV)
{
ScalarType K = (ScalarType)(A.computeDet() / B.computeDet());
if (K<-1.0)
K=-1.0;
else if (K>1.0)
K=1.0;
return K;
}
//*/
SquareMatrixd Binv = B.inv();
if (!Binv.isValid())
return NAN_VALUE;
SquareMatrixd C = B.inv() * A;
//Mean curvature H = 1/2 trace(B^-1 * A)
if (cType==MEAN_CURV)
{
ScalarType H = (ScalarType)(0.5*C.trace());
if (H<-1.0)
H=-1.0;
else if (H>1.0)
H=1.0;
return fabs(H);
}
//*/
//principal curvatures are the eigenvalues k1 and k2 of B^-1 * A
/*SquareMatrixd eig = C.computeJacobianEigenValuesAndVectors();
if (!eig.isValid())
return NAN_VALUE;
const double k1 = eig.getEigenValueAndVector(0);
const double k2 = eig.getEigenValueAndVector(1);
//*/
/*FILE* fp = fopen("computeCurvature2_trace.txt","a");
fprintf(fp,"Binv:\n%f %f\n%f %f\n",Binv.values[0][0],Binv.values[0][1],Binv.values[1][0],Binv.values[1][1]);
fprintf(fp,"C:\n%f %f\n%f %f\n",C.values[0][0],C.values[0][1],C.values[1][0],C.values[1][1]);
fprintf(fp,"k1=%f\n",k1);
fprintf(fp,"k2=%f\n",k2);
fclose(fp);
//*/
/*switch (cType)
{
case GAUSSIAN_CURV:
//Gaussian curvature
return k1*k2;
case MEAN_CURV:
//Mean curvature
return (k1+k2)/2.0;
}
//*/
return NAN_VALUE;
}
bool RegistrationTools::RegistrationProcedure(GenericCloud* P,
GenericCloud* X,
ScaledTransformation& trans,
bool adjustScale/*=false*/,
ScalarField* weightsP/*=0*/,
ScalarField* weightsX/*=0*/,
PointCoordinateType aPrioriScale/*=1.0f*/)
{
//resulting transformation (R is invalid on initialization, T is (0,0,0) and s==1)
trans.R.invalidate();
trans.T = CCVector3(0,0,0);
trans.s = PC_ONE;
if (P == 0 || X == 0 || P->size() != X->size() || P->size() < 3)
return false;
PointCoordinateType bbMin[3],bbMax[3];
X->getBoundingBox(bbMin,bbMax);
//BBox dimensions
PointCoordinateType dx = bbMax[0]-bbMin[0];
PointCoordinateType dy = bbMax[1]-bbMin[1];
PointCoordinateType dz = bbMax[2]-bbMin[2];
//centers of mass
CCVector3 Gp = GeometricalAnalysisTools::computeGravityCenter(P);
CCVector3 Gx = GeometricalAnalysisTools::computeGravityCenter(X);
//if the data cloud is equivalent to a single point (for instance
//it's the case when the two clouds are very far away from
//each other in the ICP process) we try to get the two clouds closer
if (fabs(dx)+fabs(dy)+fabs(dz) < ZERO_TOLERANCE)
{
trans.T = Gx - Gp*aPrioriScale;
return true;
}
//Cross covariance matrix, eq #24 in Besl92 (but with weights, if any)
SquareMatrixd Sigma_px = (weightsP || weightsX) ? GeometricalAnalysisTools::computeWeightedCrossCovarianceMatrix(P,X,Gp.u,Gx.u,weightsP,weightsX) : GeometricalAnalysisTools::computeCrossCovarianceMatrix(P,X,Gp.u,Gx.u);
if (!Sigma_px.isValid())
return false;
SquareMatrixd Sigma_px_t = Sigma_px; //sigma_px_t is sigma_px transposed!
Sigma_px_t.transpose();
SquareMatrixd Aij = Sigma_px-Sigma_px_t;
double trace = Sigma_px.trace(); //that is the sum of diagonal elements f sigma_px
SquareMatrixd traceI3(3); //create the I matrix with eigvals equal to trace
traceI3.m_values[0][0]=trace;
traceI3.m_values[1][1]=trace;
traceI3.m_values[2][2]=trace;
SquareMatrixd bottomMat = Sigma_px+Sigma_px_t-traceI3;
//we build up the registration matrix (see ICP algorithm)
SquareMatrixd QSigma(4); //#25 in the paper (besl)
QSigma.m_values[0][0] = trace;
QSigma.m_values[0][1] = QSigma.m_values[1][0] = Aij.m_values[1][2];
QSigma.m_values[0][2] = QSigma.m_values[2][0] = Aij.m_values[2][0];
QSigma.m_values[0][3] = QSigma.m_values[3][0] = Aij.m_values[0][1];
QSigma.m_values[1][1] = bottomMat.m_values[0][0];
QSigma.m_values[1][2] = bottomMat.m_values[0][1];
QSigma.m_values[1][3] = bottomMat.m_values[0][2];
QSigma.m_values[2][1] = bottomMat.m_values[1][0];
QSigma.m_values[2][2] = bottomMat.m_values[1][1];
QSigma.m_values[2][3] = bottomMat.m_values[1][2];
QSigma.m_values[3][1] = bottomMat.m_values[2][0];
QSigma.m_values[3][2] = bottomMat.m_values[2][1];
QSigma.m_values[3][3] = bottomMat.m_values[2][2];
//we compute its eigenvalues and eigenvectors
SquareMatrixd eig = QSigma.computeJacobianEigenValuesAndVectors();
if (!eig.isValid())
return false;
//as Besl says, the best rotation corresponds to the eigenvector associated to the biggest eigenvalue
double qR[4];
eig.getMaxEigenValueAndVector(qR);
//these eigenvalue and eigenvector correspond to a quaternion --> we get the corresponding matrix
trans.R.initFromQuaternion(qR);
if (adjustScale)
{
//two accumulators
double acc_num = 0.0;
double acc_denom = 0.0;
//now deduce the scale (refer to "Point Set Registration with Integrated Scale Estimation", Zinsser et. al, PRIP 2005)
X->placeIteratorAtBegining();
P->placeIteratorAtBegining();
unsigned count = X->size();
assert(P->size() == count);
for (unsigned i=0; i<count; ++i)
{
//'a' refers to the data 'A' (moving) = P
//'b' refers to the model 'B' (not moving) = X
CCVector3 a_tilde = trans.R * (*(P->getNextPoint()) - Gp); // a_tilde_i = R * (a_i - a_mean)
CCVector3 b_tilde = (*(X->getNextPoint()) - Gx); // b_tilde_j = (b_j - b_mean)
acc_num += (double)b_tilde.dot(a_tilde);
acc_denom += (double)a_tilde.dot(a_tilde);
}
//DGM: acc_2 can't be 0 because we already have checked that the bbox is not a single point!
assert(acc_denom > 0.0);
trans.s = static_cast<PointCoordinateType>(fabs(acc_num / acc_denom));
}
//and we deduce the translation
trans.T = Gx - (trans.R*Gp)*(aPrioriScale*trans.s); //#26 in besl paper, modified with the scale as in jschmidt
return true;
}
