（《視覺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）：
由上兩式分別解算出：
兩式聯立求解則得到：
p2 = q2 × q1^-1 × (p1 - t1) + t2
如果用尤拉矩陣（設一號尤拉矩陣為t1，二號尤拉矩陣為t2）則有：
求解p：
聯立求解則有：
p2 = t2 × t1^-1 × p1
以下則是用eigen實現的**：
#include using namespace std;
#include #include int main()
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在二號座標系下的座標 假...
eigen座標變換 座標變換
位姿變換 位姿在不同座標系中變換,更常用 posest or t,關鍵在於同乙個位姿 實際位姿固定不變 但是在不同座標系有不同的 表 示！座標系變換 座標系本身變換 並且 coordst coordst t.inverse 注意 ros tf 發布的變換是座標系變換 coordst,包括通過 tf2...
KDL與Eigen資料轉換
在ros使用過程中，kdl和eigen是使用比較多的兩個庫了，兩個庫都各有優勢。比如kdl是針對機械人運動學的，可以用來求解正逆向運動學，速度 雅克比矩陣等，也支援力的表述，很方便，但對矩陣運算卻不是那麼方便，eigen有著強大的矩陣計算能力。其實kdl內部有些資料結構也是採用eigen的，比如jn...