void Components(const MiscLib::Vector< char > &bitmap,
size_t uextent, size_t vextent, bool uwrap, bool vwrap,
MiscLib::Vector< int > *componentsImg,
MiscLib::Vector< std::pair< int, size_t > > *labels)
{
componentsImg->resize(uextent * vextent);
MiscLib::Vector< std::pair< int, size_t > > tempLabels;
tempLabels.reserve(componentsImg->size() / 2 + 1); // this is the maximum of possible tempLabels
tempLabels.push_back(std::make_pair(0, size_t(0)));
// use an eight neighborhood
// first row is special
// first pixel is special
// wrapping does not make sense in the first row: no pixels have been
// assigned components yet
int curLabel = 0;
if(bitmap[0])
{
(*componentsImg)[0] = ++curLabel;
tempLabels.push_back(std::make_pair(curLabel, size_t(1)));
}
else
{
(*componentsImg)[0] = 0;
++tempLabels[0].second;
}
// handle first row
for(size_t i = 1; i < uextent; ++i)
{
// in the first row only the previous pixel has to be considered
if(bitmap[i])
{
if((*componentsImg)[i - 1])
{
(*componentsImg)[i] = (*componentsImg)[i - 1];
++tempLabels[(*componentsImg)[i]].second;
}
else
{
(*componentsImg)[i] = ++curLabel;
tempLabels.push_back(std::make_pair(curLabel, size_t(1)));
}
}
else
{
(*componentsImg)[i] = 0;
++tempLabels[0].second;
}
}
size_t prevRow, row = 0;
size_t jend = vwrap? vextent - 1 : vextent;
for(size_t j = 1; j < jend; ++j)
{
prevRow = row;
row = prevRow + uextent;
// first pixel in row gets different treatment
if(bitmap[row])
{
if((*componentsImg)[prevRow]) // pixel above in component?
{
(*componentsImg)[row] = (*componentsImg)[prevRow];
++tempLabels[(*componentsImg)[row]].second;
}
else
{
int n[2];
n[0] = (*componentsImg)[prevRow + 1];
if(uwrap)
n[1] = (*componentsImg)[prevRow + uextent - 1];
(*componentsImg)[row] = Label(n, uwrap? 2 : 1, &curLabel,
&tempLabels);
}
}
else
{
(*componentsImg)[row] = 0;
++tempLabels[0].second;
}
for(size_t i = 1; i < uextent - 1; ++i)
{
if(!bitmap[row + i])
{
(*componentsImg)[row + i] = 0;
++tempLabels[0].second;
continue;
}
int n[4];
n[0] = (*componentsImg)[row + i - 1];
n[1] = (*componentsImg)[prevRow + i - 1];
n[2] = (*componentsImg)[prevRow + i];
n[3] = (*componentsImg)[prevRow + i + 1];
(*componentsImg)[row + i] = Label(n, 4, &curLabel, &tempLabels);
}
// last pixel in the row
if(!bitmap[row + uextent - 1])
{
(*componentsImg)[row + uextent - 1] = 0;
++tempLabels[0].second;
continue;
}
int n[5];
n[0] = (*componentsImg)[row + uextent - 2];
n[1] = (*componentsImg)[prevRow + uextent - 2];
n[2] = (*componentsImg)[prevRow + uextent - 1];
if(uwrap)
{
n[3] = (*componentsImg)[prevRow];
n[4] = (*componentsImg)[row];
}
(*componentsImg)[row + uextent - 1] = Label(n, uwrap? 5 : 3, &curLabel,
&tempLabels);
}
// last row
if(vwrap) // in case of vwrapping the last row is computed with almost full
// neighborhood
{
prevRow = (vextent - 2) * uextent;
row = (vextent - 1) * uextent;
// first pixel
if(bitmap[row])
{
int n[6];
n[0] = (*componentsImg)[prevRow];
n[1] = (*componentsImg)[prevRow + 1];
n[2] = (*componentsImg)[0];
n[3] = (*componentsImg)[1];
if(uwrap)
{
n[4] = (*componentsImg)[prevRow + uextent - 1];
n[5] = (*componentsImg)[uextent - 1];
}
(*componentsImg)[row] = Label(n, uwrap? 6 : 4, &curLabel,
&tempLabels);
}
else
{
(*componentsImg)[row] = 0;
++tempLabels[0].second;
}
for(size_t i = 1; i < uextent - 1; ++i)
{
if(!bitmap[row + i])
{
(*componentsImg)[row + i] = 0;
++tempLabels[0].second;
continue;
}
int n[7];
n[0] = (*componentsImg)[row + i - 1];
n[1] = (*componentsImg)[prevRow + i - 1];
n[2] = (*componentsImg)[prevRow + i];
n[3] = (*componentsImg)[prevRow + i + 1];
n[4] = (*componentsImg)[i - 1];
n[5] = (*componentsImg)[i];
n[6] = (*componentsImg)[i + 1];
(*componentsImg)[row + i] = Label(n, 7, &curLabel, &tempLabels);
}
// last pixel
if(!bitmap[row + uextent - 1])
{
(*componentsImg)[row + uextent - 1] = 0;
++tempLabels[0].second;
}
else
{
int n[8];
n[0] = (*componentsImg)[row + uextent - 2];
n[1] = (*componentsImg)[prevRow + uextent - 2];
n[2] = (*componentsImg)[prevRow + uextent - 1];
n[3] = (*componentsImg)[uextent - 2];
n[4] = (*componentsImg)[uextent - 1];
if(uwrap)
{
n[5] = (*componentsImg)[prevRow];
n[6] = (*componentsImg)[row];
n[7] = (*componentsImg)[0];
}
(*componentsImg)[row + uextent - 1] = Label(n, uwrap? 8 : 5,
&curLabel, &tempLabels);
}
}
// reduce the tempLabels
for(size_t i = tempLabels.size() - 1; i > 0; --i)
tempLabels[i].first = ReduceLabel(i, tempLabels);
MiscLib::Vector< int > condensed(tempLabels.size());
labels->clear();
labels->reserve(condensed.size());
int count = 0;
for(size_t i = 0; i < tempLabels.size(); ++i)
if(i == (size_t)tempLabels[i].first)
{
labels->push_back(std::make_pair(count, tempLabels[i].second));
condensed[i] = count;
++count;
}
else
(*labels)[condensed[tempLabels[i].first]].second
+= tempLabels[i].second;
// set new component ids
for(size_t i = 0; i < componentsImg->size(); ++i)
(*componentsImg)[i] =
condensed[tempLabels[(*componentsImg)[i]].first];
}
void ErodeCross(const MiscLib::Vector< char > &bitmap,
size_t uextent, size_t vextent, bool uwrap, bool vwrap,
MiscLib::Vector< char > *eroded)
{
// first pixel is special
(*eroded)[0] = bitmap[0] && bitmap[1] &&
bitmap[uextent];
if(vwrap)
(*eroded)[0] = (*eroded)[0] &&
bitmap[(vextent - 1) * uextent];
if(uwrap)
(*eroded)[0] = (*eroded)[0] && bitmap[uextent - 1];
// first row is special
if(!vwrap)
{
for(size_t i = 1; i < uextent - 1; ++i)
{
(*eroded)[i] = bitmap[i - 1] && bitmap[i] && bitmap[i + 1] &&
bitmap[uextent + i];
}
}
else
{
for(size_t i = 1; i < uextent - 1; ++i)
{
(*eroded)[i] = bitmap[i - 1] && bitmap[i] && bitmap[i + 1] &&
bitmap[uextent + i] && bitmap[(vextent - 1) * uextent + i];
}
}
// last pixel of first row is special
(*eroded)[uextent - 1] = bitmap[uextent - 1] && bitmap[uextent - 2] &&
bitmap[2 * uextent - 1];
if(vwrap)
(*eroded)[uextent - 1] = (*eroded)[uextent - 1] &&
bitmap[vextent * uextent - 1];
if(uwrap)
(*eroded)[uextent - 1] = (*eroded)[uextent - 1] && bitmap[0];
size_t row = 0, prevRow, nextRow = uextent;
for(size_t j = 1; j < vextent - 1; ++j)
{
prevRow = row;
row = nextRow;
nextRow = row + uextent;
// first pixel in row is special
(*eroded)[row] = bitmap[prevRow] &&
bitmap[row] && bitmap[row + 1] && bitmap[nextRow];
if(uwrap)
(*eroded)[row] = (*eroded)[row] && bitmap[nextRow - 1];
for(size_t i = 1; i < uextent - 1; ++i)
{
(*eroded)[row + i] = bitmap[prevRow + i ] &&
bitmap[row + i - 1] && bitmap[row + i] &&
bitmap[row + i + 1] && bitmap[nextRow + i];
}
// last pixel in row is special
(*eroded)[row + uextent - 1] = bitmap[prevRow + uextent - 1] &&
bitmap[row + uextent - 2] && bitmap[row + uextent - 1] &&
bitmap[nextRow + uextent - 1];
if(uwrap)
(*eroded)[row + uextent - 1] = (*eroded)[row + uextent - 1] &&
bitmap[row];
}
// first pixel of last row is special
(*eroded)[(vextent - 1) * uextent] = bitmap[(vextent - 1) * uextent] &&
bitmap[(vextent - 1) * uextent + 1] &&
bitmap[(vextent - 2) * uextent];
if(vwrap)
(*eroded)[(vextent - 1) * uextent] =
(*eroded)[(vextent - 1) * uextent] && bitmap[0];
if(uwrap)
(*eroded)[(vextent - 1) * uextent] =
(*eroded)[(vextent - 1) * uextent] &&
bitmap[vextent * uextent - 1];
// last row is special
if(!vwrap)
{
for(size_t i = 1; i < uextent - 1; ++i)
{
(*eroded)[(vextent - 1) * uextent + i] =
bitmap[(vextent - 1) * uextent + i] &&
bitmap[(vextent - 1) * uextent + i - 1] &&
bitmap[(vextent - 1) * uextent + i + 1] &&
bitmap[(vextent - 2) * uextent + i];
}
}
else
{
for(size_t i = 1; i < uextent - 1; ++i)
{
(*eroded)[(vextent - 1) * uextent + i] =
bitmap[(vextent - 1) * uextent + i] &&
bitmap[(vextent - 1) * uextent + i - 1] &&
bitmap[(vextent - 1) * uextent + i + 1] &&
bitmap[(vextent - 2) * uextent + i] &&
bitmap[i];
}
}
// last pixel
(*eroded)[bitmap.size() - 1] = bitmap[bitmap.size() - 1] &&
bitmap[bitmap.size() - 2] && bitmap[bitmap.size() - uextent - 1];
if(vwrap)
(*eroded)[bitmap.size() - 1] = (*eroded)[bitmap.size() - 1] &&
bitmap[uextent - 1];
if(uwrap)
(*eroded)[bitmap.size() - 1] = (*eroded)[bitmap.size() - 1] &&
bitmap[bitmap.size() - uextent];
}
void PreWrappedComponents(const MiscLib::Vector< char > &bitmap, size_t uextent,
size_t vextent, MiscLib::Vector< int > *preWrappedComponentsImg,
MiscLib::Vector< int > *relabelComponentsImg,
const MiscLib::Vector< std::pair< int, size_t > > &inLabels,
MiscLib::Vector< std::pair< int, size_t > > *labels)
{
MiscLib::Vector< std::pair< int, size_t > > tempLabels(inLabels);
tempLabels.reserve(bitmap.size() / 2 + 1); // this is the maximum of possible tempLabels
if(!tempLabels.size())
tempLabels.push_back(std::make_pair(0, size_t(0)));
int curLabel = tempLabels.size() - 1;
size_t prevRow, row = 0, nextRow = uextent;
for(size_t j = 1; j < vextent - 1; ++j)
{
prevRow = row;
row = nextRow;
nextRow = row + uextent;
for(size_t i = 1; i < uextent - 1; ++i)
{
if(!bitmap[row + i])
{
(*preWrappedComponentsImg)[row + i] = 0;
++tempLabels[0].second;
continue;
}
// get neighborhood
int n[8];
n[0] = (*preWrappedComponentsImg)[prevRow + i - 1];
n[1] = (*preWrappedComponentsImg)[prevRow + i];
n[2] = (*preWrappedComponentsImg)[prevRow + i + 1];
n[3] = (*preWrappedComponentsImg)[row + i - 1];
n[4] = (*preWrappedComponentsImg)[row + i + 1];
n[5] = (*preWrappedComponentsImg)[nextRow + i - 1];
n[6] = (*preWrappedComponentsImg)[nextRow + i];
n[7] = (*preWrappedComponentsImg)[nextRow + i + 1];
(*preWrappedComponentsImg)[row + i] = Label(n, 8,
&curLabel, &tempLabels);
}
}
// reduce the tempLabels
for(size_t i = tempLabels.size() - 1; i > 0; --i)
tempLabels[i].first = ReduceLabel(i, tempLabels);
MiscLib::Vector< int > condensed(tempLabels.size());
labels->clear();
labels->reserve(condensed.size());
int count = 0;
for(size_t i = 0; i < tempLabels.size(); ++i)
if(i == (size_t)tempLabels[i].first)
{
labels->push_back(std::make_pair(count, tempLabels[i].second));
condensed[i] = count;
++count;
}
else
(*labels)[condensed[tempLabels[i].first]].second
+= tempLabels[i].second;
// set new component ids
for(size_t i = 0; i < preWrappedComponentsImg->size(); ++i)
(*preWrappedComponentsImg)[i] =
condensed[tempLabels[(*preWrappedComponentsImg)[i]].first];
for(size_t i = 0; i < relabelComponentsImg->size(); ++i)
(*relabelComponentsImg)[i] =
condensed[tempLabels[(*relabelComponentsImg)[i]].first];
}
size_t BitmapPrimitiveShape::ConnectedComponent(
const PointCloud &pc, float epsilon,
std::shared_ptr<MiscLib::Vector< size_t > >indices, bool doFiltering, float* borderRatio )
{
MiscLib::Vector< int > componentsImg;
MiscLib::Vector< std::pair< int, size_t > > labels;
BitmapInfo bitmapInfo;
if( AllConnectedComponents( pc, epsilon, bitmapInfo, indices, componentsImg, labels, doFiltering ) <= 1 )
return 0;
size_t size = indices->size();
MiscLib::Vector< size_t >::iterator begin = indices->begin();
// find the largest component
size_t maxComp = 1;
for(size_t i = 2; i < labels.size(); ++i)
if(labels[maxComp].second < labels[i].second)
maxComp = i;
GfxTL::AABox< GfxTL::Vector2Df > bbox;
bbox.Min() = GfxTL::Vector2Df(
std::numeric_limits< float >::infinity(),
std::numeric_limits< float >::infinity());
bbox.Max() = -bbox.Min();
// compute bbox and update indices
size_t offset = 0;
for(size_t i = 0; i < size; ++i)
{
if(componentsImg[bitmapInfo.bmpIdx[i]] == labels[maxComp].first)
{
std::swap(begin[offset], begin[i]);
offset++;
// update bounding box
if(bbox.Min()[0] > bitmapInfo.params[i].first)
bbox.Min()[0] = bitmapInfo.params[i].first;
if(bbox.Max()[0] < bitmapInfo.params[i].first)
bbox.Max()[0] = bitmapInfo.params[i].first;
if(bbox.Min()[1] > bitmapInfo.params[i].second)
bbox.Min()[1] = bitmapInfo.params[i].second;
if(bbox.Max()[1] < bitmapInfo.params[i].second)
bbox.Max()[1] = bitmapInfo.params[i].second;
}
}
// ratio between border and connected-comp size should be calculated if borderRatio is a valid pointer
if( borderRatio )
{
int borderPixels = 0;
int maxLabel = labels[maxComp].first;
int row = bitmapInfo.uextent;
int pos = 0;
char numNeighbours = 0;
int ccSize = 0;
// test neightbourhood for all bitmappixels that are not marginal
for( size_t v = 1; v < bitmapInfo.vextent-1; ++v )
{
for( size_t u = 1; u < bitmapInfo.uextent-1; ++u )
{
pos = row + u;
if( componentsImg[pos] == maxLabel )
{
ccSize++;
numNeighbours = bitmapInfo.bitmap[pos-1] + bitmapInfo.bitmap[pos+1] +
bitmapInfo.bitmap[pos-bitmapInfo.uextent-1] + bitmapInfo.bitmap[pos-bitmapInfo.uextent+1] +
bitmapInfo.bitmap[pos+bitmapInfo.uextent-1] + bitmapInfo.bitmap[pos+bitmapInfo.uextent+1] +
bitmapInfo.bitmap[pos-bitmapInfo.uextent] + bitmapInfo.bitmap[pos+bitmapInfo.uextent];
if( (int)numNeighbours != 8 )
++borderPixels;
}
}
row += bitmapInfo.uextent;
}
// check left/right margins
row = bitmapInfo.uextent;
for( size_t v = 1; v < bitmapInfo.vextent-1; ++v )
{
ccSize++;
if( componentsImg[row] == maxLabel )
++borderPixels;
ccSize++;
if( componentsImg[row+bitmapInfo.uextent-1] == maxLabel )
++borderPixels;
row += bitmapInfo.uextent;
}
// check top/bottom margins
row = ( bitmapInfo.vextent-1 ) * bitmapInfo.uextent;
for( size_t u = 0; u < bitmapInfo.uextent; ++u )
{
ccSize++;
if( componentsImg[u] == maxLabel )
++borderPixels;
ccSize++;
if( componentsImg[row + u] == maxLabel )
++borderPixels;
}
*borderRatio = static_cast<float>( borderPixels ) / static_cast<float>( ccSize );
}
m_extBbox = bbox;
return offset;
}
bool Plane::Init(const MiscLib::Vector< Vec3f > &samples)
{
if(samples.size() < 6)
return false;
return Init(samples[0], samples[1], samples[2]);
}
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_TYPES::POINT_CLOUD))
{
m_app->dispToConsole("Select a real point cloud!",ccMainAppInterface::ERR_CONSOLE_MESSAGE);
return;
}
ccPointCloud* pc = static_cast<ccPointCloud*>(ent);
//input cloud
unsigned count = pc->size();
bool hasNorms = pc->hasNormals();
CCVector3 bbMin, bbMax;
pc->getBoundingBox(bbMin,bbMax);
const CCVector3d& globalShift = pc->getGlobalShift();
double globalScale = pc->getGlobalScale();
//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<count; ++i)
{
const CCVector3* P = pc->getPoint(i);
Pt.pos[0] = static_cast<float>(P->x);
Pt.pos[1] = static_cast<float>(P->y);
Pt.pos[2] = static_cast<float>(P->z);
if (hasNorms)
{
const CCVector3& N = pc->getPointNormal(i);
Pt.normal[0] = static_cast<float>(N.x);
Pt.normal[1] = static_cast<float>(N.y);
Pt.normal[2] = static_cast<float>(N.z);
}
cloud.push_back(Pt);
}
//manually set bounding box!
Vec3f cbbMin,cbbMax;
cbbMin[0] = static_cast<float>(bbMin.x);
cbbMin[1] = static_cast<float>(bbMin.y);
cbbMin[2] = static_cast<float>(bbMin.z);
cbbMax[0] = static_cast<float>(bbMax.x);
cbbMax[1] = static_cast<float>(bbMax.y);
cbbMax[2] = static_cast<float>(bbMax.z);
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(.005f * scale); // set distance threshold to 0.5% of bounding box width
rsdDlg.bitmapEpsilonDoubleSpinBox->setValue(.01f * scale); // set bitmap resolution (= sampling resolution) to 1% of bounding box width
rsdDlg.supportPointsSpinBox->setValue(s_supportPoints);
rsdDlg.maxNormDevAngleSpinBox->setValue(s_maxNormalDev_deg);
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_maxNormalDev_deg = rsdDlg.maxNormDevAngleSpinBox->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 = static_cast<float>(rsdDlg.epsilonDoubleSpinBox->value());
ransacOptions.m_bitmapEpsilon = static_cast<float>(rsdDlg.bitmapEpsilonDoubleSpinBox->value());
ransacOptions.m_normalThresh = static_cast<float>(cos(rsdDlg.maxNormDevAngleSpinBox->value() * CC_DEG_TO_RAD));
assert( ransacOptions.m_normalThresh >= 0 );
ransacOptions.m_probability = static_cast<float>(rsdDlg.probaDoubleSpinBox->value());
ransacOptions.m_minSupport = static_cast<unsigned>(rsdDlg.supportPointsSpinBox->value());
}
if (!hasNorms)
{
QProgressDialog pDlg("Computing normals (please wait)",QString(),0,0,m_app->getMainWindow());
pDlg.setWindowTitle("Ransac Shape Detection");
pDlg.show();
QApplication::processEvents();
cloud.calcNormals(.01f * scale);
if (pc->reserveTheNormsTable())
{
for (unsigned i=0; i<count; ++i)
{
Vec3f& Nvi = cloud[i].normal;
CCVector3 Ni = CCVector3::fromArray(Nvi);
//normalize the vector in case of
Ni.normalize();
pc->addNorm(Ni);
}
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());
unsigned 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 )
{
//progress dialog (Qtconcurrent::run can't be canceled!)
QProgressDialog pDlg("Operation in progress (please wait)",QString(),0,0,m_app->getMainWindow());
pDlg.setWindowTitle("Ransac Shape Detection");
pDlg.show();
QApplication::processEvents();
//run in a separate thread
s_detector = &detector;
s_shapes = &shapes;
s_cloud = &cloud;
QFuture<void> future = QtConcurrent::run(doDetection);
while (!future.isFinished())
{
#if defined(CC_WINDOWS)
::Sleep(500);
#else
usleep(500 * 1000);
#endif
pDlg.setValue(pDlg.value()+1);
QApplication::processEvents();
}
remaining = static_cast<unsigned>(s_remainingPoints);
pDlg.hide();
QApplication::processEvents();
}
//else
//{
// remaining = detector.Detect(cloud, 0, cloud.size(), &shapes);
//}
#if 0 //def _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;
size_t 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;
unsigned shapePointsCount = static_cast<unsigned>(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 (unsigned j=0; j<shapePointsCount; ++j)
{
pcShape->addPoint(CCVector3::fromArray(cloud[count-1-j].pos));
if (saveNormals)
pcShape->addNorm(CCVector3::fromArray(cloud[count-1-j].normal));
}
//random color
ccColor::Rgb col = ccColor::Generator::Random();
pcShape->setRGBColor(col);
pcShape->showColors(true);
pcShape->showNormals(saveNormals);
pcShape->setVisible(true);
pcShape->setGlobalShift(globalShift);
pcShape->setGlobalScale(globalScale);
//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 (unsigned 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/2);
G += Y * (minY+dY/2);
//we build matrix from these vectors
ccGLMatrix glMat( CCVector3::fromArray(X.getValue()),
CCVector3::fromArray(Y.getValue()),
CCVector3::fromArray(N.getValue()),
CCVector3::fromArray(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(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/2);
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::fromArray(X.getValue()),
CCVector3::fromArray(Y.getValue()),
CCVector3::fromArray(N.getValue()),
CCVector3::fromArray(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
Vec3f minP, maxP;
float minHeight, maxHeight;
minP = maxP = cloud[0].pos;
minHeight = maxHeight = cone->Internal().Height(cloud[0].pos);
for (size_t j=1; j<shapePointsCount; ++j)
{
float h = cone->Internal().Height(cloud[j].pos);
if (h < minHeight)
{
minHeight = h;
minP = cloud[j].pos;
}
else if (h > maxHeight)
{
maxHeight = h;
maxP = cloud[j].pos;
}
}
pcShape->setName(QString("Cone (alpha=%1/h=%2)").arg(alpha,0,'f').arg(maxHeight-minHeight,0,'f'));
float minRadius = tan(alpha)*minHeight;
float maxRadius = tan(alpha)*maxHeight;
//let's build the cone primitive
{
//the bottom should be the largest part so we inverse the axis direction
CCVector3 Z = -CCVector3::fromArray(CA.getValue());
Z.normalize();
//the center is halfway between the min and max height
float midHeight = (minHeight + maxHeight)/2;
CCVector3 C = CCVector3::fromArray((CC + CA * midHeight).getValue());
//radial axis
CCVector3 X = CCVector3::fromArray((maxP - (CC + maxHeight * CA)).getValue());
X.normalize();
//orthogonal radial axis
CCVector3 Y = Z * X;
//we build the transformation matrix from these vecctors
ccGLMatrix glMat(X,Y,Z,C);
//eventually create the cone primitive
prim = new ccCone(maxRadius, minRadius, maxHeight-minHeight, 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::fromArray(CA.getValue());
CCVector3 C = CCVector3::fromArray(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()));
group->addChild(pcShape);
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());
m_app->addToDB(group);
m_app->refreshAll();
}
}
}
bool Cone::InitAverage(const MiscLib::Vector< Vec3f > &samples)
{
// setup all the planes
size_t c = samples.size() / 2;
MiscLib::Vector< GfxTL::Vector4Df > planes(c);
#pragma omp parallel for schedule(static)
for (int i = 0; i < static_cast<int>(c); ++i)
{
for(unsigned int j = 0; j < 3; ++j)
planes[i][j] = samples[i][j];
planes[i][3] = samples[i].dot(samples[i + c]);
}
// compute center by intersecting the three planes given by (p1, n1)
// (p2, n2) and (p3, n3)
// set up linear system
double a[4 * 3];
double d1 = samples[0].dot(samples[c + 0]);
double d2 = samples[1].dot(samples[c + 1]);
double d3 = samples[2].dot(samples[c + 2]);
// column major
a[0 + 0 * 3] = samples[c + 0][0];
a[1 + 0 * 3] = samples[c + 1][0];
a[2 + 0 * 3] = samples[c + 2][0];
a[0 + 1 * 3] = samples[c + 0][1];
a[1 + 1 * 3] = samples[c + 1][1];
a[2 + 1 * 3] = samples[c + 2][1];
a[0 + 2 * 3] = samples[c + 0][2];
a[1 + 2 * 3] = samples[c + 1][2];
a[2 + 2 * 3] = samples[c + 2][2];
a[0 + 3 * 3] = d1;
a[1 + 3 * 3] = d2;
a[2 + 3 * 3] = d3;
if(dmat_solve(3, 1, a))
return false;
m_center[0] = a[0 + 3 * 3];
m_center[1] = a[1 + 3 * 3];
m_center[2] = a[2 + 3 * 3];
LevMarPlaneDistance planeDistance;
LevMar(planes.begin(), planes.end(), planeDistance,
(float *)m_center);
MiscLib::Vector< GfxTL::Vector3Df > spoints(c);
#pragma omp parallel for schedule(static)
for (int i = 0; i < static_cast<int>(c); ++i)
{
spoints[i] = GfxTL::Vector3Df(samples[i] - m_center);
spoints[i].Normalize();
}
GfxTL::Vector3Df axisDir;
GfxTL::MeanOfNormals(spoints.begin(), spoints.end(), &axisDir);
m_axisDir = GfxTL::Vector3Df(axisDir);
// make sure axis points in good direction
// the axis is defined to point into the interior of the cone
float heightSum = 0;
#pragma omp parallel for schedule(static) reduction(+:heightSum)
for(int i = 0; i < static_cast<int>(c); ++i)
heightSum += Height(samples[i]);
if(heightSum < 0)
m_axisDir *= -1;
float angleReduction = 0;
#pragma omp parallel for schedule(static) reduction(+:angleReduction)
for(int i = 0; i < static_cast<int>(c); ++i)
{
float angle = m_axisDir.dot(samples[i + c]);
if(angle < -1) // clamp angle to [-1, 1]
angle = -1;
else if(angle > 1)
angle = 1;
if(angle < 0)
// m_angle = omega + 90
angle = std::acos(angle) - float(M_PI) / 2;
else
// m_angle = 90 - omega
angle = float(M_PI) / 2 - std::acos(angle);
angleReduction += angle;
}
angleReduction /= c;
m_angle = angleReduction;
if(m_angle < 1.0e-6 || m_angle > float(M_PI) / 2 - 1.0e-6)
return false;
//if(m_angle > 1.3962634015954636615389526147909) // 80 degrees
if(m_angle > 1.4835298641951801403851371532153f) // 85 degrees
return false;
m_normal = Vec3f(std::cos(-m_angle), std::sin(-m_angle), 0);
m_normalY = m_normal[1] * m_axisDir;
m_n2d[0] = std::cos(m_angle);
m_n2d[1] = -std::sin(m_angle);
m_hcs.FromNormal(m_axisDir);
m_angularRotatedRadians = 0;
return true;
}
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;
}
