您的位置: 首页  >  IT文章  >  点云的基本几何计算

点云的基本几何计算

分类: IT文章 • 2022-05-09 18:38:22

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 }
Neighbourhood::computeCurvature

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

相关推荐