mirror of
https://github.com/AtsushiSakai/PythonRobotics.git
synced 2026-01-14 18:28:14 -05:00
once remove the scripts for solving refactoring
This commit is contained in:
Atsushi Sakai
@@ -1,290 +0,0 @@
|
||||
"""
|
||||
|
||||
2D (x, y, yaw) pose optimization SLAM
|
||||
|
||||
author: Atsushi Sakai
|
||||
|
||||
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)
|
||||
|
||||
"""
|
||||
|
||||
import sys
|
||||
import time
|
||||
import math
|
||||
import numpy as np
|
||||
import matplotlib.pyplot as plt
|
||||
from scipy import sparse
|
||||
from scipy.sparse import linalg
|
||||
|
||||
|
||||
class Optimizer2D:
|
||||
|
||||
def __init__(self):
|
||||
self.verbose = False
|
||||
self.animation = False
|
||||
self.p_lambda = 0.0
|
||||
self.init_w = 1e10
|
||||
self.stop_thre = 1e-3
|
||||
self.dim = 3 # state dimension
|
||||
|
||||
def optimize_path(self, nodes, consts, max_iter, min_iter):
|
||||
|
||||
graph_nodes = nodes[:]
|
||||
prev_cost = sys.float_info.max
|
||||
|
||||
for i in range(max_iter):
|
||||
start = time.time()
|
||||
cost, graph_nodes = self.optimize_path_one_step(
|
||||
graph_nodes, consts)
|
||||
elapsed = time.time() - start
|
||||
if self.verbose:
|
||||
print("step ", i, " cost: ", cost, " time:", elapsed, "s")
|
||||
|
||||
# check convergence
|
||||
if (i > min_iter) and (prev_cost - cost < self.stop_thre):
|
||||
if self.verbose:
|
||||
print("converged:", prev_cost
|
||||
- cost, " < ", self.stop_thre)
|
||||
break
|
||||
prev_cost = cost
|
||||
|
||||
if self.animation:
|
||||
plt.cla()
|
||||
plot_nodes(nodes, color="-b")
|
||||
plot_nodes(graph_nodes)
|
||||
plt.axis("equal")
|
||||
plt.pause(1.0)
|
||||
|
||||
return graph_nodes
|
||||
|
||||
def optimize_path_one_step(self, graph_nodes, constraints):
|
||||
|
||||
indlist = [i for i in range(self.dim)]
|
||||
numnodes = len(graph_nodes)
|
||||
bf = np.zeros(numnodes * self.dim)
|
||||
tripletList = TripletList()
|
||||
|
||||
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"
|
||||
r, Ja, Jb = self.calc_error(
|
||||
graph_nodes[ida], graph_nodes[idb], con.t)
|
||||
|
||||
trJaInfo = Ja.transpose() @ con.info_mat
|
||||
trJaInfoJa = trJaInfo @ Ja
|
||||
trJbInfo = Jb.transpose() @ con.info_mat
|
||||
trJbInfoJb = trJbInfo @ Jb
|
||||
trJaInfoJb = trJaInfo @ Jb
|
||||
|
||||
for k in indlist:
|
||||
for m in indlist:
|
||||
tripletList.push_back(
|
||||
ida * self.dim + k, ida * self.dim + m, trJaInfoJa[k, m])
|
||||
tripletList.push_back(
|
||||
idb * self.dim + k, idb * self.dim + m, trJbInfoJb[k, m])
|
||||
tripletList.push_back(
|
||||
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 * self.dim + 3] += trJaInfo @ r
|
||||
bf[idb * self.dim: idb * self.dim + 3] += trJbInfo @ r
|
||||
|
||||
for k in indlist:
|
||||
tripletList.push_back(k, k, self.init_w)
|
||||
|
||||
for i in range(self.dim * numnodes):
|
||||
tripletList.push_back(i, i, self.p_lambda)
|
||||
|
||||
mat = sparse.coo_matrix((tripletList.data, (tripletList.row, tripletList.col)),
|
||||
shape=(numnodes * self.dim, numnodes * self.dim))
|
||||
|
||||
x = linalg.spsolve(mat.tocsr(), -bf)
|
||||
|
||||
out_nodes = []
|
||||
for i in range(len(graph_nodes)):
|
||||
u_i = i * self.dim
|
||||
pos = Pose2D(
|
||||
graph_nodes[i].x + x[u_i],
|
||||
graph_nodes[i].y + x[u_i + 1],
|
||||
graph_nodes[i].theta + x[u_i + 2]
|
||||
)
|
||||
out_nodes.append(pos)
|
||||
|
||||
cost = self.calc_global_cost(out_nodes, constraints)
|
||||
|
||||
return cost, out_nodes
|
||||
|
||||
def calc_global_cost(self, nodes, constraints):
|
||||
cost = 0.0
|
||||
for c in constraints:
|
||||
diff = self.error_func(nodes[c.id1], nodes[c.id2], c.t)
|
||||
cost += diff.transpose() @ c.info_mat @ diff
|
||||
|
||||
return cost
|
||||
|
||||
def error_func(self, pa, pb, t):
|
||||
ba = self.calc_constraint_pose(pb, pa)
|
||||
error = np.array([ba.x - t.x,
|
||||
ba.y - t.y,
|
||||
self.pi2pi(ba.theta - t.theta)])
|
||||
return error
|
||||
|
||||
def calc_constraint_pose(self, l, r):
|
||||
diff = np.array([l.x - r.x, l.y - r.y, l.theta - r.theta])
|
||||
v = self.rot_mat_2d(-r.theta) @ diff
|
||||
v[2] = self.pi2pi(l.theta - r.theta)
|
||||
return Pose2D(v[0], v[1], v[2])
|
||||
|
||||
def rot_mat_2d(self, theta):
|
||||
return np.array([[math.cos(theta), -math.sin(theta), 0.0],
|
||||
[math.sin(theta), math.cos(theta), 0.0],
|
||||
[0.0, 0.0, 1.0]
|
||||
])
|
||||
|
||||
def calc_error(self, pa, pb, t):
|
||||
e0 = self.error_func(pa, pb, t)
|
||||
dx = pb.x - pa.x
|
||||
dy = pb.y - pa.y
|
||||
dxdt = -math.sin(pa.theta) * dx + math.cos(pa.theta) * dy
|
||||
dydt = -math.cos(pa.theta) * dx - math.sin(pa.theta) * dy
|
||||
Ja = np.array([[-math.cos(pa.theta), -math.sin(pa.theta), dxdt],
|
||||
[math.sin(pa.theta), -math.cos(pa.theta), dydt],
|
||||
[0.0, 0.0, -1.0]
|
||||
])
|
||||
Jb = np.array([[math.cos(pa.theta), math.sin(pa.theta), 0.0],
|
||||
[-math.sin(pa.theta), math.cos(pa.theta), 0.0],
|
||||
[0.0, 0.0, 1.0]
|
||||
])
|
||||
return e0, Ja, Jb
|
||||
|
||||
def pi2pi(self, rad):
|
||||
|
||||
val = math.fmod(rad, 2.0 * math.pi)
|
||||
if val > math.pi:
|
||||
val -= 2.0 * math.pi
|
||||
elif val < -math.pi:
|
||||
val += 2.0 * math.pi
|
||||
|
||||
return val
|
||||
|
||||
|
||||
class TripletList:
|
||||
|
||||
def __init__(self):
|
||||
self.row = []
|
||||
self.col = []
|
||||
self.data = []
|
||||
|
||||
def push_back(self, irow, icol, idata):
|
||||
self.row.append(irow)
|
||||
self.col.append(icol)
|
||||
self.data.append(idata)
|
||||
|
||||
|
||||
class Pose2D:
|
||||
|
||||
def __init__(self, x, y, theta):
|
||||
self.x = x
|
||||
self.y = y
|
||||
self.theta = theta
|
||||
|
||||
|
||||
class Constrant2D:
|
||||
|
||||
def __init__(self, id1, id2, t, info_mat):
|
||||
self.id1 = id1
|
||||
self.id2 = id2
|
||||
self.t = t
|
||||
self.info_mat = info_mat
|
||||
|
||||
|
||||
def plot_nodes(nodes, color ="-r", label = ""):
|
||||
x, y = [], []
|
||||
for n in nodes:
|
||||
x.append(n.x)
|
||||
y.append(n.y)
|
||||
plt.plot(x, y, color, label=label)
|
||||
|
||||
|
||||
def load_data(fname):
|
||||
nodes, consts = [], []
|
||||
|
||||
for line in open(fname):
|
||||
sline = line.split()
|
||||
tag = sline[0]
|
||||
|
||||
if tag == "VERTEX_SE2":
|
||||
data_id = int(sline[1])
|
||||
x = float(sline[2])
|
||||
y = float(sline[3])
|
||||
theta = float(sline[4])
|
||||
nodes.append(Pose2D(x, y, theta))
|
||||
elif tag == "EDGE_SE2":
|
||||
id1 = int(sline[1])
|
||||
id2 = int(sline[2])
|
||||
x = float(sline[3])
|
||||
y = float(sline[4])
|
||||
th = float(sline[5])
|
||||
c1 = float(sline[6])
|
||||
c2 = float(sline[7])
|
||||
c3 = float(sline[8])
|
||||
c4 = float(sline[9])
|
||||
c5 = float(sline[10])
|
||||
c6 = float(sline[11])
|
||||
t = Pose2D(x, y, th)
|
||||
info_mat = np.array([[c1, c2, c3],
|
||||
[c2, c4, c5],
|
||||
[c3, c5, c6]
|
||||
])
|
||||
consts.append(Constrant2D(id1, id2, t, info_mat))
|
||||
|
||||
print("n_nodes:", len(nodes))
|
||||
print("n_consts:", len(consts))
|
||||
|
||||
return nodes, consts
|
||||
|
||||
|
||||
def main():
|
||||
print("start!!")
|
||||
|
||||
fnames = ["intel.g2o",
|
||||
"manhattan3500.g2o",
|
||||
"mit_killian.g2o"
|
||||
]
|
||||
|
||||
max_iter = 20
|
||||
min_iter = 3
|
||||
|
||||
# parameter setting
|
||||
optimizer = Optimizer2D()
|
||||
optimizer.p_lambda = 1e-6
|
||||
optimizer.verbose = True
|
||||
optimizer.animation = True
|
||||
|
||||
for f in fnames:
|
||||
nodes, consts = load_data(f)
|
||||
|
||||
start = time.time()
|
||||
final_nodes = optimizer.optimize_path(nodes, consts, max_iter, min_iter)
|
||||
print("elapsed_time", time.time() - start, "sec")
|
||||
|
||||
# plotting
|
||||
plt.cla()
|
||||
plot_nodes(nodes, color="-b", label="before")
|
||||
plot_nodes(final_nodes, label="after")
|
||||
plt.axis("equal")
|
||||
plt.grid(True)
|
||||
plt.legend()
|
||||
plt.pause(3.0)
|
||||
|
||||
print("done!!")
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -1,601 +0,0 @@
|
||||
"""
|
||||
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
|
||||
|
||||
|
||||
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:
|
||||
return 0
|
||||
sqre = np.sqrt(squared_error)
|
||||
if sqre < delta:
|
||||
return 1 # no effect
|
||||
return delta / sqre # linear
|
||||
|
||||
|
||||
class Optimizer3D:
|
||||
|
||||
def __init__(self):
|
||||
self.verbose = False
|
||||
self.animation = False
|
||||
self.p_lambda = 1e-7
|
||||
self.init_w = 1e10
|
||||
self.stop_thre = 1e-3
|
||||
self.robust_delta = 1
|
||||
self.dim = 6 # state dimension
|
||||
|
||||
def optimize_path(self, nodes, consts, max_iter, min_iter):
|
||||
|
||||
graph_nodes = nodes[:]
|
||||
prev_cost = sys.float_info.max
|
||||
|
||||
est_traj_fig = plt.figure()
|
||||
ax = est_traj_fig.add_subplot(111, projection='3d')
|
||||
|
||||
for i in range(max_iter):
|
||||
start = time.time()
|
||||
cost, graph_nodes = self.optimize_path_one_step(
|
||||
graph_nodes, consts)
|
||||
elapsed = time.time() - start
|
||||
if self.verbose:
|
||||
print("step ", i, " cost: ", cost, " time:", elapsed, "s")
|
||||
|
||||
# check convergence
|
||||
if (i > min_iter) and (prev_cost - cost < self.stop_thre):
|
||||
if self.verbose:
|
||||
print("converged:", prev_cost
|
||||
- cost, " < ", self.stop_thre)
|
||||
break
|
||||
prev_cost = cost
|
||||
|
||||
if self.animation:
|
||||
plt.cla()
|
||||
plot_nodes(nodes, ax, color="-b")
|
||||
plot_nodes(graph_nodes, ax)
|
||||
plt.pause(1.0)
|
||||
|
||||
return graph_nodes
|
||||
|
||||
def optimize_path_one_step(self, graph_nodes, constraints):
|
||||
|
||||
indlist = [i for i in range(self.dim)]
|
||||
numnodes = len(graph_nodes)
|
||||
bf = np.zeros(numnodes * self.dim)
|
||||
tripletList = TripletList()
|
||||
|
||||
for con in constraints:
|
||||
ida = con.id1
|
||||
idb = con.id2
|
||||
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)
|
||||
|
||||
trJaInfo = Ja.transpose() @ info_mat
|
||||
trJaInfoJa = trJaInfo @ Ja
|
||||
trJbInfo = Jb.transpose() @ info_mat
|
||||
trJbInfoJb = trJbInfo @ Jb
|
||||
trJaInfoJb = trJaInfo @ Jb
|
||||
|
||||
for k in indlist:
|
||||
for m in indlist:
|
||||
tripletList.push_back(
|
||||
ida * self.dim + k, ida * self.dim + m, trJaInfoJa[k, m])
|
||||
tripletList.push_back(
|
||||
idb * self.dim + k, idb * self.dim + m, trJbInfoJb[k, m])
|
||||
tripletList.push_back(
|
||||
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
|
||||
|
||||
for k in indlist:
|
||||
tripletList.push_back(k, k, self.init_w)
|
||||
|
||||
for i in range(self.dim * numnodes):
|
||||
tripletList.push_back(i, i, self.p_lambda)
|
||||
|
||||
mat = sparse.coo_matrix((tripletList.data, (tripletList.row, tripletList.col)),
|
||||
shape=(numnodes * self.dim, numnodes * self.dim))
|
||||
|
||||
x = linalg.spsolve(mat.tocsr(), -bf)
|
||||
|
||||
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.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],
|
||||
q_after.qw,
|
||||
q_after.qx,
|
||||
q_after.qy,
|
||||
q_after.qz
|
||||
)
|
||||
out_nodes.append(pos)
|
||||
|
||||
cost = self.calc_global_cost(out_nodes, constraints)
|
||||
|
||||
return cost, out_nodes
|
||||
|
||||
def calc_global_cost(self, nodes, constraints):
|
||||
|
||||
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)
|
||||
cost += diff.transpose() @ info_mat @ diff
|
||||
|
||||
return cost
|
||||
|
||||
@staticmethod
|
||||
def error_func(pa, pb, t):
|
||||
|
||||
ba = pb.ominus(pa)
|
||||
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,
|
||||
drv.ax[0],
|
||||
drv.ay[0],
|
||||
drv.az[0]])
|
||||
return error
|
||||
|
||||
@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]]
|
||||
)
|
||||
return dqu
|
||||
|
||||
vd = v * 2
|
||||
v2 = v ** 2
|
||||
v3 = v ** 3
|
||||
|
||||
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]]
|
||||
)
|
||||
|
||||
return dqu
|
||||
|
||||
def dR_dRV(self, rv):
|
||||
|
||||
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]]
|
||||
)
|
||||
dRdqx = 2 * np.array(
|
||||
[[qx, qy, qz],
|
||||
[qy, -qx, -qw],
|
||||
[qz, qw, -qx]]
|
||||
)
|
||||
dRdqy = 2 * np.array(
|
||||
[[-qy, qx, qw],
|
||||
[qx, qy, qz],
|
||||
[-qw, qz, -qy]]
|
||||
)
|
||||
dRdqz = 2 * np.array(
|
||||
[[-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]
|
||||
return dux, duy, duz
|
||||
|
||||
@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:
|
||||
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]]
|
||||
)
|
||||
return ret
|
||||
|
||||
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]]
|
||||
)
|
||||
return ret
|
||||
|
||||
def QMat(self, q):
|
||||
|
||||
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]]
|
||||
)
|
||||
return Q
|
||||
|
||||
def QMatBar(self, q):
|
||||
|
||||
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]]
|
||||
)
|
||||
return Q
|
||||
|
||||
def calc_error(self, pa, pb, t):
|
||||
|
||||
e0 = self.error_func(pa, pb, t)
|
||||
Ja = np.identity(6)
|
||||
Jb = np.identity(6)
|
||||
|
||||
rva_inv = pa.rv().inverted()
|
||||
rotPaInv = rva_inv.to_rotation_matrix()
|
||||
|
||||
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]])
|
||||
|
||||
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.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())
|
||||
|
||||
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
|
||||
|
||||
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)
|
||||
|
||||
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[1:, 1:] = skew_symmetric(v)
|
||||
sigma = self.qw * np.eye(4) + sum_term
|
||||
q_new = sigma @ q.to_numpy()
|
||||
if out == 'np':
|
||||
return q_new
|
||||
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
|
||||
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
|
||||
else:
|
||||
th = 2 * np.arctan2(norm_im, quaternion.qw)
|
||||
th = self.pi2pi(th)
|
||||
self.ax = x / norm_im * th
|
||||
self.ay = y / norm_im * th
|
||||
self.az = z / norm_im * th
|
||||
|
||||
def inverted(self):
|
||||
|
||||
return RotVec(-self.ax, -self.ay, -self.az)
|
||||
|
||||
def to_rotation_matrix(self):
|
||||
|
||||
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, ))
|
||||
return mat.T
|
||||
|
||||
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)
|
||||
|
||||
@staticmethod
|
||||
def pi2pi(rad):
|
||||
|
||||
val = np.fmod(rad, 2.0 * np.pi)
|
||||
if val > np.pi:
|
||||
val -= 2.0 * np.pi
|
||||
elif val < -np.pi:
|
||||
val += 2.0 * np.pi
|
||||
|
||||
return val
|
||||
|
||||
|
||||
class TripletList:
|
||||
|
||||
def __init__(self):
|
||||
self.row = []
|
||||
self.col = []
|
||||
self.data = []
|
||||
|
||||
def push_back(self, irow, icol, idata):
|
||||
self.row.append(irow)
|
||||
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
|
||||
|
||||
def pos(self):
|
||||
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)
|
||||
|
||||
def ominus(self, base):
|
||||
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 Constraint3D:
|
||||
|
||||
def __init__(self, id1, id2, t, info_mat):
|
||||
self.id1 = id1
|
||||
self.id2 = id2
|
||||
self.t = t
|
||||
self.info_mat = info_mat
|
||||
|
||||
|
||||
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 = 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
|
||||
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.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):
|
||||
sline = line.split()
|
||||
tag = sline[0]
|
||||
|
||||
if tag == "VERTEX_SE3:QUAT":
|
||||
# data_id = int(sline[1]) # unused
|
||||
x = float(sline[2])
|
||||
y = float(sline[3])
|
||||
z = float(sline[4])
|
||||
qx = float(sline[5])
|
||||
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])
|
||||
id2 = int(sline[2])
|
||||
x = float(sline[3])
|
||||
y = float(sline[4])
|
||||
z = float(sline[5])
|
||||
qx = float(sline[6])
|
||||
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])
|
||||
c10 = float(sline[19])
|
||||
c11 = float(sline[20])
|
||||
c12 = float(sline[21])
|
||||
c13 = float(sline[22])
|
||||
c14 = float(sline[23])
|
||||
c15 = float(sline[24])
|
||||
c16 = float(sline[25])
|
||||
c17 = float(sline[26])
|
||||
c18 = float(sline[27])
|
||||
c19 = float(sline[28])
|
||||
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],
|
||||
[c5, c10, c14, c17, c19, c20],
|
||||
[c6, c11, c15, c18, c20, c21]
|
||||
])
|
||||
consts.append(Constraint3D(id1, id2, t, info_mat))
|
||||
|
||||
print("n_nodes:", len(nodes))
|
||||
print("n_consts:", len(consts))
|
||||
|
||||
return nodes, consts
|
||||
|
||||
|
||||
def main():
|
||||
print("start!!")
|
||||
|
||||
dir = os.path.dirname(os.path.abspath(__file__))
|
||||
|
||||
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 = False
|
||||
|
||||
for f in fnames:
|
||||
print(f)
|
||||
|
||||
nodes, consts = load_data(f)
|
||||
|
||||
start = time.time()
|
||||
final_nodes = optimizer.optimize_path(nodes, consts, max_iter, min_iter)
|
||||
print("elapsed_time", time.time() - start, "sec")
|
||||
|
||||
# plot
|
||||
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