github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-04-22 03:00:22 -04:00
Code Issues Packages Projects Releases Wiki Activity

Using utility rot_mat_2d (#683)

Browse Source
This commit is contained in:
Atsushi Sakai
2022-05-22 16:53:14 +09:00
committed by GitHub
parent 31178c8e37
commit dc879da7b2
10 changed files with 70 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
SLAM/GraphBasedSLAM/graph_based_slam.py
View File

@@ -10,6 +10,9 @@ Ref
(http://www2.informatik.uni-freiburg.de/~stachnis/pdf/grisetti10titsmag.pdf)
"""
import sys
import os
sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../utils/")
import copy
import itertools
@@ -63,7 +66,7 @@ def cal_observation_sigma():
return sigma
def calc_rotational_matrix(angle):
def calc_3d_rotational_matrix(angle):
return Rot.from_euler('z', angle).as_matrix()
@@ -82,8 +85,8 @@ def calc_edge(x1, y1, yaw1, x2, y2, yaw2, d1,
edge.e[1, 0] = y2 - y1 - tmp3 + tmp4
edge.e[2, 0] = 0
Rt1 = calc_rotational_matrix(tangle1)
Rt2 = calc_rotational_matrix(tangle2)
Rt1 = calc_3d_rotational_matrix(tangle1)
Rt2 = calc_3d_rotational_matrix(tangle2)
sig1 = cal_observation_sigma()
sig2 = cal_observation_sigma()