Use marker association in globalBA/localBA

src/openvslam/optimize/global_bundle_adjuster.cc

@@ -1,10 +1,14 @@
 #include "openvslam/data/keyframe.h"
 #include "openvslam/data/landmark.h"
+#include "openvslam/data/marker.h"
 #include "openvslam/data/map_database.h"
+#include "openvslam/marker_model/base.h"
 #include "openvslam/optimize/global_bundle_adjuster.h"
 #include "openvslam/optimize/internal/landmark_vertex_container.h"
+#include "openvslam/optimize/internal/marker_vertex_container.h"
 #include "openvslam/optimize/internal/se3/shot_vertex_container.h"
 #include "openvslam/optimize/internal/se3/reproj_edge_wrapper.h"
+#include "openvslam/optimize/internal/distance_edge.h"
 #include "openvslam/util/converter.h"
 
 #include <g2o/core/solver.h>
@@ -27,12 +31,20 @@ void global_bundle_adjuster::optimize(const unsigned int lead_keyfrm_id_in_globa
 
     const auto keyfrms = map_db_->get_all_keyframes();
     const auto lms = map_db_->get_all_landmarks();
+    const auto markers = map_db_->get_all_markers();
     std::vector<bool> is_optimized_lm(lms.size(), true);
 
     // 2. Construct an optimizer
 
-    auto linear_solver = g2o::make_unique<g2o::LinearSolverCSparse<g2o::BlockSolver_6_3::PoseMatrixType>>();
-    auto block_solver = g2o::make_unique<g2o::BlockSolver_6_3>(std::move(linear_solver));
+    std::unique_ptr<g2o::BlockSolverBase> block_solver;
+    if (markers.size()) {
+        auto linear_solver = g2o::make_unique<g2o::LinearSolverCSparse<g2o::BlockSolverX::PoseMatrixType>>();
+        block_solver = g2o::make_unique<g2o::BlockSolverX>(std::move(linear_solver));
+    }
+    else {
+        auto linear_solver = g2o::make_unique<g2o::LinearSolverCSparse<g2o::BlockSolver_6_3::PoseMatrixType>>();
+        block_solver = g2o::make_unique<g2o::BlockSolver_6_3>(std::move(linear_solver));
+    }
     auto algorithm = new g2o::OptimizationAlgorithmLevenberg(std::move(block_solver));
 
     g2o::SparseOptimizer optimizer;
@@ -128,6 +140,65 @@ void global_bundle_adjuster::optimize(const unsigned int lead_keyfrm_id_in_globa
         }
     }
 
+    // Container of the landmark vertices
+    internal::marker_vertex_container marker_vtx_container(vtx_id_offset, markers.size());
+
+    for (unsigned int marker_idx = 0; marker_idx < markers.size(); ++marker_idx) {
+        auto mkr = markers.at(marker_idx);
+        if (!mkr) {
+            continue;
+        }
+
+        // Convert the corners to the g2o vertex, then set it to the optimizer
+        auto corner_vertices = marker_vtx_container.create_vertices(mkr, false);
+        for (unsigned int corner_idx = 0; corner_idx < corner_vertices.size(); ++corner_idx) {
+            const auto corner_vtx = corner_vertices[corner_idx];
+            optimizer.addVertex(corner_vtx);
+
+            for (const auto& keyfrm : mkr->observations_) {
+                if (!keyfrm) {
+                    continue;
+                }
+                if (keyfrm->will_be_erased()) {
+                    continue;
+                }
+                if (!keyfrm_vtx_container.contain(keyfrm)) {
+                    continue;
+                }
+                const auto keyfrm_vtx = keyfrm_vtx_container.get_vertex(keyfrm);
+                const auto& mkr_2d = keyfrm->markers_2d_.at(mkr->id_);
+                const auto& undist_pt = mkr_2d.undist_corners_.at(corner_idx);
+                const float x_right = -1.0;
+                const float inv_sigma_sq = 1.0;
+                auto reproj_edge_wrap = reproj_edge_wrapper(keyfrm, keyfrm_vtx, nullptr, corner_vtx,
+                                                            0, undist_pt.x, undist_pt.y, x_right,
+                                                            inv_sigma_sq, 0.0, false);
+                reproj_edge_wraps.push_back(reproj_edge_wrap);
+                optimizer.addEdge(reproj_edge_wrap.edge_);
+            }
+        }
+
+        // Add edges for marker shape
+        double informationRatio = 1.0e9;
+        for (unsigned int corner_idx = 0; corner_idx < corner_vertices.size(); ++corner_idx) {
+            const auto dist_edge = new internal::distance_edge();
+            dist_edge->setMeasurement(mkr->marker_model_->width_);
+            dist_edge->setInformation(MatRC_t<1, 1>::Identity() * informationRatio);
+            dist_edge->setVertex(0, corner_vertices[corner_idx]);
+            dist_edge->setVertex(1, corner_vertices[(corner_idx + 1) % corner_vertices.size()]);
+            optimizer.addEdge(dist_edge);
+        }
+        for (unsigned int corner_idx = 0; corner_idx < 2; ++corner_idx) {
+            const auto dist_edge = new internal::distance_edge();
+            const double diagonal_length = std::hypot(mkr->marker_model_->width_, mkr->marker_model_->width_);
+            dist_edge->setMeasurement(diagonal_length);
+            dist_edge->setInformation(MatRC_t<1, 1>::Identity() * informationRatio);
+            dist_edge->setVertex(0, corner_vertices[corner_idx]);
+            dist_edge->setVertex(1, corner_vertices[(corner_idx + 2) % corner_vertices.size()]);
+            optimizer.addEdge(dist_edge);
+        }
+    }
+
     // 5. Perform optimization
 
     optimizer.initializeOptimization();
@@ -179,6 +250,20 @@ void global_bundle_adjuster::optimize(const unsigned int lead_keyfrm_id_in_globa
             lm->loop_BA_identifier_ = lead_keyfrm_id_in_global_BA;
         }
     }
+
+    for (unsigned int marker_idx = 0; marker_idx < markers.size(); ++marker_idx) {
+        auto mkr = markers.at(marker_idx);
+        if (!mkr) {
+            continue;
+        }
+
+        eigen_alloc_vector<Vec3_t> corner_pos_w;
+        for (unsigned int corner_idx = 0; corner_idx < 4; ++corner_idx) {
+            auto corner_vtx = marker_vtx_container.get_vertex(mkr, corner_idx);
+            corner_pos_w.push_back(corner_vtx->estimate());
+        }
+        mkr->set_corner_pos(corner_pos_w);
+    }
 }
 
 } // namespace optimize

src/openvslam/optimize/internal/distance_edge.h

@@ -0,0 +1,51 @@
+#ifndef OPENVSLAM_OPTIMIZER_G2O_DISTANCE_EDGE_H
+#define OPENVSLAM_OPTIMIZER_G2O_DISTANCE_EDGE_H
+
+#include "openvslam/type.h"
+#include "openvslam/optimize/internal/landmark_vertex.h"
+
+#include <g2o/core/base_binary_edge.h>
+
+namespace openvslam {
+namespace optimize {
+namespace internal {
+
+class distance_edge final : public g2o::BaseBinaryEdge<1, double, landmark_vertex, landmark_vertex> {
+public:
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+    distance_edge();
+
+    bool read(std::istream& is) override;
+
+    bool write(std::ostream& os) const override;
+
+    void computeError() override;
+};
+
+inline distance_edge::distance_edge()
+    : g2o::BaseBinaryEdge<1, double, landmark_vertex, landmark_vertex>() {}
+
+inline bool distance_edge::read(std::istream& is) {
+    is >> _measurement;
+    is >> information()(0, 0);
+    return true;
+}
+
+inline bool distance_edge::write(std::ostream& os) const {
+    os << measurement() << " ";
+    os << " " << information()(0, 0);
+    return os.good();
+}
+
+inline void distance_edge::computeError() {
+    const auto v1 = static_cast<const landmark_vertex*>(_vertices.at(0));
+    const auto v2 = static_cast<const landmark_vertex*>(_vertices.at(1));
+    _error[0] = _measurement - (v2->estimate() - v1->estimate()).norm();
+}
+
+} // namespace internal
+} // namespace optimize
+} // namespace openvslam
+
+#endif // OPENVSLAM_OPTIMIZER_G2O_DISTANCE_EDGE_H

src/openvslam/optimize/internal/marker_vertex_container.h

@@ -0,0 +1,100 @@
+#ifndef OPENVSLAM_OPTIMIZE_G2O_MARKER_VERTEX_CONTAINER_H
+#define OPENVSLAM_OPTIMIZE_G2O_MARKER_VERTEX_CONTAINER_H
+
+#include "openvslam/type.h"
+#include "openvslam/data/marker.h"
+#include "openvslam/optimize/internal/landmark_vertex.h"
+
+#include <unordered_map>
+#include <memory>
+
+namespace openvslam {
+
+namespace data {
+class marker;
+} // namespace data
+
+namespace optimize {
+namespace internal {
+
+class marker_vertex_container {
+public:
+    //! Constructor
+    explicit marker_vertex_container(const std::shared_ptr<unsigned int> offset, const unsigned int num_reserve = 200);
+
+    //! Destructor
+    virtual ~marker_vertex_container() = default;
+
+    //! Create and return the g2o vertex created from the specified marker
+    std::vector<landmark_vertex*> create_vertices(const std::shared_ptr<data::marker>& mkr, const bool is_constant);
+
+    //! Get vertex corresponding with the specified marker
+    landmark_vertex* get_vertex(const std::shared_ptr<data::marker>& mkr, const unsigned int corner_id) const;
+
+    //! Get vertex corresponding with the specified marker ID
+    landmark_vertex* get_vertex(const unsigned int marker_id, const unsigned int corner_id) const;
+
+private:
+    landmark_vertex* create_vertex(const unsigned int marker_id, const unsigned int corner_id,
+                                   const Vec3_t& pos_w, const bool is_constant);
+
+    //! vertex ID = offset + marker ID * 4 + corner ID
+    const std::shared_ptr<unsigned int> offset_ = nullptr;
+
+    //! key: marker ID, value: vertexs
+    std::unordered_map<unsigned int, std::vector<landmark_vertex*>> vtx_container_;
+
+    //! key: marker ID, value: vertex IDs
+    std::unordered_map<unsigned int, std::vector<unsigned int>> vtx_id_container_;
+
+    //! key: vertex ID, value: marker ID and corner ID
+    std::unordered_map<unsigned int, std::pair<unsigned int, unsigned int>> id_container_;
+};
+
+inline marker_vertex_container::marker_vertex_container(const std::shared_ptr<unsigned int> offset, const unsigned int num_reserve)
+    : offset_(offset) {
+    vtx_container_.reserve(num_reserve);
+    vtx_id_container_.reserve(num_reserve);
+    id_container_.reserve(num_reserve);
+}
+
+inline std::vector<landmark_vertex*> marker_vertex_container::create_vertices(const std::shared_ptr<data::marker>& mkr, const bool is_constant) {
+    std::vector<landmark_vertex*> vertices;
+    std::vector<unsigned int> vtx_ids;
+    for (unsigned int i = 0; i < mkr->corners_pos_w_.size(); ++i) {
+        vtx_ids.push_back(*offset_);
+        vertices.push_back(create_vertex(mkr->id_, i, mkr->corners_pos_w_[i], is_constant));
+    }
+    vtx_id_container_[mkr->id_] = vtx_ids;
+    vtx_container_[mkr->id_] = vertices;
+    return vertices;
+}
+
+inline landmark_vertex* marker_vertex_container::create_vertex(const unsigned int marker_id, const unsigned int corner_id,
+                                                               const Vec3_t& pos_w, const bool is_constant) {
+    // Create vertex
+    const auto vtx_id = *offset_;
+    (*offset_)++;
+    auto vtx = new landmark_vertex();
+    vtx->setId(vtx_id);
+    vtx->setEstimate(pos_w);
+    vtx->setFixed(is_constant);
+    vtx->setMarginalized(false);
+    // Set to id database
+    id_container_[vtx_id] = std::make_pair(marker_id, corner_id);
+    return vtx;
+}
+
+inline landmark_vertex* marker_vertex_container::get_vertex(const std::shared_ptr<data::marker>& mkr, const unsigned int corner_id) const {
+    return get_vertex(mkr->id_, corner_id);
+}
+
+inline landmark_vertex* marker_vertex_container::get_vertex(const unsigned int marker_id, const unsigned int corner_id) const {
+    return vtx_container_.at(marker_id)[corner_id];
+}
+
+} // namespace internal
+} // namespace optimize
+} // namespace openvslam
+
+#endif // OPENVSLAM_OPTIMIZE_G2O_MARKER_VERTEX_CONTAINER_H