diff --git a/SLAM/PoseOptimizationSLAM2D/pose_optimization_slam_2d.py b/SLAM/PoseOptimizationSLAM2D/pose_optimization_slam_2d.py deleted file mode 100644 index 764b0f5e..00000000 --- a/SLAM/PoseOptimizationSLAM2D/pose_optimization_slam_2d.py +++ /dev/null @@ -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() diff --git a/SLAM/PoseOptimizationSLAM3D/pose_optimization_slam_3d.py b/SLAM/PoseOptimizationSLAM3D/pose_optimization_slam_3d.py deleted file mode 100644 index 0f39b22f..00000000 --- a/SLAM/PoseOptimizationSLAM3D/pose_optimization_slam_3d.py +++ /dev/null @@ -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()