From acb88d755d80954c78d9ec3ed32f260f0da64520 Mon Sep 17 00:00:00 2001 From: Hamdi Sahloul Date: Tue, 21 Feb 2017 22:20:17 +0900 Subject: [PATCH] Wrap ICP for Python --- .../include/opencv2/surface_matching/icp.hpp | 14 ++--- modules/surface_matching/samples/ppf_icp.py | 52 +++++++++++++++++++ modules/surface_matching/src/icp.cpp | 28 +++++----- 3 files changed, 73 insertions(+), 21 deletions(-) create mode 100644 modules/surface_matching/samples/ppf_icp.py diff --git a/modules/surface_matching/include/opencv2/surface_matching/icp.hpp b/modules/surface_matching/include/opencv2/surface_matching/icp.hpp index d8947027a..123120a10 100644 --- a/modules/surface_matching/include/opencv2/surface_matching/icp.hpp +++ b/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_GELFAND + ICP_SAMPLING_TYPE_UNIFORM = 0, + 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 diff --git a/modules/surface_matching/samples/ppf_icp.py b/modules/surface_matching/samples/ppf_icp.py new file mode 100644 index 000000000..6a68e5287 --- /dev/null +++ b/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))) diff --git a/modules/surface_matching/src/icp.cpp b/modules/surface_matching/src/icp.cpp index 615f93d23..250b97eb7 100644 --- a/modules/surface_matching/src/icp.cpp +++ b/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 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[7] = pose[7]/scale + meanAvg[1]; - pose[11] = pose[11]/scale + meanAvg[2]; + 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, Rpose); + poseToR(pose.val, Rpose); matrixProduct331(Rpose, meanAvg, Cpose); - pose[3] -= Cpose[0]; - pose[7] -= Cpose[1]; - pose[11] -= Cpose[2]; + pose.val[3] -= Cpose[0]; + pose.val[7] -= Cpose[1]; + pose.val[11] -= Cpose[2]; residual = tempResidual; @@ -542,10 +542,10 @@ int ICP::registerModelToScene(const Mat& srcPC, const Mat& dstPC, std::vectorpose); registerModelToScene(srcTemp, dstPC, poses[i]->residual, poseICP); - poses[i]->appendPose(poseICP); + poses[i]->appendPose(poseICP.val); } return 0; }