点云密度计算方法matlab,点云中的几何计算及matlab源码

1、计算法向量

2、计算曲率

曲线的曲率(curvature)就是针对曲线上某个点的切线方向角对弧长的转动率，通过微分来定义，表明曲线偏离直线的程度。数学上表明曲线在某一点的弯曲程度的数值。曲率越大，表示曲线的弯曲程度越大。曲率的倒数就是曲率半径。

平均曲率、高斯曲率、主曲率计算

贺美芳, et al. (2005.8). "散乱点云数据的曲率估算及应用."

ScalarType Neighbourhood::computeCurvature(unsigned neighbourIndex, CC_CURVATURE_TYPE cType)

{

switch (cType)

{

case GAUSSIAN_CURV:

case MEAN_CURV:

{

//we get 2D1/2 quadric parameters

const PointCoordinateType* H = getQuadric();

if (!H)

return NAN_VALUE;

//compute centroid

const CCVector3* G = getGravityCenter();

//we compute curvature at the input neighbour position + we recenter it by the way

CCVector3 Q = *m_associatedCloud->getPoint(neighbourIndex) - *G;

const unsigned char X = m_quadricEquationDirections.x;

const unsigned char Y = m_quadricEquationDirections.y;

//z = a+b.x+c.y+d.x^2+e.x.y+f.y^2

//const PointCoordinateType& a = H[0];

const PointCoordinateType& b = H[1];

const PointCoordinateType& c = H[2];

const PointCoordinateType& d = H[3];

const PointCoordinateType& e = H[4];

const PointCoordinateType& f = H[5];

//See "CURVATURE OF CURVES AND SURFACES – A PARABOLIC APPROACH" by ZVI HAR’EL

const PointCoordinateType fx = b + (d*2) * Q.u[X] + (e ) * Q.u[Y]; // b+2d*X+eY

const PointCoordinateType fy = c + (e ) * Q.u[X] + (f*2) * Q.u[Y]; // c+2f*Y+eX

const PointCoordinateType fxx = d*2; // 2d

const PointCoordinateType fyy = f*2; // 2f

const PointCoordinateType& fxy = e; // e

const PointCoordinateType fx2 = fx*fx;

const PointCoordinateType fy2 = fy*fy;

const PointCoordinateType q = (1 + fx2 + fy2);

switch (cType)

{

case GAUSSIAN_CURV:

{

//to sign the curvature, we need a normal!

PointCoordinateType K = fabs( fxx*fyy - fxy*fxy ) / (q*q);

return static_cast(K);

}

case MEAN_CURV:

{

//to sign the curvature, we need a normal!

PointCoordinateType H = fabs( ((1+fx2)*fyy - 2*fx*fy*fxy + (1+fy2)*fxx) ) / (2*sqrt(q)*q);

return static_cast(H);

}

default:

assert(false);

}

}

break;

case NORMAL_CHANGE_RATE:

{

assert(m_associatedCloud);

unsigned pointCount = (m_associatedCloud ? m_associatedCloud->size() : 0);

//we need at least 4 points

if (pointCount < 4)

{

//not enough points!

return pointCount == 3 ? 0 : NAN_VALUE;

}

//we determine plane normal by computing the smallest eigen value of M = 1/n * S[(p-µ)*(p-µ)']

CCLib::SquareMatrixd eigVectors;

std::vector eigValues;

if (!Jacobi::ComputeEigenValuesAndVectors(computeCovarianceMatrix(), eigVectors, eigValues))

{

//failure

return NAN_VALUE;

}

//compute curvature as the rate of change of the surface

double e0 = eigValues[0];

double e1 = eigValues[1];

double e2 = eigValues[2];

double sum = fabs(e0+e1+e2);

if (sum < ZERO_TOLERANCE)

{

return NAN_VALUE;

}

double eMin = std::min(std::min(e0,e1),e2);

return static_cast(fabs(eMin) / sum);

}

break;

default:

assert(false);

}

return NAN_VALUE;

}

Neighbourhood::computeCurvature

3、计算点云密度

4.计算点云表面粗糙度

//"PER-CELL" METHOD: ROUGHNESS ESTIMATION (LEAST SQUARES PLANE FIT)

//ADDITIONNAL PARAMETERS (1):

// [0] -> (PointCoordinateType*) kernelRadius : neighbourhood radius

bool GeometricalAnalysisTools::computePointsRoughnessInACellAtLevel(const DgmOctree::octreeCell& cell,

void** additionalParameters,

NormalizedProgress* nProgress/*=0*/)

{

//parameter(s)

PointCoordinateType radius = *static_cast(additionalParameters[0]);

//structure for nearest neighbors search

DgmOctree::NearestNeighboursSphericalSearchStruct nNSS;

nNSS.level = cell.level;

nNSS.prepare(radius,cell.parentOctree->getCellSize(nNSS.level));

cell.parentOctree->getCellPos(cell.truncatedCode,cell.level,nNSS.cellPos,true);

cell.parentOctree->computeCellCenter(nNSS.cellPos,cell.level,nNSS.cellCenter);

unsigned n = cell.points->size(); //number of points in the current cell

//for each point in the cell

for (unsigned i=0; i

{

ScalarType d = NAN_VALUE;

cell.points->getPoint(i,nNSS.queryPoint);

//look for neighbors inside a sphere

//warning: there may be more points at the end of nNSS.pointsInNeighbourhood than the actual nearest neighbors (= neighborCount)!

unsigned neighborCount = cell.parentOctree->findNeighborsInASphereStartingFromCell(nNSS,radius,false);

if (neighborCount > 3)

{

//find the query point in the nearest neighbors set and place it at the end

const unsigned globalIndex = cell.points->getPointGlobalIndex(i);

unsigned localIndex = 0;

while (localIndex < neighborCount && nNSS.pointsInNeighbourhood[localIndex].pointIndex != globalIndex)

++localIndex;

//the query point should be in the nearest neighbors set!

assert(localIndex < neighborCount);

if (localIndex+1 < neighborCount) //no need to swap with another point if it's already at the end!

{

std::swap(nNSS.pointsInNeighbourhood[localIndex],nNSS.pointsInNeighbourhood[neighborCount-1]);

}

DgmOctreeReferenceCloud neighboursCloud(&nNSS.pointsInNeighbourhood,neighborCount-1); //we don't take the query point into account!

Neighbourhood Z(&neighboursCloud);

const PointCoordinateType* lsPlane = Z.getLSPlane();

if (lsPlane)

d = fabs(DistanceComputationTools::computePoint2PlaneDistance(&nNSS.queryPoint,lsPlane));

//swap the points back to their original position (DGM: not necessary)

//if (localIndex+1 < neighborCount)

//{

// std::swap(nNSS.pointsInNeighbourhood[localIndex],nNSS.pointsInNeighbourhood[neighborCount-1]);

//}

}

cell.points->setPointScalarValue(i,d);

if (nProgress && !nProgress->oneStep())

{

return false;

}

}

return true;

}

computePointsRoughnessInACellAtLevel

地面粗糙度是指在一个特定的区域内，地球表面积与其投影面积之比。它也是反映地表形态的一个宏观指标。

5.计算点云重心

//计算重心

CCVector3 GeometricalAnalysisTools::computeGravityCenter(GenericCloud* theCloud)

{

assert(theCloud);

unsigned count = theCloud->size();

if (count == 0)

return CCVector3();

CCVector3d sum(0,0,0);

theCloud->placeIteratorAtBegining();

const CCVector3 *P = 0;

while ((P = theCloud->getNextPoint()))

{

sum += CCVector3d::fromArray(P->u);

}

sum /= static_cast(count);

return CCVector3::fromArray(sum.u);

}

computeGravityCenter

6.计算点云权重重心

//计算权重中心

CCVector3 GeometricalAnalysisTools::computeWeightedGravityCenter(GenericCloud* theCloud, ScalarField* weights)

{

assert(theCloud && weights);

unsigned count = theCloud->size();

if (count == 0 || !weights || weights->currentSize() < count)

return CCVector3();

CCVector3d sum(0, 0, 0);

theCloud->placeIteratorAtBegining();

double wSum = 0;

for (unsigned i = 0; i < count; ++i)

{

const CCVector3* P = theCloud->getNextPoint();

ScalarType w = weights->getValue(i);

if (!ScalarField::ValidValue(w))

continue;

sum += CCVector3d::fromArray(P->u) * fabs(w);

wSum += w;

}

if (wSum != 0)

sum /= wSum;

return CCVector3::fromArray(sum.u);

}

computeWeightedGravityCenter

7.计算点云协方差

//计算协方差矩阵

CCLib::SquareMatrixd GeometricalAnalysisTools::computeCovarianceMatrix(GenericCloud* theCloud, const PointCoordinateType* _gravityCenter)

{

assert(theCloud);

unsigned n = (theCloud ? theCloud->size() : 0);

if (n==0)

return CCLib::SquareMatrixd();

CCLib::SquareMatrixd covMat(3);

covMat.clear();

//gravity center

CCVector3 G = (_gravityCenter ? CCVector3(_gravityCenter) : computeGravityCenter(theCloud));

//cross sums (we use doubles to avoid overflow)

double mXX = 0;

double mYY = 0;

double mZZ = 0;

double mXY = 0;

double mXZ = 0;

double mYZ = 0;

theCloud->placeIteratorAtBegining();

for (unsigned i=0;i

{

const CCVector3* Q = theCloud->getNextPoint();

CCVector3 P = *Q-G;

mXX += static_cast(P.x*P.x);

mYY += static_cast(P.y*P.y);

mZZ += static_cast(P.z*P.z);

mXY += static_cast(P.x*P.y);

mXZ += static_cast(P.x*P.z);

mYZ += static_cast(P.y*P.z);

}

covMat.m_values[0][0] = mXX/static_cast(n);

covMat.m_values[0][0] = mYY/static_cast(n);

covMat.m_values[0][0] = mZZ/static_cast(n);

covMat.m_values[1][0] = covMat.m_values[0][1] = mXY/static_cast(n);

covMat.m_values[2][0] = covMat.m_values[0][2] = mXZ/static_cast(n);

covMat.m_values[2][1] = covMat.m_values[1][2] = mYZ/static_cast(n);

return covMat;

}

computeCovarianceMatrix

8.计算点云互协方差

//计算2个点云的互协方差

CCLib::SquareMatrixd GeometricalAnalysisTools::computeCrossCovarianceMatrix(GenericCloud* P,

GenericCloud* Q,

const CCVector3& Gp,

const CCVector3& Gq)

{

assert(P && Q);

assert(Q->size() == P->size());

//shortcuts to output matrix lines

CCLib::SquareMatrixd covMat(3);

double* l1 = covMat.row(0);

double* l2 = covMat.row(1);

double* l3 = covMat.row(2);

P->placeIteratorAtBegining();

Q->placeIteratorAtBegining();

//sums

unsigned count = P->size();

for (unsigned i=0; i

{

CCVector3 Pt = *P->getNextPoint() - Gp;

CCVector3 Qt = *Q->getNextPoint() - Gq;

l1[0] += Pt.x*Qt.x;

l1[1] += Pt.x*Qt.y;

l1[2] += Pt.x*Qt.z;

l2[0] += Pt.y*Qt.x;

l2[1] += Pt.y*Qt.y;

l2[2] += Pt.y*Qt.z;

l3[0] += Pt.z*Qt.x;

l3[1] += Pt.z*Qt.y;

l3[2] += Pt.z*Qt.z;

}

covMat.scale(1.0/static_cast(count));

return covMat;

}

computeCrossCovarianceMatrix

//计算权重互协方差

CCLib::SquareMatrixd GeometricalAnalysisTools::computeWeightedCrossCovarianceMatrix(GenericCloud* P, //data

GenericCloud* Q, //model

const CCVector3& Gp,

const CCVector3& Gq,

ScalarField* coupleWeights/*=0*/)

{

assert(P && Q);

assert(Q->size() == P->size());

assert(coupleWeights);

assert(coupleWeights->currentSize() == P->size());

//shortcuts to output matrix lines

CCLib::SquareMatrixd covMat(3);

double* r1 = covMat.row(0);

double* r2 = covMat.row(1);

double* r3 = covMat.row(2);

P->placeIteratorAtBegining();

Q->placeIteratorAtBegining();

//sums

unsigned count = P->size();

double wSum = 0.0; //we will normalize by the sum

for (unsigned i = 0; i

{

CCVector3d Pt = CCVector3d::fromArray((*P->getNextPoint() - Gp).u);

CCVector3 Qt = *Q->getNextPoint() - Gq;

//Weighting scheme for cross-covariance is inspired from

//https://en.wikipedia.org/wiki/Weighted_arithmetic_mean#Weighted_sample_covariance

double wi = 1.0;

if (coupleWeights)

{

ScalarType w = coupleWeights->getValue(i);

if (!ScalarField::ValidValue(w))

continue;

wi = fabs(w);

}

//DGM: we virtually make the P (data) point nearer if it has a lower weight

Pt *= wi;

wSum += wi;

//1st row

r1[0] += Pt.x * Qt.x;

r1[1] += Pt.x * Qt.y;

r1[2] += Pt.x * Qt.z;

//2nd row

r2[0] += Pt.y * Qt.x;

r2[1] += Pt.y * Qt.y;

r2[2] += Pt.y * Qt.z;

//3rd row

r3[0] += Pt.z * Qt.x;

r3[1] += Pt.z * Qt.y;

r3[2] += Pt.z * Qt.z;

}

if (wSum != 0.0)

covMat.scale(1.0/wSum);

return covMat;

}

computeWeightedCrossCovarianceMatrix