//helper: computes a facet horizontal and vertical extensions
void ComputeFacetExtensions(CCVector3& N, ccPolyline* facetContour, double& horizExt, double& vertExt)
{
//horizontal and vertical extensions
horizExt = vertExt = 0;
CCLib::GenericIndexedCloudPersist* vertCloud = facetContour->getAssociatedCloud();
if (vertCloud)
{
//oriRotMat.applyRotation(N); //DGM: oriRotMat is only for display!
//we assume that at this point the "up" direction is always (0,0,1)
CCVector3 Xf(1,0,0), Yf(0,1,0);
//we get the horizontal vector on the plane
CCVector3 D = CCVector3(0,0,1).cross(N);
if (D.norm2() > ZERO_TOLERANCE) //otherwise the facet is horizontal!
{
Yf = D;
Yf.normalize();
Xf = N.cross(Yf);
}
const CCVector3* G = CCLib::Neighbourhood(vertCloud).getGravityCenter();
ccBBox box;
for (unsigned i=0; i<vertCloud->size(); ++i)
{
const CCVector3 P = *(vertCloud->getPoint(i)) - *G;
CCVector3 p( P.dot(Xf), P.dot(Yf), 0 );
box.add(p);
}
vertExt = box.getDiagVec().x;
horizExt = box.getDiagVec().y;
}
}
void ccClipBox::update()
{
if (m_entityContainer.getChildrenNumber() == 0)
{
return;
}
//remove any existing clipping plane
{
for (unsigned ci = 0; ci < m_entityContainer.getChildrenNumber(); ++ci)
{
m_entityContainer.getChild(ci)->removeAllClipPlanes();
}
}
//now add the 6 box clipping planes
ccBBox extents;
ccGLMatrix transformation;
get(extents, transformation);
CCVector3 C = transformation * extents.getCenter();
CCVector3 halfDim = extents.getDiagVec() / 2;
//for each dimension
for (unsigned d = 0; d < 3; ++d)
{
CCVector3 N = transformation.getColumnAsVec3D(d);
//positive side
{
ccClipPlane posPlane;
posPlane.equation.x = N.x;
posPlane.equation.y = N.y;
posPlane.equation.z = N.z;
//compute the 'constant' coefficient knowing that P belongs to the plane if (P - (C - half_dim * N)).N = 0
posPlane.equation.w = -static_cast<double>(C.dot(N)) + halfDim.u[d];
for (unsigned ci = 0; ci < m_entityContainer.getChildrenNumber(); ++ci)
{
m_entityContainer.getChild(ci)->addClipPlanes(posPlane);
}
}
//negative side
{
ccClipPlane negPlane;
negPlane.equation.x = -N.x;
negPlane.equation.y = -N.y;
negPlane.equation.z = -N.z;
//compute the 'constant' coefficient knowing that P belongs to the plane if (P - (C + half_dim * N)).N = 0
//negPlane.equation.w = -(static_cast<double>(C.dot(N)) + halfDim.u[d]);
negPlane.equation.w = static_cast<double>(C.dot(N)) + halfDim.u[d];
for (unsigned ci = 0; ci < m_entityContainer.getChildrenNumber(); ++ci)
{
m_entityContainer.getChild(ci)->addClipPlanes(negPlane);
}
}
}
}
bool HornRegistrationTools::FindAbsoluteOrientation(GenericCloud* lCloud,
GenericCloud* rCloud,
ScaledTransformation& trans,
bool fixedScale/*=false*/)
{
//resulting transformation (R is invalid on initialization and T is (0,0,0))
trans.R.invalidate();
trans.T = CCVector3(0,0,0);
trans.s = (PointCoordinateType)1.0;
assert(rCloud && lCloud);
if (!rCloud || !lCloud || rCloud->size() != lCloud->size() || rCloud->size()<3)
return false;
unsigned count = rCloud->size();
assert(count>2);
//determine best scale?
if (!fixedScale)
{
CCVector3 Gr = GeometricalAnalysisTools::computeGravityCenter(rCloud);
CCVector3 Gl = GeometricalAnalysisTools::computeGravityCenter(lCloud);
//we determine scale with the symmetrical form as proposed by Horn
double lNorm2Sum = 0.0;
{
lCloud->placeIteratorAtBegining();
for (unsigned i=0;i<count;i++)
{
CCVector3 Pi = *lCloud->getNextPoint()-Gl;
lNorm2Sum += Pi.dot(Pi);
}
}
if (lNorm2Sum >= ZERO_TOLERANCE)
{
double rNorm2Sum = 0.0;
{
rCloud->placeIteratorAtBegining();
for (unsigned i=0;i<count;i++)
{
CCVector3 Pi = *rCloud->getNextPoint()-Gr;
rNorm2Sum += Pi.dot(Pi);
}
}
//resulting scale
trans.s = (PointCoordinateType)sqrt(rNorm2Sum/lNorm2Sum);
}
//else
//{
// //shouldn't happen!
//}
}
return RegistrationProcedure(lCloud,rCloud,trans,0,0,trans.s);
}
void ccClipBox::update()
{
if (!m_associatedEntity)
{
return;
}
#ifdef USE_OPENGL
m_associatedEntity->removeAllClipPlanes();
//now add the 6 box clipping planes
ccBBox extents;
ccGLMatrix transformation;
get(extents, transformation);
CCVector3 C = transformation * extents.getCenter();
CCVector3 halfDim = extents.getDiagVec() / 2;
//for each dimension
for (unsigned d = 0; d < 3; ++d)
{
CCVector3 N = transformation.getColumnAsVec3D(d);
//positive side
{
ccClipPlane posPlane;
posPlane.equation.x = N.x;
posPlane.equation.y = N.y;
posPlane.equation.z = N.z;
//compute the 'constant' coefficient knowing that P belongs to the plane if (P - (C - half_dim * N)).N = 0
posPlane.equation.w = -static_cast<double>(C.dot(N)) + halfDim.u[d];
m_associatedEntity->addClipPlanes(posPlane);
}
//negative side
{
ccClipPlane negPlane;
negPlane.equation.x = -N.x;
negPlane.equation.y = -N.y;
negPlane.equation.z = -N.z;
//compute the 'constant' coefficient knowing that P belongs to the plane if (P - (C + half_dim * N)).N = 0
//negPlane.equation.w = -(static_cast<double>(C.dot(N)) + halfDim.u[d]);
negPlane.equation.w = static_cast<double>(C.dot(N)) + halfDim.u[d];
m_associatedEntity->addClipPlanes(negPlane);
}
}
#else
flagPointsInside();
#endif
}
float ccFastMarchingForNormsDirection::computePropagationConfidence(DirectionCell* originCell, DirectionCell* destCell) const
{
//1) it depends on the angle between the current cell's orientation
// and its neighbor's orientation (symmetric)
//2) it depends on whether the neighbor's relative position is
// compatible with the current cell orientation (symmetric)
CCVector3 AB = destCell->C - originCell->C;
AB.normalize();
float psOri = fabs(static_cast<float>(AB.dot(originCell->N))); //ideal: 90 degrees
float psDest = fabs(static_cast<float>(AB.dot(destCell->N))); //ideal: 90 degrees
float oriConfidence = (psOri + psDest)/2; //between 0 and 1 (ideal: 0)
return 1.0f - oriConfidence;
}
//return angle between two vectors (in degrees)
//warning: vectors will be normalized by default
double GetAngle_deg(CCVector3& AB, CCVector3& AC)
{
AB.normalize();
AC.normalize();
double dotprod = AB.dot(AC);
if (dotprod<=-1.0)
return 180.0;
else if (dotprod>1.0)
return 0.0;
return 180.0*acos(dotprod)/M_PI;
}
//return angle between two vectors (in degrees)
//warning: vectors will be normalized by default
static double GetAngle_deg(CCVector3 AB, CCVector3 AC)
{
AB.normalize();
AC.normalize();
double dotprod = AB.dot(AC);
//clamp value (just in case)
if (dotprod <= -1.0)
dotprod = -1.0;
else if (dotprod > 1.0)
dotprod = 1.0;
return acos(dotprod) * CC_RAD_TO_DEG;
}
float FastMarchingForFacetExtraction::computeTCoefApprox(CCLib::FastMarching::Cell* originCell, CCLib::FastMarching::Cell* destCell) const
{
PlanarCell* oCell = static_cast<PlanarCell*>(originCell);
PlanarCell* dCell = static_cast<PlanarCell*>(destCell);
//compute the 'confidence' relatively to the neighbor cell
//1) it depends on the angle between the current cell's orientation
// and its neighbor's orientation (symmetric)
//2) it depends on whether the neighbor's relative position is
// compatible with the current cell orientation (symmetric)
float orientationConfidence = 0;
{
CCVector3 AB = dCell->C - oCell->C;
AB.normalize();
float psOri = fabs(static_cast<float>(AB.dot(oCell->N))); //ideal: 90 degrees
float psDest = fabs(static_cast<float>(AB.dot(dCell->N))); //ideal: 90 degrees
orientationConfidence = (psOri + psDest)/2; //between 0 and 1 (ideal: 0)
}
//add reprojection error into balance
if (m_useRetroProjectionError && m_octree && oCell->N.norm2() != 0)
{
PointCoordinateType theLSQPlaneEquation[4];
theLSQPlaneEquation[0] = oCell->N.x;
theLSQPlaneEquation[1] = oCell->N.y;
theLSQPlaneEquation[2] = oCell->N.z;
theLSQPlaneEquation[3] = oCell->C.dot(oCell->N);
CCLib::ReferenceCloud Yk(m_octree->associatedCloud());
if (m_octree->getPointsInCell(oCell->cellCode,m_gridLevel,&Yk,true))
{
ScalarType reprojError = CCLib::DistanceComputationTools::ComputeCloud2PlaneDistance(&Yk,theLSQPlaneEquation,m_errorMeasure);
if (reprojError >= 0)
return (1.0f-orientationConfidence) * static_cast<float>(reprojError);
}
}
return (1.0f-orientationConfidence) /** oCell->planarError*/;
}
void PCVContext::setViewDirection(const CCVector3& V)
{
if (!m_pixBuffer || !m_pixBuffer->isValid())
return;
m_pixBuffer->makeCurrent();
glMatrixMode(GL_MODELVIEW);
glPushMatrix();
glLoadIdentity();
CCVector3 U(0,0,1);
if (1-fabs(V.dot(U)) < 1.0e-4)
{
U.y = 1;
U.z = 0;
}
gluLookAt(-V.x,-V.y,-V.z,0.0,0.0,0.0,U.x,U.y,U.z);
glGetFloatv(GL_MODELVIEW_MATRIX, m_viewMat);
glPopMatrix();
}
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);
}
ccPlane* ccPlane::Fit(CCLib::GenericIndexedCloudPersist *cloud, double* rms/*=0*/)
{
//number of points
unsigned count = cloud->size();
if (count < 3)
{
ccLog::Warning("[ccPlane::fitTo] Not enough points in input cloud to fit a plane!");
return 0;
}
CCLib::Neighbourhood Yk(cloud);
//plane equation
const PointCoordinateType* theLSQPlane = Yk.getLSQPlane();
if (!theLSQPlane)
{
ccLog::Warning("[ccGenericPointCloud::fitPlane] Not enough points to fit a plane!");
return 0;
}
//compute least-square fitting RMS if requested
if (rms)
{
*rms = CCLib::DistanceComputationTools::computeCloud2PlaneDistanceRMS(cloud, theLSQPlane);
}
//get the centroid
const CCVector3* G = Yk.getGravityCenter();
assert(G);
//and a local base
CCVector3 N(theLSQPlane);
const CCVector3* X = Yk.getLSQPlaneX(); //main direction
assert(X);
CCVector3 Y = N * (*X);
PointCoordinateType minX=0,maxX=0,minY=0,maxY=0;
cloud->placeIteratorAtBegining();
for (unsigned k=0; k<count; ++k)
{
//projetion into local 2D plane ref.
CCVector3 P = *(cloud->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 the plane
PointCoordinateType dX = maxX-minX;
PointCoordinateType dY = maxY-minY;
CCVector3 Gt = *G + *X * (minX + dX*static_cast<PointCoordinateType>(0.5));
Gt += Y * (minY + dY*static_cast<PointCoordinateType>(0.5));
ccGLMatrix glMat(*X,Y,N,Gt);
ccPlane* plane = new ccPlane(dX, dY, &glMat);
return plane;
}
ccQuadric* ccQuadric::Fit(CCLib::GenericIndexedCloudPersist *cloud, double* rms/*=0*/)
{
//number of points
unsigned count = cloud->size();
if (count < CC_LOCAL_MODEL_MIN_SIZE[HF])
{
ccLog::Warning(QString("[ccQuadric::fitTo] Not enough points in input cloud to fit a quadric! (%1 at the very least are required)").arg(CC_LOCAL_MODEL_MIN_SIZE[HF]));
return 0;
}
//project the points on a 2D plane
CCVector3 G, X, Y, N;
{
CCLib::Neighbourhood Yk(cloud);
//plane equation
const PointCoordinateType* theLSQPlane = Yk.getLSQPlane();
if (!theLSQPlane)
{
ccLog::Warning("[ccQuadric::Fit] Not enough points to fit a quadric!");
return 0;
}
assert(Yk.getGravityCenter());
G = *Yk.getGravityCenter();
//local base
N = CCVector3(theLSQPlane);
assert(Yk.getLSQPlaneX() && Yk.getLSQPlaneY());
X = *Yk.getLSQPlaneX(); //main direction
Y = *Yk.getLSQPlaneY(); //secondary direction
}
//project the points in a temporary cloud
ccPointCloud tempCloud("temporary");
if (!tempCloud.reserve(count))
{
ccLog::Warning("[ccQuadric::Fit] Not enough memory!");
return 0;
}
cloud->placeIteratorAtBegining();
for (unsigned k=0; k<count; ++k)
{
//projection into local 2D plane ref.
CCVector3 P = *(cloud->getNextPoint()) - G;
tempCloud.addPoint(CCVector3(P.dot(X),P.dot(Y),P.dot(N)));
}
CCLib::Neighbourhood Zk(&tempCloud);
{
//set exact values for gravity center and plane equation
//(just to be sure and to avoid re-computing them)
Zk.setGravityCenter(CCVector3(0,0,0));
PointCoordinateType perfectEq[4] = { 0, 0, 1, 0 };
Zk.setLSQPlane( perfectEq,
CCVector3(1,0,0),
CCVector3(0,1,0),
CCVector3(0,0,1));
}
uchar hfdims[3];
const PointCoordinateType* eq = Zk.getHeightFunction(hfdims);
if (!eq)
{
ccLog::Warning("[ccQuadric::Fit] Failed to fit a quadric!");
return 0;
}
//we recenter the quadric object
ccGLMatrix glMat(X,Y,N,G);
ccBBox bb = tempCloud.getBB();
CCVector2 minXY(bb.minCorner().x,bb.minCorner().y);
CCVector2 maxXY(bb.maxCorner().x,bb.maxCorner().y);
ccQuadric* quadric = new ccQuadric(minXY, maxXY, eq, hfdims, &glMat);
quadric->setMetaData(QString("Equation"),QVariant(quadric->getEquationString()));
//compute rms if necessary
if (rms)
{
const uchar& dX = hfdims[0];
const uchar& dY = hfdims[1];
const uchar& dZ = hfdims[2];
*rms = 0;
for (unsigned k=0; k<count; ++k)
{
//projection into local 2D plane ref.
const CCVector3* P = tempCloud.getPoint(k);
PointCoordinateType z = eq[0] + eq[1]*P->u[dX] + eq[2]*P->u[dY] + eq[3]*P->u[dX]*P->u[dX] + eq[4]*P->u[dX]*P->u[dY] + eq[5]*P->u[dY]*P->u[dY];
*rms += (z-P->z)*(z-P->z);
}
if (count)
{
*rms = sqrt(*rms / count);
quadric->setMetaData(QString("RMS"),QVariant(*rms));
}
}
return quadric;
}
ccGLMatrix ccGLMatrix::FromToRotation(const CCVector3& from, const CCVector3& to)
{
float e = from.dot(to);
float f = (e < 0 ? -e : e);
ccGLMatrix result;
float* mat = result.data();
if (f > 1.0-ZERO_TOLERANCE) //"from" and "to"-vector almost parallel
{
CCVector3 x; // vector most nearly orthogonal to "from"
x.x = (from.x > 0 ? from.x : -from.x);
x.y = (from.y > 0 ? from.y : -from.y);
x.z = (from.z > 0 ? from.z : -from.z);
if (x.x < x.y)
{
if (x.x < x.z)
{
x.x = 1.0f; x.y = x.z = 0;
}
else
{
x.z = 1.0f; x.x = x.y = 0;
}
}
else
{
if (x.y < x.z)
{
x.y = 1.0f; x.x = x.z = 0;
}
else
{
x.z = 1.0f; x.x = x.y = 0;
}
}
CCVector3 u(x.x-from.x, x.y-from.y, x.z-from.z);
CCVector3 v(x.x-to.x, x.y-to.y, x.z-to.z);
float c1 = 2.0f / u.dot(u);
float c2 = 2.0f / v.dot(v);
float c3 = c1 * c2 * u.dot(v);
for (unsigned i = 0; i < 3; i++)
{
for (unsigned j = 0; j < 3; j++)
{
mat[i*4+j]= c3 * v.u[i] * u.u[j]
- c2 * v.u[i] * v.u[j]
- c1 * u.u[i] * u.u[j];
}
mat[i*4+i] += 1.0f;
}
}
else // the most common case, unless "from"="to", or "from"=-"to"
{
//hand optimized version (9 mults less)
CCVector3 v = from.cross(to);
float h = 1.0f/(1.0f + e);
float hvx = h * v.x;
float hvz = h * v.z;
float hvxy = hvx * v.y;
float hvxz = hvx * v.z;
float hvyz = hvz * v.y;
mat[0] = e + hvx * v.x;
mat[1] = hvxy - v.z;
mat[2] = hvxz + v.y;
mat[4] = hvxy + v.z;
mat[5] = e + h * v.y * v.y;
mat[6] = hvyz - v.x;
mat[8] = hvxz - v.y;
mat[9] = hvyz + v.x;
mat[10] = e + hvz * v.z;
}
return result;
}
ccPlane* ccPlane::Fit(CCLib::GenericIndexedCloudPersist *cloud, double* rms/*=0*/)
{
//number of points
unsigned count = cloud->size();
if (count < 3)
{
ccLog::Warning("[ccPlane::Fit] Not enough points in input cloud to fit a plane!");
return 0;
}
CCLib::Neighbourhood Yk(cloud);
//plane equation
const PointCoordinateType* theLSPlane = Yk.getLSPlane();
if (!theLSPlane)
{
ccLog::Warning("[ccPlane::Fit] Not enough points to fit a plane!");
return 0;
}
//get the centroid
const CCVector3* G = Yk.getGravityCenter();
assert(G);
//and a local base
CCVector3 N(theLSPlane);
const CCVector3* X = Yk.getLSPlaneX(); //main direction
assert(X);
CCVector3 Y = N * (*X);
//compute bounding box in 2D plane
CCVector2 minXY(0,0), maxXY(0,0);
cloud->placeIteratorAtBegining();
for (unsigned k=0; k<count; ++k)
{
//projection into local 2D plane ref.
CCVector3 P = *(cloud->getNextPoint()) - *G;
CCVector2 P2D( P.dot(*X), P.dot(Y) );
if (k != 0)
{
if (minXY.x > P2D.x)
minXY.x = P2D.x;
else if (maxXY.x < P2D.x)
maxXY.x = P2D.x;
if (minXY.y > P2D.y)
minXY.y = P2D.y;
else if (maxXY.y < P2D.y)
maxXY.y = P2D.y;
}
else
{
minXY = maxXY = P2D;
}
}
//we recenter the plane
PointCoordinateType dX = maxXY.x-minXY.x;
PointCoordinateType dY = maxXY.y-minXY.y;
CCVector3 Gt = *G + *X * (minXY.x + dX / 2) + Y * (minXY.y + dY / 2);
ccGLMatrix glMat(*X,Y,N,Gt);
ccPlane* plane = new ccPlane(dX, dY, &glMat);
//compute least-square fitting RMS if requested
if (rms)
{
*rms = CCLib::DistanceComputationTools::computeCloud2PlaneDistanceRMS(cloud, theLSPlane);
plane->setMetaData(QString("RMS"),QVariant(*rms));
}
return plane;
}
ccHObject* qFacets::createFacets( ccPointCloud* cloud,
CCLib::ReferenceCloudContainer& components,
unsigned minPointsPerComponent,
double maxEdgeLength,
bool randomColors,
bool& error)
{
if (!cloud)
return 0;
//we create a new group to store all input CCs as 'facets'
ccHObject* ccGroup = new ccHObject(cloud->getName()+QString(" [facets]"));
ccGroup->setDisplay(cloud->getDisplay());
ccGroup->setVisible(true);
bool cloudHasNormal = cloud->hasNormals();
//number of input components
size_t componentCount = components.size();
//progress notification
ccProgressDialog pDlg(true,m_app->getMainWindow());
pDlg.setMethodTitle("Facets creation");
pDlg.setInfo(qPrintable(QString("Components: %1").arg(componentCount)));
pDlg.setMaximum(static_cast<int>(componentCount));
pDlg.show();
QApplication::processEvents();
//for each component
error = false;
while (!components.empty())
{
CCLib::ReferenceCloud* compIndexes = components.back();
components.pop_back();
//if it has enough points
if (compIndexes && compIndexes->size() >= minPointsPerComponent)
{
ccPointCloud* facetCloud = cloud->partialClone(compIndexes);
if (!facetCloud)
{
//not enough memory!
error = true;
delete facetCloud;
facetCloud = 0;
}
else
{
ccFacet* facet = ccFacet::Create(facetCloud,static_cast<PointCoordinateType>(maxEdgeLength),true);
if (facet)
{
QString facetName = QString("facet %1 (rms=%2)").arg(ccGroup->getChildrenNumber()).arg(facet->getRMS());
facet->setName(facetName);
if (facet->getPolygon())
{
facet->getPolygon()->enableStippling(false);
facet->getPolygon()->showNormals(false);
}
if (facet->getContour())
{
facet->getContour()->setGlobalScale(facetCloud->getGlobalScale());
facet->getContour()->setGlobalShift(facetCloud->getGlobalShift());
}
//check the facet normal sign
if (cloudHasNormal)
{
CCVector3 N = ccOctree::ComputeAverageNorm(compIndexes,cloud);
if (N.dot(facet->getNormal()) < 0)
facet->invertNormal();
}
#ifdef _DEBUG
facet->showNormalVector(true);
#endif
//shall we colorize it with a random color?
ccColor::Rgb col, darkCol;
if (randomColors)
{
col = ccColor::Generator::Random();
assert(c_darkColorRatio <= 1.0);
darkCol.r = static_cast<ColorCompType>(static_cast<double>(col.r) * c_darkColorRatio);
darkCol.g = static_cast<ColorCompType>(static_cast<double>(col.g) * c_darkColorRatio);
darkCol.b = static_cast<ColorCompType>(static_cast<double>(col.b) * c_darkColorRatio);
}
else
{
//use normal-based HSV coloring
CCVector3 N = facet->getNormal();
PointCoordinateType dip, dipDir;
ccNormalVectors::ConvertNormalToDipAndDipDir(N, dip, dipDir);
FacetsClassifier::GenerateSubfamilyColor(col,dip,dipDir,0,1,&darkCol);
}
facet->setColor(col);
if (facet->getContour())
{
facet->getContour()->setColor(darkCol);
facet->getContour()->setWidth(2);
}
ccGroup->addChild(facet);
}
}
if (compIndexes)
delete compIndexes;
compIndexes = 0;
}
pDlg.setValue(static_cast<int>(componentCount-components.size()));
//QApplication::processEvents();
}
if (ccGroup->getChildrenNumber() == 0)
{
delete ccGroup;
ccGroup = 0;
}
return ccGroup;
}
bool ccGriddedTools::ComputeNormals(ccPointCloud* cloud,
const std::vector<int>& indexGrid,
int width,
int height,
bool* canceledByUser/*=0*/,
int kernelWidth/*=3*/ )
{
//init
bool result = true;
if (canceledByUser)
*canceledByUser = false;
//try to compute normals
if (cloud->reserveTheNormsTable())
{
//progress dialog
ccProgressDialog pdlg(true);
CCLib::NormalizedProgress nprogress(&pdlg,cloud->size());
pdlg.setMethodTitle("Compute normals");
pdlg.setInfo(qPrintable(QString("Number of points: %1").arg(cloud->size())));
pdlg.start();
const int* _indexGrid = &(indexGrid[0]);
CCLib::ReferenceCloud knn(cloud);
//neighborhood 'half-width' (total width = 1 + 2*kernelWidth)
//max number of neighbours: (1+2*nw)^2
knn.reserve((1+2*kernelWidth)*(1+2*kernelWidth));
//for each grid cell
for (int j=0; j<height; ++j)
{
for (int i=0; i<width; ++i, ++_indexGrid)
{
if (*_indexGrid >= 0)
{
unsigned pointIndex = static_cast<unsigned>(*_indexGrid);
//add the point itself
knn.clear(false);
knn.addPointIndex(pointIndex); //warning: indexes are shifted (0 = no point)
const CCVector3* P = cloud->getPoint(pointIndex);
//look for neighbors
for (int v=std::max(0,j-kernelWidth); v<std::min<int>(height,j+kernelWidth); ++v)
{
if (v != j)
{
for (int u=std::max(0,i-kernelWidth); u<std::min<int>(width,i+kernelWidth); ++u)
{
if (u != i)
{
int indexN = indexGrid[v*width + u];
if (indexN >= 0)
{
//we don't consider points with a too much different depth than the central point
const CCVector3* Pn = cloud->getPoint(static_cast<unsigned>(indexN));
if (fabs(Pn->z - P->z) <= std::max(fabs(Pn->x - P->x),fabs(Pn->y - P->y)))
knn.addPointIndex(static_cast<unsigned>(indexN)); //warning: indexes are shifted (0 = no point)
}
}
}
}
}
CCVector3 N(0,0,1);
if (knn.size() >= 3)
{
CCLib::Neighbourhood Z(&knn);
//compute normal with quadratic func. (if we have enough points)
if (false/*knn.size() >= 6*/)
{
uchar hfDims[3];
const PointCoordinateType* h = Z.getHeightFunction(hfDims);
if (h)
{
const CCVector3* gv = Z.getGravityCenter();
assert(gv);
const uchar& iX = hfDims[0];
const uchar& iY = hfDims[1];
const uchar& iZ = hfDims[2];
PointCoordinateType lX = P->u[iX] - gv->u[iX];
PointCoordinateType lY = P->u[iY] - gv->u[iY];
N.u[iX] = h[1] + (2 * h[3] * lX) + (h[4] * lY);
N.u[iY] = h[2] + (2 * h[5] * lY) + (h[4] * lX);
N.u[iZ] = -PC_ONE;
N.normalize();
}
}
else
#define USE_LSQ_PLANE
#ifdef USE_LSQ_PLANE
{
//compute normal with best fit plane
const CCVector3* _N = Z.getLSQPlaneNormal();
if (_N)
N = *_N;
}
#else
{
//compute normals with 2D1/2 triangulation
CCLib::GenericIndexedMesh* theMesh = Z.triangulateOnPlane();
if (theMesh)
{
unsigned faceCount = theMesh->size();
N.z = 0;
//for all triangles
theMesh->placeIteratorAtBegining();
for (unsigned j=0; j<faceCount; ++j)
{
const CCLib::TriangleSummitsIndexes* tsi = theMesh->getNextTriangleIndexes();
//we look if the central point is one of the triangle's vertices
if (tsi->i1 == 0 || tsi->i2 == 0|| tsi->i3 == 0)
{
const CCVector3 *A = knn.getPoint(tsi->i1);
const CCVector3 *B = knn.getPoint(tsi->i2);
const CCVector3 *C = knn.getPoint(tsi->i3);
CCVector3 no = (*B - *A).cross(*C - *A);
//no.normalize();
N += no;
}
}
delete theMesh;
theMesh = 0;
//normalize the 'mean' vector
N.normalize();
}
}
#endif
}
//check normal vector sign
CCVector3 viewVector = *P /*- cloudTrans.getTranslationAsVec3D()*/; //clouds are still in their local coordinate system!
if (viewVector.dot(N) > 0)
N *= -PC_ONE;
cloud->addNorm(N);
//progress
if (!nprogress.oneStep())
{
result = false;
if (canceledByUser)
*canceledByUser = true;
ccLog::Warning("[ComputeNormals] Process canceled by user!");
//early stop
j = height;
break;
}
}
}
}
if (!result)
{
cloud->unallocateNorms();
}
}
else
{
ccLog::Warning("[ComputeNormals] Not enough memory!");
}
return result;
}
