安裝
- 先安裝pybind11苹丸，源碼安裝色瘩，apt安裝會報錯
git clone https://github.com/pybind/pybind11.git
cd pybind11 && mkdir build && cd build && cmake .. && make -j24
sudo make install
- 安裝sophuspy
pip install sophuspy
使用
import numpy as np
import sophus as sp
from scipy.spatial.transform import Rotation as R
def print_se(name, se):
euler = R.from_matrix(se.rotationMatrix()).as_euler("xyz", degrees=True)
print("{}:\nt:{},r{},log{}".format(name, np.round(
se.translation(), 2), np.round(euler, 2),np.round(se.log(), 2)))
r0 = R.from_euler("xyz", [0, 0, 0], degrees=True)
s1 = sp.SE3(r0.as_matrix(), [1, 0, 0])
print_se("s1", s1)
r90 = R.from_euler("xyz", [0, 0, 90], degrees=True)
s90 = sp.SE3(r90.as_matrix(), [0, 1, 0])
print_se("s90", s90)
# 先將s1繞Z逆時針旋轉(zhuǎn)90度采驻，再y+1
print_se("s90 * s1", s90 * s1)
# 將s90 x+1
print_se("s1 * s90", s1 * s90)
更多SophusPy使用方式參照
https://github.com/craigstar/SophusPy
