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,17 +123,19 @@ 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;
 };
 
 /**
-* @brief When multiple poses (see Pose3D) are grouped together (contribute to the same transformation) 
+* @brief When multiple poses (see Pose3D) are grouped together (contribute to the same transformation)
 * 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/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
 */
@@ -169,23 +173,23 @@ void PMurHash32_Process(uint32_t *ph1, uint32_t *pcarry, const void *key, int le
 {
     uint32_t h1 = *ph1;
     uint32_t c = *pcarry;
-    
+
     const uint8_t *ptr = (uint8_t*)                         key;
     const uint8_t *end;
-    
+
     /* Extract carry count from low 2 bits of c value */
     int n = c & 3;
-    
+
 #if defined(UNALIGNED_SAFE)
     /* This CPU handles unaligned word access */
-    
+
     /* Consume any carry bytes */
     int i = (4-n) & 3;
     if (i && i <= len)
     {
         DOBYTES(i, h1, c, n, ptr, len);
     }
-    
+
     /* Process 32-bit chunks */
     end = ptr + len/4*4;
     for ( ; ptr < end ; ptr+=4)
@@ -193,17 +197,17 @@ void PMurHash32_Process(uint32_t *ph1, uint32_t *pcarry, const void *key, int le
         uint32_t k1 = READ_UINT32(ptr);
         DOBLOCK(h1, k1);
     }
-    
+
 #else /*UNALIGNED_SAFE*/
     /* This CPU does not handle unaligned word access */
-    
+
     /* Consume enough so that the next data byte is word aligned */
     int i = -(long)ptr & 3;
     if (i && i <= len)
     {
         DOBYTES(i, h1, c, n, ptr, len);
     }
-    
+
     /* We're now aligned. Process in aligned blocks. Specialise for each possible carry count */
     end = ptr + len/4*4;
     switch (n)
@@ -244,13 +248,13 @@ void PMurHash32_Process(uint32_t *ph1, uint32_t *pcarry, const void *key, int le
             }
     }
 #endif /*UNALIGNED_SAFE*/
-    
+
     /* Advance over whole 32-bit chunks, possibly leaving 1..3 bytes */
     len -= len/4*4;
-    
+
     /* Append any remaining bytes into carry */
     DOBYTES(len, h1, c, n, ptr, len);
-    
+
     /* Copy out new running hash and carry */
     *ph1 = h1;
     *pcarry = (c & ~0xff) | n;
@@ -272,14 +276,14 @@ uint32_t PMurHash32_Result(uint32_t h, uint32_t carry, uint32_t total_length)
         h ^= k1;
     }
     h ^= total_length;
-    
+
     /* fmix */
     h ^= h >> 16;
     h *= 0x85ebca6b;
     h ^= h >> 13;
     h *= 0xc2b2ae35;
     h ^= h >> 16;
-    
+
     return h;
 }
 
@@ -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/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,37 +61,37 @@ 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;
-    
+
     if (size < 16)
     {
         size = 16;
     }
     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));
+
+    hashtbl=(hashtable_int*)malloc(sizeof(hashtable_int));
     if (!hashtbl)
         return NULL;
-        
-	hashtbl->nodes=(hashnode_i**)calloc(size, sizeof(struct hashnode_i*));
+
+    hashtbl->nodes=(hashnode_i**)calloc(size, sizeof(struct hashnode_i*));
     if (!hashtbl->nodes)
     {
         free(hashtbl);
         return NULL;
     }
-    
+
     hashtbl->size=size;
-    
+
     if (hashfunc)
         hashtbl->hashfunc=hashfunc;
     else
         hashtbl->hashfunc=hash;
-        
+
     return hashtbl;
 }
 
@@ -100,7 +100,7 @@ void hashtableDestroy(hashtable_int *hashtbl)
 {
     size_t n;
     struct hashnode_i *node, *oldnode;
-    
+
     for (n=0; n<hashtbl->size; ++n)
     {
         node=hashtbl->nodes[n];
@@ -120,10 +120,10 @@ int hashtableInsert(hashtable_int *hashtbl, KeyType key, void *data)
 {
     struct hashnode_i *node;
     size_t hash=hashtbl->hashfunc(key)%hashtbl->size;
-    
-    
+
+
     /* fpruintf(stderr, "hashtbl_insert() key=%s, hash=%d, data=%s\n", key, hash, (char*)data);*/
-    
+
     node=hashtbl->nodes[hash];
     while (node)
     {
@@ -134,18 +134,18 @@ int hashtableInsert(hashtable_int *hashtbl, KeyType key, void *data)
         }
         node=node->next;
     }
-    
-    
-	node=(hashnode_i*)malloc(sizeof(struct hashnode_i));
+
+
+    node=(hashnode_i*)malloc(sizeof(struct hashnode_i));
     if (!node)
         return -1;
     node->key=key;
-    
+
     node->data=data;
     node->next=hashtbl->nodes[hash];
     hashtbl->nodes[hash]=node;
-    
-    
+
+
     return 0;
 }
 
@@ -153,10 +153,10 @@ int hashtableInsertHashed(hashtable_int *hashtbl, KeyType key, void *data)
 {
     struct hashnode_i *node;
     size_t hash = key % hashtbl->size;
-    
-    
+
+
     /* fpruintf(stderr, "hashtbl_insert() key=%s, hash=%d, data=%s\n", key, hash, (char*)data);*/
-    
+
     node=hashtbl->nodes[hash];
     while (node)
     {
@@ -167,18 +167,18 @@ int hashtableInsertHashed(hashtable_int *hashtbl, KeyType key, void *data)
         }
         node=node->next;
     }
-    
-	node=(hashnode_i*)malloc(sizeof(struct hashnode_i));
+
+    node=(hashnode_i*)malloc(sizeof(struct hashnode_i));
     if (!node)
         return -1;
-		
+
     node->key=key;
-    
+
     node->data=data;
     node->next=hashtbl->nodes[hash];
     hashtbl->nodes[hash]=node;
-    
-    
+
+
     return 0;
 }
 
@@ -187,7 +187,7 @@ int hashtableRemove(hashtable_int *hashtbl, KeyType key)
 {
     struct hashnode_i *node, *prevnode=NULL;
     size_t hash=hashtbl->hashfunc(key)%hashtbl->size;
-    
+
     node=hashtbl->nodes[hash];
     while (node)
     {
@@ -203,7 +203,7 @@ int hashtableRemove(hashtable_int *hashtbl, KeyType key)
         prevnode=node;
         node=node->next;
     }
-    
+
     return -1;
 }
 
@@ -212,9 +212,9 @@ void *hashtableGet(hashtable_int *hashtbl, KeyType key)
 {
     struct hashnode_i *node;
     size_t hash=hashtbl->hashfunc(key)%hashtbl->size;
-    
+
     /* fprintf(stderr, "hashtbl_get() key=%s, hash=%d\n", key, hash);*/
-    
+
     node=hashtbl->nodes[hash];
     while (node)
     {
@@ -222,14 +222,14 @@ void *hashtableGet(hashtable_int *hashtbl, KeyType key)
             return node->data;
         node=node->next;
     }
-    
+
     return NULL;
 }
 
 hashnode_i* hashtableGetBucketHashed(hashtable_int *hashtbl, KeyType key)
 {
     size_t hash = key % hashtbl->size;
-    
+
     return hashtbl->nodes[hash];
 }
 
@@ -238,14 +238,14 @@ int hashtableResize(hashtable_int *hashtbl, size_t size)
     hashtable_int newtbl;
     size_t n;
     struct hashnode_i *node,*next;
-    
+
     newtbl.size=size;
     newtbl.hashfunc=hashtbl->hashfunc;
-    
-	newtbl.nodes=(hashnode_i**)calloc(size, sizeof(struct hashnode_i*));
+
+    newtbl.nodes=(hashnode_i**)calloc(size, sizeof(struct hashnode_i*));
     if (!newtbl.nodes)
         return -1;
-        
+
     for (n=0; n<hashtbl->size; ++n)
     {
         for (node=hashtbl->nodes[n]; node; node=next)
@@ -253,14 +253,14 @@ int hashtableResize(hashtable_int *hashtbl, size_t size)
             next = node->next;
             hashtableInsert(&newtbl, node->key, node->data);
             hashtableRemove(hashtbl, node->key);
-            
+
         }
     }
-    
+
     free(hashtbl->nodes);
     hashtbl->size=newtbl.size;
     hashtbl->nodes=newtbl.nodes;
-    
+
     return 0;
 }
 
@@ -270,24 +270,24 @@ int hashtableWrite(const hashtable_int * hashtbl, const size_t dataSize, FILE* f
     size_t hashMagic=T_HASH_MAGIC;
     size_t n=hashtbl->size;
     size_t i;
-    
+
     fwrite(&hashMagic, sizeof(size_t),1, f);
     fwrite(&n, sizeof(size_t),1, f);
     fwrite(&dataSize, sizeof(size_t),1, f);
-    
+
     for (i=0; i<hashtbl->size; i++)
     {
         struct hashnode_i* node=hashtbl->nodes[i];
         size_t noEl=0;
-        
+
         while (node)
         {
             noEl++;
             node=node->next;
         }
-        
+
         fwrite(&noEl, sizeof(size_t),1, f);
-        
+
         node=hashtbl->nodes[i];
         while (node)
         {
@@ -296,7 +296,7 @@ int hashtableWrite(const hashtable_int * hashtbl, const size_t dataSize, FILE* f
             node=node->next;
         }
     }
-    
+
     return 1;
 }
 
@@ -305,13 +305,13 @@ void hashtablePrint(hashtable_int *hashtbl)
 {
     size_t n;
     struct hashnode_i *node,*next;
-    
+
     for (n=0; n<hashtbl->size; ++n)
     {
         for (node=hashtbl->nodes[n]; node; node=next)
         {
             next = node->next;
-			std::cout<<"Key : "<<node->key<<", Data : "<<node->data<<std::endl;
+            std::cout<<"Key : "<<node->key<<", Data : "<<node->data<<std::endl;
         }
     }
 }
@@ -321,7 +321,7 @@ hashtable_int *hashtableRead(FILE* f)
     size_t hashMagic = 0;
     size_t n = 0, status;
     hashtable_int *hashtbl = 0;
-    
+
     status = fread(&hashMagic, sizeof(size_t),1, f);
     if (status && hashMagic==T_HASH_MAGIC)
     {
@@ -329,20 +329,20 @@ hashtable_int *hashtableRead(FILE* f)
         size_t dataSize;
         status = fread(&n, sizeof(size_t),1, f);
         status = fread(&dataSize, sizeof(size_t),1, f);
-        
+
         hashtbl=hashtableCreate(n, hash);
-        
+
         for (i=0; i<hashtbl->size; i++)
         {
             size_t j=0;
             status = fread(&n, sizeof(size_t),1, f);
-            
+
             for (j=0; j<n; j++)
             {
                 int key=0;
                 void* data=0;
                 status = fread(&key, sizeof(KeyType), 1, f);
-                
+
                 if (dataSize>sizeof(void*))
                 {
                     data=malloc(dataSize);
@@ -355,7 +355,7 @@ hashtable_int *hashtableRead(FILE* f)
                 }
                 else
                     status = fread(&data, dataSize, 1, f);
-                    
+
                 hashtableInsert(hashtbl, key, data);
                 //free(key);
             }
@@ -363,7 +363,7 @@ hashtable_int *hashtableRead(FILE* f)
     }
     else
         return 0;
-        
+
     return hashtbl;
 }