Update submap.cpp for loop closure detection in 3d

[zihaomu]

Related to PR3002 opencv/opencv_contirb.

The purpose is to add an edge-adding function of Loop Closure Detection detection, which is called when a loop was detected.

Pull Request Readiness Checklist

See details at https://github.com/opencv/opencv/wiki/How_to_contribute#making-a-good-pull-request

• I agree to contribute to the project under Apache 2 License.
• To the best of my knowledge, the proposed patch is not based on a code under GPL or other license that is incompatible with OpenCV
• The PR is proposed to proper branch
• There is reference to original bug report and related work
• There is accuracy test, performance test and test data in opencv_extra repository, if applicable
  Patch to opencv_extra has the same branch name.
• The feature is well documented and sample code can be built with the project CMake

[savuor]

This changes allows loop closure code to add loop closure edges to pose graph
Looks OK to me

[fengyuentau]

jenkins cn please retry a build