#include <iostream>
#include<opencv2\opencv.hpp>
#include<vector>
using namespace std;
using namespace cv;
#define PI 3.1415926535
double standardRad(double t) {
double TWOPI = 2.0 * PI;
if (t >= 0)
{
t = fmod(t + PI, TWOPI) - PI;
}
else
{
t = fmod(t - PI, -TWOPI) + PI;
}
cout << "standardRad(t):" << t << endl;
return t;
}
Mat wRo_to_euler(Mat wRo) {
cout << "wRo: " << wRo << endl;
double yaw = standardRad(atan2(wRo.at<double>(1, 0), wRo.at<double>(0, 0)));
double c = cos(yaw);
double s = sin(yaw);
double pitch = standardRad(atan2(-wRo.at<double>(2, 0),
wRo.at<double>(0, 0)*c + wRo.at<double>(1, 0)*s)) / PI * 180;
double roll = standardRad(atan2(wRo.at<double>(0, 2) * s - wRo.at<double>(1, 2)*c,
-wRo.at<double>(0, 1)*s + wRo.at<double>(1, 1)*c)) / PI * 180;
cout << "pitch: " << pitch << endl;
cout << "roll: " << roll << endl;
double returnWro_arr[3];
returnWro_arr[0] = roll;
returnWro_arr[1] = pitch;
returnWro_arr[2] = yaw / PI * 180;
Mat returnWro = Mat(1, 3, CV_64F, returnWro_arr);
cout << "returnWro: " << returnWro << endl;
/*cout << "returnWro.rows: "<<returnWro.rows << endl;
cout << "returnWro.cols: "<<returnWro.cols << endl;*/
return returnWro; //returnWro的x,y,z坐标
}
int main()
{
/*double h_arr[3][3] = { -0.01806375821938705, 0.9813313999097074, -0.1915334800712387,
-0.9997671236683783, -0.01481447609281841, 0.01505010620015893,
-0.01806375821938705, 0.1917530047987496, 0.9813706844588156 };
Mat h = Mat(3, 3, CV_64F, h_arr);*/
Mat h = (Mat_<double>(3, 3) << -0.01806375821938705, 0.9813313999097074, -0.1915334800712387,
-0.9997671236683783, -0.01481447609281841, 0.01505010620015893,
-0.01806375821938705, 0.1917530047987496, 0.9813706844588156);
cout << "h: " << h << endl;
//Mat temp = Mat(1, 3, CV_64F) ;
Mat temp;
temp = wRo_to_euler(h).clone();
//cout << "temp.rows: " << temp.rows << endl;
//cout << "temp.cols: " << temp.cols << endl;
cout << "wRo_to_euler(h): "<<temp<< endl;
system("pause");
}
这是运行结果