static bool ComputeCellStats( CCLib::ReferenceCloud* subset,
CCVector3& N,
CCVector3& C,
ScalarType& error,
CCLib::DistanceComputationTools::ERROR_MEASURES errorMeasure)
{
error = 0;
if (!subset || subset->size() == 0)
return false;
//we compute the gravity center
CCLib::Neighbourhood Yk(subset);
C = *Yk.getGravityCenter();
//we compute the (least square) best fit plane
const PointCoordinateType* planeEquation = Yk.getLSPlane();
if (planeEquation)
{
N = CCVector3(planeEquation); //normal = first 3 components
error = CCLib::DistanceComputationTools::ComputeCloud2PlaneDistance(subset, planeEquation, errorMeasure);
}
else
{
//not enough points?
N = CCVector3(0,0,0);
}
return true;
}
ScalarType FastMarchingForFacetExtraction::addCellToCurrentFacet(unsigned index)
{
if (!m_currentFacetPoints || !m_initialized || !m_octree || m_gridLevel > CCLib::DgmOctree::MAX_OCTREE_LEVEL)
return -1;
PlanarCell* cell = static_cast<PlanarCell*>(m_theGrid[index]);
if (!cell)
return -1;
CCLib::ReferenceCloud Yk(m_octree->associatedCloud());
if (!m_octree->getPointsInCell(cell->cellCode,m_gridLevel,&Yk,true))
return -1;
if (!m_currentFacetPoints->add(Yk))
{
//not enough memory?
return -1;
}
//update error
CCVector3 N,C;
ScalarType error;
ComputeCellStats(m_currentFacetPoints,N,C,error,m_errorMeasure);
return error;
}
unsigned FastMarchingForFacetExtraction::updateFlagsTable( ccGenericPointCloud* theCloud,
GenericChunkedArray<1,unsigned char> &flags,
unsigned facetIndex)
{
if (!m_initialized || !m_currentFacetPoints)
return 0;
unsigned pointCount = m_currentFacetPoints->size();
for (unsigned k=0; k<pointCount; ++k)
{
unsigned index = m_currentFacetPoints->getPointGlobalIndex(k);
flags.setValue(index,1);
theCloud->setPointScalarValue(index,static_cast<ScalarType>(facetIndex));
}
if (m_currentFacetPoints)
m_currentFacetPoints->clear(false);
/*for (size_t i=0; i<m_activeCells.size(); ++i)
{
//we remove the processed cell so as to be sure not to consider them again!
CCLib::FastMarching::Cell* cell = m_theGrid[m_activeCells[i]];
m_theGrid[m_activeCells[i]] = 0;
if (cell)
delete cell;
}
//*/
//unsigned pointCount = 0;
CCLib::ReferenceCloud Yk(m_octree->associatedCloud());
for (size_t i=0; i<m_activeCells.size(); ++i)
{
PlanarCell* aCell = static_cast<PlanarCell*>(m_theGrid[m_activeCells[i]]);
if (!m_octree->getPointsInCell(aCell->cellCode,m_gridLevel,&Yk,true))
continue;
for (unsigned k=0; k<Yk.size(); ++k)
{
unsigned index = Yk.getPointGlobalIndex(k);
assert(flags.getValue(index) == 1);
//flags.setValue(index,1);
//++pointCount;
}
m_theGrid[m_activeCells[i]] = 0;
delete aCell;
}
return pointCount;
}
int ccFastMarchingForNormsDirection::init( ccGenericPointCloud* cloud,
NormsIndexesTableType* theNorms,
ccOctree* theOctree,
unsigned char level)
{
int result = initGridWithOctree(theOctree, level);
if (result < 0)
return result;
//fill the grid with the octree
CCLib::DgmOctree::cellCodesContainer cellCodes;
theOctree->getCellCodes(level,cellCodes,true);
CCLib::ReferenceCloud Yk(theOctree->associatedCloud());
while (!cellCodes.empty())
{
if (!theOctree->getPointsInCell(cellCodes.back(),level,&Yk,true))
{
//not enough memory
return -1;
}
//convert the octree cell code to grid position
Tuple3i cellPos;
theOctree->getCellPos(cellCodes.back(),level,cellPos,true);
//convert it to FM cell pos index
unsigned gridPos = pos2index(cellPos);
//create corresponding cell
DirectionCell* aCell = new DirectionCell;
{
//aCell->signConfidence = 1;
aCell->cellCode = cellCodes.back();
aCell->N = ComputeRobustAverageNorm(&Yk,cloud);
aCell->C = *CCLib::Neighbourhood(&Yk).getGravityCenter();
}
m_theGrid[gridPos] = aCell;
cellCodes.pop_back();
}
m_initialized = true;
return 0;
}
unsigned ccFastMarchingForNormsDirection::updateResolvedTable( ccGenericPointCloud* cloud,
GenericChunkedArray<1,unsigned char> &resolved,
NormsIndexesTableType* theNorms)
{
if (!m_initialized || !m_octree || m_gridLevel > CCLib::DgmOctree::MAX_OCTREE_LEVEL)
return 0;
CCLib::ReferenceCloud Yk(m_octree->associatedCloud());
unsigned count = 0;
for (size_t i=0; i<m_activeCells.size(); ++i)
{
DirectionCell* aCell = static_cast<DirectionCell*>(m_theGrid[m_activeCells[i]]);
if (!m_octree->getPointsInCell(aCell->cellCode,m_gridLevel,&Yk,true))
{
//not enough memory
return 0;
}
for (unsigned k=0; k<Yk.size(); ++k)
{
unsigned index = Yk.getPointGlobalIndex(k);
resolved.setValue(index,1);
const CompressedNormType& norm = theNorms->getValue(index);
const CCVector3& N = ccNormalVectors::GetNormal(norm);
//inverse point normal if necessary
if (N.dot(aCell->N) < 0)
{
theNorms->setValue(index,ccNormalVectors::GetNormIndex(-N));
}
#ifdef QT_DEBUG
cloud->setPointScalarValue(index,aCell->T);
//cloud->setPointScalarValue(index,aCell->signConfidence);
//cloud->setPointScalarValue(index,aCell->scalar);
#endif
++count;
}
}
return count;
}
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*/;
}
int R() {
int SG;
int tu;
int b;
int cbnn;
int fJA;
int Ywh;
int Sd3i;
int U;
int FTe;
;
if (AgC9 > + (Y0V(UnE, 791438648 + cK(517618502, + N_ != Ed() != - (Yk(((883538282)), nBa)), sdDM(kC, (1604897559), ! ! 528836364 / (1815076270) / 1477268768), n), psl, - 126855483))) while (om(AZ)) {
int xF;
int yd;
int xEmV;
int vc;
int m9;
while (2044928990) while (- 644557172 + 1573824677 + 1727264726 < TvAP(1980609521, Jy47, C, 953854953, - WyD(RlhU(LY0), + ipA(G, (Vg((1882502790), (1790689388) == 711715734 / (_) != + ! (134548752)))), nk1(((2078180300)) - + - 1230367323 - gD13, pK38 / 1377998756, m4G(1127784867) > u, (QSs()), 1389625403), 929895424, 895835811 < dW) * - 1339857856) / 2045295101) while (pLb) while (+ 1321345616) return ! 708468611;
(J);
return eUY;
continue;
(bRvz = 1838176344);
r(F + ! 196992386, Gg);
if (1419330106) ! (- (uG)) <= ! 996971325;
else T((- ! 2092297653 <= AA >= ! 1494541558 == o((! + v7nD))), 1818433226);
}
else break;
;
;
if (! (! cQ63(1853496451, i(DW9, 988295904 > _Gv, Wlbr, b) / ! ((O(xl))) - - O(+ a_O > 318640956, 1005354893, 167517361) / If, GDD, u, B) * B) + 443946612 + (dN(BFD3(mg <= 1303591176 + hFH2, x) % hch > 1053610009 / RaZ0(ZC7O((720576768)), 124325761, K(! C, 1901412912 * 1202992483, + (+ + e * 389490648), + 1244843051 < (sm = dH), W9(1583501857)), - ! sIs, 1297610228)))) lYAi(1443174874, U8R, + 925787186);
else continue;
continue;
if (+ ! 329152016 + K2()) continue;
else ;
continue;
continue;
}
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;
}
void qHPR::doAction()
{
assert(m_app);
if (!m_app)
return;
const ccHObject::Container& selectedEntities = m_app->getSelectedEntities();
size_t selNum = selectedEntities.size();
if ( selNum != 1
|| !selectedEntities.front()->isA(CC_TYPES::POINT_CLOUD))
{
m_app->dispToConsole("Select only one cloud!",ccMainAppInterface::ERR_CONSOLE_MESSAGE);
return;
}
ccPointCloud* cloud = static_cast<ccPointCloud*>(selectedEntities[0]);
ccGLWindow* win = m_app->getActiveGLWindow();
if (!win)
{
m_app->dispToConsole("No active window!",ccMainAppInterface::ERR_CONSOLE_MESSAGE);
return;
}
//display parameters
const ccViewportParameters& params = win->getViewportParameters();
if (!params.perspectiveView)
{
m_app->dispToConsole("Perspective mode only!",ccMainAppInterface::ERR_CONSOLE_MESSAGE);
return;
}
ccHprDlg dlg(m_app->getMainWindow());
if (!dlg.exec())
return;
//progress dialog
ccProgressDialog progressCb(false,m_app->getMainWindow());
//unique parameter: the octree subdivision level
int octreeLevel = dlg.octreeLevelSpinBox->value();
assert(octreeLevel >= 0 && octreeLevel <= CCLib::DgmOctree::MAX_OCTREE_LEVEL);
//compute octree if cloud hasn't any
ccOctree::Shared theOctree = cloud->getOctree();
if (!theOctree)
{
theOctree = cloud->computeOctree(&progressCb);
if (theOctree && cloud->getParent())
{
m_app->addToDB(cloud->getOctreeProxy());
}
}
if (!theOctree)
{
m_app->dispToConsole("Couldn't compute octree!",ccMainAppInterface::ERR_CONSOLE_MESSAGE);
return;
}
CCVector3d viewPoint = params.cameraCenter;
if (params.objectCenteredView)
{
CCVector3d PC = params.cameraCenter - params.pivotPoint;
params.viewMat.inverse().apply(PC);
viewPoint = params.pivotPoint + PC;
}
//HPR
CCLib::ReferenceCloud* visibleCells = 0;
{
QElapsedTimer eTimer;
eTimer.start();
CCLib::ReferenceCloud* theCellCenters = CCLib::CloudSamplingTools::subsampleCloudWithOctreeAtLevel( cloud,
static_cast<unsigned char>(octreeLevel),
CCLib::CloudSamplingTools::NEAREST_POINT_TO_CELL_CENTER,
&progressCb,
theOctree.data());
if (!theCellCenters)
{
m_app->dispToConsole("Error while simplifying point cloud with octree!",ccMainAppInterface::ERR_CONSOLE_MESSAGE);
return;
}
visibleCells = removeHiddenPoints(theCellCenters,viewPoint,3.5);
m_app->dispToConsole(QString("[HPR] Cells: %1 - Time: %2 s").arg(theCellCenters->size()).arg(eTimer.elapsed()/1.0e3));
//warning: after this, visibleCells can't be used anymore as a
//normal cloud (as it's 'associated cloud' has been deleted).
//Only its indexes are valid! (they are corresponding to octree cells)
delete theCellCenters;
theCellCenters = 0;
}
if (visibleCells)
{
//DGM: we generate a new cloud now, instead of playing with the points visiblity! (too confusing for the user)
/*if (!cloud->isVisibilityTableInstantiated() && !cloud->resetVisibilityArray())
{
m_app->dispToConsole("Visibility array allocation failed! (Not enough memory?)",ccMainAppInterface::ERR_CONSOLE_MESSAGE);
return;
}
ccPointCloud::VisibilityTableType* pointsVisibility = cloud->getTheVisibilityArray();
assert(pointsVisibility);
pointsVisibility->fill(POINT_HIDDEN);
//*/
CCLib::ReferenceCloud visiblePoints(theOctree->associatedCloud());
unsigned visiblePointCount = 0;
unsigned visibleCellsCount = visibleCells->size();
CCLib::DgmOctree::cellIndexesContainer cellIndexes;
if (!theOctree->getCellIndexes(static_cast<unsigned char>(octreeLevel), cellIndexes))
{
m_app->dispToConsole("Couldn't fetch the list of octree cell indexes! (Not enough memory?)",ccMainAppInterface::ERR_CONSOLE_MESSAGE);
delete visibleCells;
return;
}
for (unsigned i=0; i<visibleCellsCount; ++i)
{
//cell index
unsigned index = visibleCells->getPointGlobalIndex(i);
//points in this cell...
CCLib::ReferenceCloud Yk(theOctree->associatedCloud());
theOctree->getPointsInCellByCellIndex(&Yk,cellIndexes[index],static_cast<unsigned char>(octreeLevel));
//...are all visible
/*unsigned count = Yk.size();
for (unsigned j=0;j<count;++j)
pointsVisibility->setValue(Yk.getPointGlobalIndex(j),POINT_VISIBLE);
visiblePointCount += count;
//*/
if (!visiblePoints.add(Yk))
{
m_app->dispToConsole("Not enough memory!",ccMainAppInterface::ERR_CONSOLE_MESSAGE);
delete visibleCells;
return;
}
}
delete visibleCells;
visibleCells = 0;
m_app->dispToConsole(QString("[HPR] Visible points: %1").arg(visiblePointCount));
if (visiblePoints.size() == cloud->size())
{
m_app->dispToConsole("No points were removed!",ccMainAppInterface::ERR_CONSOLE_MESSAGE);
}
else
{
//create cloud from visibility selection
ccPointCloud* newCloud = cloud->partialClone(&visiblePoints);
if (newCloud)
{
newCloud->setDisplay(newCloud->getDisplay());
newCloud->setVisible(true);
newCloud->setName(cloud->getName()+QString(".visible_points"));
cloud->setEnabled(false);
//add associated viewport object
cc2DViewportObject* viewportObject = new cc2DViewportObject(QString("Viewport"));
viewportObject->setParameters(params);
newCloud->addChild(viewportObject);
m_app->addToDB(newCloud);
newCloud->redrawDisplay();
}
else
{
m_app->dispToConsole("Not enough memory!",ccMainAppInterface::ERR_CONSOLE_MESSAGE);
}
}
}
//currently selected entities appearance may have changed!
m_app->refreshAll();
}
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;
}
ccPolyline* ccContourExtractor::ExtractFlatContour( CCLib::GenericIndexedCloudPersist* points,
bool allowMultiPass,
PointCoordinateType maxEdgeLength/*=0*/,
const PointCoordinateType* preferredNormDim/*=0*/,
const PointCoordinateType* preferredUpDir/*=0*/,
ContourType contourType/*=FULL*/,
std::vector<unsigned>* originalPointIndexes/*=0*/,
bool enableVisualDebugMode/*=false*/,
double maxAngleDeg/*=0.0*/)
{
assert(points);
if (!points)
return 0;
unsigned ptsCount = points->size();
if (ptsCount < 3)
return 0;
CCLib::Neighbourhood Yk(points);
CCVector3 O,X,Y; //local base
bool useOXYasBase = false;
//we project the input points on a plane
std::vector<Vertex2D> points2D;
PointCoordinateType* planeEq = 0;
//if the user has specified a default direction, we'll use it as 'projecting plane'
PointCoordinateType preferredPlaneEq[4] = {0, 0, 0, 0};
if (preferredNormDim != 0)
{
const CCVector3* G = points->getPoint(0); //any point through which the point passes is ok
preferredPlaneEq[0] = preferredNormDim[0];
preferredPlaneEq[1] = preferredNormDim[1];
preferredPlaneEq[2] = preferredNormDim[2];
CCVector3::vnormalize(preferredPlaneEq);
preferredPlaneEq[3] = CCVector3::vdot(G->u, preferredPlaneEq);
planeEq = preferredPlaneEq;
if (preferredUpDir != 0)
{
O = *G;
Y = CCVector3(preferredUpDir);
X = Y.cross(CCVector3(preferredNormDim));
useOXYasBase = true;
}
}
if (!Yk.projectPointsOn2DPlane<Vertex2D>(points2D, planeEq, &O, &X, &Y, useOXYasBase))
{
ccLog::Warning("[ExtractFlatContour] Failed to project the points on the LS plane (not enough memory?)!");
return 0;
}
//update the points indexes (not done by Neighbourhood::projectPointsOn2DPlane)
{
for (unsigned i = 0; i < ptsCount; ++i)
points2D[i].index = i;
}
//try to get the points on the convex/concave hull to build the contour and the polygon
Hull2D hullPoints;
if (!ExtractConcaveHull2D( points2D,
hullPoints,
contourType,
allowMultiPass,
maxEdgeLength*maxEdgeLength,
enableVisualDebugMode,
maxAngleDeg))
{
ccLog::Warning("[ExtractFlatContour] Failed to compute the convex hull of the input points!");
return 0;
}
if (originalPointIndexes)
{
try
{
originalPointIndexes->resize(hullPoints.size(), 0);
}
catch (const std::bad_alloc&)
{
//not enough memory
ccLog::Error("[ExtractFlatContour] Not enough memory!");
return 0;
}
unsigned i=0;
for (Hull2D::const_iterator it = hullPoints.begin(); it != hullPoints.end(); ++it, ++i)
(*originalPointIndexes)[i] = (*it)->index;
}
unsigned hullPtsCount = static_cast<unsigned>(hullPoints.size());
//create vertices
ccPointCloud* contourVertices = new ccPointCloud();
{
if (!contourVertices->reserve(hullPtsCount))
{
delete contourVertices;
contourVertices = 0;
ccLog::Error("[ExtractFlatContour] Not enough memory!");
return 0;
}
//projection on the LS plane (in 3D)
for (Hull2D::const_iterator it = hullPoints.begin(); it != hullPoints.end(); ++it)
contourVertices->addPoint(O + X*(*it)->x + Y*(*it)->y);
contourVertices->setName("vertices");
contourVertices->setEnabled(false);
}
//we create the corresponding (3D) polyline
ccPolyline* contourPolyline = new ccPolyline(contourVertices);
if (contourPolyline->reserve(hullPtsCount))
{
contourPolyline->addPointIndex(0, hullPtsCount);
contourPolyline->setClosed(contourType == FULL);
contourPolyline->setVisible(true);
contourPolyline->setName("contour");
contourPolyline->addChild(contourVertices);
}
else
{
delete contourPolyline;
contourPolyline = 0;
ccLog::Warning("[ExtractFlatContour] Not enough memory to create the contour polyline!");
}
return contourPolyline;
}
ccPolyline* ccPolyline::ExtractFlatContour( CCLib::GenericIndexedCloudPersist* points,
PointCoordinateType maxEdgelLength/*=0*/,
const PointCoordinateType* preferredNormDim/*=0*/,
const PointCoordinateType* preferredUpDir/*=0*/,
ContourType contourType/*=FULL*/,
std::vector<unsigned>* originalPointIndexes/*=0*/)
{
assert(points);
if (!points)
return 0;
unsigned ptsCount = points->size();
if (ptsCount < 3)
return 0;
CCLib::Neighbourhood Yk(points);
CCVector3 O,X,Y; //local base
bool useOXYasBase = false;
//we project the input points on a plane
std::vector<CCLib::PointProjectionTools::IndexedCCVector2> points2D;
PointCoordinateType* planeEq = 0;
//if the user has specified a default direction, we'll use it as 'projecting plane'
PointCoordinateType preferredPlaneEq[4] = {0, 0, 0, 0};
if (preferredNormDim != 0)
{
const CCVector3* G = points->getPoint(0); //any point through which the point passes is ok
preferredPlaneEq[0] = preferredNormDim[0];
preferredPlaneEq[1] = preferredNormDim[1];
preferredPlaneEq[2] = preferredNormDim[2];
CCVector3::vnormalize(preferredPlaneEq);
preferredPlaneEq[3] = CCVector3::vdot(G->u,preferredPlaneEq);
planeEq = preferredPlaneEq;
if (preferredUpDir != 0)
{
O = *G;
Y = CCVector3(preferredUpDir);
X = Y.cross(CCVector3(preferredNormDim));
useOXYasBase = true;
}
}
if (!Yk.projectPointsOn2DPlane<CCLib::PointProjectionTools::IndexedCCVector2>(points2D,planeEq,&O,&X,&Y,useOXYasBase))
{
ccLog::Warning("[ccPolyline::ExtractFlatContour] Failed to project the points on the LS plane (not enough memory?)!");
return 0;
}
//update the points indexes (not done by Neighbourhood::projectPointsOn2DPlane)
{
for (unsigned i=0; i<ptsCount; ++i)
points2D[i].index = i;
}
//try to get the points on the convex/concave hull to build the contour and the polygon
Hull2D hullPoints;
if (!CCLib::PointProjectionTools::extractConcaveHull2D( points2D,
hullPoints,
maxEdgelLength*maxEdgelLength) )
{
ccLog::Warning("[ccPolyline::ExtractFlatContour] Failed to compute the convex hull of the input points!");
return 0;
}
bool isClosed = true;
if (contourType != FULL)
{
//look for the min and max 'X' coordinates (preferredNormDim and preferredUpDir should have been defined ;)
PointCoordinateType xMin = 0, xMax = 0;
{
unsigned i=0;
for (Hull2D::const_iterator it = hullPoints.begin(); it != hullPoints.end(); ++it, ++i)
{
const CCLib::PointProjectionTools::IndexedCCVector2* P = *it;
if (i != 0)
{
if (P->x < xMin)
xMin = P->x;
else if (P->x > xMax)
xMax = P->x;
}
else
{
xMin = xMax = P->x;
}
}
}
if (xMin != xMax)
{
//now identify the 'min' vertex
Hull2D::const_iterator firstIt = hullPoints.end();
{
for (Hull2D::const_iterator it = hullPoints.begin(); it != hullPoints.end(); ++it)
{
const CCLib::PointProjectionTools::IndexedCCVector2* P = *it;
if (P->x == xMin)
{
if ( firstIt == hullPoints.end()
//we take the lowest (resp highest) if multiple points with x == xMin
|| (contourType == LOWER && (*firstIt)->y > P->y)
|| (contourType == UPPER && (*firstIt)->y < P->y) )
{
firstIt = it;
}
}
}
}
assert(firstIt != hullPoints.end());
//now we are going to keep only the right part
try
{
//determine the right way
Hull2D::const_iterator prevIt = (firstIt != hullPoints.begin() ? firstIt : hullPoints.end()); --prevIt;
Hull2D::const_iterator nextIt = firstIt; ++nextIt; if (nextIt == hullPoints.end()) nextIt = hullPoints.begin();
bool forward = ( (contourType == LOWER && (*nextIt)->y < (*prevIt)->y)
|| (contourType == UPPER && (*nextIt)->y > (*prevIt)->y) );
if (!forward)
std::swap(prevIt,nextIt);
Hull2D hullPart;
hullPart.push_back(*firstIt);
nextIt = firstIt;
while ((*nextIt)->x != xMax)
{
if (forward)
{
++nextIt;
if (nextIt == hullPoints.end())
nextIt = hullPoints.begin();
}
else
{
if (nextIt == hullPoints.begin())
nextIt = hullPoints.end();
--nextIt;
}
hullPart.push_back(*nextIt);
}
hullPoints = hullPart;
isClosed = false;
}
catch(std::bad_alloc)
{
ccLog::Error("[ccPolyline::ExtractFlatContour] Not enough memory!");
return 0;
}
}
else //xMin == xMax
{
//flat contour?!
}
}
if (originalPointIndexes)
{
try
{
originalPointIndexes->resize(hullPoints.size(),0);
}
catch(std::bad_alloc)
{
//not enough memory
ccLog::Error("[ccPolyline::ExtractFlatContour] Not enough memory!");
return 0;
}
unsigned i=0;
for (Hull2D::const_iterator it = hullPoints.begin(); it != hullPoints.end(); ++it, ++i)
(*originalPointIndexes)[i] = (*it)->index;
}
unsigned hullPtsCount = static_cast<unsigned>(hullPoints.size());
//create vertices
ccPointCloud* contourVertices = new ccPointCloud();
{
if (!contourVertices->reserve(hullPtsCount))
{
delete contourVertices;
contourVertices = 0;
ccLog::Error("[ccPolyline::ExtractFlatContour] Not enough memory!");
return 0;
}
//projection on the LS plane (in 3D)
for (Hull2D::const_iterator it = hullPoints.begin(); it != hullPoints.end(); ++it)
contourVertices->addPoint(O + X*(*it)->x + Y*(*it)->y);
contourVertices->setName("vertices");
contourVertices->setEnabled(false);
}
//we create the corresponding (3D) polyline
ccPolyline* contourPolyline = new ccPolyline(contourVertices);
if (contourPolyline->reserve(hullPtsCount))
{
contourPolyline->addPointIndex(0,hullPtsCount);
contourPolyline->setClosed(isClosed);
contourPolyline->setVisible(true);
contourPolyline->setName("contour");
contourPolyline->addChild(contourVertices);
}
else
{
delete contourPolyline;
contourPolyline = 0;
ccLog::Warning("[ccPolyline::ExtractFlatContour] Not enough memory to create the contour polyline!");
}
return contourPolyline;
}
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;
}
bool ccFacet::createInternalRepresentation( CCLib::GenericIndexedCloudPersist* points,
const PointCoordinateType* planeEquation/*=0*/)
{
assert(points);
if (!points)
return false;
unsigned ptsCount = points->size();
if (ptsCount < 3)
return false;
CCLib::Neighbourhood Yk(points);
//get corresponding plane
if (!planeEquation)
{
planeEquation = Yk.getLSPlane();
if (!planeEquation)
{
ccLog::Warning("[ccFacet::createInternalRepresentation] Failed to compute the LS plane passing through the input points!");
return false;
}
}
memcpy(m_planeEquation, planeEquation, sizeof(PointCoordinateType) * 4);
//we project the input points on a plane
std::vector<CCLib::PointProjectionTools::IndexedCCVector2> points2D;
CCVector3 X, Y; //local base
if (!Yk.projectPointsOn2DPlane<CCLib::PointProjectionTools::IndexedCCVector2>(points2D, nullptr, &m_center, &X, &Y))
{
ccLog::Error("[ccFacet::createInternalRepresentation] Not enough memory!");
return false;
}
//compute resulting RMS
m_rms = CCLib::DistanceComputationTools::computeCloud2PlaneDistanceRMS(points, m_planeEquation);
//update the points indexes (not done by Neighbourhood::projectPointsOn2DPlane)
{
for (unsigned i = 0; i < ptsCount; ++i)
{
points2D[i].index = i;
}
}
//try to get the points on the convex/concave hull to build the contour and the polygon
{
std::list<CCLib::PointProjectionTools::IndexedCCVector2*> hullPoints;
if (!CCLib::PointProjectionTools::extractConcaveHull2D( points2D,
hullPoints,
m_maxEdgeLength*m_maxEdgeLength))
{
ccLog::Error("[ccFacet::createInternalRepresentation] Failed to compute the convex hull of the input points!");
}
unsigned hullPtsCount = static_cast<unsigned>(hullPoints.size());
//create vertices
m_contourVertices = new ccPointCloud();
{
if (!m_contourVertices->reserve(hullPtsCount))
{
delete m_contourVertices;
m_contourVertices = nullptr;
ccLog::Error("[ccFacet::createInternalRepresentation] Not enough memory!");
return false;
}
//projection on the LS plane (in 3D)
for (std::list<CCLib::PointProjectionTools::IndexedCCVector2*>::const_iterator it = hullPoints.begin(); it != hullPoints.end(); ++it)
{
m_contourVertices->addPoint(m_center + X*(*it)->x + Y*(*it)->y);
}
m_contourVertices->setName(DEFAULT_CONTOUR_POINTS_NAME);
m_contourVertices->setLocked(true);
m_contourVertices->setEnabled(false);
addChild(m_contourVertices);
}
//we create the corresponding (3D) polyline
{
m_contourPolyline = new ccPolyline(m_contourVertices);
if (m_contourPolyline->reserve(hullPtsCount))
{
m_contourPolyline->addPointIndex(0, hullPtsCount);
m_contourPolyline->setClosed(true);
m_contourPolyline->setVisible(true);
m_contourPolyline->setLocked(true);
m_contourPolyline->setName(DEFAULT_CONTOUR_NAME);
m_contourVertices->addChild(m_contourPolyline);
m_contourVertices->setEnabled(true);
m_contourVertices->setVisible(false);
}
else
{
delete m_contourPolyline;
m_contourPolyline = nullptr;
ccLog::Warning("[ccFacet::createInternalRepresentation] Not enough memory to create the contour polyline!");
}
}
//we create the corresponding (2D) mesh
std::vector<CCVector2> hullPointsVector;
try
{
hullPointsVector.reserve(hullPoints.size());
for (std::list<CCLib::PointProjectionTools::IndexedCCVector2*>::const_iterator it = hullPoints.begin(); it != hullPoints.end(); ++it)
{
hullPointsVector.push_back(**it);
}
}
catch (...)
{
ccLog::Warning("[ccFacet::createInternalRepresentation] Not enough memory to create the contour mesh!");
}
//if we have computed a concave hull, we must remove triangles falling outside!
bool removePointsOutsideHull = (m_maxEdgeLength > 0);
if (!hullPointsVector.empty() && CCLib::Delaunay2dMesh::Available())
{
//compute the facet surface
CCLib::Delaunay2dMesh dm;
char errorStr[1024];
if (dm.buildMesh(hullPointsVector, 0, errorStr))
{
if (removePointsOutsideHull)
dm.removeOuterTriangles(hullPointsVector, hullPointsVector);
unsigned triCount = dm.size();
assert(triCount != 0);
m_polygonMesh = new ccMesh(m_contourVertices);
if (m_polygonMesh->reserve(triCount))
{
//import faces
for (unsigned i = 0; i < triCount; ++i)
{
const CCLib::VerticesIndexes* tsi = dm.getTriangleVertIndexes(i);
m_polygonMesh->addTriangle(tsi->i1, tsi->i2, tsi->i3);
}
m_polygonMesh->setVisible(true);
m_polygonMesh->enableStippling(true);
//unique normal for facets
if (m_polygonMesh->reservePerTriangleNormalIndexes())
{
NormsIndexesTableType* normsTable = new NormsIndexesTableType();
normsTable->reserve(1);
CCVector3 N(m_planeEquation);
normsTable->addElement(ccNormalVectors::GetNormIndex(N.u));
m_polygonMesh->setTriNormsTable(normsTable);
for (unsigned i = 0; i < triCount; ++i)
m_polygonMesh->addTriangleNormalIndexes(0, 0, 0); //all triangles will have the same normal!
m_polygonMesh->showNormals(true);
m_polygonMesh->setLocked(true);
m_polygonMesh->setName(DEFAULT_POLYGON_MESH_NAME);
m_contourVertices->addChild(m_polygonMesh);
m_contourVertices->setEnabled(true);
m_contourVertices->setVisible(false);
}
else
{
ccLog::Warning("[ccFacet::createInternalRepresentation] Not enough memory to create the polygon mesh's normals!");
}
//update facet surface
m_surface = CCLib::MeshSamplingTools::computeMeshArea(m_polygonMesh);
}
else
{
delete m_polygonMesh;
m_polygonMesh = nullptr;
ccLog::Warning("[ccFacet::createInternalRepresentation] Not enough memory to create the polygon mesh!");
}
}
else
{
ccLog::Warning(QString("[ccFacet::createInternalRepresentation] Failed to create the polygon mesh (third party lib. said '%1'").arg(errorStr));
}
}
}
return true;
}
int FastMarchingForFacetExtraction::init( ccGenericPointCloud* cloud,
CCLib::DgmOctree* theOctree,
unsigned char level,
ScalarType maxError,
CCLib::DistanceComputationTools::ERROR_MEASURES errorMeasure,
bool useRetroProjectionError,
CCLib::GenericProgressCallback* progressCb/*=0*/)
{
m_maxError = maxError;
m_errorMeasure = errorMeasure;
m_useRetroProjectionError = useRetroProjectionError;
if (progressCb)
{
if (progressCb->textCanBeEdited())
{
progressCb->setMethodTitle("Fast Marching grid initialization");
progressCb->setInfo(qPrintable(QString("Level: %1").arg(level)));
}
progressCb->update(0);
progressCb->start();
}
int result = initGridWithOctree(theOctree, level);
if (result < 0)
return result;
//fill the grid with the octree
CCLib::DgmOctree::cellCodesContainer cellCodes;
theOctree->getCellCodes(level,cellCodes,true);
size_t cellCount = cellCodes.size();
CCLib::NormalizedProgress nProgress(progressCb, static_cast<unsigned>(cellCount));
if (progressCb)
{
progressCb->setInfo(qPrintable(QString("Level: %1\nCells: %2").arg(level).arg(cellCount)));
}
CCLib::ReferenceCloud Yk(theOctree->associatedCloud());
while (!cellCodes.empty())
{
if (theOctree->getPointsInCell(cellCodes.back(),level,&Yk,true))
{
//convert the octree cell code to grid position
Tuple3i cellPos;
theOctree->getCellPos(cellCodes.back(),level,cellPos,true);
CCVector3 N,C;
ScalarType error;
if (ComputeCellStats(&Yk,N,C,error,m_errorMeasure))
{
//convert octree cell pos to FM cell pos index
unsigned gridPos = pos2index(cellPos);
//create corresponding cell
PlanarCell* aCell = new PlanarCell;
aCell->cellCode = cellCodes.back();
aCell->N = N;
aCell->C = C;
aCell->planarError = error;
m_theGrid[gridPos] = aCell;
}
else
{
//an error occurred?!
return -10;
}
}
cellCodes.pop_back();
if (progressCb && !nProgress.oneStep())
{
//process cancelled by user
progressCb->stop();
return -1;
}
}
if (progressCb)
{
progressCb->stop();
}
m_initialized = true;
return 0;
}
