def project(self, P, pose=None, objpose=None):
"""
Central projection for now
P world points or image plane points in column vectors only
"""
P = getmatrix(P, (3,None))
if P.shape[0] == 3:
# for 3D world points
if pose is None:
C = self.C
else:
C = self.getC(SE3(pose))
ip = h2e(C @ e2h(P))
elif P.shape[0] == 2:
# for 2D imageplane points
ip = P
return ip
def step(self):
# inputs are set
if self.bd.options.graphics:
self.xdata.append(self.inputs[0])
self.ydata.append(self.inputs[1])
#plt.figure(self.fig.number)
if self.path:
self.line.set_data(self.xdata, self.ydata)
T = base.transl2(self.inputs[0], self.inputs[1]) @ base.trot2(
self.inputs[2])
new = base.h2e(T @ self.vertices_hom)
self.vehicle.set_xy(new.T)
if self.bd.options.animation:
self.fig.canvas.flush_events()
if self.scale == 'auto':
self.ax.relim()
self.ax.autoscale_view()
super().step()
def step(self):
# inputs are set
if self.sim.graphics:
self.xdata.append(self.inputs[0])
self.ydata.append(self.inputs[1])
plt.figure(self.fig.number)
if self.path:
self.line.set_data(self.xdata, self.ydata)
T = sm.transl2(self.inputs[0], self.inputs[1]) @ sm.trot2(
self.inputs[2])
new = sm.h2e(T @ self.vertices_hom)
self.vehicle.set_xy(new.T)
plt.draw()
plt.show(block=False)
if self.sim.animation:
self.fig.canvas.start_event_loop(0.001)
if self.scale == 'auto':
self.ax.relim()
self.ax.autoscale_view()
def __mul__(left, right):
"""
Overloaded ``*`` operator (superclass method)
:arg left: left multiplicand
:arg right: right multiplicand
:return: product
:raises: ValueError
Pose composition, scaling or vector transformation:
- ``X * Y`` compounds the poses ``X`` and ``Y``
- ``X * s`` performs elementwise multiplication of the elements of ``X`` by ``s``
- ``s * X`` performs elementwise multiplication of the elements of ``X`` by ``s``
- ``X * v`` linear transform of the vector ``v``
============== ============== =========== ======================
Multiplicands Product
------------------------------- -----------------------------------
left right type operation
============== ============== =========== ======================
Pose Pose Pose matrix product
Pose scalar NxN matrix element-wise product
scalar Pose NxN matrix element-wise product
Pose N-vector N-vector vector transform
Pose NxM matrix NxM matrix transform each column
============== ============== =========== ======================
Notes:
#. Pose is ``SO2``, ``SE2``, ``SO3`` or ``SE3`` instance
#. N is 2 for ``SO2``, ``SE2``; 3 for ``SO3`` or ``SE3``
#. scalar x Pose is handled by ``__rmul__``
#. scalar multiplication is commutative but the result is not a group
operation so the result will be a matrix
#. Any other input combinations result in a ValueError.
For pose composition the ``left`` and ``right`` operands may be a sequence
========= ========== ==== ================================
len(left) len(right) len operation
========= ========== ==== ================================
1 1 1 ``prod = left * right``
1 M M ``prod[i] = left * right[i]``
N 1 M ``prod[i] = left[i] * right``
M M M ``prod[i] = left[i] * right[i]``
========= ========== ==== ================================
For vector transformation there are three cases
========= =========== ===== ==========================
Multiplicands Product
---------------------- ---------------------------------
len(left) right.shape shape operation
========= =========== ===== ==========================
1 (N,) (N,) vector transformation
M (N,) (N,M) vector transformations
1 (N,M) (N,M) column transformation
========= =========== ===== ==========================
Notes:
#. for the ``SE2`` and ``SE3`` case the vectors are converted to homogeneous
form, transformed, then converted back to Euclidean form.
"""
if isinstance(left, right.__class__):
#print('*: pose x pose')
return left.__class__(left._op2(right, lambda x, y: x @ y),
check=False)
elif isinstance(right, (list, tuple, np.ndarray)):
#print('*: pose x array')
if len(left) == 1 and argcheck.isvector(right, left.N):
# pose x vector
#print('*: pose x vector')
v = argcheck.getvector(right, out='col')
if left.isSE:
# SE(n) x vector
return tr.h2e(left.A @ tr.e2h(v))
else:
# SO(n) x vector
return left.A @ v
elif len(left) > 1 and argcheck.isvector(right, left.N):
# pose array x vector
#print('*: pose array x vector')
v = argcheck.getvector(right)
if left.isSE:
# SE(n) x vector
v = tr.e2h(v)
return np.array([tr.h2e(x @ v).flatten()
for x in left.A]).T
else:
# SO(n) x vector
return np.array([(x @ v).flatten() for x in left.A]).T
elif len(left) == 1 and isinstance(
right,
np.ndarray) and left.isSO and right.shape[0] == left.N:
# SO(n) x matrix
return left.A @ right
elif len(left) == 1 and isinstance(
right,
np.ndarray) and left.isSE and right.shape[0] == left.N:
# SE(n) x matrix
return tr.h2e(left.A @ tr.e2h(right))
elif isinstance(right, np.ndarray) and left.isSO and right.shape[
0] == left.N and len(left) == right.shape[1]:
# SO(n) x matrix
return np.c_[[x.A @ y for x, y in zip(right, left.T)]].T
elif isinstance(right, np.ndarray) and left.isSE and right.shape[
0] == left.N and len(left) == right.shape[1]:
# SE(n) x matrix
return np.c_[[
tr.h2e(x.A @ tr.e2h(y)) for x, y in zip(right, left.T)
]].T
else:
raise ValueError('bad operands')
elif argcheck.isscalar(right):
return left._op2(right, lambda x, y: x * y)
else:
return NotImplemented
def __mul__(left, right):
"""
Pose multiplication
:arg left: left multiplicand
:arg right: right multiplicand
:return: product
:raises: ValueError
- ``X * Y`` compounds the poses X and Y
- ``X * s`` performs elementwise multiplication of the elements of ``X``
- ``s * X`` performs elementwise multiplication of the elements of ``X``
- ``X * v`` transforms the vector.
============== ============== =========== ===================
Multiplicands Product
------------------------------- --------------------------------
left right type result
============== ============== =========== ===================
Pose Pose Pose matrix product
Pose scalar matrix elementwise product
scalar Pose matrix elementwise product
Pose N-vector N-vector vector transform
Pose NxM matrix NxM matrix vector transform
============== ============== =========== ===================
Any other input combinations result in a ValueError.
Note that left and right can have a length greater than 1 in which case
==== ===== ==== ================================
left right len operation
==== ===== ==== ================================
1 1 1 ``prod = left * right``
1 M M ``prod[i] = left * right[i]``
N 1 M ``prod[i] = left[i] * right``
M M M ``prod[i] = left[i] * right[i]``
==== ===== ==== ================================
A scalar of length M is list, tuple or numpy array.
An N-vector of length M is a NxM numpy array, where each column is an N-vector.
"""
if isinstance(left, right.__class__):
#print('*: pose x pose')
return left.__class__(left._op2(right, lambda x, y: x @ y))
elif isinstance(right, (list, tuple, np.ndarray)):
#print('*: pose x array')
if len(left) == 1 and argcheck.isvector(right, left.N):
# pose x vector
#print('*: pose x vector')
v = argcheck.getvector(right, out='col')
if left.isSE:
# SE(n) x vector
return tr.h2e(left.A @ tr.e2h(v))
else:
# SO(n) x vector
return left.A @ v
elif len(left) > 1 and argcheck.isvector(right, left.N):
# pose array x vector
#print('*: pose array x vector')
v = argcheck.getvector(right)
if left.isSE:
# SE(n) x vector
v = tr.e2h(v)
return np.array([tr.h2e(x @ v).flatten()
for x in left.A]).T
else:
# SO(n) x vector
return np.array([(x @ v).flatten() for x in left.A]).T
elif len(left) == 1 and isinstance(
right,
np.ndarray) and left.isSO and right.shape[0] == left.N:
# SO(n) x matrix
return left.A @ right
elif len(left) == 1 and isinstance(
right,
np.ndarray) and left.isSE and right.shape[0] == left.N:
# SE(n) x matrix
return tr.h2e(left.A @ tr.e2h(right))
else:
raise ValueError('bad operands')
elif isinstance(right, (int, float)):
return left._op2(right, lambda x, y: x * y)
else:
raise ValueError('bad operands')
def __mul__(left, right): # pylint: disable=no-self-argument
"""
Overloaded ``*`` operator (superclass method)
:return: Product of two operands
:rtype: pose instance
:raises NotImplemented: for incompatible arguments
Pose composition, scaling or vector transformation:
- ``X * Y`` compounds the poses ``X`` and ``Y``
- ``X * s`` performs element-wise multiplication of the elements of ``X`` by ``s``
- ``s * X`` performs element-wise multiplication of the elements of ``X`` by ``s``
- ``X * v`` linear transformation of the vector ``v`` where ``v`` is array-like
============== ============== =========== ======================
Multiplicands Product
------------------------------- -----------------------------------
left right type operation
============== ============== =========== ======================
Pose Pose Pose matrix product
Pose scalar NxN matrix element-wise product
scalar Pose NxN matrix element-wise product
Pose N-vector N-vector vector transform
Pose NxM matrix NxM matrix transform each column
============== ============== =========== ======================
.. note::
#. Pose is an ``SO2``, ``SE2``, ``SO3`` or ``SE3`` instance
#. N is 2 for ``SO2``, ``SE2``; 3 for ``SO3`` or ``SE3``
#. Scalar x Pose is handled by __rmul__`
#. Scalar multiplication is commutative but the result is not a group
operation so the result will be a matrix
#. Any other input combinations result in a ValueError.
For pose composition either or both operands may hold more than one value which
results in the composition holding more than one value according to:
========= ========== ==== ================================
len(left) len(right) len operation
========= ========== ==== ================================
1 1 1 ``prod = left * right``
1 M M ``prod[i] = left * right[i]``
N 1 M ``prod[i] = left[i] * right``
M M M ``prod[i] = left[i] * right[i]``
========= ========== ==== ================================
Example::
>>> SE3.Rx(pi/2) * SE3.Ry(pi/2)
SE3(array([[0., 0., 1., 0.],
[1., 0., 0., 0.],
[0., 1., 0., 0.],
[0., 0., 0., 1.]]))
>>> SE3.Rx(pi/2) * 2
array([[ 2.0000000e+00, 0.0000000e+00, 0.0000000e+00, 0.0000000e+00],
[ 0.0000000e+00, 1.2246468e-16, -2.0000000e+00, 0.0000000e+00],
[ 0.0000000e+00, 2.0000000e+00, 1.2246468e-16, 0.0000000e+00],
[ 0.0000000e+00, 0.0000000e+00, 0.0000000e+00, 2.0000000e+00]])
For vector transformation there are three cases:
========= =========== ===== ==========================
Multiplicands Product
---------------------- ---------------------------------
len(left) right.shape shape operation
========= =========== ===== ==========================
1 (N,) (N,) vector transformation
M (N,) (N,M) vector transformations
1 (N,M) (N,M) column transformation
========= =========== ===== ==========================
.. note:: For the ``SE2`` and ``SE3`` case the vectors are converted to homogeneous
form, transformed, then converted back to Euclidean form.
Example::
>>> SE3.Rx(pi/2) * [0, 1, 0]
array([0.000000e+00, 6.123234e-17, 1.000000e+00])
>>> SE3.Rx(pi/2) * np.r_[0, 0, 1]
array([ 0.000000e+00, -1.000000e+00, 6.123234e-17])
"""
if isinstance(left, right.__class__):
#print('*: pose x pose')
return left.__class__(left._op2(right, lambda x, y: x @ y),
check=False)
elif isinstance(right, (list, tuple, np.ndarray)):
#print('*: pose x array')
if len(left) == 1 and argcheck.isvector(right, left.N):
# pose x vector
#print('*: pose x vector')
v = argcheck.getvector(right, out='col')
if left.isSE:
# SE(n) x vector
return tr.h2e(left.A @ tr.e2h(v))
else:
# SO(n) x vector
return left.A @ v
elif len(left) > 1 and argcheck.isvector(right, left.N):
# pose array x vector
#print('*: pose array x vector')
v = argcheck.getvector(right)
if left.isSE:
# SE(n) x vector
v = tr.e2h(v)
return np.array([tr.h2e(x @ v).flatten()
for x in left.A]).T
else:
# SO(n) x vector
return np.array([(x @ v).flatten() for x in left.A]).T
elif len(left) == 1 and isinstance(
right,
np.ndarray) and left.isSO and right.shape[0] == left.N:
# SO(n) x matrix
return left.A @ right
elif len(left) == 1 and isinstance(
right,
np.ndarray) and left.isSE and right.shape[0] == left.N:
# SE(n) x matrix
return tr.h2e(left.A @ tr.e2h(right))
elif isinstance(right, np.ndarray) and left.isSO and right.shape[
0] == left.N and len(left) == right.shape[1]:
# SO(n) x matrix
return np.c_[[x.A @ y for x, y in zip(right, left.T)]].T
elif isinstance(right, np.ndarray) and left.isSE and right.shape[
0] == left.N and len(left) == right.shape[1]:
# SE(n) x matrix
return np.c_[[
tr.h2e(x.A @ tr.e2h(y)) for x, y in zip(right, left.T)
]].T
else:
raise ValueError('bad operands')
elif argcheck.isscalar(right):
return left._op2(right, lambda x, y: x * y)
else:
return NotImplemented
def project(self, P, pose=None, objpose=None, visibility=False):
"""
Project world points to image plane
:param P: 3D points to project into camera image plane
:type P: array_like(3), array_like(3,n)
:param pose: camera pose with respect to the world frame, defaults to
camera's ``pose`` attribute
:type pose: SE3, optional
:param objpose: 3D point reference frame, defaults to world frame
:type objpose: SE3, optional
:param visibility: test if points are visible, default False
:type visibility: bool
:raises ValueError: [description]
:return: image plane points
:rtype: ndarray(2,n)
If ``pose`` is specified it is used for the camera pose instead of the
attribute ``pose``. The objects attribute is not updated.
The points ``P`` are by default with respect to the world frame, but
they can be transformed
If points are behind the camera, the image plane points are set to
NaN.
if ``visibility`` is True then check whether the projected point lies in
the bounds of the image plane. Return two values: the image plane
coordinates and an array of booleans indicating if the corresponding
point is visible.
If ``P`` is a Plucker object, then each value is projected into a
2D line in homogeneous form :math:`p[0] u + p[1] v + p[2] = 0`.
"""
P = base.getmatrix(P, (3,None))
if pose is None:
pose = self.pose
C = self.getC(pose)
if isinstance(P, np.ndarray):
# project 3D points
if objpose is not None:
P = objpose * P
x = C @ base.e2h(P)
x[2,x[2,:]<0] = np.nan # points behind the camera are set to NaN
x = base.h2e(x)
# if self._distortion is not None:
# x = self.distort(x)
# if self._noise is not None:
# # add Gaussian noise with specified standard deviation
# x += np.diag(self._noise) * np.random.randn(x.shape)
# do visibility check if required
if visibility:
visible = ~np.isnan(x[0,:]) \
& (x[0,:] >= 0) \
& (x[1,:] >= 0) \
& (x[0,:] < self.nu) \
& (x[1,:] < self.nv)
return x, visibility
else:
return x
elif isinstance(P, Plucker):
# project Plucker lines
x = np.empty(shape=(3, 0))
for p in P:
l = base.vex( C * p.skew * C.T)
x = np.c_[x, l / np.max(np.abs(l))] # normalize by largest element
return x