Wrap ICP for Python

modules/surface_matching/include/opencv2/surface_matching/icp.hpp

@@ -77,17 +77,17 @@ namespace ppf_match_3d
 * 5. Linearization of Point-to-Plane metric by Kok Lim Low:
 * https://www.comp.nus.edu.sg/~lowkl/publications/lowk_point-to-plane_icp_techrep.pdf
 */
-class CV_EXPORTS ICP
+class CV_EXPORTS_W ICP
 {
 public:
 
-  enum ICP_SAMPLING_TYPE
+  CV_WRAP enum
   {
-    ICP_SAMPLING_TYPE_UNIFORM,
+    ICP_SAMPLING_TYPE_UNIFORM = 0,
-    ICP_SAMPLING_TYPE_GELFAND
+    ICP_SAMPLING_TYPE_GELFAND = 1
   };
 
-  ICP()
+  CV_WRAP ICP()
   {
     m_tolerance = 0.005f;
     m_rejectionScale = 2.5f;
@@ -114,7 +114,7 @@ public:
             applied. Leave it as 0.
      *  @param [in] numMaxCorr Currently this parameter is ignored and only PickyICP is applied. Leave it as 1.
      */
-  ICP(const int iterations, const float tolerence=0.05, const float rejectionScale=2.5, const int numLevels=6, const ICP_SAMPLING_TYPE sampleType = ICP_SAMPLING_TYPE_UNIFORM, const int numMaxCorr=1)
+  CV_WRAP ICP(const int iterations, const float tolerence = 0.05f, const float rejectionScale = 2.5f, const int numLevels = 6, const int sampleType = ICP::ICP_SAMPLING_TYPE_UNIFORM, const int numMaxCorr = 1)
   {
     m_tolerance = tolerence;
     m_numNeighborsCorr = numMaxCorr;
@@ -136,7 +136,7 @@ public:
      *
      *  \details It is assumed that the model is registered on the scene. Scene remains static, while the model transforms. The output poses transform the models onto the scene. Because of the point to plane minimization, the scene is expected to have the normals available. Expected to have the normals (Nx6).
      */
-  int registerModelToScene(const Mat& srcPC, const Mat& dstPC, double& residual, double pose[16]);
+  CV_WRAP int registerModelToScene(const Mat& srcPC, const Mat& dstPC, CV_OUT double& residual, CV_OUT Matx44d& pose);
 
   /**
      *  \brief Perform registration with multiple initial poses

modules/surface_matching/samples/ppf_icp.py

@@ -0,0 +1,52 @@
+import cv2
+import numpy as np
+
+def rotation(theta):
+    tx, ty, tz = theta
+
+    Rx = np.array([[1, 0, 0], [0, np.cos(tx), -np.sin(tx)], [0, np.sin(tx), np.cos(tx)]])
+    Ry = np.array([[np.cos(ty), 0, -np.sin(ty)], [0, 1, 0], [np.sin(ty), 0, np.cos(ty)]])
+    Rz = np.array([[np.cos(tz), -np.sin(tz), 0], [np.sin(tz), np.cos(tz), 0], [0, 0, 1]])
+
+    return np.dot(Rx, np.dot(Ry, Rz))
+
+width = 20
+height = 10
+max_deg = np.pi / 12
+
+cloud, rotated_cloud = [None]*3, [None]*3
+retval, residual, pose = [None]*3, [None]*3, [None]*3
+noise = np.random.normal(0.0, 0.1, height * width * 3).reshape((-1, 3))
+noise2 = np.random.normal(0.0, 1.0, height * width)
+
+x, y = np.meshgrid(
+    range(-width/2, width/2),
+    range(-height/2, height/2),
+    sparse=False, indexing='xy'
+)
+z = np.zeros((height, width))
+
+cloud[0] = np.dstack((x, y, z)).reshape((-1, 3)).astype(np.float32)
+cloud[1] = noise.astype(np.float32) + cloud[0]
+cloud[2] = cloud[1]
+cloud[2][:, 2] += noise2.astype(np.float32)
+
+R = rotation([
+    0, #np.random.uniform(-max_deg, max_deg),
+    np.random.uniform(-max_deg, max_deg),
+    0, #np.random.uniform(-max_deg, max_deg)
+])
+t = np.zeros((3, 1))
+Rt = np.vstack((
+    np.hstack((R, t)),
+    np.array([0, 0, 0, 1])
+)).astype(np.float32)
+
+icp = cv2.ppf_match_3d.ICP(100)
+
+I = np.eye(4)
+print("Unaligned error:\t%.6f" % np.linalg.norm(I - Rt))
+for i in range(3):
+    rotated_cloud[i] = np.matmul(Rt[0:3,0:3], cloud[i].T).T + Rt[:3,3].T
+    retval[i], residual[i], pose[i] = icp.registerModelToScene(rotated_cloud[i], cloud[i])
+    print("ICP error:\t\t%.6f" % np.linalg.norm(I - np.matmul(pose[0], Rt)))

modules/surface_matching/src/icp.cpp

@@ -293,7 +293,7 @@ static hashtable_int* getHashtable(int* data, size_t length, int numMaxElement)
 }
 
 // source point clouds are assumed to contain their normals
-int ICP::registerModelToScene(const Mat& srcPC, const Mat& dstPC, double& residual, double pose[16])
+int ICP::registerModelToScene(const Mat& srcPC, const Mat& dstPC, double& residual, Matx44d& pose)
 {
   int n = srcPC.rows;
 
@@ -320,7 +320,7 @@ int ICP::registerModelToScene(const Mat& srcPC, const Mat& dstPC, double& residu
   Mat dstPC0 = dstTemp;
 
   // initialize pose
-  matrixIdentity(4, pose);
+  matrixIdentity(4, pose.val);
 
   void* flann = indexPCFlann(dstPC0);
   Mat M = Mat::eye(4,4,CV_64F);
@@ -339,7 +339,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);
+    Mat srcPCT = transformPCPose(srcPC0, pose.val);
 
     const int sampleStep = cvRound((double)n/(double)numSamples);
     std::vector<int> srcSampleInd;
@@ -500,11 +500,11 @@ int ICP::registerModelToScene(const Mat& srcPC, const Mat& dstPC, double& residu
     }
 
     double TempPose[16];
-    matrixProduct44(PoseX, pose, TempPose);
+    matrixProduct44(PoseX, pose.val, TempPose);
 
     // no need to copy the last 4 rows
     for (int c=0; c<12; c++)
-      pose[c] = TempPose[c];
+      pose.val[c] = TempPose[c];
 
     residual = tempResidual;
 
@@ -519,17 +519,17 @@ int ICP::registerModelToScene(const Mat& srcPC, const Mat& dstPC, double& residu
   }
 
   // Pose(1:3, 4) = Pose(1:3, 4)./scale;
-  pose[3] = pose[3]/scale + meanAvg[0];
+  pose.val[3] = pose.val[3]/scale + meanAvg[0];
-  pose[7] = pose[7]/scale + meanAvg[1];
+  pose.val[7] = pose.val[7]/scale + meanAvg[1];
-  pose[11] = pose[11]/scale + meanAvg[2];
+  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, Rpose);
+  poseToR(pose.val, Rpose);
   matrixProduct331(Rpose, meanAvg, Cpose);
-  pose[3] -= Cpose[0];
+  pose.val[3] -= Cpose[0];
-  pose[7] -= Cpose[1];
+  pose.val[7] -= Cpose[1];
-  pose[11] -= Cpose[2];
+  pose.val[11] -= Cpose[2];
 
   residual = tempResidual;
 
@@ -542,10 +542,10 @@ int ICP::registerModelToScene(const Mat& srcPC, const Mat& dstPC, std::vector<Po
 {
   for (size_t i=0; i<poses.size(); i++)
   {
-    double poseICP[16]={0};
+    Matx44d poseICP = Matx44d::eye();
     Mat srcTemp = transformPCPose(srcPC, poses[i]->pose);
     registerModelToScene(srcTemp, dstPC, poses[i]->residual, poseICP);
-    poses[i]->appendPose(poseICP);
+    poses[i]->appendPose(poseICP.val);
   }
   return 0;
 }