1 ScalarType Neighbourhood::computeMomentOrder1(const CCVector3& P)
2 {
3 if (!m_associatedCloud || m_associatedCloud->size() < 3)
4 {
5 //not enough points
6 return NAN_VALUE;
7 }
8
9 SquareMatrixd eigVectors;
10 std::vector<double> eigValues;
11 if (!Jacobi<double>::ComputeEigenValuesAndVectors(computeCovarianceMatrix(), eigVectors, eigValues, true))
12 {
13 //failed to compute the eigen values
14 return NAN_VALUE;
15 }
16
17 Jacobi<double>::SortEigenValuesAndVectors(eigVectors, eigValues); //sort the eigenvectors in decreasing order of their associated eigenvalues
18
19 double m1 = 0.0, m2 = 0.0;
20 CCVector3d e2;
21 Jacobi<double>::GetEigenVector(eigVectors, 1, e2.u);
22
23 for (unsigned i = 0; i < m_associatedCloud->size(); ++i)
24 {
25 double dotProd = CCVector3d::fromArray((*m_associatedCloud->getPoint(i) - P).u).dot(e2);
26 m1 += dotProd;
27 m2 += dotProd * dotProd;
28 }
29
30 //see "Contour detection in unstructured 3D point clouds", Hackel et al 2016
31 return (m2 < std::numeric_limits<double>::epsilon() ? NAN_VALUE : static_cast<ScalarType>((m1 * m1) / m2));
32 }
33
34 double Neighbourhood::computeFeature(GeomFeature feature)
35 {
36 if (!m_associatedCloud || m_associatedCloud->size() < 3)
37 {
38 //not enough points
39 return std::numeric_limits<double>::quiet_NaN();
40 }
41
42 SquareMatrixd eigVectors;
43 std::vector<double> eigValues;
44 if (!Jacobi<double>::ComputeEigenValuesAndVectors(computeCovarianceMatrix(), eigVectors, eigValues, true))
45 {
46 //failed to compute the eigen values
47 return std::numeric_limits<double>::quiet_NaN();
48 }
49
50 Jacobi<double>::SortEigenValuesAndVectors(eigVectors, eigValues); //sort the eigenvectors in decreasing order of their associated eigenvalues
51
52 //shortcuts
53 const double& l1 = eigValues[0];
54 const double& l2 = eigValues[1];
55 const double& l3 = eigValues[2];
56
57 double value = std::numeric_limits<double>::quiet_NaN();
58
59 switch (feature)
60 {
61 case EigenValuesSum:
62 value = l1 + l2 + l3;
63 break;
64 case Omnivariance:
65 value = pow(l1 * l2 * l3, 1.0/3.0);
66 break;
67 case EigenEntropy:
68 value = -(l1 * log(l1) + l2 * log(l2) + l3 * log(l3));
69 break;
70 case Anisotropy:
71 if (std::abs(l1) > std::numeric_limits<double>::epsilon())
72 value = (l1 - l3) / l1;
73 break;
74 case Planarity:
75 if (std::abs(l1) > std::numeric_limits<double>::epsilon())
76 value = (l2 - l3) / l1;
77 break;
78 case Linearity:
79 if (std::abs(l1) > std::numeric_limits<double>::epsilon())
80 value = (l1 - l2) / l1;
81 break;
82 case PCA1:
83 {
84 double sum = l1 + l2 + l3;
85 if (std::abs(sum) > std::numeric_limits<double>::epsilon())
86 value = l1 / sum;
87 }
88 break;
89 case PCA2:
90 {
91 double sum = l1 + l2 + l3;
92 if (std::abs(sum) > std::numeric_limits<double>::epsilon())
93 value = l2 / sum;
94 }
95 break;
96 case SurfaceVariation:
97 {
98 double sum = l1 + l2 + l3;
99 if (std::abs(sum) > std::numeric_limits<double>::epsilon())
100 value = l3 / sum;
101 }
102 break;
103 case Sphericity:
104 if (std::abs(l1) > std::numeric_limits<double>::epsilon())
105 value = l3 / l1;
106 break;
107 case Verticality:
108 {
109 CCVector3d Z(0, 0, 1);
110 CCVector3d e3(Z);
111 Jacobi<double>::GetEigenVector(eigVectors, 2, e3.u);
112
113 value = 1.0 - std::abs(Z.dot(e3));
114 }
115 break;
116 default:
117 assert(false);
118 break;
119 }
120
121 return value;
122 }
123
124 ScalarType Neighbourhood::computeRoughness(const CCVector3& P)
125 {
126 const PointCoordinateType* lsPlane = getLSPlane();
127 if (lsPlane)
128 {
129 return std::abs(DistanceComputationTools::computePoint2PlaneDistance(&P, lsPlane));
130 }
131 else
132 {
133 return NAN_VALUE;
134 }
135 }
136
137 ScalarType Neighbourhood::computeCurvature(const CCVector3& P, CurvatureType cType)
138 {
139 switch (cType)
140 {
141 case GAUSSIAN_CURV:
142 case MEAN_CURV:
143 {
144 //we get 2D1/2 quadric parameters
145 const PointCoordinateType* H = getQuadric();
146 if (!H)
147 return NAN_VALUE;
148
149 //compute centroid
150 const CCVector3* G = getGravityCenter();
151
152 //we compute curvature at the input neighbour position + we recenter it by the way
153 const CCVector3 Q(P - *G);
154
155 const unsigned char X = m_quadricEquationDirections.x;
156 const unsigned char Y = m_quadricEquationDirections.y;
157
158 //z = a+b.x+c.y+d.x^2+e.x.y+f.y^2
159 //const PointCoordinateType& a = H[0];
160 const PointCoordinateType& b = H[1];
161 const PointCoordinateType& c = H[2];
162 const PointCoordinateType& d = H[3];
163 const PointCoordinateType& e = H[4];
164 const PointCoordinateType& f = H[5];
165
166 //See "CURVATURE OF CURVES AND SURFACES – A PARABOLIC APPROACH" by ZVI HAR’EL
167 const PointCoordinateType fx = b + (d*2) * Q.u[X] + (e ) * Q.u[Y]; // b+2d*X+eY
168 const PointCoordinateType fy = c + (e ) * Q.u[X] + (f*2) * Q.u[Y]; // c+2f*Y+eX
169 const PointCoordinateType fxx = d*2; // 2d
170 const PointCoordinateType fyy = f*2; // 2f
171 const PointCoordinateType& fxy = e; // e
172
173 const PointCoordinateType fx2 = fx*fx;
174 const PointCoordinateType fy2 = fy*fy;
175 const PointCoordinateType q = (1 + fx2 + fy2);
176
177 switch (cType)
178 {
179 case GAUSSIAN_CURV:
180 {
181 //to sign the curvature, we need a normal!
182 const PointCoordinateType K = std::abs(fxx*fyy - fxy * fxy) / (q*q);
183 return static_cast<ScalarType>(K);
184 }
185
186 case MEAN_CURV:
187 {
188 //to sign the curvature, we need a normal!
189 const PointCoordinateType H2 = std::abs(((1 + fx2)*fyy - 2 * fx*fy*fxy + (1 + fy2)*fxx)) / (2 * sqrt(q)*q);
190 return static_cast<ScalarType>(H2);
191 }
192
193 default:
194 assert(false);
195 break;
196 }
197 }
198 break;
199
200 case NORMAL_CHANGE_RATE:
201 {
202 assert(m_associatedCloud);
203 unsigned pointCount = (m_associatedCloud ? m_associatedCloud->size() : 0);
204
205 //we need at least 4 points
206 if (pointCount < 4)
207 {
208 //not enough points!
209 return pointCount == 3 ? 0 : NAN_VALUE;
210 }
211
212 //we determine plane normal by computing the smallest eigen value of M = 1/n * S[(p-µ)*(p-µ)']
213 CCLib::SquareMatrixd covMat = computeCovarianceMatrix();
214 CCVector3d e(0, 0, 0);
215 #ifdef USE_EIGEN
216 Eigen::Matrix3d A = ToEigen(covMat);
217 Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> es;
218 es.compute(A);
219
220 //eigen values (and vectors) are sorted in ascending order
221 const auto& eVal = es.eigenvalues();
222
223 //compute curvature as the rate of change of the surface
224 e = CCVector3d::fromArray(eVal.data());
225 #else
226 CCLib::SquareMatrixd eigVectors;
227 std::vector<double> eigValues;
228 if (!Jacobi<double>::ComputeEigenValuesAndVectors(covMat, eigVectors, eigValues, true))
229 {
230 //failure
231 return NAN_VALUE;
232 }
233
234 //compute curvature as the rate of change of the surface
235 e.x = eigValues[0];
236 e.y = eigValues[1];
237 e.z = eigValues[2];
238 #endif
239 const double sum = e.x + e.y + e.z; //we work with absolute values
240 if (sum < ZERO_TOLERANCE)
241 {
242 return NAN_VALUE;
243 }
244
245 const double eMin = std::min(std::min(e.x, e.y), e.z);
246 return static_cast<ScalarType>(eMin / sum);
247 }
248 break;
249
250 default:
251 assert(false);
252 }
253
254 return NAN_VALUE;
255 }
标签：std,case,const,CC,特征,PointCoordinateType,l1,点云,return From： https://www.cnblogs.com/yhlx125/p/16990918.html