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

void Candidate::Reindex(const MiscLib::Vector< int > &newIndices, int minInvalidIndex,
	size_t mergedSubsets, const MiscLib::Vector< size_t > &subsetSizes,
	const PointCloud &pc, size_t currentSize, float epsilon, float normalThresh,
	float bitmapEpsilon)
{
	size_t i = 0, j = 0;
	for(; i < m_indices->size(); ++i)
		if(newIndices[(*m_indices)[i]] < minInvalidIndex)
			(*m_indices)[j++] = newIndices[(*m_indices)[i]];
	if(m_subset <= mergedSubsets)
	{
		m_hasConnectedComponent = false;
		m_subset = 0;
		m_indices->clear();
		m_lowerBound = 0;
		m_upperBound = 0; // forget this candidate
		m_score = 0;
		return;
	}
	else
	{
		m_indices->resize(j);
		m_subset -= mergedSubsets;
		if(m_subset >= subsetSizes.size())
			// do connected component if all subsets have been computed
			ConnectedComponent(pc, bitmapEpsilon);		
	}
	size_t sampledPoints = 0,
		endi = std::min(m_subset, subsetSizes.size());;
	for(i = 0; i < endi; ++i)
		sampledPoints += subsetSizes[i];

	GetBounds(sampledPoints, currentSize);
}

bool Sphere::Init(const MiscLib::Vector< Vec3f > &samples)
{
    if(samples.size() < 4)
        return false;
    // get center
    size_t c = samples.size() / 2;
    m_center = Vec3f(0, 0, 0);
    size_t midCount = 0;
    for(size_t i = 0; i < c - 1; ++i)
        for(size_t j = i + 1; j < c; ++j)
        {
            Vec3f mid;
            if(!Midpoint(samples[i], samples[i + c], samples[j], samples[j + c], &mid))
                continue;
            m_center += mid;
            ++midCount;
        }
    if(!midCount)
        return false;
    m_center /= midCount;
    m_radius = 0;
    for(size_t i = 0; i < c; ++i)
    {
        float d = (samples[i] - m_center).length();
        m_radius += d;
    }
    m_radius /= c;
    return true;
}

bool Cone::Init(const MiscLib::Vector< Vec3f > &samples)
{
	if(samples.size() < 6)
		return false;
	size_t c = samples.size() >> 1;
	return Init(samples[0], samples[1], samples[2],
		samples[c], samples[c + 1], samples[c + 2]);
}

size_t BitmapPrimitiveShape::AllConnectedComponents(const PointCloud &pc, float epsilon,
	BitmapInfo& bitmapInfo, std::shared_ptr<MiscLib::Vector< size_t > >indices, MiscLib::Vector< int >& componentsImg, 
		MiscLib::Vector< std::pair< int, size_t > >& labels, bool doFiltering )
{
	// first find the extent in the parametrization
	// but remember the parametrized points for projection into a bitmap
	size_t size = indices->size();
	if(!size)
		return 0;

	// set up bitmap
	MiscLib::Vector< std::pair< float, float > > extParams;

	BuildBitmap(pc, &epsilon, indices->begin(), indices->end(), &bitmapInfo.params,
		&bitmapInfo.bbox, &bitmapInfo.bitmap, &bitmapInfo.uextent, &bitmapInfo.vextent, &bitmapInfo.bmpIdx);

	/*static int fname_int = 0;
	std::ostringstream fn;
	fn << "bitmapImg" << 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 << bitmap[j * uextent + i];
		file << std::endl;
	}
	file.close();*/

	// do a wrapping by copying pixels
	PreWrapBitmap(bitmapInfo.bbox, epsilon, bitmapInfo.uextent, bitmapInfo.vextent, &bitmapInfo.bitmap);

	MiscLib::Vector< char > tempBmp(bitmapInfo.bitmap.size()); // temporary bitmap object
	bool uwrap, vwrap;
	WrapBitmap(bitmapInfo.bbox, epsilon, &uwrap, &vwrap);

	if (doFiltering)
	{
		// closing
		DilateCross(bitmapInfo.bitmap, bitmapInfo.uextent, bitmapInfo.vextent, uwrap, vwrap, &tempBmp);
		ErodeCross(tempBmp, bitmapInfo.uextent, bitmapInfo.vextent, uwrap, vwrap, &bitmapInfo.bitmap);
		// opening
		//ErodeCross(bitmap, uextent, vextent, uwrap, vwrap, &tempBmp);
		//DilateCross(tempBmp, uextent, vextent, uwrap, vwrap, &bitmap);
	}

	Components(bitmapInfo.bitmap, bitmapInfo.uextent, bitmapInfo.vextent, uwrap, vwrap, &componentsImg,
		&labels);
	if(labels.size() <= 1) // found no connected component!
	{
		return 0; // associate no points with this shape
	}

	WrapComponents(bitmapInfo.bbox, epsilon, bitmapInfo.uextent, bitmapInfo.vextent, &componentsImg, &labels);

	return labels.size();
}

bool Cylinder::Init(const MiscLib::Vector< Vec3f > &samples)
{
	if(samples.size() < 4)
		return false;
	// estimate axis from all pairs
	m_axisDir = Vec3f(0, 0, 0);
	size_t c = samples.size() / 2;
	size_t axisCount = 0;
	m_axisDir = samples[0 + c].cross(samples[1 + c]);
	if(m_axisDir.normalize() < 1e-3)
		return false;
	m_axisPos = Vec3f(0, 0, 0);
	m_radius = 0;
	// project first normal into plane
	float l = m_axisDir.dot(samples[0 + c]);
	Vec3f xdir = samples[0 + 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[0] is the origin
	float lineBnx = ydir.dot(samples[1 + c]);
	if(abs(lineBnx) < 1e-6)
		return false;
	float lineBny = -xdir.dot(samples[1 + c]);
	// origin of lineB
	Vec3f originB = samples[1] - samples[0];
	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[0] + radius * xdir;
	m_radius += abs(radius);
	m_radius += std::sqrt((radius - lineBOx) * (radius - lineBOx) + lineBOy * lineBOy);
	m_radius /= 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;
}

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

void BitmapPrimitiveShape::BuildPolygons(const PointCloud &pc, float epsilon,
	size_t begin, size_t end, GfxTL::AABox< GfxTL::Vector2Df > *bbox,
	size_t *uextent, size_t *vextent,
	std::deque< ComponentPolygons > *polys) const
{
	// curves are extracted in the following way:
	// first the bitmap is constructed
	// then connected components are found
	// for each component the curves are found

	// constructing the bitmap is similar to ConnectedComponent
	// -> use the same code
	MiscLib::Vector< std::pair< float, float > > params;
	MiscLib::Vector< char > bitmap;
	MiscLib::Vector< size_t > bmpIdx;
	BuildBitmap(pc, &epsilon, IndexIterator(begin), IndexIterator(end), &params,
		bbox, &bitmap, uextent, vextent, &bmpIdx);

	// do closing
	MiscLib::Vector< char > tempBmp(bitmap.size());
	DilateCross(bitmap, *uextent, *vextent, false, false, &tempBmp);
	ErodeCross(tempBmp, *uextent, *vextent, false, false, &bitmap);

	// find connected components
	MiscLib::Vector< int > componentsImg;
	MiscLib::Vector< std::pair< int, size_t > > labels;
	Components(bitmap, *uextent, *vextent, false, false, &componentsImg,
		&labels);
	if(labels.size() <= 1) // found no connected component!
		return;

	// for each component find all polygons
	for(size_t i = 1; i < labels.size(); ++i)
	{
		polys->resize(polys->size() + 1);
		ComponentLoops(componentsImg, *uextent, *vextent, labels[i].first,
			false, false, &(*polys)[polys->size() - 1]);
	}
}

bool Sphere::Interpolate(const MiscLib::Vector< Sphere > &spheres,
                         const MiscLib::Vector< float > &weights, Sphere *is)
{
    Vec3f center(0, 0, 0);
    float radius = 0;
    for(size_t i = 0; i < spheres.size(); ++i)
    {
        center += weights[i] * spheres[i].Center();
        radius += weights[i] * spheres[i].Radius();
    }
    is->Center(center);
    is->Radius(radius);
    return true;
}

bool Plane::Interpolate(const MiscLib::Vector< Plane > &planes,
	const MiscLib::Vector< float > &weights, Plane *ip)
{
	Vec3f normal(0, 0, 0);
	Vec3f position(0, 0, 0);
	for(size_t i = 0; i < planes.size(); ++i)
	{
		normal += weights[i] * planes[i].getNormal();
		position += weights[i] * planes[i].getPosition();
	}
	normal.normalize();
	*ip = Plane(position, normal);
	return true;
}

bool Cylinder::Interpolate(const MiscLib::Vector< Cylinder > &cylinders,
	const MiscLib::Vector< float > &weights, Cylinder *ic)
{
	Vec3f axisPos(0, 0, 0);
	Vec3f axisDir(0, 0, 0);
	float r = 0;
	for(size_t i = 0; i < cylinders.size(); ++i)
	{
		axisPos += weights[i] * cylinders[i].AxisPosition();
		axisDir += weights[i] * cylinders[i].AxisDirection();
		r += weights[i] * cylinders[i].Radius();
	}
	axisDir.normalize();
	return ic->Init(axisDir, axisPos, r);
}

bool Cone::Interpolate(const MiscLib::Vector< Cone > &cones,
	const MiscLib::Vector< float > &weights, Cone *ic)
{
	Vec3f center(0, 0, 0);
	Vec3f axisDir(0, 0, 0);
	float omega = 0;
	for(size_t i = 0; i < cones.size(); ++i)
	{
		center += weights[i] * cones[i].Center();
		axisDir += weights[i] * cones[i].AxisDirection();
		omega += weights[i] * cones[i].Angle();
	}
	axisDir.normalize();
	return ic->Init(center, axisDir, omega);
}

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_POINT_CLOUD))
    {
        m_app->dispToConsole("Select a real point cloud!",ccMainAppInterface::ERR_CONSOLE_MESSAGE);
        return;
    }

    ccPointCloud* pc = static_cast<ccPointCloud*>(ent);

    //input cloud
    size_t count = (size_t)pc->size();
    bool hasNorms = pc->hasNormals();
    PointCoordinateType bbMin[3],bbMax[3];
    pc->getBoundingBox(bbMin,bbMax);

    //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<(unsigned)count; ++i)
        {
            const CCVector3* P = pc->getPoint(i);
            Pt.pos[0] = P->x;
            Pt.pos[1] = P->y;
            Pt.pos[2] = P->z;
            if (hasNorms)
            {
                const PointCoordinateType* N = pc->getPointNormal(i);
                Pt.normal[0] = N[0];
                Pt.normal[1] = N[1];
                Pt.normal[2] = N[2];
            }
            cloud.push_back(Pt);
        }

        //manually set bounding box!
        Vec3f cbbMin,cbbMax;
        cbbMin[0] = bbMin[0];
        cbbMin[1] = bbMin[1];
        cbbMin[2] = bbMin[2];
        cbbMax[0] = bbMax[0];
        cbbMax[1] = bbMax[1];
        cbbMax[2] = bbMax[2];
        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(.01f * scale);		// set distance threshold to .01f of bounding box width
    // NOTE: Internally the distance threshold is taken as 3 * ransacOptions.m_epsilon!!!
    rsdDlg.bitmapEpsilonDoubleSpinBox->setValue(.02f * scale);	// set bitmap resolution to .02f of bounding box width
    // NOTE: This threshold is NOT multiplied internally!
    rsdDlg.supportPointsSpinBox->setValue(s_supportPoints);
    rsdDlg.normThreshDoubleSpinBox->setValue(s_normThresh);
    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_normThresh = rsdDlg.normThreshDoubleSpinBox->value();
//.........这里部分代码省略.........

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

void RansacShapeDetector::GenerateCandidates(
	const IndexedOctreeType &globalOctree,
	const MiscLib::Vector< ImmediateOctreeType * > &octrees,
	const PointCloud &pc, ScoreVisitorT &scoreVisitor,
	size_t currentSize, size_t numInvalid,
	const MiscLib::Vector< double > &sampleLevelProbSum,
	size_t *drawnCandidates,
	MiscLib::Vector< std::pair< float, size_t > > *sampleLevelScores,
	float *bestExpectedValue,
	CandidatesType *candidates) const
{
	size_t genCands = 0;

#ifdef DOPARALLEL
	#pragma omp parallel
#endif
	{
	ScoreVisitorT scoreVisitorCopy(scoreVisitor);
#ifdef DOPARALLEL
	#pragma omp for schedule(dynamic, 10) reduction(+:genCands)
#endif
	for(int candIter = 0; candIter < 200; ++candIter)
	{
		// pick a sample level
		double s = ((double)rand()) / (double)RAND_MAX;
		size_t sampleLevel = 0;
		for(; sampleLevel < sampleLevelProbSum.size() - 1; ++sampleLevel)
			if(sampleLevelProbSum[sampleLevel] >= s)
				break;
		// draw samples on current sample level in octree
		MiscLib::Vector< size_t > samples;
		const IndexedOctreeType::CellType *node;
		if(!DrawSamplesStratified(globalOctree, m_reqSamples, sampleLevel,
			scoreVisitorCopy.GetShapeIndex(), &samples, &node))
			continue;
		++genCands;
		// construct the candidates
		size_t c = samples.size();
		MiscLib::Vector< Vec3f > samplePoints(samples.size() << 1);
		for(size_t i = 0; i < samples.size(); ++i)
		{
			samplePoints[i] = globalOctree.at(samples[i]).pos;
			samplePoints[i + c] = globalOctree.at(samples[i]).normal;
		}
		// construct the different primitive shapes
		PrimitiveShape *shape;
		for(ConstructorsType::const_iterator i = m_constructors.begin(),
			iend = m_constructors.end(); i != iend; ++i)
		{
			if((*i)->RequiredSamples() > samples.size() 
					|| !(shape = (*i)->Construct(samplePoints)))
				continue;
			// verify shape
			std::pair< float, float > dn;
			bool verified = true;
			for(size_t i = 0; i < c; ++i)
			{
				shape->DistanceAndNormalDeviation(samplePoints[i], samplePoints[i + c], &dn);
				if(!scoreVisitorCopy.PointCompFunc()(dn.first, dn.second))
				{
					verified = false;
					break;
				}
			}
			if(!verified)
			{
				shape->Release();
				continue;
			}
			Candidate cand(shape, node->Level());
			cand.Indices(new MiscLib::RefCounted< MiscLib::Vector< size_t > >);
			cand.Indices()->Release();
			shape->Release();
			cand.ImproveBounds(octrees, pc, scoreVisitorCopy,
				currentSize, m_options.m_bitmapEpsilon, 1);
			if(cand.UpperBound() < m_options.m_minSupport)
			{
#ifdef DOPARALLEL
				#pragma omp critical
#endif
				{
				(*sampleLevelScores)[node->Level()].first += cand.ExpectedValue();
				++(*sampleLevelScores)[node->Level()].second;
				}
				continue;
			}

#ifdef DOPARALLEL
			#pragma omp critical
#endif
			{
				(*sampleLevelScores)[node->Level()].first += cand.ExpectedValue();
				++(*sampleLevelScores)[node->Level()].second;
				candidates->push_back(cand);
				if(cand.ExpectedValue() > *bestExpectedValue)
					*bestExpectedValue = cand.ExpectedValue();
			}
		}
	}
	}
//.........这里部分代码省略.........