Add vee

include/manif/impl/bundle/BundleTangent_base.h

@@ -408,6 +408,31 @@ struct RandomEvaluatorImpl<BundleTangentBase<Derived>>
   }
 };
 
+//! @brief Vee specialization for BundleTangentBase objects.
+template <typename Derived>
+struct VeeEvaluatorImpl<BundleTangentBase<Derived>> {
+  template <typename TL, typename TR>
+  static void run(TL& t, const TR& v) {
+    return vee_impl(
+      t, v, internal::make_intseq_t<BundleTangentBase<Derived>::BundleSize>{}
+    );
+  }
+};
+
+template <typename TL, typename TR, int ... _Idx>
+void vee_impl(TL& t, const TR& v, internal::intseq<_Idx...>) {
+  // c++11 "fold expression"
+  auto l = {((t.template element<_Idx>().setVee(
+    v.template block<
+      TL::template Element<_Idx>::LieAlg::RowsAtCompileTime,
+      TL::template Element<_Idx>::LieAlg::RowsAtCompileTime
+    >(
+      std::get<_Idx>(internal::traits<typename TL::Tangent>::AlgIdx),
+      std::get<_Idx>(internal::traits<typename TL::Tangent>::AlgIdx)
+  ))), 0) ...};
+  static_cast<void>(l); // compiler warning
+}
+
 }  // namespace internal
 }  // namespace manif
 

include/manif/impl/rn/RnTangent_base.h

@@ -202,6 +202,7 @@ struct RandomEvaluatorImpl<RnTangentBase<Derived>>
   }
 };
 
+//! @brief Bracket specialization for RnTangentBase objects.
 template <typename Derived>
 struct BracketEvaluatorImpl<RnTangentBase<Derived>> {
   template <typename TL, typename TR>
@@ -210,6 +211,15 @@ struct BracketEvaluatorImpl<RnTangentBase<Derived>> {
   }
 };
 
+//! @brief Vee specialization for RnTangentBase objects.
+template <typename Derived>
+struct VeeEvaluatorImpl<RnTangentBase<Derived>> {
+  template <typename TL, typename TR>
+  static void run(TL& t, const TR& v) {
+    t.coeffs() = v.template topRightCorner<Derived::Dim, 1>();
+  }
+};
+
 } // namespace internal
 } // namespace manif
 

include/manif/impl/se2/SE2Tangent_base.h

@@ -480,6 +480,15 @@ struct RandomEvaluatorImpl<SE2TangentBase<Derived>>
   }
 };
 
+//! @brief Vee specialization for SE2TangentBase objects.
+template <typename Derived>
+struct VeeEvaluatorImpl<SE2TangentBase<Derived>> {
+  template <typename TL, typename TR>
+  static void run(TL& t, const TR& v) {
+    t.coeffs() << v(0, 2), v(1, 2), v(1, 0);
+  }
+};
+
 } /* namespace internal */
 } /* namespace manif */
 

include/manif/impl/se3/SE3Tangent_base.h

@@ -458,6 +458,15 @@ struct RandomEvaluatorImpl<SE3TangentBase<Derived>>
   }
 };
 
+//! @brief Vee specialization for SE3TangentBase objects.
+template <typename Derived>
+struct VeeEvaluatorImpl<SE3TangentBase<Derived>> {
+  template <typename TL, typename TR>
+  static void run(TL& t, const TR& v) {
+    t.coeffs() << v(0, 3), v(1, 3), v(2, 3), v(2, 1), v(0, 2), v(1, 0);
+  }
+};
+
 } /* namespace internal */
 } /* namespace manif */
 

include/manif/impl/se_2_3/SE_2_3Tangent_base.h

@@ -478,6 +478,17 @@ struct RandomEvaluatorImpl<SE_2_3TangentBase<Derived>>
   }
 };
 
+//! @brief Vee specialization for SE_2_3TangentBase objects.
+template <typename Derived>
+struct VeeEvaluatorImpl<SE_2_3TangentBase<Derived>> {
+  template <typename TL, typename TR>
+  static void run(TL& t, const TR& v) {
+    t.coeffs() << v(0, 3), v(1, 3), v(2, 3),
+      v(2, 1), v(0, 2), v(1, 0),
+      v(0, 4), v(1, 4), v(2, 4);
+  }
+};
+
 } /* namespace internal */
 } /* namespace manif */
 

include/manif/impl/sgal3/SGal3Tangent_base.h

@@ -617,6 +617,18 @@ struct RandomEvaluatorImpl<SGal3TangentBase<Derived>> {
   }
 };
 
+//! @brief Vee specialization for SGal3TangentBase objects.
+template <typename Derived>
+struct VeeEvaluatorImpl<SGal3TangentBase<Derived>> {
+  template <typename TL, typename TR>
+  static void run(TL& t, const TR& v) {
+    t.coeffs() << v(0, 4), v(1, 4), v(2, 4),
+      v(0, 3), v(1, 3), v(2, 3),
+      v(2, 1), v(0, 2), v(1, 0),
+      v(3, 4);
+  }
+};
+
 } // namespace internal
 } // namespace manif
 

include/manif/impl/so2/SO2Tangent_base.h

@@ -225,6 +225,15 @@ struct RandomEvaluatorImpl<SO2TangentBase<Derived>>
   }
 };
 
+//! @brief Vee specialization for SO2TangentBase objects.
+template <typename Derived>
+struct VeeEvaluatorImpl<SO2TangentBase<Derived>> {
+  template <typename TL, typename TR>
+  static void run(TL& t, const TR& v) {
+    t.coeffs() << v(1, 0);
+  }
+};
+
 } /* namespace internal */
 } /* namespace manif */
 

include/manif/impl/so3/SO3Tangent_base.h

@@ -327,6 +327,15 @@ struct RandomEvaluatorImpl<SO3TangentBase<Derived>>
   }
 };
 
+//! @brief Vee specialization for SO3TangentBase objects.
+template <typename Derived>
+struct VeeEvaluatorImpl<SO3TangentBase<Derived>> {
+  template <typename TL, typename TR>
+  static void run(TL& t, const TR& v) {
+    t.coeffs() << v(2, 1), v(0, 2), v(1, 0);
+  }
+};
+
 } /* namespace internal */
 } /* namespace manif */
 

include/manif/impl/tangent_base.h

@@ -6,6 +6,7 @@
 #include "manif/impl/generator.h"
 #include "manif/impl/random.h"
 #include "manif/impl/bracket.h"
+#include "manif/impl/vee.h"
 #include "manif/impl/eigen.h"
 
 #include "manif/constants.h"
@@ -99,6 +100,14 @@ struct TangentBase
    */
   _Derived& setRandom();
 
+  /**
+   * @brief Set the Tangent object this from an object in the Lie algebra.
+   * @param[in] a An object in the Lie algebra.
+   * @return A reference to this.
+   */
+  template <typename _EigenDerived>
+  _Derived& setVee(const Eigen::MatrixBase<_EigenDerived>& a);
+
   // Minimum API
   // Those functions must be implemented in the Derived class !
 
@@ -370,6 +379,17 @@ struct TangentBase
     const TangentBase<_Derived>& a, const TangentBase<_DerivedOther>& b
   );
 
+  /**
+   * @brief Instantiate a Tangent from a Lie algebra object.
+   *
+   * @tparam _EigenDerived
+   * @param alg A tangent object expressed in the Lie algebra.
+   * @return a Tangent object.
+   * @see hat
+   */
+  template <typename _EigenDerived>
+  static Tangent Vee(const Eigen::MatrixBase<_EigenDerived>& alg);
+
 protected:
 
   inline _Derived& derived() & noexcept { return *static_cast< _Derived* >(this); }
@@ -464,6 +484,13 @@ _Derived& TangentBase<_Derived>::setRandom()
   return derived();
 }
 
+template <class _Derived>
+template <typename _EigenDerived>
+_Derived& TangentBase<_Derived>::setVee(const Eigen::MatrixBase<_EigenDerived>& a) {
+  internal::VeeEvaluator<typename internal::traits<_Derived>::Base>(derived()).run(a);
+  return derived();
+}
+
 template <class _Derived>
 typename TangentBase<_Derived>::LieGroup
 TangentBase<_Derived>::exp(OptJacobianRef J_m_t) const
@@ -738,6 +765,14 @@ typename TangentBase<_Derived>::Tangent TangentBase<_Derived>::Bracket(
   return a.bracket(b);
 }
 
+template <typename _Derived>
+template <typename _EigenDerived>
+typename TangentBase<_Derived>::Tangent TangentBase<_Derived>::Vee(
+  const Eigen::MatrixBase<_EigenDerived>& alg
+) {
+  return Tangent().setVee(alg);
+}
+
 // Math
 
 template <typename _Derived>

include/manif/impl/vee.h

@@ -0,0 +1,37 @@
+#ifndef _MANIF_MANIF_IMPL_VEE_H_
+#define _MANIF_MANIF_IMPL_VEE_H_
+
+namespace manif {
+namespace internal {
+
+template <typename Derived>
+struct VeeEvaluatorImpl {
+  template <typename TL, typename TR>
+  static void run(TL& t, const TR&) {
+    static_assert(
+      constexpr_false<Derived>(), "VeeEvaluator not overloaded for Derived type!"
+    );
+    // t.setRandom();
+  }
+};
+
+template <typename Derived>
+struct VeeEvaluator : VeeEvaluatorImpl<Derived> {
+  using Base = VeeEvaluatorImpl<Derived>;
+
+  VeeEvaluator(Derived& xptr) : xptr_(xptr) {}
+
+  template <typename T>
+  void run(const T& t) {
+    Base::run(xptr_, t);
+  }
+
+protected:
+
+  Derived& xptr_;
+};
+
+} // namespace internal
+} // namespace manif
+
+#endif // _MANIF_MANIF_IMPL_VEE_H_

test/common_tester.h

@@ -110,7 +110,9 @@
   TEST_P(TEST_##manifold##_TESTER, TEST_##manifold##_INVERSE)             \
   { evalInverse(); }                                                      \
   TEST_P(TEST_##manifold##_TESTER, TEST_##manifold##_BRACKET)             \
-  { evalBracket(); }
+  { evalBracket(); }                                                      \
+  TEST_P(TEST_##manifold##_TESTER, TEST_##manifold##_HAT_VEE)             \
+  { evalHatVee(); }
 
 #define MANIF_TEST_JACOBIANS(manifold)                                                                                    \
   using manifold##JacobiansTester = JacobianTester<manifold>;                                                             \
@@ -771,6 +773,10 @@ class CommonTester
     EXPECT_MANIF_NEAR(Tangent::Bracket(getDelta(), getDelta()), Tangent::Zero());
   }
 
+  void evalHatVee() {
+    EXPECT_MANIF_NEAR(getDelta(), Tangent::Vee(getDelta().hat()));
+  }
+
 protected:
 
   // relax eps for float type