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