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