William william89413
shubh-agrawal
/ rotationMat2Quaternion.cpp
Last active
October 25, 2023 13:59
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…
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| 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); |