描述

欧式空间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, rz def 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 * Rx R_ = 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 la 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, rz def 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 * Rx R_ = 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 * T2 T1 = getT_fromPose(-0.0399, -0.048944, 0.81254, -0.19063, 0.04321, 0.76009) print(T1) x1, y1, z1, rx1, ry1, rz1 = getPose_fromT(T1) print(x1, y1, z1, rx1, ry1, rz1) T = np.mat([[0.13387, 0.19509, -0.93726, .], [0.4638, 0.40493, 0.98545, -.], [0.47054, -0.74341, 0.56589, .], [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.0399, -0.048944, 0.81254, -0.19063, 0.04321, 0.76009); std::cout<<T<<std::endl; Eigen::MatrixXd T1(4, 4); T1<<0., -0., -0.0, -0.0, 0.19135, 0., 0., -0.0, -0.0, -0., 0., 0., 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; }