diff --git a/.cproject b/.cproject
index 19d3912b0..9726fec60 100644
--- a/.cproject
+++ b/.cproject
@@ -532,6 +532,14 @@
true
true
+
+ make
+ -j4
+ testSimilarity3.run
+ true
+ true
+ true
+
make
-j2
diff --git a/.settings/.gitignore b/.settings/.gitignore
new file mode 100644
index 000000000..faa6d2991
--- /dev/null
+++ b/.settings/.gitignore
@@ -0,0 +1 @@
+/org.eclipse.cdt.codan.core.prefs
diff --git a/gtsam_unstable/geometry/Similarity3.cpp b/gtsam_unstable/geometry/Similarity3.cpp
new file mode 100644
index 000000000..cfc508ac7
--- /dev/null
+++ b/gtsam_unstable/geometry/Similarity3.cpp
@@ -0,0 +1,124 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 Similarity3.cpp
+ * @brief Implementation of Similarity3 transform
+ * @author Paul Drews
+ */
+
+#include
+
+#include
+#include
+#include
+
+#include
+
+namespace gtsam {
+
+Similarity3::Similarity3(const Matrix3& R, const Vector3& t, double s) :
+ R_(R), t_(t), s_(s) {
+}
+
+Similarity3::Similarity3() :
+ R_(), t_(), s_(1){
+}
+
+Similarity3::Similarity3(double s) :
+ s_ (s) {
+}
+
+Similarity3::Similarity3(const Rotation& R, const Translation& t, double s) :
+ R_ (R), t_ (t), s_ (s) {
+}
+
+Similarity3::operator Pose3() const {
+ return Pose3(R_, s_*t_);
+}
+
+Similarity3 Similarity3::identity() {
+ return Similarity3(); }
+
+//Vector7 Similarity3::Logmap(const Similarity3& s, OptionalJacobian<7, 7> Hm) {
+// return Vector7();
+//}
+//
+//Similarity3 Similarity3::Expmap(const Vector7& v, OptionalJacobian<7, 7> Hm) {
+// return Similarity3();
+//}
+
+bool Similarity3::operator==(const Similarity3& other) const {
+ return (R_.equals(other.R_)) && (t_ == other.t_) && (s_ == other.s_);
+}
+
+bool Similarity3::equals(const Similarity3& sim, double tol) const {
+ return R_.equals(sim.R_, tol) && t_.equals(sim.t_, tol)
+ && s_ < (sim.s_+tol) && s_ > (sim.s_-tol);
+}
+
+Similarity3::Translation Similarity3::transform_from(const Translation& p) const {
+ return R_ * (s_ * p) + t_;
+}
+
+Matrix7 Similarity3::AdjointMap() const{
+ const Matrix3 R = R_.matrix();
+ const Vector3 t = t_.vector();
+ Matrix3 A = s_ * skewSymmetric(t) * R;
+ Matrix7 adj;
+ adj << s_*R, A, -s_*t, Z_3x3, R, Eigen::Matrix::Zero(), Eigen::Matrix::Zero(), 1;
+ return adj;
+}
+
+inline Similarity3::Translation Similarity3::operator*(const Translation& p) const {
+ return transform_from(p);
+}
+
+Similarity3 Similarity3::inverse() const {
+ Rotation Rt = R_.inverse();
+ Translation sRt = R_.inverse() * (-s_ * t_);
+ return Similarity3(Rt, sRt, 1.0/s_);
+}
+
+Similarity3 Similarity3::operator*(const Similarity3& T) const {
+ return Similarity3(R_ * T.R_, ((1.0/T.s_)*t_) + R_ * T.t_, s_*T.s_);
+}
+
+void Similarity3::print(const std::string& s) const {
+ std::cout << std::endl;
+ std::cout << s;
+ rotation().print("R:\n");
+ translation().print("t: ");
+ std::cout << "s: " << scale() << std::endl;
+}
+
+Similarity3 Similarity3::ChartAtOrigin::Retract(const Vector7& v, ChartJacobian H) {
+ // Will retracting or localCoordinating R work if R is not a unit rotation?
+ // Also, how do we actually get s out? Seems like we need to store it somewhere.
+ Rotation r; //Create a zero rotation to do our retraction.
+ return Similarity3( //
+ r.retract(v.head<3>()), // retract rotation using v[0,1,2]
+ Translation(v.segment<3>(3)), // Retract the translation
+ 1.0 + v[6]); //finally, update scale using v[6]
+}
+
+Vector7 Similarity3::ChartAtOrigin::Local(const Similarity3& other, ChartJacobian H) {
+ Rotation r; //Create a zero rotation to do the retraction
+ Vector7 v;
+ v.head<3>() = r.localCoordinates(other.R_);
+ v.segment<3>(3) = other.t_.vector();
+ //v.segment<3>(3) = translation().localCoordinates(other.translation());
+ v[6] = other.s_ - 1.0;
+ return v;
+}
+}
+
+
diff --git a/gtsam_unstable/geometry/Similarity3.h b/gtsam_unstable/geometry/Similarity3.h
new file mode 100644
index 000000000..89f1010c4
--- /dev/null
+++ b/gtsam_unstable/geometry/Similarity3.h
@@ -0,0 +1,150 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 Similarity3.h
+ * @brief Implementation of Similarity3 transform
+ * @author Paul Drews
+ */
+
+#pragma once
+
+#include
+#include
+#include
+#include
+
+namespace gtsam {
+
+// Forward declarations
+class Pose3;
+
+/**
+ * 3D similarity transform
+ */
+class Similarity3: public LieGroup {
+
+ /** Pose Concept requirements */
+ typedef Rot3 Rotation;
+ typedef Point3 Translation;
+
+private:
+ Rotation R_;
+ Translation t_;
+ double s_;
+
+public:
+
+ /// @name Constructors
+ /// @{
+
+ Similarity3();
+
+ /// Construct pure scaling
+ Similarity3(double s);
+
+ /// Construct from GTSAM types
+ Similarity3(const Rotation& R, const Translation& t, double s);
+
+ /// Construct from Eigen types
+ Similarity3(const Matrix3& R, const Vector3& t, double s);
+
+ /// @}
+ /// @name Testable
+ /// @{
+
+ /// Compare with tolerance
+ bool equals(const Similarity3& sim, double tol) const;
+
+ /// Compare with standard tolerance
+ bool operator==(const Similarity3& other) const;
+
+ /// Print with optional string
+ void print(const std::string& s) const;
+
+ /// @}
+ /// @name Group
+ /// @{
+
+ /// Return an identity transform
+ static Similarity3 identity();
+
+ /// Return the inverse
+ Similarity3 inverse() const;
+
+ Translation transform_from(const Translation& p) const;
+
+ /** syntactic sugar for transform_from */
+ inline Translation operator*(const Translation& p) const;
+
+ Similarity3 operator*(const Similarity3& T) const;
+
+ /// @}
+ /// @name Standard interface
+ /// @{
+
+ /// Return a GTSAM rotation
+ const Rotation& rotation() const {
+ return R_;
+ };
+
+ /// Return a GTSAM translation
+ const Translation& translation() const {
+ return t_;
+ };
+
+ /// Return the scale
+ double scale() const {
+ return s_;
+ };
+
+ /// Convert to a rigid body pose
+ operator Pose3() const;
+
+ /// Dimensionality of tangent space = 7 DOF - used to autodetect sizes
+ inline static size_t Dim() {
+ return 7;
+ };
+
+ /// Dimensionality of tangent space = 7 DOF
+ inline size_t dim() const {
+ return 7;
+ };
+
+ /// @}
+ /// @name Chart
+ /// @{
+
+ /// Update Similarity transform via 7-dim vector in tangent space
+ struct ChartAtOrigin {
+ static Similarity3 Retract(const Vector7& v, ChartJacobian H = boost::none);
+
+ /// 7-dimensional vector v in tangent space that makes other = this->retract(v)
+ static Vector7 Local(const Similarity3& other, ChartJacobian H = boost::none);
+ };
+
+ /// Project from one tangent space to another
+ Matrix7 AdjointMap() const;
+
+ /// @}
+ /// @name Stubs
+ /// @{
+
+ /// Not currently implemented, required because this is a lie group
+ static Vector7 Logmap(const Similarity3& s, OptionalJacobian<7, 7> Hm = boost::none);
+ static Similarity3 Expmap(const Vector7& v, OptionalJacobian<7, 7> Hm = boost::none);
+
+ using LieGroup::inverse; // version with derivative
+};
+
+template<>
+struct traits : public internal::LieGroupTraits {};
+}
diff --git a/gtsam_unstable/geometry/tests/testSimilarity3.cpp b/gtsam_unstable/geometry/tests/testSimilarity3.cpp
new file mode 100644
index 000000000..b2b5d5702
--- /dev/null
+++ b/gtsam_unstable/geometry/tests/testSimilarity3.cpp
@@ -0,0 +1,256 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 testSimilarity3.cpp
+ * @brief Unit tests for Similarity3 class
+ * @author Paul Drews
+ */
+
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+#include
+#include
+
+using namespace gtsam;
+using namespace std;
+using symbol_shorthand::X;
+
+GTSAM_CONCEPT_TESTABLE_INST(Similarity3)
+
+static Point3 P(0.2,0.7,-2);
+static Rot3 R = Rot3::rodriguez(0.3,0,0);
+static Similarity3 T(R,Point3(3.5,-8.2,4.2),1);
+static Similarity3 T2(Rot3::rodriguez(0.3,0.2,0.1),Point3(3.5,-8.2,4.2),1);
+static Similarity3 T3(Rot3::rodriguez(-90, 0, 0), Point3(1, 2, 3), 1);
+
+//******************************************************************************
+TEST(Similarity3, Constructors) {
+ Similarity3 test;
+}
+
+//******************************************************************************
+TEST(Similarity3, Getters) {
+ Similarity3 test;
+ EXPECT(assert_equal(Rot3(), test.rotation()));
+ EXPECT(assert_equal(Point3(), test.translation()));
+ EXPECT_DOUBLES_EQUAL(1.0, test.scale(), 1e-9);
+}
+
+//******************************************************************************
+TEST(Similarity3, Getters2) {
+ Similarity3 test(Rot3::ypr(1, 2, 3), Point3(4, 5, 6), 7);
+ EXPECT(assert_equal(Rot3::ypr(1, 2, 3), test.rotation()));
+ EXPECT(assert_equal(Point3(4, 5, 6), test.translation()));
+ EXPECT_DOUBLES_EQUAL(7.0, test.scale(), 1e-9);
+}
+
+TEST(Similarity3, AdjointMap) {
+ Similarity3 test(Rot3::ypr(1,2,3).inverse(), Point3(4,5,6), 7);
+ Matrix7 result;
+ result << -1.5739, -2.4512, -6.3651, -50.7671, -11.2503, 16.8859, -28.0000,
+ 6.3167, -2.9884, -0.4111, 0.8502, 8.6373, -49.7260, -35.0000,
+ -2.5734, -5.8362, 2.8839, 33.1363, 0.3024, 30.1811, -42.0000,
+ 0, 0, 0, -0.2248, -0.3502, -0.9093, 0,
+ 0, 0, 0, 0.9024, -0.4269, -0.0587, 0,
+ 0, 0, 0, -0.3676, -0.8337, 0.4120, 0,
+ 0, 0, 0, 0, 0, 0, 1.0000;
+ EXPECT(assert_equal(result, test.AdjointMap(), 1e-3));
+}
+
+TEST(Similarity3, inverse) {
+ Similarity3 test(Rot3::ypr(1,2,3).inverse(), Point3(4,5,6), 7);
+ Matrix3 Re;
+ Re << -0.2248, 0.9024, -0.3676,
+ -0.3502, -0.4269, -0.8337,
+ -0.9093, -0.0587, 0.4120;
+ Vector3 te(-9.8472, 59.7640, 10.2125);
+ Similarity3 expected(Re, te, 1.0/7.0);
+ EXPECT(assert_equal(expected, test.inverse(), 1e-3));
+}
+
+TEST(Similarity3, multiplication) {
+ Similarity3 test1(Rot3::ypr(1,2,3).inverse(), Point3(4,5,6), 7);
+ Similarity3 test2(Rot3::ypr(1,2,3).inverse(), Point3(8,9,10), 11);
+ Matrix3 re;
+ re << 0.0688, 0.9863, -0.1496,
+ -0.5665, -0.0848, -0.8197,
+ -0.8211, 0.1412, 0.5530;
+ Vector3 te(-13.6797, 3.2441, -5.7794);
+ Similarity3 expected(re, te, 77);
+ EXPECT(assert_equal(expected, test1*test2, 1e-2));
+}
+
+//******************************************************************************
+TEST(Similarity3, Manifold) {
+ EXPECT_LONGS_EQUAL(7, Similarity3::Dim());
+ Vector z = Vector7::Zero();
+ Similarity3 sim;
+ EXPECT(sim.retract(z) == sim);
+
+ Vector7 v = Vector7::Zero();
+ v(6) = 2;
+ Similarity3 sim2;
+ EXPECT(sim2.retract(z) == sim2);
+
+ EXPECT(assert_equal(z, sim2.localCoordinates(sim)));
+
+ Similarity3 sim3 = Similarity3(Rot3(), Point3(1, 2, 3), 1);
+ Vector v3(7);
+ v3 << 0, 0, 0, 1, 2, 3, 0;
+ EXPECT(assert_equal(v3, sim2.localCoordinates(sim3)));
+
+// Similarity3 other = Similarity3(Rot3::ypr(0.01, 0.02, 0.03), Point3(0.4, 0.5, 0.6), 1);
+ Similarity3 other = Similarity3(Rot3::ypr(0.1, 0.2, 0.3),Point3(4,5,6),1);
+
+ Vector vlocal = sim.localCoordinates(other);
+
+ EXPECT(assert_equal(sim.retract(vlocal), other, 1e-2));
+
+ Similarity3 other2 = Similarity3(Rot3::ypr(0.3, 0, 0),Point3(4,5,6),1);
+ Rot3 R = Rot3::rodriguez(0.3,0,0);
+
+ Vector vlocal2 = sim.localCoordinates(other2);
+
+ EXPECT(assert_equal(sim.retract(vlocal2), other2, 1e-2));
+
+ // TODO add unit tests for retract and localCoordinates
+}
+
+/* ************************************************************************* */
+TEST( Similarity3, retract_first_order)
+{
+ Similarity3 id;
+ Vector v = zero(7);
+ v(0) = 0.3;
+ EXPECT(assert_equal(Similarity3(R, Point3(), 1), id.retract(v),1e-2));
+ v(3)=0.2;v(4)=0.7;v(5)=-2;
+ EXPECT(assert_equal(Similarity3(R, P, 1),id.retract(v),1e-2));
+}
+
+/* ************************************************************************* */
+TEST(Similarity3, localCoordinates_first_order)
+{
+ Vector d12 = repeat(7,0.1);
+ d12(6) = 1.0;
+ Similarity3 t1 = T, t2 = t1.retract(d12);
+ EXPECT(assert_equal(d12, t1.localCoordinates(t2)));
+}
+
+/* ************************************************************************* */
+TEST(Similarity3, manifold_first_order)
+{
+ Similarity3 t1 = T;
+ Similarity3 t2 = T3;
+ Similarity3 origin;
+ Vector d12 = t1.localCoordinates(t2);
+ EXPECT(assert_equal(t2, t1.retract(d12)));
+ Vector d21 = t2.localCoordinates(t1);
+ EXPECT(assert_equal(t1, t2.retract(d21)));
+}
+
+TEST(Similarity3, Optimization) {
+ Similarity3 prior = Similarity3(Rot3::ypr(0.1, 0.2, 0.3), Point3(1, 2, 3), 4);
+ noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(7, 1);
+ Symbol key('x',1);
+ PriorFactor factor(key, prior, model);
+
+ NonlinearFactorGraph graph;
+ graph.push_back(factor);
+
+ Values initial;
+ initial.insert(key, Similarity3());
+
+ Values result;
+ LevenbergMarquardtParams params;
+ params.setVerbosityLM("TRYCONFIG");
+ result = LevenbergMarquardtOptimizer(graph, initial).optimize();
+ EXPECT(assert_equal(prior, result.at(key), 1e-4));
+}
+
+TEST(Similarity3, Optimization2) {
+ Similarity3 prior = Similarity3();
+ Similarity3 m1 = Similarity3(Rot3::ypr(M_PI/4.0, 0, 0), Point3(2.0, 0, 0), 1.0);
+ Similarity3 m2 = Similarity3(Rot3::ypr(M_PI/2.0, 0, 0), Point3(sqrt(8)*0.9, 0, 0), 1.0);
+ Similarity3 m3 = Similarity3(Rot3::ypr(3*M_PI/4.0, 0, 0), Point3(sqrt(32)*0.8, 0, 0), 1.0);
+ Similarity3 m4 = Similarity3(Rot3::ypr(M_PI/2.0, 0, 0), Point3(6*0.7, 0, 0), 1.0);
+ Similarity3 loop = Similarity3(1.42);
+
+ //prior.print("Goal Transform");
+ noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(7, 0.01);
+ SharedDiagonal betweenNoise = noiseModel::Diagonal::Sigmas(
+ (Vector(7) << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 10).finished());
+ SharedDiagonal betweenNoise2 = noiseModel::Diagonal::Sigmas(
+ (Vector(7) << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 1.0).finished());
+ PriorFactor factor(X(1), prior, model);
+ BetweenFactor b1(X(1), X(2), m1, betweenNoise);
+ BetweenFactor b2(X(2), X(3), m2, betweenNoise);
+ BetweenFactor b3(X(3), X(4), m3, betweenNoise);
+ BetweenFactor b4(X(4), X(5), m4, betweenNoise);
+ BetweenFactor lc(X(5), X(1), loop, betweenNoise2);
+
+
+
+ NonlinearFactorGraph graph;
+ graph.push_back(factor);
+ graph.push_back(b1);
+ graph.push_back(b2);
+ graph.push_back(b3);
+ graph.push_back(b4);
+ graph.push_back(lc);
+
+ //graph.print("Full Graph\n");
+
+ Values initial;
+ initial.insert(X(1), Similarity3());
+ initial.insert(X(2), Similarity3(Rot3::ypr(M_PI/2.0, 0, 0), Point3(1, 0, 0), 1.1));
+ initial.insert(X(3), Similarity3(Rot3::ypr(2.0*M_PI/2.0, 0, 0), Point3(0.9, 1.1, 0), 1.2));
+ initial.insert(X(4), Similarity3(Rot3::ypr(3.0*M_PI/2.0, 0, 0), Point3(0, 1, 0), 1.3));
+ initial.insert(X(5), Similarity3(Rot3::ypr(4.0*M_PI/2.0, 0, 0), Point3(0, 0, 0), 1.0));
+
+ //initial.print("Initial Estimate\n");
+
+ Values result;
+ result = LevenbergMarquardtOptimizer(graph, initial).optimize();
+ //result.print("Optimized Estimate\n");
+ Pose3 p1, p2, p3, p4, p5;
+ p1 = Pose3(result.at(X(1)));
+ p2 = Pose3(result.at(X(2)));
+ p3 = Pose3(result.at(X(3)));
+ p4 = Pose3(result.at(X(4)));
+ p5 = Pose3(result.at(X(5)));
+
+ //p1.print("Pose1");
+ //p2.print("Pose2");
+ //p3.print("Pose3");
+ //p4.print("Pose4");
+ //p5.print("Pose5");
+
+ Similarity3 expected(0.7);
+ EXPECT(assert_equal(expected, result.at(X(5)), 0.4));
+}
+
+//******************************************************************************
+int main() {
+ TestResult tr;
+ return TestRegistry::runAllTests(tr);
+}
+//******************************************************************************
+