Search code examples
opencvcamera-calibration

How can I obtain real world point from intrinsic and extrinsic parameters of camera?


I'm using OpenCV for measure object length using camera.(The object is rely on plane)
For camera calibration, I used checker board. Here's my approach in pseudo code

// position array of checker corners in real world coordinate
3d_checker_position 

// possition array of checker corners in image coordinate
2d_checker_position

// find camera matrix, distortion coefficients, 
// camera rotation vector, camera translation vector
camera_mat, dist_coeffs, rvec, tvec = cv.calibrateCamera(3d_checker_position, 2d_checker_position)

// 3D -> 2D
test_2d_checker_position 
  = cv.projectPoints(3d_checker_position, rvec, tvec, camera_mat, dist_coeffs)
// test_2d_checker_position == 2d_checker_position

// 2D -> 3D
test_3d_checker_position 
  = what_function_should_I_use(2d_checker_position, rvec, tvec, camera_mat, dist_coeffs)

How to implement what_function_should_I_use, or is there any function in OpenCV?

EDIT 23.04.07

For fully understanding of converting world to image coordinate, I firstly implemented 3D -> 2D by my hand. This is based on below pinhole model equation.

sP = I * E * W

where P is 2 channel (x,y) image point, I is intrinsic parameter matrix(camera matrix), E is extrinsic parameter matrix(transformation), and W is 3 channel (x, y, z) world point.
Below is implementation code from 3D to 2D. s is weighted factor for converting homogeneous coordinate to image coordinate.

// Firstly, assumed that intrinsic parameters(camera matrix), 
// extrinsic parameters(rotation and translation vector), 
// and distortion parameters are pre-calculated from 
// cv::calibrateCamera function.
cv::Mat intrinsic; // (3x3)
cv::Mat rvec; // (3x1)
cv::Mat tvec; // (3x1) 
std::vector<float> distortion;
// ...

// Secondly, make a extrinsic matrix which is [R | t].
cv::Mat extrinsic= cv::Mat(cv::Size(4, 3), CV_64F);
cv::Mat rot_mat;
cv::Rodrigues(rvec, rot_mat);
rot_mat.copyTo(extrinsic(cv::Rect(0, 0, 3, 3)));
tvec.copyTo(extrinsic(cv::Rect(3, 0, 1, 3)));

// Thirdly, make a pinhole camera model matrix 
// by merging intrinsic and extrinsic parameters.
cv::Mat pinhole_model = intrinsic * extrinsic;


// Finally, convert 3D point to 2D point.

cv::Mat world_point_homogeneous = cv::Mat(cv::Size(1, 4), CV_64F);
world_point_homogeneous.at<double>(0, 0) = world_point.x;
world_point_homogeneous.at<double>(1, 0) = world_point.y;
world_point_homogeneous.at<double>(2, 0) = world_point.z;
world_point_homogeneous.at<double>(3, 0) = 1;

cv::Mat projected_homogeneous 
  = pinhole_model * world_point_homogeneous;

// Convert homogeneous coordinate to image coordinate.
// This equals dividing 's' from 'P' in the above equation.
const double w = projected_homogeneous.at<double>(2, 0);
cv::Point2d image_point;
image_point.x = projected_homogeneous.at<double>(0, 0) / w;
image_point.y = projected_homogeneous.at<double>(1, 0) / w;

Result
The above code do convert 3D point to 2D point which is very closing to the result of cv::projectPoints().

Problem

  1. When and how the distortion parameters are applied to the process?
  2. Back to the origin question, I need the w factor(in the equation, s) before converting 2D point to 3D point. How can I obtain it?

Solution

  • I implemented the solution. As @fana mentioned, the 3D point cannot be obtained from 2D if it's z is not specified. In my case, the target object in a image lies parallel to the same plane as the checker board, which means the z value is the same as the z of checker board when camera calibration is performed.
    Specifically, 2D point is converted to 3D by following pinhole camera model formula.
    enter image description here

    where (u,v) is point in image coordinate, (X, Y, Z) is point in world coordinate, the left matrix is intrinsic parameters, and the right matrix is extrinsic parameters.
    Simply, calling the product of intrinsic and extrinsic matrix as P,
    enter image description here

    since Z is a constant(in this case 0), a simultaneous linear equation for X and Y is established.

    // Assume world z is 0
    std::vector<cv::Point3d> unproject(
        const std::vector<cv::Point2d>& points,
        cv::Mat rvec, cv::Mat tvec,
        cv::Mat& intrinsic,
        const std::vector<float>& distortion) {
    
      // Reconstruct pinhole-camera model
      cv::Mat transformation = cv::Mat(cv::Size(4, 3), CV_64F);
      cv::Mat rot_mat;
      cv::Rodrigues(rvec, rot_mat);
      rot_mat.copyTo(transformation(cv::Rect(0, 0, 3, 3)));
      tvec.copyTo(transformation(cv::Rect(3, 0, 1, 3)));
      cv::Mat new_intrinsic = cv::getOptimalNewCameraMatrix(intrinsic, distortion, cv::Size(800, 600), 0);
      cv::Mat pinhole_model = new_intrinsic * transformation;
    
      std::vector<cv::Point3d> world_points(points.size());
      for (int i = 0; i < points.size(); i++) {
        const double a = pinhole_model.at<double>(2, 0) * points[i].x - pinhole_model.at<double>(0, 0);
        const double b = pinhole_model.at<double>(2, 1) * points[i].x - pinhole_model.at<double>(0, 1);
        const double c = pinhole_model.at<double>(0, 3) - pinhole_model.at<double>(2, 3) * points[i].x;
        const double d = pinhole_model.at<double>(2, 0) * points[i].y - pinhole_model.at<double>(1, 0);
        const double e = pinhole_model.at<double>(2, 1) * points[i].x - pinhole_model.at<double>(1, 1);
        const double f = pinhole_model.at<double>(1, 3) - pinhole_model.at<double>(2, 3) * points[i].y;
    
        double world_x, world_y;
        world_y = (a * f - c * d) / (a * e - b * d);
        world_x = (c - world_y * b) / a;
        world_points[i].x = world_x; 
        world_points[i].y = world_y;
        world_points[i].z = 0;
        std::cout << "image point " << points[i] << " to " << world_points[i] << std::endl;
      }
      return world_points;
    }
    

    The cv::getOptimalNewCameraMatrix function, which updates the intrinsic matrix through the distortion coefficients, has not been verified to work properly in this solution.