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

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

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());
//.........这里部分代码省略.........

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];
//.........这里部分代码省略.........

//.........这里部分代码省略.........
			unsigned int edgeIndex[] = { x + 1, y, 0, 1 };
			if(edges.find(edgeIndex))
				continue;
			// on top of pixel
			prevx = -1;
			prevy = 0;
			++x;
		}
		else if(uwrap && !vwrap && x == 0 && (size_t)y == vextent - 1)
		{
			size_t dx, dy;
			if(!IsEdge(componentImg, uextent, vextent, label, uwrap, vwrap,
				x + 1, y + 1, -1, 0, &dx, &dy))
				continue;
			// on bottom of pixel
			// check if edge was visited already
			unsigned int edgeIndex[] = { x + 1, y + 1, 0, 1 };
			if(edges.find(edgeIndex))
				continue;
			prevx = -1;
			prevy = 0;
			++y;
		}
		else if(!uwrap && vwrap && (size_t)x == uextent - 1)
		{
			// on right of pixel
			size_t dx, dy;
			if(!IsEdge(componentImg, uextent, vextent, label, uwrap, vwrap,
				x + 1, y + 1, 0, -1, &dx, &dy))
				continue;
			// on bottom of pixel
			// check if edge was visited already
			unsigned int edgeIndex[] = { x + 1, y + 1, 1, 0 };
			if(edges.find(edgeIndex))
				continue;
			prevx = 0;
			prevy = 1;
			++y;
		}
		else
			continue; // we are unable to start a loop at this position
		poly.push_back(Vec2(x + prevx, y + prevy));
		edges[x][y][prevx + 1][prevy + 1] = true;
		do
		{
			poly.push_back(Vec2(x, y));
			// check the four neighbors of (x, y) from left to right
			// starting from where we came from
			// we take the first edge that we encounter
			size_t nextx, nexty;
			size_t checkEdge;
			for(checkEdge = 0; checkEdge < 3; ++checkEdge)
			{
				std::swap(prevx, prevy);
				prevx *= -1;
				if(IsEdge(componentImg, uextent, vextent, label, uwrap, vwrap,
					x, y, prevx, prevy, &nextx, &nexty))
					break;
			}
			if(checkEdge > 3)
				return;
			x = nextx;
			y = nexty;
			prevx = -prevx;
			prevy = -prevy;
			edges[x][y][prevx + 1][prevy + 1] = true;
		}
		while(poly[0] != Vec2(x, y));
		polys->push_back(poly);
	}
	while(firsti < componentImg.size());
#ifdef _DEBUG
	static int fname_int = 0;
	std::ostringstream fn;
	fn << "ComponentLoopsInput" << fname_int << ".txt";
	std::ofstream file;
	file.open(fn.str().c_str(), std::ios::out);
	for(size_t j = 0; j < vextent; ++j)
	{
		for(size_t i = 0; i < uextent; ++i)
			file /*<< std::setw(3)*/ << componentImg[j * uextent + i]/* << " "*/;
		file << std::endl;
	}
	file.close();
	MiscLib::Vector< int > loopsImg((uextent + 1) * (vextent + 1), 0);
	std::ostringstream fn2;
	fn2 << "ComponentLoopsOutput" << fname_int++ << ".txt";
	for(size_t i = 0; i < polys->size(); ++i)
		for(size_t j = 0; j < (*polys)[i].size(); ++j)
			loopsImg[(*polys)[i][j][1] * (uextent + 1) + (*polys)[i][j][0]] = i + 1;
	file.open(fn2.str().c_str(), std::ios::out);
	for(size_t j = 0; j < vextent + 1; ++j)
	{
		for(size_t i = 0; i < uextent + 1; ++i)
			file /*<< std::setw(3)*/ << loopsImg[j * (uextent + 1) + i]/* << " "*/;
		file << std::endl;
	}
	file.close();
#endif
}

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