Merge pull request #1056 from Sahloul:features/surface_matching/angle_enhancement

modules/surface_matching/src/c_utils.hpp

@@ -83,11 +83,15 @@ static inline void TCross(const double a[], const double b[], double c[])
   c[2] = (a[0])*(b[1])-(a[1])*(b[0]);
 }
 
-static inline double TAngle3(const double a[3], const double b[3])
+static inline double TAngle3Normalized(const double a[3], const double b[3])
 {
-  double c[3];
-  TCross(a,b,c);
-  return (atan2(TNorm3(c), TDot3(a, b)));
+  /*
+   angle = atan2(a dot b, |a x b|) # Bertram (accidental mistake)
+   angle = atan2(|a x b|, a dot b) # Tolga Birdal (correction)
+   angle = acos(a dot b)           # Hamdi Sahloul (simplification, a & b are normalized)
+  */
+
+  return acos(TDot3(a, b));
 }
 
 static inline void matrixProduct33(double *A, double *B, double *R)

modules/surface_matching/src/ppf_match_3d.cpp

@@ -191,20 +191,9 @@ void PPF3DDetector::computePPFFeatures(const double p1[4], const double n1[4],
     return ;
   }
 
-  /*
-  Tolga Birdal's note:
-  Issues of numerical stability is of concern here.
-  Bertram's suggestion: atan2(a dot b, |axb|)
-  My correction :
-  I guess it should be: angle = atan2(norm(cross(a,b)), dot(a,b))
-  The macro is implemented accordingly.
-  TAngle3 actually outputs in range [0, pi] as
-  Bertram suggests
-  */
-
-  f[0] = TAngle3(n1, d);
-  f[1] = TAngle3(n2, d);
-  f[2] = TAngle3(n1, n2);
+  f[0] = TAngle3Normalized(n1, d);
+  f[1] = TAngle3Normalized(n2, d);
+  f[2] = TAngle3Normalized(n1, n2);
 }
 
 void PPF3DDetector::clearTrainingModels()