void ConePrimitiveShape::BitmapExtent(float epsilon,
GfxTL::AABox< GfxTL::Vector2Df > *bbox,
MiscLib::Vector< std::pair< float, float > > *params,
size_t *uextent, size_t *vextent)
{
*uextent = std::ceil((bbox->Max()[0] - bbox->Min()[0]) / epsilon); // no wrappig along u direction
*vextent = std::ceil((bbox->Max()[1] - bbox->Min()[1]) / epsilon) + 1; // add one for wrapping
if((*vextent) * (*uextent) > 1e6 && m_cone.Angle() < float(M_PI / 4))
{
// try to reparameterize
// try to find cut in the outer regions
MiscLib::Vector< float > angularParams;//(params->size());
angularParams.reserve(params->size());
float outer = 3.f * std::max(abs(bbox->Min()[0]), abs(bbox->Max()[0])) / 4.f;
for(size_t i = 0; i < params->size(); ++i)
if((*params)[i].first > outer)
angularParams.push_back(((*params)[i].second
/ m_cone.RadiusAtLength((*params)[i].first)) + float(M_PI));
std::sort(angularParams.begin(), angularParams.end());
// try to find a large gap
float maxGap = 0;
float lower, upper;
for(size_t i = 1; i < angularParams.size(); ++i)
{
float gap = angularParams[i] - angularParams[i - 1];
if(gap > maxGap)
{
maxGap = gap;
lower = angularParams[i - 1];
upper = angularParams[i];
}
}
// reparameterize with new angular cut
float newCut = (lower + upper) / 2.f;
m_cone.RotateAngularDirection(newCut);
bbox->Min()[1] = std::numeric_limits< float >::infinity();
bbox->Max()[1] = -std::numeric_limits< float >::infinity();
for(size_t i = 0; i < params->size(); ++i)
{
float r = m_cone.RadiusAtLength((*params)[i].first);
(*params)[i].second = ((*params)[i].second / r) + float(M_PI) - newCut;
if((*params)[i].second < 0)
(*params)[i].second = 2 * float(M_PI) + (*params)[i].second;
(*params)[i].second = ((*params)[i].second - float(M_PI)) * r;
if((*params)[i].second < bbox->Min()[1])
bbox->Min()[1] = (*params)[i].second;
if((*params)[i].second > bbox->Max()[1])
bbox->Max()[1] = (*params)[i].second;
}
*vextent = std::floor((bbox->Max()[1] - bbox->Min()[1]) / epsilon) + 1;
}
}
bool RansacShapeDetector::FindBestCandidate(CandidatesType &candidates,
const MiscLib::Vector< ImmediateOctreeType * > &octrees, const PointCloud &pc,
ScoreVisitorT &scoreVisitor, size_t currentSize,
size_t drawnCandidates, size_t numInvalid, size_t minSize, float numLevels,
float *maxForgottenCandidate, float *candidateFailProb) const
{
if(!candidates.size())
return false;
size_t maxImproveSubsetDuringMaxSearch = octrees.size();
// sort by expected value
std::sort(candidates.begin(), candidates.end());
// check if max is smaller than forgotten candidate
if(candidates.size() && candidates.back().ExpectedValue() < *maxForgottenCandidate)
{
// drawn candidates is wrong!
// need to correct the value
drawnCandidates = std::max(candidates.size(), (size_t)1);
*maxForgottenCandidate = 0;
}
MiscLib::Vector< Candidate * > candHeap;
for(size_t i = candidates.size() - 1; i != -1; --i)
{
if(CandidateFailureProbability(
candidates[i].ExpectedValue(),
currentSize - numInvalid, drawnCandidates, numLevels) > m_options.m_probability)
break;
candHeap.push_back(&candidates[i]);
}
if(!candHeap.size())
{
return false;
}
std::make_heap(candHeap.begin(), candHeap.end(), CandidateHeapPred());
MiscLib::Vector< Candidate * > beatenCands;
Candidate *trial = candHeap.front();
std::pop_heap(candHeap.begin(), candHeap.end(), CandidateHeapPred());
candHeap.pop_back();
float bestCandidateFailureProbability;
while(candHeap.size())
{
if(trial->IsEquivalent(*candHeap.front(), pc, m_options.m_epsilon,
m_options.m_normalThresh))
{
std::pop_heap(candHeap.begin(), candHeap.end(),
CandidateHeapPred());
candHeap.pop_back();
continue;
}
bool isEquivalent = false;
for(size_t j = 0; j < beatenCands.size(); ++j)
{
if(beatenCands[j]->IsEquivalent(*candHeap.front(), pc,
m_options.m_epsilon, m_options.m_normalThresh))
{
isEquivalent = true;
break;
}
}
if(isEquivalent)
{
std::pop_heap(candHeap.begin(), candHeap.end(),
CandidateHeapPred());
candHeap.pop_back();
continue;
}
bestCandidateFailureProbability = CandidateFailureProbability(
trial->ExpectedValue(),
currentSize - numInvalid, drawnCandidates, numLevels);
while((bestCandidateFailureProbability <= m_options.m_probability)
&& (*trial >= *candHeap.front())
&& (trial->UpperBound() >= minSize)
&& trial->ImproveBounds(octrees, pc, scoreVisitor, currentSize,
m_options.m_bitmapEpsilon, octrees.size()))
{
bestCandidateFailureProbability = CandidateFailureProbability(
trial->ExpectedValue(),
currentSize - numInvalid, drawnCandidates, numLevels);
}
if(bestCandidateFailureProbability <= m_options.m_probability
&& trial->UpperBound() >= minSize
&& trial->ComputedSubsets() >= octrees.size()
&& *trial >= *candHeap.front())
break;
if(bestCandidateFailureProbability <= m_options.m_probability
&& trial->UpperBound() >= minSize)
{
candHeap.push_back(trial);
std::push_heap(candHeap.begin(), candHeap.end(), CandidateHeapPred());
}
else if((int)trial->ComputedSubsets()
> std::max(2, ((int)octrees.size()) - 2))
beatenCands.push_back(trial);
//nextCandidate
trial = candHeap.front();
std::pop_heap(candHeap.begin(), candHeap.end(), CandidateHeapPred());
candHeap.pop_back();
}
bestCandidateFailureProbability = CandidateFailureProbability(
trial->ExpectedValue(),
currentSize - numInvalid, drawnCandidates, numLevels);
while(bestCandidateFailureProbability <= m_options.m_probability
&& trial->UpperBound() >= minSize
&& trial->ImproveBounds(octrees, pc, scoreVisitor, currentSize,
m_options.m_bitmapEpsilon, octrees.size()))
{
bestCandidateFailureProbability = CandidateFailureProbability(
trial->ExpectedValue(),
currentSize - numInvalid, drawnCandidates, numLevels);
}
if((bestCandidateFailureProbability > m_options.m_probability
|| trial->UpperBound() < minSize)
&& (!m_autoAcceptSize || trial->UpperBound() < m_autoAcceptSize))
{
return false;
}
std::sort(candidates.begin(), candidates.end());
size_t bestCandidate = candidates.size() - 1;
bestCandidateFailureProbability = CandidateFailureProbability(
candidates.back().ExpectedValue(),
currentSize - numInvalid, drawnCandidates, numLevels);
for(size_t i = bestCandidate - 1; i != -1; --i)
{
float iFailProb = CandidateFailureProbability(candidates[i].ExpectedValue(),
currentSize - numInvalid, drawnCandidates, numLevels);
if(iFailProb > m_options.m_probability || candidates[i].UpperBound() < minSize
|| candidates[i].UpperBound() < candidates[bestCandidate].LowerBound())
break;
// check if this is an identical candidate
if(candidates[bestCandidate].IsEquivalent(candidates[i], pc,
m_options.m_epsilon, m_options.m_normalThresh))
{
continue;
}
bool isEquivalent = false;
for(size_t j = 0; j < beatenCands.size(); ++j)
{
if(beatenCands[j]->IsEquivalent(candidates[i], pc,
m_options.m_epsilon, m_options.m_normalThresh))
{
isEquivalent = true;
break;
}
}
if(isEquivalent)
{
continue;
}
do
{
if(candidates[i].UpperBound() > candidates[bestCandidate].UpperBound()
&& candidates[i].LowerBound() < candidates[bestCandidate].LowerBound())
{
bool dontBreak = candidates[i].ImproveBounds(octrees, pc, scoreVisitor,
currentSize, m_options.m_bitmapEpsilon,
maxImproveSubsetDuringMaxSearch);
iFailProb = CandidateFailureProbability(candidates[i].ExpectedValue(),
currentSize - numInvalid, drawnCandidates, numLevels);
if(!dontBreak)
break;
}
else if(candidates[bestCandidate].UpperBound() > candidates[i].UpperBound()
&& candidates[bestCandidate].LowerBound() < candidates[i].LowerBound())
{
bool dontBreak = candidates[bestCandidate].ImproveBounds(octrees, pc,
scoreVisitor, currentSize, m_options.m_bitmapEpsilon,
maxImproveSubsetDuringMaxSearch);
bestCandidateFailureProbability = CandidateFailureProbability(
candidates[bestCandidate].ExpectedValue(),
currentSize - numInvalid, drawnCandidates, numLevels);
if(!dontBreak)
break;
}
else
{
bool dontBreak = candidates[bestCandidate].ImproveBounds(octrees, pc,
scoreVisitor, currentSize, m_options.m_bitmapEpsilon,
maxImproveSubsetDuringMaxSearch);
dontBreak = candidates[i].ImproveBounds(octrees, pc, scoreVisitor,
currentSize, m_options.m_bitmapEpsilon,
maxImproveSubsetDuringMaxSearch)
|| dontBreak;
iFailProb = CandidateFailureProbability(candidates[i].ExpectedValue(),
currentSize - numInvalid, drawnCandidates, numLevels);
bestCandidateFailureProbability = CandidateFailureProbability(
candidates[bestCandidate].ExpectedValue(),
currentSize - numInvalid, drawnCandidates, numLevels);
if(!dontBreak)
break;
}
}
while(bestCandidateFailureProbability <= m_options.m_probability
&& iFailProb <= m_options.m_probability
&& candidates[i].UpperBound() >= minSize
&& candidates[bestCandidate].UpperBound() >= minSize
&& candidates[i].UpperBound() > candidates[bestCandidate].LowerBound()
&& candidates[i].LowerBound() < candidates[bestCandidate].UpperBound()
);
if((
candidates[i] > candidates[bestCandidate]
|| bestCandidateFailureProbability > m_options.m_probability
|| candidates[bestCandidate].UpperBound() < minSize)
&& (iFailProb <= m_options.m_probability && candidates[i].UpperBound() >= minSize))
{
while(iFailProb <= m_options.m_probability && candidates[i].UpperBound() >= minSize
&& candidates[i] > candidates[bestCandidate]
&& candidates[i].ImproveBounds(octrees, pc, scoreVisitor,
currentSize, m_options.m_bitmapEpsilon, octrees.size()))
{
iFailProb = CandidateFailureProbability(candidates[i].ExpectedValue(),
currentSize - numInvalid, drawnCandidates, numLevels);
}
if(candidates[i] > candidates[bestCandidate])
{
beatenCands.push_back(&candidates[bestCandidate]);
bestCandidate = i;
bestCandidateFailureProbability = iFailProb;
}
else
beatenCands.push_back(&candidates[i]);
}
else
beatenCands.push_back(&candidates[i]);
if(bestCandidateFailureProbability > m_options.m_probability
|| candidates[bestCandidate].UpperBound() < minSize)
break;
} // end for
while(candidates[bestCandidate].ImproveBounds(octrees, pc, scoreVisitor,
currentSize, m_options.m_bitmapEpsilon, octrees.size()));
bestCandidateFailureProbability = CandidateFailureProbability(
candidates[bestCandidate].ExpectedValue(),
currentSize - numInvalid, drawnCandidates, numLevels);
if((bestCandidateFailureProbability <= m_options.m_probability
&& candidates[bestCandidate].UpperBound() >= minSize)
|| (m_autoAcceptSize && candidates[bestCandidate].UpperBound() >= m_autoAcceptSize))
{
std::swap(candidates.back(), candidates[bestCandidate]);
*candidateFailProb = bestCandidateFailureProbability;
return true;
}
std::sort(candidates.begin(), candidates.end()/*, std::greater< Candidate >()*/);
return false;
}
void qRansacSD::doAction()
{
assert(m_app);
if (!m_app)
return;
const ccHObject::Container& selectedEntities = m_app->getSelectedEntities();
size_t selNum = selectedEntities.size();
if (selNum!=1)
{
m_app->dispToConsole("Select only one cloud!",ccMainAppInterface::ERR_CONSOLE_MESSAGE);
return;
}
ccHObject* ent = selectedEntities[0];
assert(ent);
if (!ent || !ent->isA(CC_POINT_CLOUD))
{
m_app->dispToConsole("Select a real point cloud!",ccMainAppInterface::ERR_CONSOLE_MESSAGE);
return;
}
ccPointCloud* pc = static_cast<ccPointCloud*>(ent);
//input cloud
size_t count = (size_t)pc->size();
bool hasNorms = pc->hasNormals();
PointCoordinateType bbMin[3],bbMax[3];
pc->getBoundingBox(bbMin,bbMax);
//Convert CC point cloud to RANSAC_SD type
PointCloud cloud;
{
try
{
cloud.reserve(count);
}
catch(...)
{
m_app->dispToConsole("Not enough memory!",ccMainAppInterface::ERR_CONSOLE_MESSAGE);
return;
}
//default point & normal
Point Pt;
Pt.normal[0] = 0.0;
Pt.normal[1] = 0.0;
Pt.normal[2] = 0.0;
for (unsigned i=0; i<(unsigned)count; ++i)
{
const CCVector3* P = pc->getPoint(i);
Pt.pos[0] = P->x;
Pt.pos[1] = P->y;
Pt.pos[2] = P->z;
if (hasNorms)
{
const PointCoordinateType* N = pc->getPointNormal(i);
Pt.normal[0] = N[0];
Pt.normal[1] = N[1];
Pt.normal[2] = N[2];
}
cloud.push_back(Pt);
}
//manually set bounding box!
Vec3f cbbMin,cbbMax;
cbbMin[0] = bbMin[0];
cbbMin[1] = bbMin[1];
cbbMin[2] = bbMin[2];
cbbMax[0] = bbMax[0];
cbbMax[1] = bbMax[1];
cbbMax[2] = bbMax[2];
cloud.setBBox(cbbMin,cbbMax);
}
//cloud scale (useful for setting several parameters
const float scale = cloud.getScale();
//init dialog with default values
ccRansacSDDlg rsdDlg(m_app->getMainWindow());
rsdDlg.epsilonDoubleSpinBox->setValue(.01f * scale); // set distance threshold to .01f of bounding box width
// NOTE: Internally the distance threshold is taken as 3 * ransacOptions.m_epsilon!!!
rsdDlg.bitmapEpsilonDoubleSpinBox->setValue(.02f * scale); // set bitmap resolution to .02f of bounding box width
// NOTE: This threshold is NOT multiplied internally!
rsdDlg.supportPointsSpinBox->setValue(s_supportPoints);
rsdDlg.normThreshDoubleSpinBox->setValue(s_normThresh);
rsdDlg.probaDoubleSpinBox->setValue(s_proba);
rsdDlg.planeCheckBox->setChecked(s_primEnabled[0]);
rsdDlg.sphereCheckBox->setChecked(s_primEnabled[1]);
rsdDlg.cylinderCheckBox->setChecked(s_primEnabled[2]);
rsdDlg.coneCheckBox->setChecked(s_primEnabled[3]);
rsdDlg.torusCheckBox->setChecked(s_primEnabled[4]);
if (!rsdDlg.exec())
return;
//for parameters persistence
{
s_supportPoints = rsdDlg.supportPointsSpinBox->value();
s_normThresh = rsdDlg.normThreshDoubleSpinBox->value();
s_proba = rsdDlg.probaDoubleSpinBox->value();
//consistency check
{
unsigned char primCount = 0;
for (unsigned char k=0; k<5; ++k)
primCount += (unsigned)s_primEnabled[k];
if (primCount==0)
{
m_app->dispToConsole("No primitive type selected!",ccMainAppInterface::ERR_CONSOLE_MESSAGE);
return;
}
}
s_primEnabled[0] = rsdDlg.planeCheckBox->isChecked();
s_primEnabled[1] = rsdDlg.sphereCheckBox->isChecked();
s_primEnabled[2] = rsdDlg.cylinderCheckBox->isChecked();
s_primEnabled[3] = rsdDlg.coneCheckBox->isChecked();
s_primEnabled[4] = rsdDlg.torusCheckBox->isChecked();
}
//import parameters from dialog
RansacShapeDetector::Options ransacOptions;
{
ransacOptions.m_epsilon = rsdDlg.epsilonDoubleSpinBox->value();
ransacOptions.m_bitmapEpsilon = rsdDlg.bitmapEpsilonDoubleSpinBox->value();
ransacOptions.m_normalThresh = rsdDlg.normThreshDoubleSpinBox->value();
ransacOptions.m_minSupport = rsdDlg.supportPointsSpinBox->value();
ransacOptions.m_probability = rsdDlg.probaDoubleSpinBox->value();
}
//progress dialog
ccProgressDialog progressCb(false,m_app->getMainWindow());
progressCb.setRange(0,0);
if (!hasNorms)
{
progressCb.setInfo("Computing normals (please wait)");
progressCb.start();
QApplication::processEvents();
cloud.calcNormals(.01f * scale);
if (pc->reserveTheNormsTable())
{
for (size_t i=0; i<count; ++i)
{
CCVector3 N(cloud[i].normal);
N.normalize();
pc->addNorm(N.u);
}
pc->showNormals(true);
//currently selected entities appearance may have changed!
pc->prepareDisplayForRefresh_recursive();
}
else
{
m_app->dispToConsole("Not enough memory to compute normals!",ccMainAppInterface::ERR_CONSOLE_MESSAGE);
return;
}
}
// set which primitives are to be detected by adding the respective constructors
RansacShapeDetector detector(ransacOptions); // the detector object
if (rsdDlg.planeCheckBox->isChecked())
detector.Add(new PlanePrimitiveShapeConstructor());
if (rsdDlg.sphereCheckBox->isChecked())
detector.Add(new SpherePrimitiveShapeConstructor());
if (rsdDlg.cylinderCheckBox->isChecked())
detector.Add(new CylinderPrimitiveShapeConstructor());
if (rsdDlg.coneCheckBox->isChecked())
detector.Add(new ConePrimitiveShapeConstructor());
if (rsdDlg.torusCheckBox->isChecked())
detector.Add(new TorusPrimitiveShapeConstructor());
size_t remaining = count;
typedef std::pair< MiscLib::RefCountPtr< PrimitiveShape >, size_t > DetectedShape;
MiscLib::Vector< DetectedShape > shapes; // stores the detected shapes
// run detection
// returns number of unassigned points
// the array shapes is filled with pointers to the detected shapes
// the second element per shapes gives the number of points assigned to that primitive (the support)
// the points belonging to the first shape (shapes[0]) have been sorted to the end of pc,
// i.e. into the range [ pc.size() - shapes[0].second, pc.size() )
// the points of shape i are found in the range
// [ pc.size() - \sum_{j=0..i} shapes[j].second, pc.size() - \sum_{j=0..i-1} shapes[j].second )
{
progressCb.setInfo("Operation in progress");
progressCb.setMethodTitle("Ransac Shape Detection");
progressCb.start();
//run in a separate thread
s_detector = &detector;
s_shapes = &shapes;
s_cloud = &cloud;
QFuture<void> future = QtConcurrent::run(doDetection);
unsigned progress = 0;
while (!future.isFinished())
{
#if defined(_WIN32) || defined(WIN32)
::Sleep(500);
#else
sleep(500);
#endif
progressCb.update(++progress);
//Qtconcurrent::run can't be canceled!
/*if (progressCb.isCancelRequested())
{
future.cancel();
future.waitForFinished();
s_remainingPoints = count;
break;
}
//*/
}
remaining = s_remainingPoints;
progressCb.stop();
QApplication::processEvents();
}
//else
//{
// remaining = detector.Detect(cloud, 0, cloud.size(), &shapes);
//}
#ifdef _DEBUG
FILE* fp = fopen("RANS_SD_trace.txt","wt");
fprintf(fp,"[Options]\n");
fprintf(fp,"epsilon=%f\n",ransacOptions.m_epsilon);
fprintf(fp,"bitmap epsilon=%f\n",ransacOptions.m_bitmapEpsilon);
fprintf(fp,"normal thresh=%f\n",ransacOptions.m_normalThresh);
fprintf(fp,"min support=%i\n",ransacOptions.m_minSupport);
fprintf(fp,"probability=%f\n",ransacOptions.m_probability);
fprintf(fp,"\n[Statistics]\n");
fprintf(fp,"input points=%i\n",count);
fprintf(fp,"segmented=%i\n",count-remaining);
fprintf(fp,"remaining=%i\n",remaining);
if (shapes.size()>0)
{
fprintf(fp,"\n[Shapes]\n");
for (unsigned i=0; i<shapes.size(); ++i)
{
PrimitiveShape* shape = shapes[i].first;
unsigned shapePointsCount = shapes[i].second;
std::string desc;
shape->Description(&desc);
fprintf(fp,"#%i - %s - %i points\n",i+1,desc.c_str(),shapePointsCount);
}
}
fclose(fp);
#endif
if (remaining == count)
{
m_app->dispToConsole("Segmentation failed...",ccMainAppInterface::ERR_CONSOLE_MESSAGE);
return;
}
if (shapes.size() > 0)
{
ccHObject* group = 0;
for (MiscLib::Vector<DetectedShape>::const_iterator it = shapes.begin(); it != shapes.end(); ++it)
{
const PrimitiveShape* shape = it->first;
size_t shapePointsCount = it->second;
//too many points?!
if (shapePointsCount > count)
{
m_app->dispToConsole("Inconsistent result!",ccMainAppInterface::ERR_CONSOLE_MESSAGE);
break;
}
std::string desc;
shape->Description(&desc);
//new cloud for sub-part
ccPointCloud* pcShape = new ccPointCloud(desc.c_str());
//we fill cloud with sub-part points
if (!pcShape->reserve((unsigned)shapePointsCount))
{
m_app->dispToConsole("Not enough memory!",ccMainAppInterface::ERR_CONSOLE_MESSAGE);
delete pcShape;
break;
}
bool saveNormals = pcShape->reserveTheNormsTable();
for (size_t j=0; j<shapePointsCount; ++j)
{
pcShape->addPoint(CCVector3(cloud[count-1-j].pos));
if (saveNormals)
pcShape->addNorm(cloud[count-1-j].normal);
}
//random color
unsigned char col[3]= { (unsigned char)(255.0*(float)rand()/(float)RAND_MAX),
(unsigned char)(255.0*(float)rand()/(float)RAND_MAX),
0
};
col[2]=255-(col[1]+col[2])/2;
pcShape->setRGBColor(col);
pcShape->showColors(true);
pcShape->showNormals(saveNormals);
pcShape->setVisible(true);
//convert detected primitive into a CC primitive type
ccGenericPrimitive* prim = 0;
switch(shape->Identifier())
{
case 0: //plane
{
const PlanePrimitiveShape* plane = static_cast<const PlanePrimitiveShape*>(shape);
Vec3f G = plane->Internal().getPosition();
Vec3f N = plane->Internal().getNormal();
Vec3f X = plane->getXDim();
Vec3f Y = plane->getYDim();
//we look for real plane extents
float minX,maxX,minY,maxY;
for (size_t j=0; j<shapePointsCount; ++j)
{
std::pair<float,float> param;
plane->Parameters(cloud[count-1-j].pos,¶m);
if (j!=0)
{
if (minX<param.first)
minX=param.first;
else if (maxX>param.first)
maxX=param.first;
if (minY<param.second)
minY=param.second;
else if (maxY>param.second)
maxY=param.second;
}
else
{
minX=maxX=param.first;
minY=maxY=param.second;
}
}
//we recenter plane (as it is not always the case!)
float dX = maxX-minX;
float dY = maxY-minY;
G += X * (minX+dX*0.5);
G += Y * (minY+dY*0.5);
//we build matrix from these vecctors
ccGLMatrix glMat(CCVector3(X.getValue()),
CCVector3(Y.getValue()),
CCVector3(N.getValue()),
CCVector3(G.getValue()));
//plane primitive
prim = new ccPlane(dX,dY,&glMat);
}
break;
case 1: //sphere
{
const SpherePrimitiveShape* sphere = static_cast<const SpherePrimitiveShape*>(shape);
float radius = sphere->Internal().Radius();
Vec3f CC = sphere->Internal().Center();
pcShape->setName(QString("Sphere (r=%1)").arg(radius,0,'f'));
//we build matrix from these vecctors
ccGLMatrix glMat;
glMat.setTranslation(CCVector3(CC.getValue()));
//sphere primitive
prim = new ccSphere(radius,&glMat);
prim->setEnabled(false);
}
break;
case 2: //cylinder
{
const CylinderPrimitiveShape* cyl = static_cast<const CylinderPrimitiveShape*>(shape);
Vec3f G = cyl->Internal().AxisPosition();
Vec3f N = cyl->Internal().AxisDirection();
Vec3f X = cyl->Internal().AngularDirection();
Vec3f Y = N.cross(X);
float r = cyl->Internal().Radius();
float hMin = cyl->MinHeight();
float hMax = cyl->MaxHeight();
float h = hMax-hMin;
G += N * (hMin+h*0.5);
pcShape->setName(QString("Cylinder (r=%1/h=%2)").arg(r,0,'f').arg(h,0,'f'));
//we build matrix from these vecctors
ccGLMatrix glMat(CCVector3(X.getValue()),
CCVector3(Y.getValue()),
CCVector3(N.getValue()),
CCVector3(G.getValue()));
//cylinder primitive
prim = new ccCylinder(r,h,&glMat);
prim->setEnabled(false);
}
break;
case 3: //cone
{
const ConePrimitiveShape* cone = static_cast<const ConePrimitiveShape*>(shape);
Vec3f CC = cone->Internal().Center();
Vec3f CA = cone->Internal().AxisDirection();
float alpha = cone->Internal().Angle();
//compute max height
CCVector3 maxP(CC.getValue());
float maxHeight = 0;
for (size_t j=0; j<shapePointsCount; ++j)
{
float h = cone->Internal().Height(cloud[count-1-j].pos);
if (h>maxHeight)
{
maxHeight=h;
maxP = CCVector3(cloud[count-1-j].pos);
}
}
pcShape->setName(QString("Cone (alpha=%1/h=%2)").arg(alpha,0,'f').arg(maxHeight,0,'f'));
float radius = tan(alpha)*maxHeight;
CCVector3 Z = CCVector3(CA.getValue());
CCVector3 C = CCVector3(CC.getValue()); //cone apex
//construct remaining of base
Z.normalize();
CCVector3 X = maxP - (C + maxHeight * Z);
X.normalize();
CCVector3 Y = Z * X;
//we build matrix from these vecctors
ccGLMatrix glMat(X,Y,Z,C+(maxHeight*0.5)*Z);
//cone primitive
prim = new ccCone(0,radius,maxHeight,0,0,&glMat);
prim->setEnabled(false);
}
break;
case 4: //torus
{
const TorusPrimitiveShape* torus = static_cast<const TorusPrimitiveShape*>(shape);
if (torus->Internal().IsAppleShaped())
{
m_app->dispToConsole("[qRansacSD] Apple-shaped torus are not handled by CloudCompare!",ccMainAppInterface::WRN_CONSOLE_MESSAGE);
}
else
{
Vec3f CC = torus->Internal().Center();
Vec3f CA = torus->Internal().AxisDirection();
float minRadius = torus->Internal().MinorRadius();
float maxRadius = torus->Internal().MajorRadius();
pcShape->setName(QString("Torus (r=%1/R=%2)").arg(minRadius,0,'f').arg(maxRadius,0,'f'));
CCVector3 Z = CCVector3(CA.getValue());
CCVector3 C = CCVector3(CC.getValue());
//construct remaining of base
CCVector3 X = Z.orthogonal();
CCVector3 Y = Z * X;
//we build matrix from these vecctors
ccGLMatrix glMat(X,Y,Z,C);
//torus primitive
prim = new ccTorus(maxRadius-minRadius,maxRadius+minRadius,M_PI*2.0,false,0,&glMat);
prim->setEnabled(false);
}
}
break;
}
//is there a primitive to add to part cloud?
if (prim)
{
prim->applyGLTransformation_recursive();
pcShape->addChild(prim);
prim->setDisplay(pcShape->getDisplay());
prim->setColor(col);
prim->showColors(true);
prim->setVisible(true);
}
if (!group)
{
group = new ccHObject(QString("Ransac Detected Shapes (%1)").arg(ent->getName()));
m_app->addToDB(group,true,0,false);
}
group->addChild(pcShape);
m_app->addToDB(pcShape,true,0,false);
count -= shapePointsCount;
QApplication::processEvents();
}
if (group)
{
assert(group->getChildrenNumber()!=0);
//we hide input cloud
pc->setEnabled(false);
m_app->dispToConsole("[qRansacSD] Input cloud has been automtically hidden!",ccMainAppInterface::WRN_CONSOLE_MESSAGE);
//we add new group to DB/display
group->setVisible(true);
group->setDisplay_recursive(pc->getDisplay());
group->prepareDisplayForRefresh_recursive();
m_app->refreshAll();
}
}
}
bool Cylinder::InitAverage(const MiscLib::Vector< Vec3f > &samples)
{
if(samples.size() < 4)
return false;
// estimate axis from covariance of normal vectors
MiscLib::Vector< GfxTL::Vector3Df > normals;
size_t c = samples.size() / 2;
for(size_t i = c; i < samples.size(); ++i)
{
normals.push_back(GfxTL::Vector3Df(samples[i]));
normals.push_back(GfxTL::Vector3Df(-samples[i]));
}
GfxTL::MatrixXX< 3, 3, float > cov, eigenVectors;
GfxTL::Vector3Df eigenValues;
GfxTL::CovarianceMatrix(GfxTL::Vector3Df(0, 0, 0),
normals.begin(), normals.end(), &cov);
GfxTL::Jacobi(cov, &eigenValues, &eigenVectors);
// find the minimal eigenvalue and corresponding vector
float minEigVal = eigenValues[0];
unsigned int minEigIdx = 0;
for(unsigned int i = 1; i < 3; ++i)
if(eigenValues[i] < minEigVal)
{
minEigVal = eigenValues[i];
minEigIdx = i;
}
m_axisDir = Vec3f(eigenVectors[minEigIdx]);
// get a point on the axis from all pairs
m_axisPos = Vec3f(0, 0, 0);
m_radius = 0;
size_t pointCount = 0;
size_t pairCount = 0;
for(size_t i = 0; i < c - 1; ++i)
for(size_t j = i + 1; j < c; ++j)
{
// project first normal into plane
float l = m_axisDir.dot(samples[i + c]);
Vec3f xdir = samples[i + c] - l * m_axisDir;
xdir.normalize();
Vec3f ydir = m_axisDir.cross(xdir);
ydir.normalize();
// xdir is the x axis in the plane (y = 0) samples[i] is the origin
float lineBnx = ydir.dot(samples[j + c]);
if(abs(lineBnx) < .05f)
continue;
float lineBny = -xdir.dot(samples[j + c]);
// origin of lineB
Vec3f originB = samples[j] - samples[i];
float lineBOx = xdir.dot(originB);
float lineBOy = ydir.dot(originB);
float lineBd = lineBnx * lineBOx + lineBny * lineBOy;
// lineB in the plane complete
// point of intersection is y = 0 and x = lineBd / lineBnx
float radius = lineBd / lineBnx;
m_axisPos += samples[i] + radius * xdir;
m_radius += abs(radius);
m_radius += std::sqrt((radius - lineBOx) * (radius - lineBOx) + lineBOy * lineBOy);
++pointCount;
}
if(!pointCount)
return false;
m_axisPos /= pointCount;
m_radius /= pointCount * 2;
if(m_radius > 1e6)
return false;
// find point on axis closest to origin
float lambda = m_axisDir.dot(-m_axisPos);
m_axisPos = m_axisPos + lambda * m_axisDir;
m_hcs.FromNormal(m_axisDir);
m_angularRotatedRadians = 0;
return true;
}