提示:若只有一个方向的旋转则可以用下面公司计算旋转角度(幅值):
Mat eulerAnglesToRotationMatrix(Vec3f &theta)
{
// Calculate rotation about x axis
Mat R_x = (Mat_<double>(3,3) <<
1, 0, 0,
0, cos(theta[0]), -sin(theta[0]),
0, sin(theta[0]), cos(theta[0])
);
// Calculate rotation about y axis
Mat R_y = (Mat_<double>(3,3) <<
cos(theta[1]), 0, sin(theta[1]),
0, 1, 0,
-sin(theta[1]), 0, cos(theta[1])
);
// Calculate rotation about z axis
Mat R_z = (Mat_<double>(3,3) <<
cos(theta[2]), -sin(theta[2]), 0,
sin(theta[2]), cos(theta[2]), 0,
0, 0, 1
);
// Combined rotation matrix
Mat R = R_z * R_y * R_x;
return R;
}
Vec3f rotationMatrixToEulerAngles(Mat &R)
{
float sy = sqrt(R.at<double>(0,0) * R.at<double>(0,0) + R.at<double>(1,0) * R.at<double>(1,0) );
bool singular = sy < 1e-6; // If
float x, y, z;
if (!singular)
{
x = atan2(R.at<double>(2,1) , R.at<double>(2,2));
y = atan2(-R.at<double>(2,0), sy);
z = atan2(R.at<double>(1,0), R.at<double>(0,0));
}
else
{
x = atan2(-R.at<double>(1,2), R.at<double>(1,1));
y = atan2(-R.at<double>(2,0), sy);
z = 0;
}
#if 1
x = x*180.0f/3.141592653589793f;
y = y*180.0f/3.141592653589793f;
z = z*180.0f/3.141592653589793f;
#endif
return Vec3f(x, y, z);
}
Python语言实现:
def rotationMatrixToEulerAngles(rvecs):
R = np.zeros((3, 3), dtype=np.float64)
cv2.Rodrigues(rvecs, R)
sy = math.sqrt(R[0,0] * R[0,0] + R[1,0] * R[1,0])
singular = sy < 1e-6
if not singular:
x = math.atan2(R[2,1] , R[2,2])
y = math.atan2(-R[2,0], sy)
z = math.atan2(R[1,0], R[0,0])
else :
x = math.atan2(-R[1,2], R[1,1])
y = math.atan2(-R[2,0], sy)
z = 0
#print('dst:', R)
x = x*180.0/3.141592653589793
y = y*180.0/3.141592653589793
z = z*180.0/3.141592653589793
return x,y,z
def eulerAnglesToRotationMatrix(angles1) :
theta = np.zeros((3, 1), dtype=np.float64)
theta[0] = angles1[0]*3.141592653589793/180.0
theta[1] = angles1[1]*3.141592653589793/180.0
theta[2] = angles1[2]*3.141592653589793/180.0
R_x = np.array([[1, 0, 0 ],
[0, math.cos(theta[0]), -math.sin(theta[0]) ],
[0, math.sin(theta[0]), math.cos(theta[0]) ]
])
R_y = np.array([[math.cos(theta[1]), 0, math.sin(theta[1]) ],
[0, 1, 0 ],
[-math.sin(theta[1]), 0, math.cos(theta[1]) ]
])
R_z = np.array([[math.cos(theta[2]), -math.sin(theta[2]), 0],
[math.sin(theta[2]), math.cos(theta[2]), 0],
[0, 0, 1]
])
R = np.dot(R_z, np.dot( R_y, R_x ))
sy = math.sqrt(R[0,0] * R[0,0] + R[1,0] * R[1,0])
singular = sy < 1e-6
if not singular:
x = math.atan2(R[2,1] , R[2,2])
y = math.atan2(-R[2,0], sy)
z = math.atan2(R[1,0], R[0,0])
else :
x = math.atan2(-R[1,2], R[1,1])
y = math.atan2(-R[2,0], sy)
z = 0
#print('dst:', R)
x = x*180.0/3.141592653589793
y = y*180.0/3.141592653589793
z = z*180.0/3.141592653589793
#rvecs = np.zeros((1, 1, 3), dtype=np.float64)
#rvecs,_ = cv2.Rodrigues(R, rvecstmp)
return x,y,z
以下代码用来实现旋转向量到旋转矩阵及旋转矩阵到旋转向量的转换:
r_vec[0]=(double)(rotation_vectors[i](0));
r_vec[1]=(double)(rotation_vectors[i](1));
r_vec[2]=(double)(rotation_vectors[i](2));
cvInitMatHeader(pr_vec,1,3,CV_64FC1,r_vec,CV_AUTOSTEP);
cvInitMatHeader(pR_matrix,3,3,CV_64FC1,R_matrix,CV_AUTOSTEP);
cvRodrigues2(pr_vec, pR_matrix, 0);//从旋转向量求旋转矩阵
cvRodrigues2(pR_matrix, pnew_vec, 0);//从旋转矩阵求旋转向量
Mat rotation_vec_tmp(pnew_vec->rows,pnew_vec->cols,pnew_vec->type,pnew_vec->data.fl);
//cvMat转Mat
Mat rotation_matrix_tmp(pR_matrix->rows,pR_matrix->cols,pR_matrix->type,pR_matrix->data.fl);
//eulerAngles = rotationMatrixToEulerAngles(rotation_matrix_tmp, 0);
//rotation_matrix_tmp = eulerAnglesToRotationMatrix(eulerAngles);
eulerAngles = rotationMatrixToEulerAngles(rotation_matrix_tmp, 1);//0表示输出为弧度,否则表示输出为角度
对应的python代码如下:
R = np.zeros((3, 3), dtype=np.float64)
cv2.Rodrigues(rvecs, R)
rvecs = np.zeros((1, 1, 3), dtype=np.float64)
rvecs,_ = cv2.Rodrigues(R, rvecstmp)
相互转换的完整代码
完整的测试C++程序如下:
#include <iostream>
#include <sstream>
#include <time.h>
#include <stdio.h>
#include <fstream>
#include <cmath>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/highgui/highgui.hpp>
using namespace cv;
using namespace std;
// Calculates rotation matrix given euler angles.
Mat eulerAnglesToRotationMatrix(Vec3f &theta)
{
// Calculate rotation about x axis
Mat R_x = (Mat_<double>(3,3) <<
1, 0, 0,
0, cos(theta[0]), -sin(theta[0]),
0, sin(theta[0]), cos(theta[0])
);
// Calculate rotation about y axis
Mat R_y = (Mat_<double>(3,3) <<
cos(theta[1]), 0, sin(theta[1]),
0, 1, 0,
-sin(theta[1]), 0, cos(theta[1])
);
// Calculate rotation about z axis
Mat R_z = (Mat_<double>(3,3) <<
cos(theta[2]), -sin(theta[2]), 0,
sin(theta[2]), cos(theta[2]), 0,
0, 0, 1
);
// Combined rotation matrix
Mat R = R_z * R_y * R_x;
return R;
}
// Checks if a matrix is a valid rotation matrix.
bool isRotationMatrix(Mat &R)
{
Mat Rt;
transpose(R, Rt);
Mat shouldBeIdentity = Rt * R;
Mat I = Mat::eye(3,3, shouldBeIdentity.type());
return norm(I, shouldBeIdentity) < 1e-6;
}
// Calculates rotation matrix to euler angles
// The result is the same as MATLAB except the order
// of the euler angles ( x and z are swapped ).
Vec3f rotationMatrixToEulerAngles(Mat &R)
{
//assert(isRotationMatrix(R));
float sy = sqrt(R.at<double>(0,0) * R.at<double>(0,0) + R.at<double>(1,0) * R.at<double>(1,0) );
bool singular = sy < 1e-6; // If
float x, y, z;
if (!singular)
{
x = atan2(R.at<double>(2,1) , R.at<double>(2,2));
y = atan2(-R.at<double>(2,0), sy);
z = atan2(R.at<double>(1,0), R.at<double>(0,0));
}
else
{
x = atan2(-R.at<double>(1,2), R.at<double>(1,1));
y = atan2(-R.at<double>(2,0), sy);
z = 0;
}
#if 1
x = x*180.0f/3.141592653589793f;
y = y*180.0f/3.141592653589793f;
z = z*180.0f/3.141592653589793f;
#endif
return Vec3f(x, y, z);
}
int main()
{
//Mat rotation_matrix = Mat(3, 3, CV_32FC1, Scalar::all(0)); /* 保存每幅图像的旋转矩阵 */
Vec3f eulerAngles;
int i;
//double r_vec[3]={-2.100418,-2.167796,0.273330};[[0.78520514][0.0233998 ][0.00969251
double r_vec[3]={0.78520514,0.0233998,0.00969251};//eulerAngles[45,1,1]
double R_matrix[9];
//CvMat *pr_vec;
//CvMat *pR_matrix;
CvMat *pr_vec = cvCreateMat(1,3,CV_64FC1);
CvMat *pR_matrix = cvCreateMat(3,3,CV_64FC1);
cvInitMatHeader(pr_vec, 1, 3, CV_64FC1, r_vec, CV_AUTOSTEP);
cvInitMatHeader(pR_matrix, 3, 3, CV_64FC1, R_matrix, CV_AUTOSTEP);
cvRodrigues2(pr_vec, pR_matrix, 0);
Mat rotation_matrix(pR_matrix->rows,pR_matrix->cols,pR_matrix->type,pR_matrix->data.fl);
eulerAngles = rotationMatrixToEulerAngles(rotation_matrix);
cout << "pR_matrix = " << endl;
cout << rotation_matrix << endl;
cout << "eulerAngles = " << endl;
cout << eulerAngles << endl;
Mat_<double> r_l = (Mat_<double>(3, 1) << 0.78520514,0.0233998,0.00969251);//左摄像机的旋转向量
Mat R_L;
Rodrigues(r_l, R_L);
eulerAngles = rotationMatrixToEulerAngles(R_L);
cout << "R_L = " << endl;
cout << R_L << endl;
cout << "eulerAngles = " << endl;
cout << eulerAngles << endl;
eulerAngles(0) = 90.0;
eulerAngles(1) = 0.0;
eulerAngles(2) = 0.0;
//cvRodrigues2(&pr_vec,&pR_matrix,0);
return 0;
}
以下makefile用来在linux平台下编译成可执行文件,将它放在makefile的文件中,在终端中输入make即可完成编译:
SOURCE := $(wildcard ./test_euler.cpp)
#wildcard把指定目录 ./ 下的所有后缀是cpp的文件全部展开。
DIR := $(notdir $(SOURCE))
#notdir把展开的文件去除掉路径信息
OBJS := $(patsubst %.cpp,%.o,$(DIR))
#patsubst把$(DIR)中的变量符合后缀是.cpp的全部替换成.o,
# target you can change test to what you want
TARGET := test
$(CC) = gcc
$(CXX) = g++
#ifeq ($(CC),aarch64-linux-gcc)
#else ifeq ($(findstring arm-poky-linux-gnueabi-gcc,$(CC)), arm-poky-linux-gnueabi-gcc)
LDFLAGS := -L. -L$(SDKTARGETSYSROOT)/usr/lib
INCLUDE := -I$(SDKTARGETSYSROOT)/usr/include/ -L./lib
LIBS := -lm -lopencv_core -lopencv_highgui \
-lopencv_imgproc -lopencv_calib3d -lopencv_imgcodecs \
-lopencv_flann -lopencv_videoio -ljpeg
DEFINES := -Wl,-rpath=./lib
CFLAGS := -g -Wall $(DEFINES) $(INCLUDE)
CXXFLAGS:= $(CFLAGS)
all : $(TARGET)
$(TARGET) : $(OBJS)
$(CXX) $(CXXFLAGS) -o $@ $(OBJS) $(LDFLAGS) $(LIBS)
echo $(CC) $(TARGET) ok
objs : $(OBJS)
rebuild: clean everything
clean :
rm -rf *.o
rm -rf $(TARGET)
完整的测试Python程序如下:
import tkinter as tk
import tkinter.font as tkFont
import tkinter.messagebox
import tkinter.ttk as ttk
from tkinter import filedialog
import cv2
import numpy as np
import time
from PIL import Image,ImageTk
import threading
import os
import re
import subprocess
import random
import math
# Calculates Rotation Matrix given euler angles.
def eulerAnglesToRotationMatrix(angles1) :
theta = np.zeros((3, 1), dtype=np.float64)
theta[0] = angles1[0]*3.141592653589793/180.0
theta[1] = angles1[1]*3.141592653589793/180.0
theta[2] = angles1[2]*3.141592653589793/180.0
R_x = np.array([[1, 0, 0 ],
[0, math.cos(theta[0]), -math.sin(theta[0]) ],
[0, math.sin(theta[0]), math.cos(theta[0]) ]
])
R_y = np.array([[math.cos(theta[1]), 0, math.sin(theta[1]) ],
[0, 1, 0 ],
[-math.sin(theta[1]), 0, math.cos(theta[1]) ]
])
R_z = np.array([[math.cos(theta[2]), -math.sin(theta[2]), 0],
[math.sin(theta[2]), math.cos(theta[2]), 0],
[0, 0, 1]
])
R = np.dot(R_z, np.dot( R_y, R_x ))
sy = math.sqrt(R[0,0] * R[0,0] + R[1,0] * R[1,0])
singular = sy < 1e-6
if not singular:
x = math.atan2(R[2,1] , R[2,2])
y = math.atan2(-R[2,0], sy)
z = math.atan2(R[1,0], R[0,0])
else :
x = math.atan2(-R[1,2], R[1,1])
y = math.atan2(-R[2,0], sy)
z = 0
#print('dst:', R)
x = x*180.0/3.141592653589793
y = y*180.0/3.141592653589793
z = z*180.0/3.141592653589793
rvecs = np.zeros((1, 1, 3), dtype=np.float64)
rvecs,_ = cv2.Rodrigues(R, rvecstmp)
#print()
return R,rvecs,x,y,z
def rotationMatrixToEulerAngles(rvecs):
R = np.zeros((3, 3), dtype=np.float64)
cv2.Rodrigues(rvecs, R)
sy = math.sqrt(R[0,0] * R[0,0] + R[1,0] * R[1,0])
singular = sy < 1e-6
if not singular:
x = math.atan2(R[2,1] , R[2,2])
y = math.atan2(-R[2,0], sy)
z = math.atan2(R[1,0], R[0,0])
else :
x = math.atan2(-R[1,2], R[1,1])
y = math.atan2(-R[2,0], sy)
z = 0
#print('dst:', R)
x = x*180.0/3.141592653589793
y = y*180.0/3.141592653589793
z = z*180.0/3.141592653589793
return x,y,z
if(__name__=='__main__'):
eulerAngles = np.zeros((3, 1), dtype=np.float64)
eulerAngles[0] = 45.0
eulerAngles[1] = 0.0
eulerAngles[2] = 0.0
R,rvecstmp,x,y,z = eulerAnglesToRotationMatrix(eulerAngles)
#rvecstmp[0] = -2.100418
#rvecstmp[1] = -2.167796
#rvecstmp[2] = 0.273330 0.75467396][-0.00747155][ 0.00896453
rvecstmp[0] = 0.75467396
rvecstmp[1] = -0.00747155
rvecstmp[2] = 0.00896453
print(rvecs)
print(rotationMatrixToEulerAngles(rvecs))
文章转载地址:https://www.jianshu.com/p/5e130c04a602