Beispiel #1
0
bool ccGBLSensor::computeAutoParameters(CCLib::GenericCloud* theCloud)
{
	assert(theCloud);
	if (!theCloud)
	{
		//invalid input parameter
		return false;
	}

	std::vector<bool> nonEmptyAnglesYaw,nonEmptyAnglesPitch;
	try
	{
		nonEmptyAnglesYaw.resize(360,false);
		nonEmptyAnglesPitch.resize(360,false);
	}
	catch(std::bad_alloc)
	{
		//not enough memory
		return false;
	}
	//force no shift for auto search (we'll fix this later if necessary)
	m_yawAnglesAreShifted = false;
	m_pitchAnglesAreShifted = false;

	unsigned pointCount = theCloud->size();

	PointCoordinateType minPitch = 0, maxPitch = 0, minYaw = 0, maxYaw = 0;
	PointCoordinateType maxDepth = 0;
	{
		//first project all points to compute the (yaw,ptich) ranges
		theCloud->placeIteratorAtBegining();
		for (unsigned i=0; i<pointCount; ++i)
		{
			const CCVector3* P = theCloud->getNextPoint();
			CCVector2 Q;
			PointCoordinateType depth;
			//Q.x and Q.y are inside [-pi;pi] by default (result of atan2)
			projectPoint(*P,Q,depth,m_activeIndex);

			//yaw
			int angleYaw = static_cast<int>(Q.x * CC_RAD_TO_DEG);
			assert(angleYaw >= -180 && angleYaw <= 180);
			if (angleYaw == 180) //360 degrees warp
				angleYaw = -180;
			nonEmptyAnglesYaw[180 + angleYaw] = true;
			if (i != 0)
			{
				if (minYaw > Q.x)
					minYaw = Q.x;
				else if (maxYaw < Q.x)
					maxYaw = Q.x;
			}
			else
			{
				minYaw = maxYaw = Q.x;
			}

			//pitch
			int anglePitch = static_cast<int>(Q.y * CC_RAD_TO_DEG);
			assert(anglePitch >= -180 && anglePitch <= 180);
			if (anglePitch == 180)
				anglePitch = -180;
			nonEmptyAnglesPitch[180 + anglePitch] = true;
			if (i != 0)
			{
				if (minPitch > Q.y)
					minPitch = Q.y;
				else if (maxPitch < Q.y)
					maxPitch = Q.y;
			}
			else
			{
				minPitch = maxPitch = Q.y;
			}

			if (depth > maxDepth)
				maxDepth = depth;
		}
	}

	Interval bestEmptyPartYaw = Interval::FindBiggest<bool>(nonEmptyAnglesYaw,false,true);
	Interval bestEmptyPartPitch = Interval::FindBiggest<bool>(nonEmptyAnglesPitch,false,true);

	m_yawAnglesAreShifted = (bestEmptyPartYaw.start != 0 && bestEmptyPartYaw.span > 1 && bestEmptyPartYaw.start + bestEmptyPartYaw.span < 360);
	m_pitchAnglesAreShifted = (bestEmptyPartPitch.start != 0 && bestEmptyPartPitch.span > 1 && bestEmptyPartPitch.start + bestEmptyPartPitch.span < 360);

	if (m_yawAnglesAreShifted || m_pitchAnglesAreShifted)
	{
		//we re-project all the points in order to update the boundaries!
		theCloud->placeIteratorAtBegining();
		for (unsigned i = 0; i<pointCount; ++i)
		{
			const CCVector3 *P = theCloud->getNextPoint();
			CCVector2 Q;
			PointCoordinateType depth;
			projectPoint(*P,Q,depth,m_activeIndex);

			if (i != 0)
			{
				if (minYaw > Q.x)
					minYaw = Q.x;
				else if (maxYaw < Q.x)
					maxYaw = Q.x;

				if (minPitch > Q.y)
					minPitch = Q.y;
				else if (maxPitch < Q.y)
					maxPitch = Q.y;
			}
			else
			{
				minYaw = maxYaw = Q.x;
				minPitch = maxPitch = Q.y;
			}
		}
	}

	setYawRange(minYaw,maxYaw);
	setPitchRange(minPitch,maxPitch);
	setSensorRange(maxDepth);

	return true;
}
Beispiel #2
0
CCLib::SimpleCloud* ccGBLSensor::project(CCLib::GenericCloud* theCloud, int& errorCode, bool autoParameters/*false*/)
{
	assert(theCloud);

	CCLib::SimpleCloud* newCloud = new CCLib::SimpleCloud();

	unsigned pointCount = theCloud->size();
	if (!newCloud->reserve(pointCount) || !newCloud->enableScalarField()) //not enough memory
	{
		errorCode = -4;
		delete newCloud;
		return 0;
	}

	ScalarType maxDepth = 0;

	theCloud->placeIteratorAtBegining();
	{
		for (unsigned i=0; i<pointCount; ++i)
		{
			const CCVector3 *P = theCloud->getNextPoint();
			CCVector2 Q;
			ScalarType depth;
			projectPoint(*P,Q,depth);

			newCloud->addPoint(CCVector3(Q.x,Q.y,0));
			newCloud->setPointScalarValue(i,depth);

			if (i != 0)
				maxDepth = std::max(maxDepth,depth);
			else
				maxDepth = depth;
		}
	}

	if (autoParameters)
	{
		//dimensions = theta, phi, 0
		PointCoordinateType bbMin[3],bbMax[3];
		newCloud->getBoundingBox(bbMin,bbMax);

		setTheta(bbMin[0],bbMax[0]);
		setPhi(bbMin[1],bbMax[1]);
		setSensorRange(maxDepth);
	}

	//clear previous Z-buffer
	{
		if (m_depthBuffer.zBuff)
			delete[] m_depthBuffer.zBuff;
		m_depthBuffer.zBuff=0;
		m_depthBuffer.width=0;
		m_depthBuffer.height=0;
	}
	
	//init new Z-buffer
	{
		int width = static_cast<int>(ceil((thetaMax-thetaMin)/deltaTheta));
		int height = static_cast<int>(ceil((phiMax-phiMin)/deltaPhi));

		if (width*height == 0 || std::max(width,height) > 2048) //too small or... too big!
		{
			errorCode = -2;
			delete newCloud;
			return 0;
		}

		unsigned zBuffSize = width*height;
		m_depthBuffer.zBuff = new ScalarType[zBuffSize];
		if (!m_depthBuffer.zBuff) //not enough memory
		{
			errorCode = -4;
			delete newCloud;
			return 0;
		}
		m_depthBuffer.width = width;
		m_depthBuffer.height = height;
		memset(m_depthBuffer.zBuff,0,zBuffSize*sizeof(ScalarType));
	}

	//project points and accumulate them in Z-buffer
	newCloud->placeIteratorAtBegining();
	for (unsigned i=0; i<newCloud->size(); ++i)
	{
		const CCVector3 *P = newCloud->getNextPoint();
		ScalarType depth = newCloud->getPointScalarValue(i);

		int x = static_cast<int>(floor((P->x-thetaMin)/deltaTheta));
		int y = static_cast<int>(floor((P->y-phiMin)/deltaPhi));

		ScalarType& zBuf = m_depthBuffer.zBuff[y*m_depthBuffer.width+x];
		zBuf = std::max(zBuf,depth);
	}

	errorCode = 0;
	return newCloud;
}