Method triangulate_with_two_keyframes

src/PLPSLAM/mapping_module.cc:420–479

    }

    void mapping_module::triangulate_with_two_keyframes(data::keyframe *cur_keyfrm, data::keyframe *ngh_keyfrm)
    {
        // lowe's_ratio will not be used
        match::robust robust_matcher(0.0, false);
        // estimate matches between the current and neighbor keyframes,
        // then reject outliers using Essential matrix computed from the two camera poses

        // (cur bearing) * E_ngh_to_cur * (ngh bearing) = 0
        // const Mat33_t E_ngh_to_cur = solve::essential_solver::create_E_21(ngh_keyfrm, cur_keyfrm_);
        const Mat33_t E_ngh_to_cur = solve::essential_solver::create_E_21(ngh_keyfrm->get_rotation(), ngh_keyfrm->get_translation(),
                                                                          cur_keyfrm->get_rotation(), cur_keyfrm->get_translation());

        // vector of matches (idx in the current, idx in the neighbor)
        std::vector<std::pair<unsigned int, unsigned int>> matches;
        robust_matcher.match_for_triangulation(cur_keyfrm, ngh_keyfrm, E_ngh_to_cur, matches);

        // FW: initialize the two view triangulator -> pass the keyframe pointer to that object, with poses information
        const module::two_view_triangulator triangulator(cur_keyfrm, ngh_keyfrm, 1.0);

#ifdef USE_OPENMP
#pragma omp parallel for
#endif
        for (unsigned int i = 0; i < matches.size(); ++i)
        {
            const auto idx_1 = matches.at(i).first;
            const auto idx_2 = matches.at(i).second;

            // triangulate between idx_1 and idx_2
            Vec3_t pos_w;

            // FW: triangulation + check positive depth + check reprojection error + check scale consistency
            if (!triangulator.triangulate(idx_1, idx_2, pos_w))
            {
                // failed
                continue;
            }
            // succeeded

            // create a landmark object
            auto lm = new data::landmark(pos_w, cur_keyfrm, map_db_);

            lm->add_observation(cur_keyfrm, idx_1);
            lm->add_observation(ngh_keyfrm, idx_2);

            cur_keyfrm->add_landmark(lm, idx_1);
            ngh_keyfrm->add_landmark(lm, idx_2);

            lm->compute_descriptor();
            lm->update_normal_and_depth();

            map_db_->add_landmark(lm);
            // wait for redundancy check
#ifdef USE_OPENMP
#pragma omp critical
#endif
            {
                local_map_cleaner_->add_fresh_landmark(lm);
            }