Merge pull request #898 from borglab/feature/logmap_expression

Feature/logmap expression
borglab · Oct 21, 2021 · 6eb01aa · 6eb01aa
2 parents 9e032e4 + 60d7514
commit 6eb01aa
Show file tree

Hide file tree

Showing 2 changed files with 24 additions and 0 deletions.
diff --git a/gtsam/slam/expressions.h b/gtsam/slam/expressions.h
@@ -138,4 +138,21 @@ Point2_ uncalibrate(const Expression<CALIBRATION>& K, const Point2_& xy_hat) {
   return Point2_(K, &CALIBRATION::uncalibrate, xy_hat);
 }
 
+
+/// logmap
+// TODO(dellaert): Should work but fails because of a type deduction conflict.
+// template <typename T>
+// gtsam::Expression<typename gtsam::traits<T>::TangentVector> logmap(
+//     const gtsam::Expression<T> &x1, const gtsam::Expression<T> &x2) {
+//   return gtsam::Expression<typename gtsam::traits<T>::TangentVector>(
+//       x1, &T::logmap, x2);
+// }
+
+template <typename T>
+gtsam::Expression<typename gtsam::traits<T>::TangentVector> logmap(
+    const gtsam::Expression<T> &x1, const gtsam::Expression<T> &x2) {
+  return Expression<typename gtsam::traits<T>::TangentVector>(
+      gtsam::traits<T>::Logmap, between(x1, x2));
+}
+
 }  // \namespace gtsam
diff --git a/gtsam/slam/tests/testSlamExpressions.cpp b/gtsam/slam/tests/testSlamExpressions.cpp
@@ -58,6 +58,13 @@ TEST(SlamExpressions, unrotate) {
   const Point3_ q_ = unrotate(R_, p_);
 }
 
+/* ************************************************************************* */
+TEST(SlamExpressions, logmap) {
+  Pose3_ T1_(0);
+  Pose3_ T2_(1);
+  const Vector6_ l = logmap(T1_, T2_);
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;