C++ Vector::begin方法代码示例

bool Plane::InitAverage(const MiscLib::Vector< Vec3f > &samples)
{
	if(samples.size() < 1)
		return false;
	m_normal = Vec3f(0, 0, 0);
	m_pos = Vec3f(0, 0, 0);
	size_t c = samples.size() / 2;
	MiscLib::Vector< GfxTL::Vector3Df > normals(c);
	for(intptr_t i = 0; i < c; ++i)
		normals[i] = GfxTL::Vector3Df(samples[i + c]);
	GfxTL::Vector3Df meanNormal;
	GfxTL::MeanOfNormals(normals.begin(), normals.end(), &meanNormal);
	m_normal = Vec3f(meanNormal.Data());
	GfxTL::Vector3Df mean;
	GfxTL::Mean(samples.begin(), samples.begin() + c, &mean);
	m_pos = Vec3f(mean.Data());
	m_dist = m_pos.dot(m_normal);
	return true;
}

void ConePrimitiveShape::BitmapExtent(float epsilon,
	GfxTL::AABox< GfxTL::Vector2Df > *bbox,
	MiscLib::Vector< std::pair< float, float > > *params,
	size_t *uextent, size_t *vextent)
{
	*uextent = std::ceil((bbox->Max()[0] - bbox->Min()[0]) / epsilon); // no wrappig along u direction
	*vextent = std::ceil((bbox->Max()[1] - bbox->Min()[1]) / epsilon) + 1; // add one for wrapping
	if((*vextent) * (*uextent) > 1e6 && m_cone.Angle() < float(M_PI / 4))
	{
		// try to reparameterize
		// try to find cut in the outer regions
		MiscLib::Vector< float > angularParams;//(params->size());
		angularParams.reserve(params->size());
		float outer = 3.f * std::max(abs(bbox->Min()[0]), abs(bbox->Max()[0])) / 4.f;
		for(size_t i = 0; i < params->size(); ++i)
			if((*params)[i].first > outer)
				angularParams.push_back(((*params)[i].second
					/ m_cone.RadiusAtLength((*params)[i].first)) + float(M_PI));
		std::sort(angularParams.begin(), angularParams.end());
		// try to find a large gap
		float maxGap = 0;
		float lower, upper;
		for(size_t i = 1; i < angularParams.size(); ++i)
		{
			float gap = angularParams[i] - angularParams[i - 1];
			if(gap > maxGap)
			{
				maxGap = gap;
				lower = angularParams[i - 1];
				upper = angularParams[i];
			}
		}
		// reparameterize with new angular cut
		float newCut = (lower + upper) / 2.f;
		m_cone.RotateAngularDirection(newCut);
		bbox->Min()[1] = std::numeric_limits< float >::infinity();
		bbox->Max()[1] = -std::numeric_limits< float >::infinity();
		for(size_t i = 0; i < params->size(); ++i)
		{
			float r = m_cone.RadiusAtLength((*params)[i].first);
			(*params)[i].second = ((*params)[i].second / r) + float(M_PI) - newCut;
			if((*params)[i].second < 0)
				(*params)[i].second = 2 * float(M_PI) + (*params)[i].second;
			(*params)[i].second = ((*params)[i].second - float(M_PI)) * r;
			if((*params)[i].second < bbox->Min()[1])
				bbox->Min()[1] = (*params)[i].second;
			if((*params)[i].second > bbox->Max()[1])
				bbox->Max()[1] = (*params)[i].second;
		}
		*vextent = std::floor((bbox->Max()[1] - bbox->Min()[1]) / epsilon) + 1;
	}
}

//.........这里部分代码省略.........
    fprintf(fp,"min support=%i\n",ransacOptions.m_minSupport);
    fprintf(fp,"probability=%f\n",ransacOptions.m_probability);

    fprintf(fp,"\n[Statistics]\n");
    fprintf(fp,"input points=%i\n",count);
    fprintf(fp,"segmented=%i\n",count-remaining);
    fprintf(fp,"remaining=%i\n",remaining);

    if (shapes.size()>0)
    {
        fprintf(fp,"\n[Shapes]\n");
        for (unsigned i=0; i<shapes.size(); ++i)
        {
            PrimitiveShape* shape = shapes[i].first;
            unsigned shapePointsCount = shapes[i].second;

            std::string desc;
            shape->Description(&desc);
            fprintf(fp,"#%i - %s - %i points\n",i+1,desc.c_str(),shapePointsCount);
        }
    }
    fclose(fp);
#endif

    if (remaining == count)
    {
        m_app->dispToConsole("Segmentation failed...",ccMainAppInterface::ERR_CONSOLE_MESSAGE);
        return;
    }

    if (shapes.size() > 0)
    {
        ccHObject* group = 0;
        for (MiscLib::Vector<DetectedShape>::const_iterator it = shapes.begin(); it != shapes.end(); ++it)
        {
            const PrimitiveShape* shape = it->first;
            size_t shapePointsCount = it->second;

            //too many points?!
            if (shapePointsCount > count)
            {
                m_app->dispToConsole("Inconsistent result!",ccMainAppInterface::ERR_CONSOLE_MESSAGE);
                break;
            }

            std::string desc;
            shape->Description(&desc);

            //new cloud for sub-part
            ccPointCloud* pcShape = new ccPointCloud(desc.c_str());

            //we fill cloud with sub-part points
            if (!pcShape->reserve((unsigned)shapePointsCount))
            {
                m_app->dispToConsole("Not enough memory!",ccMainAppInterface::ERR_CONSOLE_MESSAGE);
                delete pcShape;
                break;
            }
            bool saveNormals = pcShape->reserveTheNormsTable();

            for (size_t j=0; j<shapePointsCount; ++j)
            {
                pcShape->addPoint(CCVector3(cloud[count-1-j].pos));
                if (saveNormals)
                    pcShape->addNorm(cloud[count-1-j].normal);
            }

bool RansacShapeDetector::FindBestCandidate(CandidatesType &candidates,
	const MiscLib::Vector< ImmediateOctreeType * > &octrees, const PointCloud &pc,
	ScoreVisitorT &scoreVisitor, size_t currentSize,
	size_t drawnCandidates, size_t numInvalid, size_t minSize, float numLevels,
	float *maxForgottenCandidate, float *candidateFailProb) const
{
	if(!candidates.size())
		return false;
	size_t maxImproveSubsetDuringMaxSearch = octrees.size();
	// sort by expected value
	std::sort(candidates.begin(), candidates.end());
	// check if max is smaller than forgotten candidate
	if(candidates.size() && candidates.back().ExpectedValue() < *maxForgottenCandidate)
	{
		// drawn candidates is wrong!
		// need to correct the value
		drawnCandidates = std::max(candidates.size(), (size_t)1);
		*maxForgottenCandidate = 0;
	}

	MiscLib::Vector< Candidate * > candHeap;
	for(size_t i = candidates.size() - 1; i != -1; --i)
	{
		if(CandidateFailureProbability(
			candidates[i].ExpectedValue(),
			currentSize - numInvalid, drawnCandidates, numLevels) > m_options.m_probability)
			break;
		candHeap.push_back(&candidates[i]);
	}

	if(!candHeap.size())
	{
		return false;
	}

	std::make_heap(candHeap.begin(), candHeap.end(), CandidateHeapPred());

	MiscLib::Vector< Candidate * > beatenCands;
	Candidate *trial = candHeap.front();
	std::pop_heap(candHeap.begin(), candHeap.end(), CandidateHeapPred());
	candHeap.pop_back();
	float bestCandidateFailureProbability;
	while(candHeap.size())
	{
		if(trial->IsEquivalent(*candHeap.front(), pc, m_options.m_epsilon,
			m_options.m_normalThresh))
		{
			std::pop_heap(candHeap.begin(), candHeap.end(),
				CandidateHeapPred());
			candHeap.pop_back();
			continue;
		}
		bool isEquivalent = false;
		for(size_t j = 0; j < beatenCands.size(); ++j)
		{
			if(beatenCands[j]->IsEquivalent(*candHeap.front(), pc,
				m_options.m_epsilon, m_options.m_normalThresh))
			{
				isEquivalent = true;
				break;
			}
		}
		if(isEquivalent)
		{
			std::pop_heap(candHeap.begin(), candHeap.end(),
				CandidateHeapPred());
			candHeap.pop_back();
			continue;
		}
		bestCandidateFailureProbability = CandidateFailureProbability(
			trial->ExpectedValue(),
			currentSize - numInvalid, drawnCandidates, numLevels);
		while((bestCandidateFailureProbability <= m_options.m_probability)
			&& (*trial >= *candHeap.front())
			&& (trial->UpperBound() >= minSize)
			&& trial->ImproveBounds(octrees, pc, scoreVisitor, currentSize,
				m_options.m_bitmapEpsilon, octrees.size()))
		{
			bestCandidateFailureProbability = CandidateFailureProbability(
				trial->ExpectedValue(),
				currentSize - numInvalid, drawnCandidates, numLevels);
		}
		if(bestCandidateFailureProbability <= m_options.m_probability
			&& trial->UpperBound() >= minSize
			&& trial->ComputedSubsets() >= octrees.size()
			&& *trial >= *candHeap.front())
			break;
		if(bestCandidateFailureProbability <= m_options.m_probability
			&& trial->UpperBound() >= minSize)
		{
			candHeap.push_back(trial);
			std::push_heap(candHeap.begin(), candHeap.end(), CandidateHeapPred());
		}
		else if((int)trial->ComputedSubsets()
			> std::max(2, ((int)octrees.size()) - 2))
			beatenCands.push_back(trial);

		//nextCandidate
		trial = candHeap.front();
		std::pop_heap(candHeap.begin(), candHeap.end(), CandidateHeapPred());
//.........这里部分代码省略.........

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