Commit a39509b6 authored by czb5793's avatar czb5793
Browse files

1. Add a callback function for evaluating time used for updates.

2. Use sksparse.cholmod.cholesky to solve the sparse linear system
parent ef180e17
......@@ -6,6 +6,11 @@ The parent class of any types of graph
The Graph class has to be inherited and the following methods has to be implemented,
- generate_edge_object(self, vertex1, vertex2, z, information)
- normalize_angles(self, vertices):
Please install the scikit-sparse package (https://scikit-sparse.readthedocs.io/en/latest/overview.html)
On Debian/Ubuntu systems, the following command should suffice:
$ sudo apt-get install python-scipy libsuitesparse-dev
$ pip install --user scikit-sparse
"""
import matplotlib.pyplot as plt
......@@ -13,10 +18,8 @@ import numpy as np
import time
from scipy.sparse import coo_matrix
from scipy.sparse.linalg import spsolve
import warnings
from scipy.sparse import (SparseEfficiencyWarning)
warnings.simplefilter('ignore',SparseEfficiencyWarning)
from sksparse.cholmod import cholesky
from utils.math_util import normalize_angle
def timer(function):
"""
......@@ -77,7 +80,9 @@ class Graph:
In this method you're going to normalize angles of robot's orientation of the vertices
:param vertices: vertices of the graph
"""
raise NotImplementedError()
for v in vertices:
if v.dim == 3: # a robot pose
v.pose[2, 0] = normalize_angle(v.pose[2, 0])
def compute_global_error(self):
"""
......@@ -88,38 +93,54 @@ class Graph:
global_err += edge.calc_error() # error of an edge
return global_err
def graph_optimization(self, animation = False, number_fix = 3, damp_factor = 0.01, max_iter = 10):
def graph_optimization(self, number_fix = 3,
damp_factor = 0.01, max_iter = 10,
solver = "spsolve", callback = None, epsilon = 1e-2):
"""
Optimization of the graph
:param animation: decide whether animation shall be shown
:param number_fix: fix the estimation of the initial step
:param damp_factor: how fasten you want to fix a vertex.
:param max_iter: the maximum number of iterations
:return: global error after optimization
:param callback: a callback function, callback(vertices, edges, info)
:param difference for determining convergence
:return: epsilon global error after optimization
"""
global_error = np.inf
preError = np.inf
for i in range(max_iter):
""" linearize the problem """
start_time = time.time()
H, b = self.linearize_constraints(self.vertices, self.edges, number_fix, damp_factor)
linearize_time_cost = time.time() - start_time
""" solve sparse matrix """
dx = self.__solve_sparse(H, b)
start_time = time.time()
dx = self.solve_sparse(H, b, solver = solver)
solve_time_cost = time.time() - start_time
""" update vertices """
self.__apply_dx(dx) # x = x + dx;
self.apply_dx(dx) # x = x + dx;
global_error = self.compute_global_error()
diff = dx.T @ dx
#print ("iter: {0}, diff: {1}, Global Error: {2}".format(i, diff, global_error))
if animation == True:
self.draw()
if callback is not None:
info = {"global_error": global_error, "iteration": i+1,
"linearize_time_cost": linearize_time_cost,
"solve_time_cost": solve_time_cost}
callback(self.vertices, self.edges, info)
dError = abs(preError - global_error) # check convergence
preError = global_error
if dError < 0.01 or diff < 0.01:
self.__apply_covariance(H)
if dError < epsilon or diff < epsilon:
self.apply_covariance(H)
break
if np.max(np.abs(dx)) < 0.001:
damp_factor *= 2
else:
damp_factor /= 2
return global_error
def __apply_covariance(self, H):
def apply_covariance(self, H):
H = H.tocsr()
indices, hess_size = self.get_block_index_(self.vertices)
for id, vertex in enumerate(self.vertices):
......@@ -152,28 +173,30 @@ class Graph:
H: the hessian matrix (information matrix)
b: information vector
"""
indices, hess_size = self.get_block_index_(vertices) # calculate indices for each block of the hessian matrix
b = np.zeros((hess_size, 1), dtype=np.float32) # information vector
Hessian_data = list() # a list of block matrices
row_indices = list() # row indices of the corresponding values in H matrix
col_indices = list() # column indices of the corresponding values in H matrix
indices, hess_size = self.get_block_index_(vertices) # calculate indices for each block in the hessian matrix
b = np.zeros((hess_size, 1), dtype=np.float64) # initialize an information vector
Hessian_data = list() # a list to store the block matrices (flattened)
row_indices = list() # a list of row indices for the corresponding values in H matrix
col_indices = list() # a list of column indices for the corresponding values in H matrix
""" [ISSUE] This for-loop for calculating H and b cost time! """
for edge in edges:
err, A, B = edge.linearize() # calculate error and jacobian matrix
omega = edge.information
err, A, B = edge.linearize() # calculate error and obtain the Jacobian matrices A and B
omega = edge.information # the information matrix
""" Compute the block matrices and vectors """
b1 = (err.T @ omega @ A).T
b2 = (err.T @ omega @ B).T
bi = (err.T @ omega @ A).T
bj = (err.T @ omega @ B).T
Hii = A.T @ omega @ A
Hjj = B.T @ omega @ B
Hij = A.T @ omega @ B
Hji = B.T @ omega @ A
""" Compute the hessian matrix and vector """
i1, i2 = indices[edge.id1]
j1, j2 = indices[edge.id2]
b[i1:i2, :] += b1
b[j1:j2, :] += b2
b[i1:i2, :] += bi
b[j1:j2, :] += bj
"""
H[i1:i2, i1:i2] += Hii
......@@ -181,7 +204,7 @@ class Graph:
H[i1:i2, j1:j2] += Hij
H[j1:j2, i1:i2] += Hji
"""
""" Calculate the indices of the block in the Hessian """
""" Calculate the indices of the block in the Hessian """
inxii_1, inxii_2 = self.cartesian_product(i1, i2, i1, i2)
inxjj_1, inxjj_2 = self.cartesian_product(j1, j2, j1, j2)
inxij_1, inxij_2 = self.cartesian_product(i1, i2, j1, j2)
......@@ -197,23 +220,38 @@ class Graph:
Hessian_data += I
row_indices += ii
col_indices += ii
""" Create a sparse hessian matrix and store it by a memory efficient representation. """
H = coo_matrix((Hessian_data, (row_indices, col_indices)), shape=(hess_size, hess_size), dtype=np.float32)
""" Storing the sparse matrix with a memory efficient representation.
Fast constructing a sparse hessian matrix (COO format).
Note that duplicate entries are summed together.
Convert CSC format for faster arithmetic and matrix vector operations.
"""
H = coo_matrix((Hessian_data, (row_indices, col_indices)),
shape=(hess_size, hess_size),
dtype=np.float64).tocsc()
return H, b
def __solve_sparse(self, H, b):
def solve_sparse(self, H, b, solver = "spsolve"):
"""
Solve the linear system H @ dx = -b, where dx is unknown
:param H: A sparse hessian matrix
:param b: An information matrix
:param H: A sparse hessian matrix, and also a sparse, symmetric, positive-definite matrix.
:param b: An information matrix.
:param solver: solver to solve the linear system.
:return:
dx: the solution.
"""
dx = spsolve(H, -b)
dx = np.array(dx)[:, np.newaxis]
if solver == "cholesky":
factor = cholesky(H)
dx = factor(-b)
elif solver == "spsolve":
dx = spsolve(H, -b)
dx = np.array(dx)[:, np.newaxis]
else:
print(" Incorrect solver name!")
raise ValueError
return dx
def __apply_dx(self, dx):
def apply_dx(self, dx):
"""
A apply the increment dx on all vertices of the graph
:param dx: increment
......
......@@ -5,7 +5,6 @@ from supervisor.slam.graph.baseclass.Vertex import Vertex
from supervisor.slam.graph.baseclass.Edge import Edge
from supervisor.slam.graph.baseclass.Graph import Graph
from supervisor.slam.graph.vetor2matrix import *
from utils.math_util import normalize_angle
import math
""" Define Vertex Classes
......@@ -330,10 +329,7 @@ class LMGraph(Graph):
raise RuntimeError()
return pose, landmarks
def normalize_angles(self, vertices):
for v in vertices:
if isinstance(v, PoseVertex):
v.pose[2, 0] = normalize_angle(v.pose[2, 0])
def get_last_pose_vertex(self):
"""
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment