src/geometry/motion_estimation.cpp
// (Deprecated) Choose EH by triangulation error. This helps nothing.
(Deprecated) Choose EH by triangulation error. This helps nothing.
( Deprecated ) Choose EH by triangulation error . This helps nothing .
void helperEvalEppiAndTriangErrors(
    const vector<cv::KeyPoint> &keypoints_1,
    const vector<cv::KeyPoint> &keypoints_2,
    const vector<vector<cv::DMatch>> &list_matches,
    const vector<vector<cv::Point3f>> &sols_pts3d_in_cam1_by_triang,
    const vector<cv::Mat> &list_R, const vector<cv::Mat> &list_t, const vector<cv::Mat> &list_normal,
    const cv::Mat &K,
    bool is_print_res)
{
    vector<double> list_error_epipolar;
    vector<double> list_error_triangulation;
    int num_solutions = list_R.size();
    const double TF = 3.84, TH = 5.99; 
    for (int i = 0; i < num_solutions; i++)
    {
        const cv::Mat &R = list_R[i], &t = list_t[i];
        const vector<cv::DMatch> &matches = list_matches[i];
        const vector<cv::Point3f> &pts3d = sols_pts3d_in_cam1_by_triang[i];
        vector<cv::Point2f> inlpts1, inlpts2;
        extractPtsFromMatches(keypoints_1, keypoints_2, matches, inlpts1, inlpts2);
        double err_epipolar = computeEpipolarConsError(inlpts1, inlpts2, R, t, K);
        double err_triangulation = 0; 
        double mean_score = 0;        
        int num_inlier_pts = inlpts1.size();
        for (int idx_inlier = 0; idx_inlier < num_inlier_pts; idx_inlier++)
        {
            const cv::Point2f &p1 = inlpts1[idx_inlier], &p2 = inlpts2[idx_inlier];
            cv::Mat pts3dc1 = basics::point3f_to_mat3x1(pts3d[idx_inlier]); 
            cv::Mat pts3dc2 = R * pts3dc1 + t;
            cv::Point2f pts2dc1 = cam2pixel(pts3dc1, K);
            cv::Point2f pts2dc2 = cam2pixel(pts3dc2, K);
            double dist1 = calcErrorSquare(p1, pts2dc1), dist2 = calcErrorSquare(p2, pts2dc2);
            err_triangulation += dist1 + dist2;
        }
        if (num_inlier_pts == 0)
        {
            err_triangulation = 9999999999;
            mean_score = 0;
        }
        else
        {
            err_triangulation = sqrt(err_triangulation / 2.0 / num_inlier_pts);
            mean_score /= num_inlier_pts;
        }
        list_error_epipolar.push_back(err_epipolar);
        list_error_triangulation.push_back(err_triangulation);
    }
    if (is_print_res)
    {
        printf("\n------------------------------------\n");
        printf("Print the mean error of each E/H method by using the inlier points.\n");
        for (int i = 0; i < num_solutions; i++)
        {
            printf("\n---------------\n");
            printf("Solution %d, num inliers = %d \n", i, (int)list_matches[i].size());
            basics::print_R_t(list_R[i], list_t[i]);
            if (!list_normal[i].empty())
                cout << "norm is:" << (list_normal[i]).t() << endl;
            printf("-- Epipolar cons error = %f \n", list_error_epipolar[i]);
            printf("-- Triangulation error = %f \n", list_error_triangulation[i]);
        }
    }
}
void helperEvalEppiAndTriangErrors ( const vector < cv :: KeyPoint > & keypoints_1 , const vector < cv :: KeyPoint > & keypoints_2 , const vector < vector < cv :: DMatch > > & list_matches , const vector < vector < cv :: Point3f > > & sols_pts3d_in_cam1_by_triang , const vector < cv :: Mat > & list_R , const vector < cv :: Mat > & list_t , const vector < cv :: Mat > & list_normal , const cv :: Mat & K , bool is_print_res ) { vector < double > list_error_epipolar ; vector < double > list_error_triangulation ; int num_solutions = list_R . size ( ) ; const double TF = 3.84 , TH = 5.99 ; for ( int i = 0 ; i < num_solutions ; i ++ ) { const cv :: Mat & R = list_R [ i ] , & t = list_t [ i ] ; const vector < cv :: DMatch > & matches = list_matches [ i ] ; const vector < cv :: Point3f > & pts3d = sols_pts3d_in_cam1_by_triang [ i ] ; vector < cv :: Point2f > inlpts1 , inlpts2 ; extractPtsFromMatches ( keypoints_1 , keypoints_2 , matches , inlpts1 , inlpts2 ) ; double err_epipolar = computeEpipolarConsError ( inlpts1 , inlpts2 , R , t , K ) ; double err_triangulation = 0 ; double mean_score = 0 ; int num_inlier_pts = inlpts1 . size ( ) ; for ( int idx_inlier = 0 ; idx_inlier < num_inlier_pts ; idx_inlier ++ ) { const cv :: Point2f & p1 = inlpts1 [ idx_inlier ] , & p2 = inlpts2 [ idx_inlier ] ; cv :: Mat pts3dc1 = basics :: point3f_to_mat3x1 ( pts3d [ idx_inlier ] ) ; cv :: Mat pts3dc2 = R * pts3dc1 + t ; cv :: Point2f pts2dc1 = cam2pixel ( pts3dc1 , K ) ; cv :: Point2f pts2dc2 = cam2pixel ( pts3dc2 , K ) ; double dist1 = calcErrorSquare ( p1 , pts2dc1 ) , dist2 = calcErrorSquare ( p2 , pts2dc2 ) ; err_triangulation += dist1 + dist2 ; } if ( num_inlier_pts == 0 ) { err_triangulation = 9999999999 ; mean_score = 0 ; } else { err_triangulation = sqrt ( err_triangulation / 2.0 / num_inlier_pts ) ; mean_score /= num_inlier_pts ; } list_error_epipolar . push_back ( err_epipolar ) ; list_error_triangulation . push_back ( err_triangulation ) ; } if ( is_print_res ) { printf ( " \n \n " ) ; printf ( " \n " ) ; for ( int i = 0 ; i < num_solutions ; i ++ ) { printf ( " \n \n " ) ; printf ( " \n " , i , ( int ) list_matches [ i ] . size ( ) ) ; basics :: print_R_t ( list_R [ i ] , list_t [ i ] ) ; if ( ! list_normal [ i ] . empty ( ) ) cout << " " << ( list_normal [ i ] ) . t ( ) << endl ; printf ( " \n " , list_error_epipolar [ i ] ) ; printf ( " \n " , list_error_triangulation [ i ] ) ; } } }
(Deprecated) Choose EH by triangulation error.
( Deprecated ) Choose EH by triangulation error .
// Param for computing mean_score. Here F(fundmental)==E(essential) // epipolar error // In image frame,  the error between triangulation and real // more correctly called: symmetric transfer error // f_rc(d2)+f_cr(d2) from ORB-SLAM // print triangulation result // 3d pos in camera 1 // printf("%dth inlier, err_triangulation = %f\n", idx_inlier, err_triangulation); // Store the error // -- Print out result