python 和c++实现旋转矩阵到欧拉角的变换方式

在摄影测量学科中,国际摄影测量遵循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;
}

#!/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++实现旋转矩阵到欧拉角的变换方式就是小编分享给大家的全部内容了,希望能给大家一个参考,也希望大家多多支持我们。

(0)

相关推荐

  • Python3实现的旋转矩阵图像算法示例

    本文实例讲述了Python3实现的旋转矩阵图像算法.分享给大家供大家参考,具体如下: 问题: 给定一个 n × n 的二维矩阵表示一个图像. 将图像顺时针旋转 90 度. 方案一:先按X轴对称旋转, 再用zip()解压,最后用list重组. # -*- coding:utf-8 -*- #! python3 class Solution: def rotate(self, matrix): """ :type matrix: List[List[int]] :rtype: v

  • python实现回旋矩阵方式(旋转矩阵)

    我们知道Python中是没有数组 这种数据结构的,所以要想实现回旋矩阵,需要先导入一个numpy包, 它是一个由多维数组对象和用于处理数组的例程集合组成的python扩充程序库,可以用来做数组算数和逻辑运算 思路: 首先创建一个n*m全为零的矩阵,然后按照旋转规律依次替换里面的值 最外层的循环代表替换一圈数据,这一圈数据在数组中的形状是一个矩形,我们分析可以知道,最后一轮循环有两种情况: 1.替换一圈(矩形)数据 2.替换一个数据 如果是第一种情况,我们让循环正常运行就可以了,但是如果是第二种,

  • python将四元数变换为旋转矩阵的实例

    如下所示: import numpy as np from autolab_core import RigidTransform # 写上用四元数表示的orientation和xyz表示的position orientation = {'y': -0.6971278819736084, 'x': -0.716556549511624, 'z': -0.010016582945017661, 'w': 0.02142651612120239} position = {'y': -0.2602268

  • 对Python的zip函数妙用,旋转矩阵详解

    Python的zip函数 示例1: x = [1, 2, 3] y = [4, 5, 6] z = [7, 8, 9] xyz = zip(x, y, z) print xyz 运行的结果是: [(1, 4, 7), (2, 5, 8), (3, 6, 9)] 从这个结果可以看出zip函数的基本运作方式. 示例2: x = [1, 2, 3] y = [4, 5, 6] z = [7, 8, 9] xyz = zip(x, y, z) u = zip(*xyz) print u 运行的结果是:

  • Python根据欧拉角求旋转矩阵的实例

    利用numpy和scipy,我们可以很容易根据欧拉角求出旋转矩阵,这里的旋转轴我们你理解成四元数里面的旋转轴 import numpy as np import scipy.linalg as linalg import math #参数分别是旋转轴和旋转弧度值 def rotate_mat(self, axis, radian): rot_matrix = linalg.expm(np.cross(np.eye(3), axis / linalg.norm(axis) * radian)) a

  • python 和c++实现旋转矩阵到欧拉角的变换方式

    在摄影测量学科中,国际摄影测量遵循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 namespa

  • python 与GO中操作slice,list的方式实例代码

    python 与GO中操作slice,list的方式实例代码 GO代码中遍历slice,寻找某个slice,统计个数. type Element interface{} func main() { a := []int{1, 2, 3, 4, 1} for _, i := range a { fmt.Println(i) } for i := 0; i < len(a); i++ { //fmt.Println(i) } fmt.Println(index0(a, 3)) fmt.Println

  • 浅谈python和C语言混编的几种方式(推荐)

    Python这些年风头一直很盛,占据了很多领域的位置,Web.大数据.人工智能.运维均有它的身影,甚至图形界面做的也很顺,乃至full-stack这个词语刚出来的时候,似乎就是为了描述它. Python虽有GIL的问题导致多线程无法充分利用多核,但后来的multiprocess可以从多进程的角度来利用多核,甚至affinity可以绑定具体的CPU核,这个问题也算得到解决.虽基本为全栈语言,但有的时候为了效率,可能还是会去考虑和C语言混编.混编是计算机里一个不可回避的话题,涉及的东西很多,技术.架

  • Python selenium 父子、兄弟、相邻节点定位方式详解

    今天跟大家分享下selenium中根据父子.兄弟.相邻节点定位的方法,很多人在实际应用中会遇到想定位的节点无法直接定位,需要通过附近节点来相对定位的问题,但从父节点定位子节点容易,从子节点定位父节点.定位一个节点的哥哥节点就一筹莫展了,别急,且看博主一步步讲解. 1. 由父节点定位子节点 最简单的肯定就是由父节点定位子节点了,我们有很多方法可以定位,下面上个例子: 对以下代码: <html> <body> <div id="A"> <!--父节

  • 详解python中字典的循环遍历的两种方式

    开发中经常会用到对于字典.列表等数据的循环遍历,但是python中对于字典的遍历对于很多初学者来讲非常陌生,今天就来讲一下python中字典的循环遍历的两种方式. 注意: python2和python3中,下面两种方法都是通用的. 1. 只对键的遍历 一个简单的for语句就能循环字典的所有键,就像处理序列一样: d = {'name1' : 'pythontab', 'name2' : '.', 'name3' : 'com'} for key in d: print (key, ' value

  • Python使用flask框架操作sqlite3的两种方式

    本文实例讲述了Python使用flask框架操作sqlite3的两种方式.分享给大家供大家参考,具体如下: 方式一:raw_sql import sqlite3 from flask import Flask, request, jsonify app = Flask(__name__) DATABASE_URI = ":memory:" # 创建表格.插入数据 @app.before_first_request def create_db(): # 连接 conn = sqlite3

  • python 将字符串转换成字典dict的各种方式总结

    1)利用eval可以将字典格式的字符串与字典户转 >>>mstr = '{"name":"yct","age":10}' 转换为可以用的字典: >>>eval(mstr), type( eval(mstr) ) {"name":"yct","age":10}, dict 2).JSON到字典转化: >>>dictinfo = json

  • 详解Python下载图片并保存本地的两种方式

    一:使用Python中的urllib类中的urlretrieve()函数,直接从网上下载资源到本地,具体代码: import os,stat import urllib.request img_url="https://timgsa.baidu.com/timg?image&quality=80&size=b9999_10000&sec=1516371301&di=d99af0828bb301fea27c2149a7070" \ "d44&am

  • python 使用socket传输图片视频等文件的实现方式

    在开发一些需要网络通信的应用中,经常会用到各种网络协议进行通信,博主在开发实验室的机器人的时候就遇到了需要把机器人上采集到的图片传回服务器进行处理识别,在python下的实现方式如下(只贴出了关键代码) 服务器端 LOCAL_IP = '192.168.100.22' #本机在局域网中的地址,或者写127.0.0.1 PORT = 2567 #指定一个端口 def server(): sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)

  • Python中的CSV文件使用"with"语句的方式详解

    是否可以直接使用with语句与CSV文件?能够做这样的事情似乎很自然: import csv with csv.reader(open("myfile.csv")) as reader: # do things with reader 但是csv.reader不提供__enter__和__exit__方法,所以这不行.但是我可以分两步做: import csv with open("myfile.csv") as f: reader = csv.reader(f)

随机推荐