python 和c++实现旋转矩阵到欧拉角的变换方式


Posted in Python onDecember 04, 2019

在摄影测量学科中,国际摄影测量遵循OPK系统,即是xyz转角系统,而工业中往往使用zyx转角系统。

旋转矩阵的意义:描述相对地面的旋转情况,yaw-pitch-roll对应zyx对应k,p,w

#include <iostream>
#include<stdlib.h>
#include<eigen3/Eigen/Core>
#include<eigen3/Eigen/Dense>
#include<stdlib.h>
using namespace std;
Eigen::Matrix3d rotationVectorToMatrix(Eigen::Vector3d theta)
{
  Eigen::Matrix3d R_x=Eigen::AngleAxisd(theta(0),Eigen::Vector3d(1,0,0)).toRotationMatrix();
  Eigen::Matrix3d R_y=Eigen::AngleAxisd(theta(1),Eigen::Vector3d(0,1,0)).toRotationMatrix();
  Eigen::Matrix3d R_z=Eigen::AngleAxisd(theta(2),Eigen::Vector3d(0,0,1)).toRotationMatrix();
  return R_z*R_y*R_x;

}
bool isRotationMatirx(Eigen::Matrix3d R)
{
  int err=1e-6;//判断R是否奇异
  Eigen::Matrix3d shouldIdenity;
  shouldIdenity=R*R.transpose();
  Eigen::Matrix3d I=Eigen::Matrix3d::Identity();
  return (shouldIdenity-I).norm()<err?true:false;
}

int main(int argc, char *argv[])
{
  Eigen::Matrix3d R;
  Eigen::Vector3d theta(rand() % 360 - 180.0, rand() % 360 - 180.0, rand() % 360 - 180.0);
  theta=theta*M_PI/180;
  cout<<"旋转向量是:\n"<<theta.transpose()<<endl;
  R=rotationVectorToMatrix(theta);
  cout<<"旋转矩阵是:\n"<<R<<endl;
  if(! isRotationMatirx(R)){
   cout<<"旋转矩阵--->欧拉角\n"<<R.eulerAngles(2,1,0).transpose()<<endl;//z-y-x顺序,与theta顺序是x,y,z
  }
  else{
    assert(isRotationMatirx(R));
  }

  return 0;
}

python 和c++实现旋转矩阵到欧拉角的变换方式

#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import cv2
import numpy as np
import math
import random

def isRotationMatrix(R) :
  Rt = np.transpose(R)
  shouldBeIdentity = np.dot(Rt, R)
  I = np.identity(3, dtype = R.dtype)
  n = np.linalg.norm(I - shouldBeIdentity)
  return n < 1e-6

def rotationMatrixToEulerAngles(R) :

  assert(isRotationMatrix(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

  return np.array([x, y, z])

def eulerAnglesToRotationMatrix(theta) :
  
  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 ))

  return R


if __name__ == '__main__' :

  e = np.random.rand(3) * math.pi * 2 - math.pi
  
  R = eulerAnglesToRotationMatrix(e)
  e1 = rotationMatrixToEulerAngles(R)

  R1 = eulerAnglesToRotationMatrix(e1)
  print ("\nInput Euler angles :\n{0}".format(e))
  print ("\nR :\n{0}".format(R))
  print ("\nOutput Euler angles :\n{0}".format(e1))
  print ("\nR1 :\n{0}".format(R1))

以上这篇python 和c++实现旋转矩阵到欧拉角的变换方式就是小编分享给大家的全部内容了,希望能给大家一个参考,也希望大家多多支持三水点靠木。

Python 相关文章推荐
Python运行的17个时新手常见错误小结
Aug 07 Python
Python下singleton模式的实现方法
Jul 16 Python
Python UnicodeEncodeError: 'gbk' codec can't encode character 解决方法
Apr 24 Python
python实现数独算法实例
Jun 09 Python
Python使用os模块和fileinput模块来操作文件目录
Jan 19 Python
Python使用folium excel绘制point
Jan 03 Python
Django框架ORM数据库操作实例详解
Nov 07 Python
pycharm激活码快速激活及使用步骤
Mar 12 Python
vue.js刷新当前页面的实例讲解
Dec 29 Python
使用Python webdriver图书馆抢座自动预约的正确方法
Mar 04 Python
OpenCV图像变换之傅里叶变换的一些应用
Jul 26 Python
python如何查找列表中元素的位置
May 30 Python
python实现一个点绕另一个点旋转后的坐标
Dec 04 #Python
Django配置文件代码说明
Dec 04 #Python
python实现回旋矩阵方式(旋转矩阵)
Dec 04 #Python
在Django下创建项目以及设置settings.py教程
Dec 03 #Python
Django自带的加密算法及加密模块详解
Dec 03 #Python
python Opencv计算图像相似度过程解析
Dec 03 #Python
django 中使用DateTime常用的时间查询方式
Dec 03 #Python
You might like
PHP的博客ping服务代码
2012/02/04 PHP
PHP中获取文件扩展名的N种方法小结
2012/02/27 PHP
使用Thinkphp框架开发移动端接口
2015/08/05 PHP
JavaScript中的对象化编程
2008/01/16 Javascript
编写高性能的JavaScript 脚本的加载与执行
2010/04/19 Javascript
EditPlus注册码生成器(js代码实现)
2013/03/25 Javascript
javascript中Date()函数在各浏览器中的显示效果
2015/06/18 Javascript
jQuery常用且重要方法汇总
2015/07/13 Javascript
JavaScript使用FileReader实现图片上传预览效果
2020/03/27 Javascript
微信小程序异步处理详解
2017/11/10 Javascript
vue 指定组件缓存实例详解
2018/04/01 Javascript
详解Angular5路由传值方式及其相关问题
2018/04/28 Javascript
ng-alain表单使用方式详解
2018/07/10 Javascript
javascript实现文本框标签验证的实例代码
2018/10/14 Javascript
微信小程序吸底区域适配iPhoneX的实现
2020/04/09 Javascript
微信小程序实现登录注册功能
2020/12/29 Javascript
python数据结构之链表详解
2017/09/12 Python
python生成1行四列全2矩阵的方法
2018/08/04 Python
详解Python中的各种转义符\n\r\t
2019/07/10 Python
python实现身份证实名认证的方法实例
2019/11/08 Python
tensorflow实现对张量数据的切片操作方式
2020/01/19 Python
使用python matplotlib 画图导入到word中如何保证分辨率
2020/04/16 Python
HTML5时代CSS设置漂亮字体取代图片
2014/09/04 HTML / CSS
谈谈对css属性box-sizing的了解
2017/01/04 HTML / CSS
H5 canvas中width、height和style的宽高区别详解
2018/11/02 HTML / CSS
英国领先的维生素和补充剂品牌:Higher Nature
2019/08/26 全球购物
ORACLE十问
2015/04/20 面试题
请说出以下代码输出什么
2013/08/30 面试题
实习自荐信
2013/10/13 职场文书
学前教育学生自荐信范文
2013/12/31 职场文书
2014年国庆节演讲稿精选范文1500字
2014/09/25 职场文书
人事局接收函
2015/01/31 职场文书
清洁工个人工作总结
2015/03/05 职场文书
安全第一课观后感
2015/06/18 职场文书
Win11如何设置右键单击显示所有选项?Win11右键单击显示所有选项设置教程
2022/04/08 数码科技
一篇文章带你掌握SQLite3基本用法
2022/06/14 数据库