描述

欧式空间6维位姿，与其变换矩阵，二者之间相互转换

下面贴出来python和C++代码，先贴出来的是api函数，可以直接调用。不会用的人可以看最后，有使用例子

接口代码（可直接使用）

python

def getPose_fromT(T):x = T[0,3]y = T[1,3]z = T[2,3]rx = math.atan2(T[2,1], T[2,2])ry = math.asin(-T[2,0])rz = math.atan2(T[1,0], T[0,0])return x, y, z, rx, ry, rzdef getT_fromPose(x, y, z, rx, ry, rz):Rx = np.mat([[1, 0, 0], [0, math.cos(rx), -math.sin(rx)], [0, math.sin(rx), math.cos(rx)]])Ry = np.mat([[math.cos(ry), 0, math.sin(ry)], [0, 1, 0], [-math.sin(ry), 0, math.cos(ry)]])Rz = np.mat([[math.cos(rz), -math.sin(rz), 0], [math.sin(rz), math.cos(rz), 0], [0, 0, 1]])t = np.mat([[x],[y],[z]])R = Rz * Ry * RxR_ = np.array(R)t_ = np.array(t)T_1 = np.append(R_, t_, axis = 1)zero = np.mat([0,0,0,1])T_2 = np.array(zero)T = np.append(T_1, T_2, axis = 0)T = np.mat(T)return T

C++

Eigen::MatrixXd getT_fromPose(double x, double y, double z, double rx, double ry, double rz)
{Eigen::MatrixXd Rx(3, 3);Eigen::MatrixXd Ry(3, 3);Eigen::MatrixXd Rz(3, 3);Rx << 1, 0, 0, 0, cos(rx), -sin(rx), 0, sin(rx), cos(rx);Ry << cos(ry), 0, sin(ry), 0, 1, 0, -sin(ry), 0, cos(ry);Rz << cos(rz), - sin(rz), 0, sin(rz), cos(rz), 0, 0, 0, 1;Eigen::MatrixXd R(3, 3);R = Rz * Ry * Rx;Eigen::MatrixXd P(3, 1);P << x, y, z;Eigen::MatrixXd T(4,4);T << R, P, Eigen::MatrixXd::Zero(1, 3), Eigen::MatrixXd::Identity(1,1);return T;
}std::vector<double> getPose_fromT(Eigen::MatrixXd T)
{double x = T(0, 3);double y = T(1, 3);double z = T(2, 3);double rx = atan2(T(2, 1), T(2, 2));double ry = asin(-T(2, 0));double rz = atan2(T(1, 0), T(0, 0));std::vector<double> pose;pose.push_back(x);pose.push_back(y);pose.push_back(z);pose.push_back(rx);pose.push_back(ry);pose.push_back(rz);return pose;
}

例子

python例子

import math
import numpy as np
import scipy.linalg as ladef getPose_fromT(T):x = T[0, 3] y = T[1, 3]z = T[2, 3]rx = math.atan2(T[2, 1], T[2, 2])ry = math.asin(-T[2, 0]) rz = math.atan2(T[1, 0], T[0, 0])return x, y, z, rx, ry, rzdef getT_fromPose(x, y, z, rx, ry, rz):Rx = np.mat([[1, 0, 0], [0, math.cos(rx), -math.sin(rx)], [0, math.sin(rx), math.cos(rx)]])Ry = np.mat([[math.cos(ry), 0, math.sin(ry)], [0, 1, 0], [-math.sin(ry), 0, math.cos(ry)]])Rz = np.mat([[math.cos(rz), -math.sin(rz), 0], [math.sin(rz), math.cos(rz), 0], [0, 0, 1]])t = np.mat([[x], [y], [z]])R = Rz * Ry * RxR_ = np.array(R)t_ = np.array(t)T_1 = np.append(R_, t_, axis = 1)zero = np.mat([0,0,0,1])T_2 = np.array(zero) T = np.append(T_1, T_2, axis = 0)T = np.mat(T)return T# T2 = T1 * T
def getTransT_Pose2inPose1(T1, T2):return T1.I * T2T1 = getT_fromPose(-0.072944147641399, -0.06687830562048944, 0.4340418493881254, -0.2207496117519063, 0.0256862005614321, 0.1926014162476009)
print(T1)
x1, y1, z1, rx1, ry1, rz1 = getPose_fromT(T1)
print(x1, y1, z1, rx1, ry1, rz1)T  = np.mat([[0.6373552241213387, 0.4795294143719509, -0.6031831057293726, 5590696.786710221],
[0.346752148464638, 0.5205619206440493, 0.7802424202198545, -11216440.57810941],
[0.6881433468547054, -0.7064466204374341, 0.1655050049156589, 46487322.53149694],
[0, 0, 0, 1]])
P = getPose_fromT(T)
print(P)

C++例子

#include <iostream>
#include <vector>
#include "Eigen/Dense"Eigen::MatrixXd getT_fromPose(double x, double y, double z, double rx, double ry, double rz)
{Eigen::MatrixXd Rx(3, 3);Eigen::MatrixXd Ry(3, 3);Eigen::MatrixXd Rz(3, 3);Rx << 1, 0, 0, 0, cos(rx), -sin(rx), 0, sin(rx), cos(rx);Ry << cos(ry), 0, sin(ry), 0, 1, 0, -sin(ry), 0, cos(ry);Rz << cos(rz), - sin(rz), 0, sin(rz), cos(rz), 0, 0, 0, 1;Eigen::MatrixXd R(3, 3);R = Rz * Ry * Rx;Eigen::MatrixXd P(3, 1);P << x, y, z;Eigen::MatrixXd T(4,4);T << R, P, Eigen::MatrixXd::Zero(1, 3), Eigen::MatrixXd::Identity(1,1);return T;
}std::vector<double> getPose_fromT(Eigen::MatrixXd T)
{double x = T(0, 3);double y = T(1, 3);double z = T(2, 3);double rx = atan2(T(2, 1), T(2, 2));double ry = asin(-T(2, 0));double rz = atan2(T(1, 0), T(0, 0));std::vector<double> pose;pose.push_back(x);pose.push_back(y);pose.push_back(z);pose.push_back(rx);pose.push_back(ry);pose.push_back(rz);return pose;
}int main(int argc, char** argv)
{Eigen::MatrixXd T = getT_fromPose(-0.072944147641399, -0.06687830562048944, 0.4340418493881254, -0.2207496117519063, 0.0256862005614321, 0.1926014162476009);std::cout<<T<<std::endl;Eigen::MatrixXd T1(4, 4);T1<<0.981186, -0.192288, -0.0173152, -0.0729441,0.19135, 0.956615, 0.219709, -0.0668783,-0.0256834, -0.218889, 0.975412, 0.434042,0, 0, 0, 1;std::vector<double> pose = getPose_fromT(T1);for (int i = 0; i < pose.size(); i++){std::cout<<pose[i]<<std::endl;}return 1;
}

欧氏空间位姿与变换矩阵的转换相关推荐

机械臂沿末端xyz轴移动及旋转（python）
描述机械臂末端的位姿是p时,不想沿着基坐标系来移动,而是想沿着末端的xyz轴来移动接口代码核心函数 # 移动 def move_endEffector(axis, dist, x, y, z, ...
几种位姿转换表示方式----tf::StampedTransform Eigen::Matrix4f Pose, 及其相互转换
一. tf::StampedTransform <-------------> Eigen::Matrix4f tf::StampedTransform是ros里代表TF变换的数据类型, ...
坐标系转换中位姿与位置
我在学习机器人学和机器视觉的过程中,总是容易弄混位姿和位置的概念,感到一团浆糊,下面我举一个例子,来区分这两个概念,加深自己的理解. 设两个坐标系,基坐标系B,工具坐标系T,目标物体在两个坐标系下的坐 ...
角速度与旋转矩阵的转换关系及思考
在机器人的控制和轨迹规划等领域,算法结果常常能够得到机器人执行器的速度,如移动机器人的角速度和线速度.在有些仿真情况下,或是其它需要实时更新机器人的运动状态时,我们常常需要根据这些计算出的速度参数(控 ...
Open Inventor 在STK 模型转换中的应用
Open Inventor 在STK 模型转换中的应用韩潮,曲艺摘要:为解决航天仿真系统中的航天器建模问题,对STK模型文件进行了结构分析,探讨了STK建模语言和VRML 建模语言在总体规则.语法 ...
GroundTrue和里程计输出的位姿的参考坐标系不一致的情况
这里写目录标题前言数据集描述使用TF工具包获取使用Eigen库计算置换输出误差对比没做转换之前转换之后前言最近遇到一个数据集的ground true参考坐标和vSLAM输出的位姿的 ...
三维点集拟合：平面拟合、RANSAC、ICP算法
ACM算法分类:http://www.kuqin.com/algorithm/20080229/4071.html;CSDN容易吞图,不过编辑器里面图片还是显示的..... 一. 拟合一个平面 ...
SVO: Fast Semi-Direct Monocular Visual Odometry
论文翻译论文讲解1 论文讲解2 一.符号规定在论文的第三部分,为了方便后面的叙述,将用到的大部分符号都做了一定的规定. 首先,论文采用的是图片的灰度值,所以将第k时刻的图片的灰度值记为Ik,这个I ...
ORB_SLAM2 原理、论文解读、代码流程
ORB_SLAM2 原理+论文解读+代码流程算法原理 Tracking LocalMapping LoopClosing 代码流程文件的调用关系重要变量的数据结构 Tracking流程 Loca ...

欧氏空间位姿与变换矩阵的转换

描述

接口代码（可直接使用）

python

C++

例子

python例子

C++例子

欧氏空间位姿与变换矩阵的转换相关推荐

最新文章

热门文章