如何为姿势图优化器大满贯进行数学计算？

问题描述

现在我正在尝试为 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,0]]))
print(H)
xdelta = -np.linalg.inv(H).dot(b)
print(xdelta)

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

import numpy as np

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

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

现在我想要求对这个示例进行实现。

解决方法

暂无找到可以解决该程序问题的有效方法，小编努力寻找整理中！

如果你已经找到好的解决方法，欢迎将解决方案带上本链接一起发送给小编。

小编邮箱:dio#foxmail.com (将#修改为@）