Source code for spatialmath.base.numeric

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