init commit

.gitattributes

@@ -33,3 +33,6 @@ saved_model/**/* filter=lfs diff=lfs merge=lfs -text
 *.zip filter=lfs diff=lfs merge=lfs -text
 *.zst filter=lfs diff=lfs merge=lfs -text
 *tfevents* filter=lfs diff=lfs merge=lfs -text
+simple-knn/build/lib.linux-x86_64-cpython-39/simple_knn/_C.cpython-39-x86_64-linux-gnu.so filter=lfs diff=lfs merge=lfs -text
+simple-knn/build/temp.linux-x86_64-cpython-39/.ninja_deps filter=lfs diff=lfs merge=lfs -text
+simple-knn/build/temp.linux-x86_64-cpython-39/simple_knn.o filter=lfs diff=lfs merge=lfs -text

README.md

@@ -1,12 +1,158 @@
----
-title: SparseAGS
-emoji: 🐨
-colorFrom: indigo
-colorTo: indigo
-sdk: gradio
-sdk_version: 5.7.1
-app_file: app.py
-pinned: false
----
-
-Check out the configuration reference at https://huggingface.co/docs/hub/spaces-config-reference
+# SparseAGS
+
+This repository contains the official implementation for **Sparse-view Pose Estimation and Reconstruction via Analysis by Generative Synthesis**. The paper has been accepted to [NeurIPS 2024](https://neurips.cc/Conferences/2024).
+
+### [Project Page](https://qitaozhao.github.io/SparseAGS) | [arXiv (Coming Soon)](https://qitaozhao.github.io/SparseAGS)
+
+### News
+
+- 2024.12.02: Initial code release.
+
+## Introduction
+
+**tl;dr** Given a set of unposed input images, **SparseAGS** jointly infers the corresponding camera poses and underlying 3D, allowing high-fidelity 3D inference in the wild.
+
+**Abstract.** Inferring the 3D structure underlying a set of multi-view images typically requires solving two co-dependent tasks -- accurate 3D reconstruction requires precise camera poses, and predicting camera poses relies on (implicitly or explicitly) modeling the underlying 3D. The classical framework of analysis by synthesis casts this inference as a joint optimization seeking to explain the observed pixels, and recent instantiations learn expressive 3D representations (e.g., Neural Fields) with gradient-descent-based pose refinement of initial pose estimates. However, given a sparse set of observed views, the observations may not provide sufficient direct evidence to obtain complete and accurate 3D. Moreover, large errors in pose estimation may not be easily corrected and can further degrade the inferred 3D. To allow robust 3D reconstruction and pose estimation in this challenging setup, we propose *SparseAGS*, a method that adapts this analysis-by-synthesis approach by: a) including novel-view-synthesis-based generative priors in conjunction with photometric objectives to improve the quality of the inferred 3D, and b) explicitly reasoning about outliers and using a discrete search with a continuous optimization-based strategy to correct them. We validate our framework across real-world and synthetic datasets in combination with several off-the-shelf pose estimation systems as initialization. We find that it significantly improves the base systems' pose accuracy while yielding high-quality 3D reconstructions that outperform the results from current multi-view reconstruction baselines.
+
+![teasert](assets/teaser.gif)
+
+## Install
+
+1. Clone SparseAGS:
+
+```bash
+git clone --recursive https://github.com/QitaoZhao/SparseAGS.git
+cd SparseAGS
+# if you have already cloned sparseags:
+# git submodule update --init --recursive
+```
+
+2. Create the environment and install packages:
+
+```bash
+conda create -n sparseags python=3.9
+conda activate sparseags
+
+# enable nvcc
+conda install -c conda-forge cudatoolkit-dev
+
+### torch
+# CUDA 11.7
+pip install torch==1.13.0+cu117 torchvision==0.14.0+cu117 torchaudio==0.13.0 --extra-index-url https://download.pytorch.org/whl/cu117
+
+# CUDA 12.1
+pip install torch==2.1.0 torchvision==0.16.0 torchaudio==2.1.0 --index-url https://download.pytorch.org/whl/cu121
+
+pip install -r requirements.txt
+
+### pytorch3D
+# CUDA 11.7
+conda install https://anaconda.org/pytorch3d/pytorch3d/0.7.5/download/linux-64/pytorch3d-0.7.5-py39_cu117_pyt1130.tar.bz2
+
+# CUDA 12.1
+conda install https://anaconda.org/pytorch3d/pytorch3d/0.7.8/download/linux-64/pytorch3d-0.7.8-py39_cu121_pyt210.tar.bz2
+
+# liegroups (minor modification to https://github.com/utiasSTARS/liegroups)
+pip install ./liegroups
+
+# simple-knn
+pip install ./simple-knn
+
+# a modified gaussian splatting that enables camera pose optimization
+pip install ./diff-gaussian-rasterization-camera 
+```
+
+Tested on:
+
+- Ubuntu 20.04 with torch 1.13 & CUDA 11.7 on an A5000 GPU.
+- Springdale Linux 8.6 with torch 2.1.0 & CUDA 12.1 on an A5000 GPU.
+- Red Hat Enterprise Linux 8.10 with torch 1.13 & CUDA 11.7 on a V100 GPU.
+
+Note: Look at this [issue](https://github.com/graphdeco-inria/gaussian-splatting/issues/993) or try `sudo apt-get install libglm-dev` if you encounter `fatal error: glm/glm.hpp: No such file or directory` when doing `pip install ./diff-gaussian-rasterization-camera`. 
+
+3. Download our 6-DoF Zero123 [checkpoint](https://drive.google.com/file/d/1JJ4wjaJ4IkUERRZYRrlNoQ-tXvftEYJD/view?usp=sharing) and place it in `SparseAGS/checkpoints`.
+
+```bash
+mkdir checkpoints
+cd checkpoints/
+pip install gdown
+gdown "https://drive.google.com/uc?id=1JJ4wjaJ4IkUERRZYRrlNoQ-tXvftEYJD"
+cd ..
+```
+
+## Usage
+
+(1) **Gradio Demo** (recommended, where you can upload your own images or use our preprocessed examples interactively):
+
+```bash
+# first-time running may take a longer time
+python gradio_app.py
+```
+
+(2) Use command lines:
+
+```bash
+### preprocess
+# background removal and recentering, save rgba at 256x256
+python process.py data/name.jpg
+
+# save at a larger resolution
+python process.py data/name.jpg --size 512
+
+# process all jpg images under a dir
+python process.py data
+
+### sparse-view 3D reconstruction
+# here we have some preprocessed examples in 'data/demo', with dust3r pose initialization
+# the output will be saved in 'output/demo/{category}'
+# valid category-num_views options are {[toy, 4], [butter, 6], [jordan, 8], [robot, 8], [eagle, 8]}
+
+# run single 3D reconstruction (w/o outlier removal & correction)
+python run.py --category jordan --num_views 8 
+
+# if you find the command above does not give you nice 3D, try enbaling loop-based outlier removal & correction (which takes more time)
+python run.py --category jordan --num_views 8 --enable_loop
+```
+
+Note: Actually, we include the `eagle` example to showcase how our full method works (we found in our experiments that dust3r gives one bad pose for this example). For other examples, you are supposed to see reasonable 3D with a single 3D reconstruction.
+
+## Tips
+
+* The world & camera coordinate system is the same as OpenGL:
+```
+    World            Camera        
+  
+     +y              up  target                                              
+     |               |  /                                            
+     |               | /                                                
+     |______+x       |/______right                                      
+    /                /         
+   /                /          
+  /                /           
+ +z               forward           
+
+elevation: in (-90, 90), from +y to -y is (-90, 90)
+azimuth: in (-180, 180), from +z to +x is (0, 90)
+```
+
+## Acknowledgments
+
+from https://github.com/ashawkey/diff-gaussian-rasterization
+
+This work is built on many amazing research works and open-source projects, thanks a lot to all the authors for sharing!
+
+- [gaussian-splatting](https://github.com/graphdeco-inria/gaussian-splatting) and [diff-gaussian-rasterization](https://github.com/graphdeco-inria/diff-gaussian-rasterization)
+- [threestudio](https://github.com/threestudio-project/threestudio)
+- [nvdiffrast](https://github.com/NVlabs/nvdiffrast)
+- [dearpygui](https://github.com/hoffstadt/DearPyGui)
+
+## Citation
+
+```
+@inproceedings{zhao2024sparseags,
+  title={Sparse-view Pose Estimation and Reconstruction via Analysis by Generative Synthesis}, 
+  author={Qitao Zhao and Shubham Tulsiani},
+  booktitle={NeurIPS},
+  year={2024}
+}
+```

liegroups/.gitignore

@@ -0,0 +1,6 @@
+.DS_Store
+__pycache__
+.cache
+*.egg-info
+.vscode
+*.pyc

liegroups/LICENSE

@@ -0,0 +1,21 @@
+The MIT License (MIT)
+
+Copyright (c) 2017 Lee Clement
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.

liegroups/README.md

@@ -0,0 +1,36 @@
+# liegroups
+Python implementation of SO2, SE2, SO3, and SE3 matrix Lie groups using numpy or PyTorch.
+
+[[Documentation]](docs/build/html/index.html)
+
+## Installation
+To install, `cd` into the repository directory (the one with `setup.py`) and run:
+```bash
+pip install .
+```
+or
+```bash
+pip install -e .
+```
+The `-e` flag tells pip to install the package in-place, which lets you make changes to the code without having to reinstall every time. *Do not do this on shared workstations!*
+
+## Testing
+Ensure you have `pytest` installed on your system, or install it using `conda install pytest` or `pip install pytest`. Then run `pytest` in the repository directory.
+
+## Usage
+Numpy and torch implementations are accessible through the `liegroups.numpy` and `liegroups.torch` subpackages. 
+By default, the numpy implementation is available through the top-level package.
+
+Access the numpy implementation using something like
+```python
+from liegroups import SE3
+```
+or
+```python
+from liegroups.numpy import SO2
+```
+
+Access the pytorch implementation using something like
+```python
+from liegroups.torch import SE2
+```

liegroups/build/lib/liegroups/__init__.py

@@ -0,0 +1,16 @@
+"""Special Euclidean and Special Orthogonal Lie groups."""
+
+from .numpy import SO2 as SO2
+from .numpy import SE2 as SE2
+from .numpy import SO3 as SO3
+from .numpy import SE3 as SE3
+
+
+try:
+    from . import numpy
+    from . import torch
+except:
+    pass
+
+__author__ = "Lee Clement"
+__email__ = "lee.clement@robotics.utias.utoronto.ca"

liegroups/build/lib/liegroups/_base.py

@@ -0,0 +1,218 @@
+from abc import ABCMeta, abstractmethod
+
+# support for both python 2 and 3
+from future.utils import with_metaclass
+
+
+class LieGroupBase(with_metaclass(ABCMeta)):
+    """ Common abstract base class defining basic interface for Lie groups.
+        Does not depend on any specific linear algebra library.
+    """
+
+    def __init__(self):
+        pass
+
+    @property
+    @classmethod
+    @abstractmethod
+    def dim(cls):
+        """Dimension of the underlying representation."""
+        pass
+
+    @property
+    @classmethod
+    @abstractmethod
+    def dof(cls):
+        """Underlying degrees of freedom (i.e., dimension of the tangent space)."""
+        pass
+
+    @abstractmethod
+    def dot(self, other):
+        """Multiply another group element or one or more vectors on the left.
+        """
+        pass
+
+    @classmethod
+    @abstractmethod
+    def exp(cls, vec):
+        """Exponential map for the group.
+
+        Computes a transformation from a tangent vector.
+
+        This is the inverse operation to log.
+        """
+        pass
+
+    @classmethod
+    @abstractmethod
+    def identity(cls):
+        """Return the identity transformation."""
+        pass
+
+    @abstractmethod
+    def inv(self):
+        """Return the inverse transformation."""
+        pass
+
+    @abstractmethod
+    def log(self):
+        """Logarithmic map for the group.
+
+        Computes a tangent vector from a transformation.
+
+        This is the inverse operation to exp.
+        """
+        pass
+
+    @abstractmethod
+    def normalize(self):
+        """Normalize the group element to ensure it is valid and
+        negate the effect of rounding errors.
+        """
+        pass
+
+    @abstractmethod
+    def perturb(self, vec):
+        """Perturb the group element on the left by a vector in its local tangent space.
+        """
+        pass
+
+
+class MatrixLieGroupBase(LieGroupBase):
+    """Common abstract base class defining basic interface for Matrix Lie Groups.
+       Does not depend on any specific linear algebra library.
+    """
+
+    def __repr__(self):
+        """Return a string representation of the transformation."""
+        return "<{}.{}>\n{}".format(self.__class__.__module__, self.__class__.__name__, self.as_matrix()).replace("\n", "\n| ")
+
+    @abstractmethod
+    def adjoint(self):
+        """Return the adjoint matrix of the transformation."""
+        pass
+
+    @abstractmethod
+    def as_matrix(self):
+        """Return the matrix representation of the transformation."""
+        pass
+
+    @classmethod
+    @abstractmethod
+    def from_matrix(cls, mat, normalize=False):
+        """Create a transformation from a matrix (safe, but slower)."""
+        pass
+
+    @classmethod
+    @abstractmethod
+    def inv_left_jacobian(cls, vec):
+        """Inverse of the left Jacobian for the group."""
+        pass
+
+    @classmethod
+    @abstractmethod
+    def is_valid_matrix(cls, mat):
+        """Check if a matrix is a valid transformation matrix."""
+        pass
+
+    @classmethod
+    @abstractmethod
+    def left_jacobian(cls, vec):
+        """Left Jacobian for the group."""
+        pass
+
+    @classmethod
+    @abstractmethod
+    def vee(cls, mat):
+        """vee operator as defined by Barfoot.
+
+        This is the inverse operation to wedge.
+        """
+        pass
+
+    @classmethod
+    @abstractmethod
+    def wedge(cls, vec):
+        """wedge operator as defined by Barfoot.
+
+        This is the inverse operation to vee.
+        """
+        pass
+
+
+class SOMatrixBase(MatrixLieGroupBase):
+    """Common abstract base class for Special Orthogonal Matrix Lie Groups SO(N).
+       Does not depend on any specific linear algebra library.
+    """
+
+    def __init__(self, mat):
+        """Create a transformation from a rotation matrix (unsafe, but faster)."""
+        self.mat = mat
+        """Storage for the rotation matrix."""
+
+    def as_matrix(self):
+        """Return the matrix representation of the rotation."""
+        return self.mat
+
+    def perturb(self, phi):
+        """Perturb the rotation in-place on the left by a vector in its local tangent space.
+
+        .. math::
+            \\mathbf{C} \\gets \\exp(\\boldsymbol{\\phi}^\\wedge) \\mathbf{C}
+        """
+        self.mat = self.__class__.exp(phi).dot(self).mat
+
+
+class SEMatrixBase(MatrixLieGroupBase):
+    """Common abstract base class for Special Euclidean Matrix Lie Groups SE(N).
+       Does not depend on any specific linear algebra library.
+    """
+
+    def __init__(self, rot, trans):
+        """Create a transformation from a translation and a rotation (unsafe, but faster)"""
+        self.rot = rot
+        """Storage for the rotation matrix."""
+        self.trans = trans
+        """Storage for the translation vector."""
+
+    @classmethod
+    @abstractmethod
+    def odot(cls, p, directional=False):
+        """odot operator as defined by Barfoot."""
+        pass
+
+    def perturb(self, xi):
+        """Perturb the transformation in-place on the left by a vector in its local tangent space.
+
+        .. math::
+            \\mathbf{T} \\gets \\exp(\\boldsymbol{\\xi}^\\wedge) \\mathbf{T}
+        """
+        perturbed = self.__class__.exp(xi).dot(self)
+        self.rot = perturbed.rot
+        self.trans = perturbed.trans
+
+    @property
+    @classmethod
+    @abstractmethod
+    def RotationType(cls):
+        """Rotation type."""
+        pass
+
+
+class VectorLieGroupBase(LieGroupBase):
+    """Common abstract base class for Lie Groups with vector parametrizations 
+       (complex, quaternions, dual quaternions). Does not depend on any  
+       specific linear algebra library.
+    """
+
+    def __init__(self, data):
+        self.data = data
+
+    def __repr__(self):
+        """Return a string representation of the transformation."""
+        return "<{}.{}>\n{}".format(self.__class__.__module__, self.__class__.__name__, self.data).replace("\n", "\n| ")
+
+    @abstractmethod
+    def conjugate(self):
+        """Return the conjugate of the vector"""
+        pass

liegroups/build/lib/liegroups/numpy/__init__.py

@@ -0,0 +1,9 @@
+"""Numpy implementations of Special Euclidean and Special Orthogonal Lie groups."""
+
+from .so2 import SO2Matrix as SO2
+from .se2 import SE2Matrix as SE2
+from .so3 import SO3Matrix as SO3
+from .se3 import SE3Matrix as SE3
+
+__author__ = "Lee Clement"
+__email__ = "lee.clement@robotics.utias.utoronto.ca"

liegroups/build/lib/liegroups/numpy/_base.py

@@ -0,0 +1,172 @@
+import numpy as np
+
+from .. import _base
+
+
+class SOMatrixBase(_base.SOMatrixBase):
+    """Implementation of methods common to SO(N) matrix lie groups using Numpy"""
+
+    def dot(self, other):
+        """Multiply another rotation or one or more vectors on the left.
+        """
+        if isinstance(other, self.__class__):
+            # Compound with another rotation
+            return self.__class__(np.dot(self.mat, other.mat))
+        else:
+            other = np.atleast_2d(other)
+
+            # Transform one or more 2-vectors or fail
+            if other.shape[1] == self.dim:
+                return np.squeeze(np.dot(self.mat, other.T).T)
+            else:
+                raise ValueError(
+                    "Vector must have shape ({},) or (N,{})".format(self.dim, self.dim))
+
+    @classmethod
+    def identity(cls):
+        """Return the identity rotation."""
+        return cls(np.identity(cls.dim))
+
+    def inv(self):
+        """Return the inverse rotation:
+
+        .. math::
+            \\mathbf{C}^{-1} = \\mathbf{C}^T
+        """
+        return self.__class__(self.mat.T)
+
+    @classmethod
+    def from_matrix(cls, mat, normalize=False):
+        """Create a rotation from a matrix (safe, but slower).
+
+        Throws an error if mat is invalid and normalize=False.
+        If normalize=True invalid matrices will be normalized to be valid.
+        """
+        mat_is_valid = cls.is_valid_matrix(mat)
+
+        if mat_is_valid or normalize:
+            result = cls(mat)
+            if not mat_is_valid and normalize:
+                result.normalize()
+        else:
+            raise ValueError(
+                "Invalid rotation matrix. Use normalize=True to handle rounding errors.")
+
+        return result
+
+    @classmethod
+    def is_valid_matrix(cls, mat):
+        """Check if a matrix is a valid rotation matrix."""
+        return mat.shape == (cls.dim, cls.dim) and \
+            np.isclose(np.linalg.det(mat), 1.) and \
+            np.allclose(mat.T.dot(mat), np.identity(cls.dim))
+
+    def normalize(self):
+        """Normalize the rotation matrix to ensure it is valid and
+        negate the effect of rounding errors.
+        """
+        # The SVD is commonly written as a = U S V.H.
+        # The v returned by this function is V.H and u = U.
+        U, _, V = np.linalg.svd(self.mat, full_matrices=False)
+
+        S = np.identity(self.dim)
+        S[self.dim - 1, self.dim - 1] = np.linalg.det(U) * np.linalg.det(V)
+
+        self.mat = U.dot(S).dot(V)
+
+
+class SEMatrixBase(_base.SEMatrixBase):
+    """Implementation of methods common to SE(N) matrix lie groups using Numpy"""
+
+    def as_matrix(self):
+        """Return the matrix representation of the rotation."""
+        R = self.rot.as_matrix()
+        t = np.reshape(self.trans, (self.dim - 1, 1))
+        bottom_row = np.append(np.zeros(self.dim - 1), 1.)
+        return np.vstack([np.hstack([R, t]),
+                          bottom_row])
+
+    def dot(self, other):
+        """Multiply another rotation or one or more vectors on the left.
+        """
+        if isinstance(other, self.__class__):
+            # Compound with another transformation
+            return self.__class__(self.rot.dot(other.rot),
+                                  self.rot.dot(other.trans) + self.trans)
+        else:
+            other = np.atleast_2d(other)
+
+            if other.shape[1] == self.dim - 1:
+                # Transform one or more 2-vectors
+                return np.squeeze(self.rot.dot(other) + self.trans)
+            elif other.shape[1] == self.dim:
+                # Transform one or more 3-vectors
+                return np.squeeze(self.as_matrix().dot(other.T)).T
+            else:
+                raise ValueError("Vector must have shape ({},), ({},), (N,{}) or (N,{})".format(
+                    self.dim - 1, self.dim, self.dim - 1, self.dim))
+
+    @classmethod
+    def from_matrix(cls, mat, normalize=False):
+        """Create a transformation from a matrix (safe, but slower).
+
+        Throws an error if mat is invalid and normalize=False.
+        If normalize=True invalid matrices will be normalized to be valid.
+        """
+        mat_is_valid = cls.is_valid_matrix(mat)
+
+        if mat_is_valid or normalize:
+            result = cls(
+                cls.RotationType(mat[0:cls.dim - 1, 0:cls.dim - 1]),
+                mat[0:cls.dim - 1, cls.dim - 1])
+            if not mat_is_valid and normalize:
+                result.normalize()
+        else:
+            raise ValueError(
+                "Invalid transformation matrix. Use normalize=True to handle rounding errors.")
+
+        return result
+
+    @classmethod
+    def identity(cls):
+        """Return the identity transformation."""
+        return cls.from_matrix(np.identity(cls.dim))
+
+    def inv(self):
+        """Return the inverse transformation:
+
+        .. math::
+            \\mathbf{T}^{-1} = 
+                \\begin{bmatrix}
+                    \\mathbf{C}^T & -\\mathbf{C}^T\\mathbf{r} \\\\
+                    \\mathbf{0}^T & 1
+                \\end{bmatrix}
+        """
+        inv_rot = self.rot.inv()
+        inv_trans = -(inv_rot.dot(self.trans))
+        return self.__class__(inv_rot, inv_trans)
+
+    @classmethod
+    def is_valid_matrix(cls, mat):
+        """Check if a matrix is a valid transformation matrix."""
+        bottom_row = np.append(np.zeros(cls.dim - 1), 1.)
+
+        return mat.shape == (cls.dim, cls.dim) and \
+            np.array_equal(mat[cls.dim - 1, :], bottom_row) and \
+            cls.RotationType.is_valid_matrix(mat[0:cls.dim - 1, 0:cls.dim - 1])
+
+    def normalize(self):
+        """Normalize the transformation matrix to ensure it is valid and
+        negate the effect of rounding errors.
+        """
+        self.rot.normalize()
+
+
+class VectorLieGroupBase(_base.VectorLieGroupBase):
+    """Implementation of methods common to vector-parametrized lie groups using Numpy"""
+
+    def normalize(self):
+        self.data = self.data / np.linalg.norm(self.data)
+
+    def conjugate(self):
+        return self.__class__(np.hstack([self.data[0], -self.data[1:]]))

liegroups/build/lib/liegroups/numpy/se2.py

@@ -0,0 +1,206 @@
+import numpy as np
+
+from . import _base
+from .so2 import SO2Matrix
+
+
+class SE2Matrix(_base.SEMatrixBase):
+    """Homogeneous transformation matrix in :math:`SE(2)` using active (alibi) transformations.
+
+    .. math::
+        SE(2) &= \\left\\{ \\mathbf{T}=
+                \\begin{bmatrix}
+                    \\mathbf{C} & \\mathbf{r} \\\\
+                    \\mathbf{0}^T & 1
+                \\end{bmatrix} \\in \\mathbb{R}^{3 \\times 3} ~\\middle|~ \\mathbf{C} \\in SO(2), \\mathbf{r} \\in \\mathbb{R}^2 \\right\\} \\\\
+        \\mathfrak{se}(2) &= \\left\\{ \\boldsymbol{\\Xi} =
+        \\boldsymbol{\\xi}^\\wedge \\in \\mathbb{R}^{3 \\times 3} ~\\middle|~
+         \\boldsymbol{\\xi}=
+            \\begin{bmatrix}
+                \\boldsymbol{\\rho} \\\\ \\phi
+            \\end{bmatrix} \\in \\mathbb{R}^3, \\boldsymbol{\\rho} \\in \\mathbb{R}^2, \\phi \\in \\mathbb{R} \\right\\}
+
+    :cvar ~liegroups.SE2.dim: Dimension of the rotation matrix.
+    :cvar ~liegroups.SE2.dof: Underlying degrees of freedom (i.e., dimension of the tangent space).
+    :ivar rot: Storage for the rotation matrix :math:`\\mathbf{C}`.
+    :ivar trans: Storage for the translation vector :math:`\\mathbf{r}`.
+    """
+    dim = 3
+    """Dimension of the transformation matrix."""
+    dof = 3
+    """Underlying degrees of freedom (i.e., dimension of the tangent space)."""
+    RotationType = SO2Matrix
+
+    def adjoint(self):
+        """Adjoint matrix of the transformation.
+
+        .. math::
+            \\text{Ad}(\\mathbf{T}) = 
+            \\begin{bmatrix}
+                \\mathbf{C} & 1^\\wedge \\mathbf{r} \\\\
+                \\mathbf{0}^T & 1
+            \\end{bmatrix}
+            \\in \\mathbb{R}^{3 \\times 3}
+        """
+        rot_part = self.rot.as_matrix()
+        trans_part = np.array([self.trans[1], -self.trans[0]]).reshape((2, 1))
+        return np.vstack([np.hstack([rot_part, trans_part]),
+                          [0, 0, 1]])
+
+    @classmethod
+    def exp(cls, xi):
+        """Exponential map for :math:`SE(2)`, which computes a transformation from a tangent vector:
+
+        .. math::
+            \\mathbf{T}(\\boldsymbol{\\xi}) =
+            \\exp(\\boldsymbol{\\xi}^\\wedge) =
+            \\begin{bmatrix}
+                \\exp(\\phi ^\\wedge) & \\mathbf{J} \\boldsymbol{\\rho}  \\\\
+                \\mathbf{0} ^ T & 1
+            \\end{bmatrix}
+
+        This is the inverse operation to :meth:`~liegroups.SE2.log`.
+        """
+        if len(xi) != cls.dof:
+            raise ValueError("xi must have length {}".format(cls.dof))
+
+        rho = xi[0:2]
+        phi = xi[2]
+        return cls(cls.RotationType.exp(phi),
+                   cls.RotationType.left_jacobian(phi).dot(rho))
+
+    @classmethod
+    def inv_left_jacobian(cls, xi):
+        """:math:`SE(2)` inverse left Jacobian.
+
+        .. math::
+            \\mathcal{J}^{-1}(\\boldsymbol{\\xi})
+        """
+        raise NotImplementedError
+
+    @classmethod
+    def left_jacobian(cls, xi):
+        """:math:`SE(2)` left Jacobian.
+
+        .. math::
+            \\mathcal{J}(\\boldsymbol{\\xi})
+        """
+        raise NotImplementedError
+
+    def log(self):
+        """Logarithmic map for :math:`SE(2)`, which computes a tangent vector from a transformation:
+
+        .. math::
+            \\boldsymbol{\\xi}(\\mathbf{T}) =
+            \\ln(\\mathbf{T})^\\vee =
+            \\begin{bmatrix}
+                \\mathbf{J} ^ {-1} \\mathbf{r} \\\\
+                \\ln(\\boldsymbol{C}) ^\\vee
+            \\end{bmatrix}
+
+        This is the inverse operation to :meth:`~liegroups.SE2.log`.
+        """
+        phi = self.rot.log()
+        rho = self.RotationType.inv_left_jacobian(phi).dot(self.trans)
+        return np.hstack([rho, phi])
+
+    @classmethod
+    def odot(cls, p, directional=False):
+        """:math:`SE(2)` odot operator as defined by Barfoot. 
+
+        This is the Jacobian of a vector 
+
+        .. math::
+            \\mathbf{p} = 
+            \\begin{bmatrix}
+                sx \\\\ sy \\\\ sz \\\\ s
+            \\end{bmatrix} =
+            \\begin{bmatrix}
+                \\boldsymbol{\\epsilon} \\\\ \\eta
+            \\end{bmatrix}
+
+        with respect to a perturbation in the underlying parameters of :math:`\\mathbf{T}`.
+
+        If :math:`\\mathbf{p}` is given in Euclidean coordinates and directional=False, the missing scale value :math:`\\eta` is assumed to be 1 and the Jacobian is 2x3. If directional=True, :math:`\\eta` is assumed to be 0:
+
+        .. math::
+            \\mathbf{p}^\\odot =
+            \\begin{bmatrix}
+                \\eta \\mathbf{1} & 1^\\wedge \\boldsymbol{\\epsilon}
+            \\end{bmatrix}
+
+        If :math:`\\mathbf{p}` is given in Homogeneous coordinates, the Jacobian is 3x3:
+
+        .. math::
+            \\mathbf{p}^\\odot =
+            \\begin{bmatrix}
+                \\eta \\mathbf{1} & 1^\\wedge \\boldsymbol{\\epsilon} \\\\
+                \\mathbf{0}^T & 0
+            \\end{bmatrix}
+        """
+        p = np.atleast_2d(p)
+        result = np.zeros([p.shape[0], p.shape[1], cls.dof])
+
+        if p.shape[1] == cls.dim - 1:
+            # Assume scale parameter is 1 unless p is a direction
+            # vector, in which case the scale is 0
+            if not directional:
+                result[:, 0:2, 0:2] = np.eye(2)
+
+            result[:, 0:2, 2] = cls.RotationType.wedge(1).dot(p.T).T
+
+        elif p.shape[1] == cls.dim:
+            result[:, 0:2, 0:2] = p[:, 2] * np.eye(2)
+            result[:, 0:2, 2] = cls.RotationType.wedge(1).dot(p[:, 0:2].T).T
+
+        else:
+            raise ValueError("p must have shape ({},), ({},), (N,{}) or (N,{})".format(
+                cls.dim - 1, cls.dim, cls.dim - 1, cls.dim))
+
+        return np.squeeze(result)
+
+    @classmethod
+    def vee(cls, Xi):
+        """:math:`SE(2)` vee operator as defined by Barfoot.
+
+        .. math::
+            \\boldsymbol{\\xi} = \\boldsymbol{\\Xi} ^\\vee
+
+        This is the inverse operation to :meth:`~liegroups.SE2.wedge`.
+        """
+        if Xi.ndim < 3:
+            Xi = np.expand_dims(Xi, axis=0)
+
+        if Xi.shape[1:3] != (cls.dof, cls.dof):
+            raise ValueError("Xi must have shape ({},{}) or (N,{},{})".format(
+                cls.dof, cls.dof, cls.dof, cls.dof))
+
+        xi = np.empty([Xi.shape[0], cls.dof])
+        xi[:, 0:2] = Xi[:, 0:2, 2]
+        xi[:, 2] = cls.RotationType.vee(Xi[:, 0:2, 0:2])
+        return np.squeeze(xi)
+
+    @classmethod
+    def wedge(cls, xi):
+        """:math:`SE(2)` wedge operator as defined by Barfoot.
+
+        .. math::
+            \\boldsymbol{\\Xi} =
+            \\boldsymbol{\\xi} ^\\wedge =
+            \\begin{bmatrix}
+                \\phi ^\\wedge & \\boldsymbol{\\rho} \\\\
+                \\mathbf{0} ^ T & 0
+            \\end{bmatrix}
+
+        This is the inverse operation to :meth:`~liegroups.SE2.vee`.
+        """
+        xi = np.atleast_2d(xi)
+        if xi.shape[1] != cls.dof:
+            raise ValueError(
+                "xi must have shape ({},) or (N,{})".format(cls.dof, cls.dof))
+
+        Xi = np.zeros([xi.shape[0], cls.dof, cls.dof])
+        Xi[:, 0:2, 0:2] = cls.RotationType.wedge(xi[:, 2])
+        Xi[:, 0:2, 2] = xi[:, 0:2]
+
+        return np.squeeze(Xi)

liegroups/build/lib/liegroups/numpy/se3.py

@@ -0,0 +1,354 @@
+import numpy as np
+
+from . import _base
+from .so3 import SO3Matrix
+
+
+class SE3Matrix(_base.SEMatrixBase):
+    """Homogeneous transformation matrix in :math:`SE(3)` using active (alibi) transformations.
+
+    .. math::
+        SE(3) &= \\left\\{ \\mathbf{T}=
+                \\begin{bmatrix}
+                    \\mathbf{C} & \\mathbf{r} \\\\
+                    \\mathbf{0}^T & 1
+                \\end{bmatrix} \\in \\mathbb{R}^{4 \\times 4} ~\\middle|~ \\mathbf{C} \\in SO(3), \\mathbf{r} \\in \\mathbb{R}^3 \\right\\} \\\\
+        \\mathfrak{se}(3) &= \\left\\{ \\boldsymbol{\\Xi} =
+        \\boldsymbol{\\xi}^\\wedge \\in \\mathbb{R}^{4 \\times 4} ~\\middle|~
+         \\boldsymbol{\\xi}=
+            \\begin{bmatrix}
+                \\boldsymbol{\\rho} \\\\ \\boldsymbol{\\phi}
+            \\end{bmatrix} \\in \\mathbb{R}^6, \\boldsymbol{\\rho} \\in \\mathbb{R}^3, \\boldsymbol{\\phi} \\in \\mathbb{R}^3 \\right\\}
+
+    :cvar ~liegroups.SE2.dim: Dimension of the rotation matrix.
+    :cvar ~liegroups.SE2.dof: Underlying degrees of freedom (i.e., dimension of the tangent space).
+    :ivar rot: Storage for the rotation matrix :math:`\\mathbf{C}`.
+    :ivar trans: Storage for the translation vector :math:`\\mathbf{r}`.
+    """
+    dim = 4
+    """Dimension of the transformation matrix."""
+    dof = 6
+    """Underlying degrees of freedom (i.e., dimension of the tangent space)."""
+    RotationType = SO3Matrix
+
+    def adjoint(self):
+        """Adjoint matrix of the transformation.
+
+        .. math::
+            \\text{Ad}(\\mathbf{T}) =
+            \\begin{bmatrix}
+                \\mathbf{C} & \\mathbf{r}^\\wedge\\mathbf{C} \\\\
+                \\mathbf{0} & \\mathbf{C}
+            \\end{bmatrix}
+            \\in \\mathbb{R}^{6 \\times 6}
+        """
+        rotmat = self.rot.as_matrix()
+        return np.vstack(
+            [np.hstack([rotmat,
+                        self.RotationType.wedge(self.trans).dot(rotmat)]),
+             np.hstack([np.zeros((3, 3)), rotmat])]
+        )
+
+    @classmethod
+    def curlyvee(cls, Psi):
+        """:math:`SE(3)` curlyvee operator as defined by Barfoot.
+
+        .. math::
+            \\boldsymbol{\\xi} = 
+            \\boldsymbol{\\Psi}^\\curlyvee
+
+        This is the inverse operation to :meth:`~liegroups.SE3.curlywedge`.
+        """
+        if Psi.ndim < 3:
+            Psi = np.expand_dims(Psi, axis=0)
+
+        if Psi.shape[1:3] != (cls.dof, cls.dof):
+            raise ValueError("Psi must have shape ({},{}) or (N,{},{})".format(
+                cls.dof, cls.dof, cls.dof, cls.dof))
+
+        xi = np.empty([Psi.shape[0], cls.dof])
+        xi[:, 0:3] = cls.RotationType.vee(Psi[:, 0:3, 3:6])
+        xi[:, 3:6] = cls.RotationType.vee(Psi[:, 0:3, 0:3])
+
+        return np.squeeze(xi)
+
+    @classmethod
+    def curlywedge(cls, xi):
+        """:math:`SE(3)` curlywedge operator as defined by Barfoot.
+
+        .. math::
+            \\boldsymbol{\\Psi} = 
+            \\boldsymbol{\\xi}^\\curlywedge = 
+            \\begin{bmatrix}
+                \\boldsymbol{\\phi}^\\wedge & \\boldsymbol{\\rho}^\\wedge \\\\
+                \\mathbf{0} & \\boldsymbol{\\phi}^\\wedge
+            \\end{bmatrix}
+
+        This is the inverse operation to :meth:`~liegroups.SE3.curlyvee`.
+        """
+        xi = np.atleast_2d(xi)
+        if xi.shape[1] != cls.dof:
+            raise ValueError(
+                "xi must have shape ({},) or (N,{})".format(cls.dof, cls.dof))
+
+        Psi = np.zeros([xi.shape[0], cls.dof, cls.dof])
+        Psi[:, 0:3, 0:3] = cls.RotationType.wedge(xi[:, 3:6])
+        Psi[:, 0:3, 3:6] = cls.RotationType.wedge(xi[:, 0:3])
+        Psi[:, 3:6, 3:6] = Psi[:, 0:3, 0:3]
+
+        return np.squeeze(Psi)
+
+    @classmethod
+    def exp(cls, xi):
+        """Exponential map for :math:`SE(3)`, which computes a transformation from a tangent vector:
+
+        .. math::
+            \\mathbf{T}(\\boldsymbol{\\xi}) =
+            \\exp(\\boldsymbol{\\xi}^\\wedge) =
+            \\begin{bmatrix}
+                \\exp(\\boldsymbol{\\phi}^\\wedge) & \\mathbf{J} \\boldsymbol{\\rho}  \\\\
+                \\mathbf{0} ^ T & 1
+            \\end{bmatrix}
+
+        This is the inverse operation to :meth:`~liegroups.SE3.log`.
+        """
+        if len(xi) != cls.dof:
+            raise ValueError("xi must have length {}".format(cls.dof))
+
+        rho = xi[0:3]
+        phi = xi[3:6]
+        return cls(cls.RotationType.exp(phi),
+                   cls.RotationType.left_jacobian(phi).dot(rho))
+
+    @classmethod
+    def left_jacobian_Q_matrix(cls, xi):
+        """The :math:`\\mathbf{Q}` matrix used to compute :math:`\\mathcal{J}` in :meth:`~liegroups.SE3.left_jacobian` and :math:`\\mathcal{J}^{-1}` in :meth:`~liegroups.SE3.inv_left_jacobian`.
+
+        .. math::
+            \\mathbf{Q}(\\boldsymbol{\\xi}) =
+            \\frac{1}{2}\\boldsymbol{\\rho}^\\wedge &+ 
+            \\left( \\frac{\\phi - \\sin \\phi}{\\phi^3} \\right)
+                \\left( 
+                    \\boldsymbol{\\phi}^\\wedge \\boldsymbol{\\rho}^\\wedge + 
+                    \\boldsymbol{\\rho}^\\wedge \\boldsymbol{\\phi}^\\wedge + 
+                    \\boldsymbol{\\phi}^\\wedge \\boldsymbol{\\rho}^\\wedge \\boldsymbol{\\phi}^\\wedge
+                \\right) \\\\ &+
+            \\left( \\frac{\\phi^2 + 2 \\cos \\phi - 2}{2 \\phi^4} \\right)
+                \\left( 
+                    \\boldsymbol{\\phi}^\\wedge \\boldsymbol{\\phi}^\\wedge \\boldsymbol{\\rho}^\\wedge + 
+                    \\boldsymbol{\\rho}^\\wedge \\boldsymbol{\\phi}^\\wedge \\boldsymbol{\\phi}^\\wedge - 
+                    3 \\boldsymbol{\\phi}^\\wedge \\boldsymbol{\\rho}^\\wedge \\boldsymbol{\\phi}^\\wedge
+                \\right) \\\\ &+
+            \\left( \\frac{2 \\phi - 3 \\sin \\phi + \\phi \\cos \\phi}{2 \\phi^5} \\right)
+                \\left( 
+                    \\boldsymbol{\\phi}^\\wedge \\boldsymbol{\\rho}^\\wedge \\boldsymbol{\\phi}^\\wedge \\boldsymbol{\\phi}^\\wedge + 
+                    \\boldsymbol{\\phi}^\\wedge \\boldsymbol{\\phi}^\\wedge \\boldsymbol{\\rho}^\\wedge \\boldsymbol{\\phi}^\\wedge
+                \\right)
+        """
+        if len(xi) != cls.dof:
+            raise ValueError("xi must have length {}".format(cls.dof))
+
+        rho = xi[0:3]  # translation part
+        phi = xi[3:6]  # rotation part
+
+        rx = cls.RotationType.wedge(rho)
+        px = cls.RotationType.wedge(phi)
+
+        ph = np.linalg.norm(phi)
+        ph2 = ph * ph
+        ph3 = ph2 * ph
+        ph4 = ph3 * ph
+        ph5 = ph4 * ph
+
+        cph = np.cos(ph)
+        sph = np.sin(ph)
+
+        m1 = 0.5
+        m2 = (ph - sph) / ph3
+        m3 = (0.5 * ph2 + cph - 1.) / ph4
+        m4 = (ph - 1.5 * sph + 0.5 * ph * cph) / ph5
+
+        t1 = rx
+        t2 = px.dot(rx) + rx.dot(px) + px.dot(rx).dot(px)
+        t3 = px.dot(px).dot(rx) + rx.dot(px).dot(px) - 3. * px.dot(rx).dot(px)
+        t4 = px.dot(rx).dot(px).dot(px) + px.dot(px).dot(rx).dot(px)
+
+        return m1 * t1 + m2 * t2 + m3 * t3 + m4 * t4
+
+    @classmethod
+    def inv_left_jacobian(cls, xi):
+        """:math:`SE(3)` inverse left Jacobian.
+
+        .. math::
+            \\mathcal{J}^{-1}(\\boldsymbol{\\xi}) = 
+            \\begin{bmatrix}
+                \\mathbf{J}^{-1} & -\\mathbf{J}^{-1} \\mathbf{Q} \\mathbf{J}^{-1} \\\\
+                \\mathbf{0} & \\mathbf{J}^{-1}
+            \\end{bmatrix}
+
+        with :math:`\\mathbf{J}^{-1}` as in :meth:`liegroups.SO3.inv_left_jacobian` and :math:`\\mathbf{Q}` as in :meth:`~liegroups.SE3.left_jacobian_Q_matrix`.
+        """
+        rho = xi[0:3]  # translation part
+        phi = xi[3:6]  # rotation part
+
+        # Near |phi|==0, use first order Taylor expansion
+        if np.isclose(np.linalg.norm(phi), 0.):
+            return np.identity(cls.dof) - 0.5 * cls.curlywedge(xi)
+
+        so3_inv_jac = cls.RotationType.inv_left_jacobian(phi)
+        Q_mat = cls.left_jacobian_Q_matrix(xi)
+
+        jac = np.zeros([cls.dof, cls.dof])
+        jac[0:3, 0:3] = so3_inv_jac
+        jac[0:3, 3:6] = -so3_inv_jac.dot(Q_mat).dot(so3_inv_jac)
+        jac[3:6, 3:6] = so3_inv_jac
+
+        return jac
+
+    @classmethod
+    def left_jacobian(cls, xi):
+        """:math:`SE(3)` left Jacobian.
+
+        .. math::
+            \\mathcal{J}(\\boldsymbol{\\xi}) = 
+            \\begin{bmatrix}
+                \\mathbf{J} & \\mathbf{Q} \\\\
+                \\mathbf{0} & \\mathbf{J}
+            \\end{bmatrix}
+
+        with :math:`\\mathbf{J}` as in :meth:`liegroups.SO3.left_jacobian` and :math:`\\mathbf{Q}` as in :meth:`~liegroups.SE3.left_jacobian_Q_matrix`.
+        """
+        rho = xi[0:3]  # translation part
+        phi = xi[3:6]  # rotation part
+
+        # Near |phi|==0, use first order Taylor expansion
+        if np.isclose(np.linalg.norm(phi), 0.):
+            return np.identity(cls.dof) + 0.5 * cls.curlywedge(xi)
+
+        so3_jac = cls.RotationType.left_jacobian(phi)
+        Q_mat = cls.left_jacobian_Q_matrix(xi)
+
+        jac = np.zeros([cls.dof, cls.dof])
+        jac[0:3, 0:3] = so3_jac
+        jac[0:3, 3:6] = Q_mat
+        jac[3:6, 3:6] = so3_jac
+
+        return jac
+
+    def log(self):
+        """Logarithmic map for :math:`SE(3)`, which computes a tangent vector from a transformation:
+
+        .. math::
+            \\boldsymbol{\\xi}(\\mathbf{T}) =
+            \\ln(\\mathbf{T})^\\vee =
+            \\begin{bmatrix}
+                \\mathbf{J} ^ {-1} \\mathbf{r} \\\\
+                \\ln(\\boldsymbol{C}) ^\\vee
+            \\end{bmatrix}
+
+        This is the inverse operation to :meth:`~liegroups.SE3.exp`.
+        """
+        phi = self.RotationType.log(self.rot)
+        rho = self.RotationType.inv_left_jacobian(phi).dot(self.trans)
+        return np.hstack([rho, phi])
+
+    @classmethod
+    def odot(cls, p, directional=False):
+        """:math:`SE(3)` odot operator as defined by Barfoot.
+
+        This is the Jacobian of a vector
+
+        .. math::
+            \\mathbf{p} =
+            \\begin{bmatrix}
+                sx \\\\ sy \\\\ sz \\\\ s
+            \\end{bmatrix} =
+            \\begin{bmatrix}
+                \\boldsymbol{\\epsilon} \\\\ \\eta
+            \\end{bmatrix}
+
+        with respect to a perturbation in the underlying parameters of :math:`\\mathbf{T}`.
+
+        If :math:`\\mathbf{p}` is given in Euclidean coordinates and directional=False, the missing scale value :math:`\\eta` is assumed to be 1 and the Jacobian is 3x6. If directional=True, :math:`\\eta` is assumed to be 0:
+
+        .. math::
+            \\mathbf{p}^\\odot =
+            \\begin{bmatrix}
+                \\eta \\mathbf{1} & -\\boldsymbol{\\epsilon}^\\wedge
+            \\end{bmatrix}
+
+        If :math:`\\mathbf{p}` is given in Homogeneous coordinates, the Jacobian is 4x6:
+
+        .. math::
+            \\mathbf{p}^\\odot =
+            \\begin{bmatrix}
+                \\eta \\mathbf{1} & -\\boldsymbol{\\epsilon}^\\wedge \\\\
+                \\mathbf{0}^T & \\mathbf{0}^T
+            \\end{bmatrix}
+        """
+        p = np.atleast_2d(p)
+        result = np.zeros([p.shape[0], p.shape[1], cls.dof])
+
+        if p.shape[1] == cls.dim - 1:
+            # Assume scale parameter is 1 unless p is a direction
+            # ptor, in which case the scale is 0
+            if not directional:
+                result[:, 0:3, 0:3] = np.eye(3)
+
+            result[:, 0:3, 3:6] = cls.RotationType.wedge(-p)
+
+        elif p.shape[1] == cls.dim:
+            # Broadcast magic
+            result[:, 0:3, 0:3] = p[:, 3][:, None, None] * np.eye(3)
+            result[:, 0:3, 3:6] = cls.RotationType.wedge(-p[:, 0:3])
+
+        else:
+            raise ValueError("p must have shape ({},), ({},), (N,{}) or (N,{})".format(
+                cls.dim - 1, cls.dim, cls.dim - 1, cls.dim))
+
+        return np.squeeze(result)
+
+    @classmethod
+    def vee(cls, Xi):
+        """:math:`SE(3)` vee operator as defined by Barfoot.
+
+        .. math::
+            \\boldsymbol{\\xi} = \\boldsymbol{\\Xi} ^\\vee
+
+        This is the inverse operation to :meth:`~liegroups.SE3.wedge`.
+        """
+        if Xi.ndim < 3:
+            Xi = np.expand_dims(Xi, axis=0)
+
+        if Xi.shape[1:3] != (cls.dim, cls.dim):
+            raise ValueError("Xi must have shape ({},{}) or (N,{},{})".format(
+                cls.dim, cls.dim, cls.dim, cls.dim))
+
+        xi = np.empty([Xi.shape[0], cls.dof])
+        xi[:, 0:3] = Xi[:, 0:3, 3]
+        xi[:, 3:6] = cls.RotationType.vee(Xi[:, 0:3, 0:3])
+        return np.squeeze(xi)
+
+    @classmethod
+    def wedge(cls, xi):
+        """:math:`SE(3)` wedge operator as defined by Barfoot.
+
+        .. math::
+            \\boldsymbol{\\Xi} =
+            \\boldsymbol{\\xi} ^\\wedge =
+            \\begin{bmatrix}
+                \\boldsymbol{\\phi} ^\\wedge & \\boldsymbol{\\rho} \\\\
+                \\mathbf{0} ^ T & 0
+            \\end{bmatrix}
+
+        This is the inverse operation to :meth:`~liegroups.SE2.vee`.
+        """
+        xi = np.atleast_2d(xi)
+        if xi.shape[1] != cls.dof:
+            raise ValueError(
+                "xi must have shape ({},) or (N,{})".format(cls.dof, cls.dof))
+
+        Xi = np.zeros([xi.shape[0], cls.dim, cls.dim])
+        Xi[:, 0:3, 0:3] = cls.RotationType.wedge(xi[:, 3:6])
+        Xi[:, 0:3, 3] = xi[:, 0:3]
+        return np.squeeze(Xi)

liegroups/build/lib/liegroups/numpy/so2.py

@@ -0,0 +1,161 @@
+import numpy as np
+
+from . import _base
+
+
+class SO2Matrix(_base.SOMatrixBase):
+    """Rotation matrix in :math:`SO(2)` using active (alibi) transformations.
+
+    .. math::
+        SO(2) &= \\left\\{ \\mathbf{C} \\in \\mathbb{R}^{2 \\times 2} ~\\middle|~ \\mathbf{C}\\mathbf{C}^T = \\mathbf{1}, \\det \\mathbf{C} = 1 \\right\\} \\\\
+        \\mathfrak{so}(2) &= \\left\\{ \\boldsymbol{\\Phi} = \\phi^\\wedge \\in \\mathbb{R}^{2 \\times 2} ~\\middle|~ \\phi \\in \\mathbb{R} \\right\\}
+
+    :cvar ~liegroups.SO2.dim: Dimension of the rotation matrix.
+    :cvar ~liegroups.SO2.dof: Underlying degrees of freedom (i.e., dimension of the tangent space).
+    :ivar mat: Storage for the rotation matrix :math:`\\mathbf{C}`.
+    """
+    dim = 2
+    """Dimension of the transformation matrix."""
+    dof = 1
+    """Underlying degrees of freedom (i.e., dimension of the tangent space)."""
+
+    def adjoint(self):
+        """Adjoint matrix of the transformation.
+
+        .. math::
+            \\text{Ad}(\\mathbf{C}) = 1
+        """
+        return 1.
+
+    @classmethod
+    def exp(cls, phi):
+        """Exponential map for :math:`SO(2)`, which computes a transformation from a tangent vector:
+
+        .. math::
+            \\mathbf{C}(\\phi) = 
+            \\exp(\\phi^\\wedge) =
+            \\cos \\phi \\mathbf{1} + \\sin \\phi 1^\\wedge = 
+            \\begin{bmatrix}
+                \\cos \\phi  & -\\sin \\phi  \\\\
+                \\sin \\phi & \\cos \\phi
+            \\end{bmatrix}
+
+        This is the inverse operation to :meth:`~liegroups.SO2.log`.
+        """
+        c = np.cos(phi)
+        s = np.sin(phi)
+
+        return cls(np.array([[c, -s],
+                             [s,  c]]))
+
+    @classmethod
+    def from_angle(cls, angle_in_radians):
+        """Form a rotation matrix given an angle in radians.
+
+        See :meth:`~liegroups.SO2.exp`
+        """
+        return cls.exp(angle_in_radians)
+
+    @classmethod
+    def inv_left_jacobian(cls, phi):
+        """:math:`SO(2)` inverse left Jacobian.
+
+        .. math::
+            \\mathbf{J}^{-1}(\\phi) = 
+            \\begin{cases}
+                \\mathbf{1} - \\frac{1}{2} \\phi^\\wedge, & \\text{if } \\phi \\text{ is small} \\\\
+                \\frac{\\phi}{2} \\cot \\frac{\\phi}{2} \\mathbf{1} -
+                \\frac{\\phi}{2} 1^\\wedge, & \\text{otherwise}
+            \\end{cases}
+        """
+        # Near phi==0, use first order Taylor expansion
+        if np.isclose(phi, 0.):
+            return np.identity(cls.dim) - 0.5 * cls.wedge(phi)
+
+        half_angle = 0.5 * phi
+        cot_half_angle = 1. / np.tan(half_angle)
+        return half_angle * cot_half_angle * np.identity(cls.dim) - \
+            half_angle * cls.wedge(1.)
+
+    @classmethod
+    def left_jacobian(cls, phi):
+        """:math:`SO(2)` left Jacobian.
+
+        .. math::
+            \\mathbf{J}(\\phi) = 
+            \\begin{cases}
+                \\mathbf{1} + \\frac{1}{2} \\phi^\\wedge, & \\text{if } \\phi \\text{ is small} \\\\
+                \\frac{\\sin \\phi}{\\phi} \\mathbf{1} - 
+                \\frac{1 - \\cos \\phi}{\\phi} 1^\\wedge, & \\text{otherwise}
+            \\end{cases}
+        """
+        # Near phi==0, use first order Taylor expansion
+        if np.isclose(phi, 0.):
+            return np.identity(cls.dim) + 0.5 * cls.wedge(phi)
+
+        s = np.sin(phi)
+        c = np.cos(phi)
+
+        return (s / phi) * np.identity(cls.dim) + \
+            ((1 - c) / phi) * cls.wedge(1.)
+
+    def log(self):
+        """Logarithmic map for :math:`SO(2)`, which computes a tangent vector from a transformation:
+
+        .. math::
+            \\phi(\\mathbf{C}) = 
+            \\ln(\\mathbf{C})^\\vee =
+            \\text{atan2}(C_{1,0}, C_{0,0})
+
+        This is the inverse operation to :meth:`~liegroups.SO2.exp`.
+        """
+        c = self.mat[0, 0]
+        s = self.mat[1, 0]
+        return np.arctan2(s, c)
+
+    def to_angle(self):
+        """Recover the rotation angle in radians from the rotation matrix.
+
+        See :meth:`~liegroups.SO2.log`
+        """
+        return self.log()
+
+    @classmethod
+    def vee(cls, Phi):
+        """:math:`SO(2)` vee operator as defined by Barfoot.
+
+        .. math::
+            \\phi = \\boldsymbol{\\Phi}^\\vee
+
+        This is the inverse operation to :meth:`~liegroups.SO2.wedge`.
+        """
+        if Phi.ndim < 3:
+            Phi = np.expand_dims(Phi, axis=0)
+
+        if Phi.shape[1:3] != (cls.dim, cls.dim):
+            raise ValueError(
+                "Phi must have shape ({},{}) or (N,{},{})".format(
+                    cls.dim, cls.dim, cls.dim, cls.dim))
+
+        return np.squeeze(Phi[:, 1, 0])
+
+    @classmethod
+    def wedge(cls, phi):
+        """:math:`SO(2)` wedge operator as defined by Barfoot.
+
+        .. math::
+            \\boldsymbol{\\Phi} = 
+            \\phi^\\wedge = 
+            \\begin{bmatrix} 
+                0 & -\\phi \\\\
+                \\phi & 0 
+            \\end{bmatrix}
+
+        This is the inverse operation to :meth:`~liegroups.SO2.vee`.
+        """
+        phi = np.atleast_1d(phi)
+
+        Phi = np.zeros([len(phi), cls.dim, cls.dim])
+        Phi[:, 0, 1] = -phi
+        Phi[:, 1, 0] = phi
+        return np.squeeze(Phi)

liegroups/build/lib/liegroups/numpy/so3.py

@@ -0,0 +1,430 @@
+import numpy as np
+
+from . import _base
+
+
+class SO3Matrix(_base.SOMatrixBase):
+    """Rotation matrix in :math:`SO(3)` using active (alibi) transformations.
+
+    .. math::
+        SO(3) &= \\left\\{ \\mathbf{C} \\in \\mathbb{R}^{3 \\times 3} ~\\middle|~ \\mathbf{C}\\mathbf{C}^T = \\mathbf{1}, \\det \\mathbf{C} = 1 \\right\\} \\\\
+        \\mathfrak{so}(3) &= \\left\\{ \\boldsymbol{\\Phi} = \\boldsymbol{\\phi}^\\wedge \\in \\mathbb{R}^{3 \\times 3} ~\\middle|~ \\boldsymbol{\\phi} = \\phi \\mathbf{a} \\in \\mathbb{R}^3, \\phi = \\Vert \\boldsymbol{\\phi} \\Vert \\right\\}
+
+    :cvar ~liegroups.SO3.dim: Dimension of the rotation matrix.
+    :cvar ~liegroups.SO3.dof: Underlying degrees of freedom (i.e., dimension of the tangent space).
+    :ivar mat: Storage for the rotation matrix :math:`\\mathbf{C}`.
+    """
+    dim = 3
+    """Dimension of the transformation matrix."""
+    dof = 3
+    """Underlying degrees of freedom (i.e., dimension of the tangent space)."""
+
+    def adjoint(self):
+        """Adjoint matrix of the transformation.
+
+        .. math::
+            \\text{Ad}(\\mathbf{C}) = \\mathbf{C}
+            \\in \\mathbb{R}^{3 \\times 3}
+        """
+        return self.mat
+
+    @classmethod
+    def exp(cls, phi):
+        """Exponential map for :math:`SO(3)`, which computes a transformation from a tangent vector:
+
+        .. math::
+            \\mathbf{C}(\\boldsymbol{\\phi}) =
+            \\exp(\\boldsymbol{\\phi}^\\wedge) =
+            \\begin{cases}
+                \\mathbf{1} + \\boldsymbol{\\phi}^\\wedge, & \\text{if } \\phi \\text{ is small} \\\\
+                \\cos \\phi \\mathbf{1} +
+                (1 - \\cos \\phi) \\mathbf{a}\\mathbf{a}^T +
+                \\sin \\phi \\mathbf{a}^\\wedge, & \\text{otherwise}
+            \\end{cases}
+
+        This is the inverse operation to :meth:`~liegroups.SO3.log`.
+        """
+        if len(phi) != cls.dof:
+            raise ValueError("phi must have length 3")
+
+        angle = np.linalg.norm(phi)
+
+        # Near phi==0, use first order Taylor expansion
+        if np.isclose(angle, 0.):
+            return cls(np.identity(cls.dim) + cls.wedge(phi))
+
+        axis = phi / angle
+        s = np.sin(angle)
+        c = np.cos(angle)
+
+        return cls(c * np.identity(cls.dim) +
+                   (1 - c) * np.outer(axis, axis) +
+                   s * cls.wedge(axis))
+
+    @classmethod
+    def from_quaternion(cls, quat, ordering='wxyz'):
+        """Form a rotation matrix from a unit length quaternion.
+
+        Valid orderings are 'xyzw' and 'wxyz'.
+
+        .. math::
+            \\mathbf{C} = 
+            \\begin{bmatrix}
+                1 - 2 (y^2 + z^2) & 2 (xy - wz) & 2 (wy + xz) \\\\
+                2 (wz + xy) & 1 - 2 (x^2 + z^2) & 2 (yz - wx) \\\\
+                2 (xz - wy) & 2 (wx + yz) & 1 - 2 (x^2 + y^2)
+            \\end{bmatrix}
+        """
+        if not np.isclose(np.linalg.norm(quat), 1.):
+            raise ValueError("Quaternion must be unit length")
+
+        if ordering is 'xyzw':
+            qx, qy, qz, qw = quat
+        elif ordering is 'wxyz':
+            qw, qx, qy, qz = quat
+        else:
+            raise ValueError(
+                "Valid orderings are 'xyzw' and 'wxyz'. Got '{}'.".format(ordering))
+
+        # Form the matrix
+        qw2 = qw * qw
+        qx2 = qx * qx
+        qy2 = qy * qy
+        qz2 = qz * qz
+
+        R00 = 1. - 2. * (qy2 + qz2)
+        R01 = 2. * (qx * qy - qw * qz)
+        R02 = 2. * (qw * qy + qx * qz)
+
+        R10 = 2. * (qw * qz + qx * qy)
+        R11 = 1. - 2. * (qx2 + qz2)
+        R12 = 2. * (qy * qz - qw * qx)
+
+        R20 = 2. * (qx * qz - qw * qy)
+        R21 = 2. * (qw * qx + qy * qz)
+        R22 = 1. - 2. * (qx2 + qy2)
+
+        return cls(np.array([[R00, R01, R02],
+                             [R10, R11, R12],
+                             [R20, R21, R22]]))
+
+    @classmethod
+    def from_rpy(cls, roll, pitch, yaw):
+        """Form a rotation matrix from RPY Euler angles :math:`(\\alpha, \\beta, \\gamma)`.
+
+        .. math::
+            \\mathbf{C} = \\mathbf{C}_z(\\gamma) \\mathbf{C}_y(\\beta) \\mathbf{C}_x(\\alpha)
+        """
+        return cls.rotz(yaw).dot(cls.roty(pitch).dot(cls.rotx(roll)))
+
+    @classmethod
+    def inv_left_jacobian(cls, phi):
+        """:math:`SO(3)` inverse left Jacobian.
+
+        .. math::
+            \\mathbf{J}^{-1}(\\boldsymbol{\\phi}) =
+            \\begin{cases}
+                \\mathbf{1} - \\frac{1}{2} \\boldsymbol{\\phi}^\\wedge, & \\text{if } \\phi \\text{ is small} \\\\
+                \\frac{\\phi}{2} \\cot \\frac{\\phi}{2} \\mathbf{1} +
+                \\left( 1 - \\frac{\\phi}{2} \\cot \\frac{\\phi}{2} \\right) \\mathbf{a}\\mathbf{a}^T -
+                \\frac{\\phi}{2} \\mathbf{a}^\\wedge, & \\text{otherwise}
+            \\end{cases}
+        """
+        if len(phi) != cls.dof:
+            raise ValueError("phi must have length 3")
+
+        angle = np.linalg.norm(phi)
+
+        # Near phi==0, use first order Taylor expansion
+        if np.isclose(angle, 0.):
+            return np.identity(cls.dof) - 0.5 * cls.wedge(phi)
+
+        axis = phi / angle
+        half_angle = 0.5 * angle
+        cot_half_angle = 1. / np.tan(half_angle)
+
+        return half_angle * cot_half_angle * np.identity(cls.dof) + \
+            (1 - half_angle * cot_half_angle) * np.outer(axis, axis) - \
+            half_angle * cls.wedge(axis)
+
+    @classmethod
+    def left_jacobian(cls, phi):
+        """:math:`SO(3)` left Jacobian.
+
+        .. math::
+            \\mathbf{J}(\\boldsymbol{\\phi}) =
+            \\begin{cases}
+                \\mathbf{1} + \\frac{1}{2} \\boldsymbol{\\phi}^\\wedge, & \\text{if } \\phi \\text{ is small} \\\\
+                \\frac{\\sin \\phi}{\\phi} \\mathbf{1} +
+                \\left(1 - \\frac{\\sin \\phi}{\\phi} \\right) \\mathbf{a}\\mathbf{a}^T +
+                \\frac{1 - \\cos \\phi}{\\phi} \\mathbf{a}^\\wedge, & \\text{otherwise}
+            \\end{cases}
+        """
+        if len(phi) != cls.dof:
+            raise ValueError("phi must have length 3")
+
+        angle = np.linalg.norm(phi)
+
+        # Near |phi|==0, use first order Taylor expansion
+        if np.isclose(angle, 0.):
+            return np.identity(cls.dof) + 0.5 * cls.wedge(phi)
+
+        axis = phi / angle
+        s = np.sin(angle)
+        c = np.cos(angle)
+
+        return (s / angle) * np.identity(cls.dof) + \
+            (1 - s / angle) * np.outer(axis, axis) + \
+            ((1 - c) / angle) * cls.wedge(axis)
+
+    def log(self):
+        """Logarithmic map for :math:`SO(3)`, which computes a tangent vector from a transformation:
+
+        .. math::
+            \\phi &= \\frac{1}{2} \\left( \\mathrm{Tr}(\\mathbf{C}) - 1 \\right) \\\\
+            \\boldsymbol{\\phi}(\\mathbf{C}) &= 
+            \\ln(\\mathbf{C})^\\vee =
+            \\begin{cases}
+                \\mathbf{1} - \\boldsymbol{\\phi}^\\wedge, & \\text{if } \\phi \\text{ is small} \\\\
+                \\left( \\frac{1}{2} \\frac{\\phi}{\\sin \\phi} \\left( \\mathbf{C} - \\mathbf{C}^T \\right) \\right)^\\vee, & \\text{otherwise}
+            \\end{cases}
+
+        This is the inverse operation to :meth:`~liegroups.SO3.log`.
+        """
+        # The cosine of the rotation angle is related to the trace of C
+        cos_angle = 0.5 * np.trace(self.mat) - 0.5
+        # Clip cos(angle) to its proper domain to avoid NaNs from rounding errors
+        cos_angle = np.clip(cos_angle, -1., 1.)
+        angle = np.arccos(cos_angle)
+
+        # If angle is close to zero, use first-order Taylor expansion
+        if np.isclose(angle, 0.):
+            return self.vee(self.mat - np.identity(3))
+
+        # Otherwise take the matrix logarithm and return the rotation vector
+        return self.vee((0.5 * angle / np.sin(angle)) * (self.mat - self.mat.T))
+
+    @classmethod
+    def rotx(cls, angle_in_radians):
+        """Form a rotation matrix given an angle in rad about the x-axis.
+
+        .. math::
+            \\mathbf{C}_x(\\phi) = 
+            \\begin{bmatrix}
+                1 & 0 & 0 \\\\
+                0 & \\cos \\phi & -\\sin \\phi \\\\
+                0 & \\sin \\phi & \\cos \\phi
+            \\end{bmatrix}
+        """
+        c = np.cos(angle_in_radians)
+        s = np.sin(angle_in_radians)
+
+        return cls(np.array([[1., 0., 0.],
+                             [0., c, -s],
+                             [0., s,  c]]))
+
+    @classmethod
+    def roty(cls, angle_in_radians):
+        """Form a rotation matrix given an angle in rad about the y-axis.
+
+        .. math::
+            \\mathbf{C}_y(\\phi) = 
+            \\begin{bmatrix}
+                \\cos \\phi & 0 & \\sin \\phi \\\\
+                0 & 1 & 0 \\\\
+                \\sin \\phi & 0 & \\cos \\phi
+            \\end{bmatrix}
+        """
+        c = np.cos(angle_in_radians)
+        s = np.sin(angle_in_radians)
+
+        return cls(np.array([[c,  0., s],
+                             [0., 1., 0.],
+                             [-s, 0., c]]))
+
+    @classmethod
+    def rotz(cls, angle_in_radians):
+        """Form a rotation matrix given an angle in rad about the z-axis.
+
+        .. math::
+            \\mathbf{C}_z(\\phi) = 
+            \\begin{bmatrix}
+                \\cos \\phi & -\\sin \\phi & 0 \\\\
+                \\sin \\phi  & \\cos \\phi & 0 \\\\
+                0 & 0 & 1
+            \\end{bmatrix}
+        """
+        c = np.cos(angle_in_radians)
+        s = np.sin(angle_in_radians)
+
+        return cls(np.array([[c, -s,  0.],
+                             [s,  c,  0.],
+                             [0., 0., 1.]]))
+
+    def to_quaternion(self, ordering='wxyz'):
+        """Convert a rotation matrix to a unit length quaternion.
+
+           Valid orderings are 'xyzw' and 'wxyz'.
+        """
+        R = self.mat
+        qw = 0.5 * np.sqrt(1. + R[0, 0] + R[1, 1] + R[2, 2])
+
+        if np.isclose(qw, 0.):
+            if R[0, 0] > R[1, 1] and R[0, 0] > R[2, 2]:
+                d = 2. * np.sqrt(1. + R[0, 0] - R[1, 1] - R[2, 2])
+                qw = (R[2, 1] - R[1, 2]) / d
+                qx = 0.25 * d
+                qy = (R[1, 0] + R[0, 1]) / d
+                qz = (R[0, 2] + R[2, 0]) / d
+            elif R[1, 1] > R[2, 2]:
+                d = 2. * np.sqrt(1. + R[1, 1] - R[0, 0] - R[2, 2])
+                qw = (R[0, 2] - R[2, 0]) / d
+                qx = (R[1, 0] + R[0, 1]) / d
+                qy = 0.25 * d
+                qz = (R[2, 1] + R[1, 2]) / d
+            else:
+                d = 2. * np.sqrt(1. + R[2, 2] - R[0, 0] - R[1, 1])
+                qw = (R[1, 0] - R[0, 1]) / d
+                qx = (R[0, 2] + R[2, 0]) / d
+                qy = (R[2, 1] + R[1, 2]) / d
+                qz = 0.25 * d
+        else:
+            d = 4. * qw
+            qx = (R[2, 1] - R[1, 2]) / d
+            qy = (R[0, 2] - R[2, 0]) / d
+            qz = (R[1, 0] - R[0, 1]) / d
+
+        # Check ordering last
+        if ordering is 'xyzw':
+            quat = np.array([qx, qy, qz, qw])
+        elif ordering is 'wxyz':
+            quat = np.array([qw, qx, qy, qz])
+        else:
+            raise ValueError(
+                "Valid orderings are 'xyzw' and 'wxyz'. Got '{}'.".format(ordering))
+
+        return quat
+
+    def to_rpy(self):
+        """Convert a rotation matrix to RPY Euler angles :math:`(\\alpha, \\beta, \\gamma)`."""
+        pitch = np.arctan2(-self.mat[2, 0],
+                           np.sqrt(self.mat[0, 0]**2 + self.mat[1, 0]**2))
+
+        if np.isclose(pitch, np.pi / 2.):
+            yaw = 0.
+            roll = np.arctan2(self.mat[0, 1], self.mat[1, 1])
+        elif np.isclose(pitch, -np.pi / 2.):
+            yaw = 0.
+            roll = -np.arctan2(self.mat[0, 1], self.mat[1, 1])
+        else:
+            sec_pitch = 1. / np.cos(pitch)
+            yaw = np.arctan2(self.mat[1, 0] * sec_pitch,
+                             self.mat[0, 0] * sec_pitch)
+            roll = np.arctan2(self.mat[2, 1] * sec_pitch,
+                              self.mat[2, 2] * sec_pitch)
+
+        return roll, pitch, yaw
+
+    @classmethod
+    def vee(cls, Phi):
+        """:math:`SO(3)` vee operator as defined by Barfoot.
+
+        .. math::
+            \\phi = \\boldsymbol{\\Phi}^\\vee
+
+        This is the inverse operation to :meth:`~liegroups.SO3.wedge`.
+        """
+        if Phi.ndim < 3:
+            Phi = np.expand_dims(Phi, axis=0)
+
+        if Phi.shape[1:3] != (cls.dim, cls.dim):
+            raise ValueError("Phi must have shape ({},{}) or (N,{},{})".format(
+                cls.dim, cls.dim, cls.dim, cls.dim))
+
+        phi = np.empty([Phi.shape[0], cls.dim])
+        phi[:, 0] = Phi[:, 2, 1]
+        phi[:, 1] = Phi[:, 0, 2]
+        phi[:, 2] = Phi[:, 1, 0]
+        return np.squeeze(phi)
+
+    @classmethod
+    def wedge(cls, phi):
+        """:math:`SO(3)` wedge operator as defined by Barfoot.
+
+        .. math::
+            \\boldsymbol{\\Phi} =
+            \\boldsymbol{\\phi}^\\wedge =
+            \\begin{bmatrix}
+                0 & -\\phi_3 & \\phi_2 \\\\
+                \\phi_3 & 0 & -\\phi_1 \\\\
+                -\\phi_2 & \\phi_1 & 0
+            \\end{bmatrix}
+
+        This is the inverse operation to :meth:`~liegroups.SO3.vee`.
+        """
+        phi = np.atleast_2d(phi)
+        if phi.shape[1] != cls.dof:
+            raise ValueError(
+                "phi must have shape ({},) or (N,{})".format(cls.dof, cls.dof))
+
+        Phi = np.zeros([phi.shape[0], cls.dim, cls.dim])
+        Phi[:, 0, 1] = -phi[:, 2]
+        Phi[:, 1, 0] = phi[:, 2]
+        Phi[:, 0, 2] = phi[:, 1]
+        Phi[:, 2, 0] = -phi[:, 1]
+        Phi[:, 1, 2] = -phi[:, 0]
+        Phi[:, 2, 1] = phi[:, 0]
+        return np.squeeze(Phi)
+
+
+class SO3Quaternion(_base.VectorLieGroupBase):
+    """Rotation in SO(3) using unit-length quaternions (wxyz ordering)."""
+
+    dim = 4
+    dof = 3
+
+    def from_array(self, arr, ordering='wxyz'):
+        if ordering is 'xyzw':
+            self.data = arr[[3, 0, 1, 2]]
+        elif ordering is 'wxyz':
+            self.data = arr
+        else:
+            raise ValueError(
+                "Valid orderings are 'xyzw' and 'wxyz'. Got '{}'.".format(ordering))
+
+    def dot(self, other):
+        """Multiply another rotation or one or more vectors on the left.
+        """
+        if isinstance(other, self.__class__):
+            # Compound with another rotation
+            pv = p[1:]
+            qv = q[1:]
+
+            r = np.hstack([p[0]*q[0] - np.dot(pv, qv),
+                           p[0]*qv + q[0]*pv + np.dot(skew(pv), qv)])
+            return 0
+        else:
+            other = np.atleast_2d(other)
+
+            # Transform one or more 2-vectors or fail
+            if other.shape[1] == self.dim:
+                return 0
+            else:
+                raise ValueError(
+                    "Vector must have shape ({},) or (N,{})".format(self.dim, self.dim))
+
+    @classmethod
+    def identity(cls):
+        """Return the identity rotation."""
+        return cls(np.array([1, 0, 0, 0]))
+
+    def inv(self):
+        """Return the inverse rotation:
+
+        .. math::
+            \\mathbf{C}^{-1} = \\mathbf{C}^T
+        """
+        inv = self.conjugate()
+        inv.data = inv.data / np.dot(inv.data, inv.data)
+
+        return inv

liegroups/build/lib/liegroups/torch/__init__.py

@@ -0,0 +1,9 @@
+"""PyTorch implementations of Special Euclidean and Special Orthogonal Lie groups."""
+
+from .so2 import SO2Matrix as SO2
+from .se2 import SE2Matrix as SE2
+from .so3 import SO3Matrix as SO3
+from .se3 import SE3Matrix as SE3
+
+__author__ = "Lee Clement"
+__email__ = "lee.clement@robotics.utias.utoronto.ca"

liegroups/build/lib/liegroups/torch/_base.py

@@ -0,0 +1,383 @@
+import torch
+import numpy as np  # for matrix determinant and SVD
+
+from .. import _base
+from . import utils
+
+
+class SOMatrixBase(_base.SOMatrixBase):
+    """Implementation of methods common to SO(N) matrix lie groups using PyTorch"""
+
+    def cpu(self):
+        """Return a copy with the underlying tensor on the CPU."""
+        return self.__class__(self.mat.cpu())
+
+    def cuda(self, device=None, non_blocking=False):
+        """Return a copy with the underlying tensor on the GPU."""
+        return self.__class__(self.mat.cuda(device=device, non_blocking=non_blocking))
+
+    def dot(self, other):
+        if isinstance(other, self.__class__):
+            # Compound with another rotation
+            return self.__class__(torch.matmul(self.mat, other.mat))
+        else:
+            if other.dim() < 2:
+                other = other.unsqueeze(dim=0)  # vector --> matrix
+            if other.dim() < 3:
+                other = other.unsqueeze(dim=0)  # matrix --> batch
+
+            if self.mat.dim() < 3:
+                mat = self.mat.unsqueeze(dim=0).expand(
+                    other.shape[0], self.dim, self.dim)  # matrix --> batch
+            else:
+                mat = self.mat
+                if other.shape[0] == 1:
+                    other = other.expand(
+                        mat.shape[0], other.shape[1], other.shape[2])
+
+            # Transform one or more vectors or fail
+            if other.shape[0] != mat.shape[0]:
+                raise ValueError("Expected vector-batch batch size of {}, got {}".format(
+                    mat.shape[0], other.shape[0]))
+
+            if other.shape[2] == self.dim:
+                return torch.bmm(mat, other.transpose(2, 1)).transpose_(2, 1).squeeze_()
+            else:
+                raise ValueError(
+                    "Vector or vector-batch must have shape ({},), (N,{}), or ({},N,{})".format(self.dim, self.dim, mat.shape[0], self.dim))
+
+    @classmethod
+    def from_matrix(cls, mat, normalize=False):
+
+        mat_is_valid = cls.is_valid_matrix(mat)
+
+        if mat_is_valid.all() or normalize:
+            result = cls(mat)
+
+            if normalize:
+                result.normalize(inds=mat_is_valid.logical_not().nonzero(as_tuple=False))
+
+            return result
+        else:
+            raise ValueError(
+                "Invalid rotation matrix. Use normalize=True to handle rounding errors.")
+
+    @classmethod
+    def from_numpy(cls, other, pin_memory=False):
+        """Create a torch-based copy of a numpy-based rotation."""
+        mat = torch.Tensor(other.mat)
+        if pin_memory:
+            mat = mat.pin_memory()
+
+        return cls(mat)
+
+    @classmethod
+    def identity(cls, batch_size=1, copy=False):
+        if copy:
+            mat = torch.eye(cls.dim).repeat(batch_size, 1, 1)
+        else:
+            mat = torch.eye(cls.dim).expand(
+                batch_size, cls.dim, cls.dim).squeeze()
+        return cls(mat)
+
+    def inv(self):
+        if self.mat.dim() < 3:
+            return self.__class__(self.mat.transpose(1, 0))
+        else:
+            return self.__class__(self.mat.transpose(2, 1))
+
+    def is_cuda(self):
+        """Returns true if the underlying tensor is a CUDA tensor"""
+        return self.mat.is_cuda
+
+    def is_pinned(self):
+        """Returns true if the underlying tensor resides in pinned memory"""
+        return self.mat.is_pinned()
+
+    @classmethod
+    def is_valid_matrix(cls, mat):
+        if mat.dim() < 3:
+            mat = mat.unsqueeze(dim=0)
+
+        # Check the shape
+        if mat.is_cuda:
+            shape_check = torch.cuda.BoolTensor(mat.shape[0]).fill_(False)
+        else:
+            shape_check = torch.BoolTensor(mat.shape[0]).fill_(False)
+
+        if mat.shape[1:3] != (cls.dim, cls.dim):
+            return shape_check
+        else:
+            shape_check.fill_(True)
+
+        # Determinants of each matrix in the batch should be 1
+        det_check = utils.isclose(mat.__class__(
+            np.linalg.det(mat.detach().cpu().numpy())), 1.)
+
+        # The transpose of each matrix in the batch should be its inverse
+        inv_check = utils.isclose(mat.transpose(2, 1).bmm(mat),
+                                  torch.eye(cls.dim, dtype=mat.dtype)).sum(dim=1).sum(dim=1) \
+            == cls.dim * cls.dim
+
+        return shape_check & det_check & inv_check
+
+    def _normalize_one(self, mat):
+        # U, S, V = torch.svd(A) returns the singular value
+        # decomposition of a real matrix A of size (n x m) such that A=USV′.
+        # Irrespective of the original strides, the returned matrix U will
+        # be transposed, i.e. with strides (1, n) instead of (n, 1).
+
+        # pytorch has native SVD function but not determinant...
+        # U, _, V = mat.squeeze().svd()
+        # S = torch.eye(self.dim)
+        # if U.is_cuda:
+        #     S = S.cuda()
+        # S[self.dim - 1, self.dim - 1] = float(np.linalg.det(U.cpu().numpy()) *
+        #                                       np.linalg.det(V.cpu().numpy()))
+        # mat_normalized = U.mm(S).mm(V.t_())
+
+        # pytorch SVD seems to be inaccurate, so just move to numpy immediately
+        mat_cpu = mat.detach().cpu().numpy().squeeze()
+        U, _, V = np.linalg.svd(mat_cpu, full_matrices=False)
+        S = np.eye(self.dim)
+        S[self.dim - 1, self.dim - 1] = np.linalg.det(U) * np.linalg.det(V)
+
+        mat_normalized = mat.__class__(U.dot(S).dot(V))
+
+        mat.copy_(mat_normalized)
+        return mat
+
+    def normalize(self, inds=None):
+        if self.mat.dim() < 3:
+            self._normalize_one(self.mat)
+        else:
+            if inds is None:
+                inds = range(self.mat.shape[0])
+
+            for batch_ind in inds:
+                # Slicing is a copy operation?
+                self.mat[batch_ind] = self._normalize_one(self.mat[batch_ind])
+
+    def pin_memory(self):
+        """Return a copy with the underlying tensor in pinned (page-locked) memory. Makes host-to-GPU copies faster.
+
+        See: http://pytorch.org/docs/master/notes/cuda.html?highlight=pinned
+        """
+        return self.__class__(self.mat.pin_memory())
+
+
+class SEMatrixBase(_base.SEMatrixBase):
+    """Implementation of methods common to SE(N) matrix lie groups using PyTorch"""
+
+    def __init__(self, rot, trans):
+        super(SEMatrixBase, self).__init__(rot, trans)
+
+    def as_matrix(self):
+        R = self.rot.as_matrix()
+        if R.dim() < 3:
+            R = R.unsqueeze(dim=0)
+
+        if self.trans.dim() < 2:
+            t = self.trans.unsqueeze(dim=0)
+        else:
+            t = self.trans
+
+        t = t.unsqueeze(dim=2)  # N x self.dim-1 x 1
+
+        bottom_row = self.trans.new_zeros(self.dim)
+        bottom_row[-1] = 1.
+        bottom_row = bottom_row.unsqueeze_(dim=0).unsqueeze_(
+            dim=1).expand(R.shape[0], 1, self.dim)
+
+        return torch.cat([torch.cat([R, t], dim=2),
+                          bottom_row], dim=1).squeeze_()
+
+    def cpu(self):
+        """Return a copy with the underlying tensors on the CPU."""
+        return self.__class__(self.rot.cpu(), self.trans.cpu())
+
+    def cuda(self, device=None, non_blocking=False):
+        """Return a copy with the underlying tensors on the GPU."""
+        return self.__class__(self.rot.cuda(device=device, non_blocking=non_blocking),
+                              self.trans.cuda(device=device, non_blocking=non_blocking))
+
+    def dot(self, other):
+        if isinstance(other, self.__class__):
+            if other.trans.dim() == 2:
+                # vectorbatch --> matrixbatch (NxD --> Nx1xD)
+                other_trans = other.trans.unsqueeze(dim=1)
+            else:
+                # vector --> matrix (D --> 1xD)
+                other_trans = other.trans.unsqueeze(dim=0)
+
+            # Compound with another transformation
+            return self.__class__(self.rot.dot(other.rot),
+                                  self.rot.dot(other_trans) + self.trans)
+        else:
+            if other.dim() < 2:
+                other = other.unsqueeze(dim=0)  # vector --> matrix
+            if other.dim() < 3:
+                other = other.unsqueeze(dim=0)  # matrix --> batch
+
+            # Got euclidean coordinates
+            if other.shape[2] == self.dim - 1:
+                rot = self.rot.as_matrix()
+                trans = self.trans
+
+                if trans.dim() < 2:
+                    trans = trans.unsqueeze(dim=0)  # vector --> vectorbatch
+                if trans.dim() < 3:
+                    # vectorbatch --> matrixbatch
+                    trans = trans.unsqueeze(dim=1)
+
+                if rot.dim() < 3:
+                    # matrix --> batch
+                    rot = rot.unsqueeze(dim=0).expand(
+                        other.shape[0], rot.shape[0], rot.shape[1])
+                    # matrix --> batch
+                    trans = trans.expand(
+                        other.shape[0], trans.shape[1], trans.shape[2])
+                elif other.shape[0] == 1:
+                    other = other.expand(
+                        rot.shape[0], other.shape[1], other.shape[2])
+
+                # Transform one or more vectors or fail
+                if other.shape[0] != rot.shape[0]:
+                    raise ValueError(
+                        "Expected vector-batch batch size of {}, got {}".format(rot.shape[0], other.shape[0]))
+
+                # rot * other + trans
+                return torch.baddbmm(trans.transpose(2, 1), rot,
+                                     other.transpose(2, 1)
+                                     ).transpose(2, 1).squeeze_()
+
+            # Got homogeneous coordinates
+            elif other.shape[2] == self.dim:
+                mat = self.as_matrix()
+
+                if mat.dim() < 3:
+                    mat = mat.unsqueeze(dim=0).expand(
+                        other.shape[0], self.dim, self.dim)  # matrix --> batch
+                elif other.shape[0] == 1:
+                    other = other.expand(
+                        mat.shape[0], other.shape[1], other.shape[2])
+
+                if other.shape[0] != mat.shape[0]:
+                    raise ValueError(
+                        "Expected vector-batch batch size of {}, got {}".format(mat.shape[0], other.shape[0]))
+
+                return torch.bmm(mat, other.transpose(2, 1)
+                                 ).transpose(2, 1).squeeze_()
+
+            # Got wrong dimension
+            else:
+                if self.trans.dim() < 2:
+                    batch_size = 1
+                else:
+                    batch_size = self.trans.shape[0]
+
+                raise ValueError(
+                    "Vector or vector-batch must have shape ({},), ({},), (N,{}), (N,{}), ({},N,{}), or ({},N,{})".format(self.dim - 1, self.dim, self.dim - 1, self.dim, batch_size, self.dim - 1, batch_size, self.dim))
+
+    @classmethod
+    def from_matrix(cls, mat, normalize=False):
+        if mat.dim() < 3:
+            mat = mat.unsqueeze(dim=0)
+
+        mat_is_valid = cls.is_valid_matrix(mat)
+
+        if mat_is_valid.all() or normalize:
+            rot = mat[:, 0:cls.dim - 1, 0:cls.dim - 1].squeeze()
+            trans = mat[:, 0:cls.dim - 1, cls.dim - 1].squeeze()
+            result = cls(cls.RotationType(rot), trans)
+
+            if normalize:
+                result.normalize(inds=mat_is_valid.logical_not().nonzero(as_tuple=False))
+
+            return result
+        else:
+            raise ValueError(
+                "Invalid transformation matrix. Use normalize=True to handle rounding errors.")
+
+    @classmethod
+    def from_numpy(cls, other, pin_memory=False):
+        """Create a torch-based copy of a numpy-based transformation."""
+        rot = cls.RotationType.from_numpy(other.rot, pin_memory)
+
+        trans = torch.Tensor(other.trans)
+        if pin_memory:
+            trans = torch.Tensor(other.trans).pin_memory
+
+        return cls(rot, trans)
+
+    @classmethod
+    def identity(cls, batch_size=1, copy=False):
+        if copy:
+            mat = torch.eye(cls.dim).repeat(batch_size, 1, 1)
+        else:
+            mat = torch.eye(cls.dim).expand(batch_size, cls.dim, cls.dim)
+
+        return cls.from_matrix(mat.squeeze_())
+
+    def inv(self):
+        if self.trans.dim() == 2:
+            # vectorbatch --> matrixbatch (NxD --> Nx1xD)
+            trans = self.trans.unsqueeze(dim=1)
+        else:
+            # vector --> matrix (D --> 1xD)
+            trans = self.trans.unsqueeze(dim=0)
+
+        inv_rot = self.rot.inv()
+        inv_trans = -(inv_rot.dot(trans))
+        return self.__class__(inv_rot, inv_trans)
+
+    def is_cuda(self):
+        """Returns true if the underlying tensors are CUDA tensors"""
+        return self.rot.is_cuda()
+
+    def is_pinned(self):
+        """Returns true if the underlying tensors reside in pinned memory"""
+        return self.rot.is_pinned()
+
+    @classmethod
+    def is_valid_matrix(cls, mat):
+        if mat.dim() < 3:
+            mat = mat.unsqueeze(dim=0)
+
+        # Check the shape
+        if mat.is_cuda:
+            shape_check = torch.cuda.BoolTensor(mat.shape[0]).fill_(False)
+        else:
+            shape_check = torch.BoolTensor(mat.shape[0]).fill_(False)
+
+        if mat.shape[1:3] != (cls.dim, cls.dim):
+            return shape_check
+        else:
+            shape_check.fill_(True)
+
+        # Bottom row should be [zeros, 1]
+        bottom_row = mat.new_zeros(cls.dim)
+        bottom_row[-1] = 1.
+        bottom_check = (mat[:, cls.dim - 1, :] == bottom_row.unsqueeze_(
+            dim=0).expand(mat.shape[0], cls.dim)).sum(dim=1) == cls.dim
+
+        # Check that the rotation part is valid
+        rot_check = cls.RotationType.is_valid_matrix(
+            mat[:, 0:cls.dim - 1, 0:cls.dim - 1])
+
+        return shape_check & bottom_check & rot_check
+
+    def normalize(self, inds=None):
+        self.rot.normalize(inds)
+
+    def pin_memory(self):
+        """Return a copy with the underlying tensor in pinned (page-locked) memory. Makes host-to-GPU copies faster.
+
+        See: http://pytorch.org/docs/master/notes/cuda.html?highlight=pinned
+        """
+        return self.__class__(self.rot.pin_memory(), self.trans.pin_memory())
+
+
+class VectorLieGroupBase(_base.VectorLieGroupBase):
+    """Implementation of methods common to vector-parametrized lie groups using PyTorch"""
+    pass

liegroups/build/lib/liegroups/torch/se2.py

@@ -0,0 +1,160 @@
+import torch
+
+from . import _base
+from . import utils
+from .so2 import SO2Matrix
+
+
+class SE2Matrix(_base.SEMatrixBase):
+    """See :mod:`liegroups.SE2`"""
+    dim = 3
+    dof = 3
+    RotationType = SO2Matrix
+
+    def adjoint(self):
+        rot_part = self.rot.as_matrix()
+        if rot_part.dim() < 3:
+            rot_part = rot_part.unsqueeze(dim=0)  # matrix --> batch
+
+        trans = self.trans
+        if trans.dim() < 2:
+            # vector --> vectorbatch
+            trans = trans.unsqueeze(dim=0)
+
+        trans_part = trans.new_empty(
+            trans.shape[0], trans.shape[1], 1)
+        trans_part[:, 0, 0] = trans[:, 1]
+        trans_part[:, 1, 0] = -trans[:, 0]
+
+        bottom_row = trans.new_zeros(self.dof)
+        bottom_row[-1] = 1.
+        bottom_row = bottom_row.unsqueeze_(dim=0).unsqueeze_(
+            dim=0).expand(trans.shape[0], 1, self.dof)
+
+        return torch.cat([torch.cat([rot_part, trans_part], dim=2),
+                          bottom_row], dim=1).squeeze_()
+
+    @classmethod
+    def exp(cls, xi):
+        if xi.dim() < 2:
+            xi = xi.unsqueeze(dim=0)
+
+        if xi.shape[1] != cls.dof:
+            raise ValueError(
+                "xi must have shape ({},) or (N,{})".format(cls.dof, cls.dof))
+
+        rho = xi[:, 0:2]
+        phi = xi[:, 2]
+
+        rot = cls.RotationType.exp(phi)
+        rot_jac = cls.RotationType.left_jacobian(phi)
+
+        if rot_jac.dim() < 3:
+            rot_jac.unsqueeze_(dim=0)
+        if rho.dim() < 3:
+            rho.unsqueeze_(dim=2)
+
+        trans = torch.bmm(rot_jac, rho).squeeze_()
+
+        return cls(rot, trans)
+
+    @classmethod
+    def inv_left_jacobian(cls, xi):
+        raise NotImplementedError
+
+    @classmethod
+    def left_jacobian(cls, xi):
+        raise NotImplementedError
+
+    def log(self):
+        phi = self.rot.log()
+        inv_rot_jac = self.RotationType.inv_left_jacobian(phi)
+
+        if self.trans.dim() < 2:
+            trans = self.trans.unsqueeze(dim=0)
+        else:
+            trans = self.trans
+
+        if phi.dim() < 1:
+            phi.unsqueeze_(dim=0)
+        phi.unsqueeze_(dim=1)  # because phi is 1-dimensional for SE2
+
+        if inv_rot_jac.dim() < 3:
+            inv_rot_jac.unsqueeze_(dim=0)
+        if trans.dim() < 3:
+            trans = trans.unsqueeze(dim=2)
+
+        rho = torch.bmm(inv_rot_jac, trans).squeeze_()
+        if rho.dim() < 2:
+            rho.unsqueeze_(dim=0)
+
+        return torch.cat([rho, phi], dim=1).squeeze_()
+
+    @classmethod
+    def odot(cls, p, directional=False):
+        if p.dim() < 2:
+            p = p.unsqueeze(dim=0)  # vector --> vectorbatch
+
+        result = p.__class__(p.shape[0], p.shape[1], cls.dof).zero_()
+
+        # Got euclidean coordinates
+        if p.shape[1] == cls.dim - 1:
+            # Assume scale parameter is 1 unless p is a direction
+            # vector, in which case the scale is 0
+            if not directional:
+                result[:, 0:2, 0:2] = torch.eye(
+                    cls.RotationType.dim).unsqueeze_(dim=0).expand(
+                        p.shape[0], cls.RotationType.dim, cls.RotationType.dim)
+
+            result[:, 0:2, 2] = torch.mm(
+                cls.RotationType.wedge(p.__class__([1.])),
+                p.transpose(1, 0)).transpose_(1, 0)
+
+        # Got homogeneous coordinates
+        elif p.shape[1] == cls.dim:
+            result[:, 0:2, 0:2] = \
+                p[:, 2].unsqueeze_(dim=1).unsqueeze_(dim=2) * \
+                torch.eye(
+                cls.RotationType.dim).unsqueeze_(dim=0).repeat(
+                p.shape[0], 1, 1)
+
+            result[:, 0:2, 2] = torch.mm(
+                cls.RotationType.wedge(p.__class__([1.])),
+                p[:, 0:2].transpose_(1, 0)).transpose_(1, 0)
+
+        # Got wrong dimension
+        else:
+            raise ValueError("p must have shape ({},), ({},), (N,{}) or (N,{})".format(
+                cls.dim - 1, cls.dim, cls.dim - 1, cls.dim))
+
+        return result.squeeze_()
+
+    @classmethod
+    def vee(cls, Xi):
+        if Xi.dim() < 3:
+            Xi = Xi.unsqueeze(dim=0)
+
+        if Xi.shape[1:3] != (cls.dim, cls.dim):
+            raise ValueError("Xi must have shape ({},{}) or (N,{},{})".format(
+                cls.dim, cls.dim, cls.dim, cls.dim))
+
+        xi = Xi.new_empty(Xi.shape[0], cls.dof)
+        xi[:, 0:2] = Xi[:, 0:2, 2]
+        xi[:, 2] = cls.RotationType.vee(Xi[:, 0:2, 0:2])
+
+        return xi.squeeze_()
+
+    @classmethod
+    def wedge(cls, xi):
+        if xi.dim() < 2:
+            xi = xi.unsqueeze(dim=0)
+
+        if xi.shape[1] != cls.dof:
+            raise ValueError(
+                "phi must have shape ({},) or (N,{})".format(cls.dof, cls.dof))
+
+        Xi = xi.new_zeros(xi.shape[0], cls.dim, cls.dim)
+        Xi[:, 0:2, 0:2] = cls.RotationType.wedge(xi[:, 2])
+        Xi[:, 0:2, 2] = xi[:, 0:2]
+
+        return Xi.squeeze_()

liegroups/build/lib/liegroups/torch/se3.py

@@ -0,0 +1,320 @@
+import torch
+
+from . import _base
+from . import utils
+from .so3 import SO3Matrix
+
+
+class SE3Matrix(_base.SEMatrixBase):
+    """See :mod:`liegroups.SE3` """
+    dim = 4
+    dof = 6
+    RotationType = SO3Matrix
+
+    def adjoint(self):
+        rot = self.rot.as_matrix()
+        if rot.dim() < 3:
+            rot = rot.unsqueeze(dim=0)  # matrix --> batch
+
+        trans = self.trans
+        if trans.dim() < 2:
+            # vector --> vectorbatch
+            trans = trans.unsqueeze(dim=0)
+
+        trans_wedge = self.RotationType.wedge(trans)
+        if trans_wedge.dim() < 3:
+            trans_wedge.unsqueeze_(dim=0)  # matrix --> batch
+
+        trans_wedge_bmm_rot = torch.bmm(trans_wedge, rot)
+
+        zero_block = trans.new_empty(rot.shape).zero_()
+
+        return torch.cat([torch.cat([rot, trans_wedge_bmm_rot], dim=2),
+                          torch.cat([zero_block, rot], dim=2)], dim=1
+                         ).squeeze_()
+
+    @classmethod
+    def curlyvee(cls, Psi):
+        if Psi.dim() < 3:
+            Psi = Psi.unsqueeze(dim=0)
+
+        if Psi.shape[1:] != (cls.dof, cls.dof):
+            raise ValueError("Psi must have shape ({},{}) or (N,{},{})".format(
+                cls.dof, cls.dof, cls.dof, cls.dof))
+
+        xi = Psi.new_empty(Psi.shape[0], cls.dof)
+        xi[:, :3] = cls.RotationType.vee(Psi[:, :3, 3:])
+        xi[:, 3:] = cls.RotationType.vee(Psi[:, :3, :3])
+
+        return xi.squeeze_()
+
+    @classmethod
+    def curlywedge(cls, xi):
+        if xi.dim() < 2:
+            xi = xi.unsqueeze(dim=0)
+
+        if xi.shape[1] != cls.dof:
+            raise ValueError(
+                "phi must have shape ({},) or (N,{})".format(cls.dof, cls.dof))
+
+        Psi = xi.new_empty(xi.shape[0], cls.dof, cls.dof).zero_()
+        Psi[:, :3, :3] = cls.RotationType.wedge(xi[:, 3:])
+        Psi[:, :3, 3:] = cls.RotationType.wedge(xi[:, :3])
+        Psi[:, 3:, 3:] = Psi[:, :3, :3]
+
+        return Psi.squeeze_()
+
+    @classmethod
+    def exp(cls, xi):
+        if xi.dim() < 2:
+            xi = xi.unsqueeze(dim=0)
+
+        if xi.shape[1] != cls.dof:
+            raise ValueError(
+                "xi must have shape ({},) or (N,{})".format(cls.dof, cls.dof))
+
+        rho = xi[:, :3]
+        phi = xi[:, 3:]
+
+        rot = cls.RotationType.exp(phi)
+        rot_jac = cls.RotationType.left_jacobian(phi)
+
+        if rot_jac.dim() < 3:
+            rot_jac = rot_jac.unsqueeze(dim=0)
+        if rho.dim() < 3:
+            rho = rho.unsqueeze(dim=2)
+
+        trans = torch.bmm(rot_jac, rho).squeeze_()
+
+        return cls(rot, trans)
+
+    @classmethod
+    def left_jacobian_Q_matrix(cls, xi):
+        if xi.dim() < 2:
+            xi = xi.unsqueeze(dim=0)
+
+        if xi.shape[1] != cls.dof:
+            raise ValueError(
+                "xi must have shape ({},) or (N,{})".format(cls.dof, cls.dof))
+
+        rho = xi[:, :3]  # translation part
+        phi = xi[:, 3:]  # rotation part
+
+        rx = cls.RotationType.wedge(rho)
+        if rx.dim() < 3:
+            rx.unsqueeze_(dim=0)
+
+        px = cls.RotationType.wedge(phi)
+        if px.dim() < 3:
+            px.unsqueeze_(dim=0)
+
+        ph = phi.norm(p=2, dim=1)
+        ph2 = ph * ph
+        ph3 = ph2 * ph
+        ph4 = ph3 * ph
+        ph5 = ph4 * ph
+
+        cph = ph.cos()
+        sph = ph.sin()
+
+        m1 = 0.5
+        m2 = (ph - sph) / ph3
+        m3 = (0.5 * ph2 + cph - 1.) / ph4
+        m4 = (ph - 1.5 * sph + 0.5 * ph * cph) / ph5
+
+        m2 = m2.unsqueeze_(dim=1).unsqueeze_(dim=2).expand_as(rx)
+        m3 = m3.unsqueeze_(dim=1).unsqueeze_(dim=2).expand_as(rx)
+        m4 = m4.unsqueeze_(dim=1).unsqueeze_(dim=2).expand_as(rx)
+
+        t1 = rx
+        t2 = px.bmm(rx) + rx.bmm(px) + px.bmm(rx).bmm(px)
+        t3 = px.bmm(px).bmm(rx) + rx.bmm(px).bmm(px) - 3. * px.bmm(rx).bmm(px)
+        t4 = px.bmm(rx).bmm(px).bmm(px) + px.bmm(px).bmm(rx).bmm(px)
+
+        Q = m1 * t1 + m2 * t2 + m3 * t3 + m4 * t4
+
+        return Q.squeeze_()
+
+    @classmethod
+    def inv_left_jacobian(cls, xi):
+        if xi.dim() < 2:
+            xi = xi.unsqueeze(dim=0)
+
+        if xi.shape[1] != cls.dof:
+            raise ValueError(
+                "xi must have shape ({},) or (N,{})".format(cls.dof, cls.dof))
+
+        rho = xi[:, :3]  # translation part
+        phi = xi[:, 3:]  # rotation part
+
+        jac = phi.new_empty(phi.shape[0], cls.dof, cls.dof)
+        angle = phi.norm(p=2, dim=1)
+
+        # Near phi==0, use first order Taylor expansion
+        small_angle_mask = utils.isclose(angle, 0.)
+        small_angle_inds = small_angle_mask.nonzero(as_tuple=False).squeeze_(dim=1)
+        if len(small_angle_inds) > 0:
+
+            # Create an identity matrix with a tensor type that matches the input
+            I = phi.new_empty(cls.dof, cls.dof)
+            torch.eye(cls.dof, out=I)
+
+            jac[small_angle_inds] = \
+                I.expand_as(jac[small_angle_inds]) - \
+                0.5 * cls.curlywedge(xi[small_angle_inds])
+
+        # Otherwise...
+        large_angle_mask = small_angle_mask.logical_not()
+        large_angle_inds = large_angle_mask.nonzero(as_tuple=False).squeeze_(dim=1)
+
+        if len(large_angle_inds) > 0:
+            so3_inv_jac = cls.RotationType.inv_left_jacobian(
+                phi[large_angle_inds])
+            if so3_inv_jac.dim() < 3:
+                so3_inv_jac.unsqueeze_(dim=0)
+
+            Q_mat = cls.left_jacobian_Q_matrix(xi[large_angle_inds])
+            if Q_mat.dim() < 3:
+                Q_mat.unsqueeze_(dim=0)
+
+            zero_block = phi.new_empty(Q_mat.shape).zero_()
+            inv_jac_Q_inv_jac = so3_inv_jac.bmm(Q_mat).bmm(so3_inv_jac)
+
+            jac[large_angle_inds] = torch.cat(
+                [torch.cat([so3_inv_jac, -inv_jac_Q_inv_jac], dim=2),
+                 torch.cat([zero_block, so3_inv_jac], dim=2)], dim=1)
+
+        return jac.squeeze_()
+
+    @classmethod
+    def left_jacobian(cls, xi):
+        if xi.dim() < 2:
+            xi = xi.unsqueeze(dim=0)
+
+        if xi.shape[1] != cls.dof:
+            raise ValueError(
+                "xi must have shape ({},) or (N,{})".format(cls.dof, cls.dof))
+
+        rho = xi[:, :3]  # translation part
+        phi = xi[:, 3:]  # rotation part
+
+        jac = phi.new_empty(phi.shape[0], cls.dof, cls.dof)
+        angle = phi.norm(p=2, dim=1)
+
+        # Near phi==0, use first order Taylor expansion
+        small_angle_mask = utils.isclose(angle, 0.)
+        small_angle_inds = small_angle_mask.nonzero(as_tuple=False).squeeze_(dim=1)
+        if len(small_angle_inds) > 0:
+            # Create an identity matrix with a tensor type that matches the input
+            I = phi.new_empty(cls.dof, cls.dof)
+            torch.eye(cls.dof, out=I)
+
+            jac[small_angle_inds] = \
+                I.expand_as(jac[small_angle_inds]) + \
+                0.5 * cls.curlywedge(xi[small_angle_inds])
+
+        # Otherwise...
+        large_angle_mask = small_angle_mask.logical_not()
+        large_angle_inds = large_angle_mask.nonzero(as_tuple=False).squeeze_(dim=1)
+
+        if len(large_angle_inds) > 0:
+            so3_jac = cls.RotationType.left_jacobian(phi[large_angle_inds])
+            if so3_jac.dim() < 3:
+                so3_jac.unsqueeze_(dim=0)
+
+            Q_mat = cls.left_jacobian_Q_matrix(xi[large_angle_inds])
+            if Q_mat.dim() < 3:
+                Q_mat.unsqueeze_(dim=0)
+
+            zero_block = phi.new_empty(Q_mat.shape).zero_()
+
+            jac[large_angle_inds] = torch.cat(
+                [torch.cat([so3_jac, Q_mat], dim=2),
+                 torch.cat([zero_block, so3_jac], dim=2)], dim=1)
+
+        return jac.squeeze_()
+
+    def log(self):
+        phi = self.rot.log()
+        inv_rot_jac = self.RotationType.inv_left_jacobian(phi)
+
+        if self.trans.dim() < 2:
+            trans = self.trans.unsqueeze(dim=0)
+        else:
+            trans = self.trans
+
+        if inv_rot_jac.dim() < 3:
+            inv_rot_jac.unsqueeze_(dim=0)
+        if trans.dim() < 3:
+            trans = trans.unsqueeze(dim=2)
+
+        rho = torch.bmm(inv_rot_jac, trans).squeeze_()
+        if rho.dim() < 2:
+            rho.unsqueeze_(dim=0)
+        if phi.dim() < 2:
+            phi.unsqueeze_(dim=0)
+
+        return torch.cat([rho, phi], dim=1).squeeze_()
+
+    @classmethod
+    def odot(cls, p, directional=False):
+        if p.dim() < 2:
+            p = p.unsqueeze(dim=0)  # vector --> vectorbatch
+
+        result = p.new_empty(p.shape[0], p.shape[1], cls.dof).zero_()
+
+        # Got euclidean coordinates
+        if p.shape[1] == cls.dim - 1:
+            # Assume scale parameter is 1 unless p is a direction
+            # vector, in which case the scale is 0
+            if not directional:
+                result[:, :3, :3] = torch.eye(3, dtype=p.dtype).unsqueeze_(dim=0).expand(
+                    p.shape[0], 3, 3)
+
+            result[:, :3, 3:] = cls.RotationType.wedge(-p)
+
+        # Got homogeneous coordinates
+        elif p.shape[1] == cls.dim:
+            result[:, :3, :3] = \
+                p[:, 3].unsqueeze_(dim=1).unsqueeze_(dim=2) * \
+                torch.eye(3, dtype=p.dtype).unsqueeze_(dim=0).repeat(
+                p.shape[0], 1, 1)
+
+            result[:, :3, 3:] = cls.RotationType.wedge(-p[:, :3])
+
+        # Got wrong dimension
+        else:
+            raise ValueError("p must have shape ({},), ({},), (N,{}) or (N,{})".format(
+                cls.dim - 1, cls.dim, cls.dim - 1, cls.dim))
+
+        return result.squeeze_()
+
+    @classmethod
+    def vee(cls, Xi):
+        if Xi.dim() < 3:
+            Xi = Xi.unsqueeze(dim=0)
+
+        if Xi.shape[1:3] != (cls.dim, cls.dim):
+            raise ValueError("Xi must have shape ({},{}) or (N,{},{})".format(
+                cls.dim, cls.dim, cls.dim, cls.dim))
+
+        xi = Xi.new_empty(Xi.shape[0], cls.dof)
+        xi[:, :3] = Xi[:, :3, 3]
+        xi[:, 3:] = cls.RotationType.vee(Xi[:, :3, :3])
+
+        return xi.squeeze_()
+
+    @classmethod
+    def wedge(cls, xi):
+        if xi.dim() < 2:
+            xi = xi.unsqueeze(dim=0)
+
+        if xi.shape[1] != cls.dof:
+            raise ValueError(
+                "phi must have shape ({},) or (N,{})".format(cls.dof, cls.dof))
+
+        Xi = xi.new_empty(xi.shape[0], cls.dim, cls.dim).zero_()
+        Xi[:, :3, :3] = cls.RotationType.wedge(xi[:, 3:])
+        Xi[:, :3, 3] = xi[:, :3]
+
+        return Xi.squeeze_()

liegroups/build/lib/liegroups/torch/so2.py

@@ -0,0 +1,157 @@
+import torch
+
+from . import _base
+from . import utils
+
+
+class SO2Matrix(_base.SOMatrixBase):
+    """See :mod:`liegroups.SO2`"""
+    dim = 2
+    dof = 1
+
+    def adjoint(self):
+        if self.mat.dim() < 3:
+            return self.mat.__class__([1.])
+        else:
+            return self.mat.__class__(self.mat.shape[0]).fill_(1.)
+
+    @classmethod
+    def exp(cls, phi):
+        if phi.dim() < 1:
+            phi = phi.unsqueeze(dim=0)
+
+        s = phi.sin()
+        c = phi.cos()
+
+        mat = phi.__class__(phi.shape[0], cls.dim, cls.dim)
+        mat[:, 0, 0] = c
+        mat[:, 0, 1] = -s
+        mat[:, 1, 0] = s
+        mat[:, 1, 1] = c
+
+        return cls(mat.squeeze_())
+
+    @classmethod
+    def from_angle(cls, angle_in_radians):
+        """Form a rotation matrix given an angle in rad."""
+        return cls.exp(angle_in_radians)
+
+    @classmethod
+    def inv_left_jacobian(cls, phi):
+        """(see Barfoot/Eade)."""
+        if phi.dim() < 1:
+            phi = phi.unsqueeze(dim=0)
+
+        jac = phi.__class__(phi.shape[0], cls.dim, cls.dim)
+
+        # Near phi==0, use first order Taylor expansion
+        small_angle_mask = utils.isclose(phi, 0.)
+        small_angle_inds = small_angle_mask.nonzero(as_tuple=False).squeeze_(dim=1)
+
+        if len(small_angle_inds) > 0:
+            jac[small_angle_inds] = torch.eye(cls.dim).expand(
+                len(small_angle_inds), cls.dim, cls.dim) \
+                - 0.5 * cls.wedge(phi[small_angle_inds])
+
+        # Otherwise...
+        large_angle_mask = small_angle_mask.logical_not()
+        large_angle_inds = large_angle_mask.nonzero(as_tuple=False).squeeze_(dim=1)
+
+        if len(large_angle_inds) > 0:
+            angle = phi[large_angle_inds]
+            ha = 0.5 * angle       # half angle
+            hacha = ha / ha.tan()  # half angle * cot(half angle)
+
+            ha.unsqueeze_(dim=1).unsqueeze_(
+                dim=2).expand_as(jac[large_angle_inds])
+            hacha.unsqueeze_(dim=1).unsqueeze_(
+                dim=2).expand_as(jac[large_angle_inds])
+
+            A = hacha * \
+                torch.eye(cls.dim).unsqueeze_(
+                    dim=0).expand_as(jac[large_angle_inds])
+            B = -ha * cls.wedge(phi.__class__([1.]))
+
+            jac[large_angle_inds] = A + B
+
+        return jac.squeeze_()
+
+    @classmethod
+    def left_jacobian(cls, phi):
+        """(see Barfoot/Eade)."""
+        if phi.dim() < 1:
+            phi = phi.unsqueeze(dim=0)
+
+        jac = phi.__class__(phi.shape[0], cls.dim, cls.dim)
+
+        # Near phi==0, use first order Taylor expansion
+        small_angle_mask = utils.isclose(phi, 0.)
+        small_angle_inds = small_angle_mask.nonzero(as_tuple=False).squeeze_(dim=1)
+
+        if len(small_angle_inds) > 0:
+            jac[small_angle_inds] = torch.eye(cls.dim).expand(
+                len(small_angle_inds), cls.dim, cls.dim) \
+                + 0.5 * cls.wedge(phi[small_angle_inds])
+
+        # Otherwise...
+        large_angle_mask = small_angle_mask.logical_not()
+        large_angle_inds = large_angle_mask.nonzero(as_tuple=False).squeeze_(dim=1)
+
+        if len(large_angle_inds) > 0:
+            angle = phi[large_angle_inds]
+            s = angle.sin()
+            c = angle.cos()
+
+            A = (s / angle).unsqueeze_(dim=1).unsqueeze_(
+                dim=2).expand_as(jac[large_angle_inds]) * \
+                torch.eye(cls.dim).unsqueeze_(dim=0).expand_as(
+                jac[large_angle_inds])
+            B = ((1. - c) / angle).unsqueeze_(dim=1).unsqueeze_(
+                dim=2).expand_as(jac[large_angle_inds]) * \
+                cls.wedge(phi.__class__([1.]))
+
+            jac[large_angle_inds] = A + B
+
+        return jac.squeeze_()
+
+    def log(self):
+        if self.mat.dim() < 3:
+            mat = self.mat.unsqueeze(dim=0)
+        else:
+            mat = self.mat
+
+        s = mat[:, 1, 0]
+        c = mat[:, 0, 0]
+
+        return torch.atan2(s, c).squeeze_()
+
+    def to_angle(self):
+        """Recover the rotation angle in rad from the rotation matrix."""
+        return self.log()
+
+    @classmethod
+    def vee(cls, Phi):
+        if Phi.dim() < 3:
+            Phi = Phi.unsqueeze(dim=0)
+
+        if Phi.shape[1:3] != (cls.dim, cls.dim):
+            raise ValueError(
+                "Phi must have shape ({},{}) or (N,{},{})".format(cls.dim, cls.dim, cls.dim, cls.dim))
+
+        return Phi[:, 1, 0].squeeze_()
+
+    @classmethod
+    def wedge(cls, phi):
+        if phi.dim() < 1:
+            phi = phi.unsqueeze(dim=0)  # scalar --> vector
+        if phi.dim() < 2:
+            phi = phi.unsqueeze(dim=1)  # vector --> matrix (N --> Nx1)
+
+        if phi.shape[1] != cls.dof:
+            raise ValueError(
+                "phi must have shape ({},) or (N,{})".format(cls.dof, cls.dof))
+
+        Phi = phi.new_zeros(phi.shape[0], cls.dim, cls.dim)
+        Phi[:, 0, 1] = -phi[:, 0]
+        Phi[:, 1, 0] = phi[:, 0]
+        return Phi.squeeze_()

liegroups/build/lib/liegroups/torch/so3.py

@@ -0,0 +1,458 @@
+import torch
+import numpy as np
+
+from . import _base
+from . import utils
+
+
+class SO3Matrix(_base.SOMatrixBase):
+    """See :mod:`liegroups.SO3`"""
+    dim = 3
+    dof = 3
+
+    def adjoint(self):
+        return self.mat
+
+    @classmethod
+    def exp(cls, phi):
+        if phi.dim() < 2:
+            phi = phi.unsqueeze(dim=0)
+
+        if phi.shape[1] != cls.dof:
+            raise ValueError(
+                "phi must have shape ({},) or (N,{})".format(cls.dof, cls.dof))
+
+        mat = phi.new_empty(phi.shape[0], cls.dim, cls.dim)
+        angle = phi.norm(p=2, dim=1)
+
+        # Near phi==0, use first order Taylor expansion
+        small_angle_mask = utils.isclose(angle, 0.)
+        small_angle_inds = small_angle_mask.nonzero(as_tuple=False).squeeze_(dim=1)
+
+        if len(small_angle_inds) > 0:
+            mat[small_angle_inds] = \
+                torch.eye(cls.dim, dtype=phi.dtype).expand_as(mat[small_angle_inds]) + \
+                cls.wedge(phi[small_angle_inds])
+
+        # Otherwise...
+        large_angle_mask = small_angle_mask.logical_not()
+        large_angle_inds = large_angle_mask.nonzero(as_tuple=False).squeeze_(dim=1)
+
+        if len(large_angle_inds) > 0:
+            angle = angle[large_angle_inds]
+            axis = phi[large_angle_inds] / \
+                angle.unsqueeze(dim=1).expand(len(angle), cls.dim)
+            s = angle.sin().unsqueeze_(dim=1).unsqueeze_(
+                dim=2).expand_as(mat[large_angle_inds])
+            c = angle.cos().unsqueeze_(dim=1).unsqueeze_(
+                dim=2).expand_as(mat[large_angle_inds])
+
+            A = c * torch.eye(cls.dim, dtype=phi.dtype).unsqueeze_(dim=0).expand_as(
+                mat[large_angle_inds])
+            B = (1. - c) * utils.outer(axis, axis)
+            C = s * cls.wedge(axis)
+
+            mat[large_angle_inds] = A + B + C
+
+        return cls(mat.squeeze_())
+
+    @classmethod
+    def from_quaternion(cls, quat, ordering='wxyz'):
+        """Form a rotation matrix from a unit length quaternion.
+
+           Valid orderings are 'xyzw' and 'wxyz'.
+        """
+        if quat.dim() < 2:
+            quat = quat.unsqueeze(dim=0)
+
+        if not utils.allclose(quat.norm(p=2, dim=1), 1.):
+            raise ValueError("Quaternions must be unit length")
+
+        if ordering is 'xyzw':
+            qx = quat[:, 0]
+            qy = quat[:, 1]
+            qz = quat[:, 2]
+            qw = quat[:, 3]
+        elif ordering is 'wxyz':
+            qw = quat[:, 0]
+            qx = quat[:, 1]
+            qy = quat[:, 2]
+            qz = quat[:, 3]
+        else:
+            raise ValueError(
+                "Valid orderings are 'xyzw' and 'wxyz'. Got '{}'.".format(ordering))
+
+        # Form the matrix
+        mat = quat.new_empty(quat.shape[0], cls.dim, cls.dim)
+
+        qw2 = qw * qw
+        qx2 = qx * qx
+        qy2 = qy * qy
+        qz2 = qz * qz
+
+        mat[:, 0, 0] = 1. - 2. * (qy2 + qz2)
+        mat[:, 0, 1] = 2. * (qx * qy - qw * qz)
+        mat[:, 0, 2] = 2. * (qw * qy + qx * qz)
+
+        mat[:, 1, 0] = 2. * (qw * qz + qx * qy)
+        mat[:, 1, 1] = 1. - 2. * (qx2 + qz2)
+        mat[:, 1, 2] = 2. * (qy * qz - qw * qx)
+
+        mat[:, 2, 0] = 2. * (qx * qz - qw * qy)
+        mat[:, 2, 1] = 2. * (qw * qx + qy * qz)
+        mat[:, 2, 2] = 1. - 2. * (qx2 + qy2)
+
+        return cls(mat.squeeze_())
+
+    @classmethod
+    def from_rpy(cls, rpy):
+        """Form a rotation matrix from RPY Euler angles."""
+        if rpy.dim() < 2:
+            rpy = rpy.unsqueeze(dim=0)
+
+        roll = rpy[:, 0]
+        pitch = rpy[:, 1]
+        yaw = rpy[:, 2]
+        return cls.rotz(yaw).dot(cls.roty(pitch).dot(cls.rotx(roll)))
+
+    @classmethod
+    def inv_left_jacobian(cls, phi):
+        if phi.dim() < 2:
+            phi = phi.unsqueeze(dim=0)
+
+        if phi.shape[1] != cls.dof:
+            raise ValueError(
+                "phi must have shape ({},) or (N,{})".format(cls.dof, cls.dof))
+
+        jac = phi.new_empty(phi.shape[0], cls.dof, cls.dof)
+        angle = phi.norm(p=2, dim=1)
+
+        # Near phi==0, use first order Taylor expansion
+        small_angle_mask = utils.isclose(angle, 0.)
+        small_angle_inds = small_angle_mask.nonzero(as_tuple=False).squeeze_(dim=1)
+        if len(small_angle_inds) > 0:
+            jac[small_angle_inds] = \
+                torch.eye(cls.dof, dtype=phi.dtype).expand_as(jac[small_angle_inds]) - \
+                0.5 * cls.wedge(phi[small_angle_inds])
+
+        # Otherwise...
+        large_angle_mask = small_angle_mask.logical_not()
+        large_angle_inds = large_angle_mask.nonzero(as_tuple=False).squeeze_(dim=1)
+
+        if len(large_angle_inds) > 0:
+            angle = angle[large_angle_inds]
+            axis = phi[large_angle_inds] / \
+                angle.unsqueeze(dim=1).expand(len(angle), cls.dof)
+
+            ha = 0.5 * angle       # half angle
+            hacha = ha / ha.tan()  # half angle * cot(half angle)
+
+            ha.unsqueeze_(dim=1).unsqueeze_(
+                dim=2).expand_as(jac[large_angle_inds])
+            hacha.unsqueeze_(dim=1).unsqueeze_(
+                dim=2).expand_as(jac[large_angle_inds])
+
+            A = hacha * \
+                torch.eye(cls.dof, dtype=phi.dtype).unsqueeze_(
+                    dim=0).expand_as(jac[large_angle_inds])
+            B = (1. - hacha) * utils.outer(axis, axis)
+            C = -ha * cls.wedge(axis)
+
+            jac[large_angle_inds] = A + B + C
+
+        return jac.squeeze_()
+
+    @classmethod
+    def left_jacobian(cls, phi):
+        if phi.dim() < 2:
+            phi = phi.unsqueeze(dim=0)
+
+        if phi.shape[1] != cls.dof:
+            raise ValueError(
+                "phi must have shape ({},) or (N,{})".format(cls.dof, cls.dof))
+
+        jac = phi.new_empty(phi.shape[0], cls.dof, cls.dof)
+        angle = phi.norm(p=2, dim=1)
+
+        # Near phi==0, use first order Taylor expansion
+        small_angle_mask = utils.isclose(angle, 0.)
+        small_angle_inds = small_angle_mask.nonzero(as_tuple=False).squeeze_(dim=1)
+        if len(small_angle_inds) > 0:
+            jac[small_angle_inds] = \
+                torch.eye(cls.dof, dtype=phi.dtype).expand_as(jac[small_angle_inds]) + \
+                0.5 * cls.wedge(phi[small_angle_inds])
+
+        # Otherwise...
+        large_angle_mask = small_angle_mask.logical_not()
+        large_angle_inds = large_angle_mask.nonzero(as_tuple=False).squeeze_(dim=1)
+
+        if len(large_angle_inds) > 0:
+            angle = angle[large_angle_inds]
+            axis = phi[large_angle_inds] / \
+                angle.unsqueeze(dim=1).expand(len(angle), cls.dof)
+            s = angle.sin()
+            c = angle.cos()
+
+            A = (s / angle).unsqueeze_(dim=1).unsqueeze_(
+                dim=2).expand_as(jac[large_angle_inds]) * \
+                torch.eye(cls.dof, dtype=phi.dtype).unsqueeze_(dim=0).expand_as(
+                jac[large_angle_inds])
+            B = (1. - s / angle).unsqueeze_(dim=1).unsqueeze_(
+                dim=2).expand_as(jac[large_angle_inds]) * \
+                utils.outer(axis, axis)
+            C = ((1. - c) / angle).unsqueeze_(dim=1).unsqueeze_(
+                dim=2).expand_as(jac[large_angle_inds]) * \
+                cls.wedge(axis.squeeze())
+
+            jac[large_angle_inds] = A + B + C
+
+        return jac.squeeze_()
+
+    def log(self):
+        if self.mat.dim() < 3:
+            mat = self.mat.unsqueeze(dim=0)
+        else:
+            mat = self.mat
+
+        phi = mat.new_empty(mat.shape[0], self.dof)
+
+        # The cosine of the rotation angle is related to the utils.trace of C
+        # Clamp to its proper domain to avoid NaNs from rounding errors
+        cos_angle = (0.5 * utils.trace(mat) - 0.5).clamp_(-1., 1.)
+        angle = cos_angle.acos()
+
+        # Near phi==0, use first order Taylor expansion
+        small_angle_mask = utils.isclose(angle, 0.)
+        small_angle_inds = small_angle_mask.nonzero(as_tuple=False).squeeze_(dim=1)
+
+        if len(small_angle_inds) > 0:
+            phi[small_angle_inds, :] = \
+                self.vee(mat[small_angle_inds] -
+                         torch.eye(self.dim, dtype=mat.dtype).expand_as(mat[small_angle_inds]))
+
+        # Otherwise...
+        large_angle_mask = small_angle_mask.logical_not()
+        large_angle_inds = large_angle_mask.nonzero(as_tuple=False).squeeze_(dim=1)
+
+        if len(large_angle_inds) > 0:
+            angle = angle[large_angle_inds]
+            sin_angle = angle.sin()
+            phi[large_angle_inds, :] = \
+                self.vee(
+                    (0.5 * angle / sin_angle).unsqueeze_(dim=1).unsqueeze_(dim=1).expand_as(mat[large_angle_inds]) *
+                    (mat[large_angle_inds] - mat[large_angle_inds].transpose(2, 1)))
+
+        return phi.squeeze_()
+
+    @classmethod
+    def rotx(cls, angle_in_radians):
+        """Form a rotation matrix given an angle in rad about the x-axis."""
+        s = angle_in_radians.sin()
+        c = angle_in_radians.cos()
+
+        mat = angle_in_radians.new_empty(
+            angle_in_radians.shape[0], cls.dim, cls.dim).zero_()
+        mat[:, 0, 0] = 1.
+        mat[:, 1, 1] = c
+        mat[:, 1, 2] = -s
+        mat[:, 2, 1] = s
+        mat[:, 2, 2] = c
+
+        return cls(mat.squeeze_())
+
+    @classmethod
+    def roty(cls, angle_in_radians):
+        """Form a rotation matrix given an angle in rad about the y-axis."""
+        s = angle_in_radians.sin()
+        c = angle_in_radians.cos()
+
+        mat = angle_in_radians.new_empty(
+            angle_in_radians.shape[0], cls.dim, cls.dim).zero_()
+        mat[:, 1, 1] = 1.
+        mat[:, 0, 0] = c
+        mat[:, 0, 2] = s
+        mat[:, 2, 0] = -s
+        mat[:, 2, 2] = c
+
+        return cls(mat.squeeze_())
+
+    @classmethod
+    def rotz(cls, angle_in_radians):
+        """Form a rotation matrix given an angle in rad about the z-axis."""
+        s = angle_in_radians.sin()
+        c = angle_in_radians.cos()
+
+        mat = angle_in_radians.new_empty(
+            angle_in_radians.shape[0], cls.dim, cls.dim).zero_()
+        mat[:, 2, 2] = 1.
+        mat[:, 0, 0] = c
+        mat[:, 0, 1] = -s
+        mat[:, 1, 0] = s
+        mat[:, 1, 1] = c
+
+        return cls(mat.squeeze_())
+
+    def to_quaternion(self, ordering='wxyz'):
+        """Convert a rotation matrix to a unit length quaternion.
+
+           Valid orderings are 'xyzw' and 'wxyz'.
+        """
+        if self.mat.dim() < 3:
+            R = self.mat.unsqueeze(dim=0)
+        else:
+            R = self.mat
+
+        qw = 0.5 * torch.sqrt(1. + R[:, 0, 0] + R[:, 1, 1] + R[:, 2, 2])
+        qx = qw.new_empty(qw.shape)
+        qy = qw.new_empty(qw.shape)
+        qz = qw.new_empty(qw.shape)
+
+        near_zero_mask = utils.isclose(qw, 0.)
+
+        if sum(near_zero_mask) > 0:
+            cond1_mask = near_zero_mask & \
+                (R[:, 0, 0] > R[:, 1, 1]).squeeze_() & \
+                (R[:, 0, 0] > R[:, 2, 2]).squeeze_()
+            cond1_inds = cond1_mask.nonzero(as_tuple=False).squeeze_(dim=1)
+
+            if len(cond1_inds) > 0:
+                R_cond1 = R[cond1_inds]
+                d = 2. * np.sqrt(1. + R_cond1[:, 0, 0] -
+                                 R_cond1[:, 1, 1] - R_cond1[:, 2, 2])
+                qw[cond1_inds] = (R_cond1[:, 2, 1] - R_cond1[:, 1, 2]) / d
+                qx[cond1_inds] = 0.25 * d
+                qy[cond1_inds] = (R_cond1[:, 1, 0] + R_cond1[:, 0, 1]) / d
+                qz[cond1_inds] = (R_cond1[:, 0, 2] + R_cond1[:, 2, 0]) / d
+
+            cond2_mask = near_zero_mask & (R[:, 1, 1] > R[:, 2, 2]).squeeze_()
+            cond2_inds = cond2_mask.nonzero(as_tuple=False).squeeze_(dim=1)
+
+            if len(cond2_inds) > 0:
+                R_cond2 = R[cond2_inds]
+                d = 2. * np.sqrt(1. + R_cond2[:, 1, 1] -
+                                 R_cond2[:, 0, 0] - R_cond2[:, 2, 2])
+                qw[cond2_inds] = (R_cond2[:, 0, 2] - R_cond2[:, 2, 0]) / d
+                qx[cond2_inds] = (R_cond2[:, 1, 0] + R_cond2[:, 0, 1]) / d
+                qy[cond2_inds] = 0.25 * d
+                qz[cond2_inds] = (R_cond2[:, 2, 1] + R_cond2[:, 1, 2]) / d
+
+            cond3_mask = near_zero_mask & cond1_mask.logical_not() & cond2_mask.logical_not()
+            cond3_inds = cond3_mask.nonzero(as_tuple=False).squeeze_(dim=1)
+
+            if len(cond3_inds) > 0:
+                R_cond3 = R[cond3_inds]
+                d = 2. * \
+                    np.sqrt(1. + R_cond3[:, 2, 2] -
+                            R_cond3[:, 0, 0] - R_cond3[:, 1, 1])
+                qw[cond3_inds] = (R_cond3[:, 1, 0] - R_cond3[:, 0, 1]) / d
+                qx[cond3_inds] = (R_cond3[:, 0, 2] + R_cond3[:, 2, 0]) / d
+                qy[cond3_inds] = (R_cond3[:, 2, 1] + R_cond3[:, 1, 2]) / d
+                qz[cond3_inds] = 0.25 * d
+
+        far_zero_mask = near_zero_mask.logical_not()
+        far_zero_inds = far_zero_mask.nonzero(as_tuple=False).squeeze_(dim=1)
+        if len(far_zero_inds) > 0:
+            R_fz = R[far_zero_inds]
+            d = 4. * qw[far_zero_inds]
+            qx[far_zero_inds] = (R_fz[:, 2, 1] - R_fz[:, 1, 2]) / d
+            qy[far_zero_inds] = (R_fz[:, 0, 2] - R_fz[:, 2, 0]) / d
+            qz[far_zero_inds] = (R_fz[:, 1, 0] - R_fz[:, 0, 1]) / d
+
+        # Check ordering last
+        if ordering is 'xyzw':
+            quat = torch.cat([qx.unsqueeze_(dim=1),
+                              qy.unsqueeze_(dim=1),
+                              qz.unsqueeze_(dim=1),
+                              qw.unsqueeze_(dim=1)], dim=1).squeeze_()
+        elif ordering is 'wxyz':
+            quat = torch.cat([qw.unsqueeze_(dim=1),
+                              qx.unsqueeze_(dim=1),
+                              qy.unsqueeze_(dim=1),
+                              qz.unsqueeze_(dim=1)], dim=1).squeeze_()
+        else:
+            raise ValueError(
+                "Valid orderings are 'xyzw' and 'wxyz'. Got '{}'.".format(ordering))
+
+        return quat
+
+    def to_rpy(self):
+        """Convert a rotation matrix to RPY Euler angles."""
+        if self.mat.dim() < 3:
+            mat = self.mat.unsqueeze(dim=0)
+        else:
+            mat = self.mat
+
+        pitch = torch.atan2(-mat[:, 2, 0],
+                            torch.sqrt(mat[:, 0, 0]**2 + mat[:, 1, 0]**2))
+        yaw = pitch.new_empty(pitch.shape)
+        roll = pitch.new_empty(pitch.shape)
+
+        near_pi_over_two_mask = utils.isclose(pitch, np.pi / 2.)
+        near_pi_over_two_inds = near_pi_over_two_mask.nonzero(as_tuple=False).squeeze_(dim=1)
+
+        near_neg_pi_over_two_mask = utils.isclose(pitch, -np.pi / 2.)
+        near_neg_pi_over_two_inds = near_neg_pi_over_two_mask.nonzero(as_tuple=False).squeeze_(dim=1)
+
+        remainder_inds = (near_pi_over_two_mask |
+                          near_neg_pi_over_two_mask).logical_not().nonzero(as_tuple=False).squeeze_(dim=1)
+
+        if len(near_pi_over_two_inds) > 0:
+            yaw[near_pi_over_two_inds] = 0.
+            roll[near_pi_over_two_inds] = torch.atan2(
+                mat[near_pi_over_two_inds, 0, 1],
+                mat[near_pi_over_two_inds, 1, 1])
+
+        if len(near_neg_pi_over_two_inds) > 0:
+            yaw[near_pi_over_two_inds] = 0.
+            roll[near_pi_over_two_inds] = -torch.atan2(
+                mat[near_pi_over_two_inds, 0, 1],
+                mat[near_pi_over_two_inds, 1, 1])
+
+        if len(remainder_inds) > 0:
+            sec_pitch = 1. / pitch[remainder_inds].cos()
+            remainder_mats = mat[remainder_inds]
+            yaw = torch.atan2(remainder_mats[:, 1, 0] * sec_pitch,
+                              remainder_mats[:, 0, 0] * sec_pitch)
+            roll = torch.atan2(remainder_mats[:, 2, 1] * sec_pitch,
+                               remainder_mats[:, 2, 2] * sec_pitch)
+
+        return torch.cat([roll.unsqueeze_(dim=1),
+                          pitch.unsqueeze_(dim=1),
+                          yaw.unsqueeze_(dim=1)], dim=1).squeeze_()
+
+    @classmethod
+    def vee(cls, Phi):
+        if Phi.dim() < 3:
+            Phi = Phi.unsqueeze(dim=0)
+
+        if Phi.shape[1:3] != (cls.dim, cls.dim):
+            raise ValueError("Phi must have shape ({},{}) or (N,{},{})".format(
+                cls.dim, cls.dim, cls.dim, cls.dim))
+
+        phi = Phi.new_empty(Phi.shape[0], cls.dim)
+        phi[:, 0] = Phi[:, 2, 1]
+        phi[:, 1] = Phi[:, 0, 2]
+        phi[:, 2] = Phi[:, 1, 0]
+        return phi.squeeze_()
+
+    @classmethod
+    def wedge(cls, phi):
+        if phi.dim() < 2:
+            phi = phi.unsqueeze(dim=0)
+
+        if phi.shape[1] != cls.dof:
+            raise ValueError(
+                "phi must have shape ({},) or (N,{})".format(cls.dof, cls.dof))
+
+        Phi = phi.new_empty(phi.shape[0], cls.dim, cls.dim).zero_()
+        Phi[:, 0, 1] = -phi[:, 2]
+        Phi[:, 1, 0] = phi[:, 2]
+        Phi[:, 0, 2] = phi[:, 1]
+        Phi[:, 2, 0] = -phi[:, 1]
+        Phi[:, 1, 2] = -phi[:, 0]
+        Phi[:, 2, 1] = phi[:, 0]
+        return Phi.squeeze_()
+
+
+class SO3Quaternion(_base.VectorLieGroupBase):
+    pass

liegroups/build/lib/liegroups/torch/utils.py

@@ -0,0 +1,49 @@
+import torch
+
+
+def allclose(mat1, mat2, tol=1e-6):
+    """Check if all elements of two tensors are close within some tolerance.
+
+    Either tensor can be replaced by a scalar.
+    """
+    return isclose(mat1, mat2, tol).all()
+
+
+def isclose(mat1, mat2, tol=1e-6):
+    """Check element-wise if two tensors are close within some tolerance.
+
+    Either tensor can be replaced by a scalar.
+    """
+    return (mat1 - mat2).abs_().lt(tol)
+
+
+def outer(vecs1, vecs2):
+    """Return the N x D x D outer products of a N x D batch of vectors,
+    or return the D x D outer product of two D-dimensional vectors.
+    """
+    # Default batch size is 1
+    if vecs1.dim() < 2:
+        vecs1 = vecs1.unsqueeze(dim=0)
+
+    if vecs2.dim() < 2:
+        vecs2 = vecs2.unsqueeze(dim=0)
+
+    if vecs1.shape[0] != vecs2.shape[0]:
+        raise ValueError("Got inconsistent batch sizes {} and {}".format(
+            vecs1.shape[0], vecs2.shape[0]))
+
+    return torch.bmm(vecs1.unsqueeze(dim=2),
+                     vecs2.unsqueeze(dim=2).transpose(2, 1)).squeeze_()
+
+
+def trace(mat):
+    """Return the N traces of a batch of N square matrices,
+    or return the trace of a square matrix."""
+    # Default batch size is 1
+    if mat.dim() < 3:
+        mat = mat.unsqueeze(dim=0)
+
+    # Element-wise multiply by identity and take the sum
+    tr =  (torch.eye(mat.shape[1], dtype=mat.dtype) * mat).sum(dim=1).sum(dim=1)
+    
+    return tr.view(mat.shape[0])

liegroups/docs/Makefile

@@ -0,0 +1,225 @@
+# Makefile for Sphinx documentation
+#
+
+# You can set these variables from the command line.
+SPHINXOPTS    =
+SPHINXBUILD   = sphinx-build
+PAPER         =
+BUILDDIR      = build
+
+# Internal variables.
+PAPEROPT_a4     = -D latex_paper_size=a4
+PAPEROPT_letter = -D latex_paper_size=letter
+ALLSPHINXOPTS   = -d $(BUILDDIR)/doctrees $(PAPEROPT_$(PAPER)) $(SPHINXOPTS) source
+# the i18n builder cannot share the environment and doctrees with the others
+I18NSPHINXOPTS  = $(PAPEROPT_$(PAPER)) $(SPHINXOPTS) source
+
+.PHONY: help
+help:
+	@echo "Please use \`make <target>' where <target> is one of"
+	@echo "  html       to make standalone HTML files"
+	@echo "  dirhtml    to make HTML files named index.html in directories"
+	@echo "  singlehtml to make a single large HTML file"
+	@echo "  pickle     to make pickle files"
+	@echo "  json       to make JSON files"
+	@echo "  htmlhelp   to make HTML files and a HTML help project"
+	@echo "  qthelp     to make HTML files and a qthelp project"
+	@echo "  applehelp  to make an Apple Help Book"
+	@echo "  devhelp    to make HTML files and a Devhelp project"
+	@echo "  epub       to make an epub"
+	@echo "  epub3      to make an epub3"
+	@echo "  latex      to make LaTeX files, you can set PAPER=a4 or PAPER=letter"
+	@echo "  latexpdf   to make LaTeX files and run them through pdflatex"
+	@echo "  latexpdfja to make LaTeX files and run them through platex/dvipdfmx"
+	@echo "  text       to make text files"
+	@echo "  man        to make manual pages"
+	@echo "  texinfo    to make Texinfo files"
+	@echo "  info       to make Texinfo files and run them through makeinfo"
+	@echo "  gettext    to make PO message catalogs"
+	@echo "  changes    to make an overview of all changed/added/deprecated items"
+	@echo "  xml        to make Docutils-native XML files"
+	@echo "  pseudoxml  to make pseudoxml-XML files for display purposes"
+	@echo "  linkcheck  to check all external links for integrity"
+	@echo "  doctest    to run all doctests embedded in the documentation (if enabled)"
+	@echo "  coverage   to run coverage check of the documentation (if enabled)"
+	@echo "  dummy      to check syntax errors of document sources"
+
+.PHONY: clean
+clean:
+	rm -rf $(BUILDDIR)/*
+
+.PHONY: html
+html:
+	$(SPHINXBUILD) -b html $(ALLSPHINXOPTS) $(BUILDDIR)/html
+	@echo
+	@echo "Build finished. The HTML pages are in $(BUILDDIR)/html."
+
+.PHONY: dirhtml
+dirhtml:
+	$(SPHINXBUILD) -b dirhtml $(ALLSPHINXOPTS) $(BUILDDIR)/dirhtml
+	@echo
+	@echo "Build finished. The HTML pages are in $(BUILDDIR)/dirhtml."
+
+.PHONY: singlehtml
+singlehtml:
+	$(SPHINXBUILD) -b singlehtml $(ALLSPHINXOPTS) $(BUILDDIR)/singlehtml
+	@echo
+	@echo "Build finished. The HTML page is in $(BUILDDIR)/singlehtml."
+
+.PHONY: pickle
+pickle:
+	$(SPHINXBUILD) -b pickle $(ALLSPHINXOPTS) $(BUILDDIR)/pickle
+	@echo
+	@echo "Build finished; now you can process the pickle files."
+
+.PHONY: json
+json:
+	$(SPHINXBUILD) -b json $(ALLSPHINXOPTS) $(BUILDDIR)/json
+	@echo
+	@echo "Build finished; now you can process the JSON files."
+
+.PHONY: htmlhelp
+htmlhelp:
+	$(SPHINXBUILD) -b htmlhelp $(ALLSPHINXOPTS) $(BUILDDIR)/htmlhelp
+	@echo
+	@echo "Build finished; now you can run HTML Help Workshop with the" \
+	      ".hhp project file in $(BUILDDIR)/htmlhelp."
+
+.PHONY: qthelp
+qthelp:
+	$(SPHINXBUILD) -b qthelp $(ALLSPHINXOPTS) $(BUILDDIR)/qthelp
+	@echo
+	@echo "Build finished; now you can run "qcollectiongenerator" with the" \
+	      ".qhcp project file in $(BUILDDIR)/qthelp, like this:"
+	@echo "# qcollectiongenerator $(BUILDDIR)/qthelp/liegroups.qhcp"
+	@echo "To view the help file:"
+	@echo "# assistant -collectionFile $(BUILDDIR)/qthelp/liegroups.qhc"
+
+.PHONY: applehelp
+applehelp:
+	$(SPHINXBUILD) -b applehelp $(ALLSPHINXOPTS) $(BUILDDIR)/applehelp
+	@echo
+	@echo "Build finished. The help book is in $(BUILDDIR)/applehelp."
+	@echo "N.B. You won't be able to view it unless you put it in" \
+	      "~/Library/Documentation/Help or install it in your application" \
+	      "bundle."
+
+.PHONY: devhelp
+devhelp:
+	$(SPHINXBUILD) -b devhelp $(ALLSPHINXOPTS) $(BUILDDIR)/devhelp
+	@echo
+	@echo "Build finished."
+	@echo "To view the help file:"
+	@echo "# mkdir -p $$HOME/.local/share/devhelp/liegroups"
+	@echo "# ln -s $(BUILDDIR)/devhelp $$HOME/.local/share/devhelp/liegroups"
+	@echo "# devhelp"
+
+.PHONY: epub
+epub:
+	$(SPHINXBUILD) -b epub $(ALLSPHINXOPTS) $(BUILDDIR)/epub
+	@echo
+	@echo "Build finished. The epub file is in $(BUILDDIR)/epub."
+
+.PHONY: epub3
+epub3:
+	$(SPHINXBUILD) -b epub3 $(ALLSPHINXOPTS) $(BUILDDIR)/epub3
+	@echo
+	@echo "Build finished. The epub3 file is in $(BUILDDIR)/epub3."
+
+.PHONY: latex
+latex:
+	$(SPHINXBUILD) -b latex $(ALLSPHINXOPTS) $(BUILDDIR)/latex
+	@echo
+	@echo "Build finished; the LaTeX files are in $(BUILDDIR)/latex."
+	@echo "Run \`make' in that directory to run these through (pdf)latex" \
+	      "(use \`make latexpdf' here to do that automatically)."
+
+.PHONY: latexpdf
+latexpdf:
+	$(SPHINXBUILD) -b latex $(ALLSPHINXOPTS) $(BUILDDIR)/latex
+	@echo "Running LaTeX files through pdflatex..."
+	$(MAKE) -C $(BUILDDIR)/latex all-pdf
+	@echo "pdflatex finished; the PDF files are in $(BUILDDIR)/latex."
+
+.PHONY: latexpdfja
+latexpdfja:
+	$(SPHINXBUILD) -b latex $(ALLSPHINXOPTS) $(BUILDDIR)/latex
+	@echo "Running LaTeX files through platex and dvipdfmx..."
+	$(MAKE) -C $(BUILDDIR)/latex all-pdf-ja
+	@echo "pdflatex finished; the PDF files are in $(BUILDDIR)/latex."
+
+.PHONY: text
+text:
+	$(SPHINXBUILD) -b text $(ALLSPHINXOPTS) $(BUILDDIR)/text
+	@echo
+	@echo "Build finished. The text files are in $(BUILDDIR)/text."
+
+.PHONY: man
+man:
+	$(SPHINXBUILD) -b man $(ALLSPHINXOPTS) $(BUILDDIR)/man
+	@echo
+	@echo "Build finished. The manual pages are in $(BUILDDIR)/man."
+
+.PHONY: texinfo
+texinfo:
+	$(SPHINXBUILD) -b texinfo $(ALLSPHINXOPTS) $(BUILDDIR)/texinfo
+	@echo
+	@echo "Build finished. The Texinfo files are in $(BUILDDIR)/texinfo."
+	@echo "Run \`make' in that directory to run these through makeinfo" \
+	      "(use \`make info' here to do that automatically)."
+
+.PHONY: info
+info:
+	$(SPHINXBUILD) -b texinfo $(ALLSPHINXOPTS) $(BUILDDIR)/texinfo
+	@echo "Running Texinfo files through makeinfo..."
+	make -C $(BUILDDIR)/texinfo info
+	@echo "makeinfo finished; the Info files are in $(BUILDDIR)/texinfo."
+
+.PHONY: gettext
+gettext:
+	$(SPHINXBUILD) -b gettext $(I18NSPHINXOPTS) $(BUILDDIR)/locale
+	@echo
+	@echo "Build finished. The message catalogs are in $(BUILDDIR)/locale."
+
+.PHONY: changes
+changes:
+	$(SPHINXBUILD) -b changes $(ALLSPHINXOPTS) $(BUILDDIR)/changes
+	@echo
+	@echo "The overview file is in $(BUILDDIR)/changes."
+
+.PHONY: linkcheck
+linkcheck:
+	$(SPHINXBUILD) -b linkcheck $(ALLSPHINXOPTS) $(BUILDDIR)/linkcheck
+	@echo
+	@echo "Link check complete; look for any errors in the above output " \
+	      "or in $(BUILDDIR)/linkcheck/output.txt."
+
+.PHONY: doctest
+doctest:
+	$(SPHINXBUILD) -b doctest $(ALLSPHINXOPTS) $(BUILDDIR)/doctest
+	@echo "Testing of doctests in the sources finished, look at the " \
+	      "results in $(BUILDDIR)/doctest/output.txt."
+
+.PHONY: coverage
+coverage:
+	$(SPHINXBUILD) -b coverage $(ALLSPHINXOPTS) $(BUILDDIR)/coverage
+	@echo "Testing of coverage in the sources finished, look at the " \
+	      "results in $(BUILDDIR)/coverage/python.txt."
+
+.PHONY: xml
+xml:
+	$(SPHINXBUILD) -b xml $(ALLSPHINXOPTS) $(BUILDDIR)/xml
+	@echo
+	@echo "Build finished. The XML files are in $(BUILDDIR)/xml."
+
+.PHONY: pseudoxml
+pseudoxml:
+	$(SPHINXBUILD) -b pseudoxml $(ALLSPHINXOPTS) $(BUILDDIR)/pseudoxml
+	@echo
+	@echo "Build finished. The pseudo-XML files are in $(BUILDDIR)/pseudoxml."
+
+.PHONY: dummy
+dummy:
+	$(SPHINXBUILD) -b dummy $(ALLSPHINXOPTS) $(BUILDDIR)/dummy
+	@echo
+	@echo "Build finished. Dummy builder generates no files."

liegroups/docs/build/doctrees/environment.pickle

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:e2969896cea4747f8d6e1dffc55b54095e97c7056b44abf6111618064b2a8608
+size 28411

liegroups/docs/build/html/.buildinfo

@@ -0,0 +1,4 @@
+# Sphinx build info version 1
+# This file hashes the configuration used when building these files. When it is not found, a full rebuild will be done.
+config: 17eb4b7752c8c4cbfb1c05ec79ac90ce
+tags: 645f666f9bcd5a90fca523b33c5a78b7

liegroups/docs/build/html/_sources/index.rst.txt

@@ -0,0 +1,14 @@
+.. liegroups documentation master file, created by
+   sphinx-quickstart on Thu Nov  3 18:58:59 2016.
+   You can adapt this file completely to your liking, but it should at least
+   contain the root `toctree` directive.
+
+=================
+Lie Groups
+=================
+
+.. toctree::
+    :maxdepth: 2
+
+    numpy
+    torch

liegroups/docs/build/html/_sources/numpy.rst.txt

@@ -0,0 +1,53 @@
+=================
+liegroups
+=================
+
+The default implementation uses numpy as the backend linear algebra library.
+
+SO(2)
+-----
+.. autoclass:: liegroups.SO2
+    :members:
+    :inherited-members:
+    :undoc-members:
+
+.. autoclass:: liegroups.numpy.so2.SO2Matrix
+    :members:
+    :inherited-members:
+    :undoc-members:
+
+SE(2)
+-----
+.. autoclass:: liegroups.SE2
+    :members: 
+    :inherited-members:
+    :undoc-members:
+
+.. autoclass:: liegroups.numpy.se2.SE2Matrix
+    :members:
+    :inherited-members:
+    :undoc-members:
+
+SO(3)
+-----
+.. autoclass:: liegroups.SO3
+    :members:
+    :inherited-members:
+    :undoc-members:
+
+.. autoclass:: liegroups.numpy.so3.SO3Matrix
+    :members:
+    :inherited-members:
+    :undoc-members:
+
+SE(3)
+-----
+.. autoclass:: liegroups.SE3
+    :members: 
+    :inherited-members:
+    :undoc-members:
+    
+.. autoclass:: liegroups.numpy.se3.SE3Matrix
+    :members:
+    :inherited-members:
+    :undoc-members:

liegroups/docs/build/html/_sources/torch.rst.txt

@@ -0,0 +1,31 @@
+=================
+liegroups.torch
+=================
+
+The PyTorch implementation uses torch.Tensor as the backend linear algebra library, which allows the user to on the GPU or CPU and integrate with other aspects of PyTorch.
+
+This version provides sensible options for batching the transformations themselves, as well as anything they might operate on, and is generally agnostic to the specific Tensor type (e.g., given a torch.cuda.FloatTensor as input, the output will also be a torch.cuda.FloatTensor).
+
+.. autoclass:: liegroups.torch.SO2
+    :members: cpu, cuda, from_numpy, is_cuda, is_pinned, pin_memory
+
+.. autoclass:: liegroups.torch.so2.SO2Matrix
+    :members: cpu, cuda, from_numpy, is_cuda, is_pinned, pin_memory
+
+.. autoclass:: liegroups.torch.SE2
+    :members: cpu, cuda, from_numpy, is_cuda, is_pinned, pin_memory
+
+.. autoclass:: liegroups.torch.se2.SE2Matrix
+    :members: cpu, cuda, from_numpy, is_cuda, is_pinned, pin_memory
+
+.. autoclass:: liegroups.torch.SO3
+    :members: cpu, cuda, from_numpy, is_cuda, is_pinned, pin_memory
+
+.. autoclass:: liegroups.torch.so3.SO3Matrix
+    :members: cpu, cuda, from_numpy, is_cuda, is_pinned, pin_memory
+
+.. autoclass:: liegroups.torch.SE3
+    :members: cpu, cuda, from_numpy, is_cuda, is_pinned, pin_memory
+
+.. autoclass:: liegroups.torch.se3.SE3Matrix
+    :members: cpu, cuda, from_numpy, is_cuda, is_pinned, pin_memory

liegroups/docs/build/html/_static/basic.css

@@ -0,0 +1,764 @@
+/*
+ * basic.css
+ * ~~~~~~~~~
+ *
+ * Sphinx stylesheet -- basic theme.
+ *
+ * :copyright: Copyright 2007-2019 by the Sphinx team, see AUTHORS.
+ * :license: BSD, see LICENSE for details.
+ *
+ */
+
+/* -- main layout ----------------------------------------------------------- */
+
+div.clearer {
+    clear: both;
+}
+
+/* -- relbar ---------------------------------------------------------------- */
+
+div.related {
+    width: 100%;
+    font-size: 90%;
+}
+
+div.related h3 {
+    display: none;
+}
+
+div.related ul {
+    margin: 0;
+    padding: 0 0 0 10px;
+    list-style: none;
+}
+
+div.related li {
+    display: inline;
+}
+
+div.related li.right {
+    float: right;
+    margin-right: 5px;
+}
+
+/* -- sidebar --------------------------------------------------------------- */
+
+div.sphinxsidebarwrapper {
+    padding: 10px 5px 0 10px;
+}
+
+div.sphinxsidebar {
+    float: left;
+    width: 230px;
+    margin-left: -100%;
+    font-size: 90%;
+    word-wrap: break-word;
+    overflow-wrap : break-word;
+}
+
+div.sphinxsidebar ul {
+    list-style: none;
+}
+
+div.sphinxsidebar ul ul,
+div.sphinxsidebar ul.want-points {
+    margin-left: 20px;
+    list-style: square;
+}
+
+div.sphinxsidebar ul ul {
+    margin-top: 0;
+    margin-bottom: 0;
+}
+
+div.sphinxsidebar form {
+    margin-top: 10px;
+}
+
+div.sphinxsidebar input {
+    border: 1px solid #98dbcc;
+    font-family: sans-serif;
+    font-size: 1em;
+}
+
+div.sphinxsidebar #searchbox form.search {
+    overflow: hidden;
+}
+
+div.sphinxsidebar #searchbox input[type="text"] {
+    float: left;
+    width: 80%;
+    padding: 0.25em;
+    box-sizing: border-box;
+}
+
+div.sphinxsidebar #searchbox input[type="submit"] {
+    float: left;
+    width: 20%;
+    border-left: none;
+    padding: 0.25em;
+    box-sizing: border-box;
+}
+
+
+img {
+    border: 0;
+    max-width: 100%;
+}
+
+/* -- search page ----------------------------------------------------------- */
+
+ul.search {
+    margin: 10px 0 0 20px;
+    padding: 0;
+}
+
+ul.search li {
+    padding: 5px 0 5px 20px;
+    background-image: url(file.png);
+    background-repeat: no-repeat;
+    background-position: 0 7px;
+}
+
+ul.search li a {
+    font-weight: bold;
+}
+
+ul.search li div.context {
+    color: #888;
+    margin: 2px 0 0 30px;
+    text-align: left;
+}
+
+ul.keywordmatches li.goodmatch a {
+    font-weight: bold;
+}
+
+/* -- index page ------------------------------------------------------------ */
+
+table.contentstable {
+    width: 90%;
+    margin-left: auto;
+    margin-right: auto;
+}
+
+table.contentstable p.biglink {
+    line-height: 150%;
+}
+
+a.biglink {
+    font-size: 1.3em;
+}
+
+span.linkdescr {
+    font-style: italic;
+    padding-top: 5px;
+    font-size: 90%;
+}
+
+/* -- general index --------------------------------------------------------- */
+
+table.indextable {
+    width: 100%;
+}
+
+table.indextable td {
+    text-align: left;
+    vertical-align: top;
+}
+
+table.indextable ul {
+    margin-top: 0;
+    margin-bottom: 0;
+    list-style-type: none;
+}
+
+table.indextable > tbody > tr > td > ul {
+    padding-left: 0em;
+}
+
+table.indextable tr.pcap {
+    height: 10px;
+}
+
+table.indextable tr.cap {
+    margin-top: 10px;
+    background-color: #f2f2f2;
+}
+
+img.toggler {
+    margin-right: 3px;
+    margin-top: 3px;
+    cursor: pointer;
+}
+
+div.modindex-jumpbox {
+    border-top: 1px solid #ddd;
+    border-bottom: 1px solid #ddd;
+    margin: 1em 0 1em 0;
+    padding: 0.4em;
+}
+
+div.genindex-jumpbox {
+    border-top: 1px solid #ddd;
+    border-bottom: 1px solid #ddd;
+    margin: 1em 0 1em 0;
+    padding: 0.4em;
+}
+
+/* -- domain module index --------------------------------------------------- */
+
+table.modindextable td {
+    padding: 2px;
+    border-collapse: collapse;
+}
+
+/* -- general body styles --------------------------------------------------- */
+
+div.body {
+    min-width: 450px;
+    max-width: 800px;
+}
+
+div.body p, div.body dd, div.body li, div.body blockquote {
+    -moz-hyphens: auto;
+    -ms-hyphens: auto;
+    -webkit-hyphens: auto;
+    hyphens: auto;
+}
+
+a.headerlink {
+    visibility: hidden;
+}
+
+a.brackets:before,
+span.brackets > a:before{
+    content: "[";
+}
+
+a.brackets:after,
+span.brackets > a:after {
+    content: "]";
+}
+
+h1:hover > a.headerlink,
+h2:hover > a.headerlink,
+h3:hover > a.headerlink,
+h4:hover > a.headerlink,
+h5:hover > a.headerlink,
+h6:hover > a.headerlink,
+dt:hover > a.headerlink,
+caption:hover > a.headerlink,
+p.caption:hover > a.headerlink,
+div.code-block-caption:hover > a.headerlink {
+    visibility: visible;
+}
+
+div.body p.caption {
+    text-align: inherit;
+}
+
+div.body td {
+    text-align: left;
+}
+
+.first {
+    margin-top: 0 !important;
+}
+
+p.rubric {
+    margin-top: 30px;
+    font-weight: bold;
+}
+
+img.align-left, .figure.align-left, object.align-left {
+    clear: left;
+    float: left;
+    margin-right: 1em;
+}
+
+img.align-right, .figure.align-right, object.align-right {
+    clear: right;
+    float: right;
+    margin-left: 1em;
+}
+
+img.align-center, .figure.align-center, object.align-center {
+  display: block;
+  margin-left: auto;
+  margin-right: auto;
+}
+
+img.align-default, .figure.align-default {
+  display: block;
+  margin-left: auto;
+  margin-right: auto;
+}
+
+.align-left {
+    text-align: left;
+}
+
+.align-center {
+    text-align: center;
+}
+
+.align-default {
+    text-align: center;
+}
+
+.align-right {
+    text-align: right;
+}
+
+/* -- sidebars -------------------------------------------------------------- */
+
+div.sidebar {
+    margin: 0 0 0.5em 1em;
+    border: 1px solid #ddb;
+    padding: 7px 7px 0 7px;
+    background-color: #ffe;
+    width: 40%;
+    float: right;
+}
+
+p.sidebar-title {
+    font-weight: bold;
+}
+
+/* -- topics ---------------------------------------------------------------- */
+
+div.topic {
+    border: 1px solid #ccc;
+    padding: 7px 7px 0 7px;
+    margin: 10px 0 10px 0;
+}
+
+p.topic-title {
+    font-size: 1.1em;
+    font-weight: bold;
+    margin-top: 10px;
+}
+
+/* -- admonitions ----------------------------------------------------------- */
+
+div.admonition {
+    margin-top: 10px;
+    margin-bottom: 10px;
+    padding: 7px;
+}
+
+div.admonition dt {
+    font-weight: bold;
+}
+
+div.admonition dl {
+    margin-bottom: 0;
+}
+
+p.admonition-title {
+    margin: 0px 10px 5px 0px;
+    font-weight: bold;
+}
+
+div.body p.centered {
+    text-align: center;
+    margin-top: 25px;
+}
+
+/* -- tables ---------------------------------------------------------------- */
+
+table.docutils {
+    border: 0;
+    border-collapse: collapse;
+}
+
+table.align-center {
+    margin-left: auto;
+    margin-right: auto;
+}
+
+table.align-default {
+    margin-left: auto;
+    margin-right: auto;
+}
+
+table caption span.caption-number {
+    font-style: italic;
+}
+
+table caption span.caption-text {
+}
+
+table.docutils td, table.docutils th {
+    padding: 1px 8px 1px 5px;
+    border-top: 0;
+    border-left: 0;
+    border-right: 0;
+    border-bottom: 1px solid #aaa;
+}
+
+table.footnote td, table.footnote th {
+    border: 0 !important;
+}
+
+th {
+    text-align: left;
+    padding-right: 5px;
+}
+
+table.citation {
+    border-left: solid 1px gray;
+    margin-left: 1px;
+}
+
+table.citation td {
+    border-bottom: none;
+}
+
+th > p:first-child,
+td > p:first-child {
+    margin-top: 0px;
+}
+
+th > p:last-child,
+td > p:last-child {
+    margin-bottom: 0px;
+}
+
+/* -- figures --------------------------------------------------------------- */
+
+div.figure {
+    margin: 0.5em;
+    padding: 0.5em;
+}
+
+div.figure p.caption {
+    padding: 0.3em;
+}
+
+div.figure p.caption span.caption-number {
+    font-style: italic;
+}
+
+div.figure p.caption span.caption-text {
+}
+
+/* -- field list styles ----------------------------------------------------- */
+
+table.field-list td, table.field-list th {
+    border: 0 !important;
+}
+
+.field-list ul {
+    margin: 0;
+    padding-left: 1em;
+}
+
+.field-list p {
+    margin: 0;
+}
+
+.field-name {
+    -moz-hyphens: manual;
+    -ms-hyphens: manual;
+    -webkit-hyphens: manual;
+    hyphens: manual;
+}
+
+/* -- hlist styles ---------------------------------------------------------- */
+
+table.hlist td {
+    vertical-align: top;
+}
+
+
+/* -- other body styles ----------------------------------------------------- */
+
+ol.arabic {
+    list-style: decimal;
+}
+
+ol.loweralpha {
+    list-style: lower-alpha;
+}
+
+ol.upperalpha {
+    list-style: upper-alpha;
+}
+
+ol.lowerroman {
+    list-style: lower-roman;
+}
+
+ol.upperroman {
+    list-style: upper-roman;
+}
+
+li > p:first-child {
+    margin-top: 0px;
+}
+
+li > p:last-child {
+    margin-bottom: 0px;
+}
+
+dl.footnote > dt,
+dl.citation > dt {
+    float: left;
+}
+
+dl.footnote > dd,
+dl.citation > dd {
+    margin-bottom: 0em;
+}
+
+dl.footnote > dd:after,
+dl.citation > dd:after {
+    content: "";
+    clear: both;
+}
+
+dl.field-list {
+    display: grid;
+    grid-template-columns: fit-content(30%) auto;
+}
+
+dl.field-list > dt {
+    font-weight: bold;
+    word-break: break-word;
+    padding-left: 0.5em;
+    padding-right: 5px;
+}
+
+dl.field-list > dt:after {
+    content: ":";
+}
+
+dl.field-list > dd {
+    padding-left: 0.5em;
+    margin-top: 0em;
+    margin-left: 0em;
+    margin-bottom: 0em;
+}
+
+dl {
+    margin-bottom: 15px;
+}
+
+dd > p:first-child {
+    margin-top: 0px;
+}
+
+dd ul, dd table {
+    margin-bottom: 10px;
+}
+
+dd {
+    margin-top: 3px;
+    margin-bottom: 10px;
+    margin-left: 30px;
+}
+
+dt:target, span.highlighted {
+    background-color: #fbe54e;
+}
+
+rect.highlighted {
+    fill: #fbe54e;
+}
+
+dl.glossary dt {
+    font-weight: bold;
+    font-size: 1.1em;
+}
+
+.optional {
+    font-size: 1.3em;
+}
+
+.sig-paren {
+    font-size: larger;
+}
+
+.versionmodified {
+    font-style: italic;
+}
+
+.system-message {
+    background-color: #fda;
+    padding: 5px;
+    border: 3px solid red;
+}
+
+.footnote:target  {
+    background-color: #ffa;
+}
+
+.line-block {
+    display: block;
+    margin-top: 1em;
+    margin-bottom: 1em;
+}
+
+.line-block .line-block {
+    margin-top: 0;
+    margin-bottom: 0;
+    margin-left: 1.5em;
+}
+
+.guilabel, .menuselection {
+    font-family: sans-serif;
+}
+
+.accelerator {
+    text-decoration: underline;
+}
+
+.classifier {
+    font-style: oblique;
+}
+
+.classifier:before {
+    font-style: normal;
+    margin: 0.5em;
+    content: ":";
+}
+
+abbr, acronym {
+    border-bottom: dotted 1px;
+    cursor: help;
+}
+
+/* -- code displays --------------------------------------------------------- */
+
+pre {
+    overflow: auto;
+    overflow-y: hidden;  /* fixes display issues on Chrome browsers */
+}
+
+span.pre {
+    -moz-hyphens: none;
+    -ms-hyphens: none;
+    -webkit-hyphens: none;
+    hyphens: none;
+}
+
+td.linenos pre {
+    padding: 5px 0px;
+    border: 0;
+    background-color: transparent;
+    color: #aaa;
+}
+
+table.highlighttable {
+    margin-left: 0.5em;
+}
+
+table.highlighttable td {
+    padding: 0 0.5em 0 0.5em;
+}
+
+div.code-block-caption {
+    padding: 2px 5px;
+    font-size: small;
+}
+
+div.code-block-caption code {
+    background-color: transparent;
+}
+
+div.code-block-caption + div > div.highlight > pre {
+    margin-top: 0;
+}
+
+div.code-block-caption span.caption-number {
+    padding: 0.1em 0.3em;
+    font-style: italic;
+}
+
+div.code-block-caption span.caption-text {
+}
+
+div.literal-block-wrapper {
+    padding: 1em 1em 0;
+}
+
+div.literal-block-wrapper div.highlight {
+    margin: 0;
+}
+
+code.descname {
+    background-color: transparent;
+    font-weight: bold;
+    font-size: 1.2em;
+}
+
+code.descclassname {
+    background-color: transparent;
+}
+
+code.xref, a code {
+    background-color: transparent;
+    font-weight: bold;
+}
+
+h1 code, h2 code, h3 code, h4 code, h5 code, h6 code {
+    background-color: transparent;
+}
+
+.viewcode-link {
+    float: right;
+}
+
+.viewcode-back {
+    float: right;
+    font-family: sans-serif;
+}
+
+div.viewcode-block:target {
+    margin: -1px -10px;
+    padding: 0 10px;
+}
+
+/* -- math display ---------------------------------------------------------- */
+
+img.math {
+    vertical-align: middle;
+}
+
+div.body div.math p {
+    text-align: center;
+}
+
+span.eqno {
+    float: right;
+}
+
+span.eqno a.headerlink {
+    position: relative;
+    left: 0px;
+    z-index: 1;
+}
+
+div.math:hover a.headerlink {
+    visibility: visible;
+}
+
+/* -- printout stylesheet --------------------------------------------------- */
+
+@media print {
+    div.document,
+    div.documentwrapper,
+    div.bodywrapper {
+        margin: 0 !important;
+        width: 100%;
+    }
+
+    div.sphinxsidebar,
+    div.related,
+    div.footer,
+    #top-link {
+        display: none;
+    }
+}

liegroups/docs/build/html/_static/css/badge_only.css

@@ -0,0 +1 @@
+.fa:before{-webkit-font-smoothing:antialiased}.clearfix{*zoom:1}.clearfix:before,.clearfix:after{display:table;content:""}.clearfix:after{clear:both}@font-face{font-family:FontAwesome;font-weight:normal;font-style:normal;src:url("../fonts/fontawesome-webfont.eot");src:url("../fonts/fontawesome-webfont.eot?#iefix") format("embedded-opentype"),url("../fonts/fontawesome-webfont.woff") format("woff"),url("../fonts/fontawesome-webfont.ttf") format("truetype"),url("../fonts/fontawesome-webfont.svg#FontAwesome") format("svg")}.fa:before{display:inline-block;font-family:FontAwesome;font-style:normal;font-weight:normal;line-height:1;text-decoration:inherit}a .fa{display:inline-block;text-decoration:inherit}li .fa{display:inline-block}li .fa-large:before,li .fa-large:before{width:1.875em}ul.fas{list-style-type:none;margin-left:2em;text-indent:-0.8em}ul.fas li .fa{width:.8em}ul.fas li .fa-large:before,ul.fas li .fa-large:before{vertical-align:baseline}.fa-book:before{content:""}.icon-book:before{content:""}.fa-caret-down:before{content:""}.icon-caret-down:before{content:""}.fa-caret-up:before{content:""}.icon-caret-up:before{content:""}.fa-caret-left:before{content:""}.icon-caret-left:before{content:""}.fa-caret-right:before{content:""}.icon-caret-right:before{content:""}.rst-versions{position:fixed;bottom:0;left:0;width:300px;color:#fcfcfc;background:#1f1d1d;font-family:"Lato","proxima-nova","Helvetica Neue",Arial,sans-serif;z-index:400}.rst-versions a{color:#2980B9;text-decoration:none}.rst-versions .rst-badge-small{display:none}.rst-versions .rst-current-version{padding:12px;background-color:#272525;display:block;text-align:right;font-size:90%;cursor:pointer;color:#27AE60;*zoom:1}.rst-versions .rst-current-version:before,.rst-versions .rst-current-version:after{display:table;content:""}.rst-versions .rst-current-version:after{clear:both}.rst-versions .rst-current-version .fa{color:#fcfcfc}.rst-versions .rst-current-version .fa-book{float:left}.rst-versions .rst-current-version .icon-book{float:left}.rst-versions .rst-current-version.rst-out-of-date{background-color:#E74C3C;color:#fff}.rst-versions .rst-current-version.rst-active-old-version{background-color:#F1C40F;color:#000}.rst-versions.shift-up{height:auto;max-height:100%;overflow-y:scroll}.rst-versions.shift-up .rst-other-versions{display:block}.rst-versions .rst-other-versions{font-size:90%;padding:12px;color:gray;display:none}.rst-versions .rst-other-versions hr{display:block;height:1px;border:0;margin:20px 0;padding:0;border-top:solid 1px #413d3d}.rst-versions .rst-other-versions dd{display:inline-block;margin:0}.rst-versions .rst-other-versions dd a{display:inline-block;padding:6px;color:#fcfcfc}.rst-versions.rst-badge{width:auto;bottom:20px;right:20px;left:auto;border:none;max-width:300px;max-height:90%}.rst-versions.rst-badge .icon-book{float:none}.rst-versions.rst-badge .fa-book{float:none}.rst-versions.rst-badge.shift-up .rst-current-version{text-align:right}.rst-versions.rst-badge.shift-up .rst-current-version .fa-book{float:left}.rst-versions.rst-badge.shift-up .rst-current-version .icon-book{float:left}.rst-versions.rst-badge .rst-current-version{width:auto;height:30px;line-height:30px;padding:0 6px;display:block;text-align:center}@media screen and (max-width: 768px){.rst-versions{width:85%;display:none}.rst-versions.shift{display:block}}

liegroups/docs/build/html/_static/doctools.js

@@ -0,0 +1,314 @@
+/*
+ * doctools.js
+ * ~~~~~~~~~~~
+ *
+ * Sphinx JavaScript utilities for all documentation.
+ *
+ * :copyright: Copyright 2007-2019 by the Sphinx team, see AUTHORS.
+ * :license: BSD, see LICENSE for details.
+ *
+ */
+
+/**
+ * select a different prefix for underscore
+ */
+$u = _.noConflict();
+
+/**
+ * make the code below compatible with browsers without
+ * an installed firebug like debugger
+if (!window.console || !console.firebug) {
+  var names = ["log", "debug", "info", "warn", "error", "assert", "dir",
+    "dirxml", "group", "groupEnd", "time", "timeEnd", "count", "trace",
+    "profile", "profileEnd"];
+  window.console = {};
+  for (var i = 0; i < names.length; ++i)
+    window.console[names[i]] = function() {};
+}
+ */
+
+/**
+ * small helper function to urldecode strings
+ */
+jQuery.urldecode = function(x) {
+  return decodeURIComponent(x).replace(/\+/g, ' ');
+};
+
+/**
+ * small helper function to urlencode strings
+ */
+jQuery.urlencode = encodeURIComponent;
+
+/**
+ * This function returns the parsed url parameters of the
+ * current request. Multiple values per key are supported,
+ * it will always return arrays of strings for the value parts.
+ */
+jQuery.getQueryParameters = function(s) {
+  if (typeof s === 'undefined')
+    s = document.location.search;
+  var parts = s.substr(s.indexOf('?') + 1).split('&');
+  var result = {};
+  for (var i = 0; i < parts.length; i++) {
+    var tmp = parts[i].split('=', 2);
+    var key = jQuery.urldecode(tmp[0]);
+    var value = jQuery.urldecode(tmp[1]);
+    if (key in result)
+      result[key].push(value);
+    else
+      result[key] = [value];
+  }
+  return result;
+};
+
+/**
+ * highlight a given string on a jquery object by wrapping it in
+ * span elements with the given class name.
+ */
+jQuery.fn.highlightText = function(text, className) {
+  function highlight(node, addItems) {
+    if (node.nodeType === 3) {
+      var val = node.nodeValue;
+      var pos = val.toLowerCase().indexOf(text);
+      if (pos >= 0 &&
+          !jQuery(node.parentNode).hasClass(className) &&
+          !jQuery(node.parentNode).hasClass("nohighlight")) {
+        var span;
+        var isInSVG = jQuery(node).closest("body, svg, foreignObject").is("svg");
+        if (isInSVG) {
+          span = document.createElementNS("http://www.w3.org/2000/svg", "tspan");
+        } else {
+          span = document.createElement("span");
+          span.className = className;
+        }
+        span.appendChild(document.createTextNode(val.substr(pos, text.length)));
+        node.parentNode.insertBefore(span, node.parentNode.insertBefore(
+          document.createTextNode(val.substr(pos + text.length)),
+          node.nextSibling));
+        node.nodeValue = val.substr(0, pos);
+        if (isInSVG) {
+          var rect = document.createElementNS("http://www.w3.org/2000/svg", "rect");
+          var bbox = node.parentElement.getBBox();
+          rect.x.baseVal.value = bbox.x;
+          rect.y.baseVal.value = bbox.y;
+          rect.width.baseVal.value = bbox.width;
+          rect.height.baseVal.value = bbox.height;
+          rect.setAttribute('class', className);
+          addItems.push({
+              "parent": node.parentNode,
+              "target": rect});
+        }
+      }
+    }
+    else if (!jQuery(node).is("button, select, textarea")) {
+      jQuery.each(node.childNodes, function() {
+        highlight(this, addItems);
+      });
+    }
+  }
+  var addItems = [];
+  var result = this.each(function() {
+    highlight(this, addItems);
+  });
+  for (var i = 0; i < addItems.length; ++i) {
+    jQuery(addItems[i].parent).before(addItems[i].target);
+  }
+  return result;
+};
+
+/*
+ * backward compatibility for jQuery.browser
+ * This will be supported until firefox bug is fixed.
+ */
+if (!jQuery.browser) {
+  jQuery.uaMatch = function(ua) {
+    ua = ua.toLowerCase();
+
+    var match = /(chrome)[ \/]([\w.]+)/.exec(ua) ||
+      /(webkit)[ \/]([\w.]+)/.exec(ua) ||
+      /(opera)(?:.*version|)[ \/]([\w.]+)/.exec(ua) ||
+      /(msie) ([\w.]+)/.exec(ua) ||
+      ua.indexOf("compatible") < 0 && /(mozilla)(?:.*? rv:([\w.]+)|)/.exec(ua) ||
+      [];
+
+    return {
+      browser: match[ 1 ] || "",
+      version: match[ 2 ] || "0"
+    };
+  };
+  jQuery.browser = {};
+  jQuery.browser[jQuery.uaMatch(navigator.userAgent).browser] = true;
+}
+
+/**
+ * Small JavaScript module for the documentation.
+ */
+var Documentation = {
+
+  init : function() {
+    this.fixFirefoxAnchorBug();
+    this.highlightSearchWords();
+    this.initIndexTable();
+    if (DOCUMENTATION_OPTIONS.NAVIGATION_WITH_KEYS) {
+      this.initOnKeyListeners();
+    }
+  },
+
+  /**
+   * i18n support
+   */
+  TRANSLATIONS : {},
+  PLURAL_EXPR : function(n) { return n === 1 ? 0 : 1; },
+  LOCALE : 'unknown',
+
+  // gettext and ngettext don't access this so that the functions
+  // can safely bound to a different name (_ = Documentation.gettext)
+  gettext : function(string) {
+    var translated = Documentation.TRANSLATIONS[string];
+    if (typeof translated === 'undefined')
+      return string;
+    return (typeof translated === 'string') ? translated : translated[0];
+  },
+
+  ngettext : function(singular, plural, n) {
+    var translated = Documentation.TRANSLATIONS[singular];
+    if (typeof translated === 'undefined')
+      return (n == 1) ? singular : plural;
+    return translated[Documentation.PLURALEXPR(n)];
+  },
+
+  addTranslations : function(catalog) {
+    for (var key in catalog.messages)
+      this.TRANSLATIONS[key] = catalog.messages[key];
+    this.PLURAL_EXPR = new Function('n', 'return +(' + catalog.plural_expr + ')');
+    this.LOCALE = catalog.locale;
+  },
+
+  /**
+   * add context elements like header anchor links
+   */
+  addContextElements : function() {
+    $('div[id] > :header:first').each(function() {
+      $('<a class="headerlink">\u00B6</a>').
+      attr('href', '#' + this.id).
+      attr('title', _('Permalink to this headline')).
+      appendTo(this);
+    });
+    $('dt[id]').each(function() {
+      $('<a class="headerlink">\u00B6</a>').
+      attr('href', '#' + this.id).
+      attr('title', _('Permalink to this definition')).
+      appendTo(this);
+    });
+  },
+
+  /**
+   * workaround a firefox stupidity
+   * see: https://bugzilla.mozilla.org/show_bug.cgi?id=645075
+   */
+  fixFirefoxAnchorBug : function() {
+    if (document.location.hash && $.browser.mozilla)
+      window.setTimeout(function() {
+        document.location.href += '';
+      }, 10);
+  },
+
+  /**
+   * highlight the search words provided in the url in the text
+   */
+  highlightSearchWords : function() {
+    var params = $.getQueryParameters();
+    var terms = (params.highlight) ? params.highlight[0].split(/\s+/) : [];
+    if (terms.length) {
+      var body = $('div.body');
+      if (!body.length) {
+        body = $('body');
+      }
+      window.setTimeout(function() {
+        $.each(terms, function() {
+          body.highlightText(this.toLowerCase(), 'highlighted');
+        });
+      }, 10);
+      $('<p class="highlight-link"><a href="javascript:Documentation.' +
+        'hideSearchWords()">' + _('Hide Search Matches') + '</a></p>')
+          .appendTo($('#searchbox'));
+    }
+  },
+
+  /**
+   * init the domain index toggle buttons
+   */
+  initIndexTable : function() {
+    var togglers = $('img.toggler').click(function() {
+      var src = $(this).attr('src');
+      var idnum = $(this).attr('id').substr(7);
+      $('tr.cg-' + idnum).toggle();
+      if (src.substr(-9) === 'minus.png')
+        $(this).attr('src', src.substr(0, src.length-9) + 'plus.png');
+      else
+        $(this).attr('src', src.substr(0, src.length-8) + 'minus.png');
+    }).css('display', '');
+    if (DOCUMENTATION_OPTIONS.COLLAPSE_INDEX) {
+        togglers.click();
+    }
+  },
+
+  /**
+   * helper function to hide the search marks again
+   */
+  hideSearchWords : function() {
+    $('#searchbox .highlight-link').fadeOut(300);
+    $('span.highlighted').removeClass('highlighted');
+  },
+
+  /**
+   * make the url absolute
+   */
+  makeURL : function(relativeURL) {
+    return DOCUMENTATION_OPTIONS.URL_ROOT + '/' + relativeURL;
+  },
+
+  /**
+   * get the current relative url
+   */
+  getCurrentURL : function() {
+    var path = document.location.pathname;
+    var parts = path.split(/\//);
+    $.each(DOCUMENTATION_OPTIONS.URL_ROOT.split(/\//), function() {
+      if (this === '..')
+        parts.pop();
+    });
+    var url = parts.join('/');
+    return path.substring(url.lastIndexOf('/') + 1, path.length - 1);
+  },
+
+  initOnKeyListeners: function() {
+    $(document).keyup(function(event) {
+      var activeElementType = document.activeElement.tagName;
+      // don't navigate when in search box or textarea
+      if (activeElementType !== 'TEXTAREA' && activeElementType !== 'INPUT' && activeElementType !== 'SELECT') {
+        switch (event.keyCode) {
+          case 37: // left
+            var prevHref = $('link[rel="prev"]').prop('href');
+            if (prevHref) {
+              window.location.href = prevHref;
+              return false;
+            }
+          case 39: // right
+            var nextHref = $('link[rel="next"]').prop('href');
+            if (nextHref) {
+              window.location.href = nextHref;
+              return false;
+            }
+        }
+      }
+    });
+  }
+};
+
+// quick alias for translations
+_ = Documentation.gettext;
+
+$(document).ready(function() {
+  Documentation.init();
+});

liegroups/docs/build/html/_static/documentation_options.js

@@ -0,0 +1,10 @@
+var DOCUMENTATION_OPTIONS = {
+    URL_ROOT: document.getElementById("documentation_options").getAttribute('data-url_root'),
+    VERSION: '1.1.0',
+    LANGUAGE: 'None',
+    COLLAPSE_INDEX: false,
+    FILE_SUFFIX: '.html',
+    HAS_SOURCE: true,
+    SOURCELINK_SUFFIX: '.txt',
+    NAVIGATION_WITH_KEYS: false
+};