import re
import numpy as np
from spatialmath import base
from spatialmath.base.types import *
# this is a collection of useful algorithms, not otherwise categorized
[docs]def numjac(
f: Callable,
x: ArrayLike,
dx: float = 1e-8,
SO: int = 0,
SE: int = 0,
) -> NDArray:
r"""
Numerically compute Jacobian of function
:param f: the function, returns an m-vector
:type f: callable
:param x: function argument
:type x: ndarray(n)
:param dx: the numerical perturbation, defaults to 1e-8
:type dx: float, optional
:param SO: function returns SO(N) matrix, defaults to 0
:type SO: int, optional
:param SE: function returns SE(N) matrix, defaults to 0
:type SE: int, optional
:return: Jacobian matrix
:rtype: ndarray(m,n)
Computes a numerical approximation to the Jacobian for ``f(x)`` where
:math:`f: \mathbb{R}^n \mapsto \mathbb{R}^m`.
Uses first-order difference :math:`J[:,i] = (f(x + dx) - f(x)) / dx`.
If ``SO`` is 2 or 3, then it is assumed that the function returns
an SO(N) matrix and the derivative is converted to a column vector
.. math::
\vex{\dmat{R} \mat{R}^T}
If ``SE`` is 2 or 3, then it is assumed that the function returns
an SE(N) matrix and the derivative is converted to a colun vector.
Example:
.. runblock:: pycon
>>> from spatialmath.base import rotx, numjac
>>> numjac(rotx, [0])
>>> numjac(rotx, [0], SO=3)
"""
x = np.array(x)
Jcol = []
J0 = f(x)
I = np.eye(len(x))
f0 = np.array(f(x))
for i in range(len(x)):
fi = np.array(f(x + I[:, i] * dx))
Ji = (fi - f0) / dx
if SE > 0:
t = Ji[:SE, SE]
r = base.vex(Ji[:SE, :SE] @ J0[:SE, :SE].T)
Jcol.append(np.r_[t, r])
elif SO > 0:
R = Ji[:SO, :SO]
r = base.vex(R @ J0[:SO, :SO].T)
Jcol.append(r)
else:
Jcol.append(Ji)
# print(Ji)
return np.c_[Jcol].T
[docs]def numhess(J: Callable, x: NDArray, dx: float = 1e-8):
r"""
Numerically compute Hessian given Jacobian function
:param J: the Jacobian function, returns an ndarray(m,n)
:type J: callable
:param x: function argument
:type x: ndarray(n)
:param dx: the numerical perturbation, defaults to 1e-8
:type dx: float, optional
:return: Hessian matrix
:rtype: ndarray(m,n,n)
Computes a numerical approximation to the Hessian for ``J(x)`` where
:math:`f: \mathbb{R}^n \mapsto \mathbb{R}^{m \times n}`.
The result is a 3D array where
.. math::
H_{i,j,k} = \frac{\partial J_{j,k}}{\partial x_i}
Uses first-order difference :math:`H[:,:,i] = (J(x + dx) - J(x)) / dx`.
"""
I = np.eye(len(x))
Hcol = []
J0 = J(x)
for i in range(len(x)):
Ji = J(x + I[:, i] * dx)
Hi = (Ji - J0) / dx
Hcol.append(Hi)
return np.stack(Hcol, axis=0)
[docs]def array2str(
X: NDArray,
valuesep: str = ", ",
rowsep: str = " | ",
fmt: str = "{:.3g}",
brackets: Tuple[str, str] = ("[ ", " ]"),
suppress_small: bool = True,
) -> str:
"""
Convert array to single line string
:param X: 1D or 2D array to convert
:type X: ndarray(N,M), array_like(N)
:param valuesep: separator between numbers, defaults to ", "
:type valuesep: str, optional
:param rowsep: separator between rows, defaults to " | "
:type rowsep: str, optional
:param format: format string, defaults to "{:.3g}"
:type precision: str, optional
:param brackets: strings to be added to start and end of the string,
defaults to ("[ ", " ]"). Set to None to suppress brackets.
:type brackets: list, tuple of str
:param suppress_small: small values (:math:`|x| < 10^{-12}` are converted
to zero, defaults to True
:type suppress_small: bool, optional
:return: compact string representation of array
:rtype: str
Converts a small array to a compact single line representation.
Example:
.. runblock:: pycon
>>> from spatialmath.base import array2str
>>> import numpy as np
>>> array2str(np.random.rand(2,2))
>>> array2str(np.random.rand(2,2), rowsep="; ") # MATLAB-like
>>> array2str(np.random.rand(3,))
>>> array2str(np.random.rand(3,1))
:seealso: :func:`array2str`
"""
# convert to ndarray if not already
if isinstance(X, (list, tuple)):
X = base.getvector(X)
def format_row(x):
s = ""
for j, e in enumerate(x):
if abs(e) < 1e-12:
e = 0
if j > 0:
s += valuesep
s += fmt.format(e)
return s
if X.ndim == 1:
# 1D case
s = format_row(X)
else:
# 2D case
s = ""
for i, row in enumerate(X):
if i > 0:
s += rowsep
s += format_row(row)
if brackets is not None and len(brackets) == 2:
s = brackets[0] + s + brackets[1]
return s
[docs]def str2array(s: str) -> NDArray:
"""
Convert compact single line string to array
:param s: string to convert
:type s: str
:return: array
:rtype: ndarray
Convert a string containing a "MATLAB-like" matrix definition to a NumPy
array. A scalar has no delimiting square brackets and becomes a 1x1 array.
A 2D array is delimited by square brackets, elements are separated by a comma,
and rows are separated by a semicolon. Extra white spaces are ignored.
Example:
.. runblock:: pycon
>>> from spatialmath.base import str2array
>>> str2array("5")
>>> str2array("[1 2 3]")
>>> str2array("[1 2; 3 4]")
>>> str2array(" [ 1 , 2 ; 3 4 ] ")
>>> str2array("[1; 2; 3]")
:seealso: :func:`array2str`
"""
s = s.lstrip(" [")
s = s.rstrip(" ]")
values = []
for row in s.split(";"):
values.append([float(x) for x in re.split("[, ]+", row.strip())])
return np.array(values)
[docs]def bresenham(p0: ArrayLike2, p1: ArrayLike2) -> Tuple[NDArray, NDArray]:
"""
Line drawing in a grid
:param p0: initial point
:type p0: array_like(2) of int
:param p1: end point
:type p1: array_like(2) of int
:return: arrays of x and y coordinates for points along the line
:rtype: ndarray(N), ndarray(N) of int
Return x and y coordinate vectors for points in a grid that lie on
a line from ``p0`` to ``p1`` inclusive.
* The end points, and all points along the line are integers.
* Points are always adjacent, but the slope from point to point is not constant.
Example:
.. runblock:: pycon
>>> from spatialmath.base import bresenham
>>> bresenham((2, 4), (10, 10))
.. plot::
from spatialmath.base import bresenham
import matplotlib.pyplot as plt
p = bresenham((2, 4), (10, 10))
plt.plot((2, 10), (4, 10))
plt.plot(p[0], p[1], 'ok')
plt.plot(p[0], p[1], 'k', drawstyle='steps-post')
ax = plt.gca()
ax.grid()
.. note:: The API is similar to the Bresenham algorithm but this
implementation uses NumPy vectorised arithmetic which makes it
faster than the Bresenham algorithm in Python.
"""
x0, y0 = p0
x1, y1 = p1
dx = x1 - x0
dy = y1 - y0
if abs(dx) >= abs(dy):
# shallow line -45° <= θ <= 45°
# y = mx + c
if dx == 0:
# case p0 == p1
x = np.r_[x0]
y = np.r_[y0]
else:
m = dy / dx
c = y0 - m * x0
if dx > 0:
# line to the right
x = np.arange(x0, x1 + 1)
elif dx < 0:
# line to the left
x = np.arange(x0, x1 - 1, -1)
y = np.round(x * m + c)
else:
# steep line θ < -45°, θ > 45°
# x = my + c
m = dx / dy
c = x0 - m * y0
if dy > 0:
# line to the right
y = np.arange(y0, y1 + 1)
elif dy < 0:
# line to the left
y = np.arange(y0, y1 - 1, -1)
x = np.round(y * m + c)
return x.astype(int), y.astype(int)
[docs]def mpq_point(data: Points2, p: int, q: int) -> float:
r"""
Moments of polygon
:param data: polygon vertices, points as columns
:type data: ndarray(2,N)
:param p: moment order x
:type p: int
:param q: moment order y
:type q: int
Returns the pq'th moment of the polygon
.. math::
M(p, q) = \sum_{i=0}^{n-1} x_i^p y_i^q
Example:
.. runblock:: pycon
>>> from spatialmath.base import mpq_point
>>> import numpy as np
>>> p = np.array([[1, 3, 2], [2, 2, 4]])
>>> mpq_point(p, 0, 0) # area
>>> mpq_point(p, 3, 0)
.. note:: is negative for clockwise perimeter.
"""
x = data[0, :]
y = data[1, :]
return np.sum(x**p * y**q)
[docs]def gauss1d(mu: float, var: float, x: ArrayLike):
"""
Gaussian function in 1D
:param mu: mean
:type mu: float
:param var: variance
:type var: float
:param x: x-coordinate values
:type x: array_like(n)
:return: Gaussian :math:`G(x)`
:rtype: ndarray(n)
Example::
>>> g = gauss1d(5, 2, np.linspace(0, 10, 100))
.. plot::
from spatialmath.base import gauss1d
import matplotlib.pyplot as plt
import numpy as np
x = np.linspace(0, 10, 100)
g = gauss1d(5, 2, x)
plt.plot(x, g)
plt.grid()
:seealso: :func:`gauss2d`
"""
sigma = np.sqrt(var)
x = base.getvector(x)
return (
1.0
/ np.sqrt(sigma**2 * 2 * np.pi)
* np.exp(-((x - mu) ** 2) / 2 / sigma**2)
)
[docs]def gauss2d(mu: ArrayLike2, P: NDArray, X: NDArray, Y: NDArray) -> NDArray:
"""
Gaussian function in 2D
:param mu: mean
:type mu: array_like(2)
:param P: covariance matrix
:type P: ndarray(2,2)
:param X: array of x-coordinates
:type X: ndarray(n,m)
:param Y: array of y-coordinates
:type Y: ndarray(n,m)
:return: Gaussian :math:`g(x,y)`
:rtype: ndarray(n,m)
Computed :math:`g_{i,j} = G(x_{i,j}, y_{i,j})`
Example (RVC3 Fig G.2)::
>>> a = np.linspace(-5, 5, 100)
>>> X, Y = np.meshgrid(a, a)
>>> P = np.diag([1, 2])**2;
>>> g = gauss2d(X, Y, [0, 0], P)
.. plot::
from spatialmath.base import gauss2d, plotvol3
import matplotlib.pyplot as plt
import numpy as np
a = np.linspace(-5, 5, 100)
x, y = np.meshgrid(a, a)
P = np.diag([1, 2])**2;
g = gauss2d([0, 0], P, x, y)
ax = plotvol3()
ax.plot_surface(x, y, g)
:seealso: :func:`gauss1d`
"""
x = X.ravel() - mu[0]
y = Y.ravel() - mu[1]
Pi = np.linalg.inv(P)
g = (
1
/ (2 * np.pi * np.sqrt(np.linalg.det(P)))
* np.exp(-0.5 * (x**2 * Pi[0, 0] + y**2 * Pi[1, 1] + 2 * x * y * Pi[0, 1]))
)
return g.reshape(X.shape)
if __name__ == "__main__":
r = np.linspace(-4, 4, 6)
x, y = np.meshgrid(r, r)
print(gauss2d([0, 0], np.diag([1, 2]), x, y))
# print(bresenham([2,2], [2,4]))
# print(bresenham([2,2], [2,-4]))
# print(bresenham([2,2], [4,2]))
# print(bresenham([2,2], [-4,2]))
# print(bresenham([2,2], [2,2]))
# print(bresenham([2,2], [3,6])) # steep
# print(bresenham([2,2], [6,3])) # shallow
# print(bresenham([2,2], [3,6])) # steep
# print(bresenham([2,2], [6,3])) # shallow