From 659e524fa047a2b8e71b687d5a4648941f73de7b Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Tue, 10 Jan 2012 05:05:36 +0000
Subject: [PATCH] Constructor from Pose2

---
 gtsam.h                            |  1 +
 gtsam/geometry/Pose3.cpp           |  9 ++++++++-
 gtsam/geometry/Pose3.h             |  5 +++++
 gtsam/geometry/tests/testPose3.cpp | 17 +++++++++++++----
 4 files changed, 27 insertions(+), 5 deletions(-)

diff --git a/gtsam.h b/gtsam.h
index c81737bf8..5091862ca 100644
--- a/gtsam.h
+++ b/gtsam.h
@@ -172,6 +172,7 @@ class Pose3 {
 	Pose3(const Rot3& r, const Point3& t);
 	Pose3(Vector v);
 	Pose3(Matrix t);
+	Pose3(const Pose2& pose2);
 	static Pose3 Expmap(Vector v);
 	static Vector Logmap(const Pose3& p);
 	void print(string s) const;
diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp
index 29e335d02..4a859e5db 100644
--- a/gtsam/geometry/Pose3.cpp
+++ b/gtsam/geometry/Pose3.cpp
@@ -14,9 +14,10 @@
  * @brief 3D Pose
  */
 
-#include <gtsam/base/Lie-inl.h>
 #include <gtsam/geometry/Pose3.h>
+#include <gtsam/geometry/Pose2.h>
 #include <gtsam/geometry/concepts.h>
+#include <gtsam/base/Lie-inl.h>
 #include <iostream>
 #include <cmath>
 
@@ -32,6 +33,12 @@ namespace gtsam {
 
   static const Matrix I3 = eye(3), Z3 = zeros(3, 3), _I3=-I3, I6 = eye(6);
 
+  /* ************************************************************************* */
+  Pose3::Pose3(const Pose2& pose2) :
+			R_(Rot3::rodriguez(0, 0, pose2.theta())),
+			t_(Point3(pose2.x(), pose2.y(), 0)) {
+	}
+
   /* ************************************************************************* */
   // Calculate Adjoint map
   // Ad_pose is 6*6 matrix that when applied to twist xi, returns Ad_pose(xi)
diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h
index bad34d8d5..a3a0b8762 100644
--- a/gtsam/geometry/Pose3.h
+++ b/gtsam/geometry/Pose3.h
@@ -27,6 +27,8 @@
 
 namespace gtsam {
 
+	class Pose2; // forward declare
+
   /**
    * A 3D pose (R,t) : (Rot3,Point3)
    * @ingroup geometry
@@ -54,6 +56,9 @@ namespace gtsam {
     /** Construct from R,t */
     Pose3(const Rot3& R, const Point3& t) : R_(R), t_(t) {}
 
+    /** Construct from Pose2 */
+    Pose3(const Pose2& pose2);
+
     /** Constructor from 4*4 matrix */
     Pose3(const Matrix &T) :
       R_(T(0, 0), T(0, 1), T(0, 2), T(1, 0), T(1, 1), T(1, 2), T(2, 0),
diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp
index 4cc9c07f9..eb2d410f8 100644
--- a/gtsam/geometry/tests/testPose3.cpp
+++ b/gtsam/geometry/tests/testPose3.cpp
@@ -14,12 +14,13 @@
  * @brief  Unit tests for Pose3 class
  */
 
-#include <cmath>
-#include <CppUnitLite/TestHarness.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/geometry/Pose2.h>
+#include <gtsam/base/lieProxies.h>
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/numericalDerivative.h>
-#include <gtsam/base/lieProxies.h>
-#include <gtsam/geometry/Pose3.h>
+#include <CppUnitLite/TestHarness.h>
+#include <cmath>
 
 using namespace std;
 using namespace gtsam;
@@ -43,6 +44,14 @@ TEST( Pose3, equals)
   EXPECT(!T3.equals(origin));
 }
 
+/* ************************************************************************* */
+TEST( Pose3, constructors)
+{
+	Pose3 expected(Rot3::rodriguez(0,0,3),Point3(1,2,0));
+  Pose2 pose2(1,2,3);
+  EXPECT(assert_equal(expected,Pose3(pose2)));
+}
+
 /* ************************************************************************* */
 TEST( Pose3, retract_first_order)
 {