def map_line_to_predicted_measurement(self, m):
alpha, r = m
#### TODO ####
# compute h, Hx
##############
x, y, th = self.x # Coordinates of base frame in world frame
xc, yc, thc = self.tf_base_to_camera # Corrdinates of camera in base frame
# Coordinates of camera in world frame
x_ = x + xc * cos(th) - yc * sin(th)
y_ = y + xc * sin(th) + yc * cos(th)
th_ = th + thc
# Eq. 5.94
h = np.array([alpha - (th_), r - (x_ * cos(alpha) + y_ * sin(alpha))])
Hx = np.array(
[[0, 0, -1],
[
-cos(alpha), -sin(alpha),
yc * cos(alpha - th_ + thc) - xc * sin(alpha - th_ + thc)
]])
##############
flipped, h = normalize_line_parameters(h)
if flipped:
Hx[1, :] = -Hx[1, :]
return h, Hx
def map_line_to_predicted_measurement(self, m):
alpha, r = m
# compute h, Hx
x, y, theta = self.x
x_cam, y_cam, theta_cam = self.tf_base_to_camera # camera location in robot frame
# compute camera location in world frame
T_bot_to_w = np.array([[np.cos(theta), -np.sin(theta), x],
[np.sin(theta), np.cos(theta), y], [0., 0.,
1.]])
x_cam_w, y_cam_w, _ = T_bot_to_w.dot(np.array([x_cam, y_cam, 1.]))
h = np.array([
alpha - theta - theta_cam,
r - x_cam_w * np.cos(alpha) - y_cam_w * np.sin(alpha)
])
Hx = np.array([
[0., 0., -1.],
[
-np.cos(alpha), -np.sin(alpha),
(y_cam * np.cos(alpha) - x_cam * np.sin(alpha)) * np.cos(theta)
+
(x_cam * np.cos(alpha) + y_cam * np.sin(alpha)) * np.sin(theta)
]
])
flipped, h = normalize_line_parameters(h)
if flipped:
Hx[1, :] = -Hx[1, :]
return h, Hx
def map_line_to_predicted_measurement(self, m):
alpha, r = m
# find the pose of the robot in the world frame
x, y, th = self.x
# find the pose of the camera in the robot frame
x_cam, y_cam, th_cam = self.tf_base_to_camera
# compute the line parameters in the camera frame
h = np.array([
alpha - th - th_cam, r - x * cos(alpha) - y * sin(alpha) -
x_cam * cos(alpha - th) - y_cam * sin(alpha - th)
])
# compute the Jacobian of h with respect to the belief mean
Hx = np.array([[0, 0, -1],
[
-cos(alpha), -sin(alpha),
-x_cam * sin(alpha - th) + y_cam * cos(alpha - th)
]])
flipped, h = normalize_line_parameters(h)
if flipped:
Hx[1, :] = -Hx[1, :]
return h, Hx
def map_line_to_predicted_measurement(self, m):
alpha, r = m
x, y, th = self.x
xb2c, yb2c, thb2c = self.tf_base_to_camera
# position of camera coordinate system in world frame
xcam = x + xb2c*cos(th) - yb2c*sin(th)
ycam = y + xb2c*sin(th) + yb2c*cos(th)
rcam = sqrt(xcam**2 + ycam**2)
acam = math.atan2(ycam, xcam)
# distance from camera to line m
rprime = r - rcam*cos(acam-alpha)
# angle from camera frame to line m
aprime = alpha - th - thb2c
h = np.array([aprime, rprime]);
dh2dx = -cos(alpha)
dh2dy = -sin(alpha)
dh2dth = (cos(alpha)*(xb2c*sin(th)+yb2c*cos(th)) -
sin(alpha)*(xb2c*cos(th)-yb2c*sin(th)))
Hx = np.array([[0, 0, -1],
[dh2dx, dh2dy, dh2dth]]);
flipped, h = normalize_line_parameters(h)
if flipped:
Hx[1,:] = -Hx[1,:]
return h, Hx
def map_line_to_predicted_measurement(self, j):
alpha, r = self.x[(3+2*j):(3+2*j+2)] # j is zero-indexed! (yeah yeah I know this doesn't match the pset writeup)
#### TODO ####
# compute h, Hx (you may find the skeleton for computing Hx below useful)
x, y, th = self.x[:3]
x_c, y_c, th_c = self.tf_base_to_camera
Hx = np.zeros((2,self.x.size))
h, Hx[:,:3] = generic_map_line_to_predicted_measurement(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 > 1:
Hx[0, 3+2*j] = 1
Hx[1, 3+2*j] = x*np.sin(alpha) + y*np.cos(alpha) + x_c*np.sin(alpha-th) + y_c*np.cos(alpha-th)
'''
Hx[1, 3+2*j] = np.sin(alpha) * (x + x_c *np.cos(th) - y_c * np.sin(th)) + \
np.cos(alpha) * (y - x_c *np.sin(th) + y_c * np.cos(th))
'''
Hx[0, 3+2*j+1] = 0
Hx[1, 3+2*j+1] = 1
##############
flipped, h = normalize_line_parameters(h)
if flipped:
Hx[1,:] = -Hx[1,:]
return h, Hx
def map_line_to_predicted_measurement(self, j):
alpha, r = self.x[(3 + 2 * j):(
3 + 2 * j + 2
)] # j is zero-indexed! (yeah yeah I know this doesn't match the pset writeup)
# Use mean expected position to compute line location
x, y, theta = self.x[0:3]
# (x, y, theta) transform from the robot base to the camera frame
xcam, ycam, theta_cam = self.tf_base_to_camera
alpha_cam = alpha - theta_cam - theta
# rotation matrix from robot reference frame to world frame
R_cam = np.matrix([[np.cos(theta), -np.sin(theta), 0],
[np.sin(theta), np.cos(theta), 0], [0, 0, 1]])
# rotation matrix to line reference frame from world frame
R_line = np.matrix([[np.cos(-alpha), -np.sin(-alpha)],
[np.sin(-alpha), np.cos(-alpha)]])
# Find camera coordinates in robot frame
# (x_cam_w, y_cam_w, theta_cam_w)
cam_world_coords = np.squeeze(
np.asarray(np.dot(R_cam, self.tf_base_to_camera)))
# Offset for camera coords in world frame
cam_w = np.array([x + cam_world_coords[0], y + cam_world_coords[1]])
r_proj = np.squeeze(np.asarray(np.dot(R_line, cam_w)))
r_cam = r - r_proj[0]
# Store results in h array
h = np.array([alpha_cam, r_cam])
Hx = np.zeros((2, self.x.size))
Hx[0, 2] = -1
Hx[1, 0] = -np.cos(alpha)
Hx[1, 1] = -np.sin(alpha)
Hx[1, 2] = -(np.cos(alpha) *
(-np.sin(theta) * xcam - np.cos(theta) * ycam) +
np.sin(alpha) *
(np.cos(theta) * xcam - np.sin(theta) * ycam))
# First two map lines are assumed fixed so we don't want to propagate
# any measurement correction to them
if j > 1:
Hx[0, 3 + 2 * j] = 1
Hx[1, 3 + 2 * j] = np.sin(alpha) * (
x + np.cos(theta) * xcam - np.sin(theta) * ycam) + np.cos(
alpha) * (y + np.sin(theta) * xcam + np.cos(theta) * ycam)
Hx[0, 3 + 2 * j + 1] = 0
Hx[1, 3 + 2 * j + 1] = 1
##############
flipped, h = normalize_line_parameters(h)
if flipped:
Hx[1, :] = -Hx[1, :]
return h, Hx
def map_line_to_predicted_measurement(self, m):
alpha, r = m
#### TODO ####
# compute h, Hx
##############
flipped, h = normalize_line_parameters(h)
if flipped:
Hx[1, :] = -Hx[1, :]
return h, Hx
def map_line_to_predicted_measurement(self, j):
alpha, r = self.x[(3 + 2 * j):(
3 + 2 * j + 2
)] # j is zero-indexed! (yeah yeah I know this doesn't match the pset writeup)
#### TODO ####
# compute h, Hx (you may find the skeleton for computing Hx below useful)
# Hx = np.zeros((2,self.x.size))
# Hx[:,:3] = FILLMEIN
# First two map lines are assumed fixed so we don't want to propagate any measurement correction to them
# if j > 1:
# Hx[0, 3+2*j] = FILLMEIN
# Hx[1, 3+2*j] = FILLMEIN
# Hx[0, 3+2*j+1] = FILLMEIN
# Hx[1, 3+2*j+1] = FILLMEIN
##############
# find the pose of the robot in the world frame
x, y, th = self.x[:3]
# find the pose of the camera in the robot frame
x_cam, y_cam, th_cam = self.tf_base_to_camera
# compute the line parameters in the camera frame
h = np.array([
alpha - th - th_cam, r - x * cos(alpha) - y * sin(alpha) -
x_cam * cos(alpha - th) - y_cam * sin(alpha - th)
])
# compute the Jacobian of h with respect to the belief mean
Hx = np.zeros((2, self.x.size))
Hx[:, :3] = np.array(
[[0, 0, -1],
[
-cos(alpha), -sin(alpha),
-x_cam * sin(alpha - th) + y_cam * cos(alpha - th)
]])
# First two map lines are assumed fixed so we don't want to propagate any measurement correction to them
if j > 1:
Hx[0, 3 + 2 * j] = 1
Hx[1, 3 + 2 * j] = x * sin(alpha) - y * cos(alpha) + x_cam * sin(
alpha - th) - y_cam * cos(alpha - th)
Hx[0, 3 + 2 * j + 1] = 0
Hx[1, 3 + 2 * j + 1] = 1
flipped, h = normalize_line_parameters(h)
if flipped:
Hx[1, :] = -Hx[1, :]
return h, Hx
def map_line_to_predicted_measurement(self, m):
alpha, r = m
#### TODO ####
# compute h, Hx
h, Hx = map_line_to_predicted_measurement_EKF(alpha, r, self.x,
self.tf_base_to_camera)
##############
flipped, h = normalize_line_parameters(h)
if flipped:
Hx[1, :] = -Hx[1, :]
return h, Hx
def map_line_to_predicted_measurement(self, m):
alpha, r = m
# Use mean expected position to compute line location
x, y, theta = self.x
# (x, y, theta) transform from the robot base to the camera frame
xcam, ycam, theta_cam = self.tf_base_to_camera
alpha_cam = alpha - theta_cam - theta
# rotation matrix from robot reference frame to world frame
R_cam = np.matrix([[np.cos(theta), -np.sin(theta), 0],
[np.sin(theta), np.cos(theta), 0], [0, 0, 1]])
# rotation matrix to line reference frame from world frame
R_line = np.matrix([[np.cos(-alpha), -np.sin(-alpha)],
[np.sin(-alpha), np.cos(-alpha)]])
# Find camera coordinates in robot frame
# (x_cam_w, y_cam_w, theta_cam_w)
cam_world_coords = np.squeeze(
np.asarray(np.dot(R_cam, self.tf_base_to_camera)))
# Offset for camera coords in world frame
cam_w = np.array([x + cam_world_coords[0], y + cam_world_coords[1]])
r_proj = np.squeeze(np.asarray(np.dot(R_line, cam_w)))
r_cam = r - r_proj[0]
# Store results in h array
h = np.array([alpha_cam, r_cam])
# Create and populate Hx matrix
Hx = np.zeros((2, 3))
Hx[0, 2] = -1
Hx[1, 0] = -np.cos(alpha)
Hx[1, 1] = -np.sin(alpha)
Hx[1, 2] = -(np.cos(alpha) *
(-np.sin(theta) * xcam - np.cos(theta) * ycam) +
np.sin(alpha) *
(np.cos(theta) * xcam - np.sin(theta) * ycam))
flipped, h = normalize_line_parameters(h)
if flipped:
Hx[1, :] = -Hx[1, :]
return h, Hx
def map_line_to_predicted_measurement(self, j):
alpha, r = self.x[(3 + 2 * j):(
3 + 2 * j + 2
)] # j is zero-indexed! (yeah yeah I know this doesn't match the pset writeup)
#### TODO ####
# compute h, Hx (you may find the skeleton for computing Hx below useful)
x, y, theta = self.x[:3]
x_cam, y_cam, theta_cam = self.tf_base_to_camera # camera location in robot frame
# compute camera location in world frame
T_bot_to_w = np.array([[np.cos(theta), -np.sin(theta), x],
[np.sin(theta), np.cos(theta), y], [0., 0.,
1.]])
x_cam_w, y_cam_w, _ = T_bot_to_w.dot(np.array([x_cam, y_cam, 1.]))
# compute h, Hx
h = np.array([
alpha - theta - theta_cam,
r - x_cam_w * np.cos(alpha) - y_cam_w * np.sin(alpha)
])
Hx = np.zeros((2, self.x.size))
Hx[:, :3] = np.array([
[0., 0., -1.],
[
-np.cos(alpha), -np.sin(alpha),
(y_cam * np.cos(alpha) - x_cam * np.sin(alpha)) * np.cos(theta)
+
(x_cam * np.cos(alpha) + y_cam * np.sin(alpha)) * np.sin(theta)
]
])
# First two map lines are assumed fixed so we don't want to propagate any measurement correction to them
if j > 1:
Hx[0, 3 + 2 * j] = 1.0
Hx[1,
3 + 2 * j] = x_cam_w * np.sin(alpha) - y_cam_w * np.cos(alpha)
Hx[0, 3 + 2 * j + 1] = 0.0
Hx[1, 3 + 2 * j + 1] = 1.0
##############
flipped, h = normalize_line_parameters(h)
if flipped:
Hx[1, :] = -Hx[1, :]
return h, Hx
def map_line_to_predicted_measurement(self, j):
alpha, r = self.x[(3 + 2 * j):(
3 + 2 * j + 2
)] # j is zero-indexed! (yeah yeah I know this doesn't match the pset writeup)
#### TODO ####
# compute h, Hx (you may find the skeleton for computing Hx below useful)
x, y, th = self.x[:3] # Coordinates of base frame in world frame
xc, yc, thc = self.tf_base_to_camera # Corrdinates of camera in base frame
# Coordinates of camera in world frame
x_ = x + xc * cos(th) - yc * sin(th)
y_ = y + xc * sin(th) + yc * cos(th)
th_ = th + thc
# Eq. 5.94
h = np.array([alpha - (th_), r - (x_ * cos(alpha) + y_ * sin(alpha))])
Hx = np.zeros((2, self.x.size))
Hx[:, :3] = np.array(
[[0, 0, -1],
[
-cos(alpha), -sin(alpha),
yc * cos(alpha - th_ + thc) - xc * sin(alpha - th_ + thc)
]])
# First two map lines are assumed fixed so we don't want to propagate any measurement correction to them
if j > 1:
Hx[0, 3 + 2 * j] = 1
Hx[1, 3 + 2 * j] = x_ * sin(alpha) - y_ * cos(alpha)
Hx[0, 3 + 2 * j + 1] = 0
Hx[1, 3 + 2 * j + 1] = 1
##############
flipped, h = normalize_line_parameters(h)
if flipped:
Hx[1, :] = -Hx[1, :]
return h, Hx
def map_line_to_predicted_measurement(self, j):
alpha, r = self.x[(3+2*j):(3+2*j+2)] # j is zero-indexed! (yeah yeah I know this doesn't match the pset writeup)
x, y, th = self.x[:3]
xb2c, yb2c, thb2c = self.tf_base_to_camera
# position of camera coordinate system in world frame (taken from Q1)
xcam = x + xb2c*cos(th) - yb2c*sin(th)
ycam = y + xb2c*sin(th) + yb2c*cos(th)
rcam = sqrt(xcam**2 + ycam**2)
acam = math.atan2(ycam, xcam)
aprime = alpha - th - thb2c
rprime = r - rcam*cos(acam-alpha)
h = np.array([aprime, rprime]);
# also taken from Q1
dh2dx = -cos(alpha)
dh2dy = -sin(alpha)
dh2dth = (cos(alpha)*(xb2c*sin(th)+yb2c*cos(th)) -
sin(alpha)*(xb2c*cos(th)-yb2c*sin(th)))
Hx = np.zeros((2,self.x.size))
Hx[:,:3] = np.array([[0, 0, -1],
[dh2dx, dh2dy, dh2dth]]);
# First two map lines are assumed fixed so we don't want to propagate any measurement correction to them
if j > 1:
Hx[0, 3+2*j] = 1
Hx[1, 3+2*j] = -rcam*sin(acam-alpha)
Hx[0, 3+2*j+1] = 0
Hx[1, 3+2*j+1] = 1
flipped, h = normalize_line_parameters(h)
if flipped:
Hx[1,:] = -Hx[1,:]
return h, Hx
def map_line_to_predicted_measurement(self, j):
alpha, r = self.x[(3 + 2 * j):(
3 + 2 * j + 2
)] # j is zero-indexed! (yeah yeah I know this doesn't match the pset writeup)
#### TODO ####
# compute h, Hx (you may find the skeleton for computing Hx below useful)
Hx = np.zeros((2, self.x.size))
Hx[:, :3] = FILLMEIN
# First two map lines are assumed fixed so we don't want to propagate any measurement correction to them
if j > 1:
Hx[0, 3 + 2 * j] = FILLMEIN
Hx[1, 3 + 2 * j] = FILLMEIN
Hx[0, 3 + 2 * j + 1] = FILLMEIN
Hx[1, 3 + 2 * j + 1] = FILLMEIN
##############
flipped, h = normalize_line_parameters(h)
if flipped:
Hx[1, :] = -Hx[1, :]
return h, Hx
def map_line_to_predicted_measurement(self, m):
alpha, r = m
#### TODO ####
# compute h, Hx
# First let's find our camera's location in the robot frame
x_camera, y_camera, theta_camera = self.tf_base_to_camera
x, y, theta = self.x
# Now, we will find our camera's location in the world frame
# Let's first find the transformation matrix
Transformation_robot_to_world = np.array(
[[np.cos(theta), -np.sin(theta), x],
[np.sin(theta), np.cos(theta), y], [0., 0., 1.]])
x_camera_world, y_camera_world, _ = Transformation_robot_to_world.dot(
np.array([x_camera, y_camera, 1.]))
h = np.array([
alpha - theta - theta_camera,
r - x_camera_world * np.cos(alpha) - y_camera_world * np.sin(alpha)
])
Hx = np.array(
[[0., 0., -1.],
[
-np.cos(alpha), -np.sin(alpha),
(y_camera * np.cos(alpha) - x_camera * np.sin(alpha)) *
np.cos(theta) +
(x_camera * np.cos(alpha) + y_camera * np.sin(alpha)) *
np.sin(theta)
]])
##############
flipped, h = normalize_line_parameters(h)
if flipped:
Hx[1, :] = -Hx[1, :]
return h, Hx