from abc import ABC
import numpy as np
import scipy as sp
from math import pi, sin, cos
import matplotlib.pyplot as plt
from spatialmath import base
import roboticstoolbox as rtb
[docs]class LandmarkMap:
"""
Map of planar point landmarks
:param map: map or number of landmarks
:type map: ndarray(2, N) or int
:param workspace: workspace or map bounds, defaults to 10
:type workspace: scalar, array_like(2), array_like(4), optional
:param verbose: display debug information, defaults to True
:type verbose: bool, optional
:param seed: random number seed, defaults to 0
:type seed: int, optional
:return: a landmark map object
:rtype: LandmarkMap
A LandmarkMap object represents a rectangular 2D environment with a number
of point landmarks.
The landmarks can be specified explicitly or be uniform randomly positioned
inside a region defined by the workspace. The workspace can be numeric:
============== ======= =======
``workspace`` x-range y-range
============== ======= =======
A (scalar) -A:A -A:A
[A, B] A:B A:B
[A, B, C, D] A:B C:D
============== ======= =======
or any object that has a ``workspace`` attribute.
Example:
.. runblock:: pycon
>>> from roboticstoolbox import LandmarkMap
>>> map = LandmarkMap(20)
>>> print(map)
>>> print(map[3]) # coordinates of landmark 3
The object is an iterator that returns consecutive landmark coordinates.
:Reference:
Robotics, Vision & Control, Chap 6,
Peter Corke,
Springer 2011
See also :class:`~roboticstoolbox.mobile.sensors.RangeBearingSensor` :class:`~roboticstoolbox.mobile.EKF`
"""
[docs] def __init__(self, map, workspace=10, verbose=True, seed=0):
try:
self._workspace = workspace.workspace
except:
self._workspace = base.expand_dims(workspace)
if base.ismatrix(map, (2, None)):
self._map = map
self._nlandmarks = map.shape[1]
elif isinstance(map, int):
self._nlandmarks = map
random = np.random.default_rng(seed)
x = random.uniform(self._workspace[0], self._workspace[1], self._nlandmarks)
y = random.uniform(self._workspace[2], self._workspace[3], self._nlandmarks)
self._map = np.c_[x, y].T
else:
raise ValueError("bad type for map")
self._verbose = verbose
def __str__(self):
# s = M.char() is a string showing map parameters in
# a compact human readable format.
ws = self._workspace
s = f"LandmarkMap object with {self._nlandmarks} landmarks, workspace="
s += f"({ws[0]}: {ws[1]}, {ws[2]}: {ws[3]})"
return s
def __repr__(self):
return str(self)
[docs] def __len__(self):
"""
Number of landmarks in map
:return: number of landmarks in the map
:rtype: int
"""
return self._nlandmarks
@property
def landmarks(self):
"""
xy-coordinates of all landmarks
:return: xy-coordinates for landmark points
:rtype: ndarray(2, n)
"""
return self._map
@property
def workspace(self):
"""
Size of map workspace
:return: workspace bounds [xmin, xmax, ymin, ymax]
:rtype: ndarray(4)
Returns the bounds of the workspace as specified by constructor
option ``workspace``.
"""
return self._workspace
[docs] def __getitem__(self, k):
"""
Get landmark coordinates from map
:param k: landmark index
:type k: int
:return: coordinate :math:`(x,y)` of k'th landmark
:rtype: ndarray(2)
"""
return self._map[:, k]
[docs] def plot(self, labels=False, block=None, **kwargs):
"""
Plot landmark map
:param labels: number the points on the plot, defaults to False
:type labels: bool, optional
:param block: block until figure is closed, defaults to False
:type block: bool, optional
:param kwargs: :meth:`~matplotlib.axes.Axes.plot` options
Plot landmark points using Matplotlib options. Default style is black
crosses.
"""
ax = base.plotvol2(self._workspace)
ax.set_aspect("equal")
ax.set_xlabel("x")
ax.set_ylabel("y")
if len(kwargs) == 0:
kwargs = {
"linewidth": 0,
"marker": "x",
"color": "black",
"linestyle": "None",
}
if "label" not in kwargs:
kwargs["label"] = "landmark"
# plt.plot(self._map[0,:], self._map[1,:], , **kwargs)
if labels:
labels = "#{}"
else:
labels = None
base.plot_point(self._map, text=labels, **kwargs)
plt.grid(True)
if block is not None:
plt.show(block=block)
if __name__ == "__main__":
import unittest