使用eigen计算空间坐标变换

使用eigen计算空间坐标变换

机器人经常涉及不同坐标系之间的坐标变换，这里总结下使用eigen来计算坐标变换的方法。

常见的描述旋转的方式：四元数、RPY（roll、pitch、yaw）、旋转矩阵、旋转向量。

先看一个二维情况下的例子：
假设坐标系1(frame_1)在世界坐标系(world_frame)下旋转为：$\theta$，平移$t1=(t_{1}x,t_{1}y)$。
一个在坐标系1下的点$p_1=(p_{1}x,p_{1}y)$在世界坐标系下的坐标由旋转+平移的变换可得：
$p_w=R_{\theta }\ast p_1+t_1$
$\left(\begin{array}{c}{p}_{w}x\\ p_{w}y\end{array}\right)$ = $\left(\begin{array}{cc}cos\theta & -sin\theta \\ sin\theta & cos\theta \end{array}\right)$ * $\left(\begin{array}{c}{p}_{1}x\\ p_{1}y\end{array}\right)$ + $\left(\begin{array}{c}{t}_{1}x\\ t_{1}y\end{array}\right)$

可以带入值进行简单的验证：
假设点p在frame_1下的坐标为（1,0），frame_1相对于世界系的旋转为90°（π/2），frame_1相对于世界系的平移为（2,2）。
$\left(\begin{array}{c}2\\ 3\end{array}\right)$ = $\left(\begin{array}{cc}0& -1\\ 1& 0\end{array}\right)$ * $\left(\begin{array}{c}1\\ 0\end{array}\right)$ + $\left(\begin{array}{c}2\\ 2\end{array}\right)$

三维的情况同理，当我们用eigen使用四元数来计算：
$p_w=q_1\ast p_1+t_1$
$q_1$是旋转矩阵对应的四元数

Eigen::Quaterniond q1 = Eigen::AngleAxisd(3.141593/2.0, Eigen::Vector3d::UnitZ()) * 
						Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * 
						Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX());
Eigen::Vector3d t1 = Eigen::Vector3d(2, 2, 0.0);
Eigen::Vector3d p1 = Eigen::Vector3d(1, 0.0, 0.0);
Eigen::Vector3d pw;
pw = q1 * p1 + t1;

也可以用欧氏变换矩阵来计算：
p2 = q2.inverse() * (q1 * p1 + t1 - t2)
p2 = T2.inverse() * T1 * p1
$\left[\begin{array}{cc}R& t\\ 0^T& 1\end{array}\right]$
TE=[R0Tt1]TE=[Rt0T1]

1. 旋转矩阵 RR 是正交矩阵正交矩阵；

#include <iostream>
#include <string>
#include <Eigen/Eigen>
using namespace std;
//this test show how to use eigen to compute coordinate transform
int main(int argc, char *argv[])
{
	// Eigen::Quaterniond q1 = Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0).normalized();
	Eigen::Quaterniond q1 = Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitZ()) * 
							Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * 
							Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX());
    // Eigen::Quaterniond q2 = Eigen::Quaterniond(0.92388, 0.0, 0.0, 0.382683).normalized();
	Eigen::Quaterniond q2 = Eigen::AngleAxisd(3.141593, Eigen::Vector3d::UnitZ()) * 
							Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * 
							Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX());

    Eigen::Quaterniond q3 = Eigen::AngleAxisd(3.141593/4.0, Eigen::Vector3d::UnitZ()) * 
							Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * 
							Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX());
    Eigen::Quaterniond qe = q1 * q2 * q3;
    Eigen::Vector3d eulerAngle_e = qe.matrix().eulerAngles(0,1,2);
    cout <<"qe: \n"<< qe.coeffs() << "\n";
    cout <<"eulerAngle_e: \n"<< eulerAngle_e.transpose() << "\n";
    
    //平移向量
    Eigen::Vector3d t1 = Eigen::Vector3d(1, 1, 0.0);
    Eigen::Vector3d t2 = Eigen::Vector3d(2, 2, 0.0);
    Eigen::Vector3d t3 = Eigen::Vector3d(2, 2, 0.0);

    Eigen::Vector3d p0 = Eigen::Vector3d(1, 0.0, 0.0);
    Eigen::Vector3d pe;
    pe = q1 * (q2 * (q3 * p0 + t3) + t2) + t1;
    cout <<"pe: \n"<< pe.transpose() << "\n";
    //目标向量
    Eigen::Vector3d p1 = Eigen::Vector3d(2, 2, 0.0);
    Eigen::Vector3d p2;
	Eigen::Vector3d pw;

    //打印输出
    cout <<"q1: \n"<< q1.coeffs() << "\n"
         <<"q2: \n"<< q2.coeffs() << "\n"
         <<"t1: \n"<< t1.transpose() << "\n"
         <<"t2: \n"<< t2.transpose() << endl;
	pw = q1 * p1 + t1;
	cout <<"pw: \n"<< pw.transpose() << endl;
	//pw = q2 * p2 + t2; => q2 * p2 + t2 = q1 * p1 + t1  =>  p2 = q2^-1*(q1 * p1 + t1 - t2)
	//四元数求解
	p2 = q2.inverse() * (q1 * p1 + t1 - t2);
	cout << "四元数求解" << endl;
    cout <<"p2: \n"<< p2.transpose() << endl;

	
	//欧拉矩阵
    Eigen::Isometry3d T1 = Eigen::Isometry3d::Identity();
    Eigen::Isometry3d T2 = Eigen::Isometry3d::Identity();
    T1.rotate(q1.toRotationMatrix());
    T1.pretranslate(t1);
    T2.rotate(q2.toRotationMatrix());
    T2.pretranslate(t2);
    // cout << T1.matrix() << endl;
    // cout << T2.matrix() << endl;

    //欧拉矩阵求解
    p2 = T2.inverse() * T1 * p1;
    cout << "欧拉矩阵求解" << endl;
	cout <<"p2: \n"<< p2.transpose() << endl;
	return 0;
}