bool Neighbourhood::computeQuadric()
{
//invalidate previous quadric (if any)
m_structuresValidity &= (~FLAG_QUADRIC);
assert(m_associatedCloud);
if (!m_associatedCloud)
return false;
unsigned count = m_associatedCloud->size();
assert(CC_LOCAL_MODEL_MIN_SIZE[QUADRIC] >= 5);
if (count < CC_LOCAL_MODEL_MIN_SIZE[QUADRIC])
return false;
const PointCoordinateType* lsPlane = getLSPlane();
if (!lsPlane)
return false;
//we get centroid (should already be up-to-date - see computeCovarianceMatrix)
const CCVector3* G = getGravityCenter();
assert(G);
//get the best projection axis
Tuple3ub idx(0/*x*/,1/*y*/,2/*z*/); //default configuration: z is the "normal" direction, we use (x,y) as the base plane
PointCoordinateType nxx = lsPlane[0]*lsPlane[0];
PointCoordinateType nyy = lsPlane[1]*lsPlane[1];
PointCoordinateType nzz = lsPlane[2]*lsPlane[2];
if (nxx > nyy)
{
if (nxx > nzz)
{
//as x is the "normal" direction, we use (y,z) as the base plane
idx.x = 1/*y*/; idx.y = 2/*z*/; idx.z = 0/*x*/;
}
}
else
{
if (nyy > nzz)
{
//as y is the "normal" direction, we use (z,x) as the base plane
idx.x = 2/*z*/; idx.y = 0/*x*/; idx.z = 1/*y*/;
}
}
//compute the A matrix and b vector
std::vector<float> A;
std::vector<float> b;
try
{
A.resize(6*count);
b.resize(count);
}
catch (std::bad_alloc)
{
//not enough memory
return false;
}
float lmax2 = 0; //max (squared) dimension
//for all points
{
float* _A = &(A[0]);
float* _b = &(b[0]);
for (unsigned i=0; i<count; ++i)
{
CCVector3 P = *m_associatedCloud->getPoint(i) - *G;
float lX = static_cast<float>(P.u[idx.x]);
float lY = static_cast<float>(P.u[idx.y]);
float lZ = static_cast<float>(P.u[idx.z]);
*_A++ = 1.0f;
*_A++ = lX;
*_A++ = lY;
*_A = lX*lX;
//by the way, we track the max 'X' squared dimension
if (*_A > lmax2)
lmax2 = *_A;
++_A;
*_A++ = lX*lY;
*_A = lY*lY;
//by the way, we track the max 'Y' squared dimension
if (*_A > lmax2)
lmax2 = *_A;
++_A;
*_b++ = lZ;
lZ *= lZ;
//and don't forget to track the max 'Z' squared dimension as well
if (lZ > lmax2)
lmax2 = lZ;
}
}
//conjugate gradient initialization
//we solve tA.A.X=tA.b
ConjugateGradient<6,double> cg;
CCLib::SquareMatrixd& tAA = cg.A();
double* tAb = cg.b();
//compute tA.A and tA.b
{
for (unsigned i=0; i<6; ++i)
{
//tA.A part
for (unsigned j=i; j<6; ++j)
{
double tmp = 0;
float* _Ai = &(A[i]);
float* _Aj = &(A[j]);
for (unsigned k=0; k<count; ++k)
{
//tmp += A[(6*k)+i] * A[(6*k)+j];
tmp += static_cast<double>(*_Ai) * static_cast<double>(*_Aj);
_Ai += 6;
_Aj += 6;
}
tAA.m_values[j][i] = tAA.m_values[i][j] = tmp;
}
//tA.b part
{
double tmp = 0;
float* _Ai = &(A[i]);
float* _b = &(b[i]);
for (unsigned k=0; k<count; ++k)
{
//tmp += A[(6*k)+i]*b[k];
tmp += static_cast<double>(*_Ai) * static_cast<double>(*_b++);
_Ai += 6;
}
tAb[i] = tmp;
}
}
}
//first guess for X: plane equation (a0.x+a1.y+a2.z=a3 --> z = a3/a2 - a0/a2.x - a1/a2.y)
double X0[6] = {static_cast<double>(/*lsPlane[3]/lsPlane[idx.z]*/0), //DGM: warning, points have already been recentred around the gravity center! So forget about a3
static_cast<double>(-lsPlane[idx.x]/lsPlane[idx.z]),
static_cast<double>(-lsPlane[idx.y]/lsPlane[idx.z]),
0,
0,
0 };
//special case: a0 = a1 = a2 = 0! //happens for perfectly flat surfaces!
if (X0[1] == 0 && X0[2] == 0)
X0[0] = 1.0;
//init. conjugate gradient
cg.initConjugateGradient(X0);
//conjugate gradient iterations
{
double convergenceThreshold = lmax2 * 1.0e-8; //max. error for convergence = 1e-8 of largest cloud dimension (empirical!)
for (unsigned i=0; i<1500; ++i)
{
double lastError = cg.iterConjugateGradient(X0);
if (lastError < convergenceThreshold) //converged
break;
}
}
//fprintf(fp,"X%i=(%f,%f,%f,%f,%f,%f)\n",i,X0[0],X0[1],X0[2],X0[3],X0[4],X0[5]);
//fprintf(fp,"lastError=%E/%E\n",lastError,convergenceThreshold);
//fclose(fp);
//output
{
for (unsigned i=0; i<6; ++i)
{
m_quadricEquation[i] = static_cast<PointCoordinateType>(X0[i]);
}
m_quadricEquationDirections = idx;
m_structuresValidity |= FLAG_QUADRIC;
}
return true;
}
bool Neighbourhood::computeHeightFunction()
{
//invalidate previous quadric (if any)
structuresValidity &= (~HEIGHT_FUNCTION);
assert(m_associatedCloud);
if (!m_associatedCloud)
return false;
unsigned count = m_associatedCloud->size();
assert(CC_LOCAL_MODEL_MIN_SIZE[HF] >= 5);
if (count < CC_LOCAL_MODEL_MIN_SIZE[HF])
return false;
const PointCoordinateType* lsq = getLSQPlane();
if (!lsq)
return false;
//we get centroid (should already be up-to-date - see computeCovarianceMatrix)
const CCVector3* G = getGravityCenter();
assert(G);
//get the best projection axis
uchar idx_X=0/*x*/,idx_Y=1/*y*/,idx_Z=2/*z*/; //default configuration: z is the "normal" direction, we use (x,y) as the base plane
PointCoordinateType nxx = lsq[0]*lsq[0];
PointCoordinateType nyy = lsq[1]*lsq[1];
PointCoordinateType nzz = lsq[2]*lsq[2];
if (nxx > nyy)
{
if (nxx > nzz)
{
//as x is the "normal" direction, we use (y,z) as the base plane
idx_X=1/*y*/; idx_Y=2/*z*/; idx_Z=0/*x*/;
}
}
else
{
if (nyy > nzz)
{
//as y is the "normal" direction, we use (z,x) as the base plane
idx_X=2/*z*/; idx_Y=0/*x*/; idx_Z=1/*y*/;
}
}
//compute the A matrix and b vector
float *A = new float[6*count];
float *b = new float[count];
float lmax2=0; //max (squared) dimension
//for all points
{
float* _A=A;
float* _b=b;
for (unsigned i=0;i<count;++i)
{
CCVector3 P = *m_associatedCloud->getPoint(i) - *G;
float lX = (float)P.u[idx_X];
float lY = (float)P.u[idx_Y];
float lZ = (float)P.u[idx_Z];
*_A++ = 1.0;
*_A++ = lX;
*_A++ = lY;
*_A = lX*lX;
//by the way, we track the max 'X' squared dimension
if (*_A>lmax2)
lmax2=*_A;
++_A;
*_A++ = lX*lY;
*_A = lY*lY;
//by the way, we track the max 'Y' squared dimension
if (*_A>lmax2)
lmax2=*_A;
++_A;
*_b++ = lZ;
lZ *= lZ;
//and don't forget to track the max 'Z' squared dimension as well
if (lZ>lmax2)
lmax2=lZ;
}
}
//conjugate gradient initialization
//we solve tA.A.X=tA.b
ConjugateGradient<6,double> cg;
CCLib::SquareMatrixd& tAA = cg.A();
double* tAb = cg.b();
//compute tA.A and tA.b
{
for (unsigned i=0; i<6; ++i)
{
//tA.A part
for (unsigned j=i; j<6; ++j)
{
double tmp = 0;
float* _Ai = A+i;
float* _Aj = A+j;
for (unsigned k=0; k<count; ++k)
{
//tmp += A[(6*k)+i]*A[(6*k)+j];
tmp += (double)((*_Ai) * (*_Aj));
_Ai += 6;
_Aj += 6;
}
tAA.m_values[j][i] = tAA.m_values[i][j] = tmp;
}
//tA.b part
{
double tmp = 0;
float* _Ai = A+i;
float* _b = b;
for (unsigned k=0; k<count; ++k)
{
//tmp += A[(6*k)+i]*b[k];
tmp += (double)((*_Ai) * (*_b++));
_Ai += 6;
}
tAb[i] = tmp;
}
}
}
//first guess for X: plane equation (a0.x+a1.y+a2.z=a3 --> z = a3/a2 - a0/a2.x - a1/a2.y)
double X0[6];
X0[0] = (double)/*lsq[3]/lsq[idx_Z]*/0; //DGM: warning, points have already been recentred around the gravity center! So forget about a3
X0[1] = (double)(-lsq[idx_X]/lsq[idx_Z]);
X0[2] = (double)(-lsq[idx_Y]/lsq[idx_Z]);
X0[3] = 0;
X0[4] = 0;
X0[5] = 0;
//special case: a0 = a1 = a2 = 0! //happens for perfectly flat surfaces!
if (X0[1] == 0.0 && X0[2] == 0.0)
X0[0] = 1.0;
//init. conjugate gradient
cg.initConjugateGradient(X0);
//conjugate gradient iterations
{
double convergenceThreshold = (double)lmax2 * 1e-8; //max. error for convergence = 1e-8 of largest cloud dimension (empirical!)
for (unsigned i=0; i<1500; ++i)
{
double lastError = cg.iterConjugateGradient(X0);
if (lastError < convergenceThreshold) //converged
break;
}
}
//fprintf(fp,"X%i=(%f,%f,%f,%f,%f,%f)\n",i,X0[0],X0[1],X0[2],X0[3],X0[4],X0[5]);
//fprintf(fp,"lastError=%E/%E\n",lastError,convergenceThreshold);
//fclose(fp);
delete[] A;
A=0;
delete[] b;
b=0;
//output
{
for (unsigned i=0;i<6;++i)
theHeightFunction[i]=(PointCoordinateType)X0[i];
theHeightFunctionDirections[0]=idx_X;
theHeightFunctionDirections[1]=idx_Y;
theHeightFunctionDirections[2]=idx_Z;
structuresValidity |= HEIGHT_FUNCTION;
}
return true;
}
