Fix marker integration (Update marker pose)

src/stella_vslam/data/keyframe.h

@@ -200,7 +200,6 @@ class keyframe : public std::enable_shared_from_this<keyframe> {
 
     /**
      * Get all of the markers
-     * (NOTE: including nullptr)
      */
     std::vector<std::shared_ptr<marker>> get_markers() const;
 

src/stella_vslam/data/marker2d.h

@@ -17,6 +17,7 @@ class marker2d {
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
     //! constructor
+    marker2d() = default;
     marker2d(const std::vector<cv::Point2f>& undist_corners, const eigen_alloc_vector<Vec3_t>& bearings,
              const Mat33_t& rot_cm, const Vec3_t& trans_cm, unsigned int id, const std::shared_ptr<marker_model::base>& marker_model);
 

src/stella_vslam/global_optimization_module.cc

@@ -3,7 +3,9 @@
 #include "stella_vslam/tracking_module.h"
 #include "stella_vslam/data/keyframe.h"
 #include "stella_vslam/data/landmark.h"
+#include "stella_vslam/data/marker.h"
 #include "stella_vslam/data/map_database.h"
+#include "stella_vslam/marker_model/base.h"
 #include "stella_vslam/match/fuse.h"
 #include "stella_vslam/util/converter.h"
 #include "stella_vslam/util/yaml.h"
@@ -324,6 +326,11 @@ void global_optimization_module::correct_covisibility_keyframes(const module::ke
         const Vec3_t trans_nw = Sim3_nw_after_correction.translation() / s_nw;
         const Mat44_t cam_pose_nw = util::converter::to_eigen_pose(rot_nw, trans_nw);
         neighbor->set_pose_cw(cam_pose_nw);
+        for (const auto& mkr : neighbor->get_markers()) {
+            const auto& mkr2d = neighbor->markers_2d_[mkr->id_];
+            eigen_alloc_vector<Vec3_t> corners_pos_w = mkr2d.compute_corners_pos_w(neighbor->get_pose_wc(), mkr2d.marker_model_->corners_pos_);
+            mkr->set_corner_pos(corners_pos_w);
+        }
 
         // update graph
         neighbor->graph_node_->update_connections();

src/stella_vslam/module/initializer.cc

@@ -294,6 +294,11 @@ void initializer::scale_map(const std::shared_ptr<data::keyframe>& init_keyfrm,
     Mat44_t cam_pose_cw = curr_keyfrm->get_pose_cw();
     cam_pose_cw.block<3, 1>(0, 3) *= scale;
     curr_keyfrm->set_pose_cw(cam_pose_cw);
+    for (const auto& mkr : curr_keyfrm->get_markers()) {
+        const auto& mkr2d = curr_keyfrm->markers_2d_[mkr->id_];
+        eigen_alloc_vector<Vec3_t> corners_pos_w = mkr2d.compute_corners_pos_w(curr_keyfrm->get_pose_wc(), mkr2d.marker_model_->corners_pos_);
+        mkr->set_corner_pos(corners_pos_w);
+    }
 
     // scaling landmarks
     const auto landmarks = init_keyfrm->get_landmarks();

src/stella_vslam/module/loop_bundle_adjuster.cc

@@ -1,7 +1,9 @@
 #include "stella_vslam/mapping_module.h"
 #include "stella_vslam/data/keyframe.h"
 #include "stella_vslam/data/landmark.h"
+#include "stella_vslam/data/marker.h"
 #include "stella_vslam/data/map_database.h"
+#include "stella_vslam/marker_model/base.h"
 #include "stella_vslam/module/loop_bundle_adjuster.h"
 #include "stella_vslam/optimize/global_bundle_adjuster.h"
 
@@ -98,6 +100,12 @@ void loop_bundle_adjuster::optimize() {
             keyfrm_to_cam_pose_cw_before_BA[parent->id_] = parent->get_pose_cw();
             // update the camera pose
             parent->set_pose_cw(keyfrm_to_pose_cw_after_global_BA.at(parent->id_));
+            for (const auto& mkr : parent->get_markers()) {
+                const auto& mkr2d = parent->markers_2d_[mkr->id_];
+                eigen_alloc_vector<Vec3_t> corners_pos_w = mkr2d.compute_corners_pos_w(parent->get_pose_wc(), mkr2d.marker_model_->corners_pos_);
+                mkr->set_corner_pos(corners_pos_w);
+            }
+
             // finish updating
             keyfrms_to_check.pop_front();
         }

src/stella_vslam/optimize/global_bundle_adjuster.cc

@@ -210,6 +210,11 @@ void global_bundle_adjuster::optimize_for_initialization(bool* const force_stop_
         const auto cam_pose_cw = util::converter::to_eigen_mat(keyfrm_vtx->estimate());
 
         keyfrm->set_pose_cw(cam_pose_cw);
+        for (const auto& mkr : keyfrm->get_markers()) {
+            const auto& mkr2d = keyfrm->markers_2d_[mkr->id_];
+            eigen_alloc_vector<Vec3_t> corners_pos_w = mkr2d.compute_corners_pos_w(keyfrm->get_pose_wc(), mkr2d.marker_model_->corners_pos_);
+            mkr->set_corner_pos(corners_pos_w);
+        }
     }
 
     for (unsigned int i = 0; i < lms.size(); ++i) {

src/stella_vslam/optimize/graph_optimizer.cc

@@ -1,6 +1,8 @@
 #include "stella_vslam/data/keyframe.h"
 #include "stella_vslam/data/landmark.h"
+#include "stella_vslam/data/marker.h"
 #include "stella_vslam/data/map_database.h"
+#include "stella_vslam/marker_model/base.h"
 #include "stella_vslam/optimize/graph_optimizer.h"
 #include "stella_vslam/optimize/terminate_action.h"
 #include "stella_vslam/optimize/internal/sim3/shot_vertex.h"
@@ -258,6 +260,11 @@ void graph_optimizer::optimize(const std::shared_ptr<data::keyframe>& loop_keyfr
 
             const Mat44_t cam_pose_cw = util::converter::to_eigen_pose(rot_cw, trans_cw);
             keyfrm->set_pose_cw(cam_pose_cw);
+            for (const auto& mkr : keyfrm->get_markers()) {
+                const auto& mkr2d = keyfrm->markers_2d_[mkr->id_];
+                eigen_alloc_vector<Vec3_t> corners_pos_w = mkr2d.compute_corners_pos_w(keyfrm->get_pose_wc(), mkr2d.marker_model_->corners_pos_);
+                mkr->set_corner_pos(corners_pos_w);
+            }
 
             corrected_Sim3s_wc[id] = corrected_Sim3_cw.inverse();
         }

src/stella_vslam/optimize/local_bundle_adjuster.cc

@@ -375,6 +375,11 @@ void local_bundle_adjuster::optimize(data::map_database* map_db,
 
             auto keyfrm_vtx = keyfrm_vtx_container.get_vertex(local_keyfrm);
             local_keyfrm->set_pose_cw(keyfrm_vtx->estimate());
+            for (const auto& mkr : local_keyfrm->get_markers()) {
+                const auto& mkr2d = local_keyfrm->markers_2d_[mkr->id_];
+                eigen_alloc_vector<Vec3_t> corners_pos_w = mkr2d.compute_corners_pos_w(local_keyfrm->get_pose_wc(), mkr2d.marker_model_->corners_pos_);
+                mkr->set_corner_pos(corners_pos_w);
+            }
         }
 
         for (const auto& id_local_lm_pair : local_lms) {