I am working on a project for estimating a UAV (quadcopter) location using optical-flow technique. I currently have a code that is using farneback algorithm from OpenCV. The current code is working fine when the camera is always pointing to the ground.
Now, I want to add support to the case when the camera is not pointing straight down - meaning that the quadcopter now has a pitch / roll / yaw (Euler angles). The quadcopters Euler angles are known and I am searching for a method to compute and apply the transformation needed based on the known current Euler angles. So that the result image will be as if it was taken from above (see image below).
I found methods that calculates the transformation when having 2 sets (source and destination) of 4 corners via findHomography or getPerspectiveTransform functions from OpenCV. But I couldn't find any methods that can do it with knowing only Euler angle (because I don't know the destination image corenrs).
So my question is what method can I use and how in order to transform a frame to be as if it was taken from above using only Euler angles and camera height from ground if necessary?
In order to demonstrate what I need:
The relevant part of my current code is below:
for(;;)
{
Mat m, disp, warp;
vector<Point2f> corners;
// take out frame- still distorted
cap >> origFrame;
// undistort the frame using the calibration parameters
cv::undistort(origFrame, undistortFrame, cameraMatrix, distCoeffs, noArray());
// lower the process effort by transforming the picture to gray
cvtColor(undistortFrame, gray, COLOR_BGR2GRAY);
if( !prevgray.empty() )
{
// calculate flow
calcOpticalFlowFarneback(prevgray, gray, uflow, 0.5, 3/*def 3 */, 10/* def 15*/, 3, 3, 1.2 /* def 1.2*/, 0);
uflow.copyTo(flow);
// get average
calcAvgOpticalFlow(flow, 16, corners);
// calculate range of view - 2*tan(fov/2)*distance
rovX = 2*0.44523*distanceSonar*100; // 2 * tan(48/2) * dist(cm)
rovY = 2*0.32492*distanceSonar*100; // 2 * tan(36/2) * dist(cm)
// calculate final x, y location
location[0] += (currLocation.x/WIDTH_RES)*rovX;
location[1] += (currLocation.y/HEIGHT_RES)*rovY;
}
//break conditions
if(waitKey(1)>=0)
break;
if(end_run)
break;
std::swap(prevgray, gray);
}
UPDATE:
After successfully adding the rotation, I still need my image to be centered (and not to go outside of the frame window as shown below). I guess I need some kind of translation. I want the center of the source image to be at the center of the destination image. How can I add this as well?
The rotation function that works:
void rotateFrame(const Mat &input, Mat &output, Mat &A , double roll, double pitch, double yaw){
Mat Rx = (Mat_<double>(3, 3) <<
1, 0, 0,
0, cos(roll), -sin(roll),
0, sin(roll), cos(roll));
Mat Ry = (Mat_<double>(3, 3) <<
cos(pitch), 0, sin(pitch),
0, 1, 0,
-sin(pitch), 0, cos(pitch));
Mat Rz = (Mat_<double>(3, 3) <<
cos(yaw), -sin(yaw), 0,
sin(yaw), cos(yaw), 0,
0, 0, 1);
Mat R = Rx*Ry*Rz;
Mat trans = A*R*A.inv();
warpPerspective(input, output, trans, input.size());
}
When I run it with rotateFrame(origFrame, processedFrame, cameraMatrix, 0, 0, 0); I get image as expected:
But when I run it with 10 degrees in roll rotateFrame(origFrame, processedFrame, cameraMatrix, 20*(M_PI/180), 0, 0);. The image is getting out of the frame window:
If you have a calibration intrinsics matrix A (3x3), and there is no translation between camara poses, all you need to find homography H (3x3) is to construct rotation matrix R (3x3) from euler angles and apply the following formula:
H = A * R * A.inv()
Where .inv() is matrix invertion.
UPDATED:
If you want to center the image, you should just add translation this way:
(this is finding the warped position of the center and translation of this point back to the center)
|dx| | 320 / 2 |
|dy| = H * | 240 / 2 |
|1 | | 1 |
| 1 0 (320/2-dx) |
W = | 0 1 (240/2-dy) | * H
| 0 0 1 |
W is your final transformation.
I came to a conclusion that I had to use the 4x4 Homography matrix in order to be able to get what I wanted. In order to find the right homography matrix we need:
3D Rotation matrix R.
Camera calibration intrinsic matrix A1 and its inverted matrix A2.
Translation matrix T.
We can compose the 3D rotation matrix R by multiplying the rotation matrices around axes X,Y,Z:
Mat R = RZ * RY * RX
In order to apply the transformation on the image and keep it centered we need to add translation given by a 4x4 matrix, where dx=0; dy=0; dz=1 :
Mat T = (Mat_<double>(4, 4) <<
1, 0, 0, dx,
0, 1, 0, dy,
0, 0, 1, dz,
0, 0, 0, 1);
Given all these matrices we can compose our homography matrix H:
Mat H = A2 * (T * (R * A1))
With this homography matrix we can then use warpPerspective function from OpenCV to apply the transformation.
warpPerspective(input, output, H, input.size(), INTER_LANCZOS4);
For conclusion and completeness of this solution here is the full code:
void rotateImage(const Mat &input, UMat &output, double roll, double pitch, double yaw,
double dx, double dy, double dz, double f, double cx, double cy)
{
// Camera Calibration Intrinsics Matrix
Mat A2 = (Mat_<double>(3,4) <<
f, 0, cx, 0,
0, f, cy, 0,
0, 0, 1, 0);
// Inverted Camera Calibration Intrinsics Matrix
Mat A1 = (Mat_<double>(4,3) <<
1/f, 0, -cx/f,
0, 1/f, -cy/f,
0, 0, 0,
0, 0, 1);
// Rotation matrices around the X, Y, and Z axis
Mat RX = (Mat_<double>(4, 4) <<
1, 0, 0, 0,
0, cos(roll), -sin(roll), 0,
0, sin(roll), cos(roll), 0,
0, 0, 0, 1);
Mat RY = (Mat_<double>(4, 4) <<
cos(pitch), 0, sin(pitch), 0,
0, 1, 0, 0,
-sin(pitch), 0, cos(pitch), 0,
0, 0, 0, 1);
Mat RZ = (Mat_<double>(4, 4) <<
cos(yaw), -sin(yaw), 0, 0,
sin(yaw), cos(yaw), 0, 0,
0, 0, 1, 0,
0, 0, 0, 1);
// Translation matrix
Mat T = (Mat_<double>(4, 4) <<
1, 0, 0, dx,
0, 1, 0, dy,
0, 0, 1, dz,
0, 0, 0, 1);
// Compose rotation matrix with (RX, RY, RZ)
Mat R = RZ * RY * RX;
// Final transformation matrix
Mat H = A2 * (T * (R * A1));
// Apply matrix transformation
warpPerspective(input, output, H, input.size(), INTER_LANCZOS4);
}
Result:
This how I do it in Eigen and using 4 corners:
// Desired four corners
std::vector<Eigen::Vector2f> Normalized_Reference_Pattern = { Eigen::Vector2f(0, 0), Eigen::Vector2f(0, 2), Eigen::Vector2f(2, 0), Eigen::Vector2f(2, 2) };
// Current four points
std::vector<Eigen::Vector2f> CurrentCentroids = { /* Whatever four corners you want, but relative sueqnece to above */ };
// Transform for current to desired
auto Master_Transform = get_perspective_transform(CurrentCentroids, Normalized_Reference_Pattern);
// abilitu to use the same tranformation for other points (other than the corners) in the image
Eigen::Vector2f Master_Transform_Centroid = Master_Transform * Eigen::Vector2f(currentX, currentY);
And here is my black box:
Eigen::Matrix3f get_perspective_transform(const std::vector<Eigen::Vector2f>& points_from,const std::vector<Eigen::Vector2f>& points_to)
{
cv::Mat transform_cv = cv::getPerspectiveTransform(
convert::to_cv(points_from),
convert::to_cv(points_to));
Eigen::Matrix3f transform_eigen;
cv::cv2eigen(transform_cv, transform_eigen);
return transform_eigen;
}
I have this code that draws a 3D line between two points:
void glLine(Point3D (&i)[2], double const width, int const slices = 360)
{
double const d[3] = { i[1].X - i[0].X, i[1].Y - i[0].Y, i[1].Z - i[0].Z };
glMatrixMode(GL_MODELVIEW);
glPushMatrix();
GLUquadric *const quadric = gluNewQuadric()
double z[3] = { 0, 0, 1 };
double const angle = acos(dot(z, d) / sqrt(dot(d, d) * dot(z, z)));
cross(z, d);
glTranslated(i[0].X, i[0].Y, i[0].Z);
glRotated(angle * 180 / M_PI, z[0], z[1], z[2]);
gluCylinder(quadric, width / 2, width / 2, sqrt(dot(d, d)), slices, 1);
glPopMatrix();
gluDeleteQuadric(quadric);
}
The trouble is, it's extremely slow because of the math that computes the rotation from the unit z vector to the given direction.
How can I make OpenGL (hopefully, the GPU) perform the arithmetic instead of the CPU?
Do not render lines using cylinders. Rather render quads using geometry shaders, facing the user with correct screen space scaling.
Have a look here: Wide lines in geometry shader behaves oddly
There is a function GLRotation
inline const mat4 GLRotation(float x, float y, float z)
{
const float cx = cosf(x * math_radians), sx = sinf(x * math_radians),
cy = cosf(y * math_radians), sy = sinf(y * math_radians),
cz = cosf(z * math_radians), sz = sinf(z * math_radians);
// rotationX * rotationY * rotationZ
return mat4(cy * cz, -cy * sz, sy, 0,
cx * sz + sx * cz * sy, cx * cz - sx * sy * sz, -cy * sx, 0,
sx * sz - cx * cz * sy, cz * sx + cx * sy * sz, cx * cy, 0,
0, 0, 0, 1);
}
And use can call it like this GLRotation(v.rotation) where v.rotation - vector(x, y, z).
What function I need to use in glm(library) to get the same result?
you can use the following to get a rotaion using glm, although it requires slightly more input:
glm::rotate(ModelViewProjectionMatrix4x4Here,angleToRotateHere,directionToRotate)
which follows the format layed out in the glm api page
rotate (detail::tmat4x4< T > const &m, T angle, T x, T y, T z)
so you can pass it the matrix you need to operate on, the angle to rotate by and the vector around which to rotate.
glm::rotate(yourMatrixHere, angle_in_degrees, glm::vec3(x, y, z)); // where x, y, z is axis of rotation (e.g. 0 1 0 is rotate around the y-axis)
If that exact function definition does not match what you need there are others on the same web-page which take arguments in varying ways.
Let me know if you need any more information:)
Your function GLRotation() specifies the angle in radians to rotate in each principle axis. On the other hand glm::rotate(angle, axis) specifies a rotation about a provided axis. So, strictly speaking you can define your GLRotation as follows:
inline mat4
GLRotation(float x, float y, float z)
{
return
glm::rotate(x, 1, 0, 0) *
glm::rotate(y, 0, 1, 0) *
glm::rotate(z, 0, 0, 1);
}
Although I wouldn't advise this, because describing rotations in this way can lead to Gimbal lock (when you rotate in one axis in such a way that it becomes aligned with another axis, losing you one degree of freedom).
Better to look into Axis-Angle representations of rotations, which is what glm::rotate uses. They aren't susceptible to gimbal lock, and can represent any 3D rotation about an axis through the origin.
I am trying to write a own rotation function for a camera in OpenGL, but I can't get it to run. My camera is mainly from flipcode, with some minor changes:
Camera code:
Camera::Camera(float x, float y, float z) {
memset(Transform, 0, 16*sizeof(float));
Transform[0] = 1.0f;
Transform[5] = 1.0f;
Transform[10] = 1.0f;
Transform[15] = 1.0f;
Transform[12] = x; Transform[13] = y; Transform[14] = z;
Left=&Transform[0];
Up=&Transform[4];
Forward=&Transform[8];
Position=&Transform[12];
old_x = 0;
old_y = 0;
}
The view is set before every rendered frame:
void Camera::setView() {
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
float viewmatrix[16]={//Remove the three - for non-inverted z-axis
Transform[0], Transform[4], -Transform[8], 0,
Transform[1], Transform[5], -Transform[9], 0,
Transform[2], Transform[6], -Transform[10], 0,
-(Transform[0]*Transform[12] +
Transform[1]*Transform[13] +
Transform[2]*Transform[14]),
-(Transform[4]*Transform[12] +
Transform[5]*Transform[13] +
Transform[6]*Transform[14]),
//add a - like above for non-inverted z-axis
(Transform[8]*Transform[12] +
Transform[9]*Transform[13] +
Transform[10]*Transform[14]), 1};
glLoadMatrixf(viewmatrix);
}
Now to my problem, the rotation. Consider for example rotation around the y-axis. This is the rotation matrix stack:
// deg is the angle it is not working in degree or radiant
void Camera::rotateLocal_y(float deg){
glMatrixMode(GL_MODELVIEW);
glPushMatrix();
glLoadMatrixf(Transform);
rotateMatrixf_y(Transform, deg);
glGetFloatv(GL_MODELVIEW_MATRIX, Transform);
glPopMatrix();
}
So next I am going to show the rotation function:
//rotate a matrix around y axis
void rotateMatrixf_y(float *aMatrix, float angle){
// x y z t
float rotMatrix[] = {cos(angle),0,-1*sin(angle),0, 0, 1, 0, 0, sin(angle), 0, cos(angle), 0, 0, 0, 0, 1};
multMatrixMatrix(rotMatrix,aMatrix);
}
And finally the matrix multiplication function:
void multMatrixMatrix(float* m_a, float* m_b){
float m_c[16] = {m_a[0]*m_b[0]+m_a[4]*m_b[1]+m_a[8]*m_b[2]+m_a[12]*m_b[3],
m_a[0]*m_b[4]+m_a[4]*m_b[5]+m_a[8]*m_b[6]+m_a[12]*m_b[7],
m_a[0]*m_b[8]+m_a[4]*m_b[9]+m_a[8]*m_b[10]+m_a[12]*m_b[11],
m_a[0]*m_b[12]+m_a[4]*m_b[13]+m_a[8]*m_b[14]+m_a[12]*m_b[15],
m_a[1]*m_b[0]+m_a[5]*m_b[1]+m_a[9]*m_b[2]+m_a[13]*m_b[3],
m_a[1]*m_b[4]+m_a[5]*m_b[5]+m_a[9]*m_b[6]+m_a[13]*m_b[7],
m_a[1]*m_b[8]+m_a[5]*m_b[9]+m_a[9]*m_b[10]+m_a[13]*m_b[11],
m_a[1]*m_b[12]+m_a[5]*m_b[13]+m_a[9]*m_b[14]+m_a[13]*m_b[15],
m_a[2]*m_b[0]+m_a[6]*m_b[1]+m_a[10]*m_b[2]+m_a[14]*m_b[3],
m_a[2]*m_b[4]+m_a[6]*m_b[5]+m_a[10]*m_b[6]+m_a[14]*m_b[7],
m_a[2]*m_b[8]+m_a[6]*m_b[9]+m_a[10]*m_b[10]+m_a[14]*m_b[11],
m_a[2]*m_b[12]+m_a[6]*m_b[13]+m_a[10]*m_b[14]+m_a[14]*m_b[15],
m_a[3]*m_b[0]+m_a[7]*m_b[1]+m_a[11]*m_b[2]+m_a[15]*m_b[3],
m_a[3]*m_b[4]+m_a[7]*m_b[5]+m_a[11]*m_b[6]+m_a[15]*m_b[7],
m_a[3]*m_b[8]+m_a[7]*m_b[9]+m_a[11]*m_b[10]+m_a[15]*m_b[11],
m_a[3]*m_b[12]+m_a[7]*m_b[13]+m_a[11]*m_b[14]+m_a[15]*m_b[15]
};
m_b = m_c;
}
I though this must be it, but it seems as if something is fundamentaly wrong. It is not moving at all. the camera is properly set. The method order is: cam.rotate then cam.setView.
Flipcodes originial rotate function:
void Camera::rotateLoc(float deg, float x, float y, float z) {
glMatrixMode(GL_MODELVIEW);
glPushMatrix();
glLoadMatrixf(Transform);
glRotatef(deg, x,y,z);
glGetFloatv(GL_MODELVIEW_MATRIX, Transform);
glPopMatrix();
}
Your code is pretty messy and incomplete.
I think your problem is here :
glPushMatrix();
glLoadMatrixf(Transform); // give the Transform matrix to GL (why?)
rotateMatrixf_y(Transform, deg); // modify the Transform matrix
glGetFloatv(GL_MODELVIEW_MATRIX, Transform); // (3) retrieve the original Tranform matrix
glPopMatrix();
(3) just undoes whatever changes you've been doing in 'Transform' by calling 'rotateMatrixf_y'.
The flipcode code you added is using OpenGL to update the Tranform matrix, by calling glRotatef' and reading back the result, which is fine. In your method code, you should just remove every reference to OpenGL and just keep the call to rotateMatrixf_y, which does all the work in its own.
Do you really understand what's the use of the GL matrix stack ? You should perhaps go back to the basics by either using only GL functions or using your own, but get to know why it works in either way before mixing the uses.