What is the expected rotation direction when using glm's rotate function? - c++

I'm writing a static function that uses GLM's rotate() function to rotate a vector about an arbitrary axis.
I wrote a simple test to check my work, and I found that the rotations occur in the opposite direction than what I expected.
I rotate a unit vector (0,0,1) about the X axis (1,0,0) in steps of pi/4. I expected that since OpenGL (and GLM?) use a right-handed coordinate system, the rotations would occur in a counter-clockwise direction about the X axis. Instead, they're occurring in clockwise direction.
vec3& RotateVector(vec3& targetVector, float const& radians, vec3 const &axis)
{
mat4 rotation = glm::rotate(mat4(1.0f), radians, axis);
targetVector = (vec4(targetVector, 0.0f) * rotation).xyz();
return targetVector;
}
vec3 test(0, 0, 1);
cout << "test initial vals: " << test.x << " " << test.y << " " << test.z << "\n";
RotateVector(test, 3.14f / 4.0f, vec3(1, 0, 0) );
cout << "Rotated test: " << test.x << " " << test.y << " " << test.z << "\n";
RotateVector(test, 3.14 /4.0f, vec3(1, 0, 0));
cout << "Rotated test: " << test.x << " " << test.y << " " << test.z << "\n";
RotateVector(test, 3.14 / 4.0f, vec3(1, 0, 0));
cout << "Rotated test: " << test.x << " " << test.y << " " << test.z << "\n";
RotateVector(test, 3.14 / 4.0f, vec3(1, 0, 0));
cout << "Rotated test: " << test.x << " " << test.y << " " << test.z << "\n";
When I run the code above, I get the following output:
test initial vals: 0 0 1
Rotated test: 0 0.706825 0.707388
Rotated test: 0 1 0.000796229
Rotated test: 0 0.707951 -0.706262
Rotated test: 0 0.00159246 -0.999999
The output shows that the rotation is moving clockwise about the X axis.
Why is that? I'd expect that OpenGL's right handed coordinate system would adhere to the right hand rule? Am I missing something, or am I just confused?

You're using the matrices transposed, and since rotation matrices are orthogonal matrices, this has the effect of using the inverse of those matrices: R^-1 = R^T.
glm mimics classic GL conventions with mat4 * vec4 multiplication order, where vec4 is a column vector.
When you write vec4 * mat4, vec4 is interpreted as row vector, and since (A*B)^T = B^T * A^T, you get the same result as transpose(mat4) * vec4.

Related

Aruco marker : get orientation and position after pose estimation

I want to know the real position and orientation of my marker.
If I understood well, tvecs gives me the real position of my marker, I measure the distance with a ruler and it seems be correct even I had sometimes weird values.
If someone has an idea of why ?
Unfortunately, rvecs gives me strange value too and it's hard to measure. I hold my marker in front of the camera so the angle is approximately at 0° so rvecs needs to return [0, 0, 0] and if i turn my marker of 45° in y, i need to have [0, 45, 0] etc.
Rvecs doesn't return the orientation of the marker ?
std::vector<int> ids;
std::vector<std::vector<cv::Point2f>> corners;
//Detection of markers
cv::aruco::detectMarkers(image, dictionary, corners, ids);
char key = (char) cv::waitKey(waitTime);
// if at least one marker detected
if (ids.size() > 0)
{
//std::cout << "FOUND " << std::endl;
cv::aruco::drawDetectedMarkers(imageCopy, corners, ids);
std::vector<cv::Vec3d> rvecs, tvecs;
cv::aruco::estimatePoseSingleMarkers(corners, 0.01, cameraMatrix, distCoeffs, rvecs, tvecs);
// draw axis for each marker
for(size_t i=0; i<ids.size(); i++)
{
cv::aruco::drawAxis(imageCopy, cameraMatrix, distCoeffs, rvecs[i], tvecs[i], 0.1);
if(key == 65)
{
std::cout << i << std::endl;
std::cout << rvecs[i] << std::endl;
std::cout << tvecs[i] << std::endl;
std::cout << tvecs[i]*rvecs[i] << std::endl;
double distance = sqrt( (tvecs[i][0] * tvecs[i][0]) + (tvecs[i][1] * tvecs[i][1]) + (tvecs[i][2] * tvecs[i][2]) );
std::cout << distance << std::endl;
std::cout << rvecs[i][0] * 100 << " /// " << rvecs[i][1] * 100 << " /// " << rvecs[i][2] << std::endl;
fileOut << rvecs[i][0] << "," << rvecs[i][1] << "," << rvecs[i][2] << std::endl;
}
}
}
Here my code, but I don't think I have done something wrong.
Maybe I should multiply the Rvecs with something ? But for tvecs I don't need to multiply, I get directly the values

Transforming 2D image coordinates to 3D world coordinates with z = 0

OpenCV => 3.2
Operating System / Platform => Windows 64 Bit
Compiler => Visual Studio 2015
I am currently working on my project which involves vehicle detection and tracking and estimating and optimizing a cuboid around the vehicle. For that I have accomplished detection and tracking of vehicles and I need to find the 3-D world coordinates of the image points of the edges of the bounding boxes of the vehicles and then estimate the world coordinates of the edges of the cuboid and the project it back to the image to display it.
So, I am new to computer vision and OpenCV, but in my knowledge, I just need 4 points on the image and need to know the world coordinates of those 4 points and use solvePNP in OpenCV to get the rotation and translation vectors (I already have the camera matrix and distortion coefficients). Then, I need to use Rodrigues to transform the rotation vector into a rotation matrix and then concatenate it with the translation vector to get my extrinsic matrix and then multiply the extrinsic matrix with the camera matrix to get my projection matrix. Since my z coordinate is zero, so I need to take off the third column from the projection matrix which gives the homography matrix for converting the 2D image points to 3D world points. Now, I find the inverse of the homography matrix which gives me the homography between the 3D world points to 2D image points. After that I multiply the image points [x, y, 1]t with the inverse homography matrix to get [wX, wY, w]t and the divide the entire vector by the scalar w to get [X, Y, 1] which gives me the X and Y values of the world coordinates.
My code looks like this:
#include "opencv2/opencv.hpp"
#include <stdio.h>
#include <iostream>
#include <sstream>
#include <math.h>
#include <conio.h>
using namespace cv;
using namespace std;
Mat cameraMatrix, distCoeffs, rotationVector, rotationMatrix,
translationVector,extrinsicMatrix, projectionMatrix, homographyMatrix,
inverseHomographyMatrix;
Point point;
vector<Point2d> image_points;
vector<Point3d> world_points;
int main()
{
FileStorage fs1("intrinsics.yml", FileStorage::READ);
fs1["camera_matrix"] >> cameraMatrix;
cout << "Camera Matrix: " << cameraMatrix << endl << endl;
fs1["distortion_coefficients"] >> distCoeffs;
cout << "Distortion Coefficients: " << distCoeffs << endl << endl;
image_points.push_back(Point2d(275, 204));
image_points.push_back(Point2d(331, 204));
image_points.push_back(Point2d(331, 308));
image_points.push_back(Point2d(275, 308));
cout << "Image Points: " << image_points << endl << endl;
world_points.push_back(Point3d(0.0, 0.0, 0.0));
world_points.push_back(Point3d(1.775, 0.0, 0.0));
world_points.push_back(Point3d(1.775, 4.620, 0.0));
world_points.push_back(Point3d(0.0, 4.620, 0.0));
cout << "World Points: " << world_points << endl << endl;
solvePnP(world_points, image_points, cameraMatrix, distCoeffs, rotationVector, translationVector);
cout << "Rotation Vector: " << endl << rotationVector << endl << endl;
cout << "Translation Vector: " << endl << translationVector << endl << endl;
Rodrigues(rotationVector, rotationMatrix);
cout << "Rotation Matrix: " << endl << rotationMatrix << endl << endl;
hconcat(rotationMatrix, translationVector, extrinsicMatrix);
cout << "Extrinsic Matrix: " << endl << extrinsicMatrix << endl << endl;
projectionMatrix = cameraMatrix * extrinsicMatrix;
cout << "Projection Matrix: " << endl << projectionMatrix << endl << endl;
double p11 = projectionMatrix.at<double>(0, 0),
p12 = projectionMatrix.at<double>(0, 1),
p14 = projectionMatrix.at<double>(0, 3),
p21 = projectionMatrix.at<double>(1, 0),
p22 = projectionMatrix.at<double>(1, 1),
p24 = projectionMatrix.at<double>(1, 3),
p31 = projectionMatrix.at<double>(2, 0),
p32 = projectionMatrix.at<double>(2, 1),
p34 = projectionMatrix.at<double>(2, 3);
homographyMatrix = (Mat_<double>(3, 3) << p11, p12, p14, p21, p22, p24, p31, p32, p34);
cout << "Homography Matrix: " << endl << homographyMatrix << endl << endl;
inverseHomographyMatrix = homographyMatrix.inv();
cout << "Inverse Homography Matrix: " << endl << inverseHomographyMatrix << endl << endl;
Mat point2D = (Mat_<double>(3, 1) << image_points[0].x, image_points[0].y, 1);
cout << "First Image Point" << point2D << endl << endl;
Mat point3Dw = inverseHomographyMatrix*point2D;
cout << "Point 3D-W : " << point3Dw << endl << endl;
double w = point3Dw.at<double>(2, 0);
cout << "W: " << w << endl << endl;
Mat matPoint3D;
divide(w, point3Dw, matPoint3D);
cout << "Point 3D: " << matPoint3D << endl << endl;
_getch();
return 0;
I have got the image coordinates of the four known world points and hard-coded it for simplification. image_points contain the image coordinates of the four points and world_points contain the world coordinates of the four points. I am considering the the first world point as the origin (0, 0, 0) in the world axis and using known distance calculating the coordinates of the other four points. Now after calculating the inverse homography matrix, I multiplied it with [image_points[0].x, image_points[0].y, 1]t which is related to the world coordinate (0, 0, 0). Then I divide the result by the third component w to get [X, Y, 1]. But after printing out the values of X and Y, it turns out they are not 0, 0 respectively. What am doing wrong?
The output of my code is like this:
Camera Matrix: [517.0036881709533, 0, 320;
0, 517.0036881709533, 212;
0, 0, 1]
Distortion Coefficients: [0.1128663679798094;
-1.487790079922432;
0;
0;
2.300571896761067]
Image Points: [275, 204;
331, 204;
331, 308;
275, 308]
World Points: [0, 0, 0;
1.775, 0, 0;
1.775, 4.62, 0;
0, 4.62, 0]
Rotation Vector:
[0.661476468596541;
-0.02794460022559267;
0.01206996342819649]
Translation Vector:
[-1.394495345140898;
-0.2454153722672731;
15.47126945512652]
Rotation Matrix:
[0.9995533907649279, -0.02011656447351923, -0.02209848058392758;
0.002297501163799448, 0.7890323093017149, -0.6143474069013439;
0.02979497438726573, 0.6140222623910194, 0.7887261380159]
Extrinsic Matrix:
[0.9995533907649279, -0.02011656447351923, -0.02209848058392758,
-1.394495345140898;
0.002297501163799448, 0.7890323093017149, -0.6143474069013439,
-0.2454153722672731;
0.02979497438726573, 0.6140222623910194, 0.7887261380159,
15.47126945512652]
Projection Matrix:
[526.3071813531748, 186.086785938988, 240.9673682002232, 4229.846989065414;
7.504351145361707, 538.1053336219271, -150.4099339268854, 3153.028471890794;
0.02979497438726573, 0.6140222623910194, 0.7887261380159, 15.47126945512652]
Homography Matrix:
[526.3071813531748, 186.086785938988, 4229.846989065414;
7.504351145361707, 538.1053336219271, 3153.028471890794;
0.02979497438726573, 0.6140222623910194, 15.47126945512652]
Inverse Homography Matrix:
[0.001930136511648154, -8.512427241879318e-05, -0.5103513244724983;
-6.693679705844383e-06, 0.00242178892313387, -0.4917279870709287
-3.451449134581896e-06, -9.595179260534558e-05, 0.08513443835773901]
First Image Point[275;
204;
1]
Point 3D-W : [0.003070864657310213;
0.0004761913292736786;
0.06461112415423849]
W: 0.0646111
Point 3D: [21.04004290792539;
135.683117651025;
1]
Your reasoning is sound, but you are making some mistake in the last division.. or am I missing something?
Your result before W division is:
Point 3D-W :
[0.003070864657310213;
0.0004761913292736786;
0.06461112415423849]
Now we need to normalize this by dividing all the coordinates by W (the 3rd element of the array), as you described in your question. so:
Point 3D-W Normalized =
[0.003070864657310213 / 0.06461112415423849;
0.0004761913292736786 / 0.06461112415423849;
0.06461112415423849 / 0.06461112415423849]
Which results in:
Point 3D-W Normalized =
[0.047528420183179314;
0.007370113668614144;
1.0]
Which is damn close to [0,0].

Incorrect Polar - Cartesian Coordinate Conversions. What does -0 Mean?

I am getting incorrect conversions from polar to cartesian coordinates and vice versa. My code produces weird points like (1,-0). Im using this calculator to check my conversions. Also one of the conversions is completely wrong when I convert back to cartesian coordinates.
Point b: (0,1) => (1,1.5708) => (0,0)
#include <math.h>
#include <iostream>
/* Title: Polar - Cartesian Coordinate Conversion
* References: HackerRank > All Domains > Mathematics > Geometry > Polar Angles
* Cartesian to Polar: (radius = sqrt(x^2 + y^2), theta = atan(y/x))
* Polar to Cartesian: (x = radius*cos(theta), y = radius*sin(theta))
*/
//General 2D coordinate pair
struct point{
point(float a_val, float b_val) : a(a_val), b(b_val){;};
point(void){;};
float a, b;
};
//Converts 2D Cartesian coordinates to 2D Polar coordinates
point to_polar(/*const*/ point& p){//*** Conversion of origin result in (r, -nan) ***
point ans(sqrt(pow(p.a,2) + pow(p.b,2)), atan(p.b/p.a));
return ans;
}
//Converts 2D Polar coordinates to 2D Cartesian coordinates
point to_cartesian(/*const*/ point& p){
point ans(p.a * cos(p.b), p.a * sin(p.b));
return ans;
}
//Outputs 2D coordinate pair
std::ostream& operator<<(std::ostream& stream, const point& p){
stream << "(" << p.a << "," << p.b << ")";
return stream;
}
int main(){
//Test Points - Cartesian
point a(0, 0);
point b(0, 1);
point c(1, 0);
point d(0,-1);
point e(-1,0);
//Print Cartesian/Rectangular points
std::cout << "Cartesian Coordinates:" << std::endl;
std::cout << a << std::endl;
std::cout << b << std::endl;
std::cout << c << std::endl;
std::cout << d << std::endl;
std::cout << e << std::endl;
//Print Cartesian to Polar
std::cout << "Polar Coordinates:" << std::endl;
std::cout << to_polar(a) << std::endl;//Failure (0,-nan)
std::cout << to_polar(b) << std::endl;//Success
std::cout << to_polar(c) << std::endl;//Success
std::cout << to_polar(d) << std::endl;//Success
std::cout << to_polar(e) << std::endl;//Failure (1,-0)
//Print Polar to Cartesian
std::cout << "Cartesian Coordinates:" << std::endl;
std::cout << to_cartesian(a) << std::endl;//Success
std::cout << to_cartesian(b) << std::endl;//Failure (0,0)
std::cout << to_cartesian(c) << std::endl;//Success
std::cout << to_cartesian(d) << std::endl;//Failure (0,-0)
std::cout << to_cartesian(e) << std::endl;//Failure (-1,-0)
return 0;
}
You are converting to cartesian the points which are in cartesian already. What you want is:
std::cout << "Cartesian Coordinates:" << std::endl;
std::cout << to_cartesian(to_polar(a)) << std::endl;
std::cout << to_cartesian(to_polar(b)) << std::endl;
//...
Edit: using atan2 solves the NaN problem, (0, 0) is converted to (0, 0) which is fine.
As a first step, you need to switch to atan2 instead of atan in your conversion to polar coordinates. atan gives wrong results for half the plane.

Rotating a vector around an axis with quaternion

I am trying to learn 3d programming, and right now I am trying to understand how to use quaternions to rotate a vector around an axis.
As far as I understand, to rotate a vector v around an axis a, after converting both vectors to quaternions, we multiply v by a, then the product by the conjugate of a.
I want to rotate v(0,1,0) around a(1,0,0) by 90 degrees, and I should get a resulting vector v(0,0,1) (or 0,0,-1, depending on the direction of the rotation).
I am not getting the output I am expecting.
Here is the code:
int main()
{
//I want to rotate this vector about the x axis by PI/2 radians:
Quaternion v(0, 1, 0, 0);
v.normalize();
float angle = PI / 2.0f;
float cos = math::cos(angle / 2.0f);
float sin = math::sin(angle / 2.0f);
Quaternion q(1.0f*sin, 0.0f*sin, 0.0f*sin, cos);
std::cout << "q not normalized = " <<"\t"<< q.x << " " << q.y << " " << q.z << " " << q.w << std::endl;
q.normalize();
std::cout << "q normalized = " <<"\t\t"<< q.x << " " << q.y << " " << q.z << " " << q.w << std::endl;
std::cout << std::endl;
Quaternion r;
//I multiply the vector v by the quaternion v, then I multiply by the conjugate.
r = q * v;
//do I need to normalize here?
r = r * q.conjugate();
//and here?
//shouldn't the resulting vector be 0,0,1?
std::cout << "r not normalized = " << "\t" << r.x << " " << r.y << " " << r.z << " " << r.w << std::endl;
r.normalize();
std::cout << "r normalized = " << "\t\t" << r.x << " " << r.y << " " << r.z << " " << r.w << std::endl;
std::cout << std::endl;
system("pause");
return 0;
}
and here is the output:
q not normalized, which is same as q normalized:
x = 0.707107, y = 0, z = 0, w = 0.707107
r not normalized:
x = 0.707107, y = 0, z = 1, w = -2.12132
r normalized:
x = 0.288675, y = 0, z = 0.408248, w = -0.866025
what am I doing wrong?
did I even understand anything from this process?
Basically to rotate an vector along x axis (1,0,0) with angle 90 deg, use below method, this works for both Euler and quaternion
| 1 0 0 | | 0 | | 0 |
| 0 cos90 -sin90 | * | 1 | = | 0 |
| 0 sin90 cos90 | | 0 | | 1 |
Read about rotation matrices http://en.wikipedia.org/wiki/Rotation_matrix

Ray picking direction not calculating correctly

I have the following function code:
D3DXVECTOR3 Mouse::GetClickDirection(D3DXMATRIX projMatrix, D3DXMATRIX viewMatrix, D3DXVECTOR3 cameraPosition)
{
POINT mousePoint;
GetCursorPos(&mousePoint);
ScreenToClient(hwnd, &mousePoint);
float x = ((mousePoint.x * 2.0f) / (float)backBufferWidth) - 1.0f;
float y = ((mousePoint.y * -2.0f) / (float)backBufferHeight) + 1.0f;
D3DXVECTOR3 mouseClip = D3DXVECTOR3(x, y, 1.0f);
D3DXMATRIX invViewMatrix;
D3DXMatrixInverse(&invViewMatrix, 0, &viewMatrix);
D3DXMATRIX invProjMatrix;
D3DXMatrixInverse(&invProjMatrix, 0, &projMatrix);
D3DXMATRIX inversedMatrix = invViewMatrix * invProjMatrix;
D3DXVECTOR3 mouseWorldSpace;
D3DXVec3TransformCoord(&mouseWorldSpace, &mouseClip, &inversedMatrix);
D3DXVECTOR3 direction = mouseWorldSpace - cameraPosition;
D3DXVec3Normalize(&direction, &direction);
system("CLS");
std::cout << "sX: " << x << std::endl;
std::cout << "sY: " << y << std::endl;
std::cout << "X: " << direction.x << std::endl;
std::cout << "Y: " << direction.y << std::endl;
std::cout << "Z: " << direction.z << std::endl;
return direction;
}
I'm trying to calculate and create a directional ray based on the x and y screen point that the user has clicked on in my DirectX 3D app. So far, the printed results seems to indicate that my calculations are wrong as the Z value is always around 0.9 and I have no idea why. What exactly am I doing wrong here? Please help. Thanks
I think your inverse matrix calculation is backwards.
D3DXMATRIX inversedMatrix = invProjMatrix * invViewMatrix;