mirror of
https://github.com/AtsushiSakai/PythonRobotics.git
synced 2026-04-22 03:00:22 -04:00
release PoseOptimizationSLAM3D
This commit is contained in:
Atsushi Sakai
@@ -38,7 +38,6 @@ MAXY = 25.0
|
||||
NOISE_RANGE = 2.0 # [m] 1σ range noise parameter
|
||||
NOISE_SPEED = 0.5 # [m/s] 1σ speed noise parameter
|
||||
|
||||
|
||||
show_animation = True
|
||||
|
||||
|
||||
@@ -66,11 +65,10 @@ def histogram_filter_localization(grid_map, u, z, yaw):
|
||||
|
||||
|
||||
def calc_gaussian_observation_pdf(gmap, z, iz, ix, iy, std):
|
||||
|
||||
# predicted range
|
||||
x = ix * gmap.xy_reso + gmap.minx
|
||||
y = iy * gmap.xy_reso + gmap.miny
|
||||
d = math.sqrt((x - z[iz, 1])**2 + (y - z[iz, 2])**2)
|
||||
d = math.sqrt((x - z[iz, 1]) ** 2 + (y - z[iz, 2]) ** 2)
|
||||
|
||||
# likelihood
|
||||
pdf = (1.0 - norm.cdf(abs(d - z[iz, 0]), 0.0, std))
|
||||
@@ -79,7 +77,6 @@ def calc_gaussian_observation_pdf(gmap, z, iz, ix, iy, std):
|
||||
|
||||
|
||||
def observation_update(gmap, z, std):
|
||||
|
||||
for iz in range(z.shape[0]):
|
||||
for ix in range(gmap.xw):
|
||||
for iy in range(gmap.yw):
|
||||
@@ -99,7 +96,6 @@ def calc_input():
|
||||
|
||||
|
||||
def motion_model(x, u):
|
||||
|
||||
F = np.array([[1.0, 0, 0, 0],
|
||||
[0, 1.0, 0, 0],
|
||||
[0, 0, 1.0, 0],
|
||||
@@ -122,7 +118,6 @@ def draw_heat_map(data, mx, my):
|
||||
|
||||
|
||||
def observation(xTrue, u, RFID):
|
||||
|
||||
xTrue = motion_model(xTrue, u)
|
||||
|
||||
z = np.zeros((0, 3))
|
||||
@@ -131,7 +126,7 @@ def observation(xTrue, u, RFID):
|
||||
|
||||
dx = xTrue[0, 0] - RFID[i, 0]
|
||||
dy = xTrue[1, 0] - RFID[i, 1]
|
||||
d = math.sqrt(dx**2 + dy**2)
|
||||
d = math.sqrt(dx ** 2 + dy ** 2)
|
||||
if d <= MAX_RANGE:
|
||||
# add noise to range observation
|
||||
dn = d + np.random.randn() * NOISE_RANGE
|
||||
@@ -146,7 +141,6 @@ def observation(xTrue, u, RFID):
|
||||
|
||||
|
||||
def normalize_probability(gmap):
|
||||
|
||||
sump = sum([sum(igmap) for igmap in gmap.data])
|
||||
|
||||
for ix in range(gmap.xw):
|
||||
@@ -214,11 +208,11 @@ def calc_grid_index(gmap):
|
||||
def main():
|
||||
print(__file__ + " start!!")
|
||||
|
||||
# RFID positions [x, y]
|
||||
RFID = np.array([[10.0, 0.0],
|
||||
[10.0, 10.0],
|
||||
[0.0, 15.0],
|
||||
[-5.0, 20.0]])
|
||||
# RF_ID positions [x, y]
|
||||
RF_ID = np.array([[10.0, 0.0],
|
||||
[10.0, 10.0],
|
||||
[0.0, 15.0],
|
||||
[-5.0, 20.0]])
|
||||
|
||||
time = 0.0
|
||||
|
||||
@@ -233,7 +227,7 @@ def main():
|
||||
u = calc_input()
|
||||
|
||||
yaw = xTrue[2, 0] # Orientation is known
|
||||
xTrue, z, ud = observation(xTrue, u, RFID)
|
||||
xTrue, z, ud = observation(xTrue, u, RF_ID)
|
||||
|
||||
grid_map = histogram_filter_localization(grid_map, u, z, yaw)
|
||||
|
||||
@@ -241,10 +235,10 @@ def main():
|
||||
plt.cla()
|
||||
draw_heat_map(grid_map.data, mx, my)
|
||||
plt.plot(xTrue[0, :], xTrue[1, :], "xr")
|
||||
plt.plot(RFID[:, 0], RFID[:, 1], ".k")
|
||||
plt.plot(RF_ID[:, 0], RF_ID[:, 1], ".k")
|
||||
for i in range(z.shape[0]):
|
||||
plt.plot([xTrue[0, :], z[i, 1]], [
|
||||
xTrue[1, :], z[i, 2]], "-k")
|
||||
xTrue[1, :], z[i, 2]], "-k")
|
||||
plt.title("Time[s]:" + str(time)[0: 4])
|
||||
plt.pause(0.1)
|
||||
|
||||
|
||||
15
README.md
15
README.md
@@ -609,19 +609,6 @@ This is a list: [Users comments](https://github.com/AtsushiSakai/PythonRobotics/
|
||||
|
||||
- [Alexis Paques](https://github.com/AlexisTM)
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
- [Ryohei Sasaki](https://github.com/rsasaki0109)
|
||||
|
||||
|
||||
|
||||
@@ -15,7 +15,11 @@ python pose_optimization_slam_3d.py
|
||||
~~~
|
||||
|
||||
## Reference
|
||||
[A Compact and Portable Implementation of Graph\-based SLAM](https://www.researchgate.net/publication/321287640_A_Compact_and_Portable_Implementation_of_Graph-based_SLAM)
|
||||
[GitHub \- furo\-org/p2o: Single header 2D/3D graph\-based SLAM library](https://github.com/furo-org/p2o)
|
||||
[GitHub \- AtsushiSakai/PythonRobotics
|
||||
- [A Compact and Portable Implementation of Graph\-based SLAM](https://www.researchgate.net/publication/321287640_A_Compact_and_Portable_Implementation_of_Graph-based_SLAM)
|
||||
|
||||
- [GitHub \- furo\-org/p2o: Single header 2D/3D graph\-based SLAM library](https://github.com/furo-org/p2o)
|
||||
|
||||
- [GitHub \- AtsushiSakai/PythonRobotics
|
||||
/SLAM/PoseOptimizationSLAM](https://github.com/AtsushiSakai/PythonRobotics/tree/master/SLAM/PoseOptimizationSLAM)
|
||||
|
||||
- [リー代数による3次元ポーズ調整\(Pose Adjustment\)\[PythonRobotics\]\[SLAM\] \- Qiita](https://qiita.com/saitosasaki/items/a0540cba994f08bf5e16#comment-3e6588e6b096cc2567d8)
|
||||
|
||||
@@ -6,16 +6,21 @@ author: Atsushi Sakai
|
||||
|
||||
"""
|
||||
|
||||
|
||||
import subprocess
|
||||
|
||||
|
||||
def main():
|
||||
print("start!!")
|
||||
|
||||
cmd = "wget https://www.dropbox.com/s/zu23p8d522qccor/parking-garage.g2o?dl=0 -O parking-garage.g2o -nc"
|
||||
subprocess.call(cmd, shell=True)
|
||||
cmd = "wget http://www.furo.org/irie/datasets/torus3d_guess.g2o -nc"
|
||||
subprocess.call(cmd, shell=True)
|
||||
cmd = "wget http://www.furo.org/irie/datasets/sphere2200_guess.g2o -nc"
|
||||
subprocess.call(cmd, shell=True)
|
||||
|
||||
print("done!!")
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
main()
|
||||
|
||||
@@ -1,40 +1,45 @@
|
||||
"""
|
||||
3D (x, y, z, qw, qx, qy, qz) pose optimization SLAM
|
||||
|
||||
author: Ryohei Sasaki(@rsasaki0109)
|
||||
|
||||
Ref:
|
||||
|
||||
- [A Compact and Portable Implementation of Graph\-based SLAM](https://www.researchgate.net/publication/321287640_A_Compact_and_Portable_Implementation_of_Graph-based_SLAM)
|
||||
|
||||
- [GitHub \- furo\-org/p2o: Single header 2D/3D graph\-based SLAM library](https://github.com/furo-org/p2o)
|
||||
|
||||
- [GitHub \- AtsushiSakai/PythonRobotics
|
||||
/SLAM/PoseOptimizationSLAM](https://github.com/AtsushiSakai/PythonRobotics/blob/master/SLAM/PoseOptimizationSLAM/pose_optimization_slam.py)
|
||||
|
||||
|
||||
"""
|
||||
|
||||
import os
|
||||
import sys
|
||||
import time
|
||||
|
||||
import matplotlib.pyplot as plt
|
||||
import numpy as np
|
||||
from scipy import sparse
|
||||
from scipy.sparse import linalg
|
||||
import matplotlib.pyplot as plt
|
||||
from mpl_toolkits.mplot3d import Axes3D
|
||||
|
||||
est_traj_fig = plt.figure()
|
||||
ax = est_traj_fig.add_subplot(111, projection='3d')
|
||||
|
||||
def skew_symmetric(v):
|
||||
|
||||
return np.array(
|
||||
[[0, -v[2], v[1]],
|
||||
[v[2], 0, -v[0]],
|
||||
[-v[1], v[0], 0]]
|
||||
)
|
||||
|
||||
def robust_coeff(squared_error, delta):
|
||||
|
||||
if (squared_error < 0):
|
||||
def robust_coeff(squared_error, delta):
|
||||
if squared_error < 0:
|
||||
return 0
|
||||
sqre = np.sqrt(squared_error)
|
||||
if (sqre < delta):
|
||||
return 1 # no effect
|
||||
return delta / sqre # linear
|
||||
if sqre < delta:
|
||||
return 1 # no effect
|
||||
return delta / sqre # linear
|
||||
|
||||
|
||||
class Optimizer3D:
|
||||
@@ -90,14 +95,15 @@ class Optimizer3D:
|
||||
for con in constraints:
|
||||
ida = con.id1
|
||||
idb = con.id2
|
||||
assert 0 <= ida and ida < numnodes, "ida is invalid"
|
||||
assert 0 <= idb and idb < numnodes, "idb is invalid"
|
||||
assert 0 <= ida < numnodes, "ida is invalid"
|
||||
assert 0 <= idb < numnodes, "idb is invalid"
|
||||
pa = graph_nodes[ida]
|
||||
pb = graph_nodes[idb]
|
||||
r, Ja, Jb = self.calc_error(
|
||||
pa, pb, con.t)
|
||||
|
||||
info_mat = con.info_mat * robust_coeff(r.reshape(self.dim,1).T @ con.info_mat @ r.reshape(self.dim,1), self.robust_delta)
|
||||
|
||||
info_mat = con.info_mat * robust_coeff(r.reshape(self.dim, 1).T @ con.info_mat @ r.reshape(self.dim, 1),
|
||||
self.robust_delta)
|
||||
|
||||
trJaInfo = Ja.transpose() @ info_mat
|
||||
trJaInfoJa = trJaInfo @ Ja
|
||||
@@ -115,10 +121,10 @@ class Optimizer3D:
|
||||
ida * self.dim + k, idb * self.dim + m, trJaInfoJb[k, m])
|
||||
tripletList.push_back(
|
||||
idb * self.dim + k, ida * self.dim + m, trJaInfoJb[m, k])
|
||||
|
||||
bf[ida * self.dim: (ida + 1) * self.dim ] += trJaInfo @ r
|
||||
bf[idb * self.dim: (idb + 1) * self.dim ] += trJbInfo @ r
|
||||
|
||||
|
||||
bf[ida * self.dim: (ida + 1) * self.dim] += trJaInfo @ r
|
||||
bf[idb * self.dim: (idb + 1) * self.dim] += trJbInfo @ r
|
||||
|
||||
for k in indlist:
|
||||
tripletList.push_back(k, k, self.init_w)
|
||||
|
||||
@@ -133,18 +139,17 @@ class Optimizer3D:
|
||||
out_nodes = []
|
||||
|
||||
for i in range(len(graph_nodes)):
|
||||
|
||||
u_i = i * self.dim
|
||||
|
||||
|
||||
q_before = Quaternion(graph_nodes[i].qw, graph_nodes[i].qx, graph_nodes[i].qy, graph_nodes[i].qz)
|
||||
rv_before = RotVec(quaternion = q_before)
|
||||
rv_after = RotVec(ax = rv_before.ax + x[u_i + 3], ay = rv_before.ay + x[u_i + 4], az = rv_before.az + x[u_i + 5])
|
||||
q_after = rv_after.toQuaternion()
|
||||
rv_before = RotVec(quaternion=q_before)
|
||||
rv_after = RotVec(ax=rv_before.ax + x[u_i + 3], ay=rv_before.ay + x[u_i + 4], az=rv_before.az + x[u_i + 5])
|
||||
q_after = rv_after.to_quaternion()
|
||||
|
||||
pos = Pose3D(
|
||||
graph_nodes[i].x + x[u_i],
|
||||
graph_nodes[i].y + x[u_i + 1],
|
||||
graph_nodes[i].z + x[u_i + 2],
|
||||
graph_nodes[i].x + x[u_i],
|
||||
graph_nodes[i].y + x[u_i + 1],
|
||||
graph_nodes[i].z + x[u_i + 2],
|
||||
q_after.qw,
|
||||
q_after.qx,
|
||||
q_after.qy,
|
||||
@@ -161,16 +166,18 @@ class Optimizer3D:
|
||||
cost = 0.0
|
||||
for c in constraints:
|
||||
diff = self.error_func(nodes[c.id1], nodes[c.id2], c.t)
|
||||
info_mat = c.info_mat * robust_coeff(diff.reshape(self.dim,1).T @ c.info_mat @ diff.reshape(self.dim,1), self.robust_delta)
|
||||
info_mat = c.info_mat * robust_coeff(diff.reshape(self.dim, 1).T @ c.info_mat @ diff.reshape(self.dim, 1),
|
||||
self.robust_delta)
|
||||
cost += diff.transpose() @ info_mat @ diff
|
||||
|
||||
return cost
|
||||
|
||||
def error_func(self, pa, pb, t):
|
||||
@staticmethod
|
||||
def error_func(pa, pb, t):
|
||||
|
||||
ba = pb.ominus(pa)
|
||||
q = t.rv().toQuaternion().conjugate().quat_mult(ba.rv().toQuaternion(), out = 'Quaternion')
|
||||
drv = RotVec(quaternion = q)
|
||||
q = t.rv().to_quaternion().conjugate().quat_mult(ba.rv().to_quaternion(), out='Quaternion')
|
||||
drv = RotVec(quaternion=q)
|
||||
error = np.array([ba.x - t.x,
|
||||
ba.y - t.y,
|
||||
ba.z - t.z,
|
||||
@@ -179,156 +186,177 @@ class Optimizer3D:
|
||||
drv.az[0]])
|
||||
return error
|
||||
|
||||
def dQuat_dRV(self, rv):
|
||||
|
||||
u1 = rv.ax; u2 = rv.ay; u3 = rv.az
|
||||
v = np.sqrt(u1**2 + u2**2 + u3**2)
|
||||
@staticmethod
|
||||
def dQuat_dRV(rv):
|
||||
u1 = rv.ax
|
||||
u2 = rv.ay
|
||||
u3 = rv.az
|
||||
v = np.sqrt(u1 ** 2 + u2 ** 2 + u3 ** 2)
|
||||
if v < 1e-6:
|
||||
dqu = 0.25 * np.array(
|
||||
[[ -u1, -u2, -u3],
|
||||
[ 2.0, 0.0, 0.0],
|
||||
[ 0.0, 2.0, 0.0],
|
||||
[ 0.0, 0.0, 2.0]]
|
||||
[[-u1, -u2, -u3],
|
||||
[2.0, 0.0, 0.0],
|
||||
[0.0, 2.0, 0.0],
|
||||
[0.0, 0.0, 2.0]]
|
||||
)
|
||||
return dqu
|
||||
|
||||
vd = v*2
|
||||
v2 = v**2
|
||||
v3 = v**3
|
||||
vd = v * 2
|
||||
v2 = v ** 2
|
||||
v3 = v ** 3
|
||||
|
||||
S = np.sin(v/2.0); C = np.cos(v/2.0)
|
||||
S = np.sin(v / 2.0)
|
||||
C = np.cos(v / 2.0)
|
||||
dqu = np.array(
|
||||
[[ -u1 * S/vd , -u2*S/vd , -u3*S/vd],
|
||||
[ S/v + u1*u1*C/(2*v2) - u1*u1*S/v3, u1*u2*(C/(2*v2)-S/v3) , u1*u3*(C/(2*v2)-S/v3)],
|
||||
[ u1*u2*(C/(2*v2)-S/v3) , S/v+u2*u2*C/(2*v2)-u2*u2*S/v3, u2*u3*(C/(2*v2)-S/v3)],
|
||||
[ u1*u3*(C/(2*v2)-S/v3) , u2*u3*(C/(2*v2)-S/v3) , S/v+u3*u3*C/(2*v2)-u3*u3*S/v3]]
|
||||
)
|
||||
[[-u1 * S / vd, -u2 * S / vd, -u3 * S / vd],
|
||||
[S / v + u1 * u1 * C / (2 * v2) - u1 * u1 * S / v3, u1 * u2 * (C / (2 * v2) - S / v3),
|
||||
u1 * u3 * (C / (2 * v2) - S / v3)],
|
||||
[u1 * u2 * (C / (2 * v2) - S / v3), S / v + u2 * u2 * C / (2 * v2) - u2 * u2 * S / v3,
|
||||
u2 * u3 * (C / (2 * v2) - S / v3)],
|
||||
[u1 * u3 * (C / (2 * v2) - S / v3), u2 * u3 * (C / (2 * v2) - S / v3),
|
||||
S / v + u3 * u3 * C / (2 * v2) - u3 * u3 * S / v3]]
|
||||
)
|
||||
|
||||
return dqu
|
||||
|
||||
def dR_dRV(self, rv):
|
||||
|
||||
q = rv.toQuaternion()
|
||||
qw = q.qw;qx = q.qx; qy = q.qy;qz = q.qz
|
||||
q = rv.to_quaternion()
|
||||
qw = q.qw
|
||||
qx = q.qx
|
||||
qy = q.qy
|
||||
qz = q.qz
|
||||
dRdqw = 2 * np.array(
|
||||
[[ qw, -qz, qy],
|
||||
[ qz, qw, -qx],
|
||||
[-qy, qx, qw]]
|
||||
[[qw, -qz, qy],
|
||||
[qz, qw, -qx],
|
||||
[-qy, qx, qw]]
|
||||
)
|
||||
dRdqx = 2 * np.array(
|
||||
[[ qx, qy, qz],
|
||||
[ qy, -qx, -qw],
|
||||
[ qz, qw, -qx]]
|
||||
[[qx, qy, qz],
|
||||
[qy, -qx, -qw],
|
||||
[qz, qw, -qx]]
|
||||
)
|
||||
dRdqy = 2 * np.array(
|
||||
[[-qy, qx, qw],
|
||||
[ qx, qy, qz],
|
||||
[-qw, qz, -qy]]
|
||||
[[-qy, qx, qw],
|
||||
[qx, qy, qz],
|
||||
[-qw, qz, -qy]]
|
||||
)
|
||||
dRdqz = 2 * np.array(
|
||||
[[-qz, -qw, qx],
|
||||
[ qw, -qz, -qy],
|
||||
[ qz, qy, qz]]
|
||||
[[-qz, -qw, qx],
|
||||
[qw, -qz, -qy],
|
||||
[qz, qy, qz]]
|
||||
)
|
||||
dqdu = self.dQuat_dRV(rv)
|
||||
dux = dRdqw * dqdu[0,0] + dRdqx * dqdu[1, 0] + dRdqy * dqdu[2, 0] + dRdqz * dqdu[3, 0]
|
||||
duy = dRdqw * dqdu[0,1] + dRdqx * dqdu[1, 1] + dRdqy * dqdu[2, 1] + dRdqz * dqdu[3, 1]
|
||||
duz = dRdqw * dqdu[0,2] + dRdqx * dqdu[1, 2] + dRdqy * dqdu[2, 2] + dRdqz * dqdu[3, 2]
|
||||
dux = dRdqw * dqdu[0, 0] + dRdqx * dqdu[1, 0] + dRdqy * dqdu[2, 0] + dRdqz * dqdu[3, 0]
|
||||
duy = dRdqw * dqdu[0, 1] + dRdqx * dqdu[1, 1] + dRdqy * dqdu[2, 1] + dRdqz * dqdu[3, 1]
|
||||
duz = dRdqw * dqdu[0, 2] + dRdqx * dqdu[1, 2] + dRdqy * dqdu[2, 2] + dRdqz * dqdu[3, 2]
|
||||
return dux, duy, duz
|
||||
|
||||
def dRV_dQuat(self, q):
|
||||
|
||||
|
||||
@staticmethod
|
||||
def dRV_dQuat(q):
|
||||
|
||||
qw = q.qw[0]
|
||||
qx = q.qx[0]
|
||||
qy = q.qy[0]
|
||||
qz = q.qz[0]
|
||||
|
||||
if 1 - qw**2 < 1e-7:
|
||||
if 1 - qw ** 2 < 1e-7:
|
||||
ret = np.array(
|
||||
[[ 0.0, 2.0, 0.0, 0.0],
|
||||
[ 0.0, 0.0, 2.0, 0.0],
|
||||
[ 0.0, 0.0, 0.0, 2.0]]
|
||||
[[0.0, 2.0, 0.0, 0.0],
|
||||
[0.0, 0.0, 2.0, 0.0],
|
||||
[0.0, 0.0, 0.0, 2.0]]
|
||||
)
|
||||
return ret
|
||||
|
||||
c = 1/(1 - qw**2)
|
||||
d = np.arccos(qw)/(np.sqrt(1-qw**2))
|
||||
|
||||
c = 1 / (1 - qw ** 2)
|
||||
d = np.arccos(qw) / (np.sqrt(1 - qw ** 2))
|
||||
ret = 2.0 * np.array(
|
||||
[[ c*qx*(d*qw-1), d, 0.0, 0.0],
|
||||
[ c*qy*(d*qw-1), 0.0, d, 0.0],
|
||||
[ c*qz*(d*qw-1), 0.0, 0.0, d]]
|
||||
)
|
||||
[[c * qx * (d * qw - 1), d, 0.0, 0.0],
|
||||
[c * qy * (d * qw - 1), 0.0, d, 0.0],
|
||||
[c * qz * (d * qw - 1), 0.0, 0.0, d]]
|
||||
)
|
||||
return ret
|
||||
|
||||
def QMat(self, q):
|
||||
|
||||
qw = q.qw; qx = q.qx; qy = q.qy; qz = q.qz
|
||||
qw = q.qw
|
||||
qx = q.qx
|
||||
qy = q.qy
|
||||
qz = q.qz
|
||||
Q = np.array(
|
||||
[[ qw, -qx, -qy, -qz],
|
||||
[ qx, qw, -qz, qy],
|
||||
[ qy, qz, qw, -qx],
|
||||
[ qz, -qy, qx, qw]]
|
||||
)
|
||||
[[qw, -qx, -qy, -qz],
|
||||
[qx, qw, -qz, qy],
|
||||
[qy, qz, qw, -qx],
|
||||
[qz, -qy, qx, qw]]
|
||||
)
|
||||
return Q
|
||||
|
||||
def QMatBar(self, q):
|
||||
|
||||
qw = q.qw; qx = q.qx; qy = q.qy; qz = q.qz
|
||||
qw = q.qw
|
||||
qx = q.qx
|
||||
qy = q.qy
|
||||
qz = q.qz
|
||||
Q = np.array(
|
||||
[[ qw, -qx, -qy, -qz],
|
||||
[ qx, qw, qz, -qy],
|
||||
[ qy, -qz, qw, qx],
|
||||
[ qz, qy, -qx, qw]]
|
||||
)
|
||||
[[qw, -qx, -qy, -qz],
|
||||
[qx, qw, qz, -qy],
|
||||
[qy, -qz, qw, qx],
|
||||
[qz, qy, -qx, qw]]
|
||||
)
|
||||
return Q
|
||||
|
||||
def calc_error(self, pa, pb, t):
|
||||
|
||||
e0 = self.error_func(pa, pb, t)
|
||||
Ja = np.identity(6); Jb = np.identity(6)
|
||||
Ja = np.identity(6)
|
||||
Jb = np.identity(6)
|
||||
|
||||
rva_inv = pa.rv().inverted()
|
||||
rotPaInv = rva_inv.toRotationMatrix()
|
||||
rotPaInv = rva_inv.to_rotation_matrix()
|
||||
|
||||
Ja[:3,:3] = -rotPaInv
|
||||
Jb[:3,:3] = rotPaInv
|
||||
Ja[:3, :3] = -rotPaInv
|
||||
Jb[:3, :3] = rotPaInv
|
||||
|
||||
dRux, dRuy, dRuz = self.dR_dRV(rva_inv)
|
||||
|
||||
cvec = np.array([[pb.x - pa.x],[pb.y - pa.y],[pb.z - pa.z]])
|
||||
cvec = np.array([[pb.x - pa.x], [pb.y - pa.y], [pb.z - pa.z]])
|
||||
|
||||
Ja[0:3,3:4] = -dRux @ cvec
|
||||
Ja[0:3,4:5] = -dRuy @ cvec
|
||||
Ja[0:3,5:6] = -dRuz @ cvec
|
||||
Ja[0:3, 3:4] = -dRux @ cvec
|
||||
Ja[0:3, 4:5] = -dRuy @ cvec
|
||||
Ja[0:3, 5:6] = -dRuz @ cvec
|
||||
|
||||
# rotation part: qdiff = qc-1 * qa-1 * qb
|
||||
qainv = rva_inv.toQuaternion()
|
||||
qcinv = t.rv().inverted().toQuaternion()
|
||||
qb = pb.rv().toQuaternion()
|
||||
qinvca = qcinv.quat_mult(qainv, out = 'Quaternion')
|
||||
qdiff = qinvca.quat_mult(qb, out = 'Quaternion')
|
||||
qainv = rva_inv.to_quaternion()
|
||||
qcinv = t.rv().inverted().to_quaternion()
|
||||
qb = pb.rv().to_quaternion()
|
||||
qinvca = qcinv.quat_mult(qainv, out='Quaternion')
|
||||
qdiff = qinvca.quat_mult(qb, out='Quaternion')
|
||||
|
||||
Ja[3:6,3:6] = -self.dRV_dQuat(qdiff) @ self.QMat(qcinv) @ self.QMatBar(qb) @ self.dQuat_dRV(rva_inv)
|
||||
Jb[3:6,3:6] = self.dRV_dQuat(qdiff) @ self.QMat(qcinv) @ self.QMat(qainv) @ self.dQuat_dRV(pb.rv())
|
||||
Ja[3:6, 3:6] = -self.dRV_dQuat(qdiff) @ self.QMat(qcinv) @ self.QMatBar(qb) @ self.dQuat_dRV(rva_inv)
|
||||
Jb[3:6, 3:6] = self.dRV_dQuat(qdiff) @ self.QMat(qcinv) @ self.QMat(qainv) @ self.dQuat_dRV(pb.rv())
|
||||
|
||||
return e0, Ja, Jb
|
||||
|
||||
|
||||
class Quaternion:
|
||||
|
||||
def __init__(self, qw, qx, qy, qz):
|
||||
self.qw = qw; self.qx = qx; self.qy = qy; self.qz = qz
|
||||
|
||||
self.qw = qw
|
||||
self.qx = qx
|
||||
self.qy = qy
|
||||
self.qz = qz
|
||||
|
||||
def conjugate(self):
|
||||
return Quaternion(self.qw, -self.qx, -self.qy, -self.qz)
|
||||
|
||||
|
||||
def to_numpy(self):
|
||||
return np.array([self.qw, self.qx, self.qy, self.qz]).reshape(4,1)
|
||||
return np.array([self.qw, self.qx, self.qy, self.qz]).reshape(4, 1)
|
||||
|
||||
def quat_mult(self, q, out='np'):
|
||||
v = np.array([self.qx, self.qy, self.qz]).reshape(3, 1)
|
||||
sum_term = np.zeros([4,4])
|
||||
sum_term[0,1:] = -v[:,0]
|
||||
sum_term[1:, 0] = v[:,0]
|
||||
sum_term = np.zeros([4, 4])
|
||||
sum_term[0, 1:] = -v[:, 0]
|
||||
sum_term[1:, 0] = v[:, 0]
|
||||
sum_term[1:, 1:] = skew_symmetric(v)
|
||||
sigma = self.qw * np.eye(4) + sum_term
|
||||
q_new = sigma @ q.to_numpy()
|
||||
@@ -337,18 +365,24 @@ class Quaternion:
|
||||
elif out == 'Quaternion':
|
||||
q_obj = Quaternion(*q_new)
|
||||
return q_obj
|
||||
|
||||
|
||||
|
||||
class RotVec:
|
||||
|
||||
def __init__(self, ax=0., ay=0., az=0., quaternion=None):
|
||||
if quaternion is None:
|
||||
self.ax = ax; self.ay = ay; self.az = az
|
||||
self.ax = ax
|
||||
self.ay = ay
|
||||
self.az = az
|
||||
else:
|
||||
x = quaternion.qx; y = quaternion.qy; z = quaternion.qz
|
||||
norm_im = np.sqrt(x**2 + y**2 + z**2)
|
||||
if (norm_im < 1e-7):
|
||||
self.ax = 2*x; self.ay = 2*y; self.az = 2*z
|
||||
x = quaternion.qx
|
||||
y = quaternion.qy
|
||||
z = quaternion.qz
|
||||
norm_im = np.sqrt(x ** 2 + y ** 2 + z ** 2)
|
||||
if norm_im < 1e-7:
|
||||
self.ax = 2 * x
|
||||
self.ay = 2 * y
|
||||
self.az = 2 * z
|
||||
else:
|
||||
th = 2 * np.arctan2(norm_im, quaternion.qw)
|
||||
th = self.pi2pi(th)
|
||||
@@ -360,25 +394,26 @@ class RotVec:
|
||||
|
||||
return RotVec(-self.ax, -self.ay, -self.az)
|
||||
|
||||
def toRotationMatrix(self):
|
||||
def to_rotation_matrix(self):
|
||||
|
||||
q = self.toQuaternion()
|
||||
q_vec = np.array([q.qx, q.qy, q.qz]).reshape(3,1)
|
||||
q = self.to_quaternion()
|
||||
q_vec = np.array([q.qx, q.qy, q.qz]).reshape(3, 1)
|
||||
qw = q.qw
|
||||
mat = (qw**2 - q_vec.T @ q_vec) * np.eye(3) + \
|
||||
2 * q_vec @ q_vec.T - 2 * qw * skew_symmetric(q_vec.reshape(-1,))
|
||||
mat = (qw ** 2 - q_vec.T @ q_vec) * np.eye(3) \
|
||||
+ 2 * q_vec @ q_vec.T - 2 * qw * skew_symmetric(q_vec.reshape(-1, ))
|
||||
return mat.T
|
||||
|
||||
def toQuaternion(self):
|
||||
|
||||
v = np.sqrt(self.ax**2 + self.ay**2 + self.az**2)
|
||||
def to_quaternion(self):
|
||||
|
||||
v = np.sqrt(self.ax ** 2 + self.ay ** 2 + self.az ** 2)
|
||||
if (v < 1e-6):
|
||||
return Quaternion(1, 0, 0, 0)
|
||||
else:
|
||||
return Quaternion(np.cos(v/2), np.sin(v/2)*self.ax/v,
|
||||
np.sin(v/2)*self.ay/v, np.sin(v/2)*self.az/v)
|
||||
return Quaternion(np.cos(v / 2), np.sin(v / 2) * self.ax / v,
|
||||
np.sin(v / 2) * self.ay / v, np.sin(v / 2) * self.az / v)
|
||||
|
||||
def pi2pi(self, rad):
|
||||
@staticmethod
|
||||
def pi2pi(rad):
|
||||
|
||||
val = np.fmod(rad, 2.0 * np.pi)
|
||||
if val > np.pi:
|
||||
@@ -387,7 +422,7 @@ class RotVec:
|
||||
val += 2.0 * np.pi
|
||||
|
||||
return val
|
||||
|
||||
|
||||
|
||||
class TripletList:
|
||||
|
||||
@@ -401,27 +436,33 @@ class TripletList:
|
||||
self.col.append(icol)
|
||||
self.data.append(idata)
|
||||
|
||||
|
||||
class Pose3D:
|
||||
|
||||
def __init__(self, x, y, z, qw, qx, qy, qz):
|
||||
self.x = x; self.y = y; self.z = z
|
||||
self.qw = qw; self.qx = qx; self.qy = qy; self.qz = qz
|
||||
|
||||
self.x = x
|
||||
self.y = y
|
||||
self.z = z
|
||||
self.qw = qw
|
||||
self.qx = qx
|
||||
self.qy = qy
|
||||
self.qz = qz
|
||||
|
||||
def pos(self):
|
||||
v = np.array([self.x, self.y, self.z]).reshape(3,1)
|
||||
v = np.array([self.x, self.y, self.z]).reshape(3, 1)
|
||||
return v
|
||||
|
||||
|
||||
def rv(self):
|
||||
q = Quaternion(self.qw, self.qx, self.qy, self.qz)
|
||||
return RotVec(quaternion = q)
|
||||
return RotVec(quaternion=q)
|
||||
|
||||
def ominus(self, base):
|
||||
t = base.rv().toRotationMatrix().T @ (self.pos() - base.pos())
|
||||
q = base.rv().toQuaternion().conjugate().quat_mult(self.rv().toQuaternion(), out = 'Quaternion')
|
||||
t = base.rv().to_rotation_matrix().T @ (self.pos() - base.pos())
|
||||
q = base.rv().to_quaternion().conjugate().quat_mult(self.rv().to_quaternion(), out='Quaternion')
|
||||
return Pose3D(t[0][0], t[1][0], t[2][0], q.qw, q.qx, q.qy, q.qz)
|
||||
|
||||
|
||||
class Constrant3D:
|
||||
class Constraint3D:
|
||||
|
||||
def __init__(self, id1, id2, t, info_mat):
|
||||
self.id1 = id1
|
||||
@@ -429,25 +470,28 @@ class Constrant3D:
|
||||
self.t = t
|
||||
self.info_mat = info_mat
|
||||
|
||||
def plot_nodes(nodes, ax, color ="-r", label = ""):
|
||||
|
||||
def plot_nodes(nodes, ax, color="-r", label=""):
|
||||
x, y, z = [], [], []
|
||||
for n in nodes:
|
||||
x.append(n.x); y.append(n.y); z.append(n.z)
|
||||
x.append(n.x)
|
||||
y.append(n.y)
|
||||
z.append(n.z)
|
||||
x = np.array(x)
|
||||
y = np.array(y)
|
||||
z = np.array(z)
|
||||
max_range = np.array([x.max()-x.min(), y.max()-y.min(), z.max()-z.min()]).max() / 2.0
|
||||
mid_x = (x.max()+x.min()) * 0.5
|
||||
mid_y = (y.max()+y.min()) * 0.5
|
||||
mid_z = (z.max()+z.min()) * 0.5
|
||||
max_range = np.array([x.max() - x.min(), y.max() - y.min(), z.max() - z.min()]).max() / 2.0
|
||||
mid_x = (x.max() + x.min()) * 0.5
|
||||
mid_y = (y.max() + y.min()) * 0.5
|
||||
mid_z = (z.max() + z.min()) * 0.5
|
||||
ax.set_xlim(mid_x - max_range, mid_x + max_range)
|
||||
ax.set_ylim(mid_y - max_range, mid_y + max_range)
|
||||
ax.set_zlim(mid_z - max_range, mid_z + max_range)
|
||||
ax.set_zlim(mid_z - max_range, mid_z + max_range)
|
||||
ax.plot(x, y, z, color, label=label)
|
||||
# ax.plot(x, y, color, label=label)
|
||||
|
||||
|
||||
def load_data(fname):
|
||||
|
||||
nodes, consts = [], []
|
||||
|
||||
for line in open(fname):
|
||||
@@ -455,7 +499,7 @@ def load_data(fname):
|
||||
tag = sline[0]
|
||||
|
||||
if tag == "VERTEX_SE3:QUAT":
|
||||
#data_id = int(sline[1]) # unused
|
||||
# data_id = int(sline[1]) # unused
|
||||
x = float(sline[2])
|
||||
y = float(sline[3])
|
||||
z = float(sline[4])
|
||||
@@ -463,7 +507,7 @@ def load_data(fname):
|
||||
qy = float(sline[6])
|
||||
qz = float(sline[7])
|
||||
qw = float(sline[8])
|
||||
|
||||
|
||||
nodes.append(Pose3D(x, y, z, qw, qx, qy, qz))
|
||||
elif tag == "EDGE_SE3:QUAT":
|
||||
id1 = int(sline[1])
|
||||
@@ -475,15 +519,15 @@ def load_data(fname):
|
||||
qy = float(sline[7])
|
||||
qz = float(sline[8])
|
||||
qw = float(sline[9])
|
||||
c1 = float(sline[10])
|
||||
c2 = float(sline[11])
|
||||
c3 = float(sline[12])
|
||||
c4 = float(sline[13])
|
||||
c5 = float(sline[14])
|
||||
c6 = float(sline[15])
|
||||
c7 = float(sline[16])
|
||||
c8 = float(sline[17])
|
||||
c9 = float(sline[18])
|
||||
c1 = float(sline[10])
|
||||
c2 = float(sline[11])
|
||||
c3 = float(sline[12])
|
||||
c4 = float(sline[13])
|
||||
c5 = float(sline[14])
|
||||
c6 = float(sline[15])
|
||||
c7 = float(sline[16])
|
||||
c8 = float(sline[17])
|
||||
c9 = float(sline[18])
|
||||
c10 = float(sline[19])
|
||||
c11 = float(sline[20])
|
||||
c12 = float(sline[21])
|
||||
@@ -497,14 +541,14 @@ def load_data(fname):
|
||||
c20 = float(sline[29])
|
||||
c21 = float(sline[30])
|
||||
t = Pose3D(x, y, z, qw, qx, qy, qz)
|
||||
info_mat = np.array([[c1, c2, c3, c4, c5, c6],
|
||||
[c2, c7, c8, c9, c10, c11],
|
||||
[c3, c8, c12, c13, c14, c15],
|
||||
[c4, c9, c13, c16, c17, c18],
|
||||
info_mat = np.array([[c1, c2, c3, c4, c5, c6],
|
||||
[c2, c7, c8, c9, c10, c11],
|
||||
[c3, c8, c12, c13, c14, c15],
|
||||
[c4, c9, c13, c16, c17, c18],
|
||||
[c5, c10, c14, c17, c19, c20],
|
||||
[c6, c11, c15, c18, c20, c21]
|
||||
])
|
||||
consts.append(Constrant3D(id1, id2, t, info_mat))
|
||||
consts.append(Constraint3D(id1, id2, t, info_mat))
|
||||
|
||||
print("n_nodes:", len(nodes))
|
||||
print("n_consts:", len(consts))
|
||||
@@ -512,22 +556,26 @@ def load_data(fname):
|
||||
return nodes, consts
|
||||
|
||||
|
||||
|
||||
def main():
|
||||
|
||||
print("start!!")
|
||||
|
||||
fnames = ["parking-garage.g2o"]
|
||||
dir = os.path.dirname(os.path.abspath(__file__))
|
||||
|
||||
max_iter = 20
|
||||
fnames = [
|
||||
dir + "/sphere2200_guess.g2o",
|
||||
dir + "/torus3d_guess.g2o",
|
||||
dir + "/parking-garage.g2o",
|
||||
]
|
||||
|
||||
max_iter = 10
|
||||
min_iter = 3
|
||||
|
||||
# parameter setting
|
||||
optimizer = Optimizer3D()
|
||||
optimizer.p_lambda = 1e-6
|
||||
optimizer.verbose = True
|
||||
optimizer.animation = True
|
||||
|
||||
optimizer.animation = False
|
||||
|
||||
for f in fnames:
|
||||
print(f)
|
||||
|
||||
@@ -538,14 +586,16 @@ def main():
|
||||
print("elapsed_time", time.time() - start, "sec")
|
||||
|
||||
# plot
|
||||
plt.cla()
|
||||
plt.close("all")
|
||||
est_traj_fig = plt.figure()
|
||||
ax = est_traj_fig.add_subplot(111, projection='3d')
|
||||
plot_nodes(nodes, ax, color="-b", label="before")
|
||||
plot_nodes(final_nodes, ax, label="after")
|
||||
plt.legend()
|
||||
plt.show()
|
||||
|
||||
print("done!!")
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
|
||||
Reference in New Issue
Block a user