Merge pull request #2995 from hengguan:ppf_matching

surface matching

* normalize quantionion

* keep master same as origin repository

* normalize quantonion

Author: hengguan

modules/surface_matching/src/ppf_match_3d.cpp

@@ -366,6 +366,13 @@ void PPF3DDetector::clusterPoses(std::vector<Pose3DPtr>& poseList, int numPoses,
       tAvg *= 1.0 / wSum;
       qAvg *= 1.0 / wSum;
 
+      // normalize quantonion
+      const double qNorm = cv::norm(qAvg);
+      if (qNorm > EPS)
+      {
+        qAvg *= 1.0 / qNorm;
+      }
+
       curPoses[0]->updatePoseQuat(qAvg, tAvg);
       curPoses[0]->numVotes=curCluster->numVotes;
 
@@ -397,6 +404,13 @@ void PPF3DDetector::clusterPoses(std::vector<Pose3DPtr>& poseList, int numPoses,
       tAvg *= 1.0 / curSize;
       qAvg *= 1.0 / curSize;
 
+      // normalize quantonion
+      const double qNorm = cv::norm(qAvg);
+      if (qNorm > EPS)
+      {
+        qAvg *= 1.0 / qNorm;
+      }
+
       curPoses[0]->updatePoseQuat(qAvg, tAvg);
       curPoses[0]->numVotes=curCluster->numVotes;