Commit ce24bb59 authored by czb5793's avatar czb5793
Browse files

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[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
# 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[0] * cos(observation[1])],
[observation[0] * sin(observation[1])]])
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