Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
OpenCV lacks a conversion function from rotation matrix to quaternion. In ROS applications, the rotation of a robot or link is generally described by 4 quaternion numbers. Some people might say that you have "tf" for all such transformations, but it requires your data to be of specific datatype. Here is a small function, that converts opencv Mat…
void getQuaternion(Mat R, double Q[])
{
double trace = R.at<double>(0,0) + R.at<double>(1,1) + R.at<double>(2,2);
if (trace > 0.0)
{
double s = sqrt(trace + 1.0);
Q[3] = (s * 0.5);
s = 0.5 / s;
Q[0] = ((R.at<double>(2,1) - R.at<double>(1,2)) * s);
Q[1] = ((R.at<double>(0,2) - R.at<double>(2,0)) * s);
Q[2] = ((R.at<double>(1,0) - R.at<double>(0,1)) * s);
}
else
{
int i = R.at<double>(0,0) < R.at<double>(1,1) ? (R.at<double>(1,1) < R.at<double>(2,2) ? 2 : 1) : (R.at<double>(0,0) < R.at<double>(2,2) ? 2 : 0);
int j = (i + 1) % 3;
int k = (i + 2) % 3;
double s = sqrt(R.at<double>(i, i) - R.at<double>(j,j) - R.at<double>(k,k) + 1.0);
Q[i] = s * 0.5;
s = 0.5 / s;
Q[3] = (R.at<double>(k,j) - R.at<double>(j,k)) * s;
Q[j] = (R.at<double>(j,i) + R.at<double>(i,j)) * s;
Q[k] = (R.at<double>(k,i) + R.at<double>(i,k)) * s;
}
}
@rkachach

This comment has been minimized.

Copy link

commented Oct 19, 2018

Thanks for sharing your code :)

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.