ReferenceCloud* CloudSamplingTools::subsampleCloudRandomly(GenericIndexedCloudPersist* inputCloud, unsigned newNumberOfPoints, GenericProgressCallback* progressCb/*=0*/)
{
assert(inputCloud);
unsigned theCloudSize = inputCloud->size();
//we put all input points in a ReferenceCloud
ReferenceCloud* newCloud = new ReferenceCloud(inputCloud);
if (!newCloud->addPointIndex(0,theCloudSize))
{
delete newCloud;
return 0;
}
//we have less points than requested?!
if (theCloudSize <= newNumberOfPoints)
{
return newCloud;
}
unsigned pointsToRemove = theCloudSize - newNumberOfPoints;
std::random_device rd; // non-deterministic generator
std::mt19937 gen(rd()); // to seed mersenne twister.
NormalizedProgress* normProgress = 0;
if (progressCb)
{
progressCb->setInfo("Random subsampling");
normProgress = new NormalizedProgress(progressCb, pointsToRemove);
progressCb->reset();
progressCb->start();
}
//we randomly remove "inputCloud.size() - newNumberOfPoints" points (much simpler)
unsigned lastPointIndex = theCloudSize-1;
for (unsigned i=0; i<pointsToRemove; ++i)
{
std::uniform_int_distribution<unsigned> dist(0, lastPointIndex);
unsigned index = dist(gen);
newCloud->swap(index,lastPointIndex);
--lastPointIndex;
if (normProgress && !normProgress->oneStep())
{
//cancel process
delete normProgress;
delete newCloud;
return 0;
}
}
newCloud->resize(newNumberOfPoints); //always smaller, so it should be ok!
if (normProgress)
delete normProgress;
return newCloud;
}
SimpleCloud* PointProjectionTools::applyTransformation(GenericCloud* theCloud, Transformation& trans, GenericProgressCallback* progressCb)
{
assert(theCloud);
unsigned count = theCloud->size();
SimpleCloud* transformedCloud = new SimpleCloud();
if (!transformedCloud->reserve(count))
return 0; //not enough memory
NormalizedProgress* nprogress = 0;
if (progressCb)
{
progressCb->reset();
progressCb->setMethodTitle("ApplyTransformation");
nprogress = new NormalizedProgress(progressCb,count);
char buffer[256];
sprintf(buffer,"Number of points = %u",count);
progressCb->setInfo(buffer);
progressCb->start();
}
theCloud->placeIteratorAtBegining();
const CCVector3* P;
if (trans.R.isValid())
{
while ((P = theCloud->getNextPoint()))
{
//P' = s*R.P+T
CCVector3 newP = trans.s * (trans.R * (*P)) + trans.T;
transformedCloud->addPoint(newP);
if (nprogress && !nprogress->oneStep())
break;
}
}
else
{
while ((P = theCloud->getNextPoint()))
{
//P' = s*P+T
CCVector3 newP = trans.s * (*P) + trans.T;
transformedCloud->addPoint(newP);
if (nprogress && !nprogress->oneStep())
break;
}
}
if (progressCb)
progressCb->stop();
return transformedCloud;
}
GenericIndexedMesh* ManualSegmentationTools::segmentMesh(GenericIndexedMesh* theMesh, ReferenceCloud* pointIndexes, bool pointsWillBeInside, GenericProgressCallback* progressCb, GenericIndexedCloud* destCloud, unsigned indexShift)
{
if (!theMesh || !pointIndexes || !pointIndexes->getAssociatedCloud())
return 0;
//by default we try a fast process (but with a higher memory consumption)
unsigned numberOfPoints = pointIndexes->getAssociatedCloud()->size();
unsigned numberOfIndexes = pointIndexes->size();
//we determine for each point if it is used in the output mesh or not
//(and we compute its new index by the way: 0 means that the point is not used, otherwise its index will be newPointIndexes-1)
std::vector<unsigned> newPointIndexes;
{
try
{
newPointIndexes.resize(numberOfPoints,0);
}
catch (const std::bad_alloc&)
{
return 0; //not enough memory
}
for (unsigned i=0; i<numberOfIndexes; ++i)
{
assert(pointIndexes->getPointGlobalIndex(i) < numberOfPoints);
newPointIndexes[pointIndexes->getPointGlobalIndex(i)] = i+1;
}
}
//negative array for the case where input points are "outside"
if (!pointsWillBeInside)
{
unsigned newIndex = 0;
for (unsigned i=0;i<numberOfPoints;++i)
newPointIndexes[i] = (newPointIndexes[i] == 0 ? ++newIndex : 0);
}
//create resulting mesh
SimpleMesh* newMesh = 0;
{
unsigned numberOfTriangles = theMesh->size();
//progress notification
NormalizedProgress* nprogress = 0;
if (progressCb)
{
progressCb->reset();
progressCb->setMethodTitle("Extract mesh");
char buffer[256];
sprintf(buffer,"New vertex number: %u",numberOfIndexes);
nprogress = new NormalizedProgress(progressCb,numberOfTriangles);
progressCb->setInfo(buffer);
progressCb->start();
}
newMesh = new SimpleMesh(destCloud ? destCloud : pointIndexes->getAssociatedCloud());
unsigned count = 0;
theMesh->placeIteratorAtBegining();
for (unsigned i=0; i<numberOfTriangles; ++i)
{
bool triangleIsOnTheRightSide = true;
const VerticesIndexes* tsi = theMesh->getNextTriangleVertIndexes(); //DGM: getNextTriangleVertIndexes is faster for mesh groups!
int newVertexIndexes[3];
//VERSION: WE KEEP THE TRIANGLE ONLY IF ITS 3 VERTICES ARE INSIDE
for (uchar j=0;j <3; ++j)
{
const unsigned& currentVertexFlag = newPointIndexes[tsi->i[j]];
//if the vertex is rejected, we discard this triangle
if (currentVertexFlag == 0)
{
triangleIsOnTheRightSide = false;
break;
}
newVertexIndexes[j] = currentVertexFlag-1;
}
//if we keep the triangle
if (triangleIsOnTheRightSide)
{
if (count == newMesh->size() && !newMesh->reserve(newMesh->size() + 1000)) //auto expand mesh size
{
//stop process
delete newMesh;
newMesh = 0;
break;
}
++count;
newMesh->addTriangle( indexShift + newVertexIndexes[0],
indexShift + newVertexIndexes[1],
indexShift + newVertexIndexes[2] );
}
if (nprogress && !nprogress->oneStep())
{
//cancel process
break;
}
}
if (nprogress)
{
delete nprogress;
nprogress = 0;
}
if (newMesh)
{
if (newMesh->size() == 0)
{
delete newMesh;
newMesh = 0;
}
else if (count < newMesh->size())
{
newMesh->resize(count); //should always be ok as count<maxNumberOfTriangles
}
}
}
return newMesh;
}
SimpleCloud* PointProjectionTools::developCloudOnCylinder(GenericCloud* theCloud,
PointCoordinateType radius,
unsigned char dim,
CCVector3* center,
GenericProgressCallback* progressCb)
{
if (!theCloud)
return 0;
uchar dim1 = (dim>0 ? dim-1 : 2);
uchar dim2 = (dim<2 ? dim+1 : 0);
unsigned count = theCloud->size();
SimpleCloud* newList = new SimpleCloud();
if (!newList->reserve(count)) //not enough memory
return 0;
//we compute cloud bounding box center if no center is specified
CCVector3 C;
if (!center)
{
PointCoordinateType Mins[3],Maxs[3];
theCloud->getBoundingBox(Mins,Maxs);
C = (CCVector3(Mins)+CCVector3(Maxs))*0.5;
center = &C;
}
NormalizedProgress* nprogress = 0;
if (progressCb)
{
progressCb->reset();
progressCb->setMethodTitle("Develop");
char buffer[256];
sprintf(buffer,"Number of points = %u",count);
nprogress = new NormalizedProgress(progressCb,count);
progressCb->setInfo(buffer);
progressCb->start();
}
const CCVector3 *Q;
CCVector3 P;
PointCoordinateType u,lon;
theCloud->placeIteratorAtBegining();
while ((Q = theCloud->getNextPoint()))
{
P = *Q-*center;
u = sqrt(P.u[dim1] * P.u[dim1] + P.u[dim2] * P.u[dim2]);
lon = atan2(P.u[dim1],P.u[dim2]);
newList->addPoint(CCVector3(lon*radius,P.u[dim],u-radius));
if (nprogress)
{
if (!nprogress->oneStep())
break;
}
}
if (progressCb)
{
delete nprogress;
progressCb->stop();
}
return newList;
}
//deroule la liste sur un cone dont le centre est "center" et d'angle alpha en degres
SimpleCloud* PointProjectionTools::developCloudOnCone(GenericCloud* theCloud, uchar dim, PointCoordinateType baseRadius, float alpha, const CCVector3& center, GenericProgressCallback* progressCb)
{
if (!theCloud)
return 0;
unsigned count = theCloud->size();
SimpleCloud* cloud = new SimpleCloud();
if (!cloud->reserve(count)) //not enough memory
return 0;
uchar dim1 = (dim>0 ? dim-1 : 2);
uchar dim2 = (dim<2 ? dim+1 : 0);
float tan_alpha = tan(alpha*static_cast<float>(CC_DEG_TO_RAD));
//float cos_alpha = cos(alpha*CC_DEG_TO_RAD);
//float sin_alpha = sin(alpha*CC_DEG_TO_RAD);
float q = 1.0f/(1.0f+tan_alpha*tan_alpha);
CCVector3 P;
PointCoordinateType u,lon,z2,x2,dX,dZ,lat,alt;
theCloud->placeIteratorAtBegining();
//normsType* _theNorms = theNorms.begin();
NormalizedProgress* nprogress = 0;
if (progressCb)
{
progressCb->reset();
progressCb->setMethodTitle("DevelopOnCone");
char buffer[256];
sprintf(buffer,"Number of points = %u",count);
nprogress = new NormalizedProgress(progressCb,count);
progressCb->setInfo(buffer);
progressCb->start();
}
for (unsigned i=0; i<count; i++)
{
const CCVector3 *Q = theCloud->getNextPoint();
P = *Q-center;
u = sqrt(P.u[dim1]*P.u[dim1] + P.u[dim2]*P.u[dim2]);
lon = atan2(P.u[dim1],P.u[dim2]);
//projection sur le cone
z2 = (P.u[dim]+u*tan_alpha)*q;
x2 = z2*tan_alpha;
//ordonnee
//#define ORTHO_CONIC_PROJECTION
#ifdef ORTHO_CONIC_PROJECTION
lat = sqrt(x2*x2+z2*z2)*cos_alpha;
if (lat*z2 < 0.0)
lat=-lat;
#else
lat = P.u[dim];
#endif
//altitude
dX = u-x2;
dZ = P.u[dim]-z2;
alt = sqrt(dX*dX+dZ*dZ);
//on regarde de quel cote de la surface du cone le resultat tombe par p.v.
if (x2*P.u[dim] - z2*u < 0)
alt=-alt;
cloud->addPoint(CCVector3(lon*baseRadius,lat+center[dim],alt));
if (nprogress && !nprogress->oneStep())
break;
}
if (nprogress)
{
delete nprogress;
nprogress = 0;
}
if (progressCb)
{
progressCb->stop();
}
return cloud;
}
bool GeometricalAnalysisTools::detectSphereRobust( GenericIndexedCloudPersist* cloud,
double outliersRatio,
CCVector3& center,
PointCoordinateType& radius,
double& rms,
GenericProgressCallback* progressCb/*=0*/,
double confidence/*=0.99*/)
{
if (!cloud || cloud->size() < 4)
{
//invalid input
return false;
}
assert(confidence < 1.0);
confidence = std::min(confidence,1.0-FLT_EPSILON);
const unsigned p = 4;
unsigned n = cloud->size();
//we'll need an array (sorted) to compute the medians
std::vector<PointCoordinateType> values;
try
{
values.resize(n);
}
catch (const std::bad_alloc&)
{
//not enough memory
return false;
}
//number of samples
unsigned m = 1;
if (n > p)
m = static_cast<unsigned>( log(1.0-confidence) / log(1.0-pow(1.0-outliersRatio,static_cast<double>(p))) );
//for progress notification
NormalizedProgress* nProgress = 0;
if (progressCb)
{
nProgress = new NormalizedProgress(progressCb,m);
char buffer[64];
sprintf(buffer,"Least Median of Squares samples: %u",m);
progressCb->reset();
progressCb->setInfo(buffer);
progressCb->setMethodTitle("Detect sphere");
progressCb->start();
}
//now we are going to randomly extract a subset of 4 points and test the resulting sphere each time
std::random_device rd; // non-deterministic generator
std::mt19937 gen(rd()); // to seed mersenne twister.
std::uniform_int_distribution<unsigned> dist(0, n - 1);
unsigned sampleCount = 0;
unsigned attempts = 0;
double minError = -1.0;
while (sampleCount < m && attempts < 2*m)
{
//get 4 random (different) indexes
unsigned indexes[4] = {0,0,0,0};
for (unsigned j=0; j<4; ++j)
{
bool isOK = false;
while (!isOK)
{
indexes[j] = dist(gen);
isOK = true;
for (unsigned k=0; k<j && isOK; ++k)
if (indexes[j] == indexes[k])
isOK = false;
}
}
const CCVector3* A = cloud->getPoint(indexes[0]);
const CCVector3* B = cloud->getPoint(indexes[1]);
const CCVector3* C = cloud->getPoint(indexes[2]);
const CCVector3* D = cloud->getPoint(indexes[3]);
++attempts;
CCVector3 thisCenter;
PointCoordinateType thisRadius;
if (!computeSphereFrom4(*A,*B,*C,*D,thisCenter,thisRadius))
continue;
//compute residuals
for (unsigned i=0; i<n; ++i)
{
PointCoordinateType error = (*cloud->getPoint(i) - thisCenter).norm() - thisRadius;
values[i] = error*error;
}
std::sort(values.begin(),values.end());
//the error is the median of the squared residuals
double error = values[n/2];
//we keep track of the solution with the least error
if (error < minError || minError < 0.0)
{
minError = error;
center = thisCenter;
radius = thisRadius;
}
++sampleCount;
if (nProgress && !nProgress->oneStep())
{
//progress canceled by the user
delete nProgress;
return false;
}
}
//too many failures?!
if (sampleCount < m)
{
if (nProgress)
delete nProgress;
return false;
}
//last step: robust estimation
ReferenceCloud candidates(cloud);
if (n > p)
{
//e robust standard deviation estimate (see Zhang's report)
double sigma = 1.4826 * (1.0 + 5.0 /(n-p)) * sqrt(minError);
//compute the least-squares best-fitting sphere with the points
//having residuals below 2.5 sigma
double maxResidual = 2.5 * sigma;
if (candidates.reserve(n))
{
//compute residuals and select the points
for (unsigned i=0; i<n; ++i)
{
PointCoordinateType error = (*cloud->getPoint(i) - center).norm() - radius;
if (error < maxResidual)
candidates.addPointIndex(i);
}
candidates.resize(candidates.size());
//eventually estimate the robust sphere parameters with least squares (iterative)
if (refineSphereLS(&candidates,center,radius))
{
//replace input cloud by this subset!
cloud = &candidates;
n = cloud->size();
}
}
else
{
//not enough memory!
//we'll keep the rough estimate...
}
}
//update residuals
{
double residuals = 0;
for (unsigned i=0; i<n; ++i)
{
const CCVector3* P = cloud->getPoint(i);
double e = (*P - center).norm() - radius;
residuals += e*e;
}
rms = sqrt(residuals/n);
}
if (nProgress)
{
delete nProgress;
nProgress = 0;
}
return true;
}
ReferenceCloud* CloudSamplingTools::resampleCloudSpatially(GenericIndexedCloudPersist* inputCloud,
PointCoordinateType minDistance,
const SFModulationParams& modParams,
DgmOctree* inputOctree/*=0*/,
GenericProgressCallback* progressCb/*=0*/)
{
assert(inputCloud);
unsigned cloudSize = inputCloud->size();
DgmOctree* octree = inputOctree;
if (!octree)
{
octree = new DgmOctree(inputCloud);
if (octree->build() < static_cast<int>(cloudSize))
{
delete octree;
return 0;
}
}
assert(octree && octree->associatedCloud() == inputCloud);
//output cloud
ReferenceCloud* sampledCloud = new ReferenceCloud(inputCloud);
const unsigned c_reserveStep = 65536;
if (!sampledCloud->reserve(std::min(cloudSize,c_reserveStep)))
{
if (!inputOctree)
delete octree;
return 0;
}
GenericChunkedArray<1,char>* markers = new GenericChunkedArray<1,char>(); //DGM: upgraded from vector, as this can be quite huge!
if (!markers->resize(cloudSize,true,1)) //true by default
{
markers->release();
if (!inputOctree)
delete octree;
delete sampledCloud;
return 0;
}
//best octree level (there may be several of them if we use parameter modulation)
std::vector<unsigned char> bestOctreeLevel;
bool modParamsEnabled = modParams.enabled;
ScalarType sfMin = 0, sfMax = 0;
try
{
if (modParams.enabled)
{
//compute min and max sf values
ScalarFieldTools::computeScalarFieldExtremas(inputCloud,sfMin,sfMax);
if (!ScalarField::ValidValue(sfMin))
{
//all SF values are NAN?!
modParamsEnabled = false;
}
else
{
//compute min and max 'best' levels
PointCoordinateType dist0 = static_cast<PointCoordinateType>(sfMin * modParams.a + modParams.b);
PointCoordinateType dist1 = static_cast<PointCoordinateType>(sfMax * modParams.a + modParams.b);
unsigned char level0 = octree->findBestLevelForAGivenNeighbourhoodSizeExtraction(dist0);
unsigned char level1 = octree->findBestLevelForAGivenNeighbourhoodSizeExtraction(dist1);
bestOctreeLevel.push_back(level0);
if (level1 != level0)
{
//add intermediate levels if necessary
size_t levelCount = (level1 < level0 ? level0-level1 : level1-level0) + 1;
assert(levelCount != 0);
for (size_t i=1; i<levelCount-1; ++i) //we already know level0 and level1!
{
ScalarType sfVal = sfMin + i*((sfMax-sfMin)/levelCount);
PointCoordinateType dist = static_cast<PointCoordinateType>(sfVal * modParams.a + modParams.b);
unsigned char level = octree->findBestLevelForAGivenNeighbourhoodSizeExtraction(dist);
bestOctreeLevel.push_back(level);
}
}
bestOctreeLevel.push_back(level1);
}
}
else
{
unsigned char defaultLevel = octree->findBestLevelForAGivenNeighbourhoodSizeExtraction(minDistance);
bestOctreeLevel.push_back(defaultLevel);
}
}
catch (const std::bad_alloc&)
{
//not enough memory
markers->release();
if (!inputOctree)
delete octree;
delete sampledCloud;
return 0;
}
//progress notification
NormalizedProgress* normProgress = 0;
if (progressCb)
{
progressCb->setMethodTitle("Spatial resampling");
char buffer[256];
sprintf(buffer,"Points: %u\nMin dist.: %f",cloudSize,minDistance);
progressCb->setInfo(buffer);
normProgress = new NormalizedProgress(progressCb,cloudSize);
progressCb->reset();
progressCb->start();
}
//for each point in the cloud that is still 'marked', we look
//for its neighbors and remove their own marks
markers->placeIteratorAtBegining();
bool error = false;
//default octree level
assert(!bestOctreeLevel.empty());
unsigned char octreeLevel = bestOctreeLevel.front();
//default distance between points
PointCoordinateType minDistBetweenPoints = minDistance;
for (unsigned i=0; i<cloudSize; i++, markers->forwardIterator())
{
//no mark? we skip this point
if (markers->getCurrentValue() != 0)
{
//init neighbor search structure
const CCVector3* P = inputCloud->getPoint(i);
//parameters modulation
if (modParamsEnabled)
{
ScalarType sfVal = inputCloud->getPointScalarValue(i);
if (ScalarField::ValidValue(sfVal))
{
//modulate minDistance
minDistBetweenPoints = static_cast<PointCoordinateType>(sfVal * modParams.a + modParams.b);
//get (approximate) best level
size_t levelIndex = static_cast<size_t>(bestOctreeLevel.size() * (sfVal / (sfMax-sfMin)));
if (levelIndex == bestOctreeLevel.size())
--levelIndex;
octreeLevel = bestOctreeLevel[levelIndex];
}
else
{
minDistBetweenPoints = minDistance;
octreeLevel = bestOctreeLevel.front();
}
}
//look for neighbors and 'de-mark' them
{
DgmOctree::NeighboursSet neighbours;
octree->getPointsInSphericalNeighbourhood(*P,minDistBetweenPoints,neighbours,octreeLevel);
for (DgmOctree::NeighboursSet::iterator it = neighbours.begin(); it != neighbours.end(); ++it)
if (it->pointIndex != i)
markers->setValue(it->pointIndex,0);
}
//At this stage, the ith point is the only one marked in a radius of <minDistance>.
//Therefore it will necessarily be in the final cloud!
if (sampledCloud->size() == sampledCloud->capacity() && !sampledCloud->reserve(sampledCloud->capacity() + c_reserveStep))
{
//not enough memory
error = true;
break;
}
if (!sampledCloud->addPointIndex(i))
{
//not enough memory
error = true;
break;
}
}
//progress indicator
if (normProgress && !normProgress->oneStep())
{
//cancel process
error = true;
break;
}
}
//remove unnecessarily allocated memory
if (!error)
{
if (sampledCloud->capacity() > sampledCloud->size())
sampledCloud->resize(sampledCloud->size());
}
else
{
delete sampledCloud;
sampledCloud = 0;
}
if(normProgress)
{
delete normProgress;
normProgress = 0;
progressCb->stop();
}
if (!inputOctree)
{
//locally computed octree
delete octree;
octree = 0;
}
markers->release();
markers = 0;
return sampledCloud;
}
