diff --git a/modules/surface_matching/include/opencv2/surface_matching/ppf_helpers.hpp b/modules/surface_matching/include/opencv2/surface_matching/ppf_helpers.hpp
index c6477f7b2..a1a2ba517 100644
--- a/modules/surface_matching/include/opencv2/surface_matching/ppf_helpers.hpp
+++ b/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
diff --git a/modules/surface_matching/src/icp.cpp b/modules/surface_matching/src/icp.cpp
index c18f285e7..7c2622d5d 100644
--- a/modules/surface_matching/src/icp.cpp
+++ b/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);
diff --git a/modules/surface_matching/src/ppf_helpers.cpp b/modules/surface_matching/src/ppf_helpers.cpp
index df8ed1413..46ce44e19 100644
--- a/modules/surface_matching/src/ppf_helpers.cpp
+++ b/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);