Commit c597068c authored by czb5793's avatar czb5793
Browse files

small changes

parent 00517e24
......@@ -180,7 +180,7 @@ slam:
theta: 0.1
# Configures the sensor noise. The values are currently empirically chosen.
sensor_noise:
# Standard deviation of the detected distance in meters 0.2 0.01
# Standard deviation of the detected distance in meters
detected_distance: 0.2
# Standard deviation of the detected angle in degrees
detected_angle: 30
......@@ -199,7 +199,7 @@ slam:
rotational_velocity: 0.0001
# Configures the sensor noise. The values are currently empirically chosen.
sensor_noise:
# Standard deviation of the detected distance in meters 0.2 0.01
# Standard deviation of the detected distance in meters
detected_distance: 0.2
# Standard deviation of the detected angle in degrees
detected_angle: 30
......@@ -262,7 +262,7 @@ slam:
width: 8
# Height of the map in meters
height: 8
# Number of grids per meter. Note that high resolution will lead to performance issues.
# Number of grids per meter. High resolution may lead to performance issues.
resolution: 20
path_planning:
......
......@@ -180,7 +180,7 @@ slam:
theta: 1
# Configures the sensor noise. The values are currently empirically chosen.
sensor_noise:
# Standard deviation of the detected distance in meters 0.2 0.01
# Standard deviation of the detected distance in meters
detected_distance: 0.2
# Standard deviation of the detected angle in degrees
detected_angle: 30
......@@ -193,13 +193,13 @@ slam:
n_particles: 200
# Configures the motion noise. The values are currently empirically chosen.
motion_noise:
# Standard deviation of the motion command's translational velocity in m/s. # 0.02,0.015
# Standard deviation of the motion command's translational velocity in m/s.
translational_velocity: 0.005
# Standard deviation of the motion command's rotational velocity in rad/s.
rotational_velocity: 0.005
# Configures the sensor noise. The values are currently empirically chosen.
sensor_noise:
# Standard deviation of the detected distance in meters 0.2 0.01
# Standard deviation of the detected distance in meters
detected_distance: 0.2
# Standard deviation of the detected angle in degrees
detected_angle: 30
......@@ -263,7 +263,7 @@ slam:
width: 5
# Height of the map in meters
height: 6
# Number of grids per meter. Note that high resolution will lead to performance issues.
# Number of grids per meter. High resolution may lead to performance issues.
resolution: 20
path_planning:
......
......@@ -179,7 +179,7 @@ slam:
theta: 0.1
# Configures the sensor noise. The values are currently empirically chosen.
sensor_noise:
# Standard deviation of the detected distance in meters 0.2 0.01
# Standard deviation of the detected distance in meters
detected_distance: 0.2
# Standard deviation of the detected angle in degrees
detected_angle: 30
......@@ -198,7 +198,7 @@ slam:
rotational_velocity: 0.0001
# Configures the sensor noise. The values are currently empirically chosen.
sensor_noise:
# Standard deviation of the detected distance in meters 0.2 0.01
# Standard deviation of the detected distance in meters
detected_distance: 0.2
# Standard deviation of the detected angle in degrees
detected_angle: 30
......@@ -261,7 +261,7 @@ slam:
width: 8
# Height of the map in meters
height: 8
# Number of grids per meter. Note that high resolution will lead to performance issues.
# Number of grids per meter. High resolution may lead to performance issues.
resolution: 20
path_planning:
......
......@@ -8,7 +8,7 @@ of the current robot state and a map of its surrounding environment based on its
and proximity sensor readings. While in my extension Graph-based SLAM algorithm was integrated
to perform an estimation of the full traverse trajectory additionally. Moreover, Occupancy Grid Mapping algorithm
and A* Path Planning algorithm were also integrated into the simulator. The path planning algorithm searches a path
for the robot towards the goal based on the map created by the mapping algorithm.
for the robot towards the goal based on the grid map created by the mapping algorithm.
## Setup
......@@ -18,7 +18,7 @@ It is recommended to run the simulator directly on your native machine. For this
- PyGTK 3: Please follow the instructions at https://pygobject.readthedocs.io/en/latest/getting_started.html
An additional dependency (see [scikit-sparse](https://scikit-sparse.readthedocs.io/en/latest/overview.html))
is required for the Graph-based SLAM algorithm:
is required for the Graph-based SLAM algorithm (if sparse Cholesky decomposition is enabled):
sudo apt-get install libsuitesparse-dev
pip3 install --user scikit-sparse
......@@ -27,24 +27,42 @@ Additional dependencies can then be installed using `pip`
pip3 install matplotlib numpy scipy pandas
pip3 install pyyaml
Here, *numpy* and *scipy* are required for scientific computing,
*pandas* for analysing the experimental results, and *matplotlib* for
visualizing the results.
The simulator is then started by
python rimulator.py
which uses the default `config.yaml` configuration file. A specific configuration file can be
which uses the default `config.yaml` configuration file.
A specific configuration file can be
specified as an additional program parameter:
python rimulator.py original_config.yaml
python rimulator.py config01.yaml
Note that `config.yaml` is the configuration file especially for the Graph-based SLAM simulation,
where the feature-map with large rectangular obstacles
is used:
Note that `config.yaml` is the configuration file
especially for the Graph-based SLAM simulation.
The environment is represented by
the feature-based maps with large rectangular obstacles. In particular, there
are point-like features on the boundary.
For simulation of the following map, the user can start the simulator by
python rimulator.py
and then load the map [graph_based_slam_example_1](maps/graph_based_slam_example_1)
![Screenshot](documentation/sim_config.png)
The configuration file `config01.yaml` is for the point-like feature-map without any obstacles where the EKFSLAM and FastSLAM are performed in particular:
For simulation of the following map, the user can start the simulator by
python rimulator.py config01.yaml
and then load the map [slam_example_1](maps/slam_example_1)
![Screenshot](documentation/sim_config01.png)
Alternatively, the simulator can be run using `docker`, as described in [documentation/docker.md](documentation/docker.md).
## Graphical User Interface
......@@ -88,11 +106,11 @@ representing the robot poses and landmark-vertices representing the landmark pos
- **Start/Reset Mapping**: Starts the Occupancy Grid Mapping algorithm to evaluate a grid map,
also a path is generated on the map by A* Path Planning algorithm.
## Configuration
The simulator can be configured by a variety of parameters. The default configuration file is [config.yaml](config01.yaml)
The simulator can be configured by a variety of parameters. The default configuration file is [config.yaml](config.yaml)
where all parameters are documented. This file includes the EKF Slam, FastSlam, Graph based Slam, Occupancy gird mapping and A* path planning that can be enabled or
disabled. The configuration file [config01.yaml](config01.yaml) includes
an extension performing almost identical to the [sobot rimulator](https://collaborating.tuhh.de/cva9931/sobot-rimulator) of Michael Dobler
while a frame of the Graph-based SLAM is appended on the right. The configuration file [original_config.yaml](original_config.yaml) does not include
an extension performing almost identical to the [sobot rimulator](https://collaborating.tuhh.de/cva9931/sobot-rimulator) by Michael Dobler
while a frame of the Graph-based SLAM is appended on the right in addition. The configuration file [original_config.yaml](original_config.yaml) does not include
any of the extensions made and performs completely identical to the original sobot rimulator.
The most important parameters in terms of the SLAM algorithms are:
......@@ -108,20 +126,22 @@ the frequency of the SLAM algorithm considering a landmark as "new" instead of a
### 2. Graph-based SLAM
- `feature_detector`: Determines whether SLAM algorithms should work with known data association.
Note that the Graph-based SLAM only works with known data association.
- `frontend_interval`: Specifies the time interval of adding a pose-vertex in simulation cycles. The lower the interval is, the better graph can be obtained by graph optimization.
However, the graph will grow faster when smaller interval is applied, which can lead to performance problems during graph optimization
- `frontend_pose_density`: Specifies the minimum distance between two neighboring pose-vertices while adding a new one during the front-end process.
- `num_fixed_vertexes`: Specifies the number of vertices that are fixed during optimization.
Note that the existing Graph-based SLAM algorithm only works with known data association, while EKF SLAM and FastSLAM are able to work with unknown data association.
- `frontend_interval`: Specifies the time interval of adding a pose-vertex in simulation cycles. The lower the interval is,
the faster the graph grows, and the better evaluation can be obtained by graph optimization.
However, smaller interval may lead to performance problems during graph optimization.
- `frontend_pose_density`: Specifies the minimum distance between two neighboring pose-vertices while adding a new one in the graph during the front-end process.
- `num_fixed_vertexes`: Specifies the number of the fixed vertices during the optimization.
- `solver`: Specifies the sparse solver during the optimization. We can use either `spsolve` or `cholesky`.
Note that `cholesky` requires the **scikit-sparse** library.
Note that `cholesky` requires the [**scikit-sparse**](https://scikit-sparse.readthedocs.io/en/latest/overview.html)
library.
### 3. Occupancy Gird Mapping and Path Planning:
- `resolution`: Specifies the resolution of the grid map estimated by occupancy grid
mapping algorithm. The mapping algorithm can handle high resolution maps efficiently,
but high resolution can lead to performance problems of the GUI while grids are being drawn on the frames
but high resolution may lead to performance problems.
- `heuristic_weight`: Determines how important the heuristic term is when planning a path from the robot position to the goal.
If the value is 0, that means that heuristic term is not considered.
If the value is 0, meaning that the heuristic term is not considered.
### 4. SLAM Evaluation
The data during the simulation can be recorded and saved in the folder [./scripts](./scripts). They are [sobot_information1.csv](scripts/sobot_information1.csv) and
......
......@@ -150,16 +150,19 @@ class Simulator:
# register slam estimations to the system
self.reg_slam_evaluations = [self.ekfslam_evaluation, self.fastslam_evaluation, self.graphbasedslam_evaluation]
list_slam = [self.world.supervisors[0].ekfslam, self.world.supervisors[0].fastslam,
self.world.supervisors[0].graphbasedslam]
list_mapping = [self.world.supervisors[0].ekfslam_mapping, self.world.supervisors[0].fastslam_mapping,
self.world.supervisors[0].graphbasedslam_mapping]
self.slam_evaluation = None
if self.cfg["slam"]["evaluation"]["enabled"]:
list_slam = [self.world.supervisors[0].ekfslam, self.world.supervisors[0].fastslam, self.world.supervisors[0].graphbasedslam]
self.slam_evaluation = SlamEvaluation(list_slam,
self.cfg["slam"]["evaluation"], self.world.robots[0])
for slam in list_slam:
if slam is not None:
slam.callback = self.slam_evaluation.time_per_step # set the callback function
list_mapping = [self.world.supervisors[0].ekfslam_mapping, self.world.supervisors[0].fastslam_mapping,
self.world.supervisors[0].graphbasedslam_mapping]
for mapping in list_mapping:
if mapping is not None:
mapping.callback = self.slam_evaluation.time_per_step # set the callback function
......
......@@ -50,7 +50,7 @@ class MapManager:
obstacles += self.__generate_rectangle_obstacles(world)
if self.cfg["obstacle"]["feature"]["enabled"] \
and self.cfg["obstacle"]["rectangle"]["enabled"]:
obstacles += self.__generate_features(world, obstacles)
obstacles += self.__generate_features(obstacles)
if self.cfg["obstacle"]["feature"]["enabled"] \
and not self.cfg["obstacle"]["rectangle"]["enabled"]:
obstacles += self.__generate_random_features(world)
......@@ -87,9 +87,9 @@ class MapManager:
def __generate_random_features(self, world):
"""
Generate random octagon obstacles
Generate random features represented by octagon obstacles
:param world: The world for which they are generated
:return: List of generated octagon obstacles
:return: List of generated features
"""
obs_radius = self.cfg["obstacle"]["feature"]["radius"]
obs_min_count = self.cfg["obstacle"]["feature"]["min_count"]
......@@ -172,42 +172,17 @@ class MapManager:
obstacles.append(obstacle)
return obstacles
def __generate_feature_line2(self, world, x0, y0, x1, y1):
r = 8 # resolution: pixs/meter
obs_radius = 0.04
line = geometrics.bresenham_line(x0*r, y0*r, x1*r, y1*r)
test_geometries = [r.global_geometry for r in world.robots]
obstacles = []
for x, y in line:
x = x/r
y = y/r
theta = -pi + (random() * 2 * pi)
obstacle = FeaturePoint(obs_radius, Pose(x, y, theta), 0)
# intersects = self.__check_obstacle_intersections([x,y])
# if not intersects:
# obstacles.append(obstacle)
intersects = False
for test_geometry in test_geometries:
intersects |= geometrics.convex_polygon_intersect_test(test_geometry, obstacle.global_geometry)
if not intersects:
obstacles.append(obstacle)
return obstacles
def feature_test_geometry(self, x,y):
n = 6
r = 0.3
goal_test_geometry = []
for i in range(n):
goal_test_geometry.append(
[x + r * cos(i * 2 * pi / n), y + r * sin(i * 2 * pi / n)]
)
test_geometry = Polygon(goal_test_geometry)
return test_geometry
def __generate_feature_line(self, x0, y0, x1, y1, obs_radius, density):
def __generate_feature_line(self, x0, y0, x1, y1, radius, density):
"""
Generate features along a line.
:param x0: coordinate of the starting point.
:param y0: coordinate of the end
:param x1: coordinate of the starting point
:param y1: coordinate of the end
:param radius: the radius of feature-point.
:param density: density of the features on the line.
:return:
"""
c = density # feature density
a = atan2((y1-y0), (x1-x0))
obstacles = []
......@@ -222,12 +197,17 @@ class MapManager:
theta = -pi + (random() * 2 * pi)
nx = dx*(random()-1)*2
ny = dy*(random()-1)*2
feature = FeaturePoint(obs_radius, Pose(x + nx, y + ny, theta), 0)
feature = FeaturePoint(radius, Pose(x + nx, y + ny, theta), 0)
obstacles.append(feature)
return obstacles
def __generate_feature_obstacle(self, world, vertexes):
def __generate_feature_obstacles(self, vertexes):
"""
Generate features on the boundary of a obstacle.
:param vertexes: list of vertexes of the obstacle.
:return:
"""
radius = self.cfg["obstacle"]["feature"]["radius"]
density = self.cfg["obstacle"]["feature"]["density"]
num_vertexes = len(vertexes)
......@@ -239,15 +219,19 @@ class MapManager:
x1 = vertexes[j][0]
y1 = vertexes[j][1]
obstacles += self.__generate_feature_line(x0, y0, x1, y1, radius, density)
#obstacles.pop()
return obstacles
def __generate_features(self, world, obstacles):
def __generate_features(self, obstacles):
"""
Generate features on the boundary of obstacles.
:return:
a set of obstacles.
"""
features = []
for obstacle in obstacles:
if type(obstacle) == FeaturePoint:
continue
features += self.__generate_feature_obstacle(world, obstacle.global_geometry.vertexes)
features += self.__generate_feature_obstacles(obstacle.global_geometry.vertexes)
# add identifiers for each feature.
for i, f in enumerate(features):
f.id = i
......@@ -273,7 +257,7 @@ class MapManager:
def __check_obstacle_intersections(self, goal):
"""
Check for intersections between the goal and the obstacles
:param goal: The goal posibition
:param goal: The goal position
:return: Boolean value indicating if the goal is too close to an obstacle
"""
# generate a proximity test geometry for the goal
......
import time
T_NOT_REACH_GOAL = 60 # time that robot not reaches a goal in seconds
T_NO_BEST_DIST_CHANGE = 30 # time that robot not updates the best distance to goal
T_NOT_REACH_GOAL = 60 # time that the robot dose not reaches a goal in seconds
T_NO_BEST_DIST_CHANGE = 30 # time that the robot dose not updates the best distance to goal
class Timer:
"""
......
......@@ -18,7 +18,7 @@ class AStarPlanning(PathPlanning):
def execute(self, sx, sy, gx, gy, obstacle_map, weight=1.0, type='manhattan'):
"""
A* path search
Execute A* path searching on a map.
:param sx: start x position [pix]
:param sy: start y position [pix]
:param gx: goal x position [pix]
......@@ -166,8 +166,7 @@ class AStarPlanning(PathPlanning):
else:
d = 0
d = weight * d
return d
return weight * d
if __name__ == '__main__':
......@@ -177,7 +176,7 @@ if __name__ == '__main__':
map = np.full((100, 100), False, dtype=np.bool)
for i in range(80):
map[i, 20] = True
for i in range(0, 100):
for i in range(20, 100):
map[i, 40] = True
for i in range(90):
map[i, 60] = True
......@@ -187,7 +186,7 @@ if __name__ == '__main__':
start_time = time.time()
# set start and goal
start = [5, 2] # x, y
goal = [90, 98] # x, y
goal = [90, 95] # x, y
# calculate shortest path
astar = AStarPlanning()
path = astar.execute(start[0], start[1], goal[0], goal[1], map, weight=5, type='euclidean')
......
......@@ -213,6 +213,7 @@ class OccupancyGridMapping2d(Mapping):
def __prob2log(self, p):
"""
Convert probability to log-odds ratio
:param p: probability that a grid is occupied
:return: log likelihood
"""
......@@ -220,7 +221,8 @@ class OccupancyGridMapping2d(Mapping):
def __log2prob(self, lx):
"""
:param lx: log likelihood
Convert log-odds ratio to probability
:param lx: the log-odds ratio
:return: probability that a grid is occupied
"""
return 1 - 1 / (1 + np.exp(lx))
......
class PathPlanning:
"""
An abstract class for path planning algorithm
An abstract class for path planning algorithms
"""
def execute(self, sx, sy, gx, gy, obstacle_map):
"""
......
......@@ -39,7 +39,7 @@ class GraphBasedSLAM(Slam):
self.step_counter = 0
self.graph = LMGraph()
self.flg_optim = False # determines whether the graph should be optimized
self.flg_optim = False # determines whether the graph should be optimized in a simulation cycles.
self.callback = callback # a callback function
# add the first node to the graph
......@@ -56,6 +56,9 @@ class GraphBasedSLAM(Slam):
return Pose(self.mu[0, 0], self.mu[1, 0], self.mu[2, 0])
def get_estimated_trajectory(self):
"""
Returns hte estimated trajectory of the robot.
"""
return [(v.pose[0,0], v.pose[1,0]) \
for v in self.graph.get_estimated_pose_vertices()]
......@@ -112,21 +115,21 @@ class GraphBasedSLAM(Slam):
def __front_end(self, z):
"""
Front end part of the Graph_based SLAM where a graph is built and growing as robot's moving.
Front end part of the Graph_based SLAM where a graph is constructed and growing while robot's moving.
:param z: list of range-bearing measurement, a single item is (distance, angle, landmark_id) related to robot
"""
""" calculate the next vertex of poses """
vertex1 = self.graph.get_last_pose_vertex() # a previous vertex
vertex2 = PoseVertex(self.mu, self.Sigma) # a current vertex
vertex1 = self.graph.get_last_pose_vertex() # the previous vertex
vertex2 = PoseVertex(self.mu, self.Sigma) # the current vertex
distance = (vertex1.pose[0, 0] - vertex2.pose[0, 0])**2 + (vertex1.pose[1, 0] - vertex2.pose[1, 0])**2
if distance < self.frontend_pose_density**2: # keep the vertex density not high
return
self.graph.add_vertex(vertex2)
self.graph.add_vertex(vertex2) # add a new pose-vertex in the graph
""" calculate landmark vertices """
for i, zi in enumerate(z): # zi = [x, y, id].T
lm_id = zi[2]
lm_id = zi[2] # the identifier of a detected landmark.
# Only execute if sensor has observed landmark
if not self.supervisor.proximity_sensor_positive_detections()[i] \
or lm_id == -1: # not a feature
......@@ -137,23 +140,23 @@ class GraphBasedSLAM(Slam):
if vertex3 == None:
# Detect a new landmark: this landmark has not been detected in the past
vertex3 = LandmarkVertex(pos_lm, self.sensor_noise, lm_id) # create a new vertex representing a landmark
self.graph.add_vertex(vertex3) # add the vertex to the graph
self.graph.add_vertex(vertex3) # add the landmark-vertex to the graph
else:
# 1. Known that the robot is revisiting a previous seen landmark,
# 2. Calculate the distance between the estimated landmarks via slam and via actual measurement,
# if the robot is revisiting a landmark, and the distance is larger than the threshold,
# start backend to correct the consistancy between the estimate and the measurments.
distance2 = (pos_lm[0, 0] - vertex3.pose[0, 0])**2 + (pos_lm[1, 0] - vertex3.pose[1, 0])**2
if distance2 > self.min_distance_threshold**2 and self.flg_optim == False:
# 1. The robot is revisiting a previous seen landmark,
# 2. Calculate the distance between the estimated landmark via slam and that via actual measurement,
# if the robot is revisiting a landmark, and the distance is larger than the threshold (large error),
# start backend to correct the inconsistency between the evaluation and the measurement.
sq_distance = (pos_lm[0, 0] - vertex3.pose[0, 0])**2 + (pos_lm[1, 0] - vertex3.pose[1, 0])**2
if sq_distance > self.min_distance_threshold**2 and self.flg_optim == False:
self.flg_optim = True
# Create an edge connecting a pose and a landmark.
measurement, info = PoseLandmarkEdge.encode_measurement(zi, self.sensor_noise)
measurement, info = PoseLandmarkEdge.encode_measurement(zi, self.sensor_noise) # create a spatial constraint.
self.graph.add_edge(vertex2, vertex3, measurement, info) # add an PoseLandmarkEdge edge
""" calculate pose-pose edge """
# Create an edge connecting two poses
measurement, info = PosePoseEdge.encode_measurement(vertex1.pose, vertex2.pose, self.motion_noise)
measurement, info = PosePoseEdge.encode_measurement(vertex1.pose, vertex2.pose, self.motion_noise) # create a spatial constraint.
self.graph.add_edge(vertex1, vertex2, measurement, info)
......
"""
LS-Slam
Based on implementation of dnovischi (https://github.com/aimas-upb/slam-course-solutions)
In this class, Gauss-Newton minimization algorithm is implemented.
Based on implementation by dnovischi (https://github.com/aimas-upb/slam-course-solutions)
The parent class of any types of graph
The Graph class has to be inherited and the following methods has to be implemented,
......@@ -83,7 +84,7 @@ class Graph:
damp_factor = 0.01, max_iter = 10,
solver = "spsolve", callback = None, epsilon = 1e-2):
"""
Optimization of the graph
Optimization of the graph with Gauss-Newton minimization algorithm
:param number_fix: fix the estimation of the initial step
:param damp_factor: how much you want to fix a vertex.
:param max_iter: the maximum number of iterations
......@@ -154,7 +155,7 @@ class Graph:
def linearize_constraints(self, vertices, edges, number_fix, damp_factor):
"""
Linearize the problem (global) i.e. compute the hessian matrix and the information vector
Linearize the error (global) i.e. compute the hessian matrix and the information vector
:return:
H: the hessian matrix (information matrix)
b: information vector
......@@ -167,7 +168,7 @@ class Graph:
""" [ISSUE] This for-loop for calculating H and b cost time! """
for edge in edges:
err, A, B = edge.linearize() # calculate error and obtain the Jacobian matrices A and B
err, A, B = edge.linearize() # calculate error and obtain the Jacobian matrices A and B of the error, w.r.t. xi and xj
omega = edge.information # the information matrix
""" Compute the block matrices and vectors """
bi = (err.T @ omega @ A).T
......
......@@ -7,7 +7,7 @@ from supervisor.slam.graph.baseclass.Graph import Graph
from supervisor.slam.graph.vetor2matrix import *
""" Define Vertex Classes
Vertex
Extended from the base-classs Vertex
- PoseVertex
- LandmarkVertex
"""
......@@ -40,13 +40,12 @@ class LandmarkVertex(Vertex):
assert sigma.shape == (2, 2)
self.landmark_id = landmark_id
def __str__(self):
x, y = np.squeeze(self.pose)
return "Landmark[id = {0}, pose = {1}]".format(self.id, (x, y))
""" Define Edge Classes
Edge
Extended from the base-classs Edge
- PosePoseEdge
- PoseLandmarkEdge
......@@ -62,18 +61,15 @@ class PosePoseEdge(Edge):
:param z: actual measurement obtained by sensor
:param information: information matrix
"""
Edge.__init__(self, vertex1, vertex2, z, information)
assert isinstance(self.vertex1, PoseVertex)
assert isinstance(self.vertex2, PoseVertex)
assert z.shape == (3, 1)
assert information.shape == (3, 3)
def calc_error_vector(self, x1, x2, z):
"""
Calculate the error vector.
:param x1: 3x1 vector of previous state.
:param x2: 3x1 vector of current state.
:param z: 3x1 vector of measurement from x to m.
......@@ -107,8 +103,7 @@ class PosePoseEdge(Edge):
def linearize_constraint(self, x1, x2, z):
"""
Linearize the pose-pose constraint.
Linearize the pose-pose constraint of edge.
:param x1: 3x1 vector of previous pose.
:param x2: 3x1 vector of current pose.
:param z: 3x1 vector of measurement from x to m.
......@@ -131,7 +126,7 @@ class PosePoseEdge(Edge):
def __str__(self):
x, y, yaw = np.squeeze(self.z)
s = "PPE: id1:{0},id2: {1},z: [{2}, {3}, {4}]".format(self.id1, self.id2, x, y, yaw)
s = "PPE: id1:{0},id2: {1},z: [{2}, {3}, {4}]".format(self.vertex1.id, self.vertex2.id, x, y, yaw)
return s
class PoseLandmarkEdge(Edge):
......@@ -202,12 +197,12 @@ class PoseLandmarkEdge(Edge):
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)
s = "PLE: id1:{0},id2: {1},z: [{2}, {3}]".format(self.vertex1.id, self.vertex2.id, x, y)
return s
""" Define Graph Class
Graph
Extended from the base-classs Graph
-LMGraph
"""
......@@ -215,8 +210,8 @@ class LMGraph(Graph):
def __init__(self):
"""
Posegraph to estimate only robot's poses
:param vertices: list of vertices. a single vertex is a pose of robot