python - 如何计算姿势图优化器（slam）？

Question

现在我正在尝试为 slam 算法实现一个位姿图。为此，我看过一些关于它的视频，并找到了一个我现在想扩展的示例计算x, y, theta。

我看过的视频是

https://www.youtube.com/watch?v=uHbRKvD8TWg&t=3273s

幸运的是，我能够在 python 中实现他的示例：

import numpy as np
x1 = 0
x2 = 0

z = 1
omega = np.array([2])
e = np.array([z-(x2-x1)]) # 1-(0-0) = 1
J = np.array([1, -1])
b = np.transpose(e)*omega*J
H = np.add(np.array([[1], [-1]])*omega*J, np.array([[1, 0], [0, 0]]))
print(H)
xdelta = -np.linalg.inv(H).dot(b)
print(xdelta)

此外，我能够以令人满意的响应实现第二维：

import numpy as np

x1 = np.array([0,0])
x2 = np.array([0,0])

z = np.array([0,1])
omega = np.array([[2,2],[2,2]])
e = np.array(z-(x2-x1))

J = np.array([[1,1], [-1, -1]])
b = e.T*omega*J
print(b)
H = np.add(J.T*omega*J, np.array([[1, 0], [0, 0]]))
print(H)
xdelta = -np.linalg.inv(H).dot(b)
print(xdelta)

但是当我试图将 theta 添加到它时，我没有成功：

import numpy as np

x1 = np.array([0,0,0])
x2 = np.array([0,0,0])

z = np.array([0,1,0])
e = np.array(z-(x2-x1))

J = np.array([[1,1,1], [-1, -1,-1]])
b = e.T*J
print(J.T)
print(J)
H = np.add(J.dot(J.T), np.array([[1, 0], [0, 0]]))
print(H)
xdelta = -np.linalg.inv(H).dot(b)
print(xdelta)

现在我想要求对此示例进行实施。