Method update_normal_and_depth

src/PLPSLAM/data/landmark.cc:249–295

        }

        void landmark::update_normal_and_depth()
        {
            std::map<keyframe *, unsigned int> observations;
            keyframe *ref_keyfrm;
            Vec3_t pos_w;
            {
                std::lock_guard<std::mutex> lock1(mtx_observations_);
                std::lock_guard<std::mutex> lock2(mtx_position_);
                if (will_be_erased_)
                {
                    return;
                }
                observations = observations_;
                ref_keyfrm = ref_keyfrm_;
                pos_w = pos_w_;
            }

            if (observations.empty())
            {
                return;
            }

            // FW: calculate the average viewing direction for this map point
            Vec3_t mean_normal = Vec3_t::Zero();
            unsigned int num_observations = 0;
            for (const auto &observation : observations)
            {
                auto keyfrm = observation.first;
                const Vec3_t cam_center = keyfrm->get_cam_center();
                const Vec3_t normal = pos_w_ - cam_center;
                mean_normal = mean_normal + normal.normalized();
                ++num_observations;
            }

            const Vec3_t cam_to_lm_vec = pos_w - ref_keyfrm->get_cam_center();
            const auto dist = cam_to_lm_vec.norm();
            const auto scale_level = ref_keyfrm->undist_keypts_.at(observations.at(ref_keyfrm)).octave; // which level of image pyramid the keypoint is extracted
            const auto scale_factor = ref_keyfrm->scale_factors_.at(scale_level);                       // the scale factor = pow(1.2, scale_level) corresponding to the level of image pyramid
            const auto num_scale_levels = ref_keyfrm->num_scale_levels_;                                // = 8

            {
                std::lock_guard<std::mutex> lock3(mtx_position_);
                max_valid_dist_ = dist * scale_factor;
                min_valid_dist_ = max_valid_dist_ / ref_keyfrm->scale_factors_.at(num_scale_levels - 1);
                mean_normal_ = mean_normal.normalized();
            }
        }

        float landmark::get_min_valid_distance() const
        {