def compute_predicted_measurements(self):
"""
Adapt this method from EkfLocalization.compute_predicted_measurements().
"""
J = (self.x.size - 3) // 2
hs = np.zeros((2, J))
Hx_list = []
for j in range(J):
idx_j = 3 + 2 * j
alpha, r = self.x[idx_j:idx_j + 2]
Hx = np.zeros((2, self.x.size))
########## Code starts here ##########
# TODO: Compute h, Hx.
# HINT: Call tb.transform_line_to_scanner_frame() for the j'th map line.
# HINT: The first 3 columns of Hx should be populated using the same approach as in EkfLocalization.compute_predicted_measurements().
# HINT: The first two map lines (j=0,1) are fixed so the Jacobian of h wrt the alpha and r for those lines is just 0.
# HINT: For the other map lines (j>2), write out h in terms of alpha and r to get the Jacobian Hx.
h, Hx[:, :3] = tb.transform_line_to_scanner_frame(
np.array([alpha, r]), self.x[:3], self.tf_base_to_camera)
# First two map lines are assumed fixed so we don't want to propagate
# any measurement correction to them.
if j >= 2:
Hx[:, idx_j:idx_j + 2] = np.eye(2)
x_b_w, y_b_w, theta_b_w = self.x[:3]
x_c_b, y_c_b, theta_c_b = self.tf_base_to_camera
x_cam = x_c_b * np.cos(theta_b_w) - y_c_b * np.sin(
theta_b_w) + x_b_w
y_cam = x_c_b * np.sin(theta_b_w) + y_c_b * np.cos(
theta_b_w) + y_b_w
pos_angle = np.arctan2(y_cam, x_cam)
Hx[1, idx_j] = -np.sin(pos_angle - alpha) * np.sqrt(x_cam**2 +
y_cam**2)
########## Code ends here ##########
h, Hx = tb.normalize_line_parameters(h, Hx)
hs[:, j] = h
Hx_list.append(Hx)
return hs, Hx_list
hs = np.zeros_like(self.map_lines)
Hx_list = []
for j in range(self.map_lines.shape[1]):
########## Code starts here ##########
# TODO: Compute h, Hx using tb.transform_line_to_scanner_frame() for the j'th map line.
# HINT: This should be a single line of code.
h, Hx = tb.transform_line_to_scanner_frame(self.map_lines[:, j],
self.x,
self.tf_base_to_camera)
########## Code ends here ##########
h, Hx = tb.normalize_line_parameters(h, Hx)
hs[:, j] = h
Hx_list.append(Hx)
def compute_predicted_measurements(self):
"""
Adapt this method from EkfLocalization.compute_predicted_measurements().
"""
J = (self.x.size - 3) // 2
hs = np.zeros((2, J))
Hx_list = []
for j in range(J):
idx_j = 3 + 2 * j
alpha, r = self.x[idx_j:idx_j + 2]
Hx = np.zeros((2, self.x.size))
########## Code starts here ##########
# TODO: Compute h, Hx.
# HINT: Call tb.transform_line_to_scanner_frame() for the j'th map line.
# HINT: The first 3 columns of Hx should be populated using the same approach as in EkfLocalization.compute_predicted_measurements().
# HINT: The first two map lines (j=0,1) are fixed so the Jacobian of h wrt the alpha and r for those lines is just 0.
# HINT: For the other map lines (j>2), write out h in terms of alpha and r to get the Jacobian Hx.
# First two map lines are assumed fixed so we don't want to propagate
# any measurement correction to them.
if j >= 2:
Hx[:, idx_j:idx_j + 2] = np.eye(2) # FIX ME!
########## Code ends here ##########
h, Hx = tb.normalize_line_parameters(h, Hx)
hs[:, j] = h
Hx_list.append(Hx)
return hs, Hx_list
def compute_predicted_measurements(self):
"""
Adapt this method from EkfLocalization.compute_predicted_measurements().
"""
J = (self.x.size - 3) // 2
hs = np.zeros((2, J))
Hx_list = []
for j in range(J):
idx_j = 3 + 2 * j
alpha, r = self.x[idx_j:idx_j + 2]
Hx = np.zeros((2, self.x.size))
########## Code starts here ##########
# TODO: Compute h, Hx.
# HINT: Call tb.transform_line_to_scanner_frame() for the j'th map line.
# HINT: The first 3 columns of Hx should be populated using the same approach as in EkfLocalization.compute_predicted_measurements().
# HINT: The first two map lines (j=0,1) are fixed so the Jacobian of h wrt the alpha and r for those lines is just 0.
# HINT: For the other map lines (j>2), write out h in terms of alpha and r to get the Jacobian Hx.
h, Hx[:, 0:3] = tb.transform_line_to_scanner_frame(
np.array([alpha, r]), self.x[0:3], self.tf_base_to_camera)
# First two map lines are assumed fixed so we don't want to propagate
# any measurement correction to them.
if j >= 2:
# Hx[:,idx_j:idx_j+2] = np.eye(2) # FIX ME!
rot_b_to_w = np.array(
[[np.cos(self.x[2]), -np.sin(self.x[2]), self.x[0]],
[np.sin(self.x[2]),
np.cos(self.x[2]), self.x[1]], [0, 0, 1]])
x_cam_b = self.tf_base_to_camera[0]
y_cam_b = self.tf_base_to_camera[1]
th_cam_b = self.tf_base_to_camera[2]
cam_b = np.array([x_cam_b, y_cam_b, 1])
cam_w = np.matmul(rot_b_to_w, cam_b)
cam_w[2] = th_cam_b + self.x[2]
# alpha_cam = alpha - cam_w[2]
alpha_l = alpha - np.arctan2(cam_w[1], cam_w[0])
# r_cam = r - np.sqrt(cam_w[0]**2 + cam_w[1]**2) * np.cos(alpha_l)
H11 = 1.0
H12 = 0.0
H21 = np.sqrt(cam_w[0]**2 + cam_w[1]**2) * np.sin(alpha_l)
H22 = 1.0
Hx[:, idx_j:idx_j + 2] = np.array([[H11, H12], [H21, H22]])
########## Code ends here ##########
h, Hx = tb.normalize_line_parameters(h, Hx)
hs[:, j] = h
Hx_list.append(Hx)
return hs, Hx_list
def compute_predicted_measurements(self):
"""
Given a single map line in the world frame, outputs the line parameters
in the scanner frame so it can be associated with the lines extracted
from the scanner measurements.
Input:
None
Output:
hs: np.array[M,2,J] - J line parameters in the scanner (camera) frame for M particles.
"""
########## Code starts here ##########
# TODO: Compute hs.
# Hint: We don't need Jacobians for particle filtering.
# Hint: Simple solutions: Using for loop, for each particle, for each
# map line, transform to scanner frmae using tb.transform_line_to_scanner_frame()
# and tb.normalize_line_parameters()
# Hint: To maximize speed, try to compute the predicted measurements
# without looping over the map lines. You can implement vectorized
# versions of turtlebot_model functions directly here. This
# results in a ~10x speedup.
# Hint: For the faster solution, it does not call tb.transform_line_to_scanner_frame()
# or tb.normalize_line_parameters(), but reimplement these steps vectorized.
J=self.map_lines.shape[1]
hs = np.zeros((self.M, 2,J ))
for i in range(self.M):
for j in range(J):
alpha,r=self.map_lines[:,j]
h = tb.transform_line_to_scanner_frame(np.array([alpha, r]), self.xs[i,0:3], self.tf_base_to_camera, compute_jacobian=False)
h = tb.normalize_line_parameters(h)
hs[i,:,j]=h
########## Code ends here ##########
return hs
def compute_predicted_measurements(self):
"""
Given a single map line in the world frame, outputs the line parameters
in the scanner frame so it can be associated with the lines extracted
from the scanner measurements.
Input:
None
Outputs:
hs: np.array[2,J] - J line parameters in the scanner (camera) frame.
Hx_list: [np.array[2,3]] - list of Jacobians of h with respect to the belief mean self.x.
"""
hs = np.zeros_like(self.map_lines)
Hx_list = []
for j in range(self.map_lines.shape[1]):
########## Code starts here ##########
# TODO: Compute h, Hx using tb.transform_line_to_scanner_frame().
h, Hx = tb.transform_line_to_scanner_frame(self.map_lines[:, j],
self.x,
self.tf_base_to_camera)
########## Code ends here ##########
h, Hx = tb.normalize_line_parameters(h, Hx)
hs[:, j] = h
Hx_list.append(Hx)
return hs, Hx_list
def compute_predicted_measurements(self):
"""
Given the best estimate of the map features (found inside the augmented
state vector) and our current position (also inside the augmented state
vector), return |hs| representing the map features in the robot's frame
of reference and |Hx| representing how much |hs| changes with respect
to the augmented state vector.
"""
# number of map features
J = (self.x.size - 3) // 2
hs = np.zeros((2, J))
Hx_list = []
# loop through all the features
for j in range(J):
idx_j = 3 + 2 * j
# extract best-estimate line parameters from augmented state vector
alpha, r = self.x[idx_j:idx_j + 2]
# compute h and Hx
Hx = np.zeros((2, self.x.size))
h, Hx_robot = tb.transform_line_to_scanner_frame(
(alpha, r), self.x[0:3], self.tf_base_to_camera)
Hx[:, :3] = Hx_robot
# First two map lines are assumed fixed so we don't want to propagate
# any measurement correction to them.
if j >= 2:
Hx[:, idx_j:idx_j + 2] = np.eye(2)
h, Hx = tb.normalize_line_parameters(h, Hx)
hs[:, j] = h
Hx_list.append(Hx)
return hs, Hx_list
def compute_predicted_measurements(self):
"""
Adapt this method from EkfLocalization.compute_predicted_measurements().
"""
J = (self.x.size - 3) // 2
hs = np.zeros((2, J))
Hx_list = []
th = self.tf_base_to_camera[2] + self.x[2]
R_c_w = np.array([
[ np.cos(th), np.sin(th), 0], \
[-np.sin(th), np.cos(th), 0], \
[ 0,0,1]
])
x_cam, y_cam, th_cam = np.dot(np.linalg.inv(R_c_w),
self.tf_base_to_camera) + self.x[:3]
#alpha_in_cam = alpha - th_cam
norm_cam = np.linalg.norm((x_cam, y_cam))
#r_in_cam = r - norm_cam * np.cos(alpha - np.arctan2(y_cam, x_cam))
#h = (alpha_in_cam, r_in_cam)
for j in range(J):
idx_j = 3 + 2 * j
alpha, r = self.x[idx_j:idx_j + 2]
Hx = np.zeros((2, self.x.size))
########## Code starts here ##########
# TODO: Compute h, Hx.
# HINT: Call tb.transform_line_to_scanner_frame() for the j'th map line.
# HINT: The first 3 columns of Hx should be populated using the same approach as in EkfLocalization.compute_predicted_measurements().
# HINT: The first two map lines (j=0,1) are fixed so the Jacobian of h wrt the alpha and r for those lines is just 0.
# HINT: For the other map lines (j>2), write out h in terms of alpha and r to get the Jacobian Hx.
line = (alpha, r)
h, Hx[:, :3] = tb.transform_line_to_scanner_frame(
line, self.x[:3], self.tf_base_to_camera)
# First two map lines are assumed fixed so we don't want to propagate
# any measurement correction to them.
if j >= 2:
#Hx[:,idx_j:idx_j+2] = np.eye(2) # FIX ME!
Hx[:, idx_j:idx_j + 2] = np.array(
[[1, 0],
[norm_cam * np.sin(alpha - np.arctan2(y_cam, x_cam)), 1]])
########## Code ends here ##########
h, Hx = tb.normalize_line_parameters(h, Hx)
hs[:, j] = h
Hx_list.append(Hx)
return hs, Hx_list
def compute_predicted_measurements(self):
"""
Adapt this method from EkfLocalization.compute_predicted_measurements().
"""
J = (self.x.size - 3) // 2
hs = np.zeros((2, J))
Hx_list = []
for j in range(J):
idx_j = 3 + 2 * j
alpha, r = self.x[idx_j:idx_j + 2]
Hx = np.zeros((2, self.x.size))
########## Code starts here ##########
# TODO: Compute h, Hx.
# HINT: Call tb.transform_line_to_scanner_frame() for the j'th map line.
# HINT: The first 3 columns of Hx should be populated using the same approach as in EkfLocalization.compute_predicted_measurements().
# HINT: The first two map lines (j=0,1) are fixed so the Jacobian of h wrt the alpha and r for those lines is just 0.
# HINT: For the other map lines (j>2), write out h in terms of alpha and r to get the Jacobian Hx.
h, HxTemp = tb.transform_line_to_scanner_frame(
self.x[idx_j:idx_j + 2], self.x[0:3], self.tf_base_to_camera,
True)
Hx[:, 0:3] = HxTemp
Hx[:, 3:5] = np.zeros((2, 2))
# First two map lines are assumed fixed so we don't want to propagate
# any measurement correction to them.
if j >= 2:
theta_r = self.x[2]
R = np.array([[np.cos(theta_r), -np.sin(theta_r), 0.0],
[np.sin(theta_r),
np.cos(theta_r), 0.0], [0.0, 0.0, 1.0]])
x_cam_world = self.x[0:3] + np.dot(R, self.tf_base_to_camera)
x_cam, y_cam, theta_cam = x_cam_world
"""
x_cam = self.tf_base_to_camera[0]
y_cam = self.tf_base_to_camera[1]
"""
Hx[:, idx_j:idx_j + 2] = np.array(
[[1.0, 0.0],
[x_cam * np.sin(alpha) - y_cam * np.cos(alpha), 1.0]])
########## Code ends here ##########
h, Hx = tb.normalize_line_parameters(h, Hx)
hs[:, j] = h
Hx_list.append(Hx)
return hs, Hx_list
def compute_predicted_measurements(self):
"""
Adapt this method from EkfLocalization.compute_predicted_measurements().
"""
J = (self.x.size - 3) // 2
hs = np.zeros((2, J))
Hx_list = []
for j in range(J):
idx_j = 3 + 2 * j
alpha, r = self.x[idx_j:idx_j + 2]
Hx = np.zeros((2, self.x.size))
########## Code starts here ##########
# TODO: Compute h, Hx.
# HINT: Call tb.transform_line_to_scanner_frame() for the j'th map line.
# HINT: The first 3 columns of Hx should be populated using the same approach as in EkfLocalization.compute_predicted_measurements().
# HINT: The first two map lines (j=0,1) are fixed so the Jacobian of h wrt the alpha and r for those lines is just 0.
# HINT: For the other map lines (j>2), write out h in terms of alpha and r to get the Jacobian Hx.
line = np.array([alpha, r])
h, Hx[:, :3] = tb.transform_line_to_scanner_frame(
line, self.x[:3], self.tf_base_to_camera)
# First two map lines are assumed fixed so we don't want to propagate
# any measurement correction to them.
if j >= 2:
Hx[:, idx_j:idx_j + 2] = np.eye(2)
x_base, y_base, th_base = self.x[:3]
Rz = np.array([[np.cos(th_base), -np.sin(th_base)],
[np.sin(th_base),
np.cos(th_base)]])
x_cam_rotated, y_cam_rotated = np.dot(
Rz, self.tf_base_to_camera[:2])
x_cam = x_cam_rotated + x_base
y_cam = y_cam_rotated + y_base
cam_coord_norm = np.linalg.norm(np.array([x_cam, y_cam]))
Hx[1, idx_j] = -cam_coord_norm * np.sin(
np.arctan2(y_cam, x_cam) - alpha)
########## Code ends here ##########
h, Hx = tb.normalize_line_parameters(h, Hx)
hs[:, j] = h
Hx_list.append(Hx)
return hs, Hx_list
def compute_predicted_measurements(self):
"""
Adapt this method from EkfLocalization.compute_predicted_measurements().
"""
J = (self.x.size - 3) // 2
hs = np.zeros((2, J))
Hx_list = []
for j in range(J):
idx_j = 3 + 2 * j
alpha, r = self.x[idx_j:idx_j + 2]
Hx = np.zeros((2, self.x.size))
########## Code starts here ##########
# TODO: Compute h, Hx.
# HINT: Call tb.transform_line_to_scanner_frame() for the j'th map line.
# HINT: The first 3 columns of Hx should be populated using the same approach as in EkfLocalization.compute_predicted_measurements().
# HINT: The first two map lines (j=0,1) are fixed so the Jacobian of h wrt the alpha and r for those lines is just 0.
# HINT: For the other map lines (j>2), write out h in terms of alpha and r to get the Jacobian Hx.
h, Hx_temp = tb.transform_line_to_scanner_frame(
np.array([alpha, r]), self.x[0:3], self.tf_base_to_camera)
Hx[:, 0:3] = Hx_temp
# First two map lines are assumed fixed so we don't want to propagate
# any measurement correction to them.
if j >= 2:
da_da = 1.
da_dr = 0.
x_off, y_off, th_off = self.tf_base_to_camera
x, y, th = self.x[0:3]
dr_da = np.sin(alpha) * (
x + np.cos(th_off) * x_off -
np.sin(th_off) * y_off) - np.cos(alpha) * (
y + np.sin(th_off) * x_off + np.cos(th_off) * y_off)
dr_dr = 1.
Hx[:, idx_j:idx_j + 2] = np.array([[da_da, da_dr],
[dr_da, dr_dr]]) # FIX ME!
########## Code ends here ##########
h, Hx = tb.normalize_line_parameters(h, Hx)
hs[:, j] = h
Hx_list.append(Hx)
return hs, Hx_list
def compute_predicted_measurements(self):
"""
Adapt this method from EkfLocalization.compute_predicted_measurements().
"""
J = (self.x.size - 3) // 2
hs = np.zeros((2, J))
Hx_list = []
for j in range(J):
idx_j = 3 + 2 * j
alpha, r = self.x[idx_j:idx_j + 2]
Hx = np.zeros((2, self.x.size))
########## Code starts here ##########
# TODO: Compute h, Hx.
x, y, th = self.x[:3]
x_cam, y_cam, th_cam = self.tf_base_to_camera
# compute pose of camera in world frame
x_cam_w = x_cam * np.cos(th) - y_cam * np.sin(th) + x
y_cam_w = x_cam * np.sin(th) + y_cam * np.cos(th) + y
h = np.array([
alpha - th - th_cam,
r - x_cam_w * np.cos(alpha) - y_cam_w * np.sin(alpha)
])
dh_dth = (x_cam * np.sin(th) + y_cam * np.cos(th)) * np.cos(alpha) - \
(x_cam * np.cos(th) - y_cam * np.sin(th)) * np.sin(alpha)
Hx[:, :3] = np.array([[0, 0, -1],
[-np.cos(alpha), -np.sin(alpha), dh_dth]])
# First two map lines are assumed fixed so we don't want to propagate
# any measurement correction to them.
if j >= 2:
Hx[:, idx_j:idx_j + 2] = np.eye(2) # FIX ME!
Hx[1,
idx_j] = x_cam_w * np.sin(alpha) - y_cam_w * np.cos(alpha)
########## Code ends here ##########
h, Hx = tb.normalize_line_parameters(h, Hx)
hs[:, j] = h
Hx_list.append(Hx)
return hs, Hx_list
def compute_predicted_measurements(self):
"""
Adapt this method from EkfLocalization.compute_predicted_measurements().
"""
J = (self.x.size - 3) // 2
hs = np.zeros((2, J))
Hx_list = []
for j in range(J):
idx_j = 3 + 2 * j
alpha, r = self.x[idx_j:idx_j + 2]
Hx = np.zeros((2, self.x.size))
########## Code starts here ##########
# TODO: Compute h, Hx.
h = np.zeros(2)
h[0] = alpha - self.x[2] - self.tf_base_to_camera[2]
h[1] = r - self.x[0] * np.cos(alpha) - self.x[1] * np.sin(
alpha) - self.tf_base_to_camera[0] * np.cos(
alpha - self.x[2]) - self.tf_base_to_camera[1] * np.sin(
alpha - self.x[2])
Hx[0, 2] = -1
Hx[1, 0] = -np.cos(alpha)
Hx[1, 1] = -np.sin(alpha)
Hx[1, 2] = - self.tf_base_to_camera[0] * \
np.sin(
alpha - self.x[2]) + self.tf_base_to_camera[1] * np.cos(alpha - self.x[2])
# First two map lines are assumed fixed so we don't want to propagate
# any measurement correction to them.
if j >= 2:
Hx[:, idx_j:idx_j + 2] = np.eye(2) # FIX ME!
Hx[1, idx_j] = self.x[0] * np.sin(alpha) - self.x[1] * np.cos(
alpha) + self.tf_base_to_camera[0] * np.sin(
alpha - self.x[2]
) - self.tf_base_to_camera[1] * np.cos(alpha - self.x[2])
########## Code ends here ##########
h, Hx = tb.normalize_line_parameters(h, Hx)
hs[:, j] = h
Hx_list.append(Hx)
return hs, Hx_list
def compute_predicted_measurements(self):
"""
Given a single map line in the world frame, outputs the line parameters
in the scanner frame so it can be associated with the lines extracted
from the scanner measurements.
Input:
None
Output:
hs: np.array[M,2,J] - J line parameters in the scanner (camera) frame for M particles.
"""
J = self.map_lines.shape[1]
hs_all = np.empty((self.M, 2, J))
for index, particle in enumerate(self.xs):
hs = np.empty_like(self.map_lines)
for j, line in enumerate(self.map_lines.T):
h = tb.transform_line_to_scanner_frame(line, particle, self.tf_base_to_camera, False)
h = tb.normalize_line_parameters(h)
hs[:,j] = h
hs_all[index] = hs
return hs_all
def compute_predicted_measurements(self):
"""
Adapt this method from EkfLocalization.compute_predicted_measurements().
"""
J = (self.x.size - 3) // 2
hs = np.zeros((2, J))
Hx_list = []
x_b, y_b, th_b = self.x[:3]
x_cb, y_cb, th_cb = self.tf_base_to_camera
x_c = np.cos(th_b) * x_cb - np.sin(th_b) * y_cb + x_b
y_c = np.sin(th_b) * x_cb + np.cos(th_b) * y_cb + y_b
d_c = np.linalg.norm([x_c, y_c])
for j in range(J):
idx_j = 3 + 2 * j
alpha, r = self.x[idx_j:idx_j + 2]
Hx = np.zeros((2, self.x.size))
########## Code starts here ##########
# TODO: Compute h, Hx.
h, Hxp = tb.transform_line_to_scanner_frame(
self.x[idx_j:idx_j + 2], self.x[:3], self.tf_base_to_camera)
Hx[:, :3] = Hxp
# First two map lines are assumed fixed so we don't want to propagate
# any measurement correction to them.
if j >= 2:
Hx[:, idx_j:idx_j + 2] = np.eye(2) # FIX ME!
########## Code ends here ##########
alpha, r = self.x[idx_j:idx_j + 2]
Hx[:, idx_j:idx_j + 2] = np.array(
[[1, 0], [d_c * np.sin(alpha - np.arctan2(y_c, y_b)), 1]])
h, Hx = tb.normalize_line_parameters(h, Hx)
hs[:, j] = h
Hx_list.append(Hx)
return hs, Hx_list
def compute_predicted_measurements(self):
"""
Adapt this method from EkfLocalization.compute_predicted_measurements().
"""
J = (self.x.size - 3) // 2
hs = np.zeros((2, J))
Hx_list = []
for j in range(J):
idx_j = 3 + 2 * j
alpha, r = self.x[idx_j:idx_j + 2]
Hx = np.zeros((2, self.x.size))
########## Code starts here ##########
# TODO: Compute h, Hx.
# HINT: Call tb.transform_line_to_scanner_frame() for the j'th map line.
# HINT: The first 3 columns of Hx should be populated using the same approach as in EkfLocalization.compute_predicted_measurements().
# HINT: The first two map lines (j=0,1) are fixed so the Jacobian of h wrt the alpha and r for those lines is just 0.
# HINT: For the other map lines (j>2), write out h in terms of alpha and r to get the Jacobian Hx.
h, Hx_x = tb.transform_line_to_scanner_frame(
np.array([alpha, r]),
self.x,
self.tf_base_to_camera,
compute_jacobian=True)
Hx[:, 0:3] = Hx_x
# Rotation matrix from the world frame to the base frame
x = self.x[0:3]
line = np.array([alpha, r])
tf_base_to_camera = self.tf_base_to_camera
# Rotation matrix from the world frame to the base frame
c, s = np.cos(x[2]), np.sin(x[2])
R_world2base = np.array([[c, -s], [s, c]])
# Translation vector from the world frame to the base frame
t_world2base = x[0:2]
t_world2base = np.asarray(t_world2base)
t_world2base = np.reshape(t_world2base, (2, 1))
# Rotation matrix from the base frame to the camera frame
c, s = np.cos(tf_base_to_camera[2]), np.sin(tf_base_to_camera[2])
R_base2cam = np.array([[c, -s], [s, c]])
# Translation vector from the base frame to the camera frame
t_base2cam = tf_base_to_camera[0:2]
t_base2cam = np.asarray(t_base2cam)
t_base2cam = np.reshape(t_base2cam, (2, 1))
# Pose of the camera in the world frame
# pos_cam = np.dot(np.linalg.inv(R_world2base), t_base2cam) + t_world2base
pos_cam = np.matmul((R_world2base), t_base2cam) + t_world2base
theta_cam = x[2] + tf_base_to_camera[2]
alpha_cam = line[0] - theta_cam
d = np.linalg.norm(pos_cam, 2)
theta_world2cam = np.arctan2(pos_cam[1], pos_cam[0])
# print(theta_world2cam)
r_cam = line[1] - d * np.cos(line[0] - theta_world2cam)
Hx_j = np.eye(2)
Hx_j[1, 0] = d * np.sin(alpha - theta_world2cam)
# First two map lines are assumed fixed so we don't want to propagate
# any measurement correction to them.
if j >= 2:
Hx[:, idx_j:idx_j + 2] = Hx_j # FIX ME!
########## Code ends here ##########
h, Hx = tb.normalize_line_parameters(h, Hx)
hs[:, j] = h
Hx_list.append(Hx)
return hs, Hx_list
def compute_predicted_measurements(self):
"""
Adapt this method from EkfLocalization.compute_predicted_measurements().
"""
J = (self.x.size - 3) // 2
hs = np.zeros((2, J))
Hx_list = []
X = self.x[:3]
for j in range(J):
idx_j = 3 + 2 * j
alpha, r = self.x[idx_j:idx_j + 2]
Hx = np.zeros((2, self.x.size))
########## Code starts here ##########
# TODO: Compute h, Hx.
# Exactly the same as tb.transform_line_to_scanner_frame()
alp = alpha
x, y, th = X
xcam_R, ycam_R, thcam_R = self.tf_base_to_camera # Camera pose. in Robot frame.
C_Rh = np.array(
[xcam_R, ycam_R,
1]) # Camera pose in homogeneous coords. Robot frame.
RW_T = np.array([[np.cos(th), -np.sin(th), x],
[np.sin(th), np.cos(th), y], [0, 0, 1]])
Cam_h = np.matmul(
RW_T, C_Rh) # Camera in homogeneous coords. World frame.
xcam, ycam, hcam = Cam_h
xcam, ycam = xcam / hcam, ycam / hcam # Camera in World frame coords.
alp_C = alp - th - thcam_R
r_C = r - xcam * np.cos(alp) - ycam * np.sin(alp)
h = np.array([alp_C, r_C])
dalpC_dxR = 0
dalpC_dyR = 0
dalpC_dthR = -1
drC_dxW = -np.cos(alp)
drC_dyW = -np.sin(alp)
drC_dthW = (-np.cos(alp) *
(-xcam_R * np.sin(th) - ycam_R * np.cos(th)) -
np.sin(alp) *
(xcam_R * np.cos(th) - ycam_R * np.sin(th)))
# Hx is bigger now
Hx[0:2, 0:3] = np.array([[dalpC_dxR, dalpC_dyR, dalpC_dthR],
[drC_dxW, drC_dyW, drC_dthW]])
# So the columns of Hx go dx, dy, dth, dalp1, dr1, dalp2, dr2, ...
# First two map lines are assumed fixed so we don't want to propagate
# any measurement correction to them. i.e. Entries all zero.
# Otherwise:
# dalpC_dalpC = 1
# dalpC_drC = 0
drC_dalpC = xcam * np.sin(alp) - ycam * np.cos(alp)
# drC_drC = 1
if j >= 2:
Hx[:, idx_j:idx_j + 2] = np.eye(2)
Hx[1, idx_j] = drC_dalpC
########## Code ends here ##########
h, Hx = tb.normalize_line_parameters(h, Hx)
hs[:, j] = h
Hx_list.append(Hx)
return hs, Hx_list
def compute_predicted_measurements(self):
"""
Adapt this method from EkfLocalization.compute_predicted_measurements().
"""
J = (self.x.size - 3) // 2
hs = np.zeros((2, J))
Hx_list = []
for j in range(J):
idx_j = 3 + 2 * j
alpha, r = self.x[idx_j:idx_j + 2]
Hx = np.zeros((2, self.x.size))
########## Code starts here ##########
# TODO: Compute h, Hx.
x = self.x[0:3]
rotation = np.array([
[np.cos(x[2]), -np.sin(x[2])],
[np.sin(x[2]), np.cos(x[2])]
])
base_to_camera_xy = np.array([
[self.tf_base_to_camera[0]],
[self.tf_base_to_camera[1]]
])
base_to_camera_xy_prime = np.matmul(rotation, base_to_camera_xy)
world_to_camera = base_to_camera_xy_prime + np.array([[x[0]], [x[1]]])
r_c = r - np.cos(alpha) * world_to_camera[0, 0] - np.sin(alpha) * world_to_camera[1, 0]
alpha_c = alpha - self.tf_base_to_camera[2] - x[2]
h = np.array([alpha_c, r_c])
method = 2
if method == 1:
q = (r*np.cos(alpha) - x[0])**2 + (r*np.sin(alpha) - x[1])**2
a = (x[0] - r*np.cos(alpha))/np.sqrt(q)
b = (x[1] - r*np.sin(alpha))/np.sqrt(q)
c = -b/np.sqrt(q)
d = a/np.sqrt(q)
Hx[0, 0] = a
Hx[0, 1] = b
Hx[1, 0] = c
Hx[1, 1] = d
Hx[1, 2] = -1
elif method == 2:
del_r_del_th = self.tf_base_to_camera[0] * np.cos(alpha) * np.sin(x[2]) + self.tf_base_to_camera[1] * \
np.cos(alpha) * np.cos(x[2]) - self.tf_base_to_camera[0] * np.sin(alpha) * np.cos(x[2]) + \
self.tf_base_to_camera[1] * np.sin(alpha) * np.sin(x[2])
del_r_del_alpha = np.sin(alpha) * (
self.tf_base_to_camera[0] * np.cos(x[2]) - self.tf_base_to_camera[1] * np.sin(x[2]) + x[0]
) - np.cos(alpha) * (
self.tf_base_to_camera[0] * np.sin(x[2]) + self.tf_base_to_camera[1] * np.cos(x[2]) + x[1]
)
Hx[0, 2] = -1
# Hx[0, idx_j] = 1
Hx[1, 0] = -np.cos(alpha)
Hx[1, 1] = -np.sin(alpha)
Hx[1, 2] = del_r_del_th
else:
print("WRONG METHOD NUMBER")
return -1
# Hx[1, idx_j] = del_r_del_alpha
# Hx[1, idx_j + 1] = 1
# First two map lines are assumed fixed so we don't want to propagate
# any measurement correction to them.
if j >= 2:
if method == 1:
Hx[:, idx_j:idx_j + 2] = np.array([
[-a, -b],
[-c, -d]
]) # FIX ME!
else:
Hx[:, idx_j:idx_j + 2] = np.eye(2)
Hx[1, idx_j] = del_r_del_alpha # FIXED!!
########## Code ends here ##########
h, Hx = tb.normalize_line_parameters(h, Hx)
hs[:, j] = h
Hx_list.append(Hx)
return hs, Hx_list
