opencvcomputer-vision

OpenCV - Find angle to a static depth-skewed rectangle


Given a rectangle at a fixed location and a camera at an unknown location, how can I find the horizontal angle between the camera and the rectangle? The rectangle will always be perpendicular to the ground and is a fixed size. The camera will always be at a lower elevation than the rectangle.

Rectangle head on

Rectangle from an angle

(Images are from here, which is the same premise.)

How can you calculate the horizontal angle from the camera to the target rectangle? Assume that the rectangular target has already been translated into a MatOfPoints object in OpenCV.

UPDATE: Using SolvePNP and the following images, the resulting Rotation and Position Mats give some unexpected numbers for different pictures. The detected corners have been colored with circles, and a BoundingRectangle drawn. Head on Gives resulting mat dump:

Rotation: [2.662893740213683e-05;
 -0.0001007426907390123;
 0.0484638952150343]
Position: [18.43394203112024;
 48.39531890689394;
 0.1318397318617369]

Slightly to the left Gives resulting mat dump:

Rotation: [-0.0001071012175770757;
 -4.285121336676997e-05;
 0.01258020218302199]
Position: [35.87669469188199;
 45.47657018572935;
 0.1244586014980523]

Solution

  • The hard approach :

    i) You find CLOCKWISE the rectangle 2D corners in image.

    std::vector<cv::Point2f> rect_corners_2d; // TODO find clockwise rectangle 2D corners in image
    

    I suppose you have these points since you have drawn the rectangle.

    ii) You define CLOCKWISE the rectangle 3D corners in physical units whenever that rectangle stands right on camera centered on its optical axis facing it.

    float width; // TODO set rectangle's physical width
    float height; // TODO set rectangle's physical height
    std::vector<cv::Point3f> rect_corners_3d =
    {
        cv::Point3f(-width/2, -height/2, 0.0), // top-left
        cv::Point3f(width/2, -height/2, 0.0), // top-right
        cv::Point3f(width/2, height/2, 0.0), // bottom-right
        cv::Point3f(-width/2, height/2, 0.0) // bottom-left
    };
    

    iii) You rotate your image points such that each 2D corner corresponds to its 3D counterpart.

    // rectangle's top-left corner must be the first in 2D image points since it is the first in 3D defined ones
    int index; // TODO set index of top-left corner in 2D image points
    std::rotate(rect_corners_2d.begin(), rect_corners_2d.begin() + index, rect_corners_2d.end());
    

    iv) Know your camera intrinsic parameters and lens distortion coefficients.

    cv::Mat_<double> intrinsic_parameters; // TODO set camera matrix
    cv::Mat_<double> distortion_coefficients; // TODO set lens distortion coefficients
    

    I hope you have your camera's calibration.

    v) Extract camera's pose relative to seen rectangle : http://docs.opencv.org/2.4/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html#solvepnp

    cv::Vec3d rotation;
    cv::Vec3d position;
    cv::solvePnP(rect_corners_3d, rect_corners_2d, intrinsic_parameters, distortion_coefficients, rotation, position);
    

    The position here has the same unit as you used in ii) for defining 3D corners.

    vi) Read your horizontal angle.

    double hangle = rotation[1]; // Y axis <--> horizontal angle
    

    CONCLUSION : The calculated pose is the camera's pose relative to the seen rectangle, not the other way around as you expected. The position and rotation need to be inverted. Note also that camera's Y axis goes downward and that OpenCV uses a right-handed coordinates system. You might want to reconsider the sign of the final angle.