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编写的最短路径算法
Mar 25 Python
Django csrf 验证问题的实现
Oct 09 Python
搞定这套Python爬虫面试题(面试会so easy)
Apr 03 Python
Python3内置模块之json编解码方法小结【推荐】
Dec 09 Python
python按比例随机切分数据的实现
Jul 11 Python
cProfile Python性能分析工具使用详解
Jul 22 Python
python 实现兔子生兔子示例
Nov 21 Python
使用pyqt 实现重复打开多个相同界面
Dec 13 Python
在python中计算ssim的方法(与Matlab结果一致)
Dec 19 Python
Django项目uwsgi+Nginx保姆级部署教程实现
Apr 19 Python
Django Admin 上传文件到七牛云的示例代码
Jun 20 Python
68行Python代码实现带难度升级的贪吃蛇
Jan 18 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缓存函数的使用说明
2013/05/10 PHP
百度ping方法使用示例 自动ping百度
2014/01/26 PHP
php计算数组不为空元素个数的方法
2014/01/27 PHP
PHP动态生成javascript文件的2个例子
2014/04/11 PHP
腾讯CMEM的PHP扩展编译安装方法
2015/09/25 PHP
POST一个JSON格式的数据给Restful服务实例详解
2017/04/07 PHP
深入理解Yii2.0乐观锁与悲观锁的原理与使用
2017/07/26 PHP
PHP封装的简单连接MongoDB类示例
2019/02/13 PHP
javascript之querySelector和querySelectorAll使用介绍
2011/12/20 Javascript
JS判断当前日期是否大于某个日期的实现代码
2012/09/02 Javascript
JavaScript定义变量和变量优先级问题探讨
2014/10/11 Javascript
JSON相关知识汇总
2015/07/03 Javascript
纯JS实现本地图片预览的方法
2015/07/31 Javascript
jquery+css实现的红色线条横向二级菜单效果
2015/08/22 Javascript
AngularJS指令详解及示例代码
2016/08/16 Javascript
相册展示PhotoSwipe.js插件实现
2016/08/25 Javascript
jquery-mobile基础属性与用法详解
2016/11/23 Javascript
JS正则表达式学习之贪婪和非贪婪模式实例总结
2016/12/26 Javascript
详解Node全局变量global模块
2017/09/28 Javascript
JavaScript高级函数应用之分时函数实例分析
2018/08/03 Javascript
react同构实践之实现自己的同构模板
2019/03/13 Javascript
Vue实现购物车的全选、单选、显示商品价格代码实例
2019/05/06 Javascript
使用vue for时为什么要key【推荐】
2019/07/11 Javascript
vue-cli3跨域配置的简单方法
2019/09/06 Javascript
解析vue、angular深度作用选择器
2019/09/11 Javascript
vue 导航菜单刷新状态不消失,显示对应的路由界面操作
2020/08/06 Javascript
python实现简单socket程序在两台电脑之间传输消息的方法
2015/03/13 Python
python TKinter获取文本框内容的方法
2018/10/11 Python
全面了解django的缓存机制及使用方法
2019/07/22 Python
python实现超市管理系统(后台管理)
2019/10/25 Python
python 中的[:-1]和[::-1]的具体使用
2020/02/13 Python
UGG雪地靴德国官网:UGG德国
2016/11/19 全球购物
岗位职责的含义
2013/11/17 职场文书
运动会四百米广播稿
2014/01/19 职场文书
股东合作协议书
2014/04/14 职场文书
Java 多线程并发FutureTask
2022/06/28 Java/Android