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中with语句的用法
Apr 15 Python
qpython3 读取安卓lastpass Cookies
Jun 19 Python
Python实现的简单dns查询功能示例
May 24 Python
python版简单工厂模式
Oct 16 Python
Python:Scrapy框架中Item Pipeline组件使用详解
Dec 27 Python
python制作mysql数据迁移脚本
Jan 01 Python
Python英文文本分词(无空格)模块wordninja的使用实例
Feb 20 Python
python实现多进程通信实例分析
Sep 01 Python
Python使用微信接入图灵机器人过程解析
Nov 04 Python
Python 统计位数为偶数的数字代码详解
Mar 15 Python
基于python实现监听Rabbitmq系统日志代码示例
Nov 28 Python
详解用selenium来下载小姐姐图片并保存
Jan 26 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 magic quotes的详解
2013/06/17 PHP
9个实用的PHP代码片段分享
2015/01/22 PHP
Laravel SQL语句记录方式(推荐)
2016/05/26 PHP
javascript 写类方式之七
2009/07/05 Javascript
一个JS的日期格式化算法示例
2013/07/31 Javascript
两个多选select(multiple左右)添加、删除选项和取值实例
2014/05/12 Javascript
JavaScript中的立即执行函数表达式介绍
2015/03/15 Javascript
基于JS实现移动端向左滑动出现删除按钮功能
2017/02/22 Javascript
深入理解React中何时使用箭头函数
2017/08/23 Javascript
深入理解Angular4订阅(Subscribe)与取消
2017/11/22 Javascript
vue使用代理解决请求跨域问题详解
2019/07/24 Javascript
vue实现整屏滚动切换
2020/06/29 Javascript
vue中watch和computed的区别与使用方法
2020/08/23 Javascript
JS创建自定义对象的六种方法总结
2020/12/15 Javascript
vue 实现基础组件的自动化全局注册
2020/12/25 Vue.js
Django静态资源URL STATIC_ROOT的配置方法
2014/11/08 Python
python正则表达式中的括号匹配问题
2014/12/14 Python
Python实现过滤单个Android程序日志脚本分享
2015/01/16 Python
PyQt5创建一个新窗口的实例
2019/06/20 Python
如何基于python对接钉钉并获取access_token
2020/04/21 Python
基于pycharm实现批量修改变量名
2020/06/02 Python
pytorch __init__、forward与__call__的用法小结
2021/02/27 Python
澳大利亚在线购买儿童玩具:Toy Universe
2017/12/28 全球购物
MAC Cosmetics巴西官方网站:M·A·C彩妆
2019/04/18 全球购物
乌克兰品牌化妆品和香水在线商店:Bomond
2020/01/14 全球购物
全球采购的街头服饰和帽子:Urban Excess
2020/10/28 全球购物
基于Python 函数和方法的区别说明
2021/03/24 Python
体育教育专业毕业生自荐信
2013/11/15 职场文书
入党积极分子思想汇报范文
2014/01/05 职场文书
办公室文员自荐书
2014/02/03 职场文书
财务总监管理岗位职责
2014/03/08 职场文书
幼儿评语大全
2014/04/30 职场文书
教师求职信怎么写
2015/03/20 职场文书
2019销售早会主持词
2019/06/27 职场文书
解析laravel使用workerman用户交互、服务器交互
2021/04/28 PHP
vscode中使用npm安装babel的方法
2021/08/02 Javascript