CC点云特征计算

  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 }