MatrixExponentialMap.h

Go to the documentation of this file.

// Copyright 2015 Sensics, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//        http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef INCLUDED_ExponentialMap_h_GUID_FB61635E_CF8A_4FAE_5343_2258F4BC1E60
#define INCLUDED_ExponentialMap_h_GUID_FB61635E_CF8A_4FAE_5343_2258F4BC1E60

// Internal Includes
// - none

// Library/third-party includes
#include <osvr/Util/EigenCoreGeometry.h>

// Standard includes
// - none

namespace osvr {
namespace kalman {

    template <typename Derived>
    inline Eigen::Matrix3d
    makeSkewSymmetrixCrossProductMatrix(Eigen::MatrixBase<Derived> const &v) {
        Eigen::Matrix3d ret;
        // clang-format off
                ret << 0, -v.z(), v.y(),
                       v.z(), 0, -v.x(),
                       -v.y(), v.x(), 0;
        // clang-format on
        return ret;
    }

    namespace matrix_exponential_map {
        template <typename T> inline void avoidSingularities(T &&omega) {
            // if magnitude gets too close to 2pi, in this case, pi...
            if (omega.squaredNorm() > M_PI * M_PI) {
                // replace omega with an equivalent one.
                omega = ((1 - (2 * M_PI) / omega.norm()) * omega).eval();
            }
        }

        template <typename Derived>
        inline double getAngle(Eigen::MatrixBase<Derived> const &omega) {
            return omega.norm();
        }

        template <typename Derived>
        inline Eigen::Quaterniond
        getQuat(Eigen::MatrixBase<Derived> const &omega) {
            auto theta = getAngle(omega);
            auto xyz = omega * std::sin(theta / 2.);
            return Eigen::Quaterniond(std::cos(theta / 2.), xyz.x(), xyz.y(),
                                      xyz.z());
        }

        class ExponentialMapData {
          public:
            EIGEN_MAKE_ALIGNED_OPERATOR_NEW
            template <typename Derived>
            explicit ExponentialMapData(Eigen::MatrixBase<Derived> const &omega)
                : m_omega(omega) {}

            ExponentialMapData() : m_omega(Eigen::Vector3d::Zero()) {}

            ExponentialMapData &operator=(ExponentialMapData const &other) {
                if (&other != this) {
                    m_omega = other.m_omega;
                    m_gotTheta = other.m_gotTheta;
                    if (m_gotTheta) {
                        m_theta = other.m_theta;
                    }
                    m_gotBigOmega = other.m_gotBigOmega;
                    if (m_gotBigOmega) {
                        m_bigOmega = other.m_bigOmega;
                    }
                    m_gotR = other.m_gotR;
                    if (m_gotR) {
                        m_R = other.m_R;
                    }
                    m_gotQuat = other.m_gotQuat;
                    if (m_gotQuat) {
                        m_quat = other.m_quat;
                    }
                }
                return *this;
            }

            ExponentialMapData &operator=(ExponentialMapData &&other) {
                if (&other != this) {
                    m_omega = std::move(other.m_omega);
                    m_gotTheta = std::move(other.m_gotTheta);
                    if (m_gotTheta) {
                        m_theta = std::move(other.m_theta);
                    }
                    m_gotBigOmega = std::move(other.m_gotBigOmega);
                    if (m_gotBigOmega) {
                        m_bigOmega = std::move(other.m_bigOmega);
                    }
                    m_gotR = std::move(other.m_gotR);
                    if (m_gotR) {
                        m_R = std::move(other.m_R);
                    }
                    m_gotQuat = std::move(other.m_gotQuat);
                    if (m_gotQuat) {
                        m_quat = std::move(other.m_quat);
                    }
                }
                return *this;
            }

            template <typename Derived>
            void reset(Eigen::MatrixBase<Derived> const &omega) {
                *this = ExponentialMapData(omega);
            }

            Eigen::Matrix3d const &getBigOmega() {
                if (!m_gotBigOmega) {
                    m_gotBigOmega = true;
                    m_bigOmega = makeSkewSymmetrixCrossProductMatrix(m_omega);
                }
                return m_bigOmega;
            }

            double getTheta() {
                if (!m_gotTheta) {
                    m_gotTheta = true;
                    m_theta = getAngle(m_omega);
                }
                return m_theta;
            }

            Eigen::Matrix3d const &getRotationMatrix() {
                if (!m_gotR) {
                    m_gotR = true;
                    auto theta = getTheta();
                    auto &Omega = getBigOmega();
                    double k1 = 1. - theta * theta / 6.;

                    double k2 = theta / 2. - theta * theta * theta / 24.;

                    m_R = Eigen::Matrix3d::Identity() + k1 * Omega +
                          k2 * Omega * Omega;
                }
                return m_R;
            }

            Eigen::Quaterniond const &getQuaternion() {
                if (!m_gotQuat) {
                    m_gotQuat = true;
                    auto theta = getTheta();
                    auto xyz = m_omega * std::sin(theta / 2.);
                    m_quat = Eigen::Quaterniond(std::cos(theta / 2.), xyz.x(),
                                                xyz.y(), xyz.z());
                }
                return m_quat;
            }

          private:
            Eigen::Vector3d m_omega;
            bool m_gotTheta = false;
            double m_theta;
            bool m_gotBigOmega = false;
            Eigen::Matrix3d m_bigOmega;
            bool m_gotR = false;
            Eigen::Matrix3d m_R;
            bool m_gotQuat = false;
            Eigen::Quaterniond m_quat;
        };

    } // namespace exponential_map
} // namespace kalman
} // namespace osvr
#endif // INCLUDED_ExponentialMap_h_GUID_FB61635E_CF8A_4FAE_5343_2258F4BC1E60