Revision history [back]

Here's some sample code I use to get the Line of Sight vector in the camera coordinates and then transform it by the rvec and tvec.

undistortPoints(ptsIn, ptsOut, cameraMatrix, distortionMatrix, noArray(), cameraMatrix);

double x = ptsOut[0].x;
double y = ptsOut[0].y;

x -= cameraMatrix.at<double>(0,2);
y -= cameraMatrix.at<double>(1,2);

double az = atan2(y, x);
x *= (2 * atan(_size.width / (2 * cameraMatrix.at<double>(0, 0)))) / _size.width;
y *= (2 * atan(_size.height / (2 * cameraMatrix.at<double>(1, 1)))) / _size.height;
double el = sqrt(x*x + y*y);
el = CV_PI / 2.0 - el;

Mat LOS(3,1,CV_64F);
LOS.at<double>(0) = cos(el)*cos(az);
LOS.at<double>(1) = cos(el)*sin(az);
LOS.at<double>(2) = sin(el);

Mat cameraRotation, cameraTranslation;
Rodrigues(rvec, cameraRotation);
cameraRotation = cameraRotation.t();
cameraTranslation = (-cameraRotation * tvec);
LOS = cameraRotation*LOS;

I'm not sure if you needed that last bit from reading your question.

Here's some sample code I use to get the Line of Sight vector in the camera coordinates and then transform it by the rvec and tvec.

//Find the undistorted locations of the point(s)
undistortPoints(ptsIn, ptsOut, cameraMatrix, distortionMatrix, noArray(), cameraMatrix);

double x = ptsOut[0].x;
double y = ptsOut[0].y;

//Find the location relative to the principal point
x -= cameraMatrix.at<double>(0,2);
y -= cameraMatrix.at<double>(1,2);

//Find the azimuth in radians in camera coordinates
double az = atan2(y, x);
//Turn the pixel values into angles by the formula for FOV.
x *= (2 * atan(_size.width / (2 * cameraMatrix.at<double>(0, 0)))) / _size.width;
y *= (2 * atan(_size.height / (2 * cameraMatrix.at<double>(1, 1)))) / _size.height;
//Elevation is perfectly 90 at the principal point
//and the angle from the principal point is the combination of x and y
double el = sqrt(x*x + y*y);
el = CV_PI / 2.0 - el;

//Find the unit 3D vector Line of Sight that matches the azimuth and elevation
Mat LOS(3,1,CV_64F);
LOS.at<double>(0) = cos(el)*cos(az);
LOS.at<double>(1) = cos(el)*sin(az);
LOS.at<double>(2) = sin(el);

//Transform the LOS from camera coordinates to world coordinates.
//rvec and tvec are the same as the output of calibrateCamera of solvePnP
Mat cameraRotation, cameraTranslation;
Rodrigues(rvec, cameraRotation);
cameraRotation = cameraRotation.t();
cameraTranslation = (-cameraRotation * tvec);
LOS = cameraRotation*LOS;

I'm not sure if you needed that last bit from reading your question.