I am getting metadata from a UAV along with images from an IR camera. The metadata contains pitch, roll, yaw rotation parameters and the UAV's gps location and altitude. Due to the UAV rotations the camera is not viewing constantly to the nadir.
My goal is to acquire orthorectified images using the metadata and approximating a planar surface of the ground.
I came across a previous thread (Opencv virtually camera rotating/translating for bird's eye view) that explains many gray areas about image transformations. However I still face some issues regarding the orthographic projection of the image to the ground's surface.
I am using Opencv's function s*m' = A [R|t] M' in order to get from image coordinates to world coordinates. Matrices A, R, t are known. Given pixel coordinates (x,y) I want to get the 3D surface coordinates in meters. After I get the coordinates of the four points that define my image to the ground I will use them to get the Homography and warp my image in an bird's eye view.
My code is following:
int RefEllipsoid = 23;
double UTMNorthing;
double UTMEasting;
char UTMZone[4];
LLtoUTM(RefEllipsoid, m_uavLat, m_uavLon, UTMNorthing, UTMEasting, UTMZone); ////Get UTMNNorthing, UTMEasting in meters
std::cout << UTMNorthing << "\t" << UTMEasting << "\n";
cv::Mat source= frame.clone();
cv::Mat destination;
double f = 500, dist = m_uavAlt;
double alpha, beta, gamma;
alpha = (m_yaw)*(3.1416/180);
beta = (m_pitch)*(3.1416/180);
gamma = (m_roll)*(3.1416/180);
cv::Size taille = source.size();
double w = (double)taille.width, h = (double)taille.height;
// Rotation matrices around the X,Y,Z axis
///Yaw
cv::Mat RZ = (cv::Mat_<double>(3, 3) <<
cos(alpha), -sin(alpha), 0,
sin(alpha), cos(alpha), 0,
0, 0, 1);
///Pitch
cv::Mat RY = (cv::Mat_<double>(3, 3) <<
cos(beta), 0, sin(beta),
0, 1, 0,
-sin(beta), 0, cos(beta));
///Roll
cv::Mat RX = (cv::Mat_<double>(3, 3) <<
1, 0, 0,
0, cos(gamma), -sin(gamma),
0, sin(gamma), cos(gamma));
// Composed rotation matrix with (RX,RY,RZ)
cv::Mat R = RZ * RY * RX;
///translation vector
cv::Mat tvec = cv::Mat::zeros(3,1,cv::DataType<double>::type);
tvec.at<double>(0,0) = UTMEasting;
tvec.at<double>(1,0) = UTMNorthing;
tvec.at<double>(2,0) = dist;
// Camera Intrisecs matrix 3D -> 2D
cv::Mat A2 = (cv::Mat_<double>(3,3) <<
f, 0, w/2,
0, f, h/2,
0, 0, 1);
cv::Point2f src_vertices[4];
src_vertices[0] = cv::Point2f(0,0);
src_vertices[1] = cv::Point2f(w,0);
src_vertices[2] = cv::Point2f(0,h);
src_vertices[3] = cv::Point2f(w,h);
std::vector<cv::Point2f> ptV;
for(int i=0; i<4; i++) ptV.push_back(src_vertices[i]);
cv::Mat tempMat = (cv::Mat_<double>(3, 4) <<
R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), tvec.at<double>(0,0),
R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), tvec.at<double>(1,0),
R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), tvec.at<double>(2,0));
cv::Point2f dst_vertices[4];
for(int i=0; i<4; i++)
{
cv::Mat uvPoint = cv::Mat::zeros(3,1,cv::DataType<double>::type);
uvPoint.at<double>(0,0) = ptV.at(i).x;
uvPoint.at<double>(1,0) = ptV.at(i).y;
uvPoint.at<double>(2,0) = -f;
cv::Mat P = (A2*tempMat).inv(cv::DECOMP_SVD);
cv::Mat out = P*uvPoint;
dst_vertices[i].x = out.at<double>(0,0);
dst_vertices[i].y = out.at<double>(1,0);
}
cv::Mat warpMatrix = getPerspectiveTransform(src_vertices, dst_vertices);
cv::Mat rotated;
cv::warpPerspective(source, rotated, warpMatrix, rotated.size(), cv::INTER_LINEAR, cv::BORDER_CONSTANT);
cv::imshow("final", rotated);
cv::waitKey(0);
Unfortunately the points that I am getting are far from what I expect and they seem pretty close to each other and far from the UAV position (expressed in UTM coordinates). for example for UAV altitude of 400m I am getting all the image positions at around (225,-37). Can anyone help me with that, do you have any suggestions of what I am doing wrong?
