Method triangulate

src/PLPSLAM/module/two_view_triangulator.cc:45–122

        }

        bool two_view_triangulator::triangulate(const unsigned idx_1, const unsigned int idx_2, Vec3_t &pos_w) const
        {
            const auto &keypt_1 = keyfrm_1_->undist_keypts_.at(idx_1);
            const float keypt_1_x_right = keyfrm_1_->stereo_x_right_.at(idx_1);
            const bool is_stereo_1 = 0 <= keypt_1_x_right;

            const auto &keypt_2 = keyfrm_2_->undist_keypts_.at(idx_2);
            const float keypt_2_x_right = keyfrm_2_->stereo_x_right_.at(idx_2);
            const bool is_stereo_2 = 0 <= keypt_2_x_right;

            // rays with reference of each camera
            const Vec3_t ray_c_1 = keyfrm_1_->bearings_.at(idx_1);
            const Vec3_t ray_c_2 = keyfrm_2_->bearings_.at(idx_2);
            // rays with the world reference
            const Vec3_t ray_w_1 = rot_w1_ * ray_c_1;
            const Vec3_t ray_w_2 = rot_w2_ * ray_c_2;
            const auto cos_rays_parallax = ray_w_1.dot(ray_w_2);

            // compute the stereo parallax if the keypoint is observed as stereo
            const auto cos_stereo_parallax_1 = is_stereo_1
                                                   ? std::cos(2.0 * atan2(camera_1_->true_baseline_ / 2.0, keyfrm_1_->depths_.at(idx_1)))
                                                   : 2.0;
            const auto cos_stereo_parallax_2 = is_stereo_2
                                                   ? std::cos(2.0 * atan2(camera_2_->true_baseline_ / 2.0, keyfrm_2_->depths_.at(idx_2)))
                                                   : 2.0;
            const auto cos_stereo_parallax = std::min(cos_stereo_parallax_1, cos_stereo_parallax_2);

            // select to use "linear triangulation" or "stereo triangulation"
            // threshold of minimum angle of the two rays
            const bool triangulate_with_two_cameras =
                // check if the sufficient parallax is provided
                ((!is_stereo_1 && !is_stereo_2) && 0.0 < cos_rays_parallax && cos_rays_parallax < cos_rays_parallax_thr_)
                // check if the parallax between the two cameras is larger than the stereo parallax
                || ((is_stereo_1 || is_stereo_2) && 0.0 < cos_rays_parallax && cos_rays_parallax < cos_stereo_parallax);

            // triangulate
            if (triangulate_with_two_cameras)
            {
                pos_w = solve::triangulator::triangulate(ray_c_1, ray_c_2, cam_pose_1w_, cam_pose_2w_);
            }
            else if (is_stereo_1 && cos_stereo_parallax_1 < cos_stereo_parallax_2)
            {
                pos_w = keyfrm_1_->triangulate_stereo(idx_1);
            }
            else if (is_stereo_2 && cos_stereo_parallax_2 < cos_stereo_parallax_1)
            {
                pos_w = keyfrm_2_->triangulate_stereo(idx_2);
            }
            else
            {
                return false;
            }

            // check the triangulated point is located in front of the two cameras
            if (!check_depth_is_positive(pos_w, rot_1w_, trans_1w_, camera_1_) || !check_depth_is_positive(pos_w, rot_2w_, trans_2w_, camera_2_))
            {
                return false;
            }