描述

欧式空间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 mathimport numpy as npimport 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 * Tdef getTransT_Pose2inPose1(T1, T2):return T1.I * T2T1 = getT_fromPose(-0.072944147641399, -0.06687830562048944, 0.4340418493881254, -0.2207496117519063, 0.025686614321, 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.780242498545, -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.025686614321, 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;}