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