Revert "Fix marker integration (Update marker pose) (#311)"

This reverts commit bd80221.
stella-cv · May 29, 2022 · e6d8fa5 · e6d8fa5
1 parent 2eb46bf
commit e6d8fa5
Show file tree

Hide file tree

Showing 8 changed files with 1 addition and 38 deletions.
diff --git a/src/stella_vslam/data/keyframe.h b/src/stella_vslam/data/keyframe.h
@@ -200,6 +200,7 @@ 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;
 

diff --git a/src/stella_vslam/data/marker2d.h b/src/stella_vslam/data/marker2d.h
@@ -17,7 +17,6 @@ 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);
 

diff --git a/src/stella_vslam/global_optimization_module.cc b/src/stella_vslam/global_optimization_module.cc
@@ -3,9 +3,7 @@
 #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"
@@ -326,11 +324,6 @@ 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();

diff --git a/src/stella_vslam/module/initializer.cc b/src/stella_vslam/module/initializer.cc
@@ -294,11 +294,6 @@ 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();

diff --git a/src/stella_vslam/module/loop_bundle_adjuster.cc b/src/stella_vslam/module/loop_bundle_adjuster.cc
@@ -1,9 +1,7 @@
 #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"
 
@@ -100,12 +98,6 @@ 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();
         }

diff --git a/src/stella_vslam/optimize/global_bundle_adjuster.cc b/src/stella_vslam/optimize/global_bundle_adjuster.cc
@@ -210,11 +210,6 @@ 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) {

diff --git a/src/stella_vslam/optimize/graph_optimizer.cc b/src/stella_vslam/optimize/graph_optimizer.cc
@@ -1,8 +1,6 @@
 #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"
@@ -260,11 +258,6 @@ 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();
         }

diff --git a/src/stella_vslam/optimize/local_bundle_adjuster.cc b/src/stella_vslam/optimize/local_bundle_adjuster.cc
@@ -375,11 +375,6 @@ 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) {