Parallelize `ICP::registerModelToScene`

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

@@ -125,7 +125,7 @@ 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, double Pose[16]);
+CV_EXPORTS Mat transformPCPose(Mat pc, const double Pose[16]);
 
 /**
  *  Generate a random 4x4 pose matrix

modules/surface_matching/src/icp.cpp

@@ -540,7 +540,10 @@ int ICP::registerModelToScene(const Mat& srcPC, const Mat& dstPC, double& residu
 // source point clouds are assumed to contain their normals
 int ICP::registerModelToScene(const Mat& srcPC, const Mat& dstPC, std::vector<Pose3DPtr>& poses)
 {
-  for (size_t i=0; i<poses.size(); i++)
+  #if defined _OPENMP
+  #pragma omp parallel for
+  #endif
+  for (int i=0; i<(int)poses.size(); i++)
   {
     Matx44d poseICP = Matx44d::eye();
     Mat srcTemp = transformPCPose(srcPC, poses[i]->pose);

modules/surface_matching/src/ppf_helpers.cpp

@@ -530,7 +530,7 @@ Mat transPCCoeff(Mat pc, float scale, float Cx, float Cy, float Cz, float MinVal
   return pcn;
 }
 
-Mat transformPCPose(Mat pc, double Pose[16])
+Mat transformPCPose(Mat pc, const double Pose[16])
 {
   Mat pct = Mat(pc.rows, pc.cols, CV_32F);