Add update_pose() API call

[AlexeyMerzlyakov]

Adding new system.h/system.cc API call for updating camera position by known pose. This is a part required for stella-cv/stella_vslam_ros#11 ticket to add the support of the /initialpose intentionally set by developer.

[AlexeyMerzlyakov]

The solution has been verified by calling update_pose() for a known pose(s) in parallel thread. That was checked on a EuRoC dataset in localization mode (using previously pre-build by run_euroc_slam map).

The verification patch looks like follows:

diff --git a/example/run_euroc_localization.cc b/example/run_euroc_localization.cc
index 89be0b3..41716f0 100644
--- a/example/run_euroc_localization.cc
+++ b/example/run_euroc_localization.cc
@@ -122,6 +133,18 @@ void mono_localization(const std::shared_ptr<openvslam::config>& cfg,
 #endif
     });
 
+    std::thread pose_thread([&]() {
+        while (true) {
+            Eigen::Matrix4d pose;
+            pose << 1, 0, 0, 0.0,
+                    0, 1, 0, 0.0,
+                    0, 0, 1, 0.0,
+                    0, 0, 0, 1;
+            SLAM.update_pose(pose);
+            std::this_thread::sleep_for(std::chrono::milliseconds(10));
+       }
+    });
+
     // run the viewer in the current thread
 #ifdef USE_PANGOLIN_VIEWER
     viewer.run();
@@ -130,6 +153,7 @@ void mono_localization(const std::shared_ptr<openvslam::config>& cfg,
 #endif
 
     thread.join();
+    pose_thread.join();
 
     // shutdown the SLAM process
     SLAM.shutdown();

There are two cases were checked:

• Pose (0.0, 0.0, 0.0) (belonging to track) gives correct localization on the initial part of dataset, as expected.
• Pose (1.0, 2.0, 3.0) (does not belong to track) gives Lost state, camera is being intentionally placed to a requested position until robot will be localized later:
  

[ymd-stella]

@AlexeyMerzlyakov Is this a draft?

[AlexeyMerzlyakov]

@AlexeyMerzlyakov Is this a draft?

There are some changes might be applied later, when openvslam_ros part to be added. But in general, the approach is fixed until be reviewed. I would like appreciate if you could leave a feedback on main approach and its details.

[AlexeyMerzlyakov]

@ymd-stella , @SteveMacenski Thank you for attentive review! Most of the comments I agree with and will make a next patch meeting the review items in next update.

[AlexeyMerzlyakov]

Made a force-push update (there were code refactoring conflicts) with review items met.

[ymd-stella]

LGTM! Thanks!
I have written some comments, please have a look.

[AlexeyMerzlyakov]

Okay, I've made next push. There were met all second review items & applied clang-format on changed files.

[ymd-stella]

It's getting very good. Thank you for your contribution. There are still a few things that annoy me, so let me discuss them.