//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; }