From 8f36e33566d1880b63520325da62e42866477a0e Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Tue, 10 Feb 2015 22:26:11 +0100
Subject: [PATCH] Fixed rodriguez

---
 gtsam/geometry/Rot3Q.cpp | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp
index af5c3f526..e27fb792d 100644
--- a/gtsam/geometry/Rot3Q.cpp
+++ b/gtsam/geometry/Rot3Q.cpp
@@ -85,8 +85,7 @@ namespace gtsam {
 
   /* ************************************************************************* */
   Rot3 Rot3::rodriguez(const Vector3& w, double theta) {
-// TODO   return QuaternionChart::Expmap(theta,w);
-    return Rot3();
+    return Quaternion(Eigen::AngleAxis<double>(theta, w));
   }
 
   /* ************************************************************************* */