1、计算法向量
源码出处：https://ww2.mathworks.cn/matlabcentral/fileexchange/46757-estimate-surface-normals
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<ScalarType>(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<ScalarType>(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<double> eigValues;
if (!Jacobi<double>::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<ScalarType>(fabs(eMin) / sum);
}
break;
default:
assert(false);
}
return NAN_VALUE;
}
Neighbourhood::computeCurvature 3、计算点云密度
CloudCompare中的实现方法：http://www.cnblogs.com/yhlx125/p/5874936.html
4.计算点云表面粗糙度
表面粗糙度简介：https://wenku.baidu.com/view/062507457cd184254b353578.html###
//"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<PointCoordinateType*>(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<n; ++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<double>(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<n;++i)
{
const CCVector3* Q = theCloud->getNextPoint();
CCVector3 P = *Q-G;
mXX += static_cast<double>(P.x*P.x);
mYY += static_cast<double>(P.y*P.y);
mZZ += static_cast<double>(P.z*P.z);
mXY += static_cast<double>(P.x*P.y);
mXZ += static_cast<double>(P.x*P.z);
mYZ += static_cast<double>(P.y*P.z);
}
covMat.m_values[0][0] = mXX/static_cast<double>(n);
covMat.m_values[0][0] = mYY/static_cast<double>(n);
covMat.m_values[0][0] = mZZ/static_cast<double>(n);
covMat.m_values[1][0] = covMat.m_values[0][1] = mXY/static_cast<double>(n);
covMat.m_values[2][0] = covMat.m_values[0][2] = mXZ/static_cast<double>(n);
covMat.m_values[2][1] = covMat.m_values[1][2] = mYZ/static_cast<double>(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<count; 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<double>(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<count; 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 ![查找算法之二分查找查找算法之二分查找[图]](data:image/gif;base64,R0lGODlhAQABAIAAAP///wAAACwAAAAAAQABAAACAkQBADs=)