From cb8af3c2e4db403ce524df2df8ade7dad28039a6 Mon Sep 17 00:00:00 2001 From: Ryohei Sasaki Date: Sat, 7 Sep 2019 14:34:09 +0900 Subject: [PATCH] Delete pose_optimization_slam_3d.py --- pose_optimization_slam_3d.py | 541 ----------------------------------- 1 file changed, 541 deletions(-) delete mode 100644 pose_optimization_slam_3d.py diff --git a/pose_optimization_slam_3d.py b/pose_optimization_slam_3d.py deleted file mode 100644 index 105201f8..00000000 --- a/pose_optimization_slam_3d.py +++ /dev/null @@ -1,541 +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 sys -import time -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): - 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 and ida < numnodes, "ida is invalid" - assert 0 <= idb and 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.toQuaternion() - - 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 - - def error_func(self, pa, pb, t): - - ba = pb.ominus(pa) - q = t.rv().toQuaternion().conjugate().quat_mult(ba.rv().toQuaternion(), 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 - - def dQuat_dRV(self, 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.toQuaternion() - 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 - - def dRV_dQuat(self, 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.toRotationMatrix() - - 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.toQuaternion() - qcinv = t.rv().inverted().toQuaternion() - qb = pb.rv().toQuaternion() - 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 toRotationMatrix(self): - - q = self.toQuaternion() - 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 toQuaternion(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) - - def pi2pi(self, 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().toRotationMatrix().T @ (self.pos() - base.pos()) - q = base.rv().toQuaternion().conjugate().quat_mult(self.rv().toQuaternion(), out = 'Quaternion') - return Pose3D(t[0][0], t[1][0], t[2][0], q.qw, q.qx, q.qy, q.qz) - - -class Constrant3D: - - 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) - ax.plot(x, y, z, 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(Constrant3D(id1, id2, t, info_mat)) - - print("n_nodes:", len(nodes)) - print("n_consts:", len(consts)) - - return nodes, consts - - - -def main(): - - print("start!!") - - fnames = ["parking-garage.g2o"] - - max_iter = 20 - min_iter = 3 - - # parameter setting - optimizer = Optimizer3D() - optimizer.p_lambda = 1e-6 - optimizer.verbose = True - optimizer.animation = True - - 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.cla() - 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.show() - - print("done!!") - -if __name__ == '__main__': - main()