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 相关文章推荐
python3抓取中文网页的方法
Jul 28 Python
全面解析Python的While循环语句的使用方法
Oct 13 Python
Python过滤列表用法实例分析
Apr 29 Python
Python爬取京东的商品分类与链接
Aug 26 Python
Python线程指南详细介绍
Jan 05 Python
Python3学习urllib的使用方法示例
Nov 29 Python
python随机取list中的元素方法
Apr 08 Python
用Python3创建httpServer的简单方法
Jun 04 Python
Django+Xadmin构建项目的方法步骤
Mar 06 Python
python字符串切割:str.split()与re.split()的对比分析
Jul 16 Python
Django 开发调试工具 Django-debug-toolbar使用详解
Jul 23 Python
Python测试框架:pytest学习笔记
Oct 20 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错误日志 display_errors与log_errors的区别
2012/10/09 PHP
php基于Snoopy解析网页html的方法
2015/07/09 PHP
PHP模版引擎原理、定义与用法实例
2019/03/29 PHP
分享几种好用的PHP自定义加密函数(可逆/不可逆)
2020/09/15 PHP
setinterval()与clearInterval()JS函数的调用方法
2015/01/21 Javascript
JQuery报错Uncaught TypeError: Illegal invocation的处理方法
2015/03/13 Javascript
浅谈JQ中mouseover和mouseenter的区别
2016/09/13 Javascript
JavaScript中关于iframe滚动条的去除和保留
2016/11/17 Javascript
js实现一个可以兼容PC端和移动端的div拖动效果实例
2016/12/09 Javascript
详解JavaScript树结构
2017/01/09 Javascript
javascript 开发之百度地图使用到的js函数整理
2017/05/19 Javascript
微信小程序删除处理详解
2017/08/16 Javascript
JS实现的透明度渐变动画效果示例
2018/04/28 Javascript
Vue 实现手动刷新组件的方法
2019/02/19 Javascript
基于better-scroll 实现歌词联动功能的代码
2020/05/07 Javascript
详解为什么Vue中的v-if和v-for不建议一起用
2021/01/13 Vue.js
python client使用http post 到server端的代码
2013/02/10 Python
详细解读Python中解析XML数据的方法
2015/10/15 Python
Python的Django中将文件上传至七牛云存储的代码分享
2016/06/03 Python
详解Python中的Descriptor描述符类
2016/06/14 Python
python随机取list中的元素方法
2018/04/08 Python
python批量查询、汉字去重处理CSV文件
2018/05/31 Python
使用python对文件中的数值进行累加的实例
2018/11/28 Python
postman传递当前时间戳实例详解
2019/09/14 Python
python 动态绘制爱心的示例
2020/09/27 Python
Python爬虫实战案例之爬取喜马拉雅音频数据详解
2020/12/07 Python
CSS3让登陆面板3D旋转起来
2016/05/03 HTML / CSS
Nike英国官网:Nike.com (UK)
2017/02/13 全球购物
Merchant 1948澳大利亚:新西兰领先的鞋类和靴子供应商
2018/03/24 全球购物
面向对象编程是如何提高软件开发水平的
2014/05/06 面试题
优秀研究生主要事迹
2014/06/03 职场文书
企业安全生产目标责任书
2014/07/23 职场文书
车辆年检委托书范本
2014/10/14 职场文书
金秋助学感谢信
2015/01/21 职场文书
阳光体育运动标语口号
2015/12/26 职场文书
剑指Offer之Java算法习题精讲二叉树的构造和遍历
2022/03/21 Java/Android