src/geometry/motion_estimation.cpp
// Print each solution's result in order
Print each solution's result in order
Print each solution ' s result in order
void print_EpipolarError_and_TriangulationResult_By_Solution(
    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)
{
    cout << "\n---------------------------------------" << endl;
    cout << "Check [Epipoloar error] and [Triangulation result]" << endl;
    cout << "for each solution. Printing from back to front." << endl;
    int num_solutions = list_R.size();
    for (int j = 0; j < num_solutions; j++)
    {
        const cv::Mat &R = list_R[j], &t = list_t[j];
        const vector<int> &inliers = list_inliers[j];
        int num_inliers = inliers.size();
        constexpr int kMaxNumInliersToPrint = 5;
        for (int _idx_inlier = 0; _idx_inlier < std::min(kMaxNumInliersToPrint, num_inliers); _idx_inlier++)
        {
            int idx_inlier = num_inliers - 1 - _idx_inlier;
            int idx_pts = inliers[idx_inlier];
            cout << "\n--------------" << endl;
            printf("Printing the %dth last inlier point in solution %d\n", _idx_inlier, j);
            printf("which is %dth cv::KeyPoint\n", idx_pts);
            cv::Point2f p1 = pts_img1[idx_pts], p2 = pts_img2[idx_pts];
            cout << "cam1, pixel pos (u,v): " << p1 << endl;
            cout << "cam2, pixel pos (u,v): " << p2 << endl;
            double err_epipolar = computeEpipolarConsError(p1, p2, R, t, K);
            cout << "===solu " << j << ": epipolar_error*1e6 is " << err_epipolar * 1e6 << endl;
            cv::Mat pts3dc1 = basics::point3f_to_mat3x1(sols_pts3d_in_cam1[j][idx_inlier]); 
            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;
        }
    }
}
void print_EpipolarError_and_TriangulationResult_By_Solution ( 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 ) { cout << " \n " << endl ; cout << " " << endl ; cout << " " << endl ; int num_solutions = list_R . size ( ) ; for ( int j = 0 ; j < num_solutions ; j ++ ) { const cv :: Mat & R = list_R [ j ] , & t = list_t [ j ] ; const vector < int > & inliers = list_inliers [ j ] ; int num_inliers = inliers . size ( ) ; constexpr int kMaxNumInliersToPrint = 5 ; for ( int _idx_inlier = 0 ; _idx_inlier < std :: min ( kMaxNumInliersToPrint , num_inliers ) ; _idx_inlier ++ ) { int idx_inlier = num_inliers - 1 - _idx_inlier ; int idx_pts = inliers [ idx_inlier ] ; cout << " \n " << endl ; printf ( " \n " , _idx_inlier , j ) ; printf ( " \n " , idx_pts ) ; cv :: Point2f p1 = pts_img1 [ idx_pts ] , p2 = pts_img2 [ idx_pts ] ; cout << " " << p1 << endl ; cout << " " << p2 << endl ; double err_epipolar = computeEpipolarConsError ( p1 , p2 , R , t , K ) ; cout << " " << j << " " << err_epipolar * 1e6 << endl ; cv :: Mat pts3dc1 = basics :: point3f_to_mat3x1 ( sols_pts3d_in_cam1 [ j ] [ idx_inlier ] ) ; 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 ; } } }
Print each solution's result in order
Print each solution ' s result in order
// Print point pos in image frame. // print epipolar error // print triangulation result // 3d pos in camera 1 // cv::Point2f pts2dc1 = cam2pixel(sols_pts3d_in_cam1[j][idx_inlier], K);