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

描述

欧式空间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.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;
}