Merge pull request #18 from bricerebsamen/brice/fix13-rotation_expres…

…sion Expression wrapper for rotation
borglab · May 22, 2019 · ef174aa · ef174aa
2 parents 69771f7 + 56229ed
commit ef174aa
Show file tree

Hide file tree

Showing 2 changed files with 23 additions and 0 deletions.
diff --git a/gtsam/slam/expressions.h b/gtsam/slam/expressions.h
@@ -40,6 +40,16 @@ inline Point3_ transformFrom(const Pose3_& x, const Point3_& p) {
   return Point3_(x, &Pose3::transformFrom, p);
 }
 
+namespace internal {
+Rot3 rotation(const Pose3& pose, OptionalJacobian<3, 6> H) {
+  return pose.rotation(H);
+}
+}  // namespace internal
+
+inline Rot3_ rotation(const Pose3_& pose) {
+  return Rot3_(internal::rotation, pose);
+}
+
 inline Point3_ rotate(const Rot3_& x, const Point3_& p) {
   return Point3_(x, &Rot3::rotate, p);
 }

diff --git a/gtsam/slam/tests/testSlamExpressions.cpp b/gtsam/slam/tests/testSlamExpressions.cpp
@@ -45,6 +45,19 @@ TEST(SlamExpressions, project2) {
   EXPECT_CORRECT_EXPRESSION_JACOBIANS(point2_expr, values, 1e-7, 1e-5);
 }
 
+/* ************************************************************************* */
+TEST(SlamExpressions, rotation) {
+  Pose3_ T_(0);
+  const Rot3_ R_ = rotation(T_);
+}
+
+/* ************************************************************************* */
+TEST(SlamExpressions, unrotate) {
+  Rot3_ R_(0);
+  Point3_ p_(1);
+  const Point3_ q_ = unrotate(R_, p_);
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;