歐式空間6維位姿，與其變換矩陣，二者之間相互轉換
下面貼出來python和c++**，先貼出來的是api函式，可以直接呼叫。不會用的人可以看最後，有使用例子
def
getpose_fromt
(t):
x = t[0,
3]y = t[1,
3]z = t[2,
3]rx = math.atan2(t[2,
1], t[2,
2]) ry = math.asin(
-t[2,0
])rz = math.atan2(t[1,
0], t[0,
0])return x, y, z, rx, ry, rz
defgett_frompose
(x, y, z, rx, ry, rz)
: rx = np.mat([[
1,0,
0],[
0, math.cos(rx)
,-math.sin(rx)],
[0, math.sin(rx)
, math.cos(rx)]]
) ry = np.mat(
[[math.cos(ry),0
, math.sin(ry)],
[0,1
,0],
[-math.sin(ry),0
, math.cos(ry)]]
) rz = np.mat(
[[math.cos(rz)
,-math.sin(rz),0
],[math.sin(rz)
, math.cos(rz),0
],[0
,0,1
]]) t = np.mat(
[[x]
,[y]
,[z]])
r = rz * ry * rx
r_ = np.array(r)
t_ = np.array(t)1)
zero = np.mat([0
,0,0
,1])
t_2 = np.array(zero)0)
t = np.mat(t)
return t
eigen::matrixxd gett_frompose
(double x,
double y,
double z,
double rx,
double ry,
double rz)
std::vector<
double
>
getpose_fromt
(eigen::matrixxd t)
import math
import numpy as np
import scipy.linalg as la
defgetpose_fromt
(t):
x = t[0,
3] y = t[1,
3]z = t[2,
3]rx = math.atan2(t[2,
1], t[2,
2]) ry = math.asin(
-t[2,0
]) rz = math.atan2(t[1,
0], t[0,
0])return x, y, z, rx, ry, rz
defgett_frompose
(x, y, z, rx, ry, rz)
: rx = np.mat([[
1,0,
0],[
0, math.cos(rx)
,-math.sin(rx)],
[0, math.sin(rx)
, math.cos(rx)]]
) ry = np.mat(
[[math.cos(ry),0
, math.sin(ry)],
[0,1
,0],
[-math.sin(ry),0
, math.cos(ry)]]
) rz = np.mat(
[[math.cos(rz)
,-math.sin(rz),0
],[math.sin(rz)
, math.cos(rz),0
],[0
,0,1
]]) t = np.mat(
[[x]
,[y]
,[z]])
r = rz * ry * rx
r_ = np.array(r)
t_ = np.array(t)1)
zero = np.mat([0
,0,0
,1])
t_2 = np.array(zero) 0
) t = np.mat(t)
return t
# t2 = t1 * t
defgettranst_pose2inpose1
(t1, t2)
:return t1.i * t2
t1 = gett_frompose(
-0.072944147641399,-
0.06687830562048944
,0.4340418493881254,-
0.2207496117519063
,0.0256862005614321
,0.1926014162476009
)print
(t1)
x1, y1, z1, rx1, ry1, rz1 = getpose_fromt(t1)
print
(x1, y1, z1, rx1, ry1, rz1)
t = np.mat([[
0.6373552241213387
,0.4795294143719509,-
0.6031831057293726
,5590696.786710221],
[0.346752148464638
,0.5205619206440493
,0.7802424202198545,-
11216440.57810941],
[0.6881433468547054,-
0.7064466204374341
,0.1655050049156589
,46487322.53149694],
[0,0
,0,1
]])p = getpose_fromt(t)
print
(p)
#include
#include
#include
"eigen/dense"
eigen::matrixxd gett_frompose
(double x,
double y,
double z,
double rx,
double ry,
double rz)
std::vector<
double
>
getpose_fromt
(eigen::matrixxd t)
intmain
(int argc,
char
** ar**)
return1;
}
矩陣論筆記（三） 歐氏空間與正交變換
包括兩種內積空間 1 實內積空間 歐氏空間 2 復內積空間 酉空間 本節講歐氏空間，包括四個部分 1 歐氏空間 2 正交性 3 正交變換與正交矩陣 4 對稱變換與對稱矩陣 歐氏空間即是實內積空間 定義 1 歐氏空間 實數域上的 v 定義兩向量到實數的對映 x,y 滿足交換律 分配率 齊次性 非負性，...
歐氏空間內積定義 子空間與對稱變換
摘要 本節主要介紹歐氏空間中子空間與對稱變換在考研中的考察，對於子空間的考察而言，更多的側重於考察正交補空間 而對於對稱變換，大家一定要熟記定義，看到對稱變換的時候，知道如何使用定義去處理題目，達到解決問題的目的.定義1.設 則稱為正交的，記為 乙個向量 如果對於任意的 則稱 與子空間 記作因為只有...
矩陣理論第一章 歐氏空間與酉空間
1.內積的定義 正定性 a,a 0，等於0當時且僅當a 0 齊次性 ka,b k的共軛 a,b 交換律 a,b b,a 取共軛 分配律雙線性 a，k1b1 k2b2 k1 a,b1 k2 a,b2 k1a1 k2a2，b k1共軛 a1,b k2共軛 a2,b 2.判斷是不是內積 檢視是否滿足正定，...