For Equirectangular, scale is determined by distance, not depth (#561)

src/stella_vslam/data/keyframe.cc

@@ -433,6 +433,36 @@ float keyframe::compute_median_depth(const bool abs) const {
     return depths.at((depths.size() - 1) / 2);
 }
 
+float keyframe::compute_median_distance() const {
+    std::vector<std::shared_ptr<landmark>> landmarks;
+    Mat44_t pose_cw;
+    {
+        std::lock_guard<std::mutex> lock1(mtx_observations_);
+        std::lock_guard<std::mutex> lock2(mtx_pose_);
+        landmarks = landmarks_;
+        pose_cw = pose_cw_;
+    }
+
+    std::vector<float> distances;
+    distances.reserve(frm_obs_.undist_keypts_.size());
+    const Mat33_t rot_cw = pose_cw.block<3, 3>(0, 0);
+    const Vec3_t trans_cw = pose_cw.block<3, 1>(0, 3);
+
+    for (const auto& lm : landmarks) {
+        if (!lm) {
+            continue;
+        }
+        const Vec3_t pos_w = lm->get_pos_in_world();
+        const Vec3_t pos_c = rot_cw * pos_w + trans_cw;
+        float distance = pos_c.norm();
+        distances.push_back(distance);
+    }
+
+    std::sort(distances.begin(), distances.end());
+
+    return distances.at((distances.size() - 1) / 2);
+}
+
 bool keyframe::depth_is_available() const {
     return camera_->setup_type_ != camera::setup_type_t::Monocular;
 }

src/stella_vslam/data/keyframe.h

@@ -201,6 +201,11 @@ class keyframe : public std::enable_shared_from_this<keyframe> {
      */
     float compute_median_depth(const bool abs = false) const;
 
+    /**
+     * Compute median of distances
+     */
+    float compute_median_distance() const;
+
     /**
      * Whether or not the camera setting is capable of obtaining depth information
      */

src/stella_vslam/mapping_module.cc

@@ -300,8 +300,14 @@ void mapping_module::create_new_landmarks(std::atomic<bool>& abort_create_new_la
 
         // if the scene scale is much smaller than the baseline, abort the triangulation
         if (use_baseline_dist_thr_ratio_) {
-            const float median_depth_in_ngh = ngh_keyfrm->compute_median_depth(true);
-            if (baseline_dist < baseline_dist_thr_ratio_ * median_depth_in_ngh) {
+            float median_scale_in_ngh;
+            if (ngh_keyfrm->camera_->model_type_ == camera::model_type_t::Equirectangular) {
+                median_scale_in_ngh = ngh_keyfrm->compute_median_distance();
+            }
+            else {
+                median_scale_in_ngh = ngh_keyfrm->compute_median_depth(true);
+            }
+            if (baseline_dist < baseline_dist_thr_ratio_ * median_scale_in_ngh) {
                 continue;
             }
         }

src/stella_vslam/module/initializer.cc

@@ -279,14 +279,20 @@ bool initializer::create_map_for_monocular(data::bow_vocabulary* bow_vocab, data
 
     if (indefinite_scale) {
         // scale the map so that the median of depths is 1.0
-        const auto median_depth = init_keyfrm->compute_median_depth(init_keyfrm->camera_->model_type_ == camera::model_type_t::Equirectangular);
-        const auto inv_median_depth = 1.0 / median_depth;
-        if (curr_keyfrm->get_num_tracked_landmarks(1) < min_num_triangulated_pts_ && median_depth < 0) {
+        float median_scale;
+        if (init_keyfrm->camera_->model_type_ == camera::model_type_t::Equirectangular) {
+            median_scale = init_keyfrm->compute_median_distance();
+        }
+        else {
+            median_scale = init_keyfrm->compute_median_depth(true);
+        }
+        const auto inv_median_scale = 1.0 / median_scale;
+        if (curr_keyfrm->get_num_tracked_landmarks(1) < min_num_triangulated_pts_ && median_scale < 0) {
             spdlog::info("seems to be wrong initialization, resetting");
             state_ = initializer_state_t::Wrong;
             return false;
         }
-        scale_map(init_keyfrm, curr_keyfrm, inv_median_depth * scaling_factor_);
+        scale_map(init_keyfrm, curr_keyfrm, inv_median_scale * scaling_factor_);
     }
 
     // update the current frame pose