From 40e5c2cbb02fd41561e5c6ac3617a1451802b48f Mon Sep 17 00:00:00 2001 From: Atsushi Sakai Date: Fri, 31 May 2019 10:42:36 +0900 Subject: [PATCH] first release pose_optimization_slam --- .gitignore | 1 + SLAM/PoseOptimizationSLAM/README.md | 9 + SLAM/PoseOptimizationSLAM/data_downloader.py | 28 ++ .../pose_optimization_slam.py | 290 ++++++++++++++++++ 4 files changed, 328 insertions(+) create mode 100644 SLAM/PoseOptimizationSLAM/README.md create mode 100644 SLAM/PoseOptimizationSLAM/data_downloader.py create mode 100644 SLAM/PoseOptimizationSLAM/pose_optimization_slam.py diff --git a/.gitignore b/.gitignore index e04b28e1..62e48c67 100644 --- a/.gitignore +++ b/.gitignore @@ -1,5 +1,6 @@ *.csv *.gif +*.g2o # Byte-compiled / optimized / DLL files __pycache__/ diff --git a/SLAM/PoseOptimizationSLAM/README.md b/SLAM/PoseOptimizationSLAM/README.md new file mode 100644 index 00000000..2dbb8d68 --- /dev/null +++ b/SLAM/PoseOptimizationSLAM/README.md @@ -0,0 +1,9 @@ +# How to use + +1. Download data + + python data_downloader.py + +2. run SLAM + + python pose_optimization_slam.py diff --git a/SLAM/PoseOptimizationSLAM/data_downloader.py b/SLAM/PoseOptimizationSLAM/data_downloader.py new file mode 100644 index 00000000..d1b8a2c2 --- /dev/null +++ b/SLAM/PoseOptimizationSLAM/data_downloader.py @@ -0,0 +1,28 @@ +""" + +Data down loader for pose optimization + +author: Atsushi Sakai + +""" + + +import subprocess +def main(): + print("start!!") + + cmd = "wget https://www.dropbox.com/s/vcz8cag7bo0zlaj/input_INTEL_g2o.g2o?dl=0 -O intel.g2o -nc" + subprocess.call(cmd, shell=True) + + cmd = "wget https://www.dropbox.com/s/d8fcn1jg1mebx8f/input_MITb_g2o.g2o?dl=0 -O mit_killian.g2o -nc" + + subprocess.call(cmd, shell=True) + cmd = "wget https://www.dropbox.com/s/gmdzo74b3tzvbrw/input_M3500_g2o.g2o?dl=0 -O manhattan3500.g2o -nc" + subprocess.call(cmd, shell=True) + + print("done!!") + + +if __name__ == '__main__': + main() + diff --git a/SLAM/PoseOptimizationSLAM/pose_optimization_slam.py b/SLAM/PoseOptimizationSLAM/pose_optimization_slam.py new file mode 100644 index 00000000..764b0f5e --- /dev/null +++ b/SLAM/PoseOptimizationSLAM/pose_optimization_slam.py @@ -0,0 +1,290 @@ +""" + +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()