1.计算法向量
function normal = estimateNormal(data, tree, query, radius, min_neighbors)
% ESTIMATENORMAL Given a point cloud and query point, estimate the surface
% normal by performing an eigendecomposition of the covariance matrix created
% from the nearest neighbors of the query point for a fixed radius.
%
% Example:
% data = randn(256,3);
% tree = KDTreeSearcher(data);
% query = [data(1,1) data(1,2) data(1,3)];
% radius = 1.0;
% min_neighbors = 8;
% normal = estimateNormal(data, tree, query, radius, min_neighbors)
%
% Copyright (c) 2014, William J. Beksi <beksi@cs.umn.edu>
%
% Find neighbors within the fixed radius
idx = rangesearch(tree, query, radius);
idxs = idx{1};
neighbors = [data(idxs(:),1) data(idxs(:),2) data(idxs(:),3)];
if size(neighbors) < min_neighbors
normal = {1};
return;
end
% Compute the covariance matrix C
C = cov(neighbors);
% Compute the eigenvector of C
[v, lambda] = eig(C);
% Find the eigenvector corresponding to the minimum eigenvalue in C
[~, i] = min(diag(lambda));
% Normalize
normal = v(:,i) ./ norm(v(:,i));
end
2.计算曲率
曲线的曲率（curvature）就是针对曲线上某个点的切线方向角对弧长的转动率，通过微分来定义，表明曲线偏离直线的程度。数学上表明曲线在某一点的弯曲程度的数值。曲率越大，表示曲线的弯曲程度越大。曲率的倒数就是曲率半径。
平均曲率、高斯曲率、主曲率计算
贺美芳, et al. (2005.8). "散乱点云数据的曲率估算及应用."
1 ScalarType Neighbourhood::computeCurvature(unsigned neighbourIndex, CC_CURVATURE_TYPE cType) 2 { 3 switch (cType) 4 { 5 case GAUSSIAN_CURV: 6 case MEAN_CURV: 7 { 8 //we get 2D1/2 quadric parameters 9 const PointCoordinateType* H = getQuadric(); 10 if (!H) 11 return NAN_VALUE; 12 13 //compute centroid 14 const CCVector3* G = getGravityCenter(); 15 16 //we compute curvature at the input neighbour position + we recenter it by the way 17 CCVector3 Q = *m_associatedCloud->getPoint(neighbourIndex) - *G; 18 19 const unsigned char X = m_quadricEquationDirections.x; 20 const unsigned char Y = m_quadricEquationDirections.y; 21 22 //z = a+b.x+c.y+d.x^2+e.x.y+f.y^2 23 //const PointCoordinateType& a = H[0]; 24 const PointCoordinateType& b = H[1]; 25 const PointCoordinateType& c = H[2]; 26 const PointCoordinateType& d = H[3]; 27 const PointCoordinateType& e = H[4]; 28 const PointCoordinateType& f = H[5]; 29 30 //See "CURVATURE OF CURVES AND SURFACES – A PARABOLIC APPROACH" by ZVI HAR’EL 31 const PointCoordinateType fx = b + (d*2) * Q.u[X] + (e ) * Q.u[Y]; // b+2d*X+eY 32 const PointCoordinateType fy = c + (e ) * Q.u[X] + (f*2) * Q.u[Y]; // c+2f*Y+eX 33 const PointCoordinateType fxx = d*2; // 2d 34 const PointCoordinateType fyy = f*2; // 2f 35 const PointCoordinateType& fxy = e; // e 36 37 const PointCoordinateType fx2 = fx*fx; 38 const PointCoordinateType fy2 = fy*fy; 39 const PointCoordinateType q = (1 + fx2 + fy2); 40 41 switch (cType) 42 { 43 case GAUSSIAN_CURV: 44 { 45 //to sign the curvature, we need a normal! 46 PointCoordinateType K = fabs( fxx*fyy - fxy*fxy ) / (q*q); 47 return static_cast<ScalarType>(K); 48 } 49 50 case MEAN_CURV: 51 { 52 //to sign the curvature, we need a normal! 53 PointCoordinateType H = fabs( ((1+fx2)*fyy - 2*fx*fy*fxy + (1+fy2)*fxx) ) / (2*sqrt(q)*q); 54 return static_cast<ScalarType>(H); 55 } 56 57 default: 58 assert(false); 59 } 60 } 61 break; 62 63 case NORMAL_CHANGE_RATE: 64 { 65 assert(m_associatedCloud); 66 unsigned pointCount = (m_associatedCloud ? m_associatedCloud->size() : 0); 67 68 //we need at least 4 points 69 if (pointCount < 4) 70 { 71 //not enough points! 72 return pointCount == 3 ? 0 : NAN_VALUE; 73 } 74 75 //we determine plane normal by computing the smallest eigen value of M = 1/n * S[(p-µ)*(p-µ)'] 76 CCLib::SquareMatrixd eigVectors; 77 std::vector<double> eigValues; 78 if (!Jacobi<double>::ComputeEigenValuesAndVectors(computeCovarianceMatrix(), eigVectors, eigValues)) 79 { 80 //failure 81 return NAN_VALUE; 82 } 83 84 //compute curvature as the rate of change of the surface 85 double e0 = eigValues[0]; 86 double e1 = eigValues[1]; 87 double e2 = eigValues[2]; 88 double sum = fabs(e0+e1+e2); 89 if (sum < ZERO_TOLERANCE) 90 { 91 return NAN_VALUE; 92 } 93 94 double eMin = std::min(std::min(e0,e1),e2); 95 return static_cast<ScalarType>(fabs(eMin) / sum); 96 } 97 break; 98 99 default: 100 assert(false); 101 } 102 103 return NAN_VALUE; 104 }
3.计算点云密度
CloudCompare中的实现方法 http://www.cnblogs.com/yhlx125/p/5874936.html
4.计算点云表面粗糙度
1 //"PER-CELL" METHOD: ROUGHNESS ESTIMATION (LEAST SQUARES PLANE FIT) 2 //ADDITIONNAL PARAMETERS (1): 3 // [0] -> (PointCoordinateType*) kernelRadius : neighbourhood radius 4 bool GeometricalAnalysisTools::computePointsRoughnessInACellAtLevel(const DgmOctree::octreeCell& cell, 5 void** additionalParameters, 6 NormalizedProgress* nProgress/*=0*/) 7 { 8 //parameter(s) 9 PointCoordinateType radius = *static_cast<PointCoordinateType*>(additionalParameters[0]); 10 11 //structure for nearest neighbors search 12 DgmOctree::NearestNeighboursSphericalSearchStruct nNSS; 13 nNSS.level = cell.level; 14 nNSS.prepare(radius,cell.parentOctree->getCellSize(nNSS.level)); 15 cell.parentOctree->getCellPos(cell.truncatedCode,cell.level,nNSS.cellPos,true); 16 cell.parentOctree->computeCellCenter(nNSS.cellPos,cell.level,nNSS.cellCenter); 17 18 unsigned n = cell.points->size(); //number of points in the current cell 19 20 //for each point in the cell 21 for (unsigned i=0; i<n; ++i) 22 { 23 ScalarType d = NAN_VALUE; 24 cell.points->getPoint(i,nNSS.queryPoint); 25 26 //look for neighbors inside a sphere 27 //warning: there may be more points at the end of nNSS.pointsInNeighbourhood than the actual nearest neighbors (= neighborCount)! 28 unsigned neighborCount = cell.parentOctree->findNeighborsInASphereStartingFromCell(nNSS,radius,false); 29 if (neighborCount > 3) 30 { 31 //find the query point in the nearest neighbors set and place it at the end 32 const unsigned globalIndex = cell.points->getPointGlobalIndex(i); 33 unsigned localIndex = 0; 34 while (localIndex < neighborCount && nNSS.pointsInNeighbourhood[localIndex].pointIndex != globalIndex) 35 ++localIndex; 36 //the query point should be in the nearest neighbors set! 37 assert(localIndex < neighborCount); 38 if (localIndex+1 < neighborCount) //no need to swap with another point if it's already at the end! 39 { 40 std::swap(nNSS.pointsInNeighbourhood[localIndex],nNSS.pointsInNeighbourhood[neighborCount-1]); 41 } 42 43 DgmOctreeReferenceCloud neighboursCloud(&nNSS.pointsInNeighbourhood,neighborCount-1); //we don't take the query point into account! 44 Neighbourhood Z(&neighboursCloud); 45 46 const PointCoordinateType* lsPlane = Z.getLSPlane(); 47 if (lsPlane) 48 d = fabs(DistanceComputationTools::computePoint2PlaneDistance(&nNSS.queryPoint,lsPlane)); 49 50 //swap the points back to their original position (DGM: not necessary) 51 //if (localIndex+1 < neighborCount) 52 //{ 53 // std::swap(nNSS.pointsInNeighbourhood[localIndex],nNSS.pointsInNeighbourhood[neighborCount-1]); 54 //} 55 } 56 57 cell.points->setPointScalarValue(i,d); 58 59 if (nProgress && !nProgress->oneStep()) 60 { 61 return false; 62 } 63 } 64 65 return true; 66 }
地面粗糙度是指在一个特定的区域内，地球表面积与其投影面积之比。它也是反映地表形态的一个宏观指标。
5.计算点云重心
1 //计算重心 2 CCVector3 GeometricalAnalysisTools::computeGravityCenter(GenericCloud* theCloud) 3 { 4 assert(theCloud); 5 6 unsigned count = theCloud->size(); 7 if (count == 0) 8 return CCVector3(); 9 10 CCVector3d sum(0,0,0); 11 12 theCloud->placeIteratorAtBegining(); 13 const CCVector3 *P = 0; 14 while ((P = theCloud->getNextPoint())) 15 { 16 sum += CCVector3d::fromArray(P->u); 17 } 18 19 sum /= static_cast<double>(count); 20 return CCVector3::fromArray(sum.u); 21 }
6.计算点云权重重心
1 //计算权重中心 2 CCVector3 GeometricalAnalysisTools::computeWeightedGravityCenter(GenericCloud* theCloud, ScalarField* weights) 3 { 4 assert(theCloud && weights); 5 6 unsigned count = theCloud->size(); 7 if (count == 0 || !weights || weights->currentSize() < count) 8 return CCVector3(); 9 10 CCVector3d sum(0, 0, 0); 11 12 theCloud->placeIteratorAtBegining(); 13 double wSum = 0; 14 for (unsigned i = 0; i < count; ++i) 15 { 16 const CCVector3* P = theCloud->getNextPoint(); 17 ScalarType w = weights->getValue(i); 18 if (!ScalarField::ValidValue(w)) 19 continue; 20 sum += CCVector3d::fromArray(P->u) * fabs(w); 21 wSum += w; 22 } 23 24 if (wSum != 0) 25 sum /= wSum; 26 27 return CCVector3::fromArray(sum.u); 28 }
7.计算点云协方差
1 //计算协方差矩阵 2 CCLib::SquareMatrixd GeometricalAnalysisTools::computeCovarianceMatrix(GenericCloud* theCloud, const PointCoordinateType* _gravityCenter) 3 { 4 assert(theCloud); 5 unsigned n = (theCloud ? theCloud->size() : 0); 6 if (n==0) 7 return CCLib::SquareMatrixd(); 8 9 CCLib::SquareMatrixd covMat(3); 10 covMat.clear(); 11 12 //gravity center 13 CCVector3 G = (_gravityCenter ? CCVector3(_gravityCenter) : computeGravityCenter(theCloud)); 14 15 //cross sums (we use doubles to avoid overflow) 16 double mXX = 0; 17 double mYY = 0; 18 double mZZ = 0; 19 double mXY = 0; 20 double mXZ = 0; 21 double mYZ = 0; 22 23 theCloud->placeIteratorAtBegining(); 24 for (unsigned i=0;i<n;++i) 25 { 26 const CCVector3* Q = theCloud->getNextPoint(); 27 28 CCVector3 P = *Q-G; 29 mXX += static_cast<double>(P.x*P.x); 30 mYY += static_cast<double>(P.y*P.y); 31 mZZ += static_cast<double>(P.z*P.z); 32 mXY += static_cast<double>(P.x*P.y); 33 mXZ += static_cast<double>(P.x*P.z); 34 mYZ += static_cast<double>(P.y*P.z); 35 } 36 37 covMat.m_values[0][0] = mXX/static_cast<double>(n); 38 covMat.m_values[0][0] = mYY/static_cast<double>(n); 39 covMat.m_values[0][0] = mZZ/static_cast<double>(n); 40 covMat.m_values[1][0] = covMat.m_values[0][1] = mXY/static_cast<double>(n); 41 covMat.m_values[2][0] = covMat.m_values[0][2] = mXZ/static_cast<double>(n); 42 covMat.m_values[2][1] = covMat.m_values[1][2] = mYZ/static_cast<double>(n); 43 44 return covMat; 45 }
8.计算点云互协方差
1 //计算2个点云的互协方差 2 CCLib::SquareMatrixd GeometricalAnalysisTools::computeCrossCovarianceMatrix(GenericCloud* P, 3 GenericCloud* Q, 4 const CCVector3& Gp, 5 const CCVector3& Gq) 6 { 7 assert(P && Q); 8 assert(Q->size() == P->size()); 9 10 //shortcuts to output matrix lines 11 CCLib::SquareMatrixd covMat(3); 12 double* l1 = covMat.row(0); 13 double* l2 = covMat.row(1); 14 double* l3 = covMat.row(2); 15 16 P->placeIteratorAtBegining(); 17 Q->placeIteratorAtBegining(); 18 19 //sums 20 unsigned count = P->size(); 21 for (unsigned i=0; i<count; i++) 22 { 23 CCVector3 Pt = *P->getNextPoint() - Gp; 24 CCVector3 Qt = *Q->getNextPoint() - Gq; 25 26 l1[0] += Pt.x*Qt.x; 27 l1[1] += Pt.x*Qt.y; 28 l1[2] += Pt.x*Qt.z; 29 l2[0] += Pt.y*Qt.x; 30 l2[1] += Pt.y*Qt.y; 31 l2[2] += Pt.y*Qt.z; 32 l3[0] += Pt.z*Qt.x; 33 l3[1] += Pt.z*Qt.y; 34 l3[2] += Pt.z*Qt.z; 35 } 36 37 covMat.scale(1.0/static_cast<double>(count)); 38 39 return covMat; 40 }
1 //计算权重互协方差 2 CCLib::SquareMatrixd GeometricalAnalysisTools::computeWeightedCrossCovarianceMatrix(GenericCloud* P, //data 3 GenericCloud* Q, //model 4 const CCVector3& Gp, 5 const CCVector3& Gq, 6 ScalarField* coupleWeights/*=0*/) 7 { 8 assert(P && Q); 9 assert(Q->size() == P->size()); 10 assert(coupleWeights); 11 assert(coupleWeights->currentSize() == P->size()); 12 13 //shortcuts to output matrix lines 14 CCLib::SquareMatrixd covMat(3); 15 double* r1 = covMat.row(0); 16 double* r2 = covMat.row(1); 17 double* r3 = covMat.row(2); 18 19 P->placeIteratorAtBegining(); 20 Q->placeIteratorAtBegining(); 21 22 //sums 23 unsigned count = P->size(); 24 double wSum = 0.0; //we will normalize by the sum 25 for (unsigned i = 0; i<count; i++) 26 { 27 CCVector3d Pt = CCVector3d::fromArray((*P->getNextPoint() - Gp).u); 28 CCVector3 Qt = *Q->getNextPoint() - Gq; 29 30 //Weighting scheme for cross-covariance is inspired from 31 //https://en.wikipedia.org/wiki/Weighted_arithmetic_mean#Weighted_sample_covariance 32 double wi = 1.0; 33 if (coupleWeights) 34 { 35 ScalarType w = coupleWeights->getValue(i); 36 if (!ScalarField::ValidValue(w)) 37 continue; 38 wi = fabs(w); 39 } 40 41 //DGM: we virtually make the P (data) point nearer if it has a lower weight 42 Pt *= wi; 43 wSum += wi; 44 45 //1st row 46 r1[0] += Pt.x * Qt.x; 47 r1[1] += Pt.x * Qt.y; 48 r1[2] += Pt.x * Qt.z; 49 //2nd row 50 r2[0] += Pt.y * Qt.x; 51 r2[1] += Pt.y * Qt.y; 52 r2[2] += Pt.y * Qt.z; 53 //3rd row 54 r3[0] += Pt.z * Qt.x; 55 r3[1] += Pt.z * Qt.y; 56 r3[2] += Pt.z * Qt.z; 57 } 58 59 if (wSum != 0.0) 60 covMat.scale(1.0/wSum); 61 62 return covMat; 63 }