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类装饰器用法实例
Jun 04 Python
python 根据正则表达式提取指定的内容实例详解
Dec 04 Python
浅谈python迭代器
Nov 08 Python
python实现远程控制电脑
May 23 Python
python创建子类的方法分析
Nov 28 Python
解决tensorflow由于未初始化变量而导致的错误问题
Jan 06 Python
在django中使用apscheduler 执行计划任务的实现方法
Feb 11 Python
基于django micro搭建网站实现加水印功能
May 22 Python
python实现图像外边界跟踪操作
Jul 13 Python
python爬虫多次请求超时的几种重试方法(6种)
Dec 01 Python
Django项目如何获得SSL证书与配置HTTPS
Apr 30 Python
深入浅析python3 依赖倒置原则(示例代码)
Jul 09 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中使用PDF文档功能
2006/10/09 PHP
php zend解密软件绿色版测试可用
2008/04/14 PHP
浅谈php冒泡排序
2014/12/30 PHP
PHP中Session可能会引起并发问题
2015/06/26 PHP
PHP多维数组遍历方法(2种实现方法)
2015/12/10 PHP
PHP中session跨子域的三种实现方法
2016/07/25 PHP
PHP程序员学习使用Swoole的理由
2018/06/24 PHP
说说掌握JavaScript语言的思想前提想学习js的朋友可以看看
2009/04/01 Javascript
javascript 模式设计之工厂模式详细说明
2010/05/10 Javascript
幻灯片带网页设计中的20个奇妙应用示例小结
2012/05/27 Javascript
判断window.onload是否多次使用的方法
2014/09/21 Javascript
Javascript中八种遍历方法的执行速度深度对比
2017/04/25 Javascript
全面解析Node.js 8 重要功能和修复
2017/06/02 Javascript
微信小程序 配置顶部导航条标题颜色的实现方法
2017/09/20 Javascript
webpack4.0 入门实践教程
2018/10/08 Javascript
[46:12]完美世界DOTA2联赛循环赛 DM vs Matador BO2第一场 11.04
2020/11/04 DOTA
python抓取网页时字符集转换问题处理方案分享
2014/06/19 Python
python实现上传样本到virustotal并查询扫描信息的方法
2014/10/05 Python
socket + select 完成伪并发操作的实例
2017/08/15 Python
python去除文件中重复的行实例
2018/06/29 Python
Python使用jpype模块调用jar包过程解析
2020/07/29 Python
带你认识HTML5中的WebSocket
2015/05/22 HTML / CSS
英国标志性生活方式品牌:Skinnydip London
2019/12/15 全球购物
关于.NET, HTML的五个问题
2012/08/29 面试题
实习生自荐信范文分享
2013/11/27 职场文书
学子宴答谢词
2014/01/25 职场文书
关于责任的演讲稿
2014/05/20 职场文书
解除劳动关系协议书范文
2014/09/11 职场文书
争当四好少年演讲稿
2014/09/13 职场文书
安全温馨提示语大全
2015/07/14 职场文书
活动简报范文
2015/07/22 职场文书
大学生学习十八届五中全会精神心得体会
2016/01/05 职场文书
一个家长教育孩子的心得体会
2016/01/15 职场文书
职工趣味运动会开幕词
2016/03/04 职场文书
python控制台打印log输出重复的解决方法
2021/05/14 Python
详解Vue slot插槽
2021/11/20 Vue.js