Proper way of rotating 3D points around axis [closed]
Hello!
I have a problem with apply rotation to a set of 3D points. I use depth map, which store Z coordinates of points, also I use reverse of camera intrinsic matrix to obtain X and Y coords of point. I need to rotate those 3D points aorund Y axis and compute depth map after rotation. The code I use is here:
for (int a = 0; a < depthValues.rows; ++a)
{
for (int b = 0; b < depthValues.cols; ++b)
{
float oldDepth = depthValues.at<cv::Vec3f>(a, b)[0];
if (oldDepth > EPSILON)
{
cv::Mat pointInWorldSpace = cameraMatrix.inv() * cv::Mat(cv::Vec3f(a, b , 1), false);
pointInWorldSpace *= oldDepth;
cv::Mat rotatedPointInWorldSpace = rotation * pointInWorldSpace;
float newDepth = rotatedPointInWorldSpace.at<cv::Vec3f>(0, 0)[2];
cv::Mat rotatedPointInImageSpace = cameraMatrix * rotatedPointInWorldSpace;
int x = rotatedPointInImageSpace.at<cv::Vec3f>(0, 0)[0] / newDepth;
int y = rotatedPointInImageSpace.at<cv::Vec3f>(0, 0)[1] / newDepth;
x = x < 0 ? 0 : x;
y = y < 0 ? 0 : y;
x = x > depthValues.rows - 1 ? depthValues.rows - 1 : x;
y = y > depthValues.cols - 1 ? depthValues.cols - 1 : y;
depthValuesAfterConversion.at < cv::Vec3f >(x, y) = cv::Vec3f(newDepth, newDepth, newDepth);
}
}
}
Here's how I compute rotation matrix:
float angle = (15.0 * 3.14159265f) / 180.0f;
float rotateYaxis[3][3] =
{
{ cos(angle), 0, -sin(angle) },
{ 0, 1, 0 },
{ sin(angle), 0, cos(angle) }
};
cv::Mat rotation(3, 3, CV_32FC1, rotateYaxis);
Unfortunately, after applying this rotation to my depth map it looks like it's rotated around X axis. I discovered that when I compute rotation matrix as it was rotation around X axis - my code works lke expected.
My question is: could you point me out where I made mistake to my code? Using matrix I've described I expected my depth map to be rotated around Y axis, not X.
Thank you for your help!