eigen四元数进⾏坐标旋转
（《视觉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
由上两式分别解算出： pwi
P = q1^-1 × (p1 - t1)
P = q2^-1 × (p2 - t2)
ism
两式联⽴求解则得到：
p2 = q2 × q1^-1 × (p1 - t1) + t2
如果⽤欧拉矩阵（设⼀号欧拉矩阵为T1，⼆号欧拉矩阵为T2）则有：
p1 = T1 × P
p2 = T2 × P
求解P：
P = T1^-1 × p1
breanna
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();
demand的用法//平移向量
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.transpo() << "\n"
// << t2.transpo() << endl;
//四元数求解
p2 = q2 * q1.inver() * (p1 - t1) + t2;
cout << p2.transpo() << endl;
//欧拉矩阵
Eigen::Isometry3d T1 = Eigen::Isometry3d::Identity();
Eigen::Isometry3d T2 = Eigen::Isometry3d::Identity();
T1.pretranslate(t1);
白雪公主七个小矮人T2.pretranslate(t2);
二年级英语
// cout << T1.matrix() << endl;
// cout << T2.matrix() << endl;
//欧拉矩阵求解
p2 = T2 * T1.inver() * p1;
cout << p2.transpo() << endl;
}
//////////////////////////////////////////////////////////////////////////////////////
以上使⽤变换矩阵Eigen::Isometry3d，下⾯使⽤四元数、旋转矩阵进⾏坐标变换Eigen::Quaterniond
guarantee
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;mr jones怎么读
Eigen::Matrix3d
Eigen::Matrix3d r = (Eigen::AngleAxisd(3.141593 / 2.0, Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX())).matrix();
Eigen::Vector3d t1 = Eigen::Vector3d(2, 2, 0.0);
Eigen::Vector3d p1 = Eigen::Vector3d(1, 0.0, 0.0);
Eigen::Vector3d pw;
pw = r * p1 + t1;
