void ccGBLSensor::projectPoint( const CCVector3& sourcePoint,
CCVector2& destPoint,
PointCoordinateType &depth,
double posIndex/*=0*/) const
{
//project point in sensor world
CCVector3 P = sourcePoint;
//sensor to world global transformation = sensor position * rigid transformation
ccIndexedTransformation sensorPos; //identity by default
if (m_posBuffer)
m_posBuffer->getInterpolatedTransformation(posIndex,sensorPos);
sensorPos *= m_rigidTransformation;
//apply (inverse) global transformation (i.e world to sensor)
sensorPos.inverse().apply(P);
//convert to 2D sensor field of view + compute its distance
switch (m_rotationOrder)
{
case YAW_THEN_PITCH:
{
//yaw = angle around Z, starting from 0 in the '+X' direction
destPoint.x = atan2(P.y,P.x);
//pitch = angle around the lateral axis, between -pi (-Z) to pi (+Z) by default
destPoint.y = atan2(P.z,sqrt(P.x*P.x + P.y*P.y));
break;
}
case PITCH_THEN_YAW:
{
//FIXME
//yaw = angle around Z, starting from 0 in the '+X' direction
destPoint.x = -atan2(sqrt(P.y*P.y + P.z*P.z),P.x);
//pitch = angle around the lateral axis, between -pi (-Z) to pi (+Z) by default
destPoint.y = -atan2(P.y,P.z);
break;
}
default:
assert(false);
}
//if the yaw angles are shifted
if (m_yawAnglesAreShifted && destPoint.x < 0)
destPoint.x += static_cast<PointCoordinateType>(2.0*M_PI);
//if the pitch angles are shifted
if (m_pitchAnglesAreShifted && destPoint.y < 0)
destPoint.y += static_cast<PointCoordinateType>(2.0*M_PI);
depth = P.norm();
}
bool GeometricalAnalysisTools::computeSphereFrom4( const CCVector3& A,
const CCVector3& B,
const CCVector3& C,
const CCVector3& D,
CCVector3& center,
PointCoordinateType& radius )
{
//inspired from 'tetrahedron_circumsphere_3d' by Adrian Bowyer and John Woodwark
//Set up the linear system.
double a[12];
{
CCVector3 AB = B-A;
a[0] = AB.x;
a[3] = AB.y;
a[6] = AB.z;
a[9] = AB.norm2d();
}
{
CCVector3 AC = C-A;
a[1] = AC.x;
a[4] = AC.y;
a[7] = AC.z;
a[10] = AC.norm2d();
}
{
CCVector3 AD = D-A;
a[2] = AD.x;
a[5] = AD.y;
a[8] = AD.z;
a[11] = AD.norm2d();
}
// Solve the linear system (with Gauss-Jordan elimination)
if ( dmat_solve ( 3, 1, a ) != 0 )
{
//system is singular?
return false;
}
// Compute the radius and center.
CCVector3 u = CCVector3(static_cast<PointCoordinateType>(a[0+3*3]),
static_cast<PointCoordinateType>(a[1+3*3]),
static_cast<PointCoordinateType>(a[2+3*3])) / 2;
radius = u.norm();
center = A + u;
return true;
}
void cc2DLabel::getLabelInfo3(LabelInfo3& info) const
{
info.cloud1 = info.cloud2 = info.cloud3 = 0;
if (m_points.size() != 3)
return;
//1st point
info.cloud1 = m_points[0].cloud;
info.point1Index = m_points[0].index;
const CCVector3* P1 = info.cloud1->getPointPersistentPtr(info.point1Index);
//2nd point
info.cloud2 = m_points[1].cloud;
info.point2Index = m_points[1].index;
const CCVector3* P2 = info.cloud2->getPointPersistentPtr(info.point2Index);
//3rd point
info.cloud3 = m_points[2].cloud;
info.point3Index = m_points[2].index;
const CCVector3* P3 = info.cloud3->getPointPersistentPtr(info.point3Index);
//area
CCVector3 P1P2 = *P2-*P1;
CCVector3 P1P3 = *P3-*P1;
CCVector3 P2P3 = *P3-*P2;
CCVector3 N = P1P2.cross(P1P3); //N = ABxAC
info.area = N.norm()/2;
//normal
N.normalize();
info.normal = N;
//edges length
info.edges.u[0] = P1P2.norm2d(); //edge 1-2
info.edges.u[1] = P2P3.norm2d(); //edge 2-3
info.edges.u[2] = P1P3.norm2d(); //edge 3-1
//angle
info.angles.u[0] = GetAngle_deg(P1P2,P1P3); //angleAtP1
info.angles.u[1] = GetAngle_deg(P2P3,-P1P2); //angleAtP2
info.angles.u[2] = GetAngle_deg(-P1P3,-P2P3); //angleAtP3 (should be equal to 180-a1-a2!)
}
SimpleCloud* MeshSamplingTools::samplePointsOnMesh(GenericMesh* theMesh,
double samplingDensity,
unsigned theoricNumberOfPoints,
GenericProgressCallback* progressCb,
GenericChunkedArray<1,unsigned>* triIndices/*=0*/)
{
assert(theMesh);
unsigned triCount = (theMesh ? theMesh->size() : 0);
if (triCount==0)
return 0;
if (theoricNumberOfPoints < 1)
return 0;
SimpleCloud* sampledCloud = new SimpleCloud();
if (!sampledCloud->reserve(theoricNumberOfPoints)) //not enough memory
{
delete sampledCloud;
return 0;
}
if (triIndices)
{
triIndices->clear();
//not enough memory? DGM TODO: we should warn the caller
if (!triIndices->reserve(theoricNumberOfPoints) || triIndices->capacity() < theoricNumberOfPoints)
{
delete sampledCloud;
triIndices->clear();
return 0;
}
}
NormalizedProgress* normProgress=0;
if(progressCb)
{
normProgress = new NormalizedProgress(progressCb,triCount);
progressCb->setMethodTitle("Mesh sampling");
char buffer[256];
sprintf(buffer,"Triangles: %i\nPoints: %i",triCount,theoricNumberOfPoints);
progressCb->setInfo(buffer);
progressCb->reset();
progressCb->start();
}
unsigned addedPoints=0;
//for each triangle
theMesh->placeIteratorAtBegining();
for (unsigned n=0;n<triCount;++n)
{
const GenericTriangle* tri = theMesh->_getNextTriangle();
//summits (OAB)
const CCVector3 *O = tri->_getA();
const CCVector3 *A = tri->_getB();
const CCVector3 *B = tri->_getC();
//edges (OA and OB)
CCVector3 u = *A - *O;
CCVector3 v = *B - *O;
//we compute the (twice) the triangle area
CCVector3 N = u.cross(v);
double S = N.norm()/2.0;
//we deduce the number of points to generate on this face
double fPointsToAdd = S*samplingDensity;
unsigned pointsToAdd = static_cast<unsigned>(fPointsToAdd);
//if the face area is smaller than the surface/random point
if (pointsToAdd == 0)
{
//we add a point with the same probability as its (relative) area
if (static_cast<double>(rand()) <= fPointsToAdd * static_cast<double>(RAND_MAX))
pointsToAdd = 1;
}
if (pointsToAdd)
{
if (addedPoints + pointsToAdd >= theoricNumberOfPoints)
{
theoricNumberOfPoints+=pointsToAdd;
if (!sampledCloud->reserve(theoricNumberOfPoints)
|| (triIndices && triIndices->capacity() < theoricNumberOfPoints && !triIndices->reserve(theoricNumberOfPoints))) //not enough memory
{
delete sampledCloud;
sampledCloud = 0;
triIndices->clear();
break;
}
}
for (unsigned i=0; i<pointsToAdd; ++i)
{
//we generates random points as in:
//'Greg Turk. Generating random points in triangles. In A. S. Glassner, editor,Graphics Gems, pages 24-28. Academic Press, 1990.'
double x = static_cast<double>(rand())/static_cast<double>(RAND_MAX);
double y = static_cast<double>(rand())/static_cast<double>(RAND_MAX);
//we test if the generated point lies on the right side of (AB)
if (x+y > 1.0)
{
x = 1.0-x;
y = 1.0-y;
}
CCVector3 P = (*O) + static_cast<PointCoordinateType>(x) * u + static_cast<PointCoordinateType>(y) * v;
sampledCloud->addPoint(P);
if (triIndices)
triIndices->addElement(n);
++addedPoints;
}
}
if (normProgress && !normProgress->oneStep())
break;
}
if (normProgress)
{
delete normProgress;
normProgress = 0;
}
if (sampledCloud) //can be in case of memory overflow!
{
if (addedPoints)
{
sampledCloud->resize(addedPoints); //should always be ok as addedPoints<theoricNumberOfPoints
if (triIndices)
triIndices->resize(addedPoints);
}
else
{
delete sampledCloud;
sampledCloud = 0;
if (triIndices)
triIndices->clear();
}
}
return sampledCloud;
}
ccGBLSensor::NormalGrid* ccGBLSensor::projectNormals( CCLib::GenericCloud* cloud,
const NormalGrid& theNorms,
double posIndex/*=0*/) const
{
if (!cloud || !theNorms.isAllocated())
return 0;
unsigned size = m_depthBuffer.height*m_depthBuffer.width;
if (size == 0)
return 0; //depth buffer empty/not initialized!
NormalGrid* normalGrid = new NormalGrid;
if (!normalGrid->resize(size,0))
return 0; //not enough memory
//sensor to world global transformation = sensor position * rigid transformation
ccIndexedTransformation sensorPos; //identity by default
if (m_posBuffer)
m_posBuffer->getInterpolatedTransformation(posIndex,sensorPos);
sensorPos *= m_rigidTransformation;
//poject each point + normal
{
cloud->placeIteratorAtBegining();
unsigned pointCount = cloud->size();
for (unsigned i=0; i<pointCount; ++i)
{
const CCVector3* P = cloud->getNextPoint();
const PointCoordinateType* N = theNorms.getValue(i);
//project point
CCVector2 Q;
PointCoordinateType depth1;
projectPoint(*P,Q,depth1,m_activeIndex);
CCVector3 S;
CCVector3 U = *P - sensorPos.getTranslationAsVec3D();
PointCoordinateType distToSensor = U.norm();
if (distToSensor > ZERO_TOLERANCE)
{
//normal component along sensor viewing dir.
S.z = -CCVector3::vdot(N,U.u)/distToSensor;
if (S.z > 1.0-ZERO_TOLERANCE)
{
S.x = 0;
S.y = 0;
}
else
{
//and point+normal
CCVector3 P2 = *P + CCVector3(N);
CCVector2 S2;
PointCoordinateType depth2;
projectPoint(P2,S2,depth2,m_activeIndex);
//deduce other normals components
PointCoordinateType coef = sqrt((1 - S.z*S.z)/(S.x*S.x + S.y*S.y));
S.x = coef * (S2.x - Q.x);
S.y = coef * (S2.y - Q.y);
}
}
else
{
S = CCVector3(N);
}
//project in Z-buffer
unsigned x,y;
if (convertToDepthMapCoords(Q.x,Q.y,x,y))
{
//add the transformed normal
PointCoordinateType* newN = normalGrid->getValue(y*m_depthBuffer.width + x);
CCVector3::vadd(newN,S.u,newN);
}
else
{
//shouldn't happen!
assert(false);
}
}
}
//normalize
{
normalGrid->placeIteratorAtBegining();
for (unsigned i=0; i<m_depthBuffer.height*m_depthBuffer.width; ++i)
{
PointCoordinateType* newN = normalGrid->getCurrentValue();
CCVector3::vnormalize(newN);
normalGrid->forwardIterator();
}
}
return normalGrid;
}
QStringList cc2DLabel::getLabelContent(int precision)
{
QStringList body;
switch(m_points.size())
{
case 1: //point
{
//init title
/*title = m_title;
//automatically elide the title
title = titleFontMetrics.elidedText(title,Qt::ElideRight,dx);
//*/
//coordinates
ccGenericPointCloud* cloud = m_points[0].cloud;
const unsigned& pointIndex = m_points[0].index;
const CCVector3* P = cloud->getPointPersistentPtr(pointIndex);
QString coordStr = QString("P#%0: (%1;%2;%3)").arg(pointIndex).arg(P->x,0,'f',precision).arg(P->y,0,'f',precision).arg(P->z,0,'f',precision);
body << coordStr;
//normal
if (cloud->hasNormals())
{
const PointCoordinateType* N = cloud->getPointNormal(pointIndex);
assert(N);
QString normStr = QString("Normal: (%1;%2;%3)").arg(N[0],0,'f',precision).arg(N[1],0,'f',precision).arg(N[2],0,'f',precision);
body << normStr;
}
//color
if (cloud->hasColors())
{
const colorType* C = cloud->getPointColor(pointIndex);
assert(C);
QString colorStr = QString("Color: (%1;%2;%3)").arg(C[0]).arg(C[1]).arg(C[2]);
body << colorStr;
}
//scalar field
if (cloud->hasDisplayedScalarField())
{
ScalarType D = cloud->getPointScalarValue(pointIndex);
QString sfStr = QString("Scalar: %1").arg(D,0,'f',precision);
body << sfStr;
}
}
break;
case 2: //vector
{
//1st point
ccGenericPointCloud* cloud1 = m_points[0].cloud;
const unsigned& pointIndex1 = m_points[0].index;
const CCVector3* P1 = cloud1->getPointPersistentPtr(pointIndex1);
//2nd point
ccGenericPointCloud* cloud2 = m_points[1].cloud;
const unsigned& pointIndex2 = m_points[1].index;
const CCVector3* P2 = cloud2->getPointPersistentPtr(pointIndex2);
PointCoordinateType d = (*P1-*P2).norm();
QString distStr = QString("Distance = %1").arg(d,0,'f',precision);
body << distStr;
QString coordStr1 = QString("P#%0: (%1;%2;%3)").arg(pointIndex1).arg(P1->x,0,'f',precision).arg(P1->y,0,'f',precision).arg(P1->z,0,'f',precision);
body << coordStr1;
QString coordStr2 = QString("P#%0: (%1;%2;%3)").arg(pointIndex2).arg(P2->x,0,'f',precision).arg(P2->y,0,'f',precision).arg(P2->z,0,'f',precision);
body << coordStr2;
}
break;
case 3: //triangle/plane
{
//1st point
ccGenericPointCloud* cloud1 = m_points[0].cloud;
const unsigned& pointIndex1 = m_points[0].index;
const CCVector3* P1 = cloud1->getPointPersistentPtr(pointIndex1);
//2nd point
ccGenericPointCloud* cloud2 = m_points[1].cloud;
const unsigned& pointIndex2 = m_points[1].index;
const CCVector3* P2 = cloud2->getPointPersistentPtr(pointIndex2);
//3rd point
ccGenericPointCloud* cloud3 = m_points[2].cloud;
const unsigned& pointIndex3 = m_points[2].index;
const CCVector3* P3 = cloud3->getPointPersistentPtr(pointIndex3);
//area
CCVector3 P1P2 = *P2-*P1;
CCVector3 P1P3 = *P3-*P1;
CCVector3 N = P1P2.cross(P1P3); //N=ABxAC
PointCoordinateType area = N.norm()*(PointCoordinateType)0.5;
QString areaStr = QString("Area = %1").arg(area,0,'f',precision);
body << areaStr;
//coordinates
QString coordStr1 = QString("A#%0: (%1;%2;%3)").arg(pointIndex1).arg(P1->x,0,'f',precision).arg(P1->y,0,'f',precision).arg(P1->z,0,'f',precision);
body << coordStr1;
QString coordStr2 = QString("B#%0: (%1;%2;%3)").arg(pointIndex2).arg(P2->x,0,'f',precision).arg(P2->y,0,'f',precision).arg(P2->z,0,'f',precision);
body << coordStr2;
QString coordStr3 = QString("C#%0: (%1;%2;%3)").arg(pointIndex3).arg(P3->x,0,'f',precision).arg(P3->y,0,'f',precision).arg(P3->z,0,'f',precision);
body << coordStr3;
//normal
N.normalize();
QString normStr = QString("Normal: (%1;%2;%3)").arg(N.x,0,'f',precision).arg(N.y,0,'f',precision).arg(N.z,0,'f',precision);
body << normStr;
//angle
CCVector3 P2P3 = *P3-*P2;
//negatives
CCVector3 _P1P2 = -P1P2;
CCVector3 _P1P3 = -P1P3;
CCVector3 _P2P3 = -P2P3;
double angleAtP1 = GetAngle_deg(P1P2,P1P3);
double angleAtP2 = GetAngle_deg(P2P3,_P1P2);
double angleAtP3 = GetAngle_deg(_P1P3,_P2P3); //should be equal to 180-a1-a2!
QString angleStr = QString("Angles: A=%1 - B=%3 - C=%5 deg.").arg(angleAtP1,0,'f',precision).arg(angleAtP2,0,'f',precision).arg(angleAtP3,0,'f',precision);
body << angleStr;
}
break;
default:
assert(false);
break;
}
return body;
}
bool ccGriddedTools::DetectParameters( const ccPointCloud* cloud,
const ccPointCloud::Grid::Shared grid,
GridParameters& parameters,
bool verbose/*=false*/,
ccGLMatrix* cloudToSensorTrans/*=0*/)
{
if (!cloud || !grid)
{
assert(false);
return false;
}
parameters.minPhi = static_cast<PointCoordinateType>(M_PI);
parameters.maxPhi = -parameters.minPhi;
parameters.minTheta = static_cast<PointCoordinateType>(M_PI);
parameters.maxTheta = -parameters.minTheta;
parameters.deltaPhiRad = 0;
parameters.deltaThetaRad = 0;
parameters.maxRange = 0;
//we must test if the angles are shifted (i.e the scan spans above theta = pi)
//we'll compute all parameters for both cases, and choose the best one at the end!
PointCoordinateType minPhiShifted = parameters.minPhi, maxPhiShifted = parameters.maxPhi;
PointCoordinateType minThetaShifted = parameters.minTheta, maxThetaShifted = parameters.maxTheta;
try
{
//determine the PITCH angular step
{
std::vector< AngleAndSpan > angles;
std::vector< AngleAndSpan > anglesShifted;
//for each ROW we determine the min and max valid grid point (i.e. index >= 0)
const int* _indexGrid = &(grid->indexes[0]);
for (unsigned j=0; j<grid->h; ++j)
{
unsigned minIndex = grid->w;
unsigned maxIndex = 0;
for (unsigned i=0; i<grid->w; ++i)
{
if (_indexGrid[i] >= 0)
{
if (i < minIndex)
minIndex = i;
if (i > maxIndex)
maxIndex = i;
}
}
if (maxIndex > minIndex)
{
PointCoordinateType minPhiCurrentLine = 0, maxPhiCurrentLine = 0;
PointCoordinateType minPhiCurrentLineShifted = 0, maxPhiCurrentLineShifted = 0;
for (unsigned k=minIndex; k<=maxIndex; ++k)
{
int index = _indexGrid[k];
if (index >= 0)
{
CCVector3 P = *(cloud->getPoint(static_cast<unsigned>(index)));
if (cloudToSensorTrans)
cloudToSensorTrans->apply(P);
PointCoordinateType p = atan2(P.z,sqrt(P.x*P.x + P.y*P.y)); //see ccGBLSensor::projectPoint
PointCoordinateType pShifted = (p < 0 ? p + static_cast<PointCoordinateType>(2.0*M_PI) : p);
if (k != minIndex)
{
if (minPhiCurrentLine > p)
minPhiCurrentLine = p;
else if (maxPhiCurrentLine < p)
maxPhiCurrentLine = p;
if (minPhiCurrentLineShifted > pShifted)
minPhiCurrentLineShifted = pShifted;
else if (maxPhiCurrentLineShifted < pShifted)
maxPhiCurrentLineShifted = pShifted;
}
else
{
minPhiCurrentLine = maxPhiCurrentLine = p;
minPhiCurrentLineShifted = maxPhiCurrentLineShifted = pShifted;
}
//find max range
PointCoordinateType range = P.norm();
if (range > parameters.maxRange)
parameters.maxRange = range;
}
}
if (parameters.minPhi > minPhiCurrentLine)
parameters.minPhi = minPhiCurrentLine;
if (parameters.maxPhi < maxPhiCurrentLine)
parameters.maxPhi = maxPhiCurrentLine;
if (minPhiShifted > minPhiCurrentLineShifted)
minPhiShifted = minPhiCurrentLineShifted;
if (maxPhiShifted < maxPhiCurrentLineShifted)
maxPhiShifted = maxPhiCurrentLineShifted;
unsigned span = maxIndex-minIndex+1;
ScalarType angle_rad = static_cast<ScalarType>((maxPhiCurrentLine-minPhiCurrentLine) / span);
angles.push_back(AngleAndSpan(angle_rad,span));
ScalarType angleShifted_rad = static_cast<ScalarType>((maxPhiCurrentLineShifted-minPhiCurrentLineShifted) / span);
anglesShifted.push_back(AngleAndSpan(angleShifted_rad,span));
}
_indexGrid += grid->w;
}
if (!angles.empty())
{
//check the 'shifted' hypothesis
PointCoordinateType spanShifted = maxPhiShifted - minPhiShifted;
PointCoordinateType span = parameters.maxPhi - parameters.minPhi;
if (spanShifted < 0.99 * span)
{
//we prefer the shifted version!
angles = anglesShifted;
parameters.minPhi = minPhiShifted;
parameters.maxPhi = maxPhiShifted;
}
//we simply take the biggest step evaluation for the widest span!
size_t maxSpanIndex = 0;
for (size_t i=1; i<angles.size(); ++i)
{
if ( angles[i].second > angles[maxSpanIndex].second
|| (angles[i].second == angles[maxSpanIndex].second && angles[i].first > angles[maxSpanIndex].first) )
{
maxSpanIndex = i;
}
}
parameters.deltaPhiRad = static_cast<PointCoordinateType>(angles[maxSpanIndex].first);
if (verbose)
{
ccLog::Print(QString("[Scan grid] Detected pitch step: %1 degrees (span [%2 - %3])").arg(parameters.deltaPhiRad * CC_RAD_TO_DEG).arg(parameters.minPhi * CC_RAD_TO_DEG).arg(parameters.maxPhi * CC_RAD_TO_DEG));
}
}
else
{
ccLog::Warning("[Scan grid] Not enough valid points to compute the scan angular step (pitch)!");
return false;
}
}
//now determine the YAW angular step
{
std::vector< AngleAndSpan > angles;
std::vector< AngleAndSpan > anglesShifted;
//for each COLUMN we determine the min and max valid grid point (i.e. index >= 0)
for (unsigned i=0; i<grid->w; ++i)
{
const int* _indexGrid = &(grid->indexes[i]);
unsigned minIndex = grid->h;
unsigned maxIndex = 0;
for (unsigned j=0; j<grid->h; ++j)
{
if (_indexGrid[j*grid->w] >= 0)
{
if (j < minIndex)
minIndex = j;
if (j > maxIndex)
maxIndex = j;
}
}
if (maxIndex > minIndex)
{
PointCoordinateType minThetaCurrentCol = 0, maxThetaCurrentCol = 0;
PointCoordinateType minThetaCurrentColShifted = 0, maxThetaCurrentColShifted = 0;
for (unsigned k=minIndex; k<=maxIndex; ++k)
{
int index = _indexGrid[k*grid->w];
if (index >= 0)
{
//warning: indexes are shifted (0 = no point)
CCVector3 P = *(cloud->getPoint(static_cast<unsigned>(index)));
if (cloudToSensorTrans)
cloudToSensorTrans->apply(P);
PointCoordinateType t = atan2(P.y,P.x); //see ccGBLSensor::projectPoint
PointCoordinateType tShifted = (t < 0 ? t + static_cast<PointCoordinateType>(2.0*M_PI) : t);
if (k != minIndex)
{
if (minThetaCurrentColShifted > tShifted)
minThetaCurrentColShifted = tShifted;
else if (maxThetaCurrentColShifted < tShifted)
maxThetaCurrentColShifted = tShifted;
if (minThetaCurrentCol > t)
minThetaCurrentCol = t;
else if (maxThetaCurrentCol < t)
maxThetaCurrentCol = t;
}
else
{
minThetaCurrentCol = maxThetaCurrentCol = t;
minThetaCurrentColShifted = maxThetaCurrentColShifted = tShifted;
}
}
}
if (parameters.minTheta > minThetaCurrentCol)
parameters.minTheta = minThetaCurrentCol;
if (parameters.maxTheta < maxThetaCurrentCol)
parameters.maxTheta = maxThetaCurrentCol;
if (minThetaShifted > minThetaCurrentColShifted)
minThetaShifted = minThetaCurrentColShifted;
if (maxThetaShifted < maxThetaCurrentColShifted)
maxThetaShifted = maxThetaCurrentColShifted;
unsigned span = maxIndex-minIndex;
ScalarType angle_rad = static_cast<ScalarType>((maxThetaCurrentCol-minThetaCurrentCol) / span);
angles.push_back(AngleAndSpan(angle_rad,span));
ScalarType angleShifted_rad = static_cast<ScalarType>((maxThetaCurrentColShifted-minThetaCurrentColShifted) / span);
anglesShifted.push_back(AngleAndSpan(angleShifted_rad,span));
}
}
if (!angles.empty())
{
//check the 'shifted' hypothesis
PointCoordinateType spanShifted = maxThetaShifted - minThetaShifted;
PointCoordinateType span = parameters.maxTheta - parameters.minTheta;
if (spanShifted < 0.99 * span)
{
//we prefer the shifted version!
angles = anglesShifted;
parameters.minTheta = minThetaShifted;
parameters.maxTheta = maxThetaShifted;
}
//we simply take the biggest step evaluation for the widest span!
size_t maxSpanIndex = 0;
for (size_t i=1; i<angles.size(); ++i)
{
if ( angles[i].second > angles[maxSpanIndex].second
|| (angles[i].second == angles[maxSpanIndex].second && angles[i].first > angles[maxSpanIndex].first) )
{
maxSpanIndex = i;
}
}
parameters.deltaThetaRad = static_cast<PointCoordinateType>(angles[maxSpanIndex].first);
if (verbose)
{
ccLog::Print(QString("[Scan grid] Detected yaw step: %1 degrees (span [%2 - %3])").arg(parameters.deltaThetaRad * CC_RAD_TO_DEG).arg(parameters.minTheta * CC_RAD_TO_DEG).arg(parameters.maxTheta * CC_RAD_TO_DEG));
}
}
else
{
ccLog::Warning("[Scan grid] Not enough valid points to compute the scan angular steps!");
return false;
}
}
}
catch (const std::bad_alloc&)
{
ccLog::Warning("[Scan grid] Not enough memory to compute the scan angular steps!");
return false;
}
return true;
}
bool ccClipBox::move3D(const CCVector3& uInput)
{
if (m_activeComponent == NONE || !m_box.isValid())
return false;
CCVector3 u = uInput;
//Arrows
if (m_activeComponent >= X_MINUS_ARROW && m_activeComponent <= CROSS)
{
if (m_glTransEnabled)
m_glTrans.inverse().applyRotation(u);
switch(m_activeComponent)
{
case X_MINUS_ARROW:
m_box.minCorner().x += u.x;
if (m_box.minCorner().x > m_box.maxCorner().x)
m_box.minCorner().x = m_box.maxCorner().x;
break;
case X_PLUS_ARROW:
m_box.maxCorner().x += u.x;
if (m_box.minCorner().x > m_box.maxCorner().x)
m_box.maxCorner().x = m_box.minCorner().x;
break;
case Y_MINUS_ARROW:
m_box.minCorner().y += u.y;
if (m_box.minCorner().y > m_box.maxCorner().y)
m_box.minCorner().y = m_box.maxCorner().y;
break;
case Y_PLUS_ARROW:
m_box.maxCorner().y += u.y;
if (m_box.minCorner().y > m_box.maxCorner().y)
m_box.maxCorner().y = m_box.minCorner().y;
break;
case Z_MINUS_ARROW:
m_box.minCorner().z += u.z;
if (m_box.minCorner().z > m_box.maxCorner().z)
m_box.minCorner().z = m_box.maxCorner().z;
break;
case Z_PLUS_ARROW:
m_box.maxCorner().z += u.z;
if (m_box.minCorner().z > m_box.maxCorner().z)
m_box.maxCorner().z = m_box.minCorner().z;
break;
case CROSS:
m_box.minCorner() += u;
m_box.maxCorner() += u;
break;
default:
assert(false);
return false;
}
//send 'modified' signal
emit boxModified(&m_box);
}
else if (m_activeComponent == SPHERE)
{
//handled by move2D!
return false;
}
else if (m_activeComponent >= X_MINUS_TORUS && m_activeComponent <= Z_PLUS_TORUS)
{
//we guess the rotation order by comparing the current screen 'normal'
//and the vector prod of u and the current rotation axis
CCVector3 Rb(0.0,0.0,0.0);
switch(m_activeComponent)
{
case X_MINUS_TORUS:
Rb.x = -1.0;
break;
case X_PLUS_TORUS:
Rb.x = 1.0;
break;
case Y_MINUS_TORUS:
Rb.y = -1.0;
break;
case Y_PLUS_TORUS:
Rb.y = 1.0;
break;
case Z_MINUS_TORUS:
Rb.z = -1.0;
break;
case Z_PLUS_TORUS:
Rb.z = 1.0;
break;
default:
assert(false);
return false;
}
CCVector3 R = Rb;
if (m_glTransEnabled)
m_glTrans.applyRotation(R);
CCVector3 RxU = R.cross(u);
//look for the most parallel dimension
int minDim = 0;
PointCoordinateType maxDot = CCVector3(m_viewMatrix.getColumn(0)).dot(RxU);
for (int i=1;i<3;++i)
{
PointCoordinateType dot = CCVector3(m_viewMatrix.getColumn(i)).dot(RxU);
if (fabs(dot) > fabs(maxDot))
{
maxDot = dot;
minDim = i;
}
}
//angle is proportional to absolute displacement
PointCoordinateType angle_rad = u.norm()/m_box.getDiagNorm() * M_PI;
if (maxDot < 0.0)
angle_rad = -angle_rad;
ccGLMatrix rotMat;
rotMat.initFromParameters(angle_rad,Rb,CCVector3(0.0,0.0,0.0));
CCVector3 C = m_box.getCenter();
ccGLMatrix transMat;
transMat.setTranslation(-C);
transMat = rotMat * transMat;
transMat.setTranslation(CCVector3(transMat.getTranslation())+C);
m_glTrans = m_glTrans * transMat.inverse();
enableGLTransformation(true);
}
else
{
assert(false);
return false;
}
update();
return true;
}
PointCoordinateType* GroundBasedLidarSensor::projectNormals(GenericCloud* aCloud, GenericChunkedArray<3,PointCoordinateType>& theNorms)
{
if ((!aCloud)||(!theNorms.isAllocated()))
return 0;
unsigned size = 3*dB.h_buff*dB.l_buff;
PointCoordinateType* theNewNorms = new PointCoordinateType[size];
if (!theNewNorms)
return 0; //not enough memory
memset(theNewNorms,0,size*sizeof(PointCoordinateType));
float dt = 1.0f/deltaTheta;
float dp = 1.0f/deltaPhi;
int x,y;
PointCoordinateType *N,*_theNewNorms;
CCVector3 R,S;
CCVector2 Q,S2;
DistanceType dist1,dist2;
aCloud->placeIteratorAtBegining();
theNorms.placeIteratorAtBegining();
unsigned i,n = aCloud->size();
for (i=0;i<n;++i)
{
//le ième point et sa normale
const CCVector3 *P = aCloud->getNextPoint();
N = theNorms.getCurrentValue();
CCVector3 U = *P - sensorCenter;
U.x += base;
//norme du vecteur "direction de visée laser"
float norm = U.norm();
//si sa norme est nulle, alors le point serait confondu avec le centre du capteur (peu probable ...)
if (norm>1e-7)
{
//composante de la normale selon la direction de visée ("S.z")
S.z = -CCVector3::vdot(N,U.u)/norm;
if (S.z > 0.99999)
{
S.x = 0.0;
S.y = 0.0;
}
else
{
//on projette le point
projectPoint(*P,Q,dist1);
//et l'autre extrémité du vecteur normal
R.x = P->x + N[0];
R.y = P->y + N[1];
R.z = P->z + N[2];
projectPoint(R,S2,dist2);
//la normale projetée est égale selon les autres dimensions à la différence des deux (à un coefficient près ;)
S.x = S2.x - Q.x;
S.y = S2.y - Q.y;
float coef = sqrt((1.0f-S.z*S.z)/(S.x*S.x+S.y*S.y));
S.x = coef * S.x;
S.y = coef * S.y;
}
}
else
{
//sinon on se casse pas la tête, on renvoie la même normale
S.x = N[0];
S.y = N[1];
S.z = N[2];
}
//on projette dans le Z-buffer
x = int(floor((Q.x-thetaMin)*dt));
y = int(floor((Q.y-phiMin)*dp));
//on ajoute la normale transformée
_theNewNorms = theNewNorms + 3*(y*dB.l_buff+x);
CCVector3::vadd(_theNewNorms,S.u,_theNewNorms);
theNorms.forwardIterator();
}
//on normalise tout (au cas où il y ait des accumulations)
_theNewNorms = theNewNorms;
for (i=0;i<unsigned(dB.h_buff*dB.l_buff);++i)
{
CCVector3::vnormalize(_theNewNorms);
_theNewNorms+=3;
}
return theNewNorms;
}
CCLib::ReferenceCloud* qHPR::removeHiddenPoints(CCLib::GenericIndexedCloudPersist* theCloud, const CCVector3& viewPoint, float fParam)
{
assert(theCloud);
unsigned nbPoints = theCloud->size();
if (nbPoints == 0)
return 0;
//less than 4 points? no need for calculation, we return the whole cloud
if (nbPoints < 4)
{
CCLib::ReferenceCloud* visiblePoints = new CCLib::ReferenceCloud(theCloud);
if (!visiblePoints->addPointIndex(0,nbPoints)) //well even for less than 4 points we never know ;)
{
//not enough memory!
delete visiblePoints;
visiblePoints = 0;
}
return visiblePoints;
}
PointCoordinateType maxRadius = 0;
//convert point cloud to an array of double triplets (for qHull)
coordT* pt_array = new coordT[(nbPoints+1)*3];
{
coordT* _pt_array = pt_array;
for (unsigned i=0; i<nbPoints; ++i)
{
CCVector3 P = *theCloud->getPoint(i) - viewPoint;
*_pt_array++ = (coordT)P.x;
*_pt_array++ = (coordT)P.y;
*_pt_array++ = (coordT)P.z;
//we keep track of the highest 'radius'
PointCoordinateType r2 = P.norm2();
if (maxRadius < r2)
maxRadius = r2;
}
//we add the view point (Cf. HPR)
*_pt_array++ = 0;
*_pt_array++ = 0;
*_pt_array++ = 0;
maxRadius = sqrt(maxRadius);
}
//apply spherical flipping
{
maxRadius *= 2.0f*pow(10.0f,fParam);
coordT* _pt_array = pt_array;
for (unsigned i=0; i<nbPoints; ++i)
{
CCVector3 P = *theCloud->getPoint(i) - viewPoint;
double r = (double)(maxRadius/P.norm()) - 1.0;
*_pt_array++ *= r;
*_pt_array++ *= r;
*_pt_array++ *= r;
}
}
//array to flag points on the convex hull
std::vector<bool> pointBelongsToCvxHull;
if (!qh_new_qhull(3,nbPoints+1,pt_array,False,(char*)"qhull QJ Qci",0,stderr))
{
try
{
pointBelongsToCvxHull.resize(nbPoints+1,false);
}
catch(std::bad_alloc)
{
//not enough memory!
delete[] pt_array;
return 0;
}
vertexT *vertex = 0,**vertexp = 0;
facetT *facet = 0;
FORALLfacets
{
//if (!facet->simplicial)
// error("convhulln: non-simplicial facet"); // should never happen with QJ
setT* vertices = qh_facet3vertex(facet);
FOREACHvertex_(vertices)
{
pointBelongsToCvxHull[qh_pointid(vertex->point)] = true;
}
qh_settempfree(&vertices);
}
}
delete[] pt_array;
pt_array=0;
qh_freeqhull(!qh_ALL);
//free long memory
int curlong, totlong;
qh_memfreeshort (&curlong, &totlong);
//free short memory and memory allocator
if (!pointBelongsToCvxHull.empty())
{
//compute the number of points belonging to the convex hull
unsigned cvxHullSize = 0;
{
for (unsigned i=0; i<nbPoints; ++i)
if (pointBelongsToCvxHull[i])
++cvxHullSize;
}
CCLib::ReferenceCloud* visiblePoints = new CCLib::ReferenceCloud(theCloud);
if (cvxHullSize!=0 && visiblePoints->reserve(cvxHullSize))
{
for (unsigned i=0; i<nbPoints; ++i)
if (pointBelongsToCvxHull[i])
visiblePoints->addPointIndex(i); //can't fail, see above
return visiblePoints;
}
else //not enough memory
{
delete visiblePoints;
visiblePoints=0;
}
}
return 0;
}
PointCoordinateType* ccGBLSensor::projectNormals(CCLib::GenericCloud* aCloud, GenericChunkedArray<3,PointCoordinateType>& theNorms) const
{
if (!aCloud || !theNorms.isAllocated())
return 0;
unsigned size = 3*m_depthBuffer.height*m_depthBuffer.width;
if (size == 0)
return 0; //depth buffer empty/not initialized!
PointCoordinateType* theNewNorms = new PointCoordinateType[size];
if (!theNewNorms)
return 0; //not enough memory
memset(theNewNorms,0,size*sizeof(PointCoordinateType));
//poject each point+normal
{
aCloud->placeIteratorAtBegining();
theNorms.placeIteratorAtBegining();
unsigned pointCount = aCloud->size();
for (unsigned i=0;i<pointCount;++i)
{
const CCVector3* P = aCloud->getNextPoint();
const PointCoordinateType* N = theNorms.getCurrentValue();
CCVector3 U = *P - sensorCenter;
U.x += base;
CCVector3 S;
CCVector2 Q;
PointCoordinateType norm = U.norm();
if (norm>ZERO_TOLERANCE)
{
//normal component along sensor viewing dir.
S.z = -CCVector3::vdot(N,U.u)/norm;
if (S.z > 1.0-ZERO_TOLERANCE)
{
S.x = 0;
S.y = 0;
}
else
{
//project point
ScalarType depth1;
projectPoint(*P,Q,depth1);
//and point+normal
CCVector3 R = *P + CCVector3(N);
CCVector2 S2;
ScalarType depth2;
projectPoint(R,S2,depth2);
//deduce other normals components
PointCoordinateType coef = sqrt((1 - S.z*S.z)/(S.x*S.x + S.y*S.y));
S.x = coef * (S2.x - Q.x);
S.y = coef * (S2.y - Q.y);
}
}
else
{
S = CCVector3(N);
}
//project in Z-buffer
int x = static_cast<int>(floor((Q.x-thetaMin)/deltaTheta));
int y = static_cast<int>(floor((Q.y-phiMin)/deltaPhi));
//on ajoute la normale transformee
PointCoordinateType* newN = theNewNorms+3*(y*m_depthBuffer.width+x);
CCVector3::vadd(newN,S.u,newN);
theNorms.forwardIterator();
}
}
//normalize
{
PointCoordinateType* newN = theNewNorms;
for (int i=0; i<m_depthBuffer.height*m_depthBuffer.width; ++i,newN+=3)
{
CCVector3::vnormalize(newN);
}
}
return theNewNorms;
}
ccGBLSensor* ccGriddedTools::ComputeBestSensor(ccPointCloud* cloud, const std::vector<int>& indexGrid, unsigned width, unsigned height, ccGLMatrix* cloudToSensorTrans/*=0*/)
{
PointCoordinateType minPhi = static_cast<PointCoordinateType>(M_PI), maxPhi = -minPhi;
PointCoordinateType minTheta = static_cast<PointCoordinateType>(M_PI), maxTheta = -minTheta;
PointCoordinateType deltaPhiRad = 0, deltaThetaRad = 0;
PointCoordinateType maxRange = 0;
//we must test if the angles are shifted (i.e the scan spans above theta = pi)
//we'll compute all parameters for both cases, and choose the best one at the end!
PointCoordinateType minPhiShifted = minPhi, maxPhiShifted = maxPhi;
PointCoordinateType minThetaShifted = minTheta, maxThetaShifted = maxTheta;
try
{
//determine the PITCH angular step
{
std::vector< AngleAndSpan > angles;
std::vector< AngleAndSpan > anglesShifted;
//for each LINE we determine the min and max valid grid point (i.e. != (0,0,0))
const int* _indexGrid = &(indexGrid[0]);
for (unsigned j=0; j<height; ++j)
{
unsigned minIndex = width;
unsigned maxIndex = 0;
for (unsigned i=0; i<width; ++i)
{
if (_indexGrid[i] >= 0)
{
if (i < minIndex)
minIndex = i;
if (i > maxIndex)
maxIndex = i;
}
}
if (maxIndex > minIndex)
{
PointCoordinateType minPhiCurrentLine = 0, maxPhiCurrentLine = 0;
PointCoordinateType minPhiCurrentLineShifted = 0, maxPhiCurrentLineShifted = 0;
for (unsigned k=minIndex; k<=maxIndex; ++k)
{
int index = _indexGrid[k];
if (index >= 0)
{
CCVector3 P = *(cloud->getPoint(static_cast<unsigned>(index)));
if (cloudToSensorTrans)
cloudToSensorTrans->apply(P);
PointCoordinateType p = atan2(P.z,sqrt(P.x*P.x + P.y*P.y)); //see ccGBLSensor::projectPoint
PointCoordinateType pShifted = (p < 0 ? p + static_cast<PointCoordinateType>(2.0*M_PI) : p);
if (k != minIndex)
{
if (minPhiCurrentLine > p)
minPhiCurrentLine = p;
else if (maxPhiCurrentLine < p)
maxPhiCurrentLine = p;
if (minPhiCurrentLineShifted > pShifted)
minPhiCurrentLineShifted = pShifted;
else if (maxPhiCurrentLineShifted < pShifted)
maxPhiCurrentLineShifted = pShifted;
}
else
{
minPhiCurrentLine = maxPhiCurrentLine = p;
minPhiCurrentLineShifted = maxPhiCurrentLineShifted = pShifted;
}
//find max range
PointCoordinateType range = P.norm();
if (range > maxRange)
maxRange = range;
}
}
if (minPhi > minPhiCurrentLine)
minPhi = minPhiCurrentLine;
if (maxPhi < maxPhiCurrentLine)
maxPhi = maxPhiCurrentLine;
if (minPhiShifted > minPhiCurrentLineShifted)
minPhiShifted = minPhiCurrentLineShifted;
if (maxPhiShifted < maxPhiCurrentLineShifted)
maxPhiShifted = maxPhiCurrentLineShifted;
unsigned span = maxIndex-minIndex+1;
ScalarType angle_rad = static_cast<ScalarType>((maxPhiCurrentLine-minPhiCurrentLine) / span);
angles.push_back(AngleAndSpan(angle_rad,span));
ScalarType angleShifted_rad = static_cast<ScalarType>((maxPhiCurrentLineShifted-minPhiCurrentLineShifted) / span);
anglesShifted.push_back(AngleAndSpan(angleShifted_rad,span));
}
_indexGrid += width;
}
if (!angles.empty())
{
//check the 'shifted' hypothesis
PointCoordinateType spanShifted = maxPhiShifted - minPhiShifted;
PointCoordinateType span = maxPhi - minPhi;
if (spanShifted < 0.99 * span)
{
//we prefer the shifted version!
angles = anglesShifted;
minPhi = minPhiShifted;
maxPhi = maxPhiShifted;
}
//we simply take the biggest step evaluation for the widest span!
size_t maxSpanIndex = 0;
for (size_t i=1; i<angles.size(); ++i)
{
if ( angles[i].second > angles[maxSpanIndex].second
|| (angles[i].second == angles[maxSpanIndex].second && angles[i].first > angles[maxSpanIndex].first) )
{
maxSpanIndex = i;
}
}
deltaPhiRad = static_cast<PointCoordinateType>(angles[maxSpanIndex].first);
ccLog::Print(QString("[PTX] Detected pitch step: %1 degrees (span [%2 - %3])").arg(deltaPhiRad * CC_RAD_TO_DEG).arg(minPhi * CC_RAD_TO_DEG).arg(maxPhi * CC_RAD_TO_DEG));
}
else
{
ccLog::Warning("[PTX] Not enough valid points to compute sensor angular step (pitch)!");
return 0;
}
}
//now determine the YAW angular step
{
std::vector< AngleAndSpan > angles;
std::vector< AngleAndSpan > anglesShifted;
//for each COLUMN we determine the min and max valid grid point (i.e. != (0,0,0))
for (unsigned i=0; i<width; ++i)
{
const int* _indexGrid = &(indexGrid[i]);
unsigned minIndex = height;
unsigned maxIndex = 0;
for (unsigned j=0; j<height; ++j)
{
if (_indexGrid[j*width] >= 0)
{
if (j < minIndex)
minIndex = j;
if (j > maxIndex)
maxIndex = j;
}
}
if (maxIndex > minIndex)
{
PointCoordinateType minThetaCurrentCol = 0, maxThetaCurrentCol = 0;
PointCoordinateType minThetaCurrentColShifted = 0, maxThetaCurrentColShifted = 0;
for (unsigned k=minIndex; k<=maxIndex; ++k)
{
int index = _indexGrid[k*width];
if (index >= 0)
{
//warning: indexes are shifted (0 = no point)
CCVector3 P = *(cloud->getPoint(static_cast<unsigned>(index)));
if (cloudToSensorTrans)
cloudToSensorTrans->apply(P);
PointCoordinateType t = atan2(P.y,P.x); //see ccGBLSensor::projectPoint
PointCoordinateType tShifted = (t < 0 ? t + static_cast<PointCoordinateType>(2.0*M_PI) : t);
if (k != minIndex)
{
if (minThetaCurrentColShifted > tShifted)
minThetaCurrentColShifted = tShifted;
else if (maxThetaCurrentColShifted < tShifted)
maxThetaCurrentColShifted = tShifted;
if (minThetaCurrentCol > t)
minThetaCurrentCol = t;
else if (maxThetaCurrentCol < t)
maxThetaCurrentCol = t;
}
else
{
minThetaCurrentCol = maxThetaCurrentCol = t;
minThetaCurrentColShifted = maxThetaCurrentColShifted = tShifted;
}
}
}
if (minTheta > minThetaCurrentCol)
minTheta = minThetaCurrentCol;
if (maxTheta < maxThetaCurrentCol)
maxTheta = maxThetaCurrentCol;
if (minThetaShifted > minThetaCurrentColShifted)
minThetaShifted = minThetaCurrentColShifted;
if (maxThetaShifted < maxThetaCurrentColShifted)
maxThetaShifted = maxThetaCurrentColShifted;
unsigned span = maxIndex-minIndex;
ScalarType angle_rad = static_cast<ScalarType>((maxThetaCurrentCol-minThetaCurrentCol) / span);
angles.push_back(AngleAndSpan(angle_rad,span));
ScalarType angleShifted_rad = static_cast<ScalarType>((maxThetaCurrentColShifted-minThetaCurrentColShifted) / span);
anglesShifted.push_back(AngleAndSpan(angleShifted_rad,span));
}
}
if (!angles.empty())
{
//check the 'shifted' hypothesis
PointCoordinateType spanShifted = maxThetaShifted - minThetaShifted;
PointCoordinateType span = maxTheta - minTheta;
if (spanShifted < 0.99 * span)
{
//we prefer the shifted version!
angles = anglesShifted;
minTheta = minThetaShifted;
maxTheta = maxThetaShifted;
}
//we simply take the biggest step evaluation for the widest span!
size_t maxSpanIndex = 0;
for (size_t i=1; i<angles.size(); ++i)
{
if ( angles[i].second > angles[maxSpanIndex].second
|| (angles[i].second == angles[maxSpanIndex].second && angles[i].first > angles[maxSpanIndex].first) )
{
maxSpanIndex = i;
}
}
deltaThetaRad = static_cast<PointCoordinateType>(angles[maxSpanIndex].first);
ccLog::Print(QString("[PTX] Detected yaw step: %1 degrees (span [%2 - %3])").arg(deltaThetaRad * CC_RAD_TO_DEG).arg(minTheta * CC_RAD_TO_DEG).arg(maxTheta * CC_RAD_TO_DEG));
}
else
{
ccLog::Warning("[PTX] Not enough valid points to compute sensor angular steps!");
return 0;
}
}
}
catch (std::bad_alloc)
{
ccLog::Warning("[PTX] Not enough memory to compute sensor angular steps!");
return 0;
}
ccGBLSensor* sensor = new ccGBLSensor(ccGBLSensor::YAW_THEN_PITCH);
if (sensor)
{
sensor->setPitchStep(deltaPhiRad);
sensor->setPitchRange(minPhi,maxPhi);
sensor->setYawStep(deltaThetaRad);
sensor->setYawRange(minTheta,maxTheta);
sensor->setSensorRange(maxRange);
//sensor->setRigidTransformation(cloudTrans/*sensorTrans*/); //will be called later by applyGLTransformation_recursive
sensor->setGraphicScale(PC_ONE/2);
sensor->setVisible(true);
sensor->setEnabled(false);
}
return sensor;
}
