src/geometry/epipolar_geometry.cpp
// -------------------------------------------------------------------------------------------
// ---------------- Main: Motion from Essential and Homography; Triangulation ----------------
// -------------------------------------------------------------------------------------------
Motion from Essential and Homography; Triangulation
Motion from Essential and Homography ; Triangulation
void estiMotionByEssential(
    const vector<cv::Point2f> &pts_in_img1,
    const vector<cv::Point2f> &pts_in_img2,
    const cv::Mat &camera_intrinsics,
    cv::Mat &essential_matrix,
    cv::Mat &R, cv::Mat &t, vector<int> &inliers_index)
{
    inliers_index.clear();
    cv::Mat K = camera_intrinsics;                                       
    cv::Point2f principal_point(K.at<double>(0, 2), K.at<double>(1, 2)); 
    double focal_length = (K.at<double>(0, 0) + K.at<double>(1, 1)) / 2; 
    int method = cv::RANSAC;
    static double findEssentialMat_prob = basics::Config::get<double>("findEssentialMat_prob");
    static double findEssentialMat_threshold = basics::Config::get<double>("findEssentialMat_threshold");
    cv::Mat inliers_mask; 
    essential_matrix = findEssentialMat(
        pts_in_img1, pts_in_img2, focal_length, principal_point,
        method, findEssentialMat_prob, findEssentialMat_threshold,
        inliers_mask);
    essential_matrix /= essential_matrix.at<double>(2, 2);
    for (int i = 0; i < inliers_mask.rows; i++)
    {
        if ((int)inliers_mask.at<unsigned char>(i, 0) == 1)
        {
            inliers_index.push_back(i);
        }
    }
    recoverPose(essential_matrix, pts_in_img1, pts_in_img2, R, t, focal_length, principal_point, inliers_mask);
    t = t / sqrt(t.at<double>(1, 0) * t.at<double>(1, 0) + t.at<double>(2, 0) * t.at<double>(2, 0) +
                 t.at<double>(0, 0) * t.at<double>(0, 0));
}
void estiMotionByEssential ( const vector < cv :: Point2f > & pts_in_img1 , const vector < cv :: Point2f > & pts_in_img2 , const cv :: Mat & camera_intrinsics , cv :: Mat & essential_matrix , cv :: Mat & R , cv :: Mat & t , vector < int > & inliers_index ) { inliers_index . clear ( ) ; cv :: Mat K = camera_intrinsics ; cv :: Point2f principal_point ( K . at < double > ( 0 , 2 ) , K . at < double > ( 1 , 2 ) ) ; double focal_length = ( K . at < double > ( 0 , 0 ) + K . at < double > ( 1 , 1 ) ) / 2 ; int method = cv :: RANSAC ; static double findEssentialMat_prob = basics :: Config :: get < double > ( " " ) ; static double findEssentialMat_threshold = basics :: Config :: get < double > ( " " ) ; cv :: Mat inliers_mask ; essential_matrix = findEssentialMat ( pts_in_img1 , pts_in_img2 , focal_length , principal_point , method , findEssentialMat_prob , findEssentialMat_threshold , inliers_mask ) ; essential_matrix /= essential_matrix . at < double > ( 2 , 2 ) ; for ( int i = 0 ; i < inliers_mask . rows ; i ++ ) { if ( ( int ) inliers_mask . at < unsigned char > ( i , 0 ) == 1 ) { inliers_index . push_back ( i ) ; } } recoverPose ( essential_matrix , pts_in_img1 , pts_in_img2 , R , t , focal_length , principal_point , inliers_mask ) ; t = t / sqrt ( t . at < double > ( 1 , 0 ) * t . at < double > ( 1 , 0 ) + t . at < double > ( 2 , 0 ) * t . at < double > ( 2 , 0 ) + t . at < double > ( 0 , 0 ) * t . at < double > ( 0 , 0 ) ) ; }
Main: Motion from Essential and Homography; Triangulation
Main : Motion from Essential and Homography ; Triangulation
// rename // optical center // focal length // -- Essential matrix // double prob = 0.99; //This param settings give big error. Tested by image0001 and image0015. // double threshold = 3.0; //Use print_MatProperty to know its type: 8UC1 // print_MatProperty(inliers_mask); // Get inliers // Recover R,t from Essential matrix // Normalize t