ScalarType Neighbourhood::computeCurvature(unsigned neighbourIndex, CC_CURVATURE_TYPE cType)
{
switch (cType)
{
case GAUSSIAN_CURV:
case MEAN_CURV:
{
//we get 2D1/2 quadric parameters
const PointCoordinateType* H = getQuadric();
if (!H)
return NAN_VALUE;
//compute centroid
const CCVector3* G = getGravityCenter();
//we compute curvature at the input neighbour position + we recenter it by the way
CCVector3 Q = *m_associatedCloud->getPoint(neighbourIndex) - *G;
const uchar X = m_quadricEquationDirections.x;
const uchar Y = m_quadricEquationDirections.y;
//z = a+b.x+c.y+d.x^2+e.x.y+f.y^2
//const PointCoordinateType& a = H[0];
const PointCoordinateType& b = H[1];
const PointCoordinateType& c = H[2];
const PointCoordinateType& d = H[3];
const PointCoordinateType& e = H[4];
const PointCoordinateType& f = H[5];
//See "CURVATURE OF CURVES AND SURFACES – A PARABOLIC APPROACH" by ZVI HAR’EL
const PointCoordinateType fx = b + (d*2) * Q.u[X] + (e ) * Q.u[Y]; // b+2d*X+eY
const PointCoordinateType fy = c + (e ) * Q.u[X] + (f*2) * Q.u[Y]; // c+2f*Y+eX
const PointCoordinateType fxx = d*2; // 2d
const PointCoordinateType fyy = f*2; // 2f
const PointCoordinateType& fxy = e; // e
const PointCoordinateType fx2 = fx*fx;
const PointCoordinateType fy2 = fy*fy;
const PointCoordinateType q = (1 + fx2 + fy2);
switch (cType)
{
case GAUSSIAN_CURV:
{
//to sign the curvature, we need a normal!
PointCoordinateType K = fabs( fxx*fyy - fxy*fxy ) / (q*q);
return static_cast<ScalarType>(K);
}
case MEAN_CURV:
{
//to sign the curvature, we need a normal!
PointCoordinateType H = fabs( ((1+fx2)*fyy - 2*fx*fy*fxy + (1+fy2)*fxx) ) / (2*sqrt(q)*q);
return static_cast<ScalarType>(H);
}
default:
assert(false);
}
}
break;
case NORMAL_CHANGE_RATE:
{
assert(m_associatedCloud);
unsigned pointCount = (m_associatedCloud ? m_associatedCloud->size() : 0);
//we need at least 4 points
if (pointCount < 4)
{
//not enough points!
return pointCount == 3 ? 0 : NAN_VALUE;
}
//we determine plane normal by computing the smallest eigen value of M = 1/n * S[(p-µ)*(p-µ)']
CCLib::SquareMatrixd eig = computeCovarianceMatrix().computeJacobianEigenValuesAndVectors();
//invalid matrix?
if (!eig.isValid())
return NAN_VALUE;
//compute curvature as the rate of change of the surface
double e0 = eig.getEigenValues()[0];
double e1 = eig.getEigenValues()[1];
double e2 = eig.getEigenValues()[2];
double sum = fabs(e0+e1+e2);
if (sum < ZERO_TOLERANCE)
return NAN_VALUE;
double eMin = std::min(std::min(e0,e1),e2);
return static_cast<ScalarType>(fabs(eMin) / sum);
}
break;
default:
assert(false);
}
return NAN_VALUE;
}
ccPlane* ccGenericPointCloud::fitPlane(double* rms /*= 0*/)
{
//number of points
unsigned count = size();
if (count<3)
return 0;
CCLib::Neighbourhood Yk(this);
//we determine plane normal by computing the smallest eigen value of M = 1/n * S[(p-µ)*(p-µ)']
CCLib::SquareMatrixd eig = Yk.computeCovarianceMatrix().computeJacobianEigenValuesAndVectors();
//invalid matrix?
if (!eig.isValid())
{
//ccConsole::Warning("[ccPointCloud::fitPlane] Failed to compute plane/normal for cloud '%s'",getName());
return 0;
}
eig.sortEigenValuesAndVectors();
//plane equation
PointCoordinateType theLSQPlane[4];
//the smallest eigen vector corresponds to the "least square best fitting plane" normal
double vec[3];
eig.getEigenValueAndVector(2,vec);
//PointCoordinateType sign = (vec[2] < 0.0 ? -1.0 : 1.0); //plane normal (always with a positive 'Z' by default)
for (unsigned i=0;i<3;++i)
theLSQPlane[i]=/*sign*/(PointCoordinateType)vec[i];
CCVector3 N(theLSQPlane);
//we also get centroid
const CCVector3* G = Yk.getGravityCenter();
assert(G);
//eventually we just have to compute 'constant' coefficient a3
//we use the fact that the plane pass through the centroid --> GM.N = 0 (scalar prod)
//i.e. a0*G[0]+a1*G[1]+a2*G[2]=a3
theLSQPlane[3] = G->dot(N);
//least-square fitting RMS
if (rms)
{
placeIteratorAtBegining();
*rms = 0.0;
for (unsigned k=0;k<count;++k)
{
double d = (double)CCLib::DistanceComputationTools::computePoint2PlaneDistance(getNextPoint(),theLSQPlane);
*rms += d*d;
}
*rms = sqrt(*rms)/(double)count;
}
//we has a plane primitive to the cloud
eig.getEigenValueAndVector(0,vec); //main direction
CCVector3 X(vec[0],vec[1],vec[2]); //plane normal
//eig.getEigenValueAndVector(1,vec); //intermediate direction
//CCVector3 Y(vec[0],vec[1],vec[2]); //plane normal
CCVector3 Y = N * X;
//we eventually check for plane extents
PointCoordinateType minX=0.0,maxX=0.0,minY=0.0,maxY=0.0;
placeIteratorAtBegining();
for (unsigned k=0;k<count;++k)
{
//projetion into local 2D plane ref.
CCVector3 P = *getNextPoint() - *G;
PointCoordinateType x2D = P.dot(X);
PointCoordinateType y2D = P.dot(Y);
if (k!=0)
{
if (minX<x2D)
minX=x2D;
else if (maxX>x2D)
maxX=x2D;
if (minY<y2D)
minY=y2D;
else if (maxY>y2D)
maxY=y2D;
}
else
{
minX=maxX=x2D;
minY=maxY=y2D;
}
}
//we recenter plane (as it is not always the case!)
float dX = maxX-minX;
float dY = maxY-minY;
CCVector3 Gt = *G + X * (minX+dX*0.5);
Gt += Y * (minY+dY*0.5);
ccGLMatrix glMat(X,Y,N,Gt);
return new ccPlane(dX,dY,&glMat);
}
bool Neighbourhood::computeLeastSquareBestFittingPlane()
{
//invalidate previous LS plane (if any)
m_structuresValidity &= (~FLAG_LS_PLANE);
assert(m_associatedCloud);
unsigned pointCount = (m_associatedCloud ? m_associatedCloud->size() : 0);
//we need at least 3 points to compute a plane
assert(CC_LOCAL_MODEL_MIN_SIZE[LS] >= 3);
if (pointCount < CC_LOCAL_MODEL_MIN_SIZE[LS])
{
//not enough points!
return false;
}
CCVector3 G(0,0,0);
if (pointCount > 3)
{
//we determine plane normal by computing the smallest eigen value of M = 1/n * S[(p-µ)*(p-µ)']
CCLib::SquareMatrixd eig = computeCovarianceMatrix().computeJacobianEigenValuesAndVectors();
//invalid matrix?
if (!eig.isValid())
return false;
//get normal
{
CCVector3d vec;
//the smallest eigen vector corresponds to the "least square best fitting plane" normal
eig.getMinEigenValueAndVector(vec.u);
m_lsPlaneVectors[2] = CCVector3::fromArray(vec.u);
}
//get also X (Y will be deduced by cross product, see below
{
CCVector3d vec;
eig.getMaxEigenValueAndVector(vec.u);
m_lsPlaneVectors[0] = CCVector3::fromArray(vec.u);
}
//get the centroid (should already be up-to-date - see computeCovarianceMatrix)
G = *getGravityCenter();
}
else
{
//we simply compute the normal of the 3 points by cross product!
const CCVector3* A = m_associatedCloud->getPoint(0);
const CCVector3* B = m_associatedCloud->getPoint(1);
const CCVector3* C = m_associatedCloud->getPoint(2);
//get X (AB by default) and Y (AC - will be updated later) and deduce N = X ^ Y
m_lsPlaneVectors[0] = (*B-*A);
m_lsPlaneVectors[1] = (*C-*A);
m_lsPlaneVectors[2] = m_lsPlaneVectors[0].cross(m_lsPlaneVectors[1]);
//the plane passes through any of the 3 points
G = *A;
}
//make sure all vectors are unit!
if (m_lsPlaneVectors[2].norm2() < ZERO_TOLERANCE)
{
//this means that the points are colinear!
//m_lsPlaneVectors[2] = CCVector3(0,0,1); //any normal will do
return false;
}
else
{
m_lsPlaneVectors[2].normalize();
}
//normalize X as well
m_lsPlaneVectors[0].normalize();
//and update Y
m_lsPlaneVectors[1] = m_lsPlaneVectors[2].cross(m_lsPlaneVectors[0]);
//deduce the proper equation
m_lsPlaneEquation[0] = m_lsPlaneVectors[2].x;
m_lsPlaneEquation[1] = m_lsPlaneVectors[2].y;
m_lsPlaneEquation[2] = m_lsPlaneVectors[2].z;
//eventually we just have to compute the 'constant' coefficient a3
//we use the fact that the plane pass through G --> GM.N = 0 (scalar prod)
//i.e. a0*G[0]+a1*G[1]+a2*G[2]=a3
m_lsPlaneEquation[3] = G.dot(m_lsPlaneVectors[2]);
m_structuresValidity |= FLAG_LS_PLANE;
return true;
}
