eigen 四元数进行坐标旋转
2024-08-30 01:41:30
(《视觉SLAM十四讲》第三讲习题7)设有小萝卜一号和二号在世界坐标系中。一号位姿q1 = [0.35, 0.2, 0.3, 0.1],t1=[0.3, 0.1, 0.1]。二号位姿q2=[-0.5, 0.4, -0.1, 0.2], t2=[-0.1, 0.5, 0.3].某点在一号坐标系下坐标为p=[0.5, 0, 0.2].求p在二号坐标系下的坐标
假设在世界坐标系中p点的坐标为P。
用四元数做旋转则有(在Eigen中四元数旋转为q×v,数学中则为q×v×q^-1):
- q1 × P + t1 = p1
- q2 × P + t2 = p2
由上两式分别解算出:
- P = q1^-1 × (p1 - t1)
- P = q2^-1 × (p2 - t2)
两式联立求解则得到:
p2 = q2 × q1^-1 × (p1 - t1) + t2
如果用欧拉矩阵(设一号欧拉矩阵为T1,二号欧拉矩阵为T2)则有:
- p1 = T1 × P
- p2 = T2 × P
求解P:
- P = T1^-1 × p1
- P = T2^-1 × p2
联立求解则有:
p2 = T2 × T1^-1 × p1
以下则是用Eigen实现的代码:
#include <iostream>
using namespace std;
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Geometry>
int main()
{
//四元数
Eigen::Quaterniond q1 = Eigen::Quaterniond(0.35, 0.2, 0.3, 0.1).normalized();
Eigen::Quaterniond q2 = Eigen::Quaterniond(-0.5, 0.4, -0.1, 0.2).normalized();
//平移向量
Eigen::Vector3d t1 = Eigen::Vector3d(0.3, 0.1, 0.1);
Eigen::Vector3d t2 = Eigen::Vector3d(-0.1, 0.5, 0.3);
//目标向量
Eigen::Vector3d p1 = Eigen::Vector3d(0.5, 0, 0.2);
Eigen::Vector3d p2;
//打印输出
// cout << q1.coeffs() << "\n"
// << q2.coeffs() << "\n"
// << t1.transpose() << "\n"
// << t2.transpose() << endl;
//四元数求解
p2 = q2 * q1.inverse() * (p1 - t1) + t2;
cout << 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 * T1.inverse() * p1;
cout << p2.transpose() << endl;
}
最新文章
- Python爬虫代理池
- HTTP Header
- multiparty
- javascript事件处理解析
- mysql启动报错
- java 对excel操作导入excel数据到数据库
- poj 2778 DNA Sequence ac自动机+矩阵快速幂
- UISegmetControl
- IRQL_NOT_LESS_OR_EQUAL的问题最终算攻克了
- hdu 5248 序列变换(二分枚举)
- 使用float属性布局时父元素高度不能自适应的解决方法
- Android 旋转字体 实现(应用角标,如:新,火等关键字)
- php正则表达式的基本语法
- git版本控制 for window安装和命令行使用
- java :equals()和hashcode()方法的结合使用
- python核心语法
- Tomcat的日志分割三种方法
- Excel vba map/dictionary
- Spring注解之 @EnableScheduling计划任务注解
- MVC中通过ajax判断输入的内容是否重复(新手笔记,请各位多多指教)
热门文章
- Host xxx is not allowed to connect to this MariaDb server
- 如何写一个 Burp 插件
- CSS高度坍塌问题的原因以及解决办法
- js 元素offset,client , scroll 三大系列总结
- ieda与svn的配置与使用
- Python线程学习
- DP+滚动数组 || [Usaco2007 Nov]Telephone Wire 架设电话线 || BZOJ 1705 || Luogu P2885
- idea 导出可以直接运行的jar 文件
- css不常见属性之pointer-events
- springMVC项目访问URL链接时遇到某一段然后忽略后面的部分