Commit 3f495e40 authored by czb5793's avatar czb5793
Browse files

Change the class name of landmark matcher to feature detector.

parent c7bf316e
......@@ -157,9 +157,9 @@ slam:
# These are x position and y position
# Currently only supports 2
landmark_state_size: 2
# Determines whether landmark-matcher is used
# Determines whether landmark-identifiers is used
# It determines whether correspondences of landmarks are given, i.e. identifies of landmarks are given
landmark_matcher: true
feature_detector: true
# Configures the sensor noise. The values are currently empirically chosen.
sensor_noise:
......
......@@ -156,9 +156,9 @@ slam:
# These are x position and y position
# Currently only supports 2
landmark_state_size: 2
# Determines whether landmark-matcher is used
# Determines whether landmark-identifiers is used
# It determines whether correspondences of landmarks are given, i.e. identifies of landmarks are given
landmark_matcher: true
feature_detector: true
# Configures the sensor noise. The values are currently empirically chosen.
sensor_noise:
# Standard deviation of the detected distance in meters
......
......@@ -94,8 +94,8 @@ class Physics:
else:
sensor.detect(None)
# if a feature has been extracted from the environment, update the matcher with the feature identifier
# if a feature has been extracted from the environment, update the identifiers with the feature identifiers
if dmin != float('inf') and type(solid_object) == FeaturePoint:
robot.landmark_matcher.detect(sensor.id, solid_object.id)
robot.feature_detector.detect(sensor.id, solid_object.id)
else:
robot.landmark_matcher.detect(sensor.id, -1)
robot.feature_detector.detect(sensor.id, -1)
......@@ -22,7 +22,7 @@ from models.Polygon import *
from robot.sensor.ProximitySensor import *
from supervisor.Supervisor import *
from robot.sensor.WheelEncoder import *
from robot.sensor.LandmarkMatcher import *
from robot.sensor.FeatureDetector import *
class Robot: # Khepera III robot
......@@ -54,8 +54,8 @@ class Robot: # Khepera III robot
ir_pose = Pose(_pose[0], _pose[1], radians(_pose[2]))
self.ir_sensors.append(
ProximitySensor(self, ir_pose, robot_cfg["sensor"], id))
# Landmark Matcher
self.landmark_matcher = LandmarkMatcher()
# Feature detector
self.feature_detector = FeatureDetector()
# dynamics
self.dynamics = DifferentialDriveDynamics(self.wheel_radius, self.wheel_base_length)
......
......@@ -44,12 +44,12 @@ class RobotSupervisorInterface:
return [e.read() for e in self.robot.wheel_encoders]
def read_landmark_matcher(self):
def read_feature_detector(self):
"""
:return: List of landmark ids
"""
matcher = self.robot.landmark_matcher.read()
return [matcher[sensor.id] for sensor in self.robot.ir_sensors]
feature_detector = self.robot.feature_detector.read()
return [feature_detector[sensor.id] for sensor in self.robot.ir_sensors]
def set_wheel_drive_rates(self, v_l, v_r):
"""
......
"""
This sensor provides a set of identifiers of landmarks.
To apply this class, we assume that the data associations of landmarks are known.
A "Feature Detector" can be a camera that can extract features from the environment.
"""
from robot.sensor.Sensor import *
class FeatureDetector(Sensor):
def __init__(self):
"""
Identifiers: a dictionary that maps sensor identifiers to feature identifiers
"""
self.identifiers = dict()
def detect(self, sensor_id, feature_id):
self.identifiers[sensor_id] = feature_id
def read(self):
return self.identifiers
"""
This sensor provides a set of correspondences of landmarks.
To apply this class, we assume that the data associations of landmarks are known.
A "landmark matcher" can be a camera that can extract features from the environment,
also scan-matching algorithms are apply to match those features.
"""
from robot.sensor.Sensor import *
class LandmarkMatcher(Sensor):
def __init__(self):
self.matcher = dict()
def detect(self, sensor_id, landmark_id):
self.matcher[sensor_id] = landmark_id
def read(self):
return self.matcher
......@@ -192,7 +192,7 @@ class MapManager:
if type(obstacle) == FeaturePoint:
continue
features += self.__generate_feature_obstacle(world, obstacle.global_geometry.vertexes)
# add identifier for each feature.
# add identifiers for each feature.
for i, f in enumerate(features):
f.id = i
......
......@@ -231,7 +231,7 @@ class Supervisor:
motion_command = np.array([[v], [yaw]])
measured_distances = self.proximity_sensor_distances_from_robot_center
sensor_angles = [pose.theta for pose in self.proximity_sensor_placements]
landmark_ids = self.robot.read_landmark_matcher()
landmark_ids = self.robot.read_feature_detector()
# update LAM estimations
for slam in self.reg_slam:
if slam is not None:
......
......@@ -33,7 +33,7 @@ class EKFSlam(Slam):
self.motion_noise = np.diag([slam_cfg["ekf_slam"]["motion_noise"]["x"],
slam_cfg["ekf_slam"]["motion_noise"]["y"],
np.deg2rad(slam_cfg["ekf_slam"]["motion_noise"]["theta"])]) ** 2
self.landmark_correspondence_given = slam_cfg["landmark_matcher"]
self.landmark_correspondence_given = slam_cfg["feature_detector"]
# The estimated combined state vector, initially containing the robot pose at the origin and no landmarks
self.mu = np.zeros((self.robot_state_size, 1))
# The state covariance, initially set to absolute certainty of the initial robot pose
......@@ -206,11 +206,11 @@ class EKFSlam(Slam):
if u[1, 0] == 0:
G = np.array([[0, 0, -dt * u[0] * sin(x[2, 0])],
[0, 0, dt * u[0] * cos(x[2, 0])],
[0, 0, 0]])
[0, 0, 0]], dtype=np.float)
else:
G = np.array([[0, 0, u[0, 0] / u[1, 0] * (cos(x[2, 0] + dt * u[1, 0]) - cos(x[2, 0]))],
[0, 0, u[0, 0] / u[1, 0] * (sin(x[2, 0] + dt * u[1, 0]) - sin(x[2, 0]))],
[0, 0, 0]])
[0, 0, 0]], dtype=np.float)
G = np.identity(self.robot_state_size) + G
return G
......
......@@ -63,7 +63,7 @@ class FastSlam(Slam):
np.deg2rad(slam_cfg["sensor_noise"]["detected_angle"])]) ** 2
self.motion_noise = np.diag([slam_cfg["fast_slam"]["motion_noise"]["translational_velocity"],
slam_cfg["fast_slam"]["motion_noise"]["rotational_velocity"]]) ** 2
self.landmark_correspondence_given = slam_cfg["landmark_matcher"]
self.landmark_correspondence_given = slam_cfg["feature_detector"]
# Create initial list of particles
self.particles = [Particle(self.landmark_state_size) for _ in range(self.n_particles)]
......
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