# 点云的基本几何计算

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)];
%       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
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 }
Neighbourhood::computeCurvature

3.计算点云密度

CloudCompare中的实现方法　http://www.cnblogs.com/yhlx125/p/5874936.html

4.计算点云表面粗糙度

 1 //"PER-CELL" METHOD: ROUGHNESS ESTIMATION (LEAST SQUARES PLANE FIT)
4 bool GeometricalAnalysisTools::computePointsRoughnessInACellAtLevel(const DgmOctree::octreeCell& cell,
6                                                                     NormalizedProgress* nProgress/*=0*/)
7 {
8     //parameter(s)
10
11     //structure for nearest neighbors search
13     nNSS.level = cell.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)!
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 }
computePointsRoughnessInACellAtLevel

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

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

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

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

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

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

• 3
点赞
• 1
评论
• 10
收藏
• 一键三连
• 扫一扫，分享海报

12-13
05-12

07-22
09-07
06-07 206
01-29 29万+
11-19
02-10 2430
09-28 1万+
02-28
10-11 238
09-01
05-09
12-14 5万+
01-22 2725
12-04 2450
04-24 12万+
03-25 779