diff --git a/.cproject b/.cproject
index 7111b0a6b..86952742b 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.h b/gtsam.h
index f8e0af28e..1aee996dc 100644
--- a/gtsam.h
+++ b/gtsam.h
@@ -156,7 +156,7 @@ virtual class Value {
size_t dim() const;
};
-#include
+#include
class LieScalar {
// Standard constructors
LieScalar();
@@ -185,7 +185,7 @@ class LieScalar {
static Vector Logmap(const gtsam::LieScalar& p);
};
-#include
+#include
class LieVector {
// Standard constructors
LieVector();
@@ -217,7 +217,7 @@ class LieVector {
void serialize() const;
};
-#include
+#include
class LieMatrix {
// Standard constructors
LieMatrix();
diff --git a/gtsam/base/FastSet.h b/gtsam/base/FastSet.h
index 69e841e57..4ef963f5d 100644
--- a/gtsam/base/FastSet.h
+++ b/gtsam/base/FastSet.h
@@ -19,15 +19,14 @@
#pragma once
#include
-
-#include
-#include
-#include
-#include
#include
#include
#include
#include
+#include
+#include
+#include
+#include
BOOST_MPL_HAS_XXX_TRAIT_DEF(print)
diff --git a/gtsam/base/GenericValue.h b/gtsam/base/GenericValue.h
index c67f7dd61..dd0811d8b 100644
--- a/gtsam/base/GenericValue.h
+++ b/gtsam/base/GenericValue.h
@@ -26,7 +26,7 @@
#include
#include
-#include
+#include
#include // operator typeid
namespace gtsam {
diff --git a/gtsam/base/LieMatrix.cpp b/gtsam/base/LieMatrix.cpp
index 69054fc1c..cf5b57536 100644
--- a/gtsam/base/LieMatrix.cpp
+++ b/gtsam/base/LieMatrix.cpp
@@ -15,7 +15,7 @@
* @author Richard Roberts and Alex Cunningham
*/
-#include
+#include
namespace gtsam {
diff --git a/gtsam/base/LieMatrix.h b/gtsam/base/LieMatrix.h
index 90b7207a2..e24f339e4 100644
--- a/gtsam/base/LieMatrix.h
+++ b/gtsam/base/LieMatrix.h
@@ -11,147 +11,16 @@
/**
* @file LieMatrix.h
- * @brief A wrapper around Matrix providing Lie compatibility
- * @author Richard Roberts and Alex Cunningham
+ * @brief External deprecation warning, see LieMatrix_Deprecated.h for details
+ * @author Paul Drews
*/
#pragma once
-#include
-
#ifdef _MSC_VER
#pragma message("LieMatrix.h is deprecated. Please use Eigen::Matrix instead.")
#else
#warning "LieMatrix.h is deprecated. Please use Eigen::Matrix instead."
#endif
-#include
-#include
-
-namespace gtsam {
-
-/**
- * @deprecated: LieMatrix, LieVector and LieMatrix are obsolete in GTSAM 4.0 as
- * we can directly add double, Vector, and Matrix into values now, because of
- * gtsam::traits.
- */
-struct LieMatrix : public Matrix {
-
- /// @name Constructors
- /// @{
- enum { dimension = Eigen::Dynamic };
-
- /** default constructor - only for serialize */
- LieMatrix() {}
-
- /** initialize from a normal matrix */
- LieMatrix(const Matrix& v) : Matrix(v) {}
-
- template
- LieMatrix(const M& v) : Matrix(v) {}
-
-// Currently TMP constructor causes ICE on MSVS 2013
-#if (_MSC_VER < 1800)
- /** initialize from a fixed size normal vector */
- template
- LieMatrix(const Eigen::Matrix& v) : Matrix(v) {}
-#endif
-
- /** constructor with size and initial data, row order ! */
- LieMatrix(size_t m, size_t n, const double* const data) :
- Matrix(Eigen::Map(data, m, n)) {}
-
- /// @}
- /// @name Testable interface
- /// @{
-
- /** print @param s optional string naming the object */
- GTSAM_EXPORT void print(const std::string& name="") const;
-
- /** equality up to tolerance */
- inline bool equals(const LieMatrix& expected, double tol=1e-5) const {
- return gtsam::equal_with_abs_tol(matrix(), expected.matrix(), tol);
- }
-
- /// @}
- /// @name Standard Interface
- /// @{
-
- /** get the underlying matrix */
- inline Matrix matrix() const {
- return static_cast(*this);
- }
-
- /// @}
-
- /// @name Group
- /// @{
- LieMatrix compose(const LieMatrix& q) { return (*this)+q;}
- LieMatrix between(const LieMatrix& q) { return q-(*this);}
- LieMatrix inverse() { return -(*this);}
- /// @}
-
- /// @name Manifold
- /// @{
- Vector localCoordinates(const LieMatrix& q) { return between(q).vector();}
- LieMatrix retract(const Vector& v) {return compose(LieMatrix(v));}
- /// @}
-
- /// @name Lie Group
- /// @{
- static Vector Logmap(const LieMatrix& p) {return p.vector();}
- static LieMatrix Expmap(const Vector& v) { return LieMatrix(v);}
- /// @}
-
- /// @name VectorSpace requirements
- /// @{
-
- /** Returns dimensionality of the tangent space */
- inline size_t dim() const { return size(); }
-
- /** Convert to vector, is done row-wise - TODO why? */
- inline Vector vector() const {
- Vector result(size());
- typedef Eigen::Matrix RowMajor;
- Eigen::Map(&result(0), rows(), cols()) = *this;
- return result;
- }
-
- /** identity - NOTE: no known size at compile time - so zero length */
- inline static LieMatrix identity() {
- throw std::runtime_error("LieMatrix::identity(): Don't use this function");
- return LieMatrix();
- }
- /// @}
-
-private:
-
- // Serialization function
- friend class boost::serialization::access;
- template
- void serialize(Archive & ar, const unsigned int version) {
- ar & boost::serialization::make_nvp("Matrix",
- boost::serialization::base_object(*this));
-
- }
-
-};
-
-
-template<>
-struct traits : public internal::VectorSpace {
-
- // Override Retract, as the default version does not know how to initialize
- static LieMatrix Retract(const LieMatrix& origin, const TangentVector& v,
- ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
- if (H1) *H1 = Eye(origin);
- if (H2) *H2 = Eye(origin);
- typedef const Eigen::Matrix RowMajor;
- return origin + Eigen::Map(&v(0), origin.rows(), origin.cols());
- }
-
-};
-
-} // \namespace gtsam
+#include "gtsam/base/LieMatrix_Deprecated.h"
diff --git a/gtsam/base/LieMatrix_Deprecated.h b/gtsam/base/LieMatrix_Deprecated.h
new file mode 100644
index 000000000..5dbe9935f
--- /dev/null
+++ b/gtsam/base/LieMatrix_Deprecated.h
@@ -0,0 +1,151 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 LieMatrix.h
+ * @brief A wrapper around Matrix providing Lie compatibility
+ * @author Richard Roberts and Alex Cunningham
+ */
+
+#pragma once
+
+#include
+
+#include
+#include
+
+namespace gtsam {
+
+/**
+ * @deprecated: LieMatrix, LieVector and LieMatrix are obsolete in GTSAM 4.0 as
+ * we can directly add double, Vector, and Matrix into values now, because of
+ * gtsam::traits.
+ */
+struct LieMatrix : public Matrix {
+
+ /// @name Constructors
+ /// @{
+ enum { dimension = Eigen::Dynamic };
+
+ /** default constructor - only for serialize */
+ LieMatrix() {}
+
+ /** initialize from a normal matrix */
+ LieMatrix(const Matrix& v) : Matrix(v) {}
+
+ template
+ LieMatrix(const M& v) : Matrix(v) {}
+
+// Currently TMP constructor causes ICE on MSVS 2013
+#if (_MSC_VER < 1800)
+ /** initialize from a fixed size normal vector */
+ template
+ LieMatrix(const Eigen::Matrix& v) : Matrix(v) {}
+#endif
+
+ /** constructor with size and initial data, row order ! */
+ LieMatrix(size_t m, size_t n, const double* const data) :
+ Matrix(Eigen::Map(data, m, n)) {}
+
+ /// @}
+ /// @name Testable interface
+ /// @{
+
+ /** print @param s optional string naming the object */
+ GTSAM_EXPORT void print(const std::string& name="") const;
+
+ /** equality up to tolerance */
+ inline bool equals(const LieMatrix& expected, double tol=1e-5) const {
+ return gtsam::equal_with_abs_tol(matrix(), expected.matrix(), tol);
+ }
+
+ /// @}
+ /// @name Standard Interface
+ /// @{
+
+ /** get the underlying matrix */
+ inline Matrix matrix() const {
+ return static_cast(*this);
+ }
+
+ /// @}
+
+ /// @name Group
+ /// @{
+ LieMatrix compose(const LieMatrix& q) { return (*this)+q;}
+ LieMatrix between(const LieMatrix& q) { return q-(*this);}
+ LieMatrix inverse() { return -(*this);}
+ /// @}
+
+ /// @name Manifold
+ /// @{
+ Vector localCoordinates(const LieMatrix& q) { return between(q).vector();}
+ LieMatrix retract(const Vector& v) {return compose(LieMatrix(v));}
+ /// @}
+
+ /// @name Lie Group
+ /// @{
+ static Vector Logmap(const LieMatrix& p) {return p.vector();}
+ static LieMatrix Expmap(const Vector& v) { return LieMatrix(v);}
+ /// @}
+
+ /// @name VectorSpace requirements
+ /// @{
+
+ /** Returns dimensionality of the tangent space */
+ inline size_t dim() const { return size(); }
+
+ /** Convert to vector, is done row-wise - TODO why? */
+ inline Vector vector() const {
+ Vector result(size());
+ typedef Eigen::Matrix RowMajor;
+ Eigen::Map(&result(0), rows(), cols()) = *this;
+ return result;
+ }
+
+ /** identity - NOTE: no known size at compile time - so zero length */
+ inline static LieMatrix identity() {
+ throw std::runtime_error("LieMatrix::identity(): Don't use this function");
+ return LieMatrix();
+ }
+ /// @}
+
+private:
+
+ // Serialization function
+ friend class boost::serialization::access;
+ template
+ void serialize(Archive & ar, const unsigned int version) {
+ ar & boost::serialization::make_nvp("Matrix",
+ boost::serialization::base_object(*this));
+
+ }
+
+};
+
+
+template<>
+struct traits : public internal::VectorSpace {
+
+ // Override Retract, as the default version does not know how to initialize
+ static LieMatrix Retract(const LieMatrix& origin, const TangentVector& v,
+ ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
+ if (H1) *H1 = Eye(origin);
+ if (H2) *H2 = Eye(origin);
+ typedef const Eigen::Matrix RowMajor;
+ return origin + Eigen::Map(&v(0), origin.rows(), origin.cols());
+ }
+
+};
+
+} // \namespace gtsam
diff --git a/gtsam/base/LieScalar.cpp b/gtsam/base/LieScalar.cpp
index 5f59c29bc..4c5a6a257 100644
--- a/gtsam/base/LieScalar.cpp
+++ b/gtsam/base/LieScalar.cpp
@@ -8,7 +8,7 @@
-#include
+#include
namespace gtsam {
void LieScalar::print(const std::string& name) const {
diff --git a/gtsam/base/LieScalar.h b/gtsam/base/LieScalar.h
index 9f6c56b28..650e2bb21 100644
--- a/gtsam/base/LieScalar.h
+++ b/gtsam/base/LieScalar.h
@@ -11,7 +11,7 @@
/**
* @file LieScalar.h
- * @brief A wrapper around scalar providing Lie compatibility
+ * @brief External deprecation warning, see LieScalar_Deprecated.h for details
* @author Kai Ni
*/
@@ -23,69 +23,4 @@
#warning "LieScalar.h is deprecated. Please use double/float instead."
#endif
-#include
-#include
-
-namespace gtsam {
-
- /**
- * @deprecated: LieScalar, LieVector and LieMatrix are obsolete in GTSAM 4.0 as
- * we can directly add double, Vector, and Matrix into values now, because of
- * gtsam::traits.
- */
- struct GTSAM_EXPORT LieScalar {
-
- enum { dimension = 1 };
-
- /** default constructor */
- LieScalar() : d_(0.0) {}
-
- /** wrap a double */
- /*explicit*/ LieScalar(double d) : d_(d) {}
-
- /** access the underlying value */
- double value() const { return d_; }
-
- /** Automatic conversion to underlying value */
- operator double() const { return d_; }
-
- /** convert vector */
- Vector1 vector() const { Vector1 v; v<
- struct traits : public internal::ScalarTraits {};
-
-} // \namespace gtsam
+#include
diff --git a/gtsam/base/LieScalar_Deprecated.h b/gtsam/base/LieScalar_Deprecated.h
new file mode 100644
index 000000000..6ffc76d37
--- /dev/null
+++ b/gtsam/base/LieScalar_Deprecated.h
@@ -0,0 +1,85 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 LieScalar.h
+ * @brief A wrapper around scalar providing Lie compatibility
+ * @author Kai Ni
+ */
+
+#pragma once
+
+#include
+#include
+
+namespace gtsam {
+
+ /**
+ * @deprecated: LieScalar, LieVector and LieMatrix are obsolete in GTSAM 4.0 as
+ * we can directly add double, Vector, and Matrix into values now, because of
+ * gtsam::traits.
+ */
+ struct GTSAM_EXPORT LieScalar {
+
+ enum { dimension = 1 };
+
+ /** default constructor */
+ LieScalar() : d_(0.0) {}
+
+ /** wrap a double */
+ /*explicit*/ LieScalar(double d) : d_(d) {}
+
+ /** access the underlying value */
+ double value() const { return d_; }
+
+ /** Automatic conversion to underlying value */
+ operator double() const { return d_; }
+
+ /** convert vector */
+ Vector1 vector() const { Vector1 v; v<
+ struct traits : public internal::ScalarTraits {};
+
+} // \namespace gtsam
diff --git a/gtsam/base/LieVector.cpp b/gtsam/base/LieVector.cpp
index 84afdabc8..3e4eeecf2 100644
--- a/gtsam/base/LieVector.cpp
+++ b/gtsam/base/LieVector.cpp
@@ -15,8 +15,8 @@
* @author Alex Cunningham
*/
+#include
#include
-#include
using namespace std;
diff --git a/gtsam/base/LieVector.h b/gtsam/base/LieVector.h
index c2003df3c..813431775 100644
--- a/gtsam/base/LieVector.h
+++ b/gtsam/base/LieVector.h
@@ -11,8 +11,8 @@
/**
* @file LieVector.h
- * @brief A wrapper around vector providing Lie compatibility
- * @author Alex Cunningham
+ * @brief Deprecation warning for LieVector_Deprecated.h, see LieVector_Deprecated.h for details.
+ * @author Paul Drews
*/
#pragma once
@@ -23,100 +23,4 @@
#warning "LieVector.h is deprecated. Please use Eigen::Vector instead."
#endif
-#include
-
-namespace gtsam {
-
-/**
- * @deprecated: LieVector, LieVector and LieMatrix are obsolete in GTSAM 4.0 as
- * we can directly add double, Vector, and Matrix into values now, because of
- * gtsam::traits.
- */
-struct LieVector : public Vector {
-
- enum { dimension = Eigen::Dynamic };
-
- /** default constructor - should be unnecessary */
- LieVector() {}
-
- /** initialize from a normal vector */
- LieVector(const Vector& v) : Vector(v) {}
-
- template
- LieVector(const V& v) : Vector(v) {}
-
-// Currently TMP constructor causes ICE on MSVS 2013
-#if (_MSC_VER < 1800)
- /** initialize from a fixed size normal vector */
- template
- LieVector(const Eigen::Matrix& v) : Vector(v) {}
-#endif
-
- /** wrap a double */
- LieVector(double d) : Vector((Vector(1) << d).finished()) {}
-
- /** constructor with size and initial data, row order ! */
- GTSAM_EXPORT LieVector(size_t m, const double* const data);
-
- /// @name Testable
- /// @{
- GTSAM_EXPORT void print(const std::string& name="") const;
- bool equals(const LieVector& expected, double tol=1e-5) const {
- return gtsam::equal(vector(), expected.vector(), tol);
- }
- /// @}
-
- /// @name Group
- /// @{
- LieVector compose(const LieVector& q) { return (*this)+q;}
- LieVector between(const LieVector& q) { return q-(*this);}
- LieVector inverse() { return -(*this);}
- /// @}
-
- /// @name Manifold
- /// @{
- Vector localCoordinates(const LieVector& q) { return between(q).vector();}
- LieVector retract(const Vector& v) {return compose(LieVector(v));}
- /// @}
-
- /// @name Lie Group
- /// @{
- static Vector Logmap(const LieVector& p) {return p.vector();}
- static LieVector Expmap(const Vector& v) { return LieVector(v);}
- /// @}
-
- /// @name VectorSpace requirements
- /// @{
-
- /** get the underlying vector */
- Vector vector() const {
- return static_cast(*this);
- }
-
- /** Returns dimensionality of the tangent space */
- size_t dim() const { return this->size(); }
-
- /** identity - NOTE: no known size at compile time - so zero length */
- static LieVector identity() {
- throw std::runtime_error("LieVector::identity(): Don't use this function");
- return LieVector();
- }
-
- /// @}
-
-private:
-
- // Serialization function
- friend class boost::serialization::access;
- template
- void serialize(Archive & ar, const unsigned int version) {
- ar & boost::serialization::make_nvp("Vector",
- boost::serialization::base_object(*this));
- }
-};
-
-
-template<>
-struct traits : public internal::VectorSpace {};
-
-} // \namespace gtsam
+#include
diff --git a/gtsam/base/LieVector_Deprecated.h b/gtsam/base/LieVector_Deprecated.h
new file mode 100644
index 000000000..3cb73319d
--- /dev/null
+++ b/gtsam/base/LieVector_Deprecated.h
@@ -0,0 +1,116 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 LieVector.h
+ * @brief A wrapper around vector providing Lie compatibility
+ * @author Alex Cunningham
+ */
+
+#pragma once
+
+#include
+
+namespace gtsam {
+
+/**
+ * @deprecated: LieVector, LieVector and LieMatrix are obsolete in GTSAM 4.0 as
+ * we can directly add double, Vector, and Matrix into values now, because of
+ * gtsam::traits.
+ */
+struct LieVector : public Vector {
+
+ enum { dimension = Eigen::Dynamic };
+
+ /** default constructor - should be unnecessary */
+ LieVector() {}
+
+ /** initialize from a normal vector */
+ LieVector(const Vector& v) : Vector(v) {}
+
+ template
+ LieVector(const V& v) : Vector(v) {}
+
+// Currently TMP constructor causes ICE on MSVS 2013
+#if (_MSC_VER < 1800)
+ /** initialize from a fixed size normal vector */
+ template
+ LieVector(const Eigen::Matrix& v) : Vector(v) {}
+#endif
+
+ /** wrap a double */
+ LieVector(double d) : Vector((Vector(1) << d).finished()) {}
+
+ /** constructor with size and initial data, row order ! */
+ GTSAM_EXPORT LieVector(size_t m, const double* const data);
+
+ /// @name Testable
+ /// @{
+ GTSAM_EXPORT void print(const std::string& name="") const;
+ bool equals(const LieVector& expected, double tol=1e-5) const {
+ return gtsam::equal(vector(), expected.vector(), tol);
+ }
+ /// @}
+
+ /// @name Group
+ /// @{
+ LieVector compose(const LieVector& q) { return (*this)+q;}
+ LieVector between(const LieVector& q) { return q-(*this);}
+ LieVector inverse() { return -(*this);}
+ /// @}
+
+ /// @name Manifold
+ /// @{
+ Vector localCoordinates(const LieVector& q) { return between(q).vector();}
+ LieVector retract(const Vector& v) {return compose(LieVector(v));}
+ /// @}
+
+ /// @name Lie Group
+ /// @{
+ static Vector Logmap(const LieVector& p) {return p.vector();}
+ static LieVector Expmap(const Vector& v) { return LieVector(v);}
+ /// @}
+
+ /// @name VectorSpace requirements
+ /// @{
+
+ /** get the underlying vector */
+ Vector vector() const {
+ return static_cast(*this);
+ }
+
+ /** Returns dimensionality of the tangent space */
+ size_t dim() const { return this->size(); }
+
+ /** identity - NOTE: no known size at compile time - so zero length */
+ static LieVector identity() {
+ throw std::runtime_error("LieVector::identity(): Don't use this function");
+ return LieVector();
+ }
+
+ /// @}
+
+private:
+
+ // Serialization function
+ friend class boost::serialization::access;
+ template
+ void serialize(Archive & ar, const unsigned int version) {
+ ar & boost::serialization::make_nvp("Vector",
+ boost::serialization::base_object(*this));
+ }
+};
+
+
+template<>
+struct traits : public internal::VectorSpace {};
+
+} // \namespace gtsam
diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp
index 7bcd32b9f..7fa0992ca 100644
--- a/gtsam/base/Matrix.cpp
+++ b/gtsam/base/Matrix.cpp
@@ -20,6 +20,8 @@
#include
#include
#include
+#include
+#include
#include
#include
@@ -180,6 +182,7 @@ void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, SubVec
}
/* ************************************************************************* */
+//3 argument call
void print(const Matrix& A, const string &s, ostream& stream) {
size_t m = A.rows(), n = A.cols();
@@ -198,6 +201,12 @@ void print(const Matrix& A, const string &s, ostream& stream) {
stream << "];" << endl;
}
+/* ************************************************************************* */
+//1 or 2 argument call
+void print(const Matrix& A, const string &s){
+ print(A, s, cout);
+}
+
/* ************************************************************************* */
void save(const Matrix& A, const string &s, const string& filename) {
fstream stream(filename.c_str(), fstream::out | fstream::app);
@@ -697,6 +706,19 @@ std::string formatMatrixIndented(const std::string& label, const Matrix& matrix,
return ss.str();
}
+void inplace_QR(Matrix& A){
+ size_t rows = A.rows();
+ size_t cols = A.cols();
+ size_t size = std::min(rows,cols);
+ typedef Eigen::internal::plain_diag_type::type HCoeffsType;
+ typedef Eigen::internal::plain_row_type::type RowVectorType;
+ HCoeffsType hCoeffs(size);
+ RowVectorType temp(cols);
+
+ Eigen::internal::householder_qr_inplace_blocked::run(A, hCoeffs, 48, temp.data());
+
+ zeroBelowDiagonal(A);
+}
} // namespace gtsam
diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h
index c3cbfa341..27b7ec8f7 100644
--- a/gtsam/base/Matrix.h
+++ b/gtsam/base/Matrix.h
@@ -21,11 +21,14 @@
// \callgraph
#pragma once
-
#include
+#include
+#include
+#include
+#include
#include
#include
-#include
+
/**
* Matrix is a typedef in the gtsam namespace
@@ -198,9 +201,14 @@ inline MATRIX prod(const MATRIX& A, const MATRIX&B) {
}
/**
- * print a matrix
+ * print without optional string, must specify cout yourself
*/
-GTSAM_EXPORT void print(const Matrix& A, const std::string& s = "", std::ostream& stream = std::cout);
+GTSAM_EXPORT void print(const Matrix& A, const std::string& s, std::ostream& stream);
+
+/**
+ * print with optional string to cout
+ */
+GTSAM_EXPORT void print(const Matrix& A, const std::string& s = "");
/**
* save a matrix to file, which can be loaded by matlab
@@ -367,21 +375,7 @@ GTSAM_EXPORT std::pair qr(const Matrix& A);
* @param A is the input matrix, and is the output
* @param clear_below_diagonal enables zeroing out below diagonal
*/
-template
-void inplace_QR(MATRIX& A) {
- size_t rows = A.rows();
- size_t cols = A.cols();
- size_t size = std::min(rows,cols);
-
- typedef Eigen::internal::plain_diag_type::type HCoeffsType;
- typedef Eigen::internal::plain_row_type::type RowVectorType;
- HCoeffsType hCoeffs(size);
- RowVectorType temp(cols);
-
- Eigen::internal::householder_qr_inplace_blocked::run(A, hCoeffs, 48, temp.data());
-
- zeroBelowDiagonal(A);
-}
+void inplace_QR(Matrix& A);
/**
* Imperative algorithm for in-place full elimination with
diff --git a/gtsam/base/Value.h b/gtsam/base/Value.h
index a12f453f4..70677cad1 100644
--- a/gtsam/base/Value.h
+++ b/gtsam/base/Value.h
@@ -18,9 +18,10 @@
#pragma once
-#include
-#include
#include
+#include
+#include
+
namespace gtsam {
diff --git a/gtsam/base/Vector.cpp b/gtsam/base/Vector.cpp
index a9ef8fe10..0a708427a 100644
--- a/gtsam/base/Vector.cpp
+++ b/gtsam/base/Vector.cpp
@@ -16,20 +16,18 @@
* @author Frank Dellaert
*/
+#include
+#include
+#include
+#include
#include
#include
-#include
#include
#include
#include
#include
-#include
-#include
-#include
#include
-
-#include
-
+#include
using namespace std;
@@ -55,6 +53,7 @@ Vector delta(size_t n, size_t i, double value) {
}
/* ************************************************************************* */
+//3 argument call
void print(const Vector& v, const string& s, ostream& stream) {
size_t n = v.size();
@@ -65,6 +64,12 @@ void print(const Vector& v, const string& s, ostream& stream) {
stream << "];" << endl;
}
+/* ************************************************************************* */
+//1 or 2 argument call
+void print(const Vector& v, const string& s) {
+ print(v, s, cout);
+}
+
/* ************************************************************************* */
void save(const Vector& v, const string &s, const string& filename) {
fstream stream(filename.c_str(), fstream::out | fstream::app);
diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h
index 74cd248a1..2d333848b 100644
--- a/gtsam/base/Vector.h
+++ b/gtsam/base/Vector.h
@@ -18,13 +18,12 @@
// \callgraph
-#pragma once
-#include
-#include
-#include
+#pragma once
#include
-#include
+#include
+#include
+#include
namespace gtsam {
@@ -97,9 +96,14 @@ GTSAM_EXPORT bool zero(const Vector& v);
inline size_t dim(const Vector& v) { return v.size(); }
/**
- * print with optional string
+ * print without optional string, must specify cout yourself
*/
-GTSAM_EXPORT void print(const Vector& v, const std::string& s = "", std::ostream& stream = std::cout);
+GTSAM_EXPORT void print(const Vector& v, const std::string& s, std::ostream& stream);
+
+/**
+ * print with optional string to cout
+ */
+GTSAM_EXPORT void print(const Vector& v, const std::string& s = "");
/**
* save a vector to file, which can be loaded by matlab
diff --git a/gtsam/base/tests/testLieMatrix.cpp b/gtsam/base/tests/testLieMatrix.cpp
index 7cc649e66..09caadabd 100644
--- a/gtsam/base/tests/testLieMatrix.cpp
+++ b/gtsam/base/tests/testLieMatrix.cpp
@@ -15,10 +15,10 @@
*/
#include
+#include
#include
#include
-#include
using namespace gtsam;
diff --git a/gtsam/base/tests/testLieScalar.cpp b/gtsam/base/tests/testLieScalar.cpp
index 060087c2a..141b55761 100644
--- a/gtsam/base/tests/testLieScalar.cpp
+++ b/gtsam/base/tests/testLieScalar.cpp
@@ -15,10 +15,10 @@
*/
#include
+#include
#include
#include
-#include
using namespace gtsam;
diff --git a/gtsam/base/tests/testLieVector.cpp b/gtsam/base/tests/testLieVector.cpp
index 81e03c63c..ed3afac8c 100644
--- a/gtsam/base/tests/testLieVector.cpp
+++ b/gtsam/base/tests/testLieVector.cpp
@@ -15,10 +15,10 @@
*/
#include
+#include
#include
#include
-#include
using namespace gtsam;
diff --git a/gtsam/base/tests/testTestableAssertions.cpp b/gtsam/base/tests/testTestableAssertions.cpp
index d8c62b121..364834e2a 100644
--- a/gtsam/base/tests/testTestableAssertions.cpp
+++ b/gtsam/base/tests/testTestableAssertions.cpp
@@ -15,8 +15,8 @@
*/
#include
+#include
-#include
#include
using namespace gtsam;
diff --git a/gtsam/base/types.cpp b/gtsam/base/types.cpp
index 3f86dc0c1..03e7fd120 100644
--- a/gtsam/base/types.cpp
+++ b/gtsam/base/types.cpp
@@ -17,10 +17,9 @@
* @addtogroup base
*/
-#include
-
#include
#include
+#include
namespace gtsam {
diff --git a/gtsam/base/types.h b/gtsam/base/types.h
index aca04a14b..a66db13c8 100644
--- a/gtsam/base/types.h
+++ b/gtsam/base/types.h
@@ -21,18 +21,18 @@
#include
#include
-
-#include
-#include
-#include
#include
#include
#include
+#include
+#include
+#include
#ifdef GTSAM_USE_TBB
#include
#include
#include
+#include
#endif
#ifdef GTSAM_USE_EIGEN_MKL_OPENMP
diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h
index 606f62f87..9ebcbcf5c 100644
--- a/gtsam/geometry/EssentialMatrix.h
+++ b/gtsam/geometry/EssentialMatrix.h
@@ -10,7 +10,7 @@
#include
#include
#include
-#include
+#include
namespace gtsam {
diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h
index 73d0711be..ad727f45e 100644
--- a/gtsam/nonlinear/Values.h
+++ b/gtsam/nonlinear/Values.h
@@ -26,14 +26,9 @@
#include
#include
-#include
#include
-
-#include
-#include
#include
#include
-#include
#ifdef __GNUC__
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-variable"
@@ -43,7 +38,6 @@
#pragma GCC diagnostic pop
#endif
#include
-#include
#include
#include
diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp
index bf5354606..28b5b8c89 100644
--- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp
+++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp
@@ -232,8 +232,12 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) {
values.insert(x2, cam2);
// initialize third pose with some noise, we expect it to move back to original pose_above
values.insert(x3, Camera(pose_above * noise_pose, sharedK2));
- if (isDebugTest)
- values.at(x3).print("Camera before optimization: ");
+ EXPECT(
+ assert_equal(
+ Pose3(
+ Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598,
+ -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598),
+ Point3(0.1, -0.1, 1.9)), values.at(x3).pose()));
LevenbergMarquardtParams params;
if (isDebugTest)
@@ -252,8 +256,6 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) {
// VectorValues delta = GFG->optimize();
// result.print("results of 3 camera, 3 landmark optimization \n");
- if (isDebugTest)
- result.at(x3).print("Camera after optimization: ");
EXPECT(assert_equal(cam3, result.at(x3), 1e-8));
if (isDebugTest)
tictoc_print_();
@@ -442,8 +444,13 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) {
values.insert(x2, cam2);
// initialize third pose with some noise, we expect it to move back to original pose_above
values.insert(x3, Camera(pose_above * noise_pose, sharedK));
- if (isDebugTest)
- values.at(x3).print("Camera before optimization: ");
+ EXPECT(
+ assert_equal(
+ Pose3(
+ Rot3(1.11022302e-16, -0.0314107591, 0.99950656, -0.99950656,
+ -0.0313952598, -0.000986635786, 0.0314107591, -0.999013364,
+ -0.0313952598), Point3(0.1, -0.1, 1.9)),
+ values.at(x3).pose()));
LevenbergMarquardtParams params;
if (isDebugTest)
@@ -459,8 +466,6 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) {
tictoc_finishedIteration_();
// result.print("results of 3 camera, 3 landmark optimization \n");
- if (isDebugTest)
- result.at(x3).print("Camera after optimization: ");
EXPECT(assert_equal(cam3, result.at(x3), 1e-7));
if (isDebugTest)
tictoc_print_();
@@ -819,8 +824,14 @@ TEST( SmartProjectionPoseFactor, CheckHessian) {
values.insert(x2, cam2);
// initialize third pose with some noise, we expect it to move back to original pose_above
values.insert(x3, Camera(pose_above * noise_pose, sharedK));
- if (isDebugTest)
- values.at(x3).print("Camera before optimization: ");
+ EXPECT(
+ assert_equal(
+ Pose3(
+ Rot3(0.00563056869, -0.130848107, 0.991386438, -0.991390265,
+ -0.130426831, -0.0115837907, 0.130819108, -0.98278564,
+ -0.130455917),
+ Point3(0.0897734171, -0.110201006, 0.901022872)),
+ values.at(x3).pose()));
boost::shared_ptr factor1 = smartFactor1->linearize(values);
boost::shared_ptr factor2 = smartFactor2->linearize(values);
@@ -900,8 +911,14 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac
values.insert(x2, Camera(pose2 * noise_pose, sharedK2));
// initialize third pose with some noise, we expect it to move back to original pose_above
values.insert(x3, Camera(pose3 * noise_pose * noise_pose, sharedK2));
- if (isDebugTest)
- values.at(x3).print("Camera before optimization: ");
+ EXPECT(
+ assert_equal(
+ Pose3(
+ Rot3(0.154256096, -0.632754061, 0.75883289, -0.753276814,
+ -0.572308662, -0.324093872, 0.639358349, -0.521617766,
+ -0.564921063),
+ Point3(0.145118171, -0.252907438, 0.819956033)),
+ values.at(x3).pose()));
LevenbergMarquardtParams params;
if (isDebugTest)
@@ -917,12 +934,17 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac
tictoc_finishedIteration_();
// result.print("results of 3 camera, 3 landmark optimization \n");
- if (isDebugTest)
- result.at(x3).print("Camera after optimization: ");
cout
<< "TEST COMMENTED: rotation only version of smart factors has been deprecated "
<< endl;
- // EXPECT(assert_equal(pose_above,result.at(x3)));
+ EXPECT(
+ assert_equal(
+ Pose3(
+ Rot3(0.154256096, -0.632754061, 0.75883289, -0.753276814,
+ -0.572308662, -0.324093872, 0.639358349, -0.521617766,
+ -0.564921063),
+ Point3(0.145118171, -0.252907438, 0.819956033)),
+ result.at(x3).pose()));
if (isDebugTest)
tictoc_print_();
}
@@ -990,8 +1012,14 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor )
values.insert(x2, cam2);
// initialize third pose with some noise, we expect it to move back to original pose_above
values.insert(x3, Camera(pose_above * noise_pose, sharedK));
- if (isDebugTest)
- values.at(x3).print("Camera before optimization: ");
+ EXPECT(
+ assert_equal(
+ Pose3(
+ Rot3(0.00563056869, -0.130848107, 0.991386438, -0.991390265,
+ -0.130426831, -0.0115837907, 0.130819108, -0.98278564,
+ -0.130455917),
+ Point3(0.0897734171, -0.110201006, 0.901022872)),
+ values.at(x3).pose()));
LevenbergMarquardtParams params;
if (isDebugTest)
@@ -1007,12 +1035,17 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor )
tictoc_finishedIteration_();
// result.print("results of 3 camera, 3 landmark optimization \n");
- if (isDebugTest)
- result.at(x3).print("Camera after optimization: ");
cout
<< "TEST COMMENTED: rotation only version of smart factors has been deprecated "
<< endl;
- // EXPECT(assert_equal(pose_above,result.at(x3)));
+ EXPECT(
+ assert_equal(
+ Pose3(
+ Rot3(0.00563056869, -0.130848107, 0.991386438, -0.991390265,
+ -0.130426831, -0.0115837907, 0.130819108, -0.98278564,
+ -0.130455917),
+ Point3(0.0897734171, -0.110201006, 0.901022872)),
+ result.at(x3).pose()));
if (isDebugTest)
tictoc_print_();
}
@@ -1236,9 +1269,12 @@ TEST( SmartProjectionPoseFactor, Cal3Bundler ) {
values.insert(x2, cam2);
// initialize third pose with some noise, we expect it to move back to original pose_above
values.insert(x3, Camera(pose_above * noise_pose, sharedBundlerK));
- if (isDebugTest)
- values.at(x3).print("Camera before optimization: ");
-
+ EXPECT(
+ assert_equal(
+ Pose3(
+ Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598,
+ -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598),
+ Point3(0.1, -0.1, 1.9)), values.at(x3).pose()));
LevenbergMarquardtParams params;
if (isDebugTest)
params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
@@ -1252,9 +1288,6 @@ TEST( SmartProjectionPoseFactor, Cal3Bundler ) {
gttoc_(SmartProjectionPoseFactor);
tictoc_finishedIteration_();
- // result.print("results of 3 camera, 3 landmark optimization \n");
- if (isDebugTest)
- result.at(x3).print("Camera after optimization: ");
EXPECT(assert_equal(cam3, result.at(x3), 1e-6));
if (isDebugTest)
tictoc_print_();
@@ -1342,8 +1375,14 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) {
values.insert(x2, cam2);
// initialize third pose with some noise, we expect it to move back to original pose_above
values.insert(x3, Camera(pose_above * noise_pose, sharedBundlerK));
- if (isDebugTest)
- values.at(x3).print("Camera before optimization: ");
+ EXPECT(
+ assert_equal(
+ Pose3(
+ Rot3(0.00563056869, -0.130848107, 0.991386438, -0.991390265,
+ -0.130426831, -0.0115837907, 0.130819108, -0.98278564,
+ -0.130455917),
+ Point3(0.0897734171, -0.110201006, 0.901022872)),
+ values.at(x3).pose()));
LevenbergMarquardtParams params;
if (isDebugTest)
@@ -1358,13 +1397,17 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) {
gttoc_(SmartProjectionPoseFactor);
tictoc_finishedIteration_();
- // result.print("results of 3 camera, 3 landmark optimization \n");
- if (isDebugTest)
- result.at(x3).print("Camera after optimization: ");
cout
<< "TEST COMMENTED: rotation only version of smart factors has been deprecated "
<< endl;
- // EXPECT(assert_equal(pose_above,result.at(x3)));
+ EXPECT(
+ assert_equal(
+ Pose3(
+ Rot3(0.00563056869, -0.130848107, 0.991386438, -0.991390265,
+ -0.130426831, -0.0115837907, 0.130819108, -0.98278564,
+ -0.130455917),
+ Point3(0.0897734171, -0.110201006, 0.901022872)),
+ values.at(x3).pose()));
if (isDebugTest)
tictoc_print_();
}
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