11import numpy as np
22from spatialmath import base
3+ from functools import reduce
34
5+ # this is a collection of useful algorithms, not otherwise categorized
46
57def numjac (f , x , dx = 1e-8 , SO = 0 , SE = 0 ):
68 r"""
@@ -207,14 +209,96 @@ def bresenham(p0, p1, array=None):
207209
208210 return x .astype (int ), y .astype (int )
209211
212+ def mpq_point (data , p , q ):
213+ r"""
214+ Moments of polygon
215+
216+ :param p: moment order x
217+ :type p: int
218+ :param q: moment order y
219+ :type q: int
220+
221+ Returns the pq'th moment of the polygon
222+
223+ .. math::
224+
225+ M(p, q) = \sum_{i=0}^{n-1} x_i^p y_i^q
226+
227+ Example:
228+
229+ .. runblock:: pycon
230+
231+ >>> from spatialmath import Polygon2
232+ >>> p = Polygon2([[1, 3, 2], [2, 2, 4]])
233+ >>> p.moment(0, 0) # area
234+ >>> p.moment(3, 0)
235+
236+ Note is negative for clockwise perimeter.
237+ """
238+ x = data [0 , :]
239+ y = data [1 , :]
240+
241+ return np .sum (x ** p * y ** q )
242+
243+ def gauss1d (mu , var , x ):
244+ """
245+ Gaussian function in 1D
246+
247+ :param mu: mean
248+ :type mu: float
249+ :param var: variance
250+ :type var: float
251+ :param x: x-coordinate values
252+ :type x: array_like(n)
253+ :return: Gaussian :math:`G(x)`
254+ :rtype: ndarray(n)
255+
256+ :seealso: :func:`gauss2d`
257+ """
258+ sigma = np .sqrt (var )
259+ x = base .getvector (x )
260+
261+ return 1.0 / np .sqrt (sigma ** 2 * 2 * np .pi ) * np .exp (- (x - mu )** 2 / 2 / sigma ** 2 )
262+
263+ def gauss2d (mu , P , X , Y ):
264+ """
265+ Gaussian function in 2D
266+
267+ :param mu: mean
268+ :type mu: array_like(2)
269+ :param P: covariance matrix
270+ :type P: ndarray(2,2)
271+ :param X: array of x-coordinates
272+ :type X: ndarray(n,m)
273+ :param Y: array of y-coordinates
274+ :type Y: ndarray(n,m)
275+ :return: Gaussian :math:`g(x,y)`
276+ :rtype: ndarray(n,m)
277+
278+ Computed :math:`g_{i,j} = G(x_{i,j}, y_{i,j})`
279+
280+ :seealso: :func:`gauss1d`
281+ """
282+
283+ x = X .ravel () - mu [0 ]
284+ y = Y .ravel () - mu [1 ]
285+
286+ Pi = np .linalg .inv (P );
287+ g = 1 / (2 * np .pi * np .sqrt (np .linalg .det (P ))) * np .exp (
288+ - 0.5 * (x ** 2 * Pi [0 , 0 ] + y ** 2 * Pi [1 , 1 ] + 2 * x * y * Pi [0 , 1 ]));
289+ return g .reshape (X .shape )
290+
210291if __name__ == "__main__" :
211292
212- print (bresenham ([2 ,2 ], [2 ,4 ]))
213- print (bresenham ([2 ,2 ], [2 ,- 4 ]))
214- print (bresenham ([2 ,2 ], [4 ,2 ]))
215- print (bresenham ([2 ,2 ], [- 4 ,2 ]))
216- print (bresenham ([2 ,2 ], [2 ,2 ]))
217- print (bresenham ([2 ,2 ], [3 ,6 ])) # steep
218- print (bresenham ([2 ,2 ], [6 ,3 ])) # shallow
219- print (bresenham ([2 ,2 ], [3 ,6 ])) # steep
220- print (bresenham ([2 ,2 ], [6 ,3 ])) # shallow
293+ r = np .linspace (- 4 , 4 , 6 )
294+ x , y = np .meshgrid (r , r )
295+ print (gauss2d ([0 , 0 ], np .diag ([1 ,2 ]), x , y ))
296+ # print(bresenham([2,2], [2,4]))
297+ # print(bresenham([2,2], [2,-4]))
298+ # print(bresenham([2,2], [4,2]))
299+ # print(bresenham([2,2], [-4,2]))
300+ # print(bresenham([2,2], [2,2]))
301+ # print(bresenham([2,2], [3,6])) # steep
302+ # print(bresenham([2,2], [6,3])) # shallow
303+ # print(bresenham([2,2], [3,6])) # steep
304+ # print(bresenham([2,2], [6,3])) # shallow
0 commit comments