使用eigen计算空间坐标变换

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

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

先看一个二维情况下的例子：
假设坐标系1(frame_1)在世界坐标系(world_frame)下旋转为：θθθ，平移t1=(t1x,t1y)t1 = (t_1x,t_1y)t1=(t1x,t1y)。
一个在坐标系1下的点p1=(p1x,p1y)p_1 = (p_1x,p_1y)p1=(p1x,p1y)在世界坐标系下的坐标由旋转+平移的变换可得：
pw=Rθ∗p1+t1p_w = R_θ * p_1 +t_1pw=Rθ∗p1+t1
(pwxpwy)\begin{pmatrix}p_wx \\ p_wy \\ \end{pmatrix}(pwxpwy) = (cosθ−sinθsinθcosθ)\begin{pmatrix}cosθ & -sinθ \\ sinθ & cosθ\\ \end{pmatrix}(cosθsinθ−sinθcosθ) * (p1xp1y)\begin{pmatrix}p_1x \\ p_1y \\ \end{pmatrix}(p1xp1y) + (t1xt1y)\begin{pmatrix}t_1x \\ t_1y \\ \end{pmatrix}(t1xt1y)

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

三维的情况同理，当我们用eigen使用四元数来计算：
pw=q1∗p1+t1p_w = q_1* p_1 +t_1pw=q1∗p1+t1
q1q_1q1是旋转矩阵对应的四元数


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
[Rt0T1]\begin{bmatrix}R & t \\ 0^T & 1\\ \end{bmatrix}[R0Tt1]
TE=[R0Tt1]TE=[Rt0T1]

旋转矩阵 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;
}

使用eigen计算空间坐标变换相关推荐

pcl::lineToLineSegment() 计算空间直线的交点和最小公垂线
对于两空间直线来说,计算交点和最小公垂线是一码事,交点即最小公垂线两个垂足的中心.PCL中源码中包含计算空间直线最小公垂线的函数 PCL_EXPORTS void pcl::lineToLineSeg ...
R 计算空间基尼系数
空间基尼系数主要用于衡量地理要素的空间分布差异,是地理学研究中一个常见指标.本文主要介绍如何利用R语言计算空间基尼系数. setwd("C:\\Users\\Acer\\Desktop&qu ...
计算空间中两个向量的夹角
1,直接计算空间夹角 //计算α转角 var p1 = new THREE.Vector3(13.77,0.7,37.55);// 点v坐标 var p2 = new THREE.Vector3(- ...
mysql空间计算_MySQL 空间计算空间查询
一.前言 MySQL实施了OGC建议的具有Geometry类型的SQL环境的一个子集.该术语指的是用一组集合类型扩展的环境.具有几何值的SQL列是作为拥有集合类型的列实施的.该规范描述了SQL几何类型 ...
Python计算空间二面角
定义代表三维笛卡尔坐标系上某个点的Point 类(包括x .y .z 三个属性),为该类定义一个方法,可接收second .third .forth 三个参数,用于计算当前点.second .thir ...
计算空间中两线段之间的距离
最近在建立气凝胶的有限元模型中需要计算每两根纤维(线段)之间的距离,最初参考的两篇文章确实提供了关于一些数值方法的计算思路(文章1 && 文章2),但忽略了线段距离问题的理论推导,导致 ...
matlab计算空间桁架,基于matlab的空间三维桁架结构受力分析通用程序设计
第 26 卷第 3 期喀什师范学院学报 Vol. 26 No. 3 2005 年 5 月 Journal of Kashgar Teachers College May 2005 基于 matlab ...
计算空间内三个基站定位坐标
如果O.A.B基站三个基站处在空间的高处,距离地面3米,基站的测距差距在10cm,标签在地面. 那么,如果计算到平面上则可以肯定3个圆是相交的状态.而且交点一定是在两个圆交点的连线上称之为[根轴]. ...
c++计算空间向量的夹角
代码 void UtilsClass::normalization(double unnorm_array[3]) {// 归一化double max_val = std::max(abs(unnor ...

使用eigen计算空间坐标变换

使用eigen计算空间坐标变换

使用eigen计算空间坐标变换相关推荐

最新文章

热门文章