AtHeartEngineer/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-01-10 23:18:03 -05:00
Code Issues Packages Projects Releases Wiki Activity

fix deprecation warning for latest numpy (#480)

Browse Source 
* fix deprication version for latest numpy

* fix deprication version for latest numpy

* fix deprication version for latest numpy

* fix deprication version for latest numpy
This commit is contained in:
Atsushi Sakai
2021-02-10 22:02:51 +09:00
committed by GitHub
parent f495ae8983
commit b8f28cc6a3
9 changed files with 69 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
Mapping/lidar_to_grid_map/lidar_to_grid_map.py
View File

@@ -193,7 +193,7 @@ def generate_ray_casting_grid_map(ox, oy, xy_resolution, breshen=True):
(x_w, y_w),
(min_x, min_y), xy_resolution)
flood_fill((center_x, center_y), occupancy_map)
occupancy_map = np.array(occupancy_map, dtype=np.float)
occupancy_map = np.array(occupancy_map, dtype=float)
for (x, y) in zip(ox, oy):
ix = int(round((x - min_x) / xy_resolution))
iy = int(round((y - min_y) / xy_resolution))

2
PathPlanning/SpiralSpanningTreeCPP/spiral_spanning_tree_coverage_path_planner.py
View File

@@ -42,7 +42,7 @@ class SpiralSpanningTreeCoveragePlanner:
"""
visit_times = np.zeros(
(self.merged_map_height, self.merged_map_width), dtype=np.int)
(self.merged_map_height, self.merged_map_width), dtype=int)
visit_times[start[0]][start[1]] = 1
# generate route by

2
PathPlanning/WavefrontCPP/wavefront_coverage_path_planner.py
View File

@@ -47,7 +47,7 @@ def transform(
else:
sys.exit('Unsupported distance type.')
transform_matrix = float('inf') * np.ones_like(grid_map, dtype=np.float)
transform_matrix = float('inf') * np.ones_like(grid_map, dtype=float)
transform_matrix[src[0], src[1]] = 0
if transform_type == 'distance':
eT = np.zeros_like(grid_map)

6
SLAM/EKFSLAM/ekf_slam.py
View File

@@ -115,9 +115,9 @@ def jacob_motion(x, u):
Fx = np.hstack((np.eye(STATE_SIZE), np.zeros(
(STATE_SIZE, LM_SIZE * calc_n_lm(x)))))
jF = np.array([[0.0, 0.0, -DT * u[0] * math.sin(x[2, 0])],
[0.0, 0.0, DT * u[0] * math.cos(x[2, 0])],
[0.0, 0.0, 0.0]], dtype=np.float64)
jF = np.array([[0.0, 0.0, -DT * u[0, 0] * math.sin(x[2, 0])],
[0.0, 0.0, DT * u[0, 0] * math.cos(x[2, 0])],
[0.0, 0.0, 0.0]], dtype=float)
G = np.eye(STATE_SIZE) + Fx.T @ jF @ Fx

57
SLAM/GraphBasedSLAM/graphslam/graph.py
View File

@@ -8,16 +8,14 @@ r"""A ``Graph`` class that stores the edges and vertices required for Graph SLAM
"""
import warnings
from collections import defaultdict
from functools import reduce
import warnings
import matplotlib.pyplot as plt
import numpy as np
from scipy.sparse import SparseEfficiencyWarning, lil_matrix
from scipy.sparse.linalg import spsolve
import matplotlib.pyplot as plt
warnings.simplefilter("ignore", SparseEfficiencyWarning)
warnings.filterwarnings("ignore", category=SparseEfficiencyWarning)
@@ -44,6 +42,7 @@ class _Chi2GradientHessian:
The contributions to the Hessian matrix
"""
def __init__(self, dim):
self.chi2 = 0.
self.dim = dim
@@ -59,7 +58,6 @@ class _Chi2GradientHessian:
chi2_grad_hess : _Chi2GradientHessian
The ``_Chi2GradientHessian`` that will be updated
incoming : tuple
TODO
"""
chi2_grad_hess.chi2 += incoming[0]
@@ -100,6 +98,7 @@ class Graph(object):
A list of the vertices in the graph
"""
def __init__(self, edges, vertices):
# The vertices and edges lists
self._edges = edges
@@ -117,14 +116,16 @@ class Graph(object):
"""
index_id_dict = {i: v.id for i, v in enumerate(self._vertices)}
id_index_dict = {v_id: v_index for v_index, v_id in index_id_dict.items()}
id_index_dict = {v_id: v_index for v_index, v_id in
index_id_dict.items()}
# Fill in the vertices' `index` attribute
for v in self._vertices:
v.index = id_index_dict[v.id]
for e in self._edges:
e.vertices = [self._vertices[id_index_dict[v_id]] for v_id in e.vertex_ids]
e.vertices = [self._vertices[id_index_dict[v_id]] for v_id in
e.vertex_ids]
def calc_chi2(self):
r"""Calculate the :math:`\chi^2` error for the ``Graph``.
@@ -144,22 +145,34 @@ class Graph(object):
"""
n = len(self._vertices)
dim = len(self._vertices[0].pose.to_compact())
chi2_gradient_hessian = reduce(_Chi2GradientHessian.update, (e.calc_chi2_gradient_hessian() for e in self._edges), _Chi2GradientHessian(dim))
chi2_gradient_hessian = reduce(_Chi2GradientHessian.update,
(e.calc_chi2_gradient_hessian()
for e in self._edges),
_Chi2GradientHessian(dim))
self._chi2 = chi2_gradient_hessian.chi2
# Fill in the gradient vector
self._gradient = np.zeros(n * dim, dtype=np.float64)
for idx, contrib in chi2_gradient_hessian.gradient.items():
self._gradient[idx * dim: (idx + 1) * dim] += contrib
self._gradient = np.zeros(n * dim, dtype=float)
for idx, cont in chi2_gradient_hessian.gradient.items():
self._gradient[idx * dim: (idx + 1) * dim] += cont
# Fill in the Hessian matrix
self._hessian = lil_matrix((n * dim, n * dim), dtype=np.float64)
for (row_idx, col_idx), contrib in chi2_gradient_hessian.hessian.items():
self._hessian[row_idx * dim: (row_idx + 1) * dim, col_idx * dim: (col_idx + 1) * dim] = contrib
self._hessian = lil_matrix((n * dim, n * dim), dtype=float)
for (row_idx, col_idx), cont in chi2_gradient_hessian.hessian.items():
x_start = row_idx * dim
x_end = (row_idx + 1) * dim
y_start = col_idx * dim
y_end = (col_idx + 1) * dim
self._hessian[x_start:x_end, y_start:y_end] = cont
if row_idx != col_idx:
self._hessian[col_idx * dim: (col_idx + 1) * dim, row_idx * dim: (row_idx + 1) * dim] = np.transpose(contrib)
x_start = col_idx * dim
x_end = (col_idx + 1) * dim
y_start = row_idx * dim
y_end = (row_idx + 1) * dim
self._hessian[x_start:x_end, y_start:y_end] = \
np.transpose(cont)
def optimize(self, tol=1e-4, max_iter=20, fix_first_pose=True):
r"""Optimize the :math:`\chi^2` error for the ``Graph``.
@@ -189,8 +202,10 @@ class Graph(object):
# Check for convergence (from the previous iteration); this avoids having to calculate chi^2 twice
if i > 0:
rel_diff = (chi2_prev - self._chi2) / (chi2_prev + np.finfo(float).eps)
print("{:9d} {:20.4f} {:18.6f}".format(i, self._chi2, -rel_diff))
rel_diff = (chi2_prev - self._chi2) / (
chi2_prev + np.finfo(float).eps)
print(
"{:9d} {:20.4f} {:18.6f}".format(i, self._chi2, -rel_diff))
if self._chi2 < chi2_prev and rel_diff < tol:
return
else:
@@ -207,7 +222,7 @@ class Graph(object):
self._gradient[:dim] = 0.
# Solve for the updates
dx = spsolve(self._hessian, -self._gradient) # pylint: disable=invalid-unary-operand-type
dx = spsolve(self._hessian, -self._gradient)
# Apply the updates
for v, dx_i in zip(self._vertices, np.split(dx, n)):
@@ -216,7 +231,8 @@ class Graph(object):
# If we reached the maximum number of iterations, print out the final iteration's results
self.calc_chi2()
rel_diff = (chi2_prev - self._chi2) / (chi2_prev + np.finfo(float).eps)
print("{:9d} {:20.4f} {:18.6f}".format(max_iter, self._chi2, -rel_diff))
print("{:9d} {:20.4f} {:18.6f}".format(
max_iter, self._chi2, -rel_diff))
def to_g2o(self, outfile):
"""Save the graph in .g2o format.
@@ -234,7 +250,8 @@ class Graph(object):
for e in self._edges:
f.write(e.to_g2o())
def plot(self, vertex_color='r', vertex_marker='o', vertex_markersize=3, edge_color='b', title=None):
def plot(self, vertex_color='r', vertex_marker='o', vertex_markersize=3,
edge_color='b', title=None):
"""Plot the graph.
Parameters

8
SLAM/GraphBasedSLAM/graphslam/load.py
View File

@@ -8,7 +8,6 @@
"""
import logging
import numpy as np
@@ -19,7 +18,6 @@ from .pose.se2 import PoseSE2
from .util import upper_triangular_matrix_to_full_matrix
from .vertex import Vertex
_LOGGER = logging.getLogger(__name__)
@@ -44,7 +42,8 @@ def load_g2o_se2(infile):
for line in f.readlines():
if line.startswith("VERTEX_SE2"):
numbers = line[10:].split()
arr = np.array([float(number) for number in numbers[1:]], dtype=np.float64)
arr = np.array([float(number) for number in numbers[1:]],
dtype=float)
p = PoseSE2(arr[:2], arr[2])
v = Vertex(int(numbers[0]), p)
vertices.append(v)
@@ -52,7 +51,8 @@ def load_g2o_se2(infile):
if line.startswith("EDGE_SE2"):
numbers = line[9:].split()
arr = np.array([float(number) for number in numbers[2:]], dtype=np.float64)
arr = np.array([float(number) for number in numbers[2:]],
dtype=float)
vertex_ids = [int(numbers[0]), int(numbers[1])]
estimate = PoseSE2(arr[:2], arr[2])
information = upper_triangular_matrix_to_full_matrix(arr[3:], 3)

27
SLAM/GraphBasedSLAM/graphslam/pose/se2.py
View File

@@ -9,7 +9,6 @@ r"""Representation of a pose in :math:`SE(2)`.
"""
import math
import numpy as np
from ..util import neg_pi_to_pi
@@ -26,8 +25,10 @@ class PoseSE2(np.ndarray):
The angle of the pose (in radians)
"""
def __new__(cls, position, orientation):
obj = np.array([position[0], position[1], neg_pi_to_pi(orientation)], dtype=np.float64).view(cls)
obj = np.array([position[0], position[1], neg_pi_to_pi(orientation)],
dtype=float).view(cls)
return obj
# pylint: disable=arguments-differ
@@ -73,7 +74,9 @@ class PoseSE2(np.ndarray):
The pose as an :math:`SE(2)` matrix
"""
return np.array([[np.cos(self[2]), -np.sin(self[2]), self[0]], [np.sin(self[2]), np.cos(self[2]), self[1]], [0., 0., 1.]], dtype=np.float64)
return np.array([[np.cos(self[2]), -np.sin(self[2]), self[0]],
[np.sin(self[2]), np.cos(self[2]), self[1]],
[0., 0., 1.]], dtype=float)
@classmethod
def from_matrix(cls, matrix):
@@ -90,7 +93,8 @@ class PoseSE2(np.ndarray):
The matrix as a `PoseSE2` object
"""
return cls([matrix[0, 2], matrix[1, 2]], math.atan2(matrix[1, 0], matrix[0, 0]))
return cls([matrix[0, 2], matrix[1, 2]],
math.atan2(matrix[1, 0], matrix[0, 0]))
# ======================================================================= #
# #
@@ -131,7 +135,9 @@ class PoseSE2(np.ndarray):
The pose's inverse
"""
return PoseSE2([-self[0] * np.cos(self[2]) - self[1] * np.sin(self[2]), self[0] * np.sin(self[2]) - self[1] * np.cos([self[2]])], -self[2])
return PoseSE2([-self[0] * np.cos(self[2]) - self[1] * np.sin(self[2]),
self[0] * np.sin(self[2]) - self[1] * np.cos(
[self[2]])], -self[2])
# ======================================================================= #
# #
@@ -152,7 +158,10 @@ class PoseSE2(np.ndarray):
The result of pose composition
"""
return PoseSE2([self[0] + other[0] * np.cos(self[2]) - other[1] * np.sin(self[2]), self[1] + other[0] * np.sin(self[2]) + other[1] * np.cos(self[2])], neg_pi_to_pi(self[2] + other[2]))
return PoseSE2(
[self[0] + other[0] * np.cos(self[2]) - other[1] * np.sin(self[2]),
self[1] + other[0] * np.sin(self[2]) + other[1] * np.cos(self[2])
], neg_pi_to_pi(self[2] + other[2]))
def __sub__(self, other):
r"""Subtract poses (i.e., inverse pose composition): :math:`p_1 \ominus p_2`.
@@ -168,4 +177,8 @@ class PoseSE2(np.ndarray):
The result of inverse pose composition
"""
return PoseSE2([(self[0] - other[0]) * np.cos(other[2]) + (self[1] - other[1]) * np.sin(other[2]), (other[0] - self[0]) * np.sin(other[2]) + (self[1] - other[1]) * np.cos(other[2])], neg_pi_to_pi(self[2] - other[2]))
return PoseSE2([(self[0] - other[0]) * np.cos(other[2]) + (
self[1] - other[1]) * np.sin(other[2]),
(other[0] - self[0]) * np.sin(other[2]) + (
self[1] - other[1]) * np.cos(other[2])],
neg_pi_to_pi(self[2] - other[2]))

2
SLAM/GraphBasedSLAM/graphslam/util.py
View File

@@ -71,7 +71,7 @@ def upper_triangular_matrix_to_full_matrix(arr, n):
triu1 = np.triu_indices(n, 1)
tril1 = np.tril_indices(n, -1)
mat = np.zeros((n, n), dtype=np.float64)
mat = np.zeros((n, n), dtype=float)
mat[triu0] = arr
mat[tril1] = mat[triu1]

2
requirements.txt
View File

@@ -1,4 +1,4 @@
numpy == 1.19.5
numpy == 1.20.1
scipy == 1.6.0
pandas == 1.2.1
matplotlib == 3.3.4