10000 update angle wrapping and add angular statistics functions · astyl/spatialmath-python@3b512e0 · GitHub
[go: up one dir, main page]
Skip to content

Commit 3b512e0

Browse files
petercorkepetercorke
committed
update angle wrapping and add angular statistics functions
1 parent d69a097 commit 3b512e0

File tree

1 file changed

+138
-1
lines changed

1 file changed

+138
-1
lines changed

‎spatialmath/base/vectors.py

Lines changed: 138 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -510,6 +510,8 @@ def wrap_0_pi(theta):
510510
This is used to fold angles of colatitude. If zero is the angle of the
511511
north pole, colatitude increases to :math:`\pi` at the south pole then
512512
decreases to :math:`0` as we head back to the north pole.
513+
514+
:seealso: :func:`wrap_mpi2_pi2` :func:`wrap_0_2pi` :func:`wrap_mpi_pi` :func:`angle_wrap`
513515
"""
514516
n = (theta / np.pi)
515517
if isinstance(n, np.ndarray):
@@ -519,6 +521,26 @@ def wrap_0_pi(theta):
519521

520522
return np.where(n & 1 == 0, theta - n * np.pi, (n+1) * np.pi - theta)
521523

524+
def wrap_mpi2_pi2(theta):
525+
r"""
526+
Wrap angle to range :math:`[-\pi/2, \pi/2]`
527+
528+
:param theta: input angle
529+
:type theta: scalar or ndarray
530+
:return: angle wrapped into range :math:`[-\pi/2, \pi/2]`
531+
532+
This is used to fold angles of latitude.
533+
534+
:seealso: :func:`wrap_0_pi` :func:`wrap_0_2pi` :func:`wrap_mpi_pi` :func:`angle_wrap`
535+
536+
"""
537+
n = (theta / np.pi)
538+
if isinstance(n, np.ndarray):
539+
n = n.astype(int)
540+
else:
541+
n = int(n)
542+
543+
return np.where(n & 1 == 0, theta - n * np.pi, (n+1) * np.pi - theta)
522544

523545
def wrap_0_2pi(theta):
524546
r"""
@@ -527,17 +549,22 @@ def wrap_0_2pi(theta):
527549
:param theta: input angle
528550
:type theta: scalar or ndarray
529551
:return: angle wrapped into range :math:`[0, 2\pi)`
552+
553+
:seealso: :func:`wrap_mpi_pi` :func:`wrap_0_pi` :func:`wrap_mpi2_pi2` :func:`angle_wrap`
530554
"""
555+
531556
return theta - 2.0 * math.pi * np.floor(theta / 2.0 / np.pi)
532557

533558

534559
def wrap_mpi_pi(angle):
535560
r"""
536-
Wrap angle to range :math:`[\-pi, \pi)`
561+
Wrap angle to range :math:`[-\pi, \pi)`
537562
538563
:param theta: input angle
539564
:type theta: scalar or ndarray
540565
:return: angle wrapped into range :math:`[-\pi, \pi)`
566+
567+
:seealso: :func:`wrap_0_2pi` :func:`wrap_0_pi` :func:`wrap_mpi2_pi2` :func:`angle_wrap`
541568
"""
542569
return np.mod(angle + math.pi, 2 * math.pi) - np.pi
543570

@@ -575,12 +602,122 @@ def angdiff(a, b=None):
575602
>>> angdiff(0.9 * pi, -0.9 * pi) / pi
576603
>>> angdiff(3 * pi)
577604
605+
:seealso: :func:`vector_diff` :func:`wrap_mpi_pi`
578606
"""
579607
if b is None:
580608
return np.mod(a + math.pi, 2 * math.pi) - math.pi
581609
else:
582610
return np.mod(a - b + math.pi, 2 * math.pi) - math.pi
583611

612+
def angle_std(theta):
613+
r"""
614+
Standard deviation of angular values
615+
616+
:param theta: angular values
617+
:type theta: array_like
618+
:return: circular standard deviation
619+
:rtype: float
620+
621+
.. math::
622+
623+
\sigma_{\theta} = \sqrt{-2 \log \| \left[ \frac{\sum \sin \theta_i}{N}, \frac{\sum \sin \theta_i}{N} \right] \|} \in [0, \infty)
624+
625+
:seealso: :func:`angle_mean`
626+
"""
627+
X = np.cos(theta).mean()
628+
Y = np.sin(theta).mean()
629+
R = np.sqrt(X**2 + Y**2)
630+
631+
return np.sqrt(-2 * np.log(R))
632+
633+
def angle_mean(theta):
634+
r"""
635+
Mean of angular values
636+
637+
:param theta: angular values
638+
:type v: array_like
639+
:return: circular mean
640+
:rtype: float
641+
642+
The circular mean is given by
643+
644+
.. math::
645+
646+
\bar{\theta} = \tan^{-1} \frac{\sum \sin \theta_i}{\sum \cos \theta_i} \in [-\pi, \pi)]
647+
648+
:seealso: :func:`angle_std`
649+
"""
650+
X = np.cos(theta).sum()
651+
Y = np.sin(theta).sum()
652+
return np.artan2(Y, X)
653+
654+
def angle_wrap(theta, mode='-pi:pi'):
655+
"""
656+
Generalized angle-wrapping
657+
658+
:param v: angles to wrap
659+
:type v: array_like
660+
:param mode: wrapping mode, one of: ``"0:2pi"``, ``"0:pi"``, ``"-pi/2:pi/2"`` or ``"-pi:pi"`` [default]
661+
:type mode: str, optional
662+
:return: wrapped angles
663+
:rtype: ndarray
664+
665+
.. note:: The modes ``"0:pi"`` and ``"-pi/2:pi/2"`` are used to wrap angles of
666+
colatitude and latitude respectively.
667+
668+
:seealso: :func:`wrap_0_2pi` :func:`wrap_mpi_pi` :func:`wrap_0_pi` :func:`wrap_mpi2_pi2`
669+
"""
670+
if mode == "0:2pi":
671+
return wrap_0_2pi(theta)
672+
elif mode == "-pi:pi":
673+
return wrap_mpi_pi(theta)
674+
elif mode == "0:pi":
675+
return wrap_0_pi(theta)
676+
elif mode == "0:pi":
677+
return wrap_mpi2_pi2(theta)
678+
else:
679+
raise ValueError('bad method specified')
680+
681+
def vector_diff(v1, v2, mode):
682+
"""
683+
Generalized vector differnce
684+
685+
:param v1: first vector
686+
:type v1: array_like(n)
687+
:param v2: second vector
688+
:type v2: array_like(n)
689+
:param mode: subtraction mode
690+
:type mode: str of length n
691+
692+
============== ====================================
693+
mode character purpose
694+
========== DEA7 ==== ====================================
695+
r real number, don't wrap
696+
c angle on circle, wrap to [-π, π)
697+
C angle on circle, wrap to [0, 2π)
698+
l latitude angle, wrap to [-π/2, π/2]
699+
L colatitude angle, wrap to [0, π]
700+
============== ====================================
701+
702+
:seealso: :func:`angdiff` :func:`wrap_0_2pi` :func:`wrap_mpi_pi` :func:`wrap_0_pi` :func:`wrap_mpi2_pi2`
703+
"""
704+
v = getvector(v1) - getvector(v2)
705+
for i, m in enumerate(mode):
706+
if m == "r":
707+
pass
708+
elif m == "c":
709+
v[i] = wrap_mpi_pi(v[i])
710+
elif m == "C":
711+
v[i] = wrap_0_2pi(v[i])
712+
elif m == "l":
713+
v[i] = wrap_mpi2_pi2(v[i])
714+
elif m == "L":
715+
v[i] = wrap_0_pi(v[i])
716+
else:
717+
raise ValueError('bad mode character')
718+
719+
return v
720+
584721

585722
def removesmall(v, tol=100):
586723
"""

0 commit comments

Comments
 (0)
0