Eigen實現座標轉換

2021-08-20 07:26:37 字數 606 閱讀 6459

(《視覺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...