SLAM学习三第三章习题7解

2019-03-17 本文已影响0人打出了枫采

原题：
小萝卜一号，位姿为 $q_1=[0.35,0.2,0.3,0.1]$ ， $t_1=[0.3,0.1,0.1]^T$ ，
小萝卜二号，位姿为 $q_2=[-0.5,0.4,-0.1,0.2]$ ， $t_2=[-0.1,0.5,0.3]^T$ ，
小萝卜一号看到某点在自身坐标系下坐标为 $p_1=[0.5,0.0.2]^T$ ，求该点在小萝卜二号坐标系下的坐标。

两点需要注意：

单位四元数可以用于表示三维空间的任意旋转，所以对于 $q_1,q_2$ 需要做归一化处理；

用四元数做位姿变换求解时，坐标也点应转换成四元数形式。

$p_0$ 为该点在世界系下坐标点， $p_2$ 为该点在小萝卜二号坐标系下的坐标
用 $m$ 表示坐标点对应四元数， $m_1=[0,p_1]$

$m_1=q_1·m_0·q_1^{-1} + m_{t_1}$
$m_2=q_2·m_0·q_2^{-1} + m_{t_2}$
即： $m_2 = q_2·(q_1^{-1}(m_1-m_{t_1})q_1)·q_2^{-1}+m_{t_2}$

利用eigen库函数及四元数运算规则，即可编程实现，暂略。

上一篇下一篇

猜你喜欢

热点阅读