Merge pull request #18 from bricerebsamen/brice/fix13-rotation_expression

Expression wrapper for rotation

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);
 }

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;