def update_pos(self, pose, absolute=False):
"""Convenience function to do a position update."""
# assuming that the ekf is never lost by more than pi (except on startup),
# if we have absolute (i.e. QR-code) data, and it's far from our current
# estimate, then assume we're lost and reset the EKF.
if absolute:
# we can intelligently mod the incoming pose angle to be close
# to the current estimate - note that the ordered if statements
# mean that if we are super-lost, we will randomly add some pi
# but not infinitely loop.
# this if statement says that if we're lost in theta, don't worry about it.
if self.covariance[2,2] < 10:
while pose.theta - self.state[2,0] > pi:
pose.theta -= tau
while self.state[2,0] - pose.theta > pi:
pose.theta += tau
dx = self.state[0,0]-pose.x
dy = self.state[1,0]-pose.y
dt = fmod(self.state[2,0]-pose.theta,2*pi)
if dx*dx + dy*dy > 0.1 or abs(dt) > 0.3:
self.state = column_vector(pose.x, pose.y, pose.theta)
self.covariance = matrix(pose.cov).reshape(3, 3)
return
else:
pose.x += self.state[0,0]
pose.y += self.state[1,0]
while pose.theta > pi:
pose.theta -= tau
while pose.theta < -pi:
pose.theta += tau
pose.theta += self.state[2,0]
y = column_vector(pose.x, pose.y, pose.theta)
rospy.loginfo("Updating with position %s",y)
W = matrix(pose.cov).reshape(3, 3)
self.update(y, W)
def update_pos(self, pose, absolute=False):
"""Convenience function to do a position update."""
# assuming that the ekf is never lost by more than pi (except on startup),
# if we have absolute (i.e. QR-code) data, and it's far from our current
# estimate, then assume we're lost and reset the EKF.
if absolute:
# we can intelligently mod the incoming pose angle to be close
# to the current estimate - note that the ordered if statements
# mean that if we are super-lost, we will randomly add some pi
# but not infinitely loop.
# this if statement says that if we're lost in theta, don't worry about it.
if self.covariance[2, 2] < 10:
while pose.theta - self.state[2, 0] > pi:
pose.theta -= tau
while self.state[2, 0] - pose.theta > pi:
pose.theta += tau
dx = self.state[0, 0] - pose.x
dy = self.state[1, 0] - pose.y
dt = fmod(self.state[2, 0] - pose.theta, 2 * pi)
if dx * dx + dy * dy > 0.1 or abs(dt) > 0.3:
self.state = column_vector(pose.x, pose.y, pose.theta)
self.covariance = matrix(pose.cov).reshape(3, 3)
return
else:
pose.x += self.state[0, 0]
pose.y += self.state[1, 0]
while pose.theta > pi:
pose.theta -= tau
while pose.theta < -pi:
pose.theta += tau
pose.theta += self.state[2, 0]
y = column_vector(pose.x, pose.y, pose.theta)
rospy.loginfo("Updating with position %s", y)
W = matrix(pose.cov).reshape(3, 3)
self.update(y, W)
def backward_pass(self, target):
""" Perform a backward pass on the network given the
negative of the derivative of the error with respect to the output.
"""
self.errs.append(self.loss_func(self.output, target))
out = column_vector(self.output)
dE_dOut = -1 * self.loss_func(out, column_vector(target), True)
dOut_dNet = self.output_activ_func(out, True, True)
delta_out = np.multiply(column_vector(dE_dOut),
column_vector(dOut_dNet))
deltas = []
t = np.dot(self.W_oh.T, delta_out)
mat = np.dot(delta_out, self.hidden_layers[-1].a_h.T)
self.W_oh += self.l_rate * mat
for i in range(1, len(self.hidden_layers) + 1):
layer = self.hidden_layers[-i]
delta = np.multiply(t, layer.activ_func(layer.a_h, True, True))
deltas.append(delta)
if i < len(self.hidden_layers):
t = np.dot(layer.weights.T, delta)
# t = np.dot(self.W_oh.T, delta_out)
mat = np.dot(delta, self.hidden_layers[-(i + 1)].a_h.T)
layer.weights += self.l_rate * mat
def __init__(self, rotMatrix, transVect, cameraMatrix,
distCoeffs, anchor_points=None):
"""
Extrinsic parameters : rotMatrix, transVect.
Intrinsic parameters : cameraMatrix, distCoeffs.
"""
self.rotMatrix = check_shape(rotMatrix, (3, 3), "rotation matrix")
self.transVect = column_vector(transVect, 3, "translation vector")
self.cameraMatrix = check_shape(cameraMatrix, (3, 3), "camera matrix")
self.distCoeffs = column_vector(distCoeffs, 4, "distort coefficients")
self.anchor_points = anchor_points
def encode(self, data_vector):
"""Encode a single data_vector
"""
# make sure its a column vector
data_vector = column_vector(data_vector)
self.net_h = np.dot(self.weights, data_vector)
self.a_h = self.activ_func(self.net_h)
return self.a_h
def __init__(self):
# x(k|k); the system state column vector.
self.state = column_vector(0.0, 0.0, 0.0)
# P(k|k); the system covariance matrix.
self.covariance = matrix([[1000.0, 0.0, 0.0], [0.0, 1000.0, 0.0],
[0.0, 0.0, 1000.0]])
# Need to store old odom state for delta updates.
self.odom_state = None
self.lastdt = rospy.get_time() - 1
def update_pos(self, pose): if self.covariance[2,2] < 1000: while pose.theta - self.state[2] > pi: pose.theta -= tau while self.state[2] - pose.theta > pi: pose.theta += tau y = column_vector(pose.x, pose.y, pose.theta) W = matrix(pose.cov).reshape(3, 3) self.update2(y, W)
def update_pos(self, pose):
if self.covariance[2, 2] < 1000:
while pose.theta - self.state[2] > pi:
pose.theta -= tau
while self.state[2] - pose.theta > pi:
pose.theta += tau
y = column_vector(pose.x, pose.y, pose.theta)
W = matrix(pose.cov).reshape(3, 3)
self.update2(y, W)
def forward_pass(self, input):
""" Perform a forward pass on the network given
an input vector.
"""
self.input = input
for layer in self.hidden_layers:
self.input = layer.encode(self.input)
self.output = np.dot(self.W_oh, column_vector(self.input))
self.output = self.output_activ_func(self.output + self.B_o)
return self.output
def __init__(self):
# x(k|k); the system state column vector.
self.state = column_vector(0.0, 0.0, 0.0)
# P(k|k); the system covariance matrix.
self.covariance = matrix([
[1000.0, 0.0, 0.0],
[ 0.0, 1000.0, 0.0],
[ 0.0, 0.0, 1000.0]])
# Need to store old odom state for delta updates.
self.odom_state = None
self.lastdt = rospy.get_time() - 1
def update_pos(self, pose):
"""Convenience function to do a position update."""
# assuming that the ekf is never lost by more than pi (except on startup),
# we can intelligently mod the incoming pose angle to be close to the current estimate
# note that the ordered if statements mean that if we are super-lost, we
# will randomly add some pi but not infinitely loop.
# this if statement says that if we're lost in theta, don't worry about it.
if self.covariance[2, 2] < 1000:
while pose.theta - self.state[2] > pi:
pose.theta -= tau
while self.state[2] - pose.theta > pi:
pose.theta += tau
y = column_vector(pose.x, pose.y, pose.theta)
W = matrix(pose.cov).reshape(3, 3)
self.update(y, W)
def update_pos(self, pose):
"""Convenience function to do a position update."""
# assuming that the ekf is never lost by more than pi (except on startup),
# we can intelligently mod the incoming pose angle to be close to the current estimate
# note that the ordered if statements mean that if we are super-lost, we
# will randomly add some pi but not infinitely loop.
# this if statement says that if we're lost in theta, don't worry about it.
if self.covariance[2,2] < 1000:
while pose.theta - self.state[2] > pi:
pose.theta -= tau
while self.state[2] - pose.theta > pi:
pose.theta += tau
y = column_vector(pose.x, pose.y, pose.theta)
W = matrix(pose.cov).reshape(3, 3)
self.update(y, W)
def get_jacobi_matrix(self, world_point):
"""
Given a point `q` in world frame and its corresponding pixel `Pix`,
return the jacobi matrix of Pix = f(q) at q.
"""
R, T, K, D = self.params
# Pix = (x, y, 1), sPix = s * Pix = K * (R * q + T), s = sPix[2, 0]
q = column_vector(world_point, 3)
sPix = K.dot(R.dot(q) + T)
s = sPix[2, 0]
Pix = sPix / s
# d(sPix)/dq = K * R
# d(sPix)/dq = (d(s*x)/dq, d(s*y)/dq, ds/dq) ==> ds/dq = (K*R)[2]
# d(sPix)/dq = ds/dq * Pix + dPix/dq * s ==
# ==> dPix/dq = (d(sPix)/dq - ds/dq * Pix) / s
dsPix = np.dot(K, R)
ds = dsPix[2].reshape(1, 3)
dPix = (dsPix - np.dot(Pix, ds)) / s
return dPix[:2]
def get_odom_delta(self, pose):
"""Calculates the delta of the odometry position since the last time called.
Returns None the first time, and a 3x1 state matrix thereafter.
"""
# Convert the pose into matrix form.
y_odom = column_vector(pose.x, pose.y, pose.theta)
# None is the fallback in case we can't get an actual delta.
odom_delta = None
if self.odom_state is not None:
# Transform the sensor state into the map frame.
y = coord_transform(y_odom, self.odom_origin)
# Calculate the change odom state, in map coords (delta y).
odom_delta = y - coord_transform(self.odom_state, self.odom_origin)
# Update the stored odom state.
self.odom_state = y_odom
# Get the odom frame origin in the map frame and store it for next time.
self.odom_origin = get_offset(self.state, self.odom_state)
return odom_delta
def get_odom_delta(self, pose):
"""Calculates the delta of the odometry position since the last time called.
Returns None the first time, and a 3x1 state matrix thereafter.
"""
# Convert the pose into matrix form.
y_odom = column_vector(pose.x, pose.y, pose.theta)
# None is the fallback in case we can't get an actual delta.
odom_delta = None
if self.odom_state is not None:
# Transform the sensor state into the map frame.
y = coord_transform(y_odom, self.odom_origin)
# Calculate the change odom state, in map coords (delta y).
odom_delta = y - coord_transform(self.odom_state, self.odom_origin)
# Update the stored odom state.
self.odom_state = y_odom
# Get the odom frame origin in the map frame and store it for next time.
self.odom_origin = get_offset(self.state, self.odom_state)
return odom_delta
def get_odom_delta(self, pose): # odom_state is the previous x, y, theta of odom_pose # y_odom is the current x, y, theta of odom_pose # odom_origin kind keeps track of all the odom changes till date y_odom = column_vector(pose.x, pose.y, pose.theta) odom_delta = None if self.odom_state is not None: # y = y_odom * rot(odom_origin.theta) + odom_origin y = coord_transform(y_odom, self.odom_origin) # current change - previous change odom_delta = y - coord_transform(self.odom_state, self.odom_origin) self.odom_state = y_odom # these two should be exactly the same. Using y_odom to # differentiate between previous and new odom values #self.odom_origin = get_offset(self.state, self.odom_state) self.odom_origin = get_offset(self.state, y_odom) return odom_delta
def get_odom_delta(self, pose):
# odom_state is the previous x, y, theta of odom_pose
# y_odom is the current x, y, theta of odom_pose
# odom_origin kind keeps track of all the odom changes till date
y_odom = column_vector(pose.x, pose.y, pose.theta)
odom_delta = None
if self.odom_state is not None:
# y = y_odom * rot(odom_origin.theta) + odom_origin
y = coord_transform(y_odom, self.odom_origin)
# current change - previous change
odom_delta = y - coord_transform(self.odom_state, self.odom_origin)
self.odom_state = y_odom
# these two should be exactly the same. Using y_odom to
# differentiate between previous and new odom values
#self.odom_origin = get_offset(self.state, self.odom_state)
self.odom_origin = get_offset(self.state, y_odom)
return odom_delta