external: Update flexkalman

src/external/flexkalman/AbsoluteOrientationMeasurement.h

@@ -1,7 +1,7 @@
 /** @file
     @brief Header for measurements of absolute orientation.
 
-    @date 2015
+    @date 2015, 2020
 
     @author
     Ryan Pavlik
@@ -12,6 +12,9 @@
 */
 
 // Copyright 2015 Sensics, Inc.
+// Copyright 2020 Collabora, Ltd.
+//
+// SPDX-License-Identifier: Apache-2.0
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -43,6 +46,13 @@
 
 namespace flexkalman {
 
+//! Default implementation: overload if this won't work for your state type.
+//! @see AbsoluteOrientationMeasurementBase
+template <typename State>
+types::Vector<3> predictAbsoluteOrientationMeasurement(State const &s) {
+    return s.incrementalOrientation();
+}
+
 class AbsoluteOrientationMeasurementBase {
   public:
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
@@ -60,7 +70,7 @@ class AbsoluteOrientationMeasurementBase {
 
     template <typename State>
     MeasurementVector predictMeasurement(State const &state) const {
-        return state.incrementalOrientation();
+        return predictAbsoluteOrientationMeasurement(state);
     }
     template <typename State>
     MeasurementVector getResidual(MeasurementVector const &predictedMeasurement,

src/external/flexkalman/AbsolutePositionLeverArmMeasurement.h

@@ -0,0 +1,73 @@
+/** @file
+    @brief Header for measurements of absolute position with an offset.
+
+    @date 2019-2020
+
+    @author
+    Ryan Pavlik
+    <ryan.pavlik@collabora.com>
+*/
+// Copyright 2019-2020, Collabora, Ltd.
+// SPDX-License-Identifier: BSL-1.0 OR Apache-2.0
+
+#pragma once
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+#include "BaseTypes.h"
+
+namespace flexkalman {
+/*!
+ * For PS Move-like things, where there's a directly-computed absolute position
+ * that is not at the tracked body's origin.
+ */
+class AbsolutePositionLeverArmMeasurement
+    : public MeasurementBase<AbsolutePositionLeverArmMeasurement> {
+  public:
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+    static constexpr size_t Dimension = 3;
+    using MeasurementVector = types::Vector<Dimension>;
+    using MeasurementSquareMatrix = types::SquareMatrix<Dimension>;
+
+    /*!
+     * @todo the point we get from the camera isn't the center of the ball,
+     * but the center of the visible surface of the ball - a closer
+     * approximation would be translation along the vector to the center of
+     * projection....
+     */
+    AbsolutePositionLeverArmMeasurement(
+        MeasurementVector const &measurement,
+        MeasurementVector const &knownLocationInBodySpace,
+        MeasurementVector const &variance)
+        : measurement_(measurement),
+          knownLocationInBodySpace_(knownLocationInBodySpace),
+          covariance_(variance.asDiagonal()) {}
+
+    template <typename State>
+    MeasurementSquareMatrix const &getCovariance(State const & /*s*/) {
+        return covariance_;
+    }
+
+    template <typename State>
+    types::Vector<3> predictMeasurement(State const &s) const {
+        return s.getIsometry() * knownLocationInBodySpace_;
+    }
+
+    template <typename State>
+    MeasurementVector getResidual(MeasurementVector const &predictedMeasurement,
+                                  State const & /*s*/) const {
+        return measurement_ - predictedMeasurement;
+    }
+
+    template <typename State>
+    MeasurementVector getResidual(State const &s) const {
+        return getResidual(predictMeasurement(s), s);
+    }
+
+  private:
+    MeasurementVector measurement_;
+    MeasurementVector knownLocationInBodySpace_;
+    MeasurementSquareMatrix covariance_;
+};
+} // namespace flexkalman

src/external/flexkalman/AbsolutePositionMeasurement.h

@@ -10,6 +10,8 @@
 
 // Copyright 2015 Sensics, Inc.
 //
+// SPDX-License-Identifier: Apache-2.0
+//
 // 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

src/external/flexkalman/AngularVelocityMeasurement.h

@@ -14,6 +14,8 @@
 // Copyright 2015 Sensics, Inc.
 // Copyright 2019 Collabora, Ltd.
 //
+// SPDX-License-Identifier: Apache-2.0
+//
 // 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

src/external/flexkalman/AugmentedProcessModel.h

@@ -10,6 +10,8 @@
 
 // Copyright 2015 Sensics, Inc.
 //
+// SPDX-License-Identifier: Apache-2.0
+//
 // 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

src/external/flexkalman/AugmentedState.h

@@ -10,6 +10,8 @@
 
 // Copyright 2015 Sensics, Inc.
 //
+// SPDX-License-Identifier: Apache-2.0
+//
 // 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

src/external/flexkalman/BaseTypes.h

@@ -10,6 +10,8 @@
 
 // Copyright 2019 Collabora, Ltd.
 //
+// SPDX-License-Identifier: Apache-2.0 OR BSL-1.0
+//
 // 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

src/external/flexkalman/ConstantProcess.h

@@ -10,6 +10,8 @@
 
 // Copyright 2015 Sensics, Inc.
 //
+// SPDX-License-Identifier: Apache-2.0
+//
 // 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

src/external/flexkalman/EigenQuatExponentialMap.h

@@ -10,6 +10,8 @@
 
 // Copyright 2016 Sensics, Inc.
 //
+// SPDX-License-Identifier: Apache-2.0
+//
 // 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

src/external/flexkalman/ExternalQuaternion.h

@@ -10,6 +10,8 @@
 
 // Copyright 2015 Sensics, Inc.
 //
+// SPDX-License-Identifier: Apache-2.0
+//
 // 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

src/external/flexkalman/FlexibleKalmanBase.h

@@ -15,6 +15,8 @@
 // Copyright 2015-2016 Sensics, Inc.
 // Copyright 2019 Collabora, Ltd.
 //
+// SPDX-License-Identifier: Apache-2.0
+//
 // 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

src/external/flexkalman/FlexibleKalmanCorrect.h

@@ -15,6 +15,8 @@
 // Copyright 2015-2016 Sensics, Inc.
 // Copyright 2019 Collabora, Ltd.
 //
+// SPDX-License-Identifier: Apache-2.0
+//
 // 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

src/external/flexkalman/FlexibleKalmanFilter.h

@@ -15,6 +15,8 @@
 // Copyright 2015 Sensics, Inc.
 // Copyright 2019 Collabora, Ltd.
 //
+// SPDX-License-Identifier: Apache-2.0
+//
 // 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

src/external/flexkalman/FlexibleKalmanMeta.h

@@ -57,6 +57,7 @@ namespace flexkalman {
 #include "AngularVelocityMeasurement.h"
 #include "AugmentedProcessModel.h"
 #include "AugmentedState.h"
+#include "BaseTypes.h"
 #include "ConstantProcess.h"
 #include "EigenQuatExponentialMap.h"
 #include "ExternalQuaternion.h"
@@ -67,10 +68,14 @@ namespace flexkalman {
 #include "MatrixExponentialMap.h"
 #include "OrientationConstantVelocity.h"
 #include "OrientationState.h"
+#include "PoseConstantAccel.h"
 #include "PoseConstantVelocity.h"
+#include "PoseConstantVelocityGeneric.h"
 #include "PoseDampedConstantVelocity.h"
 #include "PoseSeparatelyDampedConstantVelocity.h"
 #include "PoseState.h"
 #include "PoseStateExponentialMap.h"
+#include "PoseStateWithAccel.h"
 #include "PureVectorState.h"
+#include "SO3.h"
 #include "SigmaPointGenerator.h"

src/external/flexkalman/FlexibleUnscentedCorrect.h

@@ -23,6 +23,8 @@
 // Copyright 2016 Sensics, Inc.
 // Copyright 2019 Collabora, Ltd.
 //
+// SPDX-License-Identifier: Apache-2.0
+//
 // 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

src/external/flexkalman/MatrixExponentialMap.h

@@ -1,7 +1,7 @@
 /** @file
     @brief Header
 
-    @date 2015
+    @date 2015, 2019
 
     @author
     Sensics, Inc.
@@ -9,6 +9,9 @@
 */
 
 // Copyright 2015 Sensics, Inc.
+// Copyright 2019 Collabora, Ltd.
+//
+// SPDX-License-Identifier: Apache-2.0
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.

src/external/flexkalman/OrientationConstantVelocity.h

@@ -15,6 +15,8 @@
 // Copyright 2015 Sensics, Inc.
 // Copyright 2019 Collabora, Ltd.
 //
+// SPDX-License-Identifier: Apache-2.0
+//
 // 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

src/external/flexkalman/OrientationState.h

@@ -15,6 +15,8 @@
 // Copyright 2015 Sensics, Inc.
 // Copyright 2019 Collabora, Ltd.
 //
+// SPDX-License-Identifier: Apache-2.0
+//
 // 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

src/external/flexkalman/PoseConstantVelocity.h

@@ -15,6 +15,8 @@
 // Copyright 2015 Sensics, Inc.
 // Copyright 2019 Collabora, Ltd.
 //
+// SPDX-License-Identifier: Apache-2.0
+//
 // 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

src/external/flexkalman/PoseConstantVelocityGeneric.h

@@ -0,0 +1,134 @@
+/** @file
+    @brief Header
+
+    @date 2015-2020
+
+    @author
+    Ryan Pavlik
+    <ryan.pavlik@collabora.com>
+
+    @author
+    Sensics, Inc.
+    <http://sensics.com/osvr>
+*/
+
+// Copyright 2015 Sensics, Inc.
+// Copyright 2019-2020 Collabora, Ltd.
+//
+// SPDX-License-Identifier: Apache-2.0
+//
+// 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.
+
+#pragma once
+
+// Internal Includes
+#include "FlexibleKalmanBase.h"
+
+// Library/third-party includes
+// - none
+
+// Standard includes
+#include <cassert>
+
+namespace flexkalman {
+
+//! A constant-velocity model for a 6DOF pose (with velocities)
+template <typename StateType>
+class PoseConstantVelocityGenericProcessModel
+    : public ProcessModelBase<
+          PoseConstantVelocityGenericProcessModel<StateType>> {
+  public:
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+    using State = StateType;
+    using StateVector = typename State::StateVector;
+    using StateSquareMatrix = typename State::StateSquareMatrix;
+    using NoiseAutocorrelation = types::Vector<6>;
+    PoseConstantVelocityGenericProcessModel(double positionNoise = 0.01,
+                                            double orientationNoise = 0.1) {
+        setNoiseAutocorrelation(positionNoise, orientationNoise);
+    }
+    void setNoiseAutocorrelation(double positionNoise = 0.01,
+                                 double orientationNoise = 0.1) {
+        m_mu.head<3>() = types::Vector<3>::Constant(positionNoise);
+        m_mu.tail<3>() = types::Vector<3>::Constant(orientationNoise);
+    }
+    void setNoiseAutocorrelation(NoiseAutocorrelation const &noise) {
+        m_mu = noise;
+    }
+
+    //! Also known as the "process model jacobian" in TAG, this is A.
+    StateSquareMatrix getStateTransitionMatrix(State const &s,
+                                               double dt) const {
+        // using argument-dependent lookup
+        return stateTransitionMatrix(s, dt);
+    }
+
+    //! Does not update error covariance
+    void predictStateOnly(State &s, double dt) const {
+        FLEXKALMAN_DEBUG_OUTPUT("Time change", dt);
+        // using argument-dependent lookup
+        applyVelocity(s, dt);
+    }
+    //! Updates state vector and error covariance
+    void predictState(State &s, double dt) const {
+        predictStateOnly(s, dt);
+        auto Pminus = predictErrorCovariance(s, *this, dt);
+        s.setErrorCovariance(Pminus);
+    }
+
+    /*!
+     * This is Q(deltaT) - the Sampled Process Noise Covariance
+     * @return a matrix of dimension n x n.
+     *
+     * Like all covariance matrices, it is real symmetrical (self-adjoint),
+     * so .selfAdjointView<Eigen::Upper>() might provide useful performance
+     * enhancements in some algorithms.
+     */
+    StateSquareMatrix getSampledProcessNoiseCovariance(double dt) const {
+        constexpr auto dim = getDimension<State>();
+        StateSquareMatrix cov = StateSquareMatrix::Zero();
+        auto dt3 = (dt * dt * dt) / 3;
+        auto dt2 = (dt * dt) / 2;
+        for (std::size_t xIndex = 0; xIndex < dim / 2; ++xIndex) {
+            auto xDotIndex = xIndex + dim / 2;
+            // xIndex is 'i' and xDotIndex is 'j' in eq. 4.8
+            const auto mu = getMu(xIndex);
+            cov(xIndex, xIndex) = mu * dt3;
+            auto symmetric = mu * dt2;
+            cov(xIndex, xDotIndex) = symmetric;
+            cov(xDotIndex, xIndex) = symmetric;
+            cov(xDotIndex, xDotIndex) = mu * dt;
+        }
+        return cov;
+    }
+
+  private:
+    /*!
+     * this is mu-arrow, the auto-correlation vector of the noise
+     * sources
+     */
+    NoiseAutocorrelation m_mu;
+    double getMu(std::size_t index) const {
+        assert(index < (getDimension<State>() / 2) &&
+               "Should only be passing "
+               "'i' - the main state, not "
+               "the derivative");
+        // This may not be totally correct but it's one of the parameters
+        // you can kind of fudge in kalman filters anyway.
+        // Should techincally be the diagonal of the correlation kernel of
+        // the noise sources. (p77, p197 in Welch 1996)
+        return m_mu(index);
+    }
+};
+
+} // namespace flexkalman

src/external/flexkalman/PoseDampedConstantVelocity.h

@@ -13,7 +13,9 @@
 */
 
 // Copyright 2015 Sensics, Inc.
-// Copyright 2019 Collabora, Ltd.
+// Copyright 2019-2020 Collabora, Ltd.
+//
+// SPDX-License-Identifier: Apache-2.0
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -79,9 +81,10 @@ class PoseDampedConstantVelocityProcessModel
     }
 
     //! Also known as the "process model jacobian" in TAG, this is A.
-    StateSquareMatrix getStateTransitionMatrix(State const &, double dt) const {
+    StateSquareMatrix getStateTransitionMatrix(State const &s,
+                                               double dt) const {
         return pose_externalized_rotation::
-            stateTransitionMatrixWithVelocityDamping(dt, m_damp);
+            stateTransitionMatrixWithVelocityDamping(s, dt, m_damp);
     }
 
     void predictStateOnly(State &s, double dt) const {

src/external/flexkalman/PoseSeparatelyDampedConstantVelocity.h

@@ -13,7 +13,9 @@
 */
 
 // Copyright 2015 Sensics, Inc.
-// Copyright 2019 Collabora, Ltd.
+// Copyright 2019-2020 Collabora, Ltd.
+//
+// SPDX-License-Identifier: Apache-2.0
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -31,8 +33,7 @@
 
 // Internal Includes
 #include "BaseTypes.h"
-#include "PoseConstantVelocity.h"
+#include "PoseConstantVelocityGeneric.h"
-#include "PoseState.h"
 
 // Library/third-party includes
 // - none
@@ -47,16 +48,17 @@ namespace flexkalman {
  * damping of the velocities inspired by TAG. This model has separate
  * damping/attenuation of linear and angular velocities.
  */
+template <typename StateType>
 class PoseSeparatelyDampedConstantVelocityProcessModel
     : public ProcessModelBase<
-          PoseSeparatelyDampedConstantVelocityProcessModel> {
+          PoseSeparatelyDampedConstantVelocityProcessModel<StateType>> {
   public:
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-    using State = pose_externalized_rotation::State;
+    using State = StateType;
-    using StateVector = pose_externalized_rotation::StateVector;
+    using StateVector = typename StateType::StateVector;
-    using StateSquareMatrix = pose_externalized_rotation::StateSquareMatrix;
+    using StateSquareMatrix = typename StateType::StateSquareMatrix;
-    using BaseProcess = PoseConstantVelocityProcessModel;
+    using BaseProcess = PoseConstantVelocityGenericProcessModel<State>;
-    using NoiseAutocorrelation = BaseProcess::NoiseAutocorrelation;
+    using NoiseAutocorrelation = typename BaseProcess::NoiseAutocorrelation;
     PoseSeparatelyDampedConstantVelocityProcessModel(
         double positionDamping = 0.3, double orientationDamping = 0.01,
         double positionNoise = 0.01, double orientationNoise = 0.1)
@@ -84,17 +86,17 @@ class PoseSeparatelyDampedConstantVelocityProcessModel
     }
 
     //! Also known as the "process model jacobian" in TAG, this is A.
-    StateSquareMatrix getStateTransitionMatrix(State const &, double dt) const {
+    StateSquareMatrix getStateTransitionMatrix(State const &s,
-        return pose_externalized_rotation::
+                                               double dt) const {
-            stateTransitionMatrixWithSeparateVelocityDamping(dt, m_posDamp,
+        // using argument-dependent lookup
-                                                             m_oriDamp);
+        return stateTransitionMatrixWithSeparateVelocityDamping(
+            s, dt, m_posDamp, m_oriDamp);
     }
 
     void predictStateOnly(State &s, double dt) const {
         m_constantVelModel.predictStateOnly(s, dt);
-        // Dampen velocities
+        // Dampen velocities - using argument-dependent lookup
-        pose_externalized_rotation::separatelyDampenVelocities(s, m_posDamp,
+        separatelyDampenVelocities(s, m_posDamp, m_oriDamp, dt);
-                                                               m_oriDamp, dt);
     }
     void predictState(State &s, double dt) const {
         predictStateOnly(s, dt);

src/external/flexkalman/PoseState.h

@@ -13,7 +13,9 @@
 */
 
 // Copyright 2015 Sensics, Inc.
-// Copyright 2019 Collabora, Ltd.
+// Copyright 2019-2020 Collabora, Ltd.
+//
+// SPDX-License-Identifier: Apache-2.0
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -75,40 +77,13 @@ namespace pose_externalized_rotation {
         return std::pow(damping, dt);
     }
 
-    /*!
-     * Returns the state transition matrix for a constant velocity with a
-     * single damping parameter (not for direct use in computing state
-     * transition, because it is very sparse, but in computing other
-     * values)
-     */
-    inline StateSquareMatrix
-    stateTransitionMatrixWithVelocityDamping(double dt, double damping) {
-        // eq. 4.5 in Welch 1996
-        auto A = stateTransitionMatrix(dt);
-        A.bottomRightCorner<6, 6>() *= computeAttenuation(damping, dt);
-        return A;
-    }
-
-    /*!
-     * Returns the state transition matrix for a constant velocity with
-     * separate damping paramters for linear and angular velocity (not for
-     * direct use in computing state transition, because it is very sparse,
-     * but in computing other values)
-     */
-    inline StateSquareMatrix stateTransitionMatrixWithSeparateVelocityDamping(
-        double dt, double posDamping, double oriDamping) {
-        // eq. 4.5 in Welch 1996
-        auto A = stateTransitionMatrix(dt);
-        A.block<3, 3>(6, 6) *= computeAttenuation(posDamping, dt);
-        A.bottomRightCorner<3, 3>() *= computeAttenuation(oriDamping, dt);
-        return A;
-    }
-
     class State : public StateBase<State> {
       public:
         EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
         static constexpr size_t Dimension = 12;
+        using StateVector = types::Vector<Dimension>;
+        using StateSquareMatrix = types::SquareMatrix<Dimension>;
 
         //! Default constructor
         State()
@@ -245,6 +220,40 @@ namespace pose_externalized_rotation {
         state.velocity() *= computeAttenuation(posDamping, dt);
         state.angularVelocity() *= computeAttenuation(oriDamping, dt);
     }
+
+    inline StateSquareMatrix stateTransitionMatrix(State const & /* state */,
+                                                   double dt) {
+        return stateTransitionMatrix(dt);
+    }
+    /*!
+     * Returns the state transition matrix for a constant velocity with a
+     * single damping parameter (not for direct use in computing state
+     * transition, because it is very sparse, but in computing other
+     * values)
+     */
+    inline StateSquareMatrix
+    stateTransitionMatrixWithVelocityDamping(State const &state, double dt,
+                                             double damping) {
+        // eq. 4.5 in Welch 1996
+        auto A = stateTransitionMatrix(state, dt);
+        A.bottomRightCorner<6, 6>() *= computeAttenuation(damping, dt);
+        return A;
+    }
+
+    /*!
+     * Returns the state transition matrix for a constant velocity with
+     * separate damping paramters for linear and angular velocity (not for
+     * direct use in computing state transition, because it is very sparse,
+     * but in computing other values)
+     */
+    inline StateSquareMatrix stateTransitionMatrixWithSeparateVelocityDamping(
+        State const &state, double dt, double posDamping, double oriDamping) {
+        // eq. 4.5 in Welch 1996
+        auto A = stateTransitionMatrix(state, dt);
+        A.block<3, 3>(6, 6) *= computeAttenuation(posDamping, dt);
+        A.bottomRightCorner<3, 3>() *= computeAttenuation(oriDamping, dt);
+        return A;
+    }
 } // namespace pose_externalized_rotation
 
 } // namespace flexkalman

src/external/flexkalman/PoseStateExponentialMap.h

@@ -13,6 +13,9 @@
 */
 
 // Copyright 2015 Sensics, Inc.
+// Copyright 2020 Collabora, Ltd.
+//
+// SPDX-License-Identifier: Apache-2.0
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -32,6 +35,8 @@
 #include "BaseTypes.h"
 #include "FlexibleKalmanBase.h"
 #include "MatrixExponentialMap.h"
+#include "PoseConstantVelocityGeneric.h"
+#include "SO3.h"
 
 // Library/third-party includes
 // - none
@@ -50,93 +55,18 @@ namespace pose_exp_map {
         StateVector::ConstFixedSegmentReturnType<3>::Type;
 
     using StateVectorBlock6 = StateVector::FixedSegmentReturnType<6>::Type;
+    using ConstStateVectorBlock6 =
+        StateVector::ConstFixedSegmentReturnType<6>::Type;
     using StateSquareMatrix = types::SquareMatrix<Dimension>;
-
-    //! @name Accessors to blocks in the state vector.
-    /// @{
-    inline StateVectorBlock3 position(StateVector &vec) {
-        return vec.head<3>();
-    }
-    inline ConstStateVectorBlock3 position(StateVector const &vec) {
-        return vec.head<3>();
-    }
-
-    inline StateVectorBlock3 orientation(StateVector &vec) {
-        return vec.segment<3>(3);
-    }
-    inline ConstStateVectorBlock3 orientation(StateVector const &vec) {
-        return vec.segment<3>(3);
-    }
-
-    inline StateVectorBlock3 velocity(StateVector &vec) {
-        return vec.segment<3>(6);
-    }
-    inline ConstStateVectorBlock3 velocity(StateVector const &vec) {
-        return vec.segment<3>(6);
-    }
-
-    inline StateVectorBlock3 angularVelocity(StateVector &vec) {
-        return vec.segment<3>(9);
-    }
-    inline ConstStateVectorBlock3 angularVelocity(StateVector const &vec) {
-        return vec.segment<3>(9);
-    }
-
-    //! both translational and angular velocities
-    inline StateVectorBlock6 velocities(StateVector &vec) {
-        return vec.segment<6>(6);
-    }
-    //! @}
-
-    /*!
-     * This returns A(deltaT), though if you're just predicting xhat-, use
-     * applyVelocity() instead for performance.
-     */
-    inline StateSquareMatrix stateTransitionMatrix(double dt) {
-        // eq. 4.5 in Welch 1996 - except we have all the velocities at the
-        // end
-        StateSquareMatrix A = StateSquareMatrix::Identity();
-        A.topRightCorner<6, 6>() = types::SquareMatrix<6>::Identity() * dt;
-
-        return A;
-    }
     inline double computeAttenuation(double damping, double dt) {
         return std::pow(damping, dt);
     }
-    inline StateSquareMatrix
-    stateTransitionMatrixWithVelocityDamping(double dt, double damping) {
-
-        // eq. 4.5 in Welch 1996
-
-        auto A = stateTransitionMatrix(dt);
-        auto attenuation = computeAttenuation(damping, dt);
-        A.bottomRightCorner<6, 6>() *= attenuation;
-        return A;
-    }
-    //! Computes A(deltaT)xhat(t-deltaT)
-    inline StateVector applyVelocity(StateVector const &state, double dt) {
-        // eq. 4.5 in Welch 1996
-
-        /*!
-         * @todo benchmark - assuming for now that the manual small
-         * calcuations are faster than the matrix ones.
-         */
-
-        StateVector ret = state;
-        position(ret) += velocity(state) * dt;
-        orientation(ret) += angularVelocity(state) * dt;
-        return ret;
-    }
-
-    inline void dampenVelocities(StateVector &state, double damping,
-                                 double dt) {
-        auto attenuation = computeAttenuation(damping, dt);
-        velocities(state) *= attenuation;
-    }
     class State : public StateBase<State> {
       public:
         EIGEN_MAKE_ALIGNED_OPERATOR_NEW
         static constexpr size_t Dimension = 12;
+        using StateVector = ::flexkalman::pose_exp_map::StateVector;
+        using StateSquareMatrix = ::flexkalman::pose_exp_map::StateSquareMatrix;
 
         //! Default constructor
         State()
@@ -158,37 +88,55 @@ namespace pose_exp_map {
         }
 
         void postCorrect() {
-            types::Vector<3> ori = orientation(m_state);
+            types::Vector<3> ori = rotationVector();
             matrix_exponential_map::avoidSingularities(ori);
-            orientation(m_state) = ori;
+            rotationVector() = ori;
-            m_cacheData.reset(ori);
         }
 
-        StateVectorBlock3 position() { return pose_exp_map::position(m_state); }
+        StateVectorBlock3 position() { return m_state.head<3>(); }
 
-        ConstStateVectorBlock3 position() const {
+        ConstStateVectorBlock3 position() const { return m_state.head<3>(); }
-            return pose_exp_map::position(m_state);
+
+        Eigen::Quaterniond getQuaternion() const {
+            return matrix_exponential_map::toQuat(rotationVector());
+        }
+        Eigen::Matrix3d getRotationMatrix() const {
+            return matrix_exponential_map::rodrigues(rotationVector());
         }
 
-        Eigen::Quaterniond const &getQuaternion() {
+        StateVectorBlock3 velocity() { return m_state.segment<3>(6); }
-            return m_cacheData.getQuaternion();
-        }
-        Eigen::Matrix3d const &getRotationMatrix() {
-            return m_cacheData.getRotationMatrix();
-        }
-
-        StateVectorBlock3 velocity() { return pose_exp_map::velocity(m_state); }
 
         ConstStateVectorBlock3 velocity() const {
-            return pose_exp_map::velocity(m_state);
+            return m_state.segment<3>(6);
         }
 
-        StateVectorBlock3 angularVelocity() {
+        StateVectorBlock3 angularVelocity() { return m_state.segment<3>(9); }
-            return pose_exp_map::angularVelocity(m_state);
-        }
 
         ConstStateVectorBlock3 angularVelocity() const {
-            return pose_exp_map::angularVelocity(m_state);
+            return m_state.segment<3>(9);
+        }
+
+        /// Linear and angular velocities
+        StateVectorBlock6 velocities() { return m_state.tail<6>(); }
+
+        /// Linear and angular velocities
+        ConstStateVectorBlock6 velocities() const { return m_state.tail<6>(); }
+
+        StateVectorBlock3 rotationVector() { return m_state.segment<3>(3); }
+
+        ConstStateVectorBlock3 rotationVector() const {
+            return m_state.segment<3>(3);
+        }
+
+        /*!
+         * Get the position and quaternion combined into a single isometry
+         * (transformation)
+         */
+        Eigen::Isometry3d getIsometry() const {
+            Eigen::Isometry3d ret;
+            ret.fromPositionOrientationScale(position(), getQuaternion(),
+                                             Eigen::Vector3d::Constant(1));
+            return ret;
         }
 
       private:
@@ -199,9 +147,6 @@ namespace pose_exp_map {
         StateVector m_state;
         //! P
         StateSquareMatrix m_errorCovariance;
-
-        //! Cached data for use in consuming the exponential map rotation.
-        matrix_exponential_map::ExponentialMapData m_cacheData;
     };
 
     /*!
@@ -214,6 +159,73 @@ namespace pose_exp_map {
         os << "error:\n" << state.errorCovariance() << "\n";
         return os;
     }
+
+    /*!
+     * This returns A(deltaT), though if you're just predicting xhat-, use
+     * applyVelocity() instead for performance.
+     *
+     * State is a parameter for ADL.
+     */
+    inline StateSquareMatrix stateTransitionMatrix(State const &, double dt) {
+        // eq. 4.5 in Welch 1996 - except we have all the velocities at the
+        // end
+        StateSquareMatrix A = StateSquareMatrix::Identity();
+        A.topRightCorner<6, 6>() = types::SquareMatrix<6>::Identity() * dt;
+
+        return A;
+    }
+
+    inline StateSquareMatrix
+    stateTransitionMatrixWithVelocityDamping(State const &s, double dt,
+                                             double damping) {
+
+        // eq. 4.5 in Welch 1996
+
+        auto A = stateTransitionMatrix(s, dt);
+        auto attenuation = computeAttenuation(damping, dt);
+        A.bottomRightCorner<6, 6>() *= attenuation;
+        return A;
+    }
+
+    /*!
+     * Returns the state transition matrix for a constant velocity with
+     * separate damping paramters for linear and angular velocity (not for
+     * direct use in computing state transition, because it is very sparse,
+     * but in computing other values)
+     */
+    inline StateSquareMatrix stateTransitionMatrixWithSeparateVelocityDamping(
+        State const &state, double dt, double posDamping, double oriDamping) {
+        // eq. 4.5 in Welch 1996
+        auto A = stateTransitionMatrix(state, dt);
+        A.block<3, 3>(6, 6) *= computeAttenuation(posDamping, dt);
+        A.bottomRightCorner<3, 3>() *= computeAttenuation(oriDamping, dt);
+        return A;
+    }
+
+    //! Separately dampen the linear and angular velocities
+    inline void separatelyDampenVelocities(State &state, double posDamping,
+                                           double oriDamping, double dt) {
+        state.velocity() *= computeAttenuation(posDamping, dt);
+        state.angularVelocity() *= computeAttenuation(oriDamping, dt);
+    }
+
+    /// Computes A(deltaT)xhat(t-deltaT) (or, the more precise, non-linear thing
+    /// that is intended to simulate.)
+    inline void applyVelocity(State &state, double dt) {
+        state.position() += state.velocity() * dt;
+
+        // Do the full thing, not just the small-angle approximation as we have
+        // in the state transition matrix.
+        SO3 newOrientation = SO3{(state.angularVelocity() * dt).eval()} *
+                             SO3{(state.rotationVector()).eval()};
+        state.rotationVector() = newOrientation.getVector();
+    }
+
+    inline types::Vector<3> predictAbsoluteOrientationMeasurement(State const &s) {
+        return s.rotationVector();
+    }
+    using ConstantVelocityProcessModel =
+        PoseConstantVelocityGenericProcessModel<State>;
 } // namespace pose_exp_map
 
 } // namespace flexkalman

src/external/flexkalman/PureVectorState.h

@@ -10,6 +10,8 @@
 
 // Copyright 2015 Sensics, Inc.
 //
+// SPDX-License-Identifier: Apache-2.0
+//
 // 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

src/external/flexkalman/SO3.h

@@ -0,0 +1,111 @@
+/** @file
+    @brief Header for SO3 pose representation
+
+    @date 2019-2020
+
+    @author
+    Ryan Pavlik
+    <ryan.pavlik@collabora.com>
+*/
+
+// Copyright 2019-2020, Collabora, Ltd.
+// SPDX-License-Identifier: BSL-1.0 OR Apache-2.0
+
+#pragma once
+
+#include "MatrixExponentialMap.h"
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+namespace flexkalman {
+
+template <typename Derived>
+static inline Eigen::Matrix<typename Derived::Scalar, 3, 1>
+rot_matrix_ln(Eigen::MatrixBase<Derived> const &mat) {
+    EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived, 3, 3);
+    using Scalar = typename Derived::Scalar;
+    Eigen::AngleAxis<Scalar> angleAxis =
+        Eigen::AngleAxis<Scalar>{mat.derived()};
+    return angleAxis.angle() * angleAxis.axis();
+}
+
+template <typename Derived>
+static inline Eigen::Matrix<typename Derived::Scalar, 3, 1>
+rot_matrix_ln(Eigen::QuaternionBase<Derived> const &q) {
+    using Scalar = typename Derived::Scalar;
+    Eigen::AngleAxis<Scalar> angleAxis{q.derived()};
+    return angleAxis.angle() * angleAxis.axis();
+}
+
+/*!
+ * Represents an orientation as a member of the "special orthogonal group in 3D"
+ * SO3.
+ *
+ * This means we're logically using a 3D vector that can be converted to a
+ * rotation matrix using the matrix exponential map aka "Rodrigues' formula".
+ * We're actually storing both the rotation matrix and the vector for simplicity
+ * right now.
+ */
+class SO3 {
+  public:
+    SO3() = default;
+    explicit SO3(Eigen::Vector3d const &v)
+        : matrix_(matrix_exponential_map::rodrigues(
+              matrix_exponential_map::singularitiesAvoided(v))) {}
+    explicit SO3(Eigen::Matrix3d const &mat) : matrix_(mat) {}
+
+    static SO3 fromQuat(Eigen::Quaterniond const &q) {
+        Eigen::AngleAxisd angleAxis{q};
+        Eigen::Vector3d omega = angleAxis.angle() * angleAxis.axis();
+        return SO3{omega};
+    }
+    Eigen::Vector3d getVector() const {
+        double angle = getAngle();
+        while (angle < -EIGEN_PI) {
+            angle += 2 * EIGEN_PI;
+        }
+        while (angle > EIGEN_PI) {
+            angle -= 2 * EIGEN_PI;
+        }
+        return angle * getAxis();
+    }
+
+    Eigen::Matrix3d const &getRotationMatrix() const noexcept {
+        return matrix_;
+    }
+
+    Eigen::Quaterniond getQuat() const {
+        return matrix_exponential_map::toQuat(getVector());
+    }
+
+    SO3 getInverse() const { return SO3{getRotationMatrix(), InverseTag{}}; }
+
+    double getAngle() const {
+        return Eigen::AngleAxisd{getRotationMatrix()}.angle();
+    }
+
+    Eigen::Vector3d getAxis() const {
+        return Eigen::AngleAxisd{getRotationMatrix()}.axis();
+    }
+
+  private:
+    //! tag type to choose the inversion constructor.
+    struct InverseTag {};
+
+    //! Inversion constructor - fast
+    SO3(Eigen::Matrix3d const &mat, InverseTag const & /*tag*/)
+        : //  omega_(Eigen::Vector3d::Zero()),
+          matrix_(mat.transpose()) {
+        // omega_ = rot_matrix_ln(matrix_);
+    }
+
+    // Eigen::Vector3d omega_{Eigen::Vector3d::Zero()};
+    Eigen::Matrix3d matrix_{Eigen::Matrix3d::Identity()};
+};
+
+static inline SO3 operator*(SO3 const &lhs, SO3 const &rhs) {
+    Eigen::Matrix3d product = lhs.getRotationMatrix() * rhs.getRotationMatrix();
+    return SO3{product};
+}
+} // namespace flexkalman

src/external/flexkalman/SigmaPointGenerator.h

@@ -10,6 +10,8 @@
 
 // Copyright 2016 Sensics, Inc.
 //
+// SPDX-License-Identifier: Apache-2.0
+//
 // 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

src/xrt/auxiliary/tracking/t_tracker_psmv_fusion.cpp

@@ -30,7 +30,7 @@
 
 using State = flexkalman::pose_externalized_rotation::State;
 using ProcessModel =
-    flexkalman::PoseSeparatelyDampedConstantVelocityProcessModel;
+    flexkalman::PoseSeparatelyDampedConstantVelocityProcessModel<State>;
 
 namespace xrt_fusion {