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 的描述符 descriptor详解
Feb 27 Python
Python保存MongoDB上的文件到本地的方法
Mar 16 Python
Java实现的执行python脚本工具类示例【使用jython.jar】
Mar 29 Python
Python中几种属性访问的区别与用法详解
Oct 10 Python
python利用pandas将excel文件转换为txt文件的方法
Oct 23 Python
Python面向对象之继承和多态用法分析
Jun 08 Python
python 随机森林算法及其优化详解
Jul 11 Python
简单了解python反射机制的一些知识
Jul 13 Python
FFrpc python客户端lib使用解析
Aug 24 Python
在python Numpy中求向量和矩阵的范数实例
Aug 26 Python
python开发入门——set的使用
Sep 03 Python
python 基于UDP协议套接字通信的实现
Jan 22 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
用mysql_fetch_array()获取当前行数据的方法详解
2013/06/05 PHP
老生常谈PHP中的数据结构:DS扩展
2017/07/17 PHP
swoole和websocket简单聊天室开发
2017/11/18 PHP
php ajax数据传输和响应方法
2018/08/21 PHP
laravel-admin解决表单select联动时,编辑默认没选上的问题
2019/09/30 PHP
laravel config文件配置全局变量的例子
2019/10/13 PHP
动手学习无线电
2021/03/10 无线电
Jquery 高亮显示文本中重要的关键字
2009/12/24 Javascript
javascript标签在页面中的位置探讨
2013/04/11 Javascript
上传文件返回的json数据会被提示下载问题解决方案
2014/12/03 Javascript
jQuery实现首页图片淡入淡出效果的方法
2015/06/10 Javascript
JavaScript获得指定对象大小的方法
2015/07/01 Javascript
Bootstrap Img 图片样式(推荐)
2016/12/13 Javascript
遍历json获得数据的几种方法小结
2017/01/21 Javascript
JavaScript实现时钟滴答声效果
2017/01/29 Javascript
JS简单获取当前年月日星期的方法示例
2017/02/07 Javascript
详解Vue使用命令行搭建单页面应用
2017/05/24 Javascript
vue 项目地址去掉 #的方法
2018/10/20 Javascript
ES6入门教程之变量的解构赋值详解
2019/04/13 Javascript
解决layUI的页面显示不全的问题
2019/09/20 Javascript
IntelliJ IDEA编辑器配置vue高亮显示
2019/09/26 Javascript
JavaScript实现手机号码 3-4-4格式并控制新增和删除时光标的位置
2020/06/02 Javascript
Vue 实现v-for循环的时候更改 class的样式名称
2020/07/17 Javascript
图解JS原型和原型链实现原理
2020/09/15 Javascript
js对象属性名驼峰式转下划线的实例代码
2020/09/17 Javascript
Python中pandas模块DataFrame创建方法示例
2018/06/20 Python
python 实现提取某个索引中某个时间段的数据方法
2019/02/01 Python
Tensorflow的常用矩阵生成方式
2020/01/04 Python
Python日期格式和字符串格式相互转换的方法
2020/02/18 Python
python GUI库图形界面开发之PyQt5布局控件QGridLayout详细使用方法与实例
2020/03/06 Python
专业毕业生个性的自我评价
2013/10/03 职场文书
公司员工检讨书
2014/02/08 职场文书
关于读书的演讲稿1000字
2014/08/27 职场文书
2014年维修工作总结
2014/11/22 职场文书
“学党章、守党纪、讲党规”学习心得体会
2016/01/14 职场文书
一篇文章带你学习Mybatis-Plus(新手入门)
2021/08/02 Java/Android