The quaternion of pose in Surface Matching should be normalized

[hengguan]

System information (version)

• OpenCV => :4.4
• Operating System / Platform => :ubuntu18.04 64 Bit
• Compiler => : g++ (Ubuntu 7.5.0-3ubuntu1~18.04) 7.5.0

Detailed description

I found a bug in Surface Matching that the quaternion of Pose in PPF3DDetector::clusterPoses function must be normalized before being updated, Reference the PPF Matching of PCL library.

Steps to reproduce

before updated (line number in ppf_match_3d.cpp: 366-370, 397-401):

  tAvg *= 1.0 / curSize;
  qAvg *= 1.0 / curSize;

  curPoses[0]->updatePoseQuat(qAvg, tAvg);
  curPoses[0]->numVotes=curCluster->numVotes;

afer updated:

  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;

[asmorkalov]

@hengguan Thanks for the issue report. Could you provide small reproducer to highlight the bug?

[hengguan]

@hengguan Thanks for the issue report. Could you provide small reproducer to highlight the bug?

i improve origin code in opencv based on Joel Vidal and Stefan Hinterstoisser work, and using Linemod as test data. my improved code link: https://github.com/hengguan/ppf_matcher.