SO(3) is a *Matrix* Lie group. This is it. Rot3 will inherit from this *or* Quaternion.

.cproject

@@ -592,7 +592,6 @@
 			</target>
 			<target name="tests/testBayesTree.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>tests/testBayesTree.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -600,6 +599,7 @@
 			</target>
 			<target name="testBinaryBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>testBinaryBayesNet.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -647,6 +647,7 @@
 			</target>
 			<target name="testSymbolicBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>testSymbolicBayesNet.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -654,6 +655,7 @@
 			</target>
 			<target name="tests/testSymbolicFactor.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>tests/testSymbolicFactor.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -661,6 +663,7 @@
 			</target>
 			<target name="testSymbolicFactorGraph.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>testSymbolicFactorGraph.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -676,6 +679,7 @@
 			</target>
 			<target name="tests/testBayesTree" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>tests/testBayesTree</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -1131,6 +1135,7 @@
 			</target>
 			<target name="testErrors.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>testErrors.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -1360,6 +1365,46 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="testBTree.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>testBTree.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testDSF.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>testDSF.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testDSFMap.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>testDSFMap.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testDSFVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>testDSFVector.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testFixedVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>testFixedVector.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="all" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
@@ -1442,7 +1487,6 @@
 			</target>
 			<target name="testSimulated2DOriented.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>testSimulated2DOriented.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -1482,7 +1526,6 @@
 			</target>
 			<target name="testSimulated2D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>testSimulated2D.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -1490,7 +1533,6 @@
 			</target>
 			<target name="testSimulated3D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>testSimulated3D.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -1504,46 +1546,6 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="testBTree.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>testBTree.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testDSF.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>testDSF.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testDSFMap.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>testDSFMap.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testDSFVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>testDSFVector.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testFixedVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>testFixedVector.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
 			<target name="testEliminationTree.run" path="build/gtsam/inference/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j5</buildArguments>
@@ -1801,6 +1803,7 @@
 			</target>
 			<target name="Generate DEB Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>cpack</buildCommand>
+				<buildArguments/>
 				<buildTarget>-G DEB</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -1808,6 +1811,7 @@
 			</target>
 			<target name="Generate RPM Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>cpack</buildCommand>
+				<buildArguments/>
 				<buildTarget>-G RPM</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -1815,6 +1819,7 @@
 			</target>
 			<target name="Generate TGZ Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>cpack</buildCommand>
+				<buildArguments/>
 				<buildTarget>-G TGZ</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -1822,6 +1827,7 @@
 			</target>
 			<target name="Generate TGZ Source Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>cpack</buildCommand>
+				<buildArguments/>
 				<buildTarget>--config CPackSourceConfig.cmake</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -2194,6 +2200,14 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="testSO3.run" path="build/gtsam/geometry/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j4</buildArguments>
+				<buildTarget>testSO3.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="all" path="release" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
@@ -2692,6 +2706,7 @@
 			</target>
 			<target name="testGraph.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>testGraph.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -2699,6 +2714,7 @@
 			</target>
 			<target name="testJunctionTree.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>testJunctionTree.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -2706,6 +2722,7 @@
 			</target>
 			<target name="testSymbolicBayesNetB.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>testSymbolicBayesNetB.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -3257,7 +3274,6 @@
 			</target>
 			<target name="tests/testGaussianISAM2" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>tests/testGaussianISAM2</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>

gtsam/geometry/SO3.cpp

@@ -0,0 +1,83 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file    SO3.cpp
+ * @brief   3*3 matrix representation o SO(3)
+ * @author  Frank Dellaert
+ * @date    December 2014
+ */
+
+#include <gtsam/geometry/SO3.h>
+#include <cmath>
+
+namespace gtsam {
+
+SO3 SO3Chart::Expmap(const double& theta, const Vector3& axis) {
+  using std::cos;
+  using std::sin;
+
+  // get components of axis \omega
+  double wx = axis(0), wy = axis(1), wz = axis(2);
+
+  double c = cos(theta), s = sin(theta), c_1 = 1 - c;
+  double wwTxx = wx * wx, wwTyy = wy * wy, wwTzz = wz * wz;
+  double swx = wx * s, swy = wy * s, swz = wz * s;
+
+  double C00 = c_1 * wwTxx, C01 = c_1 * wx * wy, C02 = c_1 * wx * wz;
+  double C11 = c_1 * wwTyy, C12 = c_1 * wy * wz;
+  double C22 = c_1 * wwTzz;
+
+  Matrix3 R;
+  R << c + C00, -swz + C01, swy + C02, //
+  swz + C01, c + C11, -swx + C12, //
+  -swy + C02, swx + C12, c + C22;
+
+  return R;
+}
+
+Vector3 SO3Chart::Logmap(const SO3& R) {
+
+  // note switch to base 1
+  const double& R11 = R(0, 0), R12 = R(0, 1), R13 = R(0, 2);
+  const double& R21 = R(1, 0), R22 = R(1, 1), R23 = R(1, 2);
+  const double& R31 = R(2, 0), R32 = R(2, 1), R33 = R(2, 2);
+
+  // Get trace(R)
+  double tr = R.trace();
+
+  // when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc.
+  // we do something special
+  if (std::abs(tr + 1.0) < 1e-10) {
+    if (std::abs(R33 + 1.0) > 1e-10)
+      return (M_PI / sqrt(2.0 + 2.0 * R33)) * Vector3(R13, R23, 1.0 + R33);
+    else if (std::abs(R22 + 1.0) > 1e-10)
+      return (M_PI / sqrt(2.0 + 2.0 * R22)) * Vector3(R12, 1.0 + R22, R32);
+    else
+      // if(std::abs(R.r1_.x()+1.0) > 1e-10)  This is implicit
+      return (M_PI / sqrt(2.0 + 2.0 * R11)) * Vector3(1.0 + R11, R21, R31);
+  } else {
+    double magnitude;
+    double tr_3 = tr - 3.0; // always negative
+    if (tr_3 < -1e-7) {
+      double theta = acos((tr - 1.0) / 2.0);
+      magnitude = theta / (2.0 * sin(theta));
+    } else {
+      // when theta near 0, +-2pi, +-4pi, etc. (trace near 3.0)
+      // use Taylor expansion: theta \approx 1/2-(t-3)/12 + O((t-3)^2)
+      magnitude = 0.5 - tr_3 * tr_3 / 12.0;
+    }
+    return magnitude * Vector3(R32 - R23, R13 - R31, R21 - R12);
+  }
+}
+
+} // end namespace gtsam
+

gtsam/geometry/SO3.h

@@ -0,0 +1,101 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file    SO3.h
+ * @brief   3*3 matrix representation o SO(3)
+ * @author  Frank Dellaert
+ * @date    December 2014
+ */
+
+#pragma once
+
+#include <gtsam/base/Matrix.h>
+#include <gtsam/base/concepts.h>
+
+#include <cmath>
+
+namespace gtsam {
+
+/**
+ *  True SO(3), i.e., 3*3 matrix subgroup
+ *  We guarantee (all but first) constructors only generate from sub-manifold.
+ *  However, round-off errors in repeated composition could move off it...
+ */
+class SO3: public Matrix3 {
+
+protected:
+
+public:
+
+  /// Constructor from Eigen Matrix
+  template<typename Derived>
+  SO3(const MatrixBase<Derived>& R) :
+      Matrix3(R.eval()) {
+  }
+
+  /// Constructor from AngleAxisd
+  SO3(const Eigen::AngleAxisd& angleAxis) :
+      Matrix3(angleAxis) {
+  }
+};
+
+/// Default Chart for SO3
+struct SO3Chart {
+
+  typedef SO3 ManifoldType;
+
+  /// Exponential map given axis/angle representation of Lie algebra
+  static SO3 Expmap(const double& theta, const Vector3& w);
+
+  /// Exponential map, simply be converting omega to axis/angle representation
+  static SO3 Expmap(const Eigen::Ref<const Vector3>& omega) {
+    if (omega.isZero())
+      return SO3::Identity();
+    else {
+      double angle = omega.norm();
+      return Expmap(angle, omega / angle);
+    }
+  }
+
+  /// retract, simply be converting omega to AngleAxis
+  static SO3 Retract(const SO3& p, const Eigen::Ref<const Vector3>& omega) {
+    return p * Expmap(omega);
+  }
+
+  /// We use our own Logmap, as there is a slight bug in Eigen
+  static Vector3 Logmap(const SO3& R);
+
+  /// local is our own, as there is a slight bug in Eigen
+  static Vector3 Local(const SO3& q1, const SO3& q2) {
+    return Logmap(q1.inverse() * q2);
+  }
+
+};
+
+#define SO3_TEMPLATE
+GTSAM_GROUP_IDENTITY0(SO3)
+GTSAM_MULTIPLICATIVE_GROUP(SO3_TEMPLATE, SO3)
+
+#define SO3_TANGENT Vector3
+#define SO3_CHART SO3Chart
+GTSAM_MANIFOLD(SO3_TEMPLATE, SO3, 3, SO3_TANGENT, SO3_CHART)
+
+/// Define SO3 to be a model of the Lie Group concept
+namespace traits {
+template<>
+struct structure_category<SO3> {
+  typedef lie_group_tag type;
+};
+}
+
+} // end namespace gtsam
+

gtsam/geometry/tests/testSO3.cpp

@@ -0,0 +1,83 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file   testQuaternion.cpp
+ * @brief  Unit tests for SO3, as a GTSAM-adapted Lie Group
+ * @author Frank Dellaert
+ **/
+
+#include <gtsam/geometry/SO3.h>
+#include <CppUnitLite/TestHarness.h>
+
+using namespace std;
+using namespace gtsam;
+
+//******************************************************************************
+TEST(SO3 , Concept) {
+  BOOST_CONCEPT_ASSERT((IsGroup<SO3 >));
+  BOOST_CONCEPT_ASSERT((IsManifold<SO3 >));
+//  BOOST_CONCEPT_ASSERT((IsLieGroup<SO3 >));
+}
+
+//******************************************************************************
+TEST(SO3 , Constructor) {
+  SO3 q(Eigen::AngleAxisd(1, Vector3(0, 0, 1)));
+}
+
+//******************************************************************************
+TEST(SO3 , Invariants) {
+  SO3 q1(Eigen::AngleAxisd(1, Vector3(0, 0, 1)));
+  SO3 q2(Eigen::AngleAxisd(2, Vector3(0, 1, 0)));
+  // group::check_invariants(q1,q2); Does not satisfy Testable concept (yet!)
+}
+
+//******************************************************************************
+TEST(SO3 , Local) {
+  Vector3 z_axis(0, 0, 1);
+  SO3 q1(Eigen::AngleAxisd(0, z_axis));
+  SO3 q2(Eigen::AngleAxisd(0.1, z_axis));
+  typedef manifold::traits::DefaultChart<SO3>::type Chart;
+  Vector3 expected(0, 0, 0.1);
+  Vector3 actual = Chart::Local(q1, q2);
+  EXPECT(assert_equal((Vector)expected,actual));
+}
+
+//******************************************************************************
+TEST(SO3 , Retract) {
+  Vector3 z_axis(0, 0, 1);
+  SO3 q(Eigen::AngleAxisd(0, z_axis));
+  SO3 expected(Eigen::AngleAxisd(0.1, z_axis));
+  typedef manifold::traits::DefaultChart<SO3>::type Chart;
+  Vector3 v(0, 0, 0.1);
+  SO3 actual = Chart::Retract(q, v);
+  EXPECT(actual.isApprox(expected));
+}
+
+//******************************************************************************
+TEST(SO3 , Compose) {
+}
+
+//******************************************************************************
+TEST(SO3 , Between) {
+}
+
+//******************************************************************************
+TEST(SO3 , Inverse) {
+}
+
+//******************************************************************************
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+//******************************************************************************
+