def weight(self, lines):
'''
Look for the lines seen from the robot and compare them to the given map.
Lines expressed as [x1 y1 x2 y2].
'''
# TODO: code here!!
# Constant values for all weightings
val_rng = 1.0 / (self.meas_rng_noise * np.sqrt(2 * np.pi))
val_ang = 1.0 / (self.meas_ang_noise * np.sqrt(2 * np.pi))
tunung_factor = 15
# Loop over particles
for i in range(self.num):
map_polar = np.zeros((self.map.shape[0],2))
# Transform map lines to local frame and to [range theta]
for j in range(self.map.shape[0]):
map_polar[j,:] = get_polar_line(self.map[j,:],[self.p_xy[0,i],self.p_xy[1,i],self.p_ang[i]])
# Transform measured lines to [range theta] and weight them
for j in range(lines.shape[0]):
if ( (lines[j,0]-lines[j,2])**2 + (lines[j,1]-lines[j,3])**2 ) > 0:
local_map_polar = get_polar_line(lines[j,:])
w = 0
index = -1
# Weight them
for k in range(map_polar.shape[0]):
w_r = np.exp(-(map_polar[k,0]-local_map_polar[0])**2/(tunung_factor*2*self.odom_lin_sigma**2)) * val_rng
w_a = np.exp(-angle_wrap(map_polar[k,1]-local_map_polar[1])**2/(tunung_factor*2*self.odom_ang_sigma**2)) * val_ang
# w = np.maximum(w,w_r*w_a)
if w_r*w_a > w:
w = w_r * w_a
index = k
# self.p_wei[i] *= w
# OPTIONAL question
# make sure segments correspond, if not put weight to zero
len_line_map = np.linalg.norm([(self.map[index,0]-self.map[index,2]), (self.map[index,1]-self.map[index,3])])
len_line_odom = np.linalg.norm([(lines[j,0]-lines[j,2]), (lines[j,1]-lines[j,3])])
# Check if measured line is larger than best fitting map line
# and set p_wei to 0 if it is (best match is invalid)
if len_line_odom > 1.1*len_line_map: # 10% error margin
# not 0 to avoid problems with turning all values to 0
self.p_wei[i] *= 0.00000000000000001
else:
self.p_wei[i] *= w
# Normalize weights
self.p_wei /= np.sum(self.p_wei)
# TODO: Compute efficient number
self.n_eff = 1.0/np.sum(self.p_wei**2)
def weight(self, lines):
'''
Look for the lines seen from the robot and compare them to the given map.
Lines expressed as [x1 y1 x2 y2].
'''
# TODO: code here!!
# Constant values for all weightings
val_rng = 1.0 / (self.meas_rng_noise * np.sqrt(2 * np.pi))
val_ang = 1.0 / (self.meas_ang_noise * np.sqrt(2 * np.pi))
# Loop over particles
for i in range(self.num):
odompass = [self.p_xy[0, i], self.p_xy[1, i], self.p_ang[i]]
# Transform map lines to local frame and to [range theta]
room = np.zeros((self.map.shape[0], 2))
for j in range(self.map.shape[0]):
room[j, :] = get_polar_line(self.map[j, :], odompass)
# Transform measured lines to [range theta] and weight them
#print lines
for j in range(lines.shape[0]):
meas = get_polar_line(lines[j, :])
# Weight them
wr = val_rng * np.exp(
-np.power((meas[0] - room[:, 0]), 2) /
(2 * self.meas_rng_noise * self.meas_rng_noise))
wt = val_ang * np.exp(
-np.power((meas[1] - room[:, 1]), 2) /
(2 * self.meas_rng_noise * self.meas_rng_noise))
#optional part
# make sure segments correspond, if not put weight to zero
for k in range(room.shape[0]):
rooml = math.sqrt((self.map[k, 1] - self.map[k, 3]) *
(self.map[k, 1] - self.map[k, 3]) +
(self.map[k, 2] - self.map[k, 0]) *
(self.map[k, 2] - self.map[k, 0]))
measl = math.sqrt((lines[j, 1] - lines[j, 3]) *
(lines[j, 1] - lines[j, 3]) +
(lines[j, 2] - lines[j, 0]) *
(lines[j, 2] - lines[j, 0]))
if rooml < measl:
wr[k] = 0
wt[k] = 0
# Take best weighting (best associated lines)
self.p_wei[i] *= np.max(wr * wt)
# Normalize weights
self.p_wei /= np.sum(self.p_wei)
# TODO: Compute efficient number
self.n_eff = 1 / np.sum(self.p_wei * self.p_wei)
def weight(self, lines):
'''
Look for the lines seen from the robot and compare them to the given map.
Lines expressed as [x1 y1 x2 y2].
'''
# TODO: code here!!
# Constant values for all weightings
val_rng = 1.0 / (self.meas_rng_noise * np.sqrt(2 * np.pi))
val_ang = 1.0 / (self.meas_ang_noise * np.sqrt(2 * np.pi))
# Loop over particles
for i in range(self.num):
# Transform map lines to local frame and to [range theta]
map_polar = np.zeros((self.map.shape[0], 2))
d1 = np.zeros(map_polar.shape[0])
for j in range(self.map.shape[0]):
map_polar[j, :] = get_polar_line(
self.map[j, :],
np.array([self.p_xy[0, i], self.p_xy[1, i],
self.p_ang[i]]))
d1[j] = np.sqrt((self.map[j, 2] - self.map[j, 0])**2 +
(self.map[j, 3] - self.map[j, 1])**2)
# Transform measured lines to [range theta] and weight them
lines_polar = np.zeros((lines.shape[0], 2))
for j in range(lines.shape[0]):
# Weight them
modify = np.ones(map_polar.shape[0])
d2 = np.sqrt((lines[j, 2] - lines[j, 0])**2 +
(lines[j, 3] - lines[j, 1])**2)
lines_polar[j, :] = get_polar_line(lines[j, :])
for c in range(self.map.shape[0]):
if d1[c] < d2:
modify[c] = 0
WR = val_rng * np.exp(-(
(lines_polar[j, 0] - map_polar[:, 0])**2) /
((2 * self.meas_rng_noise)**2))
Wang = val_ang * np.exp(-(
(lines_polar[j, 1] - map_polar[:, 1])**2) /
((2 * self.meas_ang_noise)**2))
self.p_wei[i] *= np.max(Wang * WR * modify)
# Normalize weights
self.p_wei /= np.sum(self.p_wei)
# TODO: Compute efficient number
self.n_eff = 1.0 / np.sum(self.p_wei**2)
def data_association(self, lines):
"""
Look for closest correspondences.
The correspondences are between the provided measured lines and the map
known a priori.
Input:
lines : n
f4 matrix with a segment in each row as [x1 y1 x2 y2]
Return:
Hk_list : list of 2x3 matrices (jacobian)
Yk_list : list of 2x1 matrices (innovation)
Sk_list : list of 2x2 matrices (innovation uncertainty)
Rk_list : list of 2x2 matrices (measurement uncertainty)
"""
# TODO: Program this function
################################################################
# 1. Map lines (self.map) to polar robot frame: get_polar_line
# 2. Sensed lines (lines) to polar robot frame: get_polar_line
# 3. Data association
# Init variables
chi_thres = 0.103 # chi square 2DoF 95% confidence
associd = list()
Hk_list = list()
Vk_list = list()
Sk_list = list()
Rk_list = list()
#return Hk_list, Vk_list, Sk_list, Rk_list # TODO: delete this line
# For each obseved line
#print('\n-------- Associations --------')
for i in range(0, lines.shape[0]):
# Get the polar line representation in robot frame
z = funcs.get_polar_line(lines[i, :])
# Variables for finding minimum
minD = 1e9
minj = -1
# For each line in the known map
for j in range(0, self.map.shape[0]):
# Compute matrices
h = funcs.get_polar_line(self.map[j, :], self.xk)
H = self.jacobianH(funcs.get_polar_line(self.map[j, :]))
v = z - h
S = np.dot(np.dot(H, self.Pk), H.T) + self.Rk
# Mahalanobis distance
D = (np.dot(np.dot(v.T, np.linalg.inv(S)), v))
# Optional: Check if observed line is longer than map
########################################################
islonger = False
mapl = np.sqrt((self.map[j, 1] - self.map[j, 3]) *
(self.map[j, 1] - self.map[j, 3]) +
(self.map[j, 2] - self.map[j, 0]) *
(self.map[j, 2] - self.map[j, 0]))
measl = np.sqrt((lines[i, 1] - lines[i, 3]) *
(lines[i, 1] - lines[i, 3]) +
(lines[i, 2] - lines[i, 0]) *
(lines[i, 2] - lines[i, 0]))
if mapl < measl:
islonger = True
# Check if the obseved line is the one with smallest
# mahalanobis distance
if np.sqrt(D) < minD and not islonger:
minj = j
minz = z
minh = h
minH = H
minv = v
minS = S
minD = D
# Minimum distance below threshold
if minD < chi_thres:
#print("\t{:.2f} -> {:.2f}".format(minz, minh))
# Append results
associd.append([i, minj])
Hk_list.append(minH)
Vk_list.append(minv)
Sk_list.append(minS)
Rk_list.append(self.Rk)
return Hk_list, Vk_list, Sk_list, Rk_list
def weight(self, lines):
'''
Look for the lines seen from the robot and compare them to the given map.
Lines expressed as [x1 y1 x2 y2].
'''
# check point
print(
'------------------------weighting------------------------------- '
)
# TODO: code here!!
# Constant values for all weightings
val_rng = 1.0 / (self.meas_rng_noise * np.sqrt(2 * np.pi))
val_ang = 1.0 / (self.meas_ang_noise * np.sqrt(2 * np.pi))
# Loop over particles
for i in range(self.num):
particle_pos = np.array(
[self.p_xy[0, i], self.p_xy[1, i], self.p_ang[i]])
# Transform map lines to local frame and to [range theta]
map_polar = np.zeros((self.map.shape[0], 2))
for j in range(self.map.shape[0]):
map_polar[j, :] = get_polar_line(self.map[j, :], particle_pos)
# Transform measured lines to [range theta] and weight them
#print(map_polar)
line_max_wei = np.zeros(lines.shape[0])
for j in range(lines.shape[0]):
#
lines_polar = get_polar_line(lines[j, :])
#print(lines_polar)
#
# Weight them
for m in range(self.map.shape[0]):
wei_rng = val_rng * exp(
-(lines_polar[0] - map_polar[m, 0])**2 /
(2 * (self.meas_rng_noise**2)))
wei_ang = val_ang * exp(
-(lines_polar[1] - map_polar[m, 1])**2 /
(2 * (self.meas_ang_noise**2)))
# OPTIONAL question
# make sure segments correspond, if not put weight to zero
map_seg = math.sqrt((self.map[m, 2] - self.map[m, 0])**2 +
(self.map[m, 3] - self.map[m, 1])**2)
line_seg = math.sqrt((lines[j, 2] - lines[j, 0])**2 +
(lines[j, 3] - lines[j, 1])**2)
if line_seg > map_seg:
wei_rng = 0
wei_ang = 0
line_wei = wei_rng * wei_ang
if line_wei > line_max_wei[j]:
line_max_wei[j] = line_wei
# Take best weighting (best associated lines)
self.p_wei[i] *= np.prod(line_max_wei)
# Normalize weights
self.p_wei /= np.sum(self.p_wei)
#print(self.p_wei)
# TODO: Compute efficient number
self.n_eff = 1.0 / np.sum(self.p_wei**2)
def data_association(self, lines):
"""
Look for closest correspondences.
The correspondences are between the provided measured lines and the map
known a priori.
Input:
lines : nx4 matrix with a segment in each row as [x1 y1 x2 y2]
Return:
Hk_list : list of 2x3 matrices (jacobian)
Yk_list : list of 2x1 matrices (innovation)
Sk_list : list of 2x2 matrices (innovation uncertainty)
Rk_list : list of 2x2 matrices (measurement uncertainty)
"""
################################################################
# 1. Map lines (self.map) to polar robot frame: get_polar_line
# 2. Sensed lines (lines) to polar robot frame: get_polar_line
# 3. Data association
# Init variables
# chi_thres = 0.103 # chi square 2DoF 95% confidence
chi_thres = 0.4
associd = list()
Hk_list = list()
Vk_list = list()
Sk_list = list()
Rk_list = list()
# Get the lengths for map lines and measured lines
map_length = np.sqrt(np.power(self.map[:,2] - self.map[:,0],2) + np.power(self.map[:,3] - self.map[:,1],2))
lines_length = np.sqrt(np.power(lines[:,2] - lines[:,0],2) + np.power(lines[:,3] - lines[:,1],2))
# For each observed line
print('\n-------- Associations --------')
for i in range(0, lines.shape[0]):
# Get the polar line representation in robot frame
z = funcs.get_polar_line(lines[i],[0,0,0])
# Variables for finding minimum
minD = 1e9
minj = -1
# For each line in the known map
for j in range(0, self.map.shape[0]):
# Compute matrices
h = funcs.get_polar_line(self.map[j],[0,0,0]) # The map line is in the world frame now, only used to calculate the jacobian
H = self.jacobianH(h,self.xk)
h = funcs.get_polar_line(self.map[j],self.xk) # Map line is in the robot frame now, used to get innovation
v = z - h
S = np.dot(np.dot(H,self.Pk),np.transpose(H)) + self.Rk
# Mahalanobis distance
D = np.dot(np.dot(np.transpose(v), np.linalg.inv(S)), v)
# Optional: Check if observed line is longer than map
########################################################
islonger = False
if lines_length[i] > map_length[j]:
islonger = True
# Check if the obseved line is the one with smallest
# mahalanobis distance
if np.sqrt(D) < minD and not islonger:
minj = j
minz = z
minh = h
minH = H
minv = v
minS = S
minD = D
# Minimum distance below threshold
if minD < chi_thres:
print("\t{} -> {}".format(minz, minh))
# Append results
associd.append([i, minj])
Hk_list.append(minH)
Vk_list.append(minv)
Sk_list.append(minS)
Rk_list.append(self.Rk)
return Hk_list, Vk_list, Sk_list, Rk_list