Repository navigation
Expand file tree
/
Copy pathvertex.py
More file actions
67 lines (53 loc) · 1.68 KB
/
Copy pathvertex.py
File metadata and controls
67 lines (53 loc) · 1.68 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
# Copyright (c) 2020 Jeff Irion and contributors
#
# This file originated from the `graphslam` package:
#
# https://github.com/JeffLIrion/python-graphslam
"""A ``Vertex`` class.
"""
import matplotlib.pyplot as plt
# pylint: disable=too-few-public-methods
class Vertex:
"""A class for representing a vertex in Graph SLAM.
Parameters
----------
vertex_id : int
The vertex's unique ID
pose : graphslam.pose.se2.PoseSE2
The pose associated with the vertex
vertex_index : int, None
The vertex's index in the graph's ``vertices`` list
Attributes
----------
id : int
The vertex's unique ID
index : int, None
The vertex's index in the graph's ``vertices`` list
pose : graphslam.pose.se2.PoseSE2
The pose associated with the vertex
"""
def __init__(self, vertex_id, pose, vertex_index=None):
self.id = vertex_id
self.pose = pose
self.index = vertex_index
def to_g2o(self):
"""Export the vertex to the .g2o format.
Returns
-------
str
The vertex in .g2o format
"""
return "VERTEX_SE2 {} {} {} {}\n".format(self.id, self.pose[0], self.pose[1], self.pose[2])
def plot(self, color='r', marker='o', markersize=3):
"""Plot the vertex.
Parameters
----------
color : str
The color that will be used to plot the vertex
marker : str
The marker that will be used to plot the vertex
markersize : int
The size of the plotted vertex
"""
x, y = self.pose.position
plt.plot(x, y, color=color, marker=marker, markersize=markersize)