Update flexkalman

src/external/flexkalman/EigenQuatExponentialMap.h

@@ -94,7 +94,7 @@ namespace util {
             Eigen::Quaternion<Scalar> ret;
             ret.vec() = vecscale * vec;
             ret.w() = std::cos(theta);
-            return ret.normalized();
+            return ret;
         }
 
         /*!
@@ -107,14 +107,21 @@ namespace util {
             EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived, 3);
             using Scalar = typename Derived::Scalar;
             //! @todo get better way of determining "zero vec" for this purpose.
-            if (vec.isApproxToConstant(0, 1.e-4)) {
+            if (vec.derived().isApproxToConstant(0, 1.e-4)) {
                 return Eigen::Quaternion<Scalar>::Identity();
             }
-            // For non-zero vectors, the vector scale sinc(theta) approximately
-            // equals 1, while the scale for w, cos(theta), is approximately 0.
-            // So, we treat this as a "pure" quaternion and normalize it.
-            return Eigen::Quaternion<Scalar>{0, vec.x(), vec.y(), vec.z()}
-                .normalized();
+            // For non-zero vectors, the vector scale
+            // sinc(theta)=sin(theta)/theta approximately equals 1, and w,
+            // cos(theta), is approximately 1 - theta^2/2.
+            // To ensure we're exactly normalized, we could treat the vec as the
+            // vector portion of a quaternion and compute the other part to make
+            // it exactly normalized:
+            // Scalar w = std::sqrt(1 - vec.derived().squaredNorm());
+            // Instead we'll do the small-angle approx to really skip the sqrt,
+            // and we'll be approximately normalized.
+            Scalar w = 1 - vec.derived().squaredNorm() / 2.;
+            return Eigen::Quaternion<Scalar>{
+                w, vec.derived().x(), vec.derived().y(), vec.derived().z()};
         }
 
         /*!
@@ -150,7 +157,8 @@ namespace util {
             // result (i.e., ln(qv, qw) = (phi/sin(phi)) * qv )
             Scalar vecnorm = quat.vec().norm();
 
-            // "best for numerical stability" vs asin or acos
+            // "best for numerical stability" vs asin or acos.
+            // Approximately vecnorm near 0.
             Scalar phi = std::atan2(vecnorm, quat.w());
 
             // Here is where we compute the coefficient to scale the vector part

src/external/flexkalman/FlexibleKalmanBase.h

@@ -100,9 +100,9 @@ inline types::SquareMatrix<getDimension<StateType>()>
 predictErrorCovariance(StateType const &state, ProcessModelType &processModel,
                        double dt) {
     using StateSquareMatrix = types::SquareMatrix<getDimension<StateType>()>;
-    StateSquareMatrix A = processModel.getStateTransitionMatrix(state, dt);
+    const auto A = processModel.getStateTransitionMatrix(state, dt);
     // FLEXKALMAN_DEBUG_OUTPUT("State transition matrix", A);
-    StateSquareMatrix P = state.errorCovariance();
+    auto &&P = state.errorCovariance();
     /*!
      * @todo Determine if the fact that Q is (at least in one case)
      * symmetrical implies anything else useful performance-wise here or

src/external/flexkalman/PoseStateExponentialMap.h

@@ -158,7 +158,7 @@ namespace pose_exp_map {
         }
 
         void postCorrect() {
-            Eigen::Vector3d ori(orientation(m_state));
+            types::Vector<3> ori = orientation(m_state);
             matrix_exponential_map::avoidSingularities(ori);
             orientation(m_state) = ori;
             m_cacheData.reset(ori);