Exemplo n.º 1
0
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];
}
Exemplo n.º 2
0
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];
}
Exemplo n.º 3
0
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];
}
Exemplo n.º 4
0
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]);
}
Exemplo n.º 6
0
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,&param);
					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();		
		}
	}
}
Exemplo n.º 7
0
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;
}
Exemplo n.º 8
0
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;
}