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;
}
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;
}
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;
}
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();
}
}
}
