src/geometry/motion_estimation.cpp
// Check [Epipoloar error] and [Triangulation result] for each common inlier in both E and H
Check [Epipoloar error] and [Triangulation result] for each common inlier in both E and H
Check [ Epipoloar error ] and [ Triangulation result ] for each common inlier in both E and H
void print_EpipolarError_and_TriangulationResult_By_Common_Inlier(
    const vector<cv::Point2f> &pts_img1, const vector<cv::Point2f> &pts_img2,
    const vector<cv::Point2f> &pts_on_np1, const vector<cv::Point2f> &pts_on_np2,
    const vector<vector<cv::Point3f>> &sols_pts3d_in_cam1,
    const vector<vector<int>> &list_inliers,
    const vector<cv::Mat> &list_R, const vector<cv::Mat> &list_t, const cv::Mat &K)
{
    constexpr int kMaxNumPtsToCheckAndPrint = 1000;
    int num_solutions = list_R.size();
    const vector<int> &inliers_index_e = list_inliers[0];
    const vector<int> &inliers_index_h = list_inliers[1];
    cout << "\n---------------------------------------" << endl;
    cout << "Check [Epipoloar error] and [Triangulation result]" << endl;
    cout << "for the first " << kMaxNumPtsToCheckAndPrint << " points:";
    int cnt = 0;
    int num_points = pts_img1.size();
    for (int i = 0; i < num_points && cnt < kMaxNumPtsToCheckAndPrint; i++)
    {
        auto pe = find(inliers_index_e.begin(), inliers_index_e.end(), i);
        auto ph = find(inliers_index_h.begin(), inliers_index_h.end(), i);
        if (pe == inliers_index_e.end() || ph == inliers_index_h.end())
            continue;
        int ith_in_e_inliers = pe - inliers_index_e.begin();
        int ith_in_h_inliers = ph - inliers_index_h.begin();
        cout << "\n--------------" << endl;
        printf("Printing the %dth (in common) and %dth (in matched) point's real position in image:\n", cnt++, i);
        cv::Point2f p1 = pts_img1[i], p2 = pts_img2[i];
        cout << "cam1, pixel pos (u,v): " << p1 << endl;
        cout << "cam2, pixel pos (u,v): " << p2 << endl;
        cv::Point2f p_cam1 = pts_on_np1[i]; 
        cv::Point2f p_cam2 = pts_on_np2[i];
        for (int j = 0; j < num_solutions; j++)
        {
            const cv::Mat &R = list_R[j], &t = list_t[j];
            double err_epipolar = computeEpipolarConsError(p1, p2, R, t, K);
            cout << "===solu " << j << ": epipolar_error*1e6 is " << err_epipolar * 1e6 << endl;
            int ith_in_curr_sol;
            if (j == 0)
                ith_in_curr_sol = ith_in_e_inliers;
            else
                ith_in_curr_sol = ith_in_h_inliers;
            cv::Mat pts3dc1 = basics::point3f_to_mat3x1(sols_pts3d_in_cam1[j][ith_in_curr_sol]); 
            cv::Mat pts3dc2 = R * pts3dc1 + t;
            cv::Point2f pts2dc1 = cam2pixel(pts3dc1, K);
            cv::Point2f pts2dc2 = cam2pixel(pts3dc2, K);
            cout << "-- In img1, pos: " << pts2dc1 << endl;
            cout << "-- In img2, pos: " << pts2dc2 << endl;
            cout << "-- On cam1, pos: " << pts3dc1.t() << endl;
            cout << "-- On cam2, pos: " << pts3dc2.t() << endl;
        }
        cout << endl;
    }
}
void print_EpipolarError_and_TriangulationResult_By_Common_Inlier ( const vector < cv :: Point2f > & pts_img1 , const vector < cv :: Point2f > & pts_img2 , const vector < cv :: Point2f > & pts_on_np1 , const vector < cv :: Point2f > & pts_on_np2 , const vector < vector < cv :: Point3f > > & sols_pts3d_in_cam1 , const vector < vector < int > > & list_inliers , const vector < cv :: Mat > & list_R , const vector < cv :: Mat > & list_t , const cv :: Mat & K ) { constexpr int kMaxNumPtsToCheckAndPrint = 1000 ; int num_solutions = list_R . size ( ) ; const vector < int > & inliers_index_e = list_inliers [ 0 ] ; const vector < int > & inliers_index_h = list_inliers [ 1 ] ; cout << " \n " << endl ; cout << " " << endl ; cout << " " << kMaxNumPtsToCheckAndPrint << " " ; int cnt = 0 ; int num_points = pts_img1 . size ( ) ; for ( int i = 0 ; i < num_points && cnt < kMaxNumPtsToCheckAndPrint ; i ++ ) { auto pe = find ( inliers_index_e . begin ( ) , inliers_index_e . end ( ) , i ) ; auto ph = find ( inliers_index_h . begin ( ) , inliers_index_h . end ( ) , i ) ; if ( pe == inliers_index_e . end ( ) || ph == inliers_index_h . end ( ) ) continue ; int ith_in_e_inliers = pe - inliers_index_e . begin ( ) ; int ith_in_h_inliers = ph - inliers_index_h . begin ( ) ; cout << " \n " << endl ; printf ( " \n " , cnt ++ , i ) ; cv :: Point2f p1 = pts_img1 [ i ] , p2 = pts_img2 [ i ] ; cout << " " << p1 << endl ; cout << " " << p2 << endl ; cv :: Point2f p_cam1 = pts_on_np1 [ i ] ; cv :: Point2f p_cam2 = pts_on_np2 [ i ] ; for ( int j = 0 ; j < num_solutions ; j ++ ) { const cv :: Mat & R = list_R [ j ] , & t = list_t [ j ] ; double err_epipolar = computeEpipolarConsError ( p1 , p2 , R , t , K ) ; cout << " " << j << " " << err_epipolar * 1e6 << endl ; int ith_in_curr_sol ; if ( j == 0 ) ith_in_curr_sol = ith_in_e_inliers ; else ith_in_curr_sol = ith_in_h_inliers ; cv :: Mat pts3dc1 = basics :: point3f_to_mat3x1 ( sols_pts3d_in_cam1 [ j ] [ ith_in_curr_sol ] ) ; cv :: Mat pts3dc2 = R * pts3dc1 + t ; cv :: Point2f pts2dc1 = cam2pixel ( pts3dc1 , K ) ; cv :: Point2f pts2dc2 = cam2pixel ( pts3dc2 , K ) ; cout << " " << pts2dc1 << endl ; cout << " " << pts2dc2 << endl ; cout << " " << pts3dc1 . t ( ) << endl ; cout << " " << pts3dc2 . t ( ) << endl ; } cout << endl ; } }
Check [Epipoloar error] and [Triangulation result] for each common inlier in both E and H
Check [ Epipoloar error ] and [ Triangulation result ] for each common inlier in both E and H
// Iterate through points. // Print point pos in image frame. // Print result of each method. // point pos on the normalized plane // print epipolar error // print triangulation result // 3d pos in camera 1