0

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]
Community
  • 1
  • 1
Psifrost
  • 107
  • 7

1 Answers1

2

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.

RCYR
  • 1,452
  • 13
  • 28
  • When you mention Image dimensions and Rectangle dimensions in section ii, what units are those in? Also, do you mean the size of the physical rectangle, or the bounding rectangle object? – Psifrost Feb 19 '16 at 07:48
  • I'm really sorry... I lead you foolishly to an error in ii) regarding definition of 3D corners. I edited my answer. The 3D corners must not be mixed with image pixels, obviously. You choose whatever physical unit you want to use and the same unit will be used in calculated camera's pose for position. – RCYR Feb 19 '16 at 16:14
  • Thanks for the help! In section ii, you state that the rectangle 3D corners are clockwise, yet in the following code you list the corners in the order of: Top-Left,Top-Right,Bottom-Left,Bottom-Right, which does not appear to be clockwise. Which of these are correct, or are they both correct and I'm misunderstanding? – Psifrost Feb 19 '16 at 18:04
  • Another mistake of mine. Good looking of yours! I edited my answer once again to set 3D corners clockwise. – RCYR Feb 19 '16 at 19:03
  • Note: As I suppose you understood, it does not really matter whether corners are laid in clockwise, anti-clockwise or anything else. What is really important is that 2D points order and 3D points order both match. – RCYR Feb 19 '16 at 19:10