Re-write surface matching using vectors and matrices

modules/surface_matching/include/opencv2/surface_matching/pose_3d.hpp

@@ -77,42 +77,40 @@ public:
     numVotes=0;
     residual = 0;
 
-    for (int i=0; i<16; i++)
-      pose[i]=0;
+    pose = Matx44d::all(0);
   }
 
-  Pose3D(double Alpha, unsigned int ModelIndex=0, unsigned int NumVotes=0)
+  Pose3D(double Alpha, size_t ModelIndex=0, size_t NumVotes=0)
   {
     alpha = Alpha;
     modelIndex = ModelIndex;
     numVotes = NumVotes;
     residual=0;
 
-    for (int i=0; i<16; i++)
-      pose[i]=0;
+    pose = Matx44d::all(0);
   }
 
   /**
    *  \brief Updates the pose with the new one
    *  \param [in] NewPose New pose to overwrite
    */
-  void updatePose(double NewPose[16]);
+  void updatePose(Matx44d& NewPose);
 
   /**
    *  \brief Updates the pose with the new one
    */
-  void updatePose(double NewR[9], double NewT[3]);
+  void updatePose(Matx33d& NewR, Vec3d& NewT);
 
   /**
    *  \brief Updates the pose with the new one, but this time using quaternions to represent rotation
    */
-  void updatePoseQuat(double Q[4], double NewT[3]);
+  void updatePoseQuat(Vec4d& Q, Vec3d& NewT);
 
   /**
    *  \brief Left multiplies the existing pose in order to update the transformation
    *  \param [in] IncrementalPose New pose to apply
    */
-  void appendPose(double IncrementalPose[16]);
+  void appendPose(Matx44d& IncrementalPose);
   void printPose();
 
   Pose3DPtr clone();
@@ -125,9 +123,11 @@ public:
   virtual ~Pose3D() {}
 
   double alpha, residual;
-  unsigned int modelIndex;
-  unsigned int numVotes;
-  double pose[16], angle, t[3], q[4];
+  size_t modelIndex, numVotes;
+  Matx44d pose;
+  double angle;
+  Vec3d t;
+  Vec4d q;
 };
 
 /**
@@ -135,7 +135,7 @@ public:
 * pose clusters occur. This class is a general container for such groups of poses. It is possible to store,
 * load and perform IO on these poses.
 */
-class CV_EXPORTS PoseCluster3D
+class CV_EXPORTS_W PoseCluster3D
 {
 public:
   PoseCluster3D()
@@ -175,7 +175,7 @@ public:
   int readPoseCluster(const std::string& FileName);
 
   std::vector<Pose3DPtr> poseList;
-  int numVotes;
+  size_t numVotes;
   int id;
 };
 

modules/surface_matching/include/opencv2/surface_matching/ppf_helpers.hpp

@@ -61,14 +61,14 @@ namespace ppf_match_3d
  *  and whether it should be loaded or not
  *  @return Returns the matrix on successfull load
  */
-CV_EXPORTS Mat loadPLYSimple(const char* fileName, int withNormals = 0);
+CV_EXPORTS_W Mat loadPLYSimple(const char* fileName, int withNormals = 0);
 
 /**
  *  @brief Write a point cloud to PLY file
  *  @param [in] PC Input point cloud
  *  @param [in] fileName The PLY model file to write
 */
-CV_EXPORTS void writePLY(Mat PC, const char* fileName);
+CV_EXPORTS_W void writePLY(Mat PC, const char* fileName);
 
 /**
 *  @brief Used for debbuging pruposes, writes a point cloud to a PLY file with the tip
@@ -76,7 +76,7 @@ CV_EXPORTS void writePLY(Mat PC, const char* fileName);
 *  @param [in] PC Input point cloud
 *  @param [in] fileName The PLY model file to write
 */
-CV_EXPORTS void writePLYVisibleNormals(Mat PC, const char* fileName);
+CV_EXPORTS_W void writePLYVisibleNormals(Mat PC, const char* fileName);
 
 Mat samplePCUniform(Mat PC, int sampleStep);
 Mat samplePCUniformInd(Mat PC, int sampleStep, std::vector<int>& indices);
@@ -94,26 +94,15 @@ Mat samplePCUniformInd(Mat PC, int sampleStep, std::vector<int>& indices);
  *  by the distance to the origin. This parameter enables/disables the use of weighting.
  *  @return Sampled point cloud
 */
-CV_EXPORTS Mat samplePCByQuantization(Mat pc, float xrange[2], float yrange[2], float zrange[2], float sample_step_relative, int weightByCenter=0);
+CV_EXPORTS_W Mat samplePCByQuantization(Mat pc, Vec2f& xrange, Vec2f& yrange, Vec2f& zrange, float sample_step_relative, int weightByCenter=0);
 
-void computeBboxStd(Mat pc, float xRange[2], float yRange[2], float zRange[2]);
+void computeBboxStd(Mat pc, Vec2f& xRange, Vec2f& yRange, Vec2f& zRange);
 
 void* indexPCFlann(Mat pc);
 void destroyFlann(void* flannIndex);
 void queryPCFlann(void* flannIndex, Mat& pc, Mat& indices, Mat& distances);
 void queryPCFlann(void* flannIndex, Mat& pc, Mat& indices, Mat& distances, const int numNeighbors);
 
-/**
- *  Mostly for visualization purposes. Normalizes the point cloud in a Hartley-Zissermann
- *  fashion. In other words, the point cloud is centered, and scaled such that the largest
- *  distance from the origin is sqrt(2). Finally a rescaling is applied.
- *  @param [in] pc Input point cloud (CV_32F family). Point clouds with 3 or 6 elements per
- *  row are expected.
- *  @param [in] scale The scale after normalization. Default to 1.
- *  @return Normalized point cloud
-*/
-CV_EXPORTS Mat normalize_pc(Mat pc, float scale);
-
 Mat normalizePCCoeff(Mat pc, float scale, float* Cx, float* Cy, float* Cz, float* MinVal, float* MaxVal);
 Mat transPCCoeff(Mat pc, float scale, float Cx, float Cy, float Cz, float MinVal, float MaxVal);
 
@@ -125,20 +114,20 @@ Mat transPCCoeff(Mat pc, float scale, float Cx, float Cy, float Cz, float MinVal
  *  @param [in] Pose 4x4 pose matrix, but linearized in row-major form.
  *  @return Transformed point cloud
 */
-CV_EXPORTS Mat transformPCPose(Mat pc, const double Pose[16]);
+CV_EXPORTS_W Mat transformPCPose(Mat pc, const Matx44d& Pose);
 
 /**
  *  Generate a random 4x4 pose matrix
  *  @param [out] Pose The random pose
 */
-CV_EXPORTS void getRandomPose(double Pose[16]);
+CV_EXPORTS_W void getRandomPose(Matx44d& Pose);
 
 /**
  *  Adds a uniform noise in the given scale to the input point cloud
  *  @param [in] pc Input point cloud (CV_32F family).
  *  @param [in] scale Input scale of the noise. The larger the scale, the more noisy the output
 */
-CV_EXPORTS Mat addNoisePC(Mat pc, double scale);
+CV_EXPORTS_W Mat addNoisePC(Mat pc, double scale);
 
 /**
  *  @brief Compute the normals of an arbitrary point cloud
@@ -154,7 +143,7 @@ CV_EXPORTS Mat addNoisePC(Mat pc, double scale);
  *  @param [in] viewpoint
  *  @return Returns 0 on success
  */
-CV_EXPORTS_W int computeNormalsPC3d(const Mat& PC, CV_OUT Mat& PCNormals, const int NumNeighbors, const bool FlipViewpoint, const Vec3d& viewpoint);
+CV_EXPORTS_W int computeNormalsPC3d(const Mat& PC, CV_OUT Mat& PCNormals, const int NumNeighbors, const bool FlipViewpoint, const Vec3f& viewpoint);
 
 //! @}
 

modules/surface_matching/include/opencv2/surface_matching/ppf_match_3d.hpp

@@ -94,7 +94,7 @@ typedef struct THash
   * detector.match(pcTest, results, 1.0/5.0,0.05);
   * @endcode
   */
-class CV_EXPORTS PPF3DDetector
+class CV_EXPORTS_W PPF3DDetector
 {
 public:
 
@@ -160,9 +160,9 @@ protected:
   void clearTrainingModels();
 
 private:
-  void computePPFFeatures(const double p1[4], const double n1[4],
-                          const double p2[4], const double n2[4],
-                          double f[4]);
+  void computePPFFeatures(const Vec3d& p1, const Vec3d& n1,
+                          const Vec3d& p2, const Vec3d& n2,
+                          Vec4d& f);
 
   bool matchPose(const Pose3D& sourcePose, const Pose3D& targetPose);
 

modules/surface_matching/include/opencv2/surface_matching/t_hash_int.hpp

@@ -55,7 +55,7 @@ namespace ppf_match_3d
 //! @addtogroup surface_matching
 //! @{
 
-typedef unsigned int KeyType;
+typedef uint KeyType;
 
 typedef struct hashnode_i
 {
@@ -68,7 +68,7 @@ typedef struct HSHTBL_i
 {
   size_t size;
   struct hashnode_i **nodes;
-  size_t (*hashfunc)(unsigned int);
+  size_t (*hashfunc)(uint);
 } hashtable_int;
 
 
@@ -76,7 +76,7 @@ typedef struct HSHTBL_i
 
 from http://www-graphics.stanford.edu/~seander/bithacks.html
 */
-inline static unsigned int next_power_of_two(unsigned int value)
+inline static uint next_power_of_two(uint value)
 {
 
   --value;
@@ -90,7 +90,7 @@ inline static unsigned int next_power_of_two(unsigned int value)
   return value;
 }
 
-hashtable_int *hashtableCreate(size_t size, size_t (*hashfunc)(unsigned int));
+hashtable_int *hashtableCreate(size_t size, size_t (*hashfunc)(uint));
 void hashtableDestroy(hashtable_int *hashtbl);
 int hashtableInsert(hashtable_int *hashtbl, KeyType key, void *data);
 int hashtableInsertHashed(hashtable_int *hashtbl, KeyType key, void *data);

modules/surface_matching/src/c_utils.hpp

@@ -55,35 +55,23 @@ const float EPS = 1.192092896e-07F;        /* smallest such that 1.0+FLT_EPSILON
 #define M_PI  3.1415926535897932384626433832795
 #endif
 
-static inline double TNorm3(const double v[])
+static inline void TNormalize3(Vec3d& v)
 {
-  return (sqrt(v[0]*v[0]+v[1]*v[1]+v[2]*v[2]));
-}
-
-static inline void TNormalize3(double v[])
-{
-  double normTemp=TNorm3(v);
-  if (normTemp>0)
+  double norm = cv::norm(v);
+  if (norm > EPS)
   {
-    v[0]/=normTemp;
-    v[1]/=normTemp;
-    v[2]/=normTemp;
+    v *= 1.0 / norm;
   }
 }
 
-static inline double TDot3(const double a[3], const double b[3])
-{
-  return  ((a[0])*(b[0])+(a[1])*(b[1])+(a[2])*(b[2]));
-}
-
-static inline void TCross(const double a[], const double b[], double c[])
-{
-  c[0] = (a[1])*(b[2])-(a[2])*(b[1]);
-  c[1] = (a[2])*(b[0])-(a[0])*(b[2]);
-  c[2] = (a[0])*(b[1])-(a[1])*(b[0]);
-}
-
-static inline double TAngle3Normalized(const double a[3], const double b[3])
+/**
+ *  \brief Calculate angle between two normalized vectors
+ *
+ *  \param [in] a normalized vector
+ *  \param [in] b normalized vector
+ *  \return angle between a and b vectors in radians
+ */
+static inline double TAngle3Normalized(const Vec3d& a, const Vec3d& b)
 {
   /*
    angle = atan2(a dot b, |a x b|) # Bertram (accidental mistake)
@@ -91,417 +79,161 @@ static inline double TAngle3Normalized(const double a[3], const double b[3])
    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)
-{
-  R[0] = A[0] * B[0] + A[1] * B[3] + A[2] * B[6];
-  R[1] = A[0] * B[1] + A[1] * B[4] + A[2] * B[7];
-  R[2] = A[0] * B[2] + A[1] * B[5] + A[2] * B[8];
-
-  R[3] = A[3] * B[0] + A[4] * B[3] + A[5] * B[6];
-  R[4] = A[3] * B[1] + A[4] * B[4] + A[5] * B[7];
-  R[5] = A[3] * B[2] + A[4] * B[5] + A[5] * B[8];
-
-  R[6] = A[6] * B[0] + A[7] * B[3] + A[8] * B[6];
-  R[7] = A[6] * B[1] + A[7] * B[4] + A[8] * B[7];
-  R[8] = A[6] * B[2] + A[7] * B[5] + A[8] * B[8];
+  return acos(a.dot(b));
 }
 
-// A is a vector
-static inline void matrixProduct133(double *A, double *B, double *R)
+static inline void rtToPose(const Matx33d& R, const Vec3d& t, Matx44d& Pose)
 {
-  R[0] = A[0] * B[0] + A[1] * B[3] + A[2] * B[6];
-  R[1] = A[0] * B[1] + A[1] * B[4] + A[2] * B[7];
-  R[2] = A[0] * B[2] + A[1] * B[5] + A[2] * B[8];
+  Matx34d P;
+  hconcat(R, t, P);
+  vconcat(P, Matx14d(0, 0, 0, 1), Pose);
 }
 
-static inline void matrixProduct331(const double A[9], const double b[3], double r[3])
+static inline void poseToR(const Matx44d& Pose, Matx33d& R)
 {
-  r[0] = A[0] * b[0] + A[1] * b[1] + A[2] * b[2];
-  r[1] = A[3] * b[0] + A[4] * b[1] + A[5] * b[2];
-  r[2] = A[6] * b[0] + A[7] * b[1] + A[8] * b[2];
+  Mat(Pose).rowRange(0, 3).colRange(0, 3).copyTo(R);
 }
 
-static inline void matrixTranspose33(double *A, double *At)
+static inline void poseToRT(const Matx44d& Pose, Matx33d& R, Vec3d& t)
 {
-  At[0] = A[0];
-  At[4] = A[4];
-  At[8] = A[8];
-  At[1] = A[3];
-  At[2] = A[6];
-  At[3] = A[1];
-  At[5] = A[7];
-  At[6] = A[2];
-  At[7] = A[5];
+  poseToR(Pose, R);
+  Mat(Pose).rowRange(0, 3).colRange(3, 4).copyTo(t);
 }
 
-static inline void matrixProduct44(const double A[16], const double B[16], double R[16])
-{
-  R[0] = A[0] * B[0] + A[1] * B[4] + A[2] * B[8] + A[3] * B[12];
-  R[1] = A[0] * B[1] + A[1] * B[5] + A[2] * B[9] + A[3] * B[13];
-  R[2] = A[0] * B[2] + A[1] * B[6] + A[2] * B[10] + A[3] * B[14];
-  R[3] = A[0] * B[3] + A[1] * B[7] + A[2] * B[11] + A[3] * B[15];
-
-  R[4] = A[4] * B[0] + A[5] * B[4] + A[6] * B[8] + A[7] * B[12];
-  R[5] = A[4] * B[1] + A[5] * B[5] + A[6] * B[9] + A[7] * B[13];
-  R[6] = A[4] * B[2] + A[5] * B[6] + A[6] * B[10] + A[7] * B[14];
-  R[7] = A[4] * B[3] + A[5] * B[7] + A[6] * B[11] + A[7] * B[15];
-
-  R[8] = A[8] * B[0] + A[9] * B[4] + A[10] * B[8] + A[11] * B[12];
-  R[9] = A[8] * B[1] + A[9] * B[5] + A[10] * B[9] + A[11] * B[13];
-  R[10] = A[8] * B[2] + A[9] * B[6] + A[10] * B[10] + A[11] * B[14];
-  R[11] = A[8] * B[3] + A[9] * B[7] + A[10] * B[11] + A[11] * B[15];
-
-  R[12] = A[12] * B[0] + A[13] * B[4] + A[14] * B[8] + A[15] * B[12];
-  R[13] = A[12] * B[1] + A[13] * B[5] + A[14] * B[9] + A[15] * B[13];
-  R[14] = A[12] * B[2] + A[13] * B[6] + A[14] * B[10] + A[15] * B[14];
-  R[15] = A[12] * B[3] + A[13] * B[7] + A[14] * B[11] + A[15] * B[15];
-}
-
-static inline void matrixProduct441(const double A[16], const double B[4], double R[4])
+/**
+ *  \brief Axis angle to rotation
+ */
+static inline void aaToR(const Vec3d& axis, double angle, Matx33d& R)
 {
-  R[0] = A[0] * B[0] + A[1] * B[1] + A[2] * B[2] + A[3] * B[3];
-  R[1] = A[4] * B[0] + A[5] * B[1] + A[6] * B[2] + A[7] * B[3];
-  R[2] = A[8] * B[0] + A[9] * B[1] + A[10] * B[2] + A[11] * B[3];
-  R[3] = A[12] * B[0] + A[13] * B[1] + A[14] * B[2] + A[15] * B[3];
-}
+  const double sinA = sin(angle);
+  const double cosA = cos(angle);
+  const double cos1A = (1 - cosA);
+  uint i, j;
 
-static inline void matrixPrint(double *A, int m, int n)
-{
-  int i, j;
+  Mat(cosA * Matx33d::eye()).copyTo(R);
 
-  for (i = 0; i < m; i++)
+  for (i = 0; i < 3; i++)
+    for (j = 0; j < 3; j++)
     {
-    printf("  ");
-    for (j = 0; j < n; j++)
+      if (i != j)
       {
-      printf(" %0.6f ", A[i * n + j]);
+        // Symmetry skew matrix
+        R(i, j) += (((i + 1) % 3 == j) ? -1 : 1) * sinA * axis[3 - i - j];
       }
-    printf("\n");
+      R(i, j) += cos1A * axis[i] * axis[j];
     }
 }
 
-static inline void matrixIdentity(int n, double *A)
-{
-  int i;
-
-  for (i = 0; i < n*n; i++)
-  {
-    A[i] = 0.0;
-  }
-
-  for (i = 0; i < n; i++)
-  {
-    A[i * n + i] = 1.0;
-  }
-}
-
-static inline void rtToPose(const double R[9], const double t[3], double Pose[16])
-{
-  Pose[0]=R[0];
-  Pose[1]=R[1];
-  Pose[2]=R[2];
-  Pose[4]=R[3];
-  Pose[5]=R[4];
-  Pose[6]=R[5];
-  Pose[8]=R[6];
-  Pose[9]=R[7];
-  Pose[10]=R[8];
-  Pose[3]=t[0];
-  Pose[7]=t[1];
-  Pose[11]=t[2];
-
-  Pose[15] = 1;
-}
-
-
-static inline void poseToRT(const double Pose[16], double R[9], double t[3])
-{
-  R[0] = Pose[0];
-  R[1] = Pose[1];
-  R[2] = Pose[2];
-  R[3] = Pose[4];
-  R[4] = Pose[5];
-  R[5] = Pose[6];
-  R[6] = Pose[8];
-  R[7] = Pose[9];
-  R[8] = Pose[10];
-
-  t[0]=Pose[3];
-  t[1]=Pose[7];
-  t[2]=Pose[11];
-}
-
-static inline void poseToR(const double Pose[16], double R[9])
-{
-  R[0] = Pose[0];
-  R[1] = Pose[1];
-  R[2] = Pose[2];
-  R[3] = Pose[4];
-  R[4] = Pose[5];
-  R[5] = Pose[6];
-  R[6] = Pose[8];
-  R[7] = Pose[9];
-  R[8] = Pose[10];
-}
-
 /**
- *  \brief Axis angle to rotation but only compute y and z components
+ *  \brief Compute a rotation in order to rotate around X direction
  */
-static inline void aaToRyz(double angle, const double r[3], double row2[3], double row3[3])
+static inline void getUnitXRotation(double angle, Matx33d& Rx)
 {
-  const double sinA=sin(angle);
-  const double cosA=cos(angle);
-  const double cos1A=(1-cosA);
-
-  row2[0] =  0.f;
-  row2[1] = cosA;
-  row2[2] =  0.f;
-  row3[0] =  0.f;
-  row3[1] =  0.f;
-  row3[2] = cosA;
-
-  row2[0] +=  r[2] * sinA;
-  row2[2] += -r[0] * sinA;
-  row3[0] += -r[1] * sinA;
-  row3[1] +=  r[0] * sinA;
-
-  row2[0] += r[1] * r[0] * cos1A;
-  row2[1] += r[1] * r[1] * cos1A;
-  row2[2] += r[1] * r[2] * cos1A;
-  row3[0] += r[2] * r[0] * cos1A;
-  row3[1] += r[2] * r[1] * cos1A;
-  row3[2] += r[2] * r[2] * cos1A;
+  const double sx = sin(angle);
+  const double cx = cos(angle);
+
+  Mat(Rx.eye()).copyTo(Rx);
+  Rx(1, 1) = cx;
+  Rx(1, 2) = -sx;
+  Rx(2, 1) = sx;
+  Rx(2, 2) = cx;
 }
 
 /**
- *  \brief Axis angle to rotation
- */
-static inline void aaToR(double angle, const double r[3], double R[9])
+*  \brief Compute a rotation in order to rotate around Y direction
+*/
+static inline void getUnitYRotation(double angle, Matx33d& Ry)
 {
-  const double sinA=sin(angle);
-  const double cosA=cos(angle);
-  const double cos1A=(1-cosA);
-  double *row1 = &R[0];
-  double *row2 = &R[3];
-  double *row3 = &R[6];
-
-  row1[0] =  cosA;
-  row1[1] = 0.0f;
-  row1[2] =  0.f;
-  row2[0] =  0.f;
-  row2[1] = cosA;
-  row2[2] =  0.f;
-  row3[0] =  0.f;
-  row3[1] =  0.f;
-  row3[2] = cosA;
-
-  row1[1] += -r[2] * sinA;
-  row1[2] +=  r[1] * sinA;
-  row2[0] +=  r[2] * sinA;
-  row2[2] += -r[0] * sinA;
-  row3[0] += -r[1] * sinA;
-  row3[1] +=  r[0] * sinA;
-
-  row1[0] += r[0] * r[0] * cos1A;
-  row1[1] += r[0] * r[1] * cos1A;
-  row1[2] += r[0] * r[2] * cos1A;
-  row2[0] += r[1] * r[0] * cos1A;
-  row2[1] += r[1] * r[1] * cos1A;
-  row2[2] += r[1] * r[2] * cos1A;
-  row3[0] += r[2] * r[0] * cos1A;
-  row3[1] += r[2] * r[1] * cos1A;
-  row3[2] += r[2] * r[2] * cos1A;
+  const double sy = sin(angle);
+  const double cy = cos(angle);
+
+  Mat(Ry.eye()).copyTo(Ry);
+  Ry(0, 0) = cy;
+  Ry(0, 2) = sy;
+  Ry(2, 0) = -sy;
+  Ry(2, 2) = cy;
 }
 
 /**
- *  \brief Compute a rotation in order to rotate around X direction
- */
-static inline void getUnitXRotation(double angle, double R[9])
+*  \brief Compute a rotation in order to rotate around Z direction
+*/
+static inline void getUnitZRotation(double angle, Matx33d& Rz)
 {
-  const double sinA=sin(angle);
-  const double cosA=cos(angle);
-  double *row1 = &R[0];
-  double *row2 = &R[3];
-  double *row3 = &R[6];
-
-  row1[0] =  1;
-  row1[1] = 0.0f;
-  row1[2] =  0.f;
-  row2[0] =  0.f;
-  row2[1] = cosA;
-  row2[2] =  -sinA;
-  row3[0] =  0.f;
-  row3[1] =  sinA;
-  row3[2] = cosA;
-}
-/**
- *  \brief Compute a transformation in order to rotate around X direction
- */
-static inline void getUnitXRotation_44(double angle, double T[16])
-{
-  const double sinA=sin(angle);
-  const double cosA=cos(angle);
-  double *row1 = &T[0];
-  double *row2 = &T[4];
-  double *row3 = &T[8];
-
-  row1[0] =  1;
-  row1[1] = 0.0f;
-  row1[2] =  0.f;
-  row2[0] =  0.f;
-  row2[1] = cosA;
-  row2[2] =  -sinA;
-  row3[0] =  0.f;
-  row3[1] =  sinA;
-  row3[2] = cosA;
-
-  row1[3]=0;
-  row2[3]=0;
-  row3[3]=0;
-  T[3]=0;
-  T[7]=0;
-  T[11]=0;
-  T[15] = 1;
+  const double sz = sin(angle);
+  const double cz = cos(angle);
+
+  Mat(Rz.eye()).copyTo(Rz);
+  Rz(0, 0) = cz;
+  Rz(0, 1) = -sz;
+  Rz(1, 0) = sz;
+  Rz(1, 1) = cz;
 }
 
 /**
- *  \brief Compute the yz components of the transformation needed to rotate n1 onto x axis and p1 to origin
- */
-static inline void computeTransformRTyz(const double p1[4], const double n1[4], double row2[3], double row3[3], double t[3])
+*  \brief Convert euler representation to rotation matrix
+*
+*  \param [in] euler RPY angles
+*  \param [out] R 3x3 Rotation matrix
+*/
+static inline void eulerToDCM(const Vec3d& euler, Matx33d& R)
 {
-  // dot product with x axis
-  double angle=acos( n1[0] );
+  Matx33d Rx, Ry, Rz;
 
-  // cross product with x axis
-  double axis[3]={0, n1[2], -n1[1]};
-  double axisNorm;
-
-  // we try to project on the ground plane but it's already parallel
-  if (n1[1]==0 && n1[2]==0)
-  {
-    axis[1]=1;
-    axis[2]=0;
-  }
-  else
-  {
-    axisNorm=sqrt(axis[2]*axis[2]+axis[1]*axis[1]);
+  getUnitXRotation(euler[0], Rx);
+  getUnitYRotation(euler[1], Ry);
+  getUnitZRotation(euler[2], Rz);
 
-    if (axisNorm>EPS)
-    {
-      axis[1]/=axisNorm;
-      axis[2]/=axisNorm;
-    }
-  }
-
-  aaToRyz(angle, axis, row2, row3);
-
-  t[1] = row2[0] * (-p1[0]) + row2[1] * (-p1[1]) + row2[2] * (-p1[2]);
-  t[2] = row3[0] * (-p1[0]) + row3[1] * (-p1[1]) + row3[2] * (-p1[2]);
+  Mat(Rx * (Ry * Rz)).copyTo(R);
 }
 
+
 /**
  *  \brief Compute the transformation needed to rotate n1 onto x axis and p1 to origin
  */
-static inline void computeTransformRT(const double p1[4], const double n1[4], double R[9], double t[3])
+static inline void computeTransformRT(const Vec3d& p1, const Vec3d& n1, Matx33d& R, Vec3d& t)
 {
   // dot product with x axis
-  double angle=acos( n1[0] );
+  double angle = acos(n1[0]);
 
   // cross product with x axis
-  double axis[3]={0, n1[2], -n1[1]};
-  double axisNorm;
-  double *row1, *row2, *row3;
+  Vec3d axis(0, n1[2], -n1[1]);
 
   // we try to project on the ground plane but it's already parallel
-  if (n1[1]==0 && n1[2]==0)
+  if (n1[1] == 0 && n1[2] == 0)
   {
-    axis[1]=1;
-    axis[2]=0;
+    axis[1] = 1;
+    axis[2] = 0;
   }
   else
   {
-    axisNorm=sqrt(axis[2]*axis[2]+axis[1]*axis[1]);
-
-    if (axisNorm>EPS)
-    {
-      axis[1]/=axisNorm;
-      axis[2]/=axisNorm;
-    }
+    TNormalize3(axis);
   }
 
-  aaToR(angle, axis, R);
-  row1 = &R[0];
-  row2 = &R[3];
-  row3 = &R[6];
-
-  t[0] = row1[0] * (-p1[0]) + row1[1] * (-p1[1]) + row1[2] * (-p1[2]);
-  t[1] = row2[0] * (-p1[0]) + row2[1] * (-p1[1]) + row2[2] * (-p1[2]);
-  t[2] = row3[0] * (-p1[0]) + row3[1] * (-p1[1]) + row3[2] * (-p1[2]);
+  aaToR(axis, angle, R);
+  t = -R * p1;
 }
 
 /**
  *  \brief Flip a normal to the viewing direction
  *
  *  \param [in] point Scene point
- *  \param [in] vp_x X component of view direction
- *  \param [in] vp_y Y component of view direction
- *  \param [in] vp_z Z component of view direction
- *  \param [in] nx X component of normal
- *  \param [in] ny Y component of normal
- *  \param [in] nz Z component of normal
- */
-static inline void flipNormalViewpoint(const float* point, double vp_x, double vp_y, double vp_z, double *nx, double *ny, double *nz)
-{
-  double cos_theta;
-
-  // See if we need to flip any plane normals
-  vp_x -= (double)point[0];
-  vp_y -= (double)point[1];
-  vp_z -= (double)point[2];
-
-  // Dot product between the (viewpoint - point) and the plane normal
-  cos_theta = (vp_x * (*nx) + vp_y * (*ny) + vp_z * (*nz));
-
-  // Flip the plane normal
-  if (cos_theta < 0)
-  {
-    (*nx) *= -1;
-    (*ny) *= -1;
-    (*nz) *= -1;
-  }
-}
-/**
- *  \brief Flip a normal to the viewing direction
- *
- *  \param [in] point Scene point
- *  \param [in] vp_x X component of view direction
- *  \param [in] vp_y Y component of view direction
- *  \param [in] vp_z Z component of view direction
- *  \param [in] nx X component of normal
- *  \param [in] ny Y component of normal
- *  \param [in] nz Z component of normal
+ *  \param [in] vp view direction
+ *  \param [in] n normal
  */
-static inline void flipNormalViewpoint_32f(const float* point, float vp_x, float vp_y, float vp_z, float *nx, float *ny, float *nz)
+static inline void flipNormalViewpoint(const Vec3f& point, const Vec3f& vp, Vec3f& n)
 {
   float cos_theta;
 
   // See if we need to flip any plane normals
-  vp_x -= (float)point[0];
-  vp_y -= (float)point[1];
-  vp_z -= (float)point[2];
+  Vec3f diff = vp - point;
 
   // Dot product between the (viewpoint - point) and the plane normal
-  cos_theta = (vp_x * (*nx) + vp_y * (*ny) + vp_z * (*nz));
+  cos_theta = diff.dot(n);
 
   // Flip the plane normal
   if (cos_theta < 0)
   {
-    (*nx) *= -1;
-    (*ny) *= -1;
-    (*nz) *= -1;
+    n *= -1;
   }
 }
 
@@ -509,25 +241,16 @@ static inline void flipNormalViewpoint_32f(const float* point, float vp_x, float
  *  \brief Convert a rotation matrix to axis angle representation
  *
  *  \param [in] R Rotation matrix
- *  \param [in] axis Axis vector
- *  \param [in] angle Angle in radians
+ *  \param [out] axis Axis vector
+ *  \param [out] angle Angle in radians
  */
-static inline void dcmToAA(double *R, double *axis, double *angle)
+static inline void dcmToAA(Matx33d& R, Vec3d& axis, double *angle)
 {
-  double d1 = R[7] - R[5];
-  double d2 = R[2] - R[6];
-  double d3 = R[3] - R[1];
-
-  double norm = sqrt(d1 * d1 + d2 * d2 + d3 * d3);
-  double x = (R[7] - R[5]) / norm;
-  double y = (R[2] - R[6]) / norm;
-  double z = (R[3] - R[1]) / norm;
-
-  *angle = acos((R[0] + R[4] + R[8] - 1.0) * 0.5);
-
-  axis[0] = x;
-  axis[1] = y;
-  axis[2] = z;
+  Mat(Vec3d(R(2, 1) - R(2, 1),
+            R(0, 2) - R(2, 0),
+            R(1, 0) - R(0, 1))).copyTo(axis);
+  TNormalize3(axis);
+  *angle = acos(0.5 * (cv::trace(R) - 1.0));
 }
 
 /**
@@ -535,39 +258,18 @@ static inline void dcmToAA(double *R, double *axis, double *angle)
  *
  *  \param [in] axis Axis Vector
  *  \param [in] angle Angle (In radians)
- *  \param [in] R 3x3 Rotation matrix
+ *  \param [out] R 3x3 Rotation matrix
  */
-static inline void aaToDCM(double *axis, double angle, double *R)
+static inline void aaToDCM(const Vec3d& axis, double angle, Matx33d& R)
 {
-  double ident[9]={1,0,0,0,1,0,0,0,1};
-  double n[9] = { 0.0, -axis[2], axis[1],
-                  axis[2], 0.0, -axis[0],
-                  -axis[1], axis[0], 0.0
-                };
-
-  double nsq[9];
-  double c, s;
-  int i;
-
-  //c = 1-cos(angle);
-  c = cos(angle);
-  s = sin(angle);
-
-  matrixProduct33(n, n, nsq);
-
-  for (i = 0; i < 9; i++)
-  {
-    const double sni = n[i]*s;
-    const double cnsqi = nsq[i]*(c);
-    R[i]=ident[i]+sni+cnsqi;
-  }
-
-  // The below code is the matrix based implemntation of the above
-  // double nsq[9], sn[9], cnsq[9], tmp[9];
-  //matrix_scale(3, 3, n, s, sn);
-  //matrix_scale(3, 3, nsq, (1 - c), cnsq);
-  //matrix_sum(3, 3, 3, 3, ident, sn, tmp);
-  //matrix_sum(3, 3, 3, 3, tmp, cnsq, R);
+  uint i, j;
+  Matx33d n = Matx33d::all(0);
+
+  for (i = 0; i < 3; i++)
+    for (j = 0; j < 3; j++)
+      if (i != j)
+        n(i, j) = (((i + 1) % 3 == j) ? -1 : 1) * axis[3 - i - j];
+  Mat(Matx33d::eye() + sin(angle) * n + cos(angle) * n * n).copyTo(R);
 }
 
 /**
@@ -576,52 +278,20 @@ static inline void aaToDCM(double *axis, double angle, double *R)
  *  \param [in] R Rotation Matrix
  *  \param [in] q Quaternion
  */
-static inline void dcmToQuat(double *R, double *q)
+static inline void dcmToQuat(Matx33d& R, Vec4d& q)
 {
-  double n4; // the norm of quaternion multiplied by 4
-  double tr = R[0] + R[4] + R[8]; // trace of martix
-  double factor;
-
-  if (tr > 0.0)
+  double tr = cv::trace(R);
+  Vec3d v(R(0, 0), R(1, 1), R(2, 2));
+  int idx = tr > 0.0 ? 3 : (int)(std::max_element(v.val, v.val + 3) - v.val);
+  double norm4 = q[(idx + 1) % 4] = 1.0 + (tr > 0.0 ? tr : 2 * R(idx, idx) - tr);
+  int i, prev, next, step = idx % 2 ? 1 : -1, curr = 3;
+  for (i = 0; i < 3; i++)
   {
-    q[1] = R[5] - R[7];
-    q[2] = R[6] - R[2];
-    q[3] = R[1] - R[3];
-    q[0] = tr + 1.0;
-    n4 = q[0];
+    curr = (curr + step) % 4;
+    next = (curr + 1) % 3, prev = (curr + 2) % 3;
+    q[(idx + i + 2) % 4] = R(next, prev) + (tr > 0.0 || idx == curr ? -1 : 1) * R(prev, next);
   }
-  else
-    if ((R[0] > R[4]) && (R[0] > R[8]))
-    {
-      q[1] = 1.0 + R[0] - R[4] - R[8];
-      q[2] = R[3] + R[1];
-      q[3] = R[6] + R[2];
-      q[0] = R[5] - R[7];
-      n4 = q[1];
-    }
-    else
-      if (R[4] > R[8])
-      {
-        q[1] = R[3] + R[1];
-        q[2] = 1.0 + R[4] - R[0] - R[8];
-        q[3] = R[7] + R[5];
-        q[0] = R[6] - R[2];
-        n4 = q[2];
-      }
-      else
-      {
-        q[1] = R[6] + R[2];
-        q[2] = R[7] + R[5];
-        q[3] = 1.0 + R[8] - R[0] - R[4];
-        q[0] = R[1] - R[3];
-        n4 = q[3];
-      }
-
-  factor = 0.5 / sqrt(n4);
-  q[0] *= factor;
-  q[1] *= factor;
-  q[2] *= factor;
-  q[3] *= factor;
+  q *= 0.5 / sqrt(norm4);
 }
 
 /**
@@ -631,36 +301,33 @@ static inline void dcmToQuat(double *R, double *q)
  *  \param [in] R Rotation Matrix
  *
  */
-static inline void quatToDCM(double *q, double *R)
+static inline void quatToDCM(Vec4d& q, Matx33d& R)
 {
-  double sqw = q[0] * q[0];
-  double sqx = q[1] * q[1];
-  double sqy = q[2] * q[2];
-  double sqz = q[3] * q[3];
+  Vec4d sq = q.mul(q);
 
   double tmp1, tmp2;
 
-  R[0] =  sqx - sqy - sqz + sqw; // since sqw + sqx + sqy + sqz = 1
-  R[4] = -sqx + sqy - sqz + sqw;
-  R[8] = -sqx - sqy + sqz + sqw;
+  R(0, 0) = sq[0] + sq[1] - sq[2] - sq[3]; // since norm(q) = 1
+  R(1, 1) = sq[0] - sq[1] + sq[2] - sq[3];
+  R(2, 2) = sq[0] - sq[1] - sq[2] + sq[3];
 
   tmp1 = q[1] * q[2];
   tmp2 = q[3] * q[0];
 
-  R[1] = 2.0 * (tmp1 + tmp2);
-  R[3] = 2.0 * (tmp1 - tmp2);
+  R(0, 1) = 2.0 * (tmp1 + tmp2);
+  R(1, 0) = 2.0 * (tmp1 - tmp2);
 
   tmp1 = q[1] * q[3];
   tmp2 = q[2] * q[0];
 
-  R[2] = 2.0 * (tmp1 - tmp2);
-  R[6] = 2.0 * (tmp1 + tmp2);
+  R(0, 2) = 2.0 * (tmp1 - tmp2);
+  R(2, 0) = 2.0 * (tmp1 + tmp2);
 
   tmp1 = q[2] * q[3];
   tmp2 = q[1] * q[0];
 
-  R[5] = 2.0 * (tmp1 + tmp2);
-  R[7] = 2.0 * (tmp1 - tmp2);
+  R(1, 2) = 2.0 * (tmp1 + tmp2);
+  R(2, 1) = 2.0 * (tmp1 - tmp2);
 }
 
 } // namespace ppf_match_3d

modules/surface_matching/src/hash_murmur64.hpp

@@ -26,7 +26,7 @@ THE SOFTWARE.
 // Block read - if your platform needs to do endian-swapping or can only
 // handle aligned reads, do the conversion here
 
-FORCE_INLINE unsigned int getblock ( const unsigned int * p, int i )
+FORCE_INLINE uint getblock ( const uint * p, int i )
 {
   return p[i];
 }
@@ -36,7 +36,7 @@ FORCE_INLINE unsigned int getblock ( const unsigned int * p, int i )
 
 // avalanches all bits to within 0.25% bias
 
-FORCE_INLINE unsigned int fmix32 ( unsigned int h )
+FORCE_INLINE uint fmix32 ( uint h )
 {
   h ^= h >> 16;
   h *= 0x85ebca6b;
@@ -49,7 +49,7 @@ FORCE_INLINE unsigned int fmix32 ( unsigned int h )
 
 //-----------------------------------------------------------------------------
 
-FORCE_INLINE void bmix32 ( unsigned int & h1, unsigned int & k1, unsigned int & c1, unsigned int & c2 )
+FORCE_INLINE void bmix32 ( uint & h1, uint & k1, uint & c1, uint & c2 )
 {
   k1 *= c1;
   k1  = ROTL32(k1,11);
@@ -64,7 +64,7 @@ FORCE_INLINE void bmix32 ( unsigned int & h1, unsigned int & k1, unsigned int &
 
 //-----------------------------------------------------------------------------
 
-FORCE_INLINE void bmix32 ( unsigned int & h1, unsigned int & h2, unsigned int & k1, unsigned int & k2, unsigned int & c1, unsigned int & c2 )
+FORCE_INLINE void bmix32 ( uint & h1, uint & h2, uint & k1, uint & k2, uint & c1, uint & c2 )
 {
   k1 *= c1;
   k1  = ROTL32(k1,11);
@@ -89,26 +89,26 @@ FORCE_INLINE void bmix32 ( unsigned int & h1, unsigned int & h2, unsigned int &
 
 //----------
 
-FORCE_INLINE void hashMurmurx64 ( const void * key, const int len, const unsigned int seed, void * out )
+FORCE_INLINE void hashMurmurx64 ( const void * key, const int len, const uint seed, void * out )
 {
-  const unsigned char * data = (const unsigned char*)key;
+  const uchar * data = (const uchar*)key;
   const int nblocks = len / 8;
 
-  unsigned int h1 = 0x8de1c3ac ^ seed;
-  unsigned int h2 = 0xbab98226 ^ seed;
+  uint h1 = 0x8de1c3ac ^ seed;
+  uint h2 = 0xbab98226 ^ seed;
 
-  unsigned int c1 = 0x95543787;
-  unsigned int c2 = 0x2ad7eb25;
+  uint c1 = 0x95543787;
+  uint c2 = 0x2ad7eb25;
 
   //----------
   // body
 
-  const unsigned int * blocks = (const unsigned int *)(data + nblocks*8);
+  const uint * blocks = (const uint *)(data + nblocks*8);
 
   for (int i = -nblocks; i; i++)
   {
-    unsigned int k1 = getblock(blocks,i*2+0);
-    unsigned int k2 = getblock(blocks,i*2+1);
+    uint k1 = getblock(blocks,i*2+0);
+    uint k2 = getblock(blocks,i*2+1);
 
     bmix32(h1,h2,k1,k2,c1,c2);
   }
@@ -116,10 +116,10 @@ FORCE_INLINE void hashMurmurx64 ( const void * key, const int len, const unsigne
   //----------
   // tail
 
-  const unsigned char * tail = (const unsigned char*)(data + nblocks*8);
+  const uchar * tail = (const uchar*)(data + nblocks*8);
 
-  unsigned int k1 = 0;
-  unsigned int k2 = 0;
+  uint k1 = 0;
+  uint k2 = 0;
 
   switch (len & 7)
   {
@@ -154,8 +154,8 @@ FORCE_INLINE void hashMurmurx64 ( const void * key, const int len, const unsigne
   h1 += h2;
   h2 += h1;
 
-  ((unsigned int*)out)[0] = h1;
-  ((unsigned int*)out)[1] = h2;
+  ((uint*)out)[0] = h1;
+  ((uint*)out)[1] = h2;
 }
 
 

modules/surface_matching/src/hash_murmur86.hpp

@@ -14,9 +14,13 @@
 /* We can't use the name 'uint32_t' here because it will conflict with
 * any version provided by the system headers or application. */
 
+#if !defined(ulong)
+#define ulong unsigned long
+#endif
+
 /* First look for special cases */
 #if defined(_MSC_VER)
-#define MH_UINT32 unsigned long
+#define MH_UINT32 ulong
 #endif
 
 /* If the compiler says it's C99 then take its word for it */
@@ -30,25 +34,25 @@
 #if !defined(MH_UINT32)
 #include  <limits.h>
 #if   (USHRT_MAX == 0xffffffffUL)
-#define MH_UINT32 unsigned short
+#define MH_UINT32 ushort
 #elif (UINT_MAX == 0xffffffffUL)
-#define MH_UINT32 unsigned int
+#define MH_UINT32 uint
 #elif (ULONG_MAX == 0xffffffffUL)
-#define MH_UINT32 unsigned long
+#define MH_UINT32 ulong
 #endif
 #endif
 
 #if !defined(MH_UINT32)
-#error Unable to determine type name for unsigned 32-bit int
+#error Unable to determine type name for u32-bit int
 #endif
 
-/* I'm yet to work on a platform where 'unsigned char' is not 8 bits */
-#define MH_UINT8  unsigned char
+/* I'm yet to work on a platform where 'uchar' is not 8 bits */
+#define MH_UINT8  uchar
 
 void PMurHash32_Process(MH_UINT32 *ph1, MH_UINT32 *pcarry, const void *key, int len);
 MH_UINT32 PMurHash32_Result(MH_UINT32 h1, MH_UINT32 carry, MH_UINT32 total_length);
 MH_UINT32 PMurHash32(MH_UINT32 seed, const void *key, int len);
-void hashMurmurx86 ( const void * key, const int len, const unsigned int seed, void * out );
+void hashMurmurx86 ( const void * key, const int len, const uint seed, void * out );
 
 /* I used ugly type names in the header to avoid potential conflicts with
 * application or system typedefs & defines. Since I'm not including any more
@@ -69,7 +73,7 @@ void hashMurmurx86 ( const void * key, const int len, const unsigned int seed, v
 * The following 3 macros are defined in this section. The other macros defined
 * are only needed to help derive these 3.
 *
-* READ_UINT32(x)   Read a little endian unsigned 32-bit int
+* READ_UINT32(x)   Read a little endian u32-bit int
 * UNALIGNED_SAFE   Defined if READ_UINT32 works on non-word boundaries
 * ROTL32(x,r)      Rotate x left by r bits
 */
@@ -293,8 +297,8 @@ uint32_t PMurHash32(uint32_t seed, const void *key, int len)
     return PMurHash32_Result(h1, carry, len);
 }
 
-void hashMurmurx86 ( const void * key, const int len, const unsigned int seed, void * out )
+void hashMurmurx86 ( const void * key, const int len, const uint seed, void * out )
 {
-    *(unsigned int*)out = PMurHash32 (seed, key, len);
+    *(uint*)out = PMurHash32 (seed, key, len);
 }
 

modules/surface_matching/src/icp.cpp

@@ -44,7 +44,7 @@ namespace cv
 {
 namespace ppf_match_3d
 {
-static void subtractColumns(Mat srcPC, double mean[3])
+static void subtractColumns(Mat srcPC, Vec3d& mean)
 {
   int height = srcPC.rows;
 
@@ -60,7 +60,7 @@ static void subtractColumns(Mat srcPC, double mean[3])
 }
 
 // as in PCA
-static void computeMeanCols(Mat srcPC, double mean[3])
+static void computeMeanCols(Mat srcPC, Vec3d& mean)
 {
   int height = srcPC.rows;
 
@@ -86,7 +86,7 @@ static void computeMeanCols(Mat srcPC, double mean[3])
 }
 
 // as in PCA
-/*static void subtractMeanFromColumns(Mat srcPC, double mean[3])
+/*static void subtractMeanFromColumns(Mat srcPC, Vec3d& mean)
 {
     computeMeanCols(srcPC, mean);
     subtractColumns(srcPC, mean);
@@ -192,88 +192,38 @@ static float getRejectionThreshold(float* r, int m, float outlierScale)
 }
 
 // Kok Lim Low's linearization
-static void minimizePointToPlaneMetric(Mat Src, Mat Dst, Mat& X)
+static void minimizePointToPlaneMetric(Mat Src, Mat Dst, Vec3d& rpy, Vec3d& t)
 {
   //Mat sub = Dst - Src;
   Mat A = Mat(Src.rows, 6, CV_64F);
   Mat b = Mat(Src.rows, 1, CV_64F);
+  Mat rpy_t;
 
 #if defined _OPENMP
 #pragma omp parallel for
 #endif
   for (int i=0; i<Src.rows; i++)
   {
-    const double *srcPt = Src.ptr<double>(i);
-    const double *dstPt = Dst.ptr<double>(i);
-    const double *normals = &dstPt[3];
-    double *bVal = b.ptr<double>(i);
-    double *aRow = A.ptr<double>(i);
-
-    const double sub[3]={dstPt[0]-srcPt[0], dstPt[1]-srcPt[1], dstPt[2]-srcPt[2]};
-
-    *bVal = TDot3(sub, normals);
-    TCross(srcPt, normals, aRow);
-
-    aRow[3] = normals[0];
-    aRow[4] = normals[1];
-    aRow[5] = normals[2];
+    const Vec3d srcPt(Src.ptr<double>(i));
+    const Vec3d dstPt(Dst.ptr<double>(i));
+    const Vec3d normals(Dst.ptr<double>(i) + 3);
+    const Vec3d sub = dstPt - srcPt;
+    const Vec3d axis = srcPt.cross(normals);
+
+    *b.ptr<double>(i) = sub.dot(normals);
+    hconcat(axis.reshape<1, 3>(), normals.reshape<1, 3>(), A.row(i));
   }
 
-  cv::solve(A, b, X, DECOMP_SVD);
+  cv::solve(A, b, rpy_t, DECOMP_SVD);
+  rpy_t.rowRange(0, 3).copyTo(rpy);
+  rpy_t.rowRange(3, 6).copyTo(t);
 }
 
-
-static void getTransformMat(Mat X, double Pose[16])
+static void getTransformMat(Vec3d& euler, Vec3d& t, Matx44d& Pose)
 {
-  Mat DCM;
-  double *r1, *r2, *r3;
-  double* x = (double*)X.data;
-
-  const double sx = sin(x[0]);
-  const double cx = cos(x[0]);
-  const double sy = sin(x[1]);
-  const double cy = cos(x[1]);
-  const double sz = sin(x[2]);
-  const double cz = cos(x[2]);
-
-  Mat R1 = Mat::eye(3,3, CV_64F);
-  Mat R2 = Mat::eye(3,3, CV_64F);
-  Mat R3 = Mat::eye(3,3, CV_64F);
-
-  r1= (double*)R1.data;
-  r2= (double*)R2.data;
-  r3= (double*)R3.data;
-
-  r1[4]= cx;
-  r1[5]= -sx;
-  r1[7]= sx;
-  r1[8]= cx;
-
-  r2[0]= cy;
-  r2[2]= sy;
-  r2[6]= -sy;
-  r2[8]= cy;
-
-  r3[0]= cz;
-  r3[1]= -sz;
-  r3[3]= sz;
-  r3[4]= cz;
-
-  DCM = R1*(R2*R3);
-
-  Pose[0] = DCM.at<double>(0,0);
-  Pose[1] = DCM.at<double>(0,1);
-  Pose[2] = DCM.at<double>(0,2);
-  Pose[4] = DCM.at<double>(1,0);
-  Pose[5] = DCM.at<double>(1,1);
-  Pose[6] = DCM.at<double>(1,2);
-  Pose[8] = DCM.at<double>(2,0);
-  Pose[9] = DCM.at<double>(2,1);
-  Pose[10] = DCM.at<double>(2,2);
-  Pose[3]=x[3];
-  Pose[7]=x[4];
-  Pose[11]=x[5];
-  Pose[15]=1;
+  Matx33d R;
+  eulerToDCM(euler, R);
+  rtToPose(R, t, Pose);
 }
 
 /* Fast way to look up the duplicates
@@ -301,10 +251,10 @@ int ICP::registerModelToScene(const Mat& srcPC, const Mat& dstPC, double& residu
 
   Mat srcTemp = srcPC.clone();
   Mat dstTemp = dstPC.clone();
-  double meanSrc[3], meanDst[3];
+  Vec3d meanSrc, meanDst;
   computeMeanCols(srcTemp, meanSrc);
   computeMeanCols(dstTemp, meanDst);
-  double meanAvg[3]={0.5*(meanSrc[0]+meanDst[0]), 0.5*(meanSrc[1]+meanDst[1]), 0.5*(meanSrc[2]+meanDst[2])};
+  Vec3d meanAvg = 0.5 * (meanSrc + meanDst);
   subtractColumns(srcTemp, meanAvg);
   subtractColumns(dstTemp, meanAvg);
 
@@ -320,7 +270,7 @@ int ICP::registerModelToScene(const Mat& srcPC, const Mat& dstPC, double& residu
   Mat dstPC0 = dstTemp;
 
   // initialize pose
-  matrixIdentity(4, pose.val);
+  pose = Matx44d::eye();
 
   Mat M = Mat::eye(4,4,CV_64F);
 
@@ -338,7 +288,7 @@ int ICP::registerModelToScene(const Mat& srcPC, const Mat& dstPC, double& residu
     const int MaxIterationsPyr = cvRound((double)m_maxIterations/(level+1));
 
     // Obtain the sampled point clouds for this level: Also rotates the normals
-    Mat srcPCT = transformPCPose(srcPC0, pose.val);
+    Mat srcPCT = transformPCPose(srcPC0, pose);
 
     const int sampleStep = cvRound((double)n/(double)numSamples);
 
@@ -372,8 +322,7 @@ int ICP::registerModelToScene(const Mat& srcPC, const Mat& dstPC, double& residu
     int* newI = new int[numElSrc];
     int* newJ = new int[numElSrc];
 
-    double PoseX[16]={0};
-    matrixIdentity(4, PoseX);
+    Matx44d PoseX = Matx44d::eye();
 
     while ( (!(fval_perc<(1+TolP) && fval_perc>(1-TolP))) && i<MaxIterationsPyr)
     {
@@ -470,10 +419,11 @@ int ICP::registerModelToScene(const Mat& srcPC, const Mat& dstPC, double& residu
           }
         }
 
-        Mat X;
-        minimizePointToPlaneMetric(Src_Match, Dst_Match, X);
-
-        getTransformMat(X, PoseX);
+        Vec3d rpy, t;
+        minimizePointToPlaneMetric(Src_Match, Dst_Match, rpy, t);
+        if (cvIsNaN(cv::trace(rpy)) || cvIsNaN(cv::norm(t)))
+          break;
+        getTransformMat(rpy, t, PoseX);
         Src_Moved = transformPCPose(srcPCT, PoseX);
 
         double fval = cv::norm(Src_Match, Dst_Match)/(double)(Src_Moved.rows);
@@ -494,13 +444,7 @@ int ICP::registerModelToScene(const Mat& srcPC, const Mat& dstPC, double& residu
 
     }
 
-    double TempPose[16];
-    matrixProduct44(PoseX, pose.val, TempPose);
-
-    // no need to copy the last 4 rows
-    for (int c=0; c<12; c++)
-      pose.val[c] = TempPose[c];
-
+    pose = PoseX * pose;
     residual = tempResidual;
 
     delete[] newI;
@@ -514,18 +458,11 @@ int ICP::registerModelToScene(const Mat& srcPC, const Mat& dstPC, double& residu
     destroyFlann(flann);
   }
 
-  // Pose(1:3, 4) = Pose(1:3, 4)./scale;
-  pose.val[3] = pose.val[3]/scale + meanAvg[0];
-  pose.val[7] = pose.val[7]/scale + meanAvg[1];
-  pose.val[11] = pose.val[11]/scale + meanAvg[2];
-
-  // In MATLAB this would be : Pose(1:3, 4) = Pose(1:3, 4)./scale + meanAvg' - Pose(1:3, 1:3)*meanAvg';
-  double Rpose[9], Cpose[3];
-  poseToR(pose.val, Rpose);
-  matrixProduct331(Rpose, meanAvg, Cpose);
-  pose.val[3] -= Cpose[0];
-  pose.val[7] -= Cpose[1];
-  pose.val[11] -= Cpose[2];
+  Matx33d Rpose;
+  Vec3d Cpose;
+  poseToRT(pose, Rpose, Cpose);
+  Cpose = Cpose / scale + meanAvg - Rpose * meanAvg;
+  rtToPose(Rpose, Cpose, pose);
 
   residual = tempResidual;
 
@@ -543,7 +480,7 @@ int ICP::registerModelToScene(const Mat& srcPC, const Mat& dstPC, std::vector<Po
     Matx44d poseICP = Matx44d::eye();
     Mat srcTemp = transformPCPose(srcPC, poses[i]->pose);
     registerModelToScene(srcTemp, dstPC, poses[i]->residual, poseICP);
-    poses[i]->appendPose(poseICP.val);
+    poses[i]->appendPose(poseICP);
   }
   return 0;
 }

modules/surface_matching/src/pose_3d.cpp

@@ -45,29 +45,15 @@ namespace cv
 namespace ppf_match_3d
 {
 
-void Pose3D::updatePose(double NewPose[16])
+void Pose3D::updatePose(Matx44d& NewPose)
 {
-  double R[9];
+  Matx33d R;
 
-  for (int i=0; i<16; i++)
-    pose[i]=NewPose[i];
-
-  R[0] = pose[0];
-  R[1] = pose[1];
-  R[2] = pose[2];
-  R[3] = pose[4];
-  R[4] = pose[5];
-  R[5] = pose[6];
-  R[6] = pose[8];
-  R[7] = pose[9];
-  R[8] = pose[10];
-
-  t[0]=pose[3];
-  t[1]=pose[7];
-  t[2]=pose[11];
+  pose = NewPose;
+  poseToRT(pose, R, t);
 
   // compute the angle
-  const double trace = R[0] + R[4] + R[8];
+  const double trace = cv::trace(R);
 
   if (fabs(trace - 3) <= EPS)
   {
@@ -87,27 +73,12 @@ void Pose3D::updatePose(double NewPose[16])
   dcmToQuat(R, q);
 }
 
-void Pose3D::updatePose(double NewR[9], double NewT[3])
+void Pose3D::updatePose(Matx33d& NewR, Vec3d& NewT)
 {
-  pose[0]=NewR[0];
-  pose[1]=NewR[1];
-  pose[2]=NewR[2];
-  pose[3]=NewT[0];
-  pose[4]=NewR[3];
-  pose[5]=NewR[4];
-  pose[6]=NewR[5];
-  pose[7]=NewT[1];
-  pose[8]=NewR[6];
-  pose[9]=NewR[7];
-  pose[10]=NewR[8];
-  pose[11]=NewT[2];
-  pose[12]=0;
-  pose[13]=0;
-  pose[14]=0;
-  pose[15]=1;
+  rtToPose(NewR, NewT, pose);
 
   // compute the angle
-  const double trace = NewR[0] + NewR[4] + NewR[8];
+  const double trace = cv::trace(NewR);
 
   if (fabs(trace - 3) <= EPS)
   {
@@ -127,35 +98,17 @@ void Pose3D::updatePose(double NewR[9], double NewT[3])
   dcmToQuat(NewR, q);
 }
 
-void Pose3D::updatePoseQuat(double Q[4], double NewT[3])
+void Pose3D::updatePoseQuat(Vec4d& Q, Vec3d& NewT)
 {
-  double NewR[9];
+  Matx33d NewR;
 
   quatToDCM(Q, NewR);
-  q[0]=Q[0];
-  q[1]=Q[1];
-  q[2]=Q[2];
-  q[3]=Q[3];
-
-  pose[0]=NewR[0];
-  pose[1]=NewR[1];
-  pose[2]=NewR[2];
-  pose[3]=NewT[0];
-  pose[4]=NewR[3];
-  pose[5]=NewR[4];
-  pose[6]=NewR[5];
-  pose[7]=NewT[1];
-  pose[8]=NewR[6];
-  pose[9]=NewR[7];
-  pose[10]=NewR[8];
-  pose[11]=NewT[2];
-  pose[12]=0;
-  pose[13]=0;
-  pose[14]=0;
-  pose[15]=1;
+  q = Q;
+
+  rtToPose(NewR, NewT, pose);
 
   // compute the angle
-  const double trace = NewR[0] + NewR[4] + NewR[8];
+  const double trace = cv::trace(NewR);
 
   if (fabs(trace - 3) <= EPS)
   {
@@ -175,28 +128,15 @@ void Pose3D::updatePoseQuat(double Q[4], double NewT[3])
 }
 
 
-void Pose3D::appendPose(double IncrementalPose[16])
+void Pose3D::appendPose(Matx44d& IncrementalPose)
 {
-  double R[9], PoseFull[16]={0};
+  Matx33d R;
+  Matx44d PoseFull = IncrementalPose * this->pose;
 
-  matrixProduct44(IncrementalPose, this->pose, PoseFull);
-
-  R[0] = PoseFull[0];
-  R[1] = PoseFull[1];
-  R[2] = PoseFull[2];
-  R[3] = PoseFull[4];
-  R[4] = PoseFull[5];
-  R[5] = PoseFull[6];
-  R[6] = PoseFull[8];
-  R[7] = PoseFull[9];
-  R[8] = PoseFull[10];
-
-  t[0]=PoseFull[3];
-  t[1]=PoseFull[7];
-  t[2]=PoseFull[11];
+  poseToRT(PoseFull, R, t);
 
   // compute the angle
-  const double trace = R[0] + R[4] + R[8];
+  const double trace = cv::trace(R);
 
   if (fabs(trace - 3) <= EPS)
   {
@@ -215,42 +155,25 @@ void Pose3D::appendPose(double IncrementalPose[16])
   // compute the quaternion
   dcmToQuat(R, q);
 
-  for (int i=0; i<16; i++)
-    pose[i]=PoseFull[i];
+  pose = PoseFull;
 }
 
 Pose3DPtr Pose3D::clone()
 {
   Ptr<Pose3D> new_pose(new Pose3D(alpha, modelIndex, numVotes));
-  for (int i=0; i<16; i++)
-    new_pose->pose[i]= this->pose[i];
-
-  new_pose->q[0]=q[0];
-  new_pose->q[1]=q[1];
-  new_pose->q[2]=q[2];
-  new_pose->q[3]=q[3];
 
-  new_pose->t[0]=t[0];
-  new_pose->t[1]=t[1];
-  new_pose->t[2]=t[2];
-
-  new_pose->angle=angle;
+  new_pose->pose = this->pose;
+  new_pose->q = q;
+  new_pose->t = t;
+  new_pose->angle = angle;
 
   return new_pose;
 }
 
 void Pose3D::printPose()
 {
-  printf("\n-- Pose to Model Index %d: NumVotes = %d, Residual = %f\n", this->modelIndex, this->numVotes, this->residual);
-  for (int j=0; j<4; j++)
-  {
-    for (int k=0; k<4; k++)
-    {
-      printf("%f ", this->pose[j*4+k]);
-    }
-    printf("\n");
-  }
-  printf("\n");
+  printf("\n-- Pose to Model Index %d: NumVotes = %d, Residual = %f\n", (uint)this->modelIndex, (uint)this->numVotes, this->residual);
+  std::cout << this->pose << std::endl;
 }
 
 int Pose3D::writePose(FILE* f)
@@ -260,9 +183,9 @@ int Pose3D::writePose(FILE* f)
   fwrite(&angle, sizeof(double), 1, f);
   fwrite(&numVotes, sizeof(int), 1, f);
   fwrite(&modelIndex, sizeof(int), 1, f);
-  fwrite(pose, sizeof(double)*16, 1, f);
-  fwrite(t, sizeof(double)*3, 1, f);
-  fwrite(q, sizeof(double)*4, 1, f);
+  fwrite(pose.val, sizeof(double)*16, 1, f);
+  fwrite(t.val, sizeof(double)*3, 1, f);
+  fwrite(q.val, sizeof(double)*4, 1, f);
   fwrite(&residual, sizeof(double), 1, f);
   return 0;
 }
@@ -277,9 +200,9 @@ int Pose3D::readPose(FILE* f)
     status = fread(&angle, sizeof(double), 1, f);
     status = fread(&numVotes, sizeof(int), 1, f);
     status = fread(&modelIndex, sizeof(int), 1, f);
-    status = fread(pose, sizeof(double)*16, 1, f);
-    status = fread(t, sizeof(double)*3, 1, f);
-    status = fread(q, sizeof(double)*4, 1, f);
+    status = fread(pose.val, sizeof(double)*16, 1, f);
+    status = fread(t.val, sizeof(double)*3, 1, f);
+    status = fread(q.val, sizeof(double)*4, 1, f);
     status = fread(&residual, sizeof(double), 1, f);
     return 0;
   }

modules/surface_matching/src/ppf_helpers.cpp

@@ -50,10 +50,10 @@ typedef cv::flann::GenericIndex< Distance_32F > FlannIndex;
 
 void shuffle(int *array, size_t n);
 Mat genRandomMat(int rows, int cols, double mean, double stddev, int type);
-void getRandQuat(double q[4]);
-void getRandomRotation(double R[9]);
-void meanCovLocalPC(const float* pc, const int ws, const int point_count, double CovMat[3][3], double Mean[4]);
-void meanCovLocalPCInd(const float* pc, const int* Indices, const int ws, const int point_count, double CovMat[3][3], double Mean[4]);
+void getRandQuat(Vec4d& q);
+void getRandomRotation(Matx33d& R);
+void meanCovLocalPC(const Mat& pc, const int point_count, Matx33d& CovMat, Vec3d& Mean);
+void meanCovLocalPCInd(const Mat& pc, const int* Indices, const int point_count, Matx33d& CovMat, Vec3d& Mean);
 
 static std::vector<std::string> split(const std::string &text, char sep) {
   std::vector<std::string> tokens;
@@ -183,7 +183,7 @@ void writePLY(Mat PC, const char* FileName)
   {
     const float* point = PC.ptr<float>(pi);
 
-    outFile << point[0] << " "<<point[1]<<" "<<point[2];
+    outFile << point[0] << " " << point[1] << " " << point[2];
 
     if (vertNum==6)
     {
@@ -238,7 +238,7 @@ void writePLYVisibleNormals(Mat PC, const char* FileName)
     if (hasNormals)
     {
       outFile << " 127 127 127" << std::endl;
-      outFile << point[0]+point[3] << " " << point[1]+point[4] << " " << point[2]+point[5];
+      outFile << point[0] + point[3] << " " << point[1] + point[4] << " " << point[2] + point[5];
       outFile << " 255 0 0";
     }
 
@@ -306,7 +306,7 @@ void queryPCFlann(void* flannIndex, Mat& pc, Mat& indices, Mat& distances, const
 // uses a volume instead of an octree
 // TODO: Right now normals are required.
 // This is much faster than sample_pc_octree
-Mat samplePCByQuantization(Mat pc, float xrange[2], float yrange[2], float zrange[2], float sampleStep, int weightByCenter)
+Mat samplePCByQuantization(Mat pc, Vec2f& xrange, Vec2f& yrange, Vec2f& zrange, float sampleStep, int weightByCenter)
 {
   std::vector< std::vector<int> > map;
 
@@ -466,7 +466,7 @@ void shuffle(int *array, size_t n)
 }
 
 // compute the standard bounding box
-void computeBboxStd(Mat pc, float xRange[2], float yRange[2], float zRange[2])
+void computeBboxStd(Mat pc, Vec2f& xRange, Vec2f& yRange, Vec2f& zRange)
 {
   Mat pcPts = pc.colRange(0, 3);
   int num = pcPts.rows;
@@ -513,9 +513,9 @@ Mat normalizePCCoeff(Mat pc, float scale, float* Cx, float* Cy, float* Cz, float
   pc.col(1).copyTo(y);
   pc.col(2).copyTo(z);
 
-  float cx = (float) cv::mean(x).val[0];
-  float cy = (float) cv::mean(y).val[0];
-  float cz = (float) cv::mean(z).val[0];
+  float cx = (float) cv::mean(x)[0];
+  float cy = (float) cv::mean(y)[0];
+  float cz = (float) cv::mean(z)[0];
 
   cv::minMaxIdx(pc, &minVal, &maxVal);
 
@@ -559,11 +559,12 @@ Mat transPCCoeff(Mat pc, float scale, float Cx, float Cy, float Cz, float MinVal
   return pcn;
 }
 
-Mat transformPCPose(Mat pc, const double Pose[16])
+Mat transformPCPose(Mat pc, const Matx44d& Pose)
 {
   Mat pct = Mat(pc.rows, pc.cols, CV_32F);
 
-  double R[9], t[3];
+  Matx33d R;
+  Vec3d t;
   poseToRT(Pose, R, t);
 
 #if defined _OPENMP
@@ -572,37 +573,29 @@ Mat transformPCPose(Mat pc, const double Pose[16])
   for (int i=0; i<pc.rows; i++)
   {
     const float *pcData = pc.ptr<float>(i);
-    float *pcDataT = pct.ptr<float>(i);
-    const float *n1 = &pcData[3];
-    float *nT = &pcDataT[3];
-
-    double p[4] = {(double)pcData[0], (double)pcData[1], (double)pcData[2], 1};
-    double p2[4];
+    const Vec3f n1(&pcData[3]);
 
-    matrixProduct441(Pose, p, p2);
+    Vec4d p = Pose * Vec4d(pcData[0], pcData[1], pcData[2], 1);
+    Vec3d p2(p.val);
 
     // p2[3] should normally be 1
-    if (fabs(p2[3])>EPS)
+    if (fabs(p[3]) > EPS)
     {
-      pcDataT[0] = (float)(p2[0]/p2[3]);
-      pcDataT[1] = (float)(p2[1]/p2[3]);
-      pcDataT[2] = (float)(p2[2]/p2[3]);
+      Mat((1.0 / p[3]) * p2).reshape(1, 1).convertTo(pct.row(i).colRange(0, 3), CV_32F);
     }
 
     // If the point cloud has normals,
     // then rotate them as well
     if (pc.cols == 6)
     {
-      double n[3] = { (double)n1[0], (double)n1[1], (double)n1[2] }, n2[3];
+      Vec3d n(n1), n2;
 
-      matrixProduct331(R, n, n2);
-      double nNorm = sqrt(n2[0]*n2[0]+n2[1]*n2[1]+n2[2]*n2[2]);
+      n2 = R * n;
+      double nNorm = cv::norm(n2);
 
-      if (nNorm>EPS)
+      if (nNorm > EPS)
       {
-        nT[0]=(float)(n2[0]/nNorm);
-        nT[1]=(float)(n2[1]/nNorm);
-        nT[2]=(float)(n2[2]/nNorm);
+        Mat((1.0 / nNorm) * n2).reshape(1, 1).convertTo(pct.row(i).colRange(3, 6), CV_32F);
       }
     }
   }
@@ -621,32 +614,28 @@ Mat genRandomMat(int rows, int cols, double mean, double stddev, int type)
   return matr;
 }
 
-void getRandQuat(double q[4])
+void getRandQuat(Vec4d& q)
 {
   q[0] = (float)rand()/(float)(RAND_MAX);
   q[1] = (float)rand()/(float)(RAND_MAX);
   q[2] = (float)rand()/(float)(RAND_MAX);
   q[3] = (float)rand()/(float)(RAND_MAX);
 
-  double n = sqrt(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]);
-  q[0]/=n;
-  q[1]/=n;
-  q[2]/=n;
-  q[3]/=n;
-
+  q *= 1.0 / cv::norm(q);
   q[0]=fabs(q[0]);
 }
 
-void getRandomRotation(double R[9])
+void getRandomRotation(Matx33d& R)
 {
-  double q[4];
+  Vec4d q;
   getRandQuat(q);
   quatToDCM(q, R);
 }
 
-void getRandomPose(double Pose[16])
+void getRandomPose(Matx44d& Pose)
 {
-  double R[9], t[3];
+  Matx33d R;
+  Vec3d t;
 
   srand((unsigned int)time(0));
   getRandomRotation(R);
@@ -672,84 +661,37 @@ to improve accuracy and increase speed
 Also, view point flipping as in point cloud library is implemented
 */
 
-void meanCovLocalPC(const float* pc, const int ws, const int point_count, double CovMat[3][3], double Mean[4])
+void meanCovLocalPC(const Mat& pc, const int point_count, Matx33d& CovMat, Vec3d& Mean)
 {
-  int i;
-  double accu[16]={0};
-
-  // For each point in the cloud
-  for (i = 0; i < point_count; ++i)
-  {
-    const float* cloud = &pc[i*ws];
-    accu [0] += cloud[0] * cloud[0];
-    accu [1] += cloud[0] * cloud[1];
-    accu [2] += cloud[0] * cloud[2];
-    accu [3] += cloud[1] * cloud[1]; // 4
-    accu [4] += cloud[1] * cloud[2]; // 5
-    accu [5] += cloud[2] * cloud[2]; // 8
-    accu [6] += cloud[0];
-    accu [7] += cloud[1];
-    accu [8] += cloud[2];
-  }
-
-  for (i = 0; i < 9; ++i)
-    accu[i]/=(double)point_count;
-
-  Mean[0] = accu[6];
-  Mean[1] = accu[7];
-  Mean[2] = accu[8];
-  Mean[3] = 0;
-  CovMat[0][0] = accu [0] - accu [6] * accu [6];
-  CovMat[0][1] = accu [1] - accu [6] * accu [7];
-  CovMat[0][2] = accu [2] - accu [6] * accu [8];
-  CovMat[1][1] = accu [3] - accu [7] * accu [7];
-  CovMat[1][2] = accu [4] - accu [7] * accu [8];
-  CovMat[2][2] = accu [5] - accu [8] * accu [8];
-  CovMat[1][0] = CovMat[0][1];
-  CovMat[2][0] = CovMat[0][2];
-  CovMat[2][1] = CovMat[1][2];
-
+  cv::calcCovarMatrix(pc.rowRange(0, point_count), CovMat, Mean, cv::COVAR_NORMAL | cv::COVAR_ROWS);
+  CovMat *= 1.0 / (point_count - 1);
 }
 
-void meanCovLocalPCInd(const float* pc, const int* Indices, const int ws, const int point_count, double CovMat[3][3], double Mean[4])
+void meanCovLocalPCInd(const Mat& pc, const int* Indices, const int point_count, Matx33d& CovMat, Vec3d& Mean)
 {
-  int i;
-  double accu[16]={0};
+  int i, j, k;
 
+  CovMat = Matx33d::all(0);
+  Mean = Vec3d::all(0);
   for (i = 0; i < point_count; ++i)
   {
-    const float* cloud = &pc[ Indices[i] * ws ];
-    accu [0] += cloud[0] * cloud[0];
-    accu [1] += cloud[0] * cloud[1];
-    accu [2] += cloud[0] * cloud[2];
-    accu [3] += cloud[1] * cloud[1]; // 4
-    accu [4] += cloud[1] * cloud[2]; // 5
-    accu [5] += cloud[2] * cloud[2]; // 8
-    accu [6] += cloud[0];
-    accu [7] += cloud[1];
-    accu [8] += cloud[2];
-  }
-
-  for (i = 0; i < 9; ++i)
-    accu[i]/=(double)point_count;
-
-  Mean[0] = accu[6];
-  Mean[1] = accu[7];
-  Mean[2] = accu[8];
-  Mean[3] = 0;
-  CovMat[0][0] = accu [0] - accu [6] * accu [6];
-  CovMat[0][1] = accu [1] - accu [6] * accu [7];
-  CovMat[0][2] = accu [2] - accu [6] * accu [8];
-  CovMat[1][1] = accu [3] - accu [7] * accu [7];
-  CovMat[1][2] = accu [4] - accu [7] * accu [8];
-  CovMat[2][2] = accu [5] - accu [8] * accu [8];
-  CovMat[1][0] = CovMat[0][1];
-  CovMat[2][0] = CovMat[0][2];
-  CovMat[2][1] = CovMat[1][2];
+    const float* cloud = pc.ptr<float>(Indices[i]);
+    for (j = 0; j < 3; ++j)
+    {
+      for (k = 0; k < 3; ++k)
+        CovMat(j, k) += cloud[j] * cloud[k];
+      Mean[j] += cloud[j];
+    }
+  }
+  Mean *= 1.0 / point_count;
+  CovMat *= 1.0 / point_count;
 
+  for (j = 0; j < 3; ++j)
+    for (k = 0; k < 3; ++k)
+      CovMat(j, k) -= Mean[j] * Mean[k];
 }
 
-CV_EXPORTS int computeNormalsPC3d(const Mat& PC, Mat& PCNormals, const int NumNeighbors, const bool FlipViewpoint, const Vec3d& viewpoint)
+int computeNormalsPC3d(const Mat& PC, Mat& PCNormals, const int NumNeighbors, const bool FlipViewpoint, const Vec3f& viewpoint)
 {
   int i;
 
@@ -759,86 +701,45 @@ CV_EXPORTS int computeNormalsPC3d(const Mat& PC, Mat& PCNormals, const int NumNe
     CV_Error(cv::Error::BadImageSize, "PC should have 3 or 6 elements in its columns");
   }
 
-  int sizes[2] = {PC.rows, 3};
-  int sizesResult[2] = {PC.rows, NumNeighbors};
-  float* dataset = new float[PC.rows*3];
-  float* distances = new float[PC.rows*NumNeighbors];
-  int* indices = new int[PC.rows*NumNeighbors];
+  PCNormals.create(PC.rows, 6, CV_32F);
+  Mat PCInput = PCNormals.colRange(0, 3);
+  Mat Distances(PC.rows, NumNeighbors, CV_32F);
+  Mat Indices(PC.rows, NumNeighbors, CV_32S);
 
-  for (i=0; i<PC.rows; i++)
-  {
-    const float* src = PC.ptr<float>(i);
-    float* dst = (float*)(&dataset[i*3]);
-
-    dst[0] = src[0];
-    dst[1] = src[1];
-    dst[2] = src[2];
-  }
-
-  Mat PCInput(2, sizes, CV_32F, dataset, 0);
+  PC.rowRange(0, PC.rows).colRange(0, 3).copyTo(PCNormals.rowRange(0, PC.rows).colRange(0, 3));
 
   void* flannIndex = indexPCFlann(PCInput);
 
-  Mat Indices(2, sizesResult, CV_32S, indices, 0);
-  Mat Distances(2, sizesResult, CV_32F, distances, 0);
-
   queryPCFlann(flannIndex, PCInput, Indices, Distances, NumNeighbors);
   destroyFlann(flannIndex);
   flannIndex = 0;
 
-  PCNormals = Mat(PC.rows, 6, CV_32F);
-
+#if defined _OPENMP
+#pragma omp parallel for
+#endif
   for (i=0; i<PC.rows; i++)
   {
-    double C[3][3], mu[4];
-    const float* pci = &dataset[i*3];
-    float* pcr = PCNormals.ptr<float>(i);
-    double nr[3];
-
-    int* indLocal = &indices[i*NumNeighbors];
+    Matx33d C;
+    Vec3d mu;
+    const int* indLocal = Indices.ptr<int>(i);
 
     // compute covariance matrix
-    meanCovLocalPCInd(dataset, indLocal, 3, NumNeighbors, C, mu);
+    meanCovLocalPCInd(PCNormals, indLocal, NumNeighbors, C, mu);
 
     // eigenvectors of covariance matrix
-    Mat cov(3, 3, CV_64F), eigVect, eigVal;
-    double* covData = (double*)cov.data;
-    covData[0] = C[0][0];
-    covData[1] = C[0][1];
-    covData[2] = C[0][2];
-    covData[3] = C[1][0];
-    covData[4] = C[1][1];
-    covData[5] = C[1][2];
-    covData[6] = C[2][0];
-    covData[7] = C[2][1];
-    covData[8] = C[2][2];
-    eigen(cov, eigVal, eigVect);
-    Mat lowestEigVec;
-    //the eigenvector for the lowest eigenvalue is in the last row
-    eigVect.row(eigVect.rows - 1).copyTo(lowestEigVec);
-    double* eigData = (double*)lowestEigVec.data;
-    nr[0] = eigData[0];
-    nr[1] = eigData[1];
-    nr[2] = eigData[2];
-
-    pcr[0] = pci[0];
-    pcr[1] = pci[1];
-    pcr[2] = pci[2];
+    Mat eigVect, eigVal;
+    eigen(C, eigVal, eigVect);
+    eigVect.row(2).convertTo(PCNormals.row(i).colRange(3, 6), CV_32F);
 
     if (FlipViewpoint)
     {
-      flipNormalViewpoint(pci, viewpoint[0], viewpoint[1], viewpoint[2], &nr[0], &nr[1], &nr[2]);
+      Vec3f nr(PCNormals.ptr<float>(i) + 3);
+      Vec3f pci(PCNormals.ptr<float>(i));
+      flipNormalViewpoint(pci, viewpoint, nr);
+      Mat(nr).reshape(1, 1).copyTo(PCNormals.row(i).colRange(3, 6));
     }
-
-    pcr[3] = (float)nr[0];
-    pcr[4] = (float)nr[1];
-    pcr[5] = (float)nr[2];
   }
 
-  delete[] indices;
-  delete[] distances;
-  delete[] dataset;
-
   return 1;
 }
 

modules/surface_matching/src/ppf_match_3d.cpp

@@ -62,50 +62,47 @@ static int sortPoseClusters(const PoseCluster3DPtr& a, const PoseCluster3DPtr& b
 }
 
 // simple hashing
-/*static int hashPPFSimple(const double f[4], const double AngleStep, const double DistanceStep)
+/*static int hashPPFSimple(const Vec4d& f, const double AngleStep, const double DistanceStep)
 {
-  const unsigned char d1 = (unsigned char) (floor ((double)f[0] / (double)AngleStep));
-  const unsigned char d2 = (unsigned char) (floor ((double)f[1] / (double)AngleStep));
-  const unsigned char d3 = (unsigned char) (floor ((double)f[2] / (double)AngleStep));
-  const unsigned char d4 = (unsigned char) (floor ((double)f[3] / (double)DistanceStep));
+  Vec4i key(
+      (int)(f[0] / AngleStep),
+      (int)(f[1] / AngleStep),
+      (int)(f[2] / AngleStep),
+      (int)(f[3] / DistanceStep));
 
-  int hashKey = (d1 | (d2<<8) | (d3<<16) | (d4<<24));
+  int hashKey = d.val[0] | (d.val[1] << 8) | (d.val[2] << 16) | (d.val[3] << 24);
   return hashKey;
 }*/
 
 // quantize ppf and hash it for proper indexing
-static KeyType hashPPF(const double f[4], const double AngleStep, const double DistanceStep)
+static KeyType hashPPF(const Vec4d& f, const double AngleStep, const double DistanceStep)
 {
-  const int d1 = (int) (floor ((double)f[0] / (double)AngleStep));
-  const int d2 = (int) (floor ((double)f[1] / (double)AngleStep));
-  const int d3 = (int) (floor ((double)f[2] / (double)AngleStep));
-  const int d4 = (int) (floor ((double)f[3] / (double)DistanceStep));
-  int key[4]={d1,d2,d3,d4};
-  KeyType hashKey=0;
-
-  murmurHash(key, 4*sizeof(int), 42, &hashKey);
-
+  Vec4i key(
+      (int)(f[0] / AngleStep),
+      (int)(f[1] / AngleStep),
+      (int)(f[2] / AngleStep),
+      (int)(f[3] / DistanceStep));
+  KeyType hashKey = 0;
+
+  murmurHash(key.val, 4*sizeof(int), 42, &hashKey);
   return hashKey;
 }
 
-/*static size_t hashMurmur(unsigned int key)
+/*static size_t hashMurmur(uint key)
 {
   size_t hashKey=0;
   hashMurmurx86((void*)&key, 4, 42, &hashKey);
   return hashKey;
 }*/
 
-static double computeAlpha(const double p1[4], const double n1[4], const double p2[4])
+static double computeAlpha(const Vec3d& p1, const Vec3d& n1, const Vec3d& p2)
 {
-  double Tmg[3], mpt[3], row2[3], row3[3], alpha;
-
-  computeTransformRTyz(p1, n1, row2, row3, Tmg);
-
-  // checked row2, row3: They are correct
-
-  mpt[1] = Tmg[1] + row2[0] * p2[0] + row2[1] * p2[1] + row2[2] * p2[2];
-  mpt[2] = Tmg[2] + row3[0] * p2[0] + row3[1] * p2[1] + row3[2] * p2[2];
+  Vec3d Tmg, mpt;
+  Matx33d R;
+  double alpha;
 
+  computeTransformRT(p1, n1, R, Tmg);
+  mpt = Tmg + R * p2;
   alpha=atan2(-mpt[2], mpt[1]);
 
   if ( alpha != alpha)
@@ -167,35 +164,15 @@ void PPF3DDetector::setSearchParams(const double positionThreshold, const double
 }
 
 // compute per point PPF as in paper
-void PPF3DDetector::computePPFFeatures(const double p1[4], const double n1[4],
-                                       const double p2[4], const double n2[4],
-                                       double f[4])
+void PPF3DDetector::computePPFFeatures(const Vec3d& p1, const Vec3d& n1,
+                                       const Vec3d& p2, const Vec3d& n2,
+                                       Vec4d& f)
 {
-  /*
-  Vectors will be defined as of length 4 instead of 3, because of:
-  - Further SIMD vectorization
-  - Cache alignment
-  */
-
-  double d[4] = {p2[0] - p1[0], p2[1] - p1[1], p2[2] - p1[2], 0};
-
-  double norm = TNorm3(d);
-  f[3] = norm;
-
-  if (norm)
-  {
-    d[0] /= f[3];
-    d[1] /= f[3];
-    d[2] /= f[3];
-  }
-  else
-  {
-    // TODO: Handle this
-    f[0] = 0;
-    f[1] = 0;
-    f[2] = 0;
-    return ;
-  }
+  Vec3d d(p2 - p1);
+  f[3] = cv::norm(d);
+  if (f[3] <= EPS)
+    return;
+  d *= 1.0 / f[3];
 
   f[0] = TAngle3Normalized(n1, d);
   f[1] = TAngle3Normalized(n2, d);
@@ -228,7 +205,7 @@ void PPF3DDetector::trainModel(const Mat &PC)
   CV_Assert(PC.type() == CV_32F || PC.type() == CV_32FC1);
 
   // compute bbox
-  float xRange[2], yRange[2], zRange[2];
+  Vec2f xRange, yRange, zRange;
   computeBboxStd(PC, xRange, yRange, zRange);
 
   // compute sampling step from diameter of bbox
@@ -261,9 +238,8 @@ void PPF3DDetector::trainModel(const Mat &PC)
   // since this is just a training part.
   for (int i=0; i<numRefPoints; i++)
   {
-    float* f1 = sampled.ptr<float>(i);
-    const double p1[4] = {f1[0], f1[1], f1[2], 0};
-    const double n1[4] = {f1[3], f1[4], f1[5], 0};
+    const Vec3f p1(sampled.ptr<float>(i));
+    const Vec3f n1(sampled.ptr<float>(i) + 3);
 
     //printf("///////////////////// NEW REFERENCE ////////////////////////\n");
     for (int j=0; j<numRefPoints; j++)
@@ -271,15 +247,14 @@ void PPF3DDetector::trainModel(const Mat &PC)
       // cannnot compute the ppf with myself
       if (i!=j)
       {
-        float* f2 = sampled.ptr<float>(j);
-        const double p2[4] = {f2[0], f2[1], f2[2], 0};
-        const double n2[4] = {f2[3], f2[4], f2[5], 0};
+        const Vec3f p2(sampled.ptr<float>(j));
+        const Vec3f n2(sampled.ptr<float>(j) + 3);
 
-        double f[4]={0};
+        Vec4d f = Vec4d::all(0);
         computePPFFeatures(p1, n1, p2, n2, f);
         KeyType hashValue = hashPPF(f, angle_step_radians, distanceStep);
         double alpha = computeAlpha(p1, n1, p2);
-        unsigned int ppfInd = i*numRefPoints+j;
+        uint ppfInd = i*numRefPoints+j;
 
         THash* hashNode = &hash_nodes[i*numRefPoints+j];
         hashNode->id = hashValue;
@@ -288,12 +263,8 @@ void PPF3DDetector::trainModel(const Mat &PC)
 
         hashtableInsertHashed(hashTable, hashValue, (void*)hashNode);
 
-        float* ppfRow = ppf.ptr<float>(ppfInd);
-        ppfRow[0] = (float)f[0];
-        ppfRow[1] = (float)f[1];
-        ppfRow[2] = (float)f[2];
-        ppfRow[3] = (float)f[3];
-        ppfRow[4] = (float)alpha;
+        Mat(f).reshape(1, 1).convertTo(ppf.row(ppfInd).colRange(0, 4), CV_32F);
+        ppf.ptr<float>(ppfInd)[4] = (float)alpha;
       }
     }
   }
@@ -314,8 +285,8 @@ void PPF3DDetector::trainModel(const Mat &PC)
 bool PPF3DDetector::matchPose(const Pose3D& sourcePose, const Pose3D& targetPose)
 {
   // translational difference
-  double dv[3] = {targetPose.t[0]-sourcePose.t[0], targetPose.t[1]-sourcePose.t[1], targetPose.t[2]-sourcePose.t[2]};
-  double dNorm = sqrt(dv[0]*dv[0]+dv[1]*dv[1]+dv[2]*dv[2]);
+  Vec3d dv = targetPose.t - sourcePose.t;
+  double dNorm = cv::norm(dv);
 
   const double phi = fabs ( sourcePose.angle - targetPose.angle );
 
@@ -369,13 +340,14 @@ void PPF3DDetector::clusterPoses(std::vector<Pose3DPtr>& poseList, int numPoses,
     for (int i=0; i<static_cast<int>(poseClusters.size()); i++)
     {
       // We could only average the quaternions. So I will make use of them here
-      double qAvg[4]={0}, tAvg[3]={0};
+      Vec4d qAvg = Vec4d::all(0);
+      Vec3d tAvg = Vec3d::all(0);
 
       // Perform the final averaging
       PoseCluster3DPtr curCluster = poseClusters[i];
       std::vector<Pose3DPtr> curPoses = curCluster->poseList;
       int curSize = (int)curPoses.size();
-      int numTotalVotes = 0;
+      size_t numTotalVotes = 0;
 
       for (int j=0; j<curSize; j++)
         numTotalVotes += curPoses[j]->numVotes;
@@ -386,25 +358,13 @@ void PPF3DDetector::clusterPoses(std::vector<Pose3DPtr>& poseList, int numPoses,
       {
         const double w = (double)curPoses[j]->numVotes / (double)numTotalVotes;
 
-        qAvg[0]+= w*curPoses[j]->q[0];
-        qAvg[1]+= w*curPoses[j]->q[1];
-        qAvg[2]+= w*curPoses[j]->q[2];
-        qAvg[3]+= w*curPoses[j]->q[3];
-
-        tAvg[0]+= w*curPoses[j]->t[0];
-        tAvg[1]+= w*curPoses[j]->t[1];
-        tAvg[2]+= w*curPoses[j]->t[2];
-        wSum+=w;
+        qAvg += w * curPoses[j]->q;
+        tAvg += w * curPoses[j]->t;
+        wSum += w;
       }
 
-      tAvg[0]/=wSum;
-      tAvg[1]/=wSum;
-      tAvg[2]/=wSum;
-
-      qAvg[0]/=wSum;
-      qAvg[1]/=wSum;
-      qAvg[2]/=wSum;
-      qAvg[3]/=wSum;
+      tAvg *= 1.0 / wSum;
+      qAvg *= 1.0 / wSum;
 
       curPoses[0]->updatePoseQuat(qAvg, tAvg);
       curPoses[0]->numVotes=curCluster->numVotes;
@@ -420,7 +380,8 @@ void PPF3DDetector::clusterPoses(std::vector<Pose3DPtr>& poseList, int numPoses,
     for (int i=0; i<static_cast<int>(poseClusters.size()); i++)
     {
       // We could only average the quaternions. So I will make use of them here
-      double qAvg[4]={0}, tAvg[3]={0};
+      Vec4d qAvg = Vec4d::all(0);
+      Vec3d tAvg = Vec3d::all(0);
 
       // Perform the final averaging
       PoseCluster3DPtr curCluster = poseClusters[i];
@@ -429,24 +390,12 @@ void PPF3DDetector::clusterPoses(std::vector<Pose3DPtr>& poseList, int numPoses,
 
       for (int j=0; j<curSize; j++)
       {
-        qAvg[0]+= curPoses[j]->q[0];
-        qAvg[1]+= curPoses[j]->q[1];
-        qAvg[2]+= curPoses[j]->q[2];
-        qAvg[3]+= curPoses[j]->q[3];
-
-        tAvg[0]+= curPoses[j]->t[0];
-        tAvg[1]+= curPoses[j]->t[1];
-        tAvg[2]+= curPoses[j]->t[2];
+        qAvg += curPoses[j]->q;
+        tAvg += curPoses[j]->t;
       }
 
-      tAvg[0]/=(double)curSize;
-      tAvg[1]/=(double)curSize;
-      tAvg[2]/=(double)curSize;
-
-      qAvg[0]/=(double)curSize;
-      qAvg[1]/=(double)curSize;
-      qAvg[2]/=(double)curSize;
-      qAvg[3]/=(double)curSize;
+      tAvg *= 1.0 / curSize;
+      qAvg *= 1.0 / curSize;
 
       curPoses[0]->updatePoseQuat(qAvg, tAvg);
       curPoses[0]->numVotes=curCluster->numVotes;
@@ -473,12 +422,12 @@ void PPF3DDetector::match(const Mat& pc, std::vector<Pose3DPtr>& results, const
   //int numNeighbors = 10;
   int numAngles = (int) (floor (2 * M_PI / angle_step));
   float distanceStep = (float)distance_step;
-  unsigned int n = num_ref_points;
+  uint n = num_ref_points;
   std::vector<Pose3DPtr> poseList;
   int sceneSamplingStep = scene_sample_step;
 
   // compute bbox
-  float xRange[2], yRange[2], zRange[2];
+  Vec2f xRange, yRange, zRange;
   computeBboxStd(pc, xRange, yRange, zRange);
 
   // sample the point cloud
@@ -491,7 +440,7 @@ void PPF3DDetector::match(const Mat& pc, std::vector<Pose3DPtr>& results, const
 
   // allocate the accumulator : Moved this to the inside of the loop
   /*#if !defined (_OPENMP)
-     unsigned int* accumulator = (unsigned int*)calloc(numAngles*n, sizeof(unsigned int));
+     uint* accumulator = (uint*)calloc(numAngles*n, sizeof(uint));
   #endif*/
 
   poseList.reserve((sampled.rows/sceneSamplingStep)+4);
@@ -501,18 +450,16 @@ void PPF3DDetector::match(const Mat& pc, std::vector<Pose3DPtr>& results, const
 #endif
   for (int i = 0; i < sampled.rows; i += sceneSamplingStep)
   {
-    unsigned int refIndMax = 0, alphaIndMax = 0;
-    unsigned int maxVotes = 0;
+    uint refIndMax = 0, alphaIndMax = 0;
+    uint maxVotes = 0;
 
-    float* f1 = sampled.ptr<float>(i);
-    const double p1[4] = {f1[0], f1[1], f1[2], 0};
-    const double n1[4] = {f1[3], f1[4], f1[5], 0};
-    double *row2, *row3, tsg[3]={0}, Rsg[9]={0}, RInv[9]={0};
+    const Vec3f p1(sampled.ptr<float>(i));
+    const Vec3f n1(sampled.ptr<float>(i) + 3);
+    Vec3d tsg = Vec3d::all(0);
+    Matx33d Rsg = Matx33d::all(0), RInv = Matx33d::all(0);
 
-    unsigned int* accumulator = (unsigned int*)calloc(numAngles*n, sizeof(unsigned int));
+    uint* accumulator = (uint*)calloc(numAngles*n, sizeof(uint));
     computeTransformRT(p1, n1, Rsg, tsg);
-    row2=&Rsg[3];
-    row3=&Rsg[6];
 
     // Tolga Birdal's notice:
     // As a later update, we might want to look into a local neighborhood only
@@ -523,19 +470,16 @@ void PPF3DDetector::match(const Mat& pc, std::vector<Pose3DPtr>& results, const
     {
       if (i!=j)
       {
-        float* f2 = sampled.ptr<float>(j);
-        const double p2[4] = {f2[0], f2[1], f2[2], 0};
-        const double n2[4] = {f2[3], f2[4], f2[5], 0};
-        double p2t[4], alpha_scene;
+        const Vec3f p2(sampled.ptr<float>(j));
+        const Vec3f n2(sampled.ptr<float>(j) + 3);
+        Vec3d p2t;
+        double alpha_scene;
 
-        double f[4]={0};
+        Vec4d f = Vec4d::all(0);
         computePPFFeatures(p1, n1, p2, n2, f);
         KeyType hashValue = hashPPF(f, angle_step, distanceStep);
 
-        // we don't need to call this here, as we already estimate the tsg from scene reference point
-        // double alpha = computeAlpha(p1, n1, p2);
-        p2t[1] = tsg[1] + row2[0] * p2[0] + row2[1] * p2[1] + row2[2] * p2[2];
-        p2t[2] = tsg[2] + row3[0] * p2[0] + row3[1] * p2[1] + row3[2] * p2[2];
+        p2t = tsg + Rsg * Vec3d(p2);
 
         alpha_scene=atan2(-p2t[2], p2t[1]);
 
@@ -570,7 +514,7 @@ void PPF3DDetector::match(const Mat& pc, std::vector<Pose3DPtr>& results, const
           //printf("%f\n", alpha);
           int alpha_index = (int)(numAngles*(alpha + 2*M_PI) / (4*M_PI));
 
-          unsigned int accIndex = corrI * numAngles + alpha_index;
+          uint accIndex = corrI * numAngles + alpha_index;
 
           accumulator[accIndex]++;
           node = node->next;
@@ -579,12 +523,12 @@ void PPF3DDetector::match(const Mat& pc, std::vector<Pose3DPtr>& results, const
     }
 
     // Maximize the accumulator
-    for (unsigned int k = 0; k < n; k++)
+    for (uint k = 0; k < n; k++)
     {
       for (int j = 0; j < numAngles; j++)
       {
-        const unsigned int accInd = k*numAngles + j;
-        const unsigned int accVal = accumulator[ accInd ];
+        const uint accInd = k*numAngles + j;
+        const uint accVal = accumulator[ accInd ];
         if (accVal > maxVotes)
         {
           maxVotes = accVal;
@@ -600,43 +544,35 @@ void PPF3DDetector::match(const Mat& pc, std::vector<Pose3DPtr>& results, const
 
     // invert Tsg : Luckily rotation is orthogonal: Inverse = Transpose.
     // We are not required to invert.
-    double tInv[3], tmg[3], Rmg[9];
-    matrixTranspose33(Rsg, RInv);
-    matrixProduct331(RInv, tsg, tInv);
+    Vec3d tInv, tmg;
+    Matx33d Rmg;
+    RInv = Rsg.t();
+    tInv = -RInv * tsg;
 
-    double TsgInv[16] = { RInv[0], RInv[1], RInv[2], -tInv[0],
-                          RInv[3], RInv[4], RInv[5], -tInv[1],
-                          RInv[6], RInv[7], RInv[8], -tInv[2],
-                          0, 0, 0, 1
-                        };
+    Matx44d TsgInv;
+    rtToPose(RInv, tInv, TsgInv);
 
     // TODO : Compute pose
-    const float* fMax = sampled_pc.ptr<float>(refIndMax);
-    const double pMax[4] = {fMax[0], fMax[1], fMax[2], 1};
-    const double nMax[4] = {fMax[3], fMax[4], fMax[5], 1};
+    const Vec3f pMax(sampled_pc.ptr<float>(refIndMax));
+    const Vec3f nMax(sampled_pc.ptr<float>(refIndMax) + 3);
 
     computeTransformRT(pMax, nMax, Rmg, tmg);
-    row2=&Rsg[3];
-    row3=&Rsg[6];
 
-    double Tmg[16] = { Rmg[0], Rmg[1], Rmg[2], tmg[0],
-                       Rmg[3], Rmg[4], Rmg[5], tmg[1],
-                       Rmg[6], Rmg[7], Rmg[8], tmg[2],
-                       0, 0, 0, 1
-                     };
+    Matx44d Tmg;
+    rtToPose(Rmg, tmg, Tmg);
 
     // convert alpha_index to alpha
     int alpha_index = alphaIndMax;
     double alpha = (alpha_index*(4*M_PI))/numAngles-2*M_PI;
 
     // Equation 2:
-    double Talpha[16]={0};
-    getUnitXRotation_44(alpha, Talpha);
+    Matx44d Talpha;
+    Matx33d R;
+    Vec3d t = Vec3d::all(0);
+    getUnitXRotation(alpha, R);
+    rtToPose(R, t, Talpha);
 
-    double Temp[16]={0};
-    double rawPose[16]={0};
-    matrixProduct44(Talpha, Tmg, Temp);
-    matrixProduct44(TsgInv, Temp, rawPose);
+    Matx44d rawPose = TsgInv * (Talpha * Tmg);
 
     Pose3DPtr pose(new Pose3D(alpha, refIndMax, maxVotes));
     pose->updatePose(rawPose);

modules/surface_matching/src/t_hash_int.cpp

@@ -47,10 +47,10 @@ namespace ppf_match_3d
 // This magic value is just
 #define T_HASH_MAGIC 427462442
 
-size_t hash( unsigned int a);
+size_t hash( uint a);
 
 // default hash function
-size_t hash( unsigned int a)
+size_t hash( uint a)
 {
     a = (a+0x7ed55d16) + (a<<12);
     a = (a^0xc761c23c) ^ (a>>19);
@@ -61,7 +61,7 @@ size_t hash( unsigned int a)
     return a;
 }
 
-hashtable_int *hashtableCreate(size_t size, size_t (*hashfunc)(unsigned int))
+hashtable_int *hashtableCreate(size_t size, size_t (*hashfunc)(uint))
 {
     hashtable_int *hashtbl;
 
@@ -71,7 +71,7 @@ hashtable_int *hashtableCreate(size_t size, size_t (*hashfunc)(unsigned int))
     }
     else
     {
-        size = (size_t)next_power_of_two((unsigned int)size);
+        size = (size_t)next_power_of_two((uint)size);
     }
 
     hashtbl=(hashtable_int*)malloc(sizeof(hashtable_int));