Commit c3deb338 authored by czb5793's avatar czb5793
Browse files

1. Simplify the code of calculating the Jacobian of linearization

2. Move the raw measurement encoding to this file
parent fabf5df0
......@@ -97,6 +97,24 @@ class PosePoseEdge(Edge):
e12 = t2v(inv(Z) @ inv(X1) @ X2)
return e12
@staticmethod
def encode_measurement(pose, odom_pose, covariance):
"""
Calculate the measurement vector that represents how the robot move from previous pose to the expected pose.
The jacobian matrix of the error function is calculated by this model.
The vector (x, y, theta) is obtained from a rotation matrix R(theta) and a translational vector t = [x, y].T
:param pose: previous pose of the robot
:param odom_pose: expected pose of the next step calculated by the motion model
:param covariance: covariance matrix
return:
meas: a converted measurement related to the previous pose, which will be set to an edge as a measurement vector z.
info: an information matrix
"""
M = inv(v2t(pose)) @ v2t(odom_pose) # 3x3 transformation matrix from x_meas to vertex1.pose
info = inv(covariance) # information matrix
meas = t2v(M) # a converted measurement.
return meas, info
def linearize_constraint(self, x1, x2, z):
"""
Linearize the pose-pose constraint.
......@@ -111,13 +129,15 @@ class PosePoseEdge(Edge):
"""
c = cos(x1[2, 0] + z[2, 0])
s = sin(x1[2, 0] + z[2, 0])
t = x2[0:2, :] - x1[0:2, :]
Rd = np.array([[-s, c], [-c, -s]])
Re = np.array([[-c, -s], [s, -c]])
A = np.hstack((Re, Rd @ t))
A = np.vstack((A, [0, 0, -1]))
B = np.hstack((-Re, np.zeros((2, 1))))
B = np.vstack((B, [0, 0, 1]))
tx = x2[0, 0] - x1[0, 0]
ty = x2[1, 0] - x1[1, 0]
A = np.array([[-c, -s, -tx*s+ty*c],
[s, -c, -tx*c-ty*s],
[0, 0, -1]])
B = np.array([[c, s, 0],
[-s, c, 0],
[0, 0, 1]])
e = self.calc_error_vector(x1, x2, z)
return e, A, B
......@@ -136,6 +156,23 @@ class PoseLandmarkEdge(Edge):
assert z.shape == (2, 1)
assert information.shape == (2, 2)
@staticmethod
def encode_measurement(observation, covariance):
"""
Encode the raw_measurement of landmark observation.
The vector meas_xy represents how the robot sees a landmark. This vector is a cartesian coordinate (x, y).
The Jacobian matrix of the error function is calculated by this model.
:param observation: The raw measurement from range bearing sensor (distance, angle)
:param covariance: The covariance matrix of the measurement
return
meas_xy: a encoded measurement vector (x, y) related to the robot, which will be set to an edge as a measurement vector z.
info: an information matrix
"""
info = inv(covariance)
meas_xy = np.array([[observation[0] * cos(observation[1])],
[observation[0] * sin(observation[1])]])
return meas_xy, info
def calc_error_vector(self, x1, x2, z):
X1 = v2t(x1) # homogeneous matrix of vector x1
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment