diff --git a/.cproject b/.cproject
index 57611d5b8..1e86e3c10 100644
--- a/.cproject
+++ b/.cproject
@@ -592,7 +592,6 @@
make
-
tests/testBayesTree.run
true
false
@@ -600,6 +599,7 @@
make
+
testBinaryBayesNet.run
true
false
@@ -647,6 +647,7 @@
make
+
testSymbolicBayesNet.run
true
false
@@ -654,6 +655,7 @@
make
+
tests/testSymbolicFactor.run
true
false
@@ -661,6 +663,7 @@
make
+
testSymbolicFactorGraph.run
true
false
@@ -676,6 +679,7 @@
make
+
tests/testBayesTree
true
false
@@ -1131,6 +1135,7 @@
make
+
testErrors.run
true
false
@@ -1360,6 +1365,46 @@
true
true
+
+ make
+ -j5
+ testBTree.run
+ true
+ true
+ true
+
+
+ make
+ -j5
+ testDSF.run
+ true
+ true
+ true
+
+
+ make
+ -j5
+ testDSFMap.run
+ true
+ true
+ true
+
+
+ make
+ -j5
+ testDSFVector.run
+ true
+ true
+ true
+
+
+ make
+ -j5
+ testFixedVector.run
+ true
+ true
+ true
+
make
-j2
@@ -1442,7 +1487,6 @@
make
-
testSimulated2DOriented.run
true
false
@@ -1482,7 +1526,6 @@
make
-
testSimulated2D.run
true
false
@@ -1490,7 +1533,6 @@
make
-
testSimulated3D.run
true
false
@@ -1504,46 +1546,6 @@
true
true
-
- make
- -j5
- testBTree.run
- true
- true
- true
-
-
- make
- -j5
- testDSF.run
- true
- true
- true
-
-
- make
- -j5
- testDSFMap.run
- true
- true
- true
-
-
- make
- -j5
- testDSFVector.run
- true
- true
- true
-
-
- make
- -j5
- testFixedVector.run
- true
- true
- true
-
make
-j5
@@ -1801,6 +1803,7 @@
cpack
+
-G DEB
true
false
@@ -1808,6 +1811,7 @@
cpack
+
-G RPM
true
false
@@ -1815,6 +1819,7 @@
cpack
+
-G TGZ
true
false
@@ -1822,6 +1827,7 @@
cpack
+
--config CPackSourceConfig.cmake
true
false
@@ -2194,6 +2200,14 @@
true
true
+
+ make
+ -j4
+ testSO3.run
+ true
+ true
+ true
+
make
-j2
@@ -2692,6 +2706,7 @@
make
+
testGraph.run
true
false
@@ -2699,6 +2714,7 @@
make
+
testJunctionTree.run
true
false
@@ -2706,6 +2722,7 @@
make
+
testSymbolicBayesNetB.run
true
false
@@ -3257,7 +3274,6 @@
make
-
tests/testGaussianISAM2
true
false
diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp
new file mode 100644
index 000000000..2c23a7f1d
--- /dev/null
+++ b/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
+#include
+
+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
+
diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h
new file mode 100644
index 000000000..4bfa80754
--- /dev/null
+++ b/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
+#include
+
+#include
+
+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
+ SO3(const MatrixBase& 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& 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& 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 {
+ typedef lie_group_tag type;
+};
+}
+
+} // end namespace gtsam
+
diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp
new file mode 100644
index 000000000..003415dd3
--- /dev/null
+++ b/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
+#include
+
+using namespace std;
+using namespace gtsam;
+
+//******************************************************************************
+TEST(SO3 , Concept) {
+ BOOST_CONCEPT_ASSERT((IsGroup));
+ BOOST_CONCEPT_ASSERT((IsManifold));
+// BOOST_CONCEPT_ASSERT((IsLieGroup));
+}
+
+//******************************************************************************
+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::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::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);
+}
+//******************************************************************************
+