### Change the way of encoding the constraint of the landmark observation. i.e....

`Change the way of encoding the constraint of the landmark observation. i.e. x-y sensor model to range-bearing model.`
parent 1eda5cb4
 ... ... @@ -6,6 +6,7 @@ from supervisor.slam.graph.baseclass.Edge import Edge from supervisor.slam.graph.baseclass.Graph import Graph from supervisor.slam.graph.vetor2matrix import * from utils.math_util import normalize_angle import math """ Define Vertex Classes Vertex ... ... @@ -147,6 +148,70 @@ class PosePoseEdge(Edge): s = "PPE: id1:{0},id2: {1},z: [{2}, {3}, {4}]".format(self.id1, self.id2, x, y, yaw) return s # class PoseLandmarkEdge(Edge): # # def __init__(self, id_vertex1, id_vertex2, z, information, list_vertices): # Edge.__init__(self, id_vertex1, id_vertex2, z, information, list_vertices) # assert isinstance(self.vertex1, PoseVertex) # assert isinstance(self.vertex2, LandmarkVertex) # 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 * cos(observation)], # [observation * sin(observation)]]) # return meas_xy, info # # def calc_error_vector(self, x1, x2, z): # # X1 = v2t(x1) # homogeneous matrix of vector x1 # R1 = X1[0:2, 0:2] # rotation matrix # e12 = R1.T @ (x2 - x1[0:2, :]) - z # return e12 # # def linearize_constraint(self, x, lm, z): # """ # Compute the error of a pose-landmark constraint. # :param x1: 3x1 vector (x,y,theta) of the robot pose. # :param lm: 2x1 vector (x,y) of the landmark. # :param z: 2x1 vector (x,y) of the measurement, the position of the landmark in. # the coordinate frame of the robot given by the vector x. # :return: # e 2x1 error of the constraint. # A 2x3 Jacobian wrt x. # B 2x2 Jacobian wrt lm. # """ # theta_i = x[2, 0] # xi, yi = x[0, 0], x[1, 0] # xl, yl = lm[0, 0], lm[1, 0] # # Compute eij partial derivative with respect to pose x # c = cos(theta_i) # s = sin(theta_i) # A = np.array([[-c, -s, -(xl - xi) * s + (yl - yi) * c], # [s, -c, -(xl - xi) * c - (yl - yi) * s]]) # B = np.array([[c, s], # [-s, c]]) # # e = self.calc_error_vector(x, lm, z) # return e, A, B # # def __str__(self): # x, y, yaw = np.squeeze(self.z) # s = "PLE: id1:{0},id2: {1},z: [{2}, {3}]".format(self.id1, self.id2, x, y) # return s class PoseLandmarkEdge(Edge): def __init__(self, id_vertex1, id_vertex2, z, information, list_vertices): ... ... @@ -165,20 +230,20 @@ class PoseLandmarkEdge(Edge): :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. meas_rb: a encoded measurement vector (d, a) related to the robot, which will be set to an edge as a measurement vector z. info: an information matrix """ meas_rb = np.array(observation)[0:2].reshape(2, 1) info = inv(covariance) meas_xy = np.array([[observation * cos(observation)], [observation * sin(observation)]]) return meas_xy, info def calc_error_vector(self, x1, x2, z): return meas_rb, info X1 = v2t(x1) # homogeneous matrix of vector x1 R1 = X1[0:2, 0:2] # rotation matrix e12 = R1.T @ (x2 - x1[0:2, :]) - z return e12 def calc_error_vector(self, x, lm, z): delta = lm - x[0:2] distance = math.sqrt(delta[0,0]**2 + delta[1, 0]**2) # expected measurement angle = math.atan2(delta[1, 0], delta[0, 0]) error = np.array([[distance - z[0, 0]], [angle - z[1, 0]]]) return error def linearize_constraint(self, x, lm, z): """ ... ... @@ -192,25 +257,17 @@ class PoseLandmarkEdge(Edge): A 2x3 Jacobian wrt x. B 2x2 Jacobian wrt lm. """ theta_i = x[2, 0] xi, yi = x[0, 0], x[1, 0] xl, yl = lm[0, 0], lm[1, 0] # Compute eij partial derivative with respect to pose x c = cos(theta_i) s = sin(theta_i) A = np.array([[-c, -s, -(xl - xi) * s + (yl - yi) * c], [s, -c, -(xl - xi) * c - (yl - yi) * s]]) B = np.array([[c, s], [-s, c]]) delta_x = lm[0, 0] - x[0, 0] delta_y = lm[1, 0] - x[1, 0] q = delta_x**2 + delta_y**2 sq = math.sqrt(q) A = np.array([[-sq*delta_x, -sq*delta_y, 0], [delta_y, -delta_x, -q]]) / q B = np.array([[sq*delta_x, sq*delta_y], [-delta_y, delta_x]]) / q e = self.calc_error_vector(x, lm, z) return e, A, B def __str__(self): x, y, yaw = np.squeeze(self.z) s = "PLE: id1:{0},id2: {1},z: [{2}, {3}]".format(self.id1, self.id2, x, y) return s """ Define Graph Class Graph ... ...
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