surface matching

[hengguan]

normalize quaternion

After weighted average quaternion of poses in the same cluster, the weighted average quaternion should be normalized.

[savuor]

This is not the only (and not the most accurate) pose averaging method, see my comments to issue #2553
But it is accurate enough if the poses are close to each other. The only way to check this is to run the corresponding tests.

[savuor]

👍