def test_conv_pnt_local_2_global(self):
local_fr = Pose2D(1, 1, 45.0/180.0 * math.pi)
loc_pnt = Coord2D(x=math.sqrt(2), y=-math.sqrt(2))
glo_pnt = conv_pnt_local_2_global(local_fr, loc_pnt)
self.assertAlmostEqual(glo_pnt.x, 3.0, places=5)
self.assertAlmostEqual(glo_pnt.y, 1.0, places=5)
def __init__(self,
*,
particle_num=10,
init_pose=Pose2D(),
sampler=LowVarianceSampler(),
err_model,
likelihood_generator,
grid_map_conf,
inv_sens_model):
self._particle_num = particle_num
self._init_pose = init_pose
self._sampler = sampler
self._err_model = err_model
self._likelihood_generator = likelihood_generator
self._inv_sens_model = inv_sens_model
# Particle Vector Generation
map_elem = OccupancyGridMap2D(conf=grid_map_conf)
particle = ParticleSLAM(pose=self._init_pose,
weight=float(1 / self._particle_num),
map=map_elem)
self._particles = [
copy.deepcopy(particle) for i in range(self._particle_num)
]
def test_conv_pnt_global_2_local1(self):
local_fr = Pose2D(1, 1, 45.0/180.0 * math.pi)
glo_pnt = Coord2D(x=3.0, y=1.0)
loc_pnt = conv_pnt_global_2_local(local_fr, glo_pnt)
self.assertAlmostEqual(loc_pnt.x, math.sqrt(2), places=5)
self.assertAlmostEqual(loc_pnt.y, -math.sqrt(2), places=5)
def __convert_numpy_list_2_list_poses(self, np_list_poses):
list_poses = [Pose2D() for i in range(np_list_poses.shape[1])]
for i in range(np_list_poses.shape[1]):
list_poses[i].x = np_list_poses[0, i]
list_poses[i].y = np_list_poses[1, i]
list_poses[i].theta = np_list_poses[2, i]
return list_poses
def __init__(self, *, config, landmarks, ini_pose=Pose2D()):
self._conf = config
self._landmarks = landmarks
self._last_avg = ini_pose
self._mm_pose = ini_pose
self._last_sigma = np.matrix(np.zeros((3, 3)))
self._Q = np.matrix(np.eye(3))
self._Q[0, 0] = self._conf.err_r**2
self._Q[1, 1] = self._conf.err_phi**2
self._Q[2, 2] = self._conf.err_feat**2
def _calc_jacobi_H(self, pose_mat, lm_glob):
pose = Pose2D(x=pose_mat[0, 0], y=pose_mat[1, 0], theta=pose_mat[2, 0])
r = (lm_glob.x - pose.x)**2 + (lm_glob.y - pose.y)**2
H = np.matrix(np.zeros((3, 3)))
H[0, 0] = -(lm_glob.x - pose.x) / math.sqrt(r)
H[1, 0] = (lm_glob.y - pose.y) / r
H[0, 1] = -(lm_glob.y - pose.y) / math.sqrt(r)
H[1, 1] = -(lm_glob.x - pose.x) / r
H[1, 2] = -1
return H
def _predict(self, control, last_pose):
v = control.v
omg = control.omg
theta = last_pose.theta
dT = self._conf.dT
pred_pose = Pose2D()
pred_pose.x = last_pose.x + v * dT * math.cos(theta - omg * dT / 2.0)
pred_pose.y = last_pose.y + v * dT * math.sin(theta - omg * dT / 2.0)
pred_pose.theta = normalize_angle_pi_2_pi(theta + omg * dT)
G = self._calc_jacobi_G(control, last_pose)
V, M = self.__calc_control_transmat_V_M(control, last_pose)
pred_sigma = G * self._last_sigma * G.T + V * M * V.T
return pred_pose, pred_sigma
def _correct(self, pred_pose, pred_sigma, ekf_kc_meas):
pose_mat = np.matrix(pred_pose.numpy_array()).T
sigma = copy.deepcopy(pred_sigma)
pred_meas_list = []
S_list = []
z_diff_list = []
for index, meas in enumerate(ekf_kc_meas._measurements):
# Measurement Update for each landmark.
lm_idx = ekf_kc_meas._corresp[index]
lm_glob = self._landmarks[lm_idx]
calc_meas = self._calc_measurement(pose_mat, lm_glob)
pred_meas_list.append(calc_meas)
H = self._calc_jacobi_H(pose_mat, lm_glob)
S = H * sigma * H.T + self._Q
S_list.append(copy.deepcopy(S))
K = sigma * H.T * np.linalg.inv(S)
z_diff_mat = np.matrix(meas.numpy_array() -
calc_meas.numpy_array()).T
z_diff_mat[1, 0] = normalize_angle_pi_2_pi(z_diff_mat[1, 0])
if (abs(z_diff_mat[0, 0]) > 5 or abs(z_diff_mat[1, 0]) > 0.5):
sys.exit()
z_diff_list.append(copy.deepcopy(z_diff_mat))
pose_mat = pose_mat + K * (z_diff_mat)
sigma = (np.matrix(np.eye(3)) - K * H) * sigma
likelihood = 1.0
for index, meas in enumerate(ekf_kc_meas._measurements):
S_inv = np.linalg.inv(S_list[index])
z_diff = z_diff_list[index]
likelihood = likelihood \
* 1 / math.sqrt(2 * math.pi) \
* math.exp(-1/2*(z_diff).T * S_inv * (z_diff))
normalized_theta = normalize_angle_pi_2_pi(pose_mat[2, 0])
updated_pose = Pose2D(x=pose_mat[0, 0],
y=pose_mat[1, 0],
theta=normalized_theta)
return updated_pose, sigma, likelihood
def create_robocup_ellipse_path(dT):
a = 1.5
b = 0.9
omg = 2 * math.pi / 60.0
start = -3.0 / 4.0 * math.pi
end = 3.0 / 4.0 * math.pi
path = []
controls = []
for angle in np.arange(start, end, omg * dT):
x = a * math.cos(angle)
y = b * math.sin(angle)
dx_dt = -a * math.sin(angle) * omg
dy_dt = b * math.cos(angle) * omg
v = math.sqrt(dx_dt**2 + dy_dt**2)
theta = normalize_angle_pi_2_pi(math.atan2(dy_dt, dx_dt))
path.append(Pose2D(x=x, y=y, theta=theta))
controls.append(Control(v=v, omg=omg))
return path, controls
def _calc_measurement(self, pose_mat, lm_glob):
pose = Pose2D(x=pose_mat[0, 0], y=pose_mat[1, 0], theta=pose_mat[2, 0])
lm_loc = conv_pnt_global_2_local(pose, lm_glob)
r = math.sqrt(lm_loc.x**2 + lm_loc.y**2)
phi = normalize_angle_pi_2_pi(math.atan2(lm_loc.y, lm_loc.x))
return LandmarkMeas(r=r, phi=phi)
def __init__(self, *, pose=Pose2D(), weight=1.0, map):
self._pose_list = [pose]
self._weight = weight
self._map = map
grid2d_odom_disp = GridMap2D(gridmap_config=grid_conf)
grid2d_pose = GridMap2D(gridmap_config=grid_conf)
# Create Testing Configuration.
create_sample_room(grid2d_gt)
create_sample_room(grid2d_odom_disp)
create_sample_room(grid2d_pose)
path = create_sample_robot_path(grid2d_odom_disp)
# Visualization Preparation
fig, ax_main = plt.subplots(nrows=1,ncols=1,figsize=(13, 9),dpi=100)
fig, (ax00, ax10) = plt.subplots(nrows=2,ncols=1,figsize=(4, 6),dpi=100)
grid2d_gt.show_heatmap(ax00)
grid2d_odom_disp.show_heatmap(ax10)
cur_particle_poses = [Pose2D() for i in range(particle_num)]
last_particle_poses = [Pose2D() for i in range(particle_num)]
last_pose = Pose2D()
for index, cur_pose in enumerate(path):
print("Loop {0}".format(str(index)))
if index == 0:
last_pose = cur_pose
for i in range(particle_num):
cur_particle_poses[i] = cur_pose
continue
# Sample Artificial Odometry Error
cur_particle_poses = \
err_model.sample_motion(cur_odom=cur_pose, \
import math
# Common Module
from common.container import Pose2D
# Motion Model
from motion_model.motion_model import MotionErrorModel2dConfigParams
from motion_model.motion_model import MotionErrorModel2D
if __name__ == "__main__":
import matplotlib.pyplot as plt
print("Sample Motion")
path = [Pose2D(0.0, 0.0, 0.0), Pose2D(0.0, 0.0, 0.0), Pose2D(1.0, 0.0, 0.0), \
Pose2D(2.0, 0.0, 0.0), Pose2D(3.0, 0.0, 0.0), Pose2D(4.0, 0.0, 0.0), \
Pose2D(5.0, 0.0, 0.0), Pose2D(5.0, 0.0, math.pi/2.0), \
Pose2D(5.0, 1.0, math.pi/2.0), Pose2D(5.0, 2.0, math.pi/2.0), \
Pose2D(5.0, 2.0, math.pi/1.0), Pose2D(4.0, 2.0, math.pi/1.0), \
Pose2D(3.0, 2.0, math.pi/1.0), Pose2D(2.0, 2.0, math.pi/1.0), \
Pose2D(1.0, 2.0, math.pi/1.0), Pose2D(0.0, 2.0, math.pi/1.0)]
# Error Model
err_conf = MotionErrorModel2dConfigParams(std_rot_per_rot=0.01,
std_rot_per_trans=0.02,
std_trans_per_trans=0.05,
std_trans_per_rot=0.01)
err_model = MotionErrorModel2D(conf=err_conf)
particle_num = 5
if (map2d_src.is_valid_global_coord(10.0, y_coor)):
map2d_src.update_val_via_global_coord(x=10.0, y=y_coor, value=1.0)
range_max = 10.0
# Inverse Sensor Model
inv_lidar_conf = InverseRangeSensorModelConfigParams(\
range_max=range_max,
l0 = 0.5, locc = 1.0, lfree = 0.0, \
alpha = 0.4, beta = 0.0)
inv_lidar_model = InverseRangeSensorModel(conf=inv_lidar_conf)
# Lidar Property
lidar_config = VirtualLidar2DConfigParams(range_max=range_max, \
min_angle=-math.pi/2.0, \
max_angle=math.pi/2.0, \
angle_res=math.pi/360.0, \
sigma=2.0)
v_lidar = VirtualLidar2D(lidar_config=lidar_config)
pose = Pose2D(x=5.0, y=0.0, theta=45.0 / 180.0 * math.pi)
scans = v_lidar.generate_scans(pose, map2d_src)
fig, ax = plt.subplots(1, 1)
#v_lidar.register_fixed_scan(pose=pose, scans=scans, map2d=map2d_dst)
#map2d_dst.register_scan(pose=pose, scans=scans)
inv_lidar_model.register_fixed_scan(pose=pose,
scans=scans,
map2d=map2d_dst)
map2d_dst.show_heatmap(ax)
plt.show()
def create_sample_robot_path(map2d, trans_reso=1.0, angle_reso=math.pi / 2.0):
path = []
for i in range(34 * 4):
path.append(Pose2D(-49 + i * 0.25, -17.5, 0))
path.append(Pose2D(-15, -17.5, math.pi / 2.0 * 0.1))
path.append(Pose2D(-15, -17.5, math.pi / 2.0 * 0.2))
path.append(Pose2D(-15, -17.5, math.pi / 2.0 * 0.3))
path.append(Pose2D(-15, -17.5, math.pi / 2.0 * 0.4))
path.append(Pose2D(-15, -17.5, math.pi / 2.0 * 0.5))
path.append(Pose2D(-15, -17.5, math.pi / 2.0 * 0.6))
path.append(Pose2D(-15, -17.5, math.pi / 2.0 * 0.7))
path.append(Pose2D(-15, -17.5, math.pi / 2.0 * 0.8))
path.append(Pose2D(-15, -17.5, math.pi / 2.0 * 0.9))
path.append(Pose2D(-15, -17.5, math.pi / 2.0))
for i in range(35 * 4):
path.append(Pose2D(-15, -17.5 + i * 0.25, math.pi / 2.0))
path.append(Pose2D(-15, 17.5, math.pi / 2.0 + math.pi / 2.0 * 0.1))
path.append(Pose2D(-15, 17.5, math.pi / 2.0 + math.pi / 2.0 * 0.2))
path.append(Pose2D(-15, 17.5, math.pi / 2.0 + math.pi / 2.0 * 0.3))
path.append(Pose2D(-15, 17.5, math.pi / 2.0 + math.pi / 2.0 * 0.4))
path.append(Pose2D(-15, 17.5, math.pi / 2.0 + math.pi / 2.0 * 0.5))
path.append(Pose2D(-15, 17.5, math.pi / 2.0 + math.pi / 2.0 * 0.6))
path.append(Pose2D(-15, 17.5, math.pi / 2.0 + math.pi / 2.0 * 0.7))
path.append(Pose2D(-15, 17.5, math.pi / 2.0 + math.pi / 2.0 * 0.8))
path.append(Pose2D(-15, 17.5, math.pi / 2.0 + math.pi / 2.0 * 0.9))
path.append(Pose2D(-15, 17.5, math.pi / 2.0 + math.pi / 2.0 * 1.0))
for i in range(22 * 4):
path.append(Pose2D(-15 - i * 0.25, 17.5, math.pi))
path.append(Pose2D(-37, 17.5, math.pi + math.pi / 2.0 * 0.1))
path.append(Pose2D(-37, 17.5, math.pi + math.pi / 2.0 * 0.2))
path.append(Pose2D(-37, 17.5, math.pi + math.pi / 2.0 * 0.3))
path.append(Pose2D(-37, 17.5, math.pi + math.pi / 2.0 * 0.4))
path.append(Pose2D(-37, 17.5, math.pi + math.pi / 2.0 * 0.5))
path.append(Pose2D(-37, 17.5, math.pi + math.pi / 2.0 * 0.6))
path.append(Pose2D(-37, 17.5, math.pi + math.pi / 2.0 * 0.7))
path.append(Pose2D(-37, 17.5, math.pi + math.pi / 2.0 * 0.8))
path.append(Pose2D(-37, 17.5, math.pi + math.pi / 2.0 * 0.9))
path.append(Pose2D(-37, 17.5, math.pi + math.pi / 2.0 * 1.0))
for i in range(35 * 4):
path.append(Pose2D(-37, 17.5 - i * 0.25, math.pi * 3.0 / 2.0))
path.append(Pose2D(-37, -17.5, math.pi * 3 / 2.0 + math.pi / 2.0 * 0.1))
path.append(Pose2D(-37, -17.5, math.pi * 3 / 2.0 + math.pi / 2.0 * 0.2))
path.append(Pose2D(-37, -17.5, math.pi * 3 / 2.0 + math.pi / 2.0 * 0.3))
path.append(Pose2D(-37, -17.5, math.pi * 3 / 2.0 + math.pi / 2.0 * 0.4))
path.append(Pose2D(-37, -17.5, math.pi * 3 / 2.0 + math.pi / 2.0 * 0.5))
path.append(Pose2D(-37, -17.5, math.pi * 3 / 2.0 + math.pi / 2.0 * 0.6))
path.append(Pose2D(-37, -17.5, math.pi * 3 / 2.0 + math.pi / 2.0 * 0.7))
path.append(Pose2D(-37, -17.5, math.pi * 3 / 2.0 + math.pi / 2.0 * 0.8))
path.append(Pose2D(-37, -17.5, math.pi * 3 / 2.0 + math.pi / 2.0 * 0.9))
path.append(Pose2D(-37, -17.5, math.pi * 3 / 2.0 + math.pi / 2.0 * 1.0))
for i in range(61 * 4):
path.append(Pose2D(-37 + i * 0.25, -17.5, 0))
path.append(Pose2D(24, -17.5, math.pi / 2.0 * 0.1))
path.append(Pose2D(24, -17.5, math.pi / 2.0 * 0.2))
path.append(Pose2D(24, -17.5, math.pi / 2.0 * 0.3))
path.append(Pose2D(24, -17.5, math.pi / 2.0 * 0.4))
path.append(Pose2D(24, -17.5, math.pi / 2.0 * 0.5))
path.append(Pose2D(24, -17.5, math.pi / 2.0 * 0.6))
path.append(Pose2D(24, -17.5, math.pi / 2.0 * 0.7))
path.append(Pose2D(24, -17.5, math.pi / 2.0 * 0.8))
path.append(Pose2D(24, -17.5, math.pi / 2.0 * 0.9))
path.append(Pose2D(24, -17.5, math.pi / 2.0 * 1.0))
for i in range(35 * 4):
path.append(Pose2D(24, -17.5 + i * 0.25, math.pi / 2.0))
path.append(Pose2D(24, 17.5, math.pi / 2.0 + math.pi / 2.0 * 0.1))
path.append(Pose2D(24, 17.5, math.pi / 2.0 + math.pi / 2.0 * 0.2))
path.append(Pose2D(24, 17.5, math.pi / 2.0 + math.pi / 2.0 * 0.3))
path.append(Pose2D(24, 17.5, math.pi / 2.0 + math.pi / 2.0 * 0.4))
path.append(Pose2D(24, 17.5, math.pi / 2.0 + math.pi / 2.0 * 0.5))
path.append(Pose2D(24, 17.5, math.pi / 2.0 + math.pi / 2.0 * 0.6))
path.append(Pose2D(24, 17.5, math.pi / 2.0 + math.pi / 2.0 * 0.7))
path.append(Pose2D(24, 17.5, math.pi / 2.0 + math.pi / 2.0 * 0.8))
path.append(Pose2D(24, 17.5, math.pi / 2.0 + math.pi / 2.0 * 0.9))
path.append(Pose2D(24, 17.5, math.pi / 2.0 + math.pi / 2.0 * 1.0))
for i in range(20 * 4):
path.append(Pose2D(24 - i * 0.25, 17.5, math.pi))
path.append(Pose2D(4, 17.5, math.pi + math.pi / 2.0 * 0.1))
path.append(Pose2D(4, 17.5, math.pi + math.pi / 2.0 * 0.2))
path.append(Pose2D(4, 17.5, math.pi + math.pi / 2.0 * 0.3))
path.append(Pose2D(4, 17.5, math.pi + math.pi / 2.0 * 0.4))
path.append(Pose2D(4, 17.5, math.pi + math.pi / 2.0 * 0.5))
path.append(Pose2D(4, 17.5, math.pi + math.pi / 2.0 * 0.6))
path.append(Pose2D(4, 17.5, math.pi + math.pi / 2.0 * 0.7))
path.append(Pose2D(4, 17.5, math.pi + math.pi / 2.0 * 0.8))
path.append(Pose2D(4, 17.5, math.pi + math.pi / 2.0 * 0.9))
path.append(Pose2D(4, 17.5, math.pi + math.pi / 2.0 * 1.0))
for i in range(32 * 4):
path.append(Pose2D(4, 17.5 - i * 0.25, math.pi * 3.0 / 2.0))
path.append(Pose2D(4, -14.5, math.pi * 3.0 / 2.0 + math.pi / 2.0 * 0.1))
path.append(Pose2D(4, -14.5, math.pi * 3.0 / 2.0 + math.pi / 2.0 * 0.2))
path.append(Pose2D(4, -14.5, math.pi * 3.0 / 2.0 + math.pi / 2.0 * 0.3))
path.append(Pose2D(4, -14.5, math.pi * 3.0 / 2.0 + math.pi / 2.0 * 0.4))
path.append(Pose2D(4, -14.5, math.pi * 3.0 / 2.0 + math.pi / 2.0 * 0.5))
path.append(Pose2D(4, -14.5, math.pi * 3.0 / 2.0 + math.pi / 2.0 * 0.6))
path.append(Pose2D(4, -14.5, math.pi * 3.0 / 2.0 + math.pi / 2.0 * 0.7))
path.append(Pose2D(4, -14.5, math.pi * 3.0 / 2.0 + math.pi / 2.0 * 0.8))
path.append(Pose2D(4, -14.5, math.pi * 3.0 / 2.0 + math.pi / 2.0 * 0.9))
path.append(Pose2D(4, -14.5, math.pi * 3.0 / 2.0 + math.pi / 2.0 * 1.0))
for i in range(18 * 4):
path.append(Pose2D(4 + i * 0.25, -14.5, 0))
path.append(Pose2D(22, -14.5, math.pi / 2.0 * 0.1))
path.append(Pose2D(22, -14.5, math.pi / 2.0 * 0.2))
path.append(Pose2D(22, -14.5, math.pi / 2.0 * 0.3))
path.append(Pose2D(22, -14.5, math.pi / 2.0 * 0.4))
path.append(Pose2D(22, -14.5, math.pi / 2.0 * 0.5))
path.append(Pose2D(22, -14.5, math.pi / 2.0 * 0.6))
path.append(Pose2D(22, -14.5, math.pi / 2.0 * 0.7))
path.append(Pose2D(22, -14.5, math.pi / 2.0 * 0.8))
path.append(Pose2D(22, -14.5, math.pi / 2.0 * 0.9))
path.append(Pose2D(22, -14.5, math.pi / 2.0 * 1.0))
for i in range(30 * 4):
path.append(Pose2D(22, -14.5 + i * 0.25, math.pi / 2.0))
path.append(Pose2D(22, 15.5, math.pi / 2.0 * 0.9))
path.append(Pose2D(22, 15.5, math.pi / 2.0 * 0.8))
path.append(Pose2D(22, 15.5, math.pi / 2.0 * 0.7))
path.append(Pose2D(22, 15.5, math.pi / 2.0 * 0.8))
path.append(Pose2D(22, 15.5, math.pi / 2.0 * 0.5))
path.append(Pose2D(22, 15.5, math.pi / 2.0 * 0.4))
path.append(Pose2D(22, 15.5, math.pi / 2.0 * 0.3))
path.append(Pose2D(22, 15.5, math.pi / 2.0 * 0.2))
path.append(Pose2D(22, 15.5, math.pi / 2.0 * 0.1))
path.append(Pose2D(22, 15.5, math.pi / 2.0 * 0.0))
for i in range(27 * 4):
path.append(Pose2D(22 + i * 0.25, 15.5, 0))
for pose in path:
GridMap2DDrawer.draw_point(map2d, pose.x, pose.y, 0.25, 1.0)
return path
map2d_src = GridMap2D(gridmap_config=grid_conf)
for i in range(int(50 / 0.2)):
y_coor = i * grid_conf.reso - 25.0
if (map2d_src.is_valid_global_coord(10.0, y_coor)):
map2d_src.update_val_via_global_coord(x=10.0, y=y_coor, value=1.0)
# Lidar Property
lidar_config = VirtualLidar2DConfigParams(range_max=10.0, \
min_angle=-math.pi/2.0, \
max_angle=math.pi/2.0, \
angle_res=math.pi/360.0, \
sigma=2.0)
fakeScanGen = VirtualLidar2D(lidar_config=lidar_config)
true_pose = Pose2D(x=5.0, y=0.0, theta=45.0 / 180.0 * math.pi)
diff_pose = Pose2D(x=4.3, y=0.0, theta=42.0 / 180.0 * math.pi)
scans = fakeScanGen.generate_scans(true_pose, map2d_src)
scan_match_cfg = ScanMatcherConfigParams(
zhit=0.8, \
zshort=0.0,
zmax=0.1,
zmax_width=0.2,
zrand=0.1,
lambda_short=0.0)
likelihoodGen = ScanMatcher(lidar_config=lidar_config,
scan_match_config=scan_match_cfg)
prob = likelihoodGen.calc_likelihood(pose=diff_pose,