From 23d4c0fd9f9b9453c929beee94c559668822c6c1 Mon Sep 17 00:00:00 2001 From: Frank Date: Mon, 8 Feb 2016 13:27:38 -0800 Subject: [PATCH 01/90] Deprecated several Point3 methods --- examples/SFMExample.cpp | 2 +- examples/SFMExampleExpressions.cpp | 2 +- examples/SelfCalibrationExample.cpp | 2 +- examples/VisualISAM2Example.cpp | 2 +- examples/VisualISAMExample.cpp | 2 +- gtsam/geometry/Point3.h | 4 +++- gtsam/geometry/tests/testPoint3.cpp | 2 +- gtsam/nonlinear/tests/testAdaptAutoDiff.cpp | 2 +- gtsam_unstable/dynamics/PoseRTV.cpp | 2 +- gtsam_unstable/dynamics/VelocityConstraint.h | 6 +++--- gtsam_unstable/geometry/Event.h | 4 ++-- 11 files changed, 16 insertions(+), 14 deletions(-) diff --git a/examples/SFMExample.cpp b/examples/SFMExample.cpp index 8da9847b8..eaabb3ea9 100644 --- a/examples/SFMExample.cpp +++ b/examples/SFMExample.cpp @@ -99,7 +99,7 @@ int main(int argc, char* argv[]) { for (size_t i = 0; i < poses.size(); ++i) initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)))); for (size_t j = 0; j < points.size(); ++j) - initialEstimate.insert(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15))); + initialEstimate.insert(Symbol('l', j), points[j] + Point3(-0.25, 0.20, 0.15)); initialEstimate.print("Initial Estimates:\n"); /* Optimize the graph and print results */ diff --git a/examples/SFMExampleExpressions.cpp b/examples/SFMExampleExpressions.cpp index df5488ec3..8a59100da 100644 --- a/examples/SFMExampleExpressions.cpp +++ b/examples/SFMExampleExpressions.cpp @@ -88,7 +88,7 @@ int main(int argc, char* argv[]) { for (size_t i = 0; i < poses.size(); ++i) initial.insert(Symbol('x', i), poses[i].compose(delta)); for (size_t j = 0; j < points.size(); ++j) - initial.insert(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15))); + initial.insert(Symbol('l', j), points[j] + Point3(-0.25, 0.20, 0.15)); cout << "initial error = " << graph.error(initial) << endl; /* Optimize the graph and print results */ diff --git a/examples/SelfCalibrationExample.cpp b/examples/SelfCalibrationExample.cpp index f5702e7fb..066b16e8a 100644 --- a/examples/SelfCalibrationExample.cpp +++ b/examples/SelfCalibrationExample.cpp @@ -84,7 +84,7 @@ int main(int argc, char* argv[]) { for (size_t i = 0; i < poses.size(); ++i) initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)))); for (size_t j = 0; j < points.size(); ++j) - initialEstimate.insert(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15))); + initialEstimate.insert(Symbol('l', j), points[j] + Point3(-0.25, 0.20, 0.15)); /* Optimize the graph and print results */ Values result = DoglegOptimizer(graph, initialEstimate).optimize(); diff --git a/examples/VisualISAM2Example.cpp b/examples/VisualISAM2Example.cpp index 75c0a2fa5..c0b4b5b0a 100644 --- a/examples/VisualISAM2Example.cpp +++ b/examples/VisualISAM2Example.cpp @@ -113,7 +113,7 @@ int main(int argc, char* argv[]) { // Add initial guesses to all observed landmarks // Intentionally initialize the variables off from the ground truth for (size_t j = 0; j < points.size(); ++j) - initialEstimate.insert(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15))); + initialEstimate.insert(Symbol('l', j), points[j] +Point3(-0.25, 0.20, 0.15)); } else { // Update iSAM with the new factors diff --git a/examples/VisualISAMExample.cpp b/examples/VisualISAMExample.cpp index 9380410ea..a839289c2 100644 --- a/examples/VisualISAMExample.cpp +++ b/examples/VisualISAMExample.cpp @@ -120,7 +120,7 @@ int main(int argc, char* argv[]) { Point3 noise(-0.25, 0.20, 0.15); for (size_t j = 0; j < points.size(); ++j) { // Intentionally initialize the variables off from the ground truth - Point3 initial_lj = points[j].compose(noise); + Point3 initial_lj = points[j] + noise; initialEstimate.insert(Symbol('l', j), initial_lj); } diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index e19b74d1c..20d316915 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -158,6 +158,7 @@ namespace gtsam { /// Output stream operator GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Point3& p); +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 /// @name Deprecated /// @{ Point3 inverse() const { return -(*this);} @@ -168,6 +169,7 @@ namespace gtsam { static Vector3 Logmap(const Point3& p) { return p.vector();} static Point3 Expmap(const Vector3& v) { return Point3(v);} /// @} +#endif private: diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp index 246d23c9a..a695b95dc 100644 --- a/gtsam/geometry/tests/testPoint3.cpp +++ b/gtsam/geometry/tests/testPoint3.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) diff --git a/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp index 16117845e..d210be789 100644 --- a/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp +++ b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp @@ -167,7 +167,7 @@ Camera camera(Pose3(Rot3().retract(Vector3(0.1, 0.2, 0.3)), Point3(0, 5, 0)), Cal3Bundler0(1, 0, 0)); Point3 point(10, 0, -5); // negative Z-axis convention of Snavely! Vector9 P = Camera().localCoordinates(camera); -Vector3 X = Point3::Logmap(point); +Vector3 X = point.vector(); Vector2 expectedMeasurement(1.2431567, 1.2525694); Matrix E1 = numericalDerivative21(adapted, P, X); Matrix E2 = numericalDerivative22(adapted, P, X); diff --git a/gtsam_unstable/dynamics/PoseRTV.cpp b/gtsam_unstable/dynamics/PoseRTV.cpp index 1a7fdf3de..b3953dd23 100644 --- a/gtsam_unstable/dynamics/PoseRTV.cpp +++ b/gtsam_unstable/dynamics/PoseRTV.cpp @@ -161,7 +161,7 @@ Vector6 PoseRTV::imuPrediction(const PoseRTV& x2, double dt) const { Point3 PoseRTV::translationIntegration(const Rot3& r2, const Velocity3& v2, double dt) const { // predict point for constraint // NOTE: uses simple Euler approach for prediction - Point3 pred_t2 = t().retract(v2 * dt); + Point3 pred_t2 = t() + Point3(v2 * dt); return pred_t2; } diff --git a/gtsam_unstable/dynamics/VelocityConstraint.h b/gtsam_unstable/dynamics/VelocityConstraint.h index c41ea220c..4d1518f4b 100644 --- a/gtsam_unstable/dynamics/VelocityConstraint.h +++ b/gtsam_unstable/dynamics/VelocityConstraint.h @@ -110,9 +110,9 @@ private: const Point3& p1 = x1.t(), p2 = x2.t(); Point3 hx; switch(mode) { - case dynamics::TRAPEZOIDAL: hx = p1.retract((v1 + v2) * dt *0.5); break; - case dynamics::EULER_START: hx = p1.retract(v1 * dt); break; - case dynamics::EULER_END : hx = p1.retract(v2 * dt); break; + case dynamics::TRAPEZOIDAL: hx = p1 + Point3((v1 + v2) * dt *0.5); break; + case dynamics::EULER_START: hx = p1 + Point3(v1 * dt); break; + case dynamics::EULER_END : hx = p1 + Point3(v2 * dt); break; default: assert(false); break; } return (p2 - hx).vector(); diff --git a/gtsam_unstable/geometry/Event.h b/gtsam_unstable/geometry/Event.h index dd362c7f4..5bd453424 100644 --- a/gtsam_unstable/geometry/Event.h +++ b/gtsam_unstable/geometry/Event.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -72,7 +72,7 @@ public: /// Updates a with tangent space delta inline Event retract(const Vector4& v) const { - return Event(time_ + v[0], location_.retract(v.tail(3))); + return Event(time_ + v[0], location_ + Point3(v.tail<3>())); } /// Returns inverse retraction From 4319bece1e7fdcb09d0814d5e83452fcbfd8d37b Mon Sep 17 00:00:00 2001 From: Frank Date: Mon, 8 Feb 2016 15:16:11 -0800 Subject: [PATCH 02/90] Deprecated some more methods --- gtsam.h | 39 +++++++++----------------- gtsam/geometry/Point3.cpp | 5 ++-- gtsam/geometry/Point3.h | 20 ++++--------- gtsampy.h | 39 +++++++++----------------- python/handwritten/geometry/Point3.cpp | 7 ++--- 5 files changed, 39 insertions(+), 71 deletions(-) diff --git a/gtsam.h b/gtsam.h index 16f247a34..c42cf7031 100644 --- a/gtsam.h +++ b/gtsam.h @@ -127,13 +127,13 @@ namespace std { void pop_back();*/ }; //typedef std::vector - + #include template class list { - - + + }; } @@ -360,17 +360,6 @@ class Point3 { // Group static gtsam::Point3 identity(); - gtsam::Point3 inverse() const; - gtsam::Point3 compose(const gtsam::Point3& p2) const; - gtsam::Point3 between(const gtsam::Point3& p2) const; - - // Manifold - gtsam::Point3 retract(Vector v) const; - Vector localCoordinates(const gtsam::Point3& p) const; - - // Lie Group - static gtsam::Point3 Expmap(Vector v); - static Vector Logmap(const gtsam::Point3& p); // Standard Interface Vector vector() const; @@ -1069,7 +1058,7 @@ class SymbolicBayesTree { void clear(); void deleteCachedShortcuts(); size_t numCachedSeparatorMarginals() const; - + gtsam::SymbolicConditional* marginalFactor(size_t key) const; gtsam::SymbolicFactorGraph* joint(size_t key1, size_t key2) const; gtsam::SymbolicBayesNet* jointBayesNet(size_t key1, size_t key2) const; @@ -1079,19 +1068,19 @@ class SymbolicBayesTree { // BayesTreeClique(); // BayesTreeClique(CONDITIONAL* conditional); // // BayesTreeClique(const std::pair& result) : Base(result) {} -// +// // bool equals(const This& other, double tol) const; // void print(string s) const; // void printTree() const; // Default indent of "" // void printTree(string indent) const; // size_t numCachedSeparatorMarginals() const; -// +// // CONDITIONAL* conditional() const; // bool isRoot() const; // size_t treeSize() const; // // const std::list& children() const { return children_; } // // derived_ptr parent() const { return parent_.lock(); } -// +// // // FIXME: need wrapped versions graphs, BayesNet // // BayesNet shortcut(derived_ptr root, Eliminate function) const; // // FactorGraph marginal(derived_ptr root, Eliminate function) const; @@ -1345,7 +1334,7 @@ virtual class JacobianFactor : gtsam::GaussianFactor { void transposeMultiplyAdd(double alpha, const Vector& e, gtsam::VectorValues& x) const; gtsam::JacobianFactor whiten() const; - + pair eliminate(const gtsam::Ordering& keys) const; void setModel(bool anyConstrained, const Vector& sigmas); @@ -1422,7 +1411,7 @@ class GaussianFactorGraph { gtsam::GaussianFactorGraph clone() const; gtsam::GaussianFactorGraph negate() const; - // Optimizing and linear algebra + // Optimizing and linear algebra gtsam::VectorValues optimize() const; gtsam::VectorValues optimize(const gtsam::Ordering& ordering) const; gtsam::VectorValues optimizeGradientSearch() const; @@ -1511,7 +1500,7 @@ virtual class GaussianBayesNet { //Constructors GaussianBayesNet(); GaussianBayesNet(const gtsam::GaussianConditional* conditional); - + // Testable void print(string s) const; bool equals(const gtsam::GaussianBayesNet& other, double tol) const; @@ -1527,7 +1516,7 @@ virtual class GaussianBayesNet { gtsam::GaussianConditional* back() const; void push_back(gtsam::GaussianConditional* conditional); void push_back(const gtsam::GaussianBayesNet& bayesNet); - + gtsam::VectorValues optimize() const; gtsam::VectorValues optimize(gtsam::VectorValues& solutionForMissing) const; gtsam::VectorValues optimizeGradientSearch() const; @@ -1551,7 +1540,7 @@ virtual class GaussianBayesTree { bool empty() const; size_t numCachedSeparatorMarginals() const; void saveGraph(string s) const; - + gtsam::VectorValues optimize() const; gtsam::VectorValues optimizeGradientSearch() const; gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const; @@ -1833,7 +1822,7 @@ class Values { /// Fixed size versions, for n in 1..9 Vector atFixed(size_t j, size_t n); - + /// version for double void insertDouble(size_t j, double c); double atDouble(size_t j) const; @@ -2004,7 +1993,7 @@ virtual class NonlinearOptimizerParams { void setVerbosity(string s); string getLinearSolverType() const; - + void setLinearSolverType(string solver); void setOrdering(const gtsam::Ordering& ordering); void setIterativeParams(gtsam::IterativeOptimizationParameters* params); diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp index bffda9fef..29d59d9e3 100644 --- a/gtsam/geometry/Point3.cpp +++ b/gtsam/geometry/Point3.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -75,6 +75,7 @@ double Point3::distance(const Point3 &p2, OptionalJacobian<1, 3> H1, } /* ************************************************************************* */ +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 Point3 Point3::add(const Point3 &q, OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const { if (H1) *H1 = I_3x3; @@ -82,13 +83,13 @@ Point3 Point3::add(const Point3 &q, OptionalJacobian<3,3> H1, return *this + q; } -/* ************************************************************************* */ Point3 Point3::sub(const Point3 &q, OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const { if (H1) *H1 = I_3x3; if (H2) *H2 = I_3x3; return *this - q; } +#endif /* ************************************************************************* */ Point3 Point3::cross(const Point3 &q, OptionalJacobian<3, 3> H_p, OptionalJacobian<3, 3> H_q) const { diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 20d316915..978d7b963 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -107,11 +107,6 @@ namespace gtsam { double distance(const Point3& p2, OptionalJacobian<1, 3> H1 = boost::none, OptionalJacobian<1, 3> H2 = boost::none) const; - /** @deprecated The following function has been deprecated, use distance above */ - inline double dist(const Point3& p2) const { - return (p2 - *this).norm(); - } - /** Distance of the point from the origin, with Jacobian */ double norm(OptionalJacobian<1,3> H = boost::none) const; @@ -145,14 +140,6 @@ namespace gtsam { /// get z inline double z() const {return z_;} - /** add two points, add(this,q) is same as this + q */ - Point3 add (const Point3 &q, - OptionalJacobian<3, 3> H1=boost::none, OptionalJacobian<3, 3> H2=boost::none) const; - - /** subtract two points, sub(this,q) is same as this - q */ - Point3 sub (const Point3 &q, - OptionalJacobian<3,3> H1=boost::none, OptionalJacobian<3,3> H2=boost::none) const; - /// @} /// Output stream operator @@ -168,7 +155,12 @@ namespace gtsam { Point3 retract(const Vector3& v) const { return compose(Point3(v));} static Vector3 Logmap(const Point3& p) { return p.vector();} static Point3 Expmap(const Vector3& v) { return Point3(v);} - /// @} + inline double dist(const Point3& p2) const { return (p2 - *this).norm(); } + Point3 add(const Point3& q, OptionalJacobian<3, 3> H1 = boost::none, + OptionalJacobian<3, 3> H2 = boost::none) const; + Point3 sub(const Point3& q, OptionalJacobian<3, 3> H1 = boost::none, + OptionalJacobian<3, 3> H2 = boost::none) const; + /// @} #endif private: diff --git a/gtsampy.h b/gtsampy.h index ca014ad5d..9cadf6be3 100644 --- a/gtsampy.h +++ b/gtsampy.h @@ -127,13 +127,13 @@ namespace std { void pop_back();*/ }; //typedef std::vector - + #include template class list { - - + + }; } @@ -360,17 +360,6 @@ class Point3 { // Group static gtsam::Point3 identity(); - gtsam::Point3 inverse() const; - gtsam::Point3 compose(const gtsam::Point3& p2) const; - gtsam::Point3 between(const gtsam::Point3& p2) const; - - // Manifold - gtsam::Point3 retract(Vector v) const; - Vector localCoordinates(const gtsam::Point3& p) const; - - // Lie Group - static gtsam::Point3 Expmap(Vector v); - static Vector Logmap(const gtsam::Point3& p); // Standard Interface Vector vector() const; @@ -1069,7 +1058,7 @@ class SymbolicBayesTree { void clear(); void deleteCachedShortcuts(); size_t numCachedSeparatorMarginals() const; - + gtsam::SymbolicConditional* marginalFactor(size_t key) const; gtsam::SymbolicFactorGraph* joint(size_t key1, size_t key2) const; gtsam::SymbolicBayesNet* jointBayesNet(size_t key1, size_t key2) const; @@ -1079,19 +1068,19 @@ class SymbolicBayesTree { // BayesTreeClique(); // BayesTreeClique(CONDITIONAL* conditional); // // BayesTreeClique(const std::pair& result) : Base(result) {} -// +// // bool equals(const This& other, double tol) const; // void print(string s) const; // void printTree() const; // Default indent of "" // void printTree(string indent) const; // size_t numCachedSeparatorMarginals() const; -// +// // CONDITIONAL* conditional() const; // bool isRoot() const; // size_t treeSize() const; // // const std::list& children() const { return children_; } // // derived_ptr parent() const { return parent_.lock(); } -// +// // // FIXME: need wrapped versions graphs, BayesNet // // BayesNet shortcut(derived_ptr root, Eliminate function) const; // // FactorGraph marginal(derived_ptr root, Eliminate function) const; @@ -1345,7 +1334,7 @@ virtual class JacobianFactor : gtsam::GaussianFactor { void transposeMultiplyAdd(double alpha, const Vector& e, gtsam::VectorValues& x) const; gtsam::JacobianFactor whiten() const; - + pair eliminate(const gtsam::Ordering& keys) const; void setModel(bool anyConstrained, const Vector& sigmas); @@ -1422,7 +1411,7 @@ class GaussianFactorGraph { gtsam::GaussianFactorGraph clone() const; gtsam::GaussianFactorGraph negate() const; - // Optimizing and linear algebra + // Optimizing and linear algebra gtsam::VectorValues optimize() const; gtsam::VectorValues optimize(const gtsam::Ordering& ordering) const; gtsam::VectorValues optimizeGradientSearch() const; @@ -1511,7 +1500,7 @@ virtual class GaussianBayesNet { //Constructors GaussianBayesNet(); GaussianBayesNet(const gtsam::GaussianConditional* conditional); - + // Testable void print(string s) const; bool equals(const gtsam::GaussianBayesNet& other, double tol) const; @@ -1527,7 +1516,7 @@ virtual class GaussianBayesNet { gtsam::GaussianConditional* back() const; void push_back(gtsam::GaussianConditional* conditional); void push_back(const gtsam::GaussianBayesNet& bayesNet); - + gtsam::VectorValues optimize() const; gtsam::VectorValues optimize(gtsam::VectorValues& solutionForMissing) const; gtsam::VectorValues optimizeGradientSearch() const; @@ -1551,7 +1540,7 @@ virtual class GaussianBayesTree { bool empty() const; size_t numCachedSeparatorMarginals() const; void saveGraph(string s) const; - + gtsam::VectorValues optimize() const; gtsam::VectorValues optimizeGradientSearch() const; gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const; @@ -1833,7 +1822,7 @@ class Values { /// Fixed size versions, for n in 1..9 Vector atFixed(size_t j, size_t n); - + /// version for double void insertDouble(size_t j, double c); double atDouble(size_t j) const; @@ -2004,7 +1993,7 @@ virtual class NonlinearOptimizerParams { void setVerbosity(string s); string getLinearSolverType() const; - + void setLinearSolverType(string solver); void setOrdering(const gtsam::Ordering& ordering); void setIterativeParams(gtsam::IterativeOptimizationParameters* params); diff --git a/python/handwritten/geometry/Point3.cpp b/python/handwritten/geometry/Point3.cpp index 664b4ffda..5a3ba8fb5 100644 --- a/python/handwritten/geometry/Point3.cpp +++ b/python/handwritten/geometry/Point3.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -36,16 +36,13 @@ class_("Point3") .def(init()) .def("identity", &Point3::identity) .staticmethod("identity") - .def("add", &Point3::add) .def("cross", &Point3::cross) - .def("dist", &Point3::dist) .def("distance", &Point3::distance) .def("dot", &Point3::dot) .def("equals", &Point3::equals, equals_overloads(args("q","tol"))) .def("norm", &Point3::norm) .def("normalize", &Point3::normalize) .def("print", &Point3::print, print_overloads(args("s"))) - .def("sub", &Point3::sub) .def("vector", &Point3::vector) .def("x", &Point3::x) .def("y", &Point3::y) @@ -61,4 +58,4 @@ class_("Point3") .def(self == self) ; -} \ No newline at end of file +} From a19aa793d7b8f9a80c853da668c5cca5e0dcf92a Mon Sep 17 00:00:00 2001 From: Frank Date: Mon, 8 Feb 2016 17:30:44 -0800 Subject: [PATCH 03/90] Cleaned up a number of Point3/Rot3 related uses --- gtsam/geometry/tests/testCalibratedCamera.cpp | 12 +++----- gtsam/geometry/tests/testPinholeCamera.cpp | 4 +-- gtsam/geometry/tests/testPinholePose.cpp | 4 +-- gtsam/geometry/tests/testPoint3.cpp | 12 ++++---- gtsam/geometry/tests/testRot3.cpp | 29 +++++++++---------- gtsam/geometry/tests/testSimpleCamera.cpp | 12 +++----- gtsam/geometry/tests/testStereoCamera.cpp | 6 ++-- gtsam/navigation/GPSFactor.cpp | 4 +-- gtsam/navigation/ScenarioRunner.h | 2 +- gtsam/navigation/tests/testNavState.cpp | 2 +- gtsam/slam/tests/testGeneralSFMFactor.cpp | 8 ++--- gtsam/slam/tests/testStereoFactor.cpp | 6 +--- gtsam_unstable/dynamics/VelocityConstraint.h | 2 +- tests/simulated3D.h | 6 ++-- 14 files changed, 48 insertions(+), 61 deletions(-) diff --git a/gtsam/geometry/tests/testCalibratedCamera.cpp b/gtsam/geometry/tests/testCalibratedCamera.cpp index 5bc645a58..cc7a0c0f8 100644 --- a/gtsam/geometry/tests/testCalibratedCamera.cpp +++ b/gtsam/geometry/tests/testCalibratedCamera.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -30,13 +30,9 @@ using namespace gtsam; GTSAM_CONCEPT_MANIFOLD_INST(CalibratedCamera) // Camera situated at 0.5 meters high, looking down -static const Pose3 pose1((Matrix3() << - 1., 0., 0., - 0.,-1., 0., - 0., 0.,-1. - ).finished(), - Point3(0,0,0.5)); - +static const Pose3 pose1(Rot3(Vector3(1, -1, -1).asDiagonal()), + Point3(0, 0, 0.5)); + static const CalibratedCamera camera(pose1); static const Point3 point1(-0.08,-0.08, 0.0); diff --git a/gtsam/geometry/tests/testPinholeCamera.cpp b/gtsam/geometry/tests/testPinholeCamera.cpp index b2579c0d9..ce9932879 100644 --- a/gtsam/geometry/tests/testPinholeCamera.cpp +++ b/gtsam/geometry/tests/testPinholeCamera.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -34,7 +34,7 @@ typedef PinholeCamera Camera; static const Cal3_S2 K(625, 625, 0, 0, 0); -static const Pose3 pose(Matrix3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5)); +static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5)); static const Camera camera(pose, K); static const Pose3 pose1(Rot3(), Point3(0, 1, 0.5)); diff --git a/gtsam/geometry/tests/testPinholePose.cpp b/gtsam/geometry/tests/testPinholePose.cpp index 8f3eadc51..3c79a45e3 100644 --- a/gtsam/geometry/tests/testPinholePose.cpp +++ b/gtsam/geometry/tests/testPinholePose.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -35,7 +35,7 @@ typedef PinholePose Camera; static const Cal3_S2::shared_ptr K = boost::make_shared(625, 625, 0, 0, 0); -static const Pose3 pose(Matrix3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5)); +static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5)); static const Camera camera(pose, K); static const Pose3 pose1(Rot3(), Point3(0, 1, 0.5)); diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp index a695b95dc..d0123af83 100644 --- a/gtsam/geometry/tests/testPoint3.cpp +++ b/gtsam/geometry/tests/testPoint3.cpp @@ -61,12 +61,12 @@ TEST(Point3, Lie) { /* ************************************************************************* */ TEST( Point3, arithmetic) { CHECK(P * 3 == 3 * P); - CHECK(assert_equal(Point3(-1, -5, -6), -Point3(1, 5, 6))); - CHECK(assert_equal(Point3(2, 5, 6), Point3(1, 4, 5) + Point3(1, 1, 1))); - CHECK(assert_equal(Point3(0, 3, 4), Point3(1, 4, 5) - Point3(1, 1, 1))); - CHECK(assert_equal(Point3(2, 8, 6), Point3(1, 4, 3) * 2)); - CHECK(assert_equal(Point3(2, 2, 6), 2 * Point3(1, 1, 3))); - CHECK(assert_equal(Point3(1, 2, 3), Point3(2, 4, 6) / 2)); + CHECK(assert_equal(Point3(-1, -5, -6), -Point3(1, 5, 6))); + CHECK(assert_equal(Point3(2, 5, 6), Point3(1, 4, 5) + Point3(1, 1, 1))); + CHECK(assert_equal(Point3(0, 3, 4), Point3(1, 4, 5) - Point3(1, 1, 1))); + CHECK(assert_equal(Point3(2, 8, 6), Point3(1, 4, 3) * 2)); + CHECK(assert_equal(Point3(2, 2, 6), 2 * Point3(1, 1, 3))); + CHECK(assert_equal(Point3(1, 2, 3), Point3(2, 4, 6) / 2)); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 4b3c84e01..ce5d3268c 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -97,10 +97,10 @@ TEST( Rot3, equals) // Notice this uses J^2 whereas fast uses w*w', and has cos(t)*I + .... Rot3 slow_but_correct_Rodrigues(const Vector& w) { double t = norm_2(w); - Matrix J = skewSymmetric(w / t); + Matrix3 J = skewSymmetric(w / t); if (t < 1e-5) return Rot3(); - Matrix R = I_3x3 + sin(t) * J + (1.0 - cos(t)) * (J * J); - return R; + Matrix3 R = I_3x3 + sin(t) * J + (1.0 - cos(t)) * (J * J); + return Rot3(R); } /* ************************************************************************* */ @@ -201,7 +201,7 @@ TEST(Rot3, log) // Windows and Linux have flipped sign in quaternion mode #if !defined(__APPLE__) && defined (GTSAM_USE_QUATERNIONS) w = (Vector(3) << x*PI, y*PI, z*PI).finished(); - R = Rot3::Rodrigues(w); + R = Rot3::Rodrigues(w); EXPECT(assert_equal(Vector(-w), Rot3::Logmap(R),1e-12)); #else CHECK_OMEGA(x*PI,y*PI,z*PI) @@ -274,14 +274,13 @@ TEST(Rot3, manifold_expmap) } /* ************************************************************************* */ -class AngularVelocity: public Point3 { -public: - AngularVelocity(const Point3& p) : - Point3(p) { - } - AngularVelocity(double wx, double wy, double wz) : - Point3(wx, wy, wz) { - } +class AngularVelocity : public Vector3 { + public: + template + inline AngularVelocity(const Eigen::MatrixBase& v) + : Vector3(v) {} + + AngularVelocity(double wx, double wy, double wz) : Vector3(wx, wy, wz) {} }; AngularVelocity bracket(const AngularVelocity& X, const AngularVelocity& Y) { @@ -294,10 +293,10 @@ TEST(Rot3, BCH) // Approximate exmap by BCH formula AngularVelocity w1(0.2, -0.1, 0.1); AngularVelocity w2(0.01, 0.02, -0.03); - Rot3 R1 = Rot3::Expmap (w1.vector()), R2 = Rot3::Expmap (w2.vector()); + Rot3 R1 = Rot3::Expmap (w1), R2 = Rot3::Expmap (w2); Rot3 R3 = R1 * R2; Vector expected = Rot3::Logmap(R3); - Vector actual = BCH(w1, w2).vector(); + Vector actual = BCH(w1, w2); CHECK(assert_equal(expected, actual,1e-5)); } diff --git a/gtsam/geometry/tests/testSimpleCamera.cpp b/gtsam/geometry/tests/testSimpleCamera.cpp index 515542298..8db8113a1 100644 --- a/gtsam/geometry/tests/testSimpleCamera.cpp +++ b/gtsam/geometry/tests/testSimpleCamera.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -28,13 +28,9 @@ using namespace gtsam; static const Cal3_S2 K(625, 625, 0, 0, 0); -static const Pose3 pose1((Matrix3() << - 1., 0., 0., - 0.,-1., 0., - 0., 0.,-1. - ).finished(), - Point3(0,0,0.5)); - +static const Pose3 pose1(Rot3(Vector3(1, -1, -1).asDiagonal()), + Point3(0, 0, 0.5)); + static const SimpleCamera camera(pose1, K); static const Point3 point1(-0.08,-0.08, 0.0); diff --git a/gtsam/geometry/tests/testStereoCamera.cpp b/gtsam/geometry/tests/testStereoCamera.cpp index 3adf2257d..6508965ec 100644 --- a/gtsam/geometry/tests/testStereoCamera.cpp +++ b/gtsam/geometry/tests/testStereoCamera.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -74,11 +74,11 @@ TEST( StereoCamera, project) /* ************************************************************************* */ -static Pose3 camPose((Matrix)(Matrix(3,3) << +static Pose3 camPose(Rot3((Matrix3() << 1., 0., 0., 0.,-1., 0., 0., 0.,-1. - ).finished(), + ).finished()), Point3(0,0,6.25)); static Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5)); diff --git a/gtsam/navigation/GPSFactor.cpp b/gtsam/navigation/GPSFactor.cpp index 87913cda6..2f4cb9bc3 100644 --- a/gtsam/navigation/GPSFactor.cpp +++ b/gtsam/navigation/GPSFactor.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -43,7 +43,7 @@ Vector GPSFactor::evaluateError(const Pose3& p, H->block < 3, 3 > (0, 0) << zeros(3, 3); H->block < 3, 3 > (0, 3) << p.rotation().matrix(); } - return (p.translation() -nT_).vector(); + return p.translation().vector() -nT_.vector(); } //*************************************************************************** diff --git a/gtsam/navigation/ScenarioRunner.h b/gtsam/navigation/ScenarioRunner.h index b038a831b..537785e06 100644 --- a/gtsam/navigation/ScenarioRunner.h +++ b/gtsam/navigation/ScenarioRunner.h @@ -73,7 +73,7 @@ class ScenarioRunner { // An accelerometer measures acceleration in body, but not gravity Vector3 actualSpecificForce(double t) const { - Rot3 bRn = scenario_->rotation(t).transpose(); + Rot3 bRn(scenario_->rotation(t).transpose()); return scenario_->acceleration_b(t) - bRn * gravity_n(); } diff --git a/gtsam/navigation/tests/testNavState.cpp b/gtsam/navigation/tests/testNavState.cpp index a62ca06a8..2fbb9fee3 100644 --- a/gtsam/navigation/tests/testNavState.cpp +++ b/gtsam/navigation/tests/testNavState.cpp @@ -81,7 +81,7 @@ TEST( NavState, Velocity) { TEST( NavState, BodyVelocity) { Matrix39 aH, eH; Velocity3 actual = kState1.bodyVelocity(aH); - EXPECT(assert_equal(actual, kAttitude.unrotate(kVelocity))); + EXPECT(assert_equal(actual, kAttitude.unrotate(kVelocity))); eH = numericalDerivative11( boost::bind(&NavState::bodyVelocity, _1, boost::none), kState1); EXPECT(assert_equal((Matrix )eH, aH)); diff --git a/gtsam/slam/tests/testGeneralSFMFactor.cpp b/gtsam/slam/tests/testGeneralSFMFactor.cpp index a634928dc..07abb557f 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor.cpp @@ -105,16 +105,16 @@ static vector genPoint3() { static vector genCameraDefaultCalibration() { vector X; - X.push_back(GeneralCamera(Pose3(eye(3), Point3(-baseline / 2., 0., 0.)))); - X.push_back(GeneralCamera(Pose3(eye(3), Point3(baseline / 2., 0., 0.)))); + X.push_back(GeneralCamera(Pose3(Rot3(), Point3(-baseline / 2., 0., 0.)))); + X.push_back(GeneralCamera(Pose3(Rot3(), Point3(baseline / 2., 0., 0.)))); return X; } static vector genCameraVariableCalibration() { const Cal3_S2 K(640, 480, 0.1, 320, 240); vector X; - X.push_back(GeneralCamera(Pose3(eye(3), Point3(-baseline / 2., 0., 0.)), K)); - X.push_back(GeneralCamera(Pose3(eye(3), Point3(baseline / 2., 0., 0.)), K)); + X.push_back(GeneralCamera(Pose3(Rot3(), Point3(-baseline / 2., 0., 0.)), K)); + X.push_back(GeneralCamera(Pose3(Rot3(), Point3(baseline / 2., 0., 0.)), K)); return X; } diff --git a/gtsam/slam/tests/testStereoFactor.cpp b/gtsam/slam/tests/testStereoFactor.cpp index b6d2e2ef8..45b978c27 100644 --- a/gtsam/slam/tests/testStereoFactor.cpp +++ b/gtsam/slam/tests/testStereoFactor.cpp @@ -31,11 +31,7 @@ using namespace std; using namespace gtsam; -static Pose3 camera1((Matrix) (Matrix(3, 3) << - 1., 0., 0., - 0.,-1., 0., - 0., 0.,-1. - ).finished(), +static Pose3 camera1(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0,0,6.25)); static boost::shared_ptr K(new Cal3_S2Stereo(625, 625, 0, 320, 240, 0.5)); diff --git a/gtsam_unstable/dynamics/VelocityConstraint.h b/gtsam_unstable/dynamics/VelocityConstraint.h index 4d1518f4b..4ce4d5758 100644 --- a/gtsam_unstable/dynamics/VelocityConstraint.h +++ b/gtsam_unstable/dynamics/VelocityConstraint.h @@ -115,7 +115,7 @@ private: case dynamics::EULER_END : hx = p1 + Point3(v2 * dt); break; default: assert(false); break; } - return (p2 - hx).vector(); + return p2.vector() - hx.vector(); } }; diff --git a/tests/simulated3D.h b/tests/simulated3D.h index 46945558a..cf69a8fa3 100644 --- a/tests/simulated3D.h +++ b/tests/simulated3D.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -91,7 +91,7 @@ struct PointPrior3D: public NoiseModelFactor1 { */ Vector evaluateError(const Point3& x, boost::optional H = boost::none) const { - return (prior(x, H) - measured_).vector(); + return prior(x, H).vector() - measured_.vector(); } }; @@ -122,7 +122,7 @@ struct Simulated3DMeasurement: public NoiseModelFactor2 { */ Vector evaluateError(const Point3& x1, const Point3& x2, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { - return (mea(x1, x2, H1, H2) - measured_).vector(); + return mea(x1, x2, H1, H2).vector() - measured_.vector(); } }; From a86a3eee3e7b427910f6ca7b24a3a7206701358d Mon Sep 17 00:00:00 2001 From: Frank Date: Mon, 8 Feb 2016 17:31:57 -0800 Subject: [PATCH 04/90] Fixed BCH compile issue --- gtsam/base/Lie.h | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index 1576aca1d..23ac0f4df 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -295,19 +295,18 @@ private: }; /** - * Three term approximation of the Baker�Campbell�Hausdorff formula + * Three term approximation of the Baker-Campbell-Hausdorff formula * In non-commutative Lie groups, when composing exp(Z) = exp(X)exp(Y) * it is not true that Z = X+Y. Instead, Z can be calculated using the BCH * formula: Z = X + Y + [X,Y]/2 + [X-Y,[X,Y]]/12 - [Y,[X,[X,Y]]]/24 - * http://en.wikipedia.org/wiki/Baker�Campbell�Hausdorff_formula + * http://en.wikipedia.org/wiki/Baker-Campbell-Hausdorff_formula */ /// AGC: bracket() only appears in Rot3 tests, should this be used elsewhere? template T BCH(const T& X, const T& Y) { static const double _2 = 1. / 2., _12 = 1. / 12., _24 = 1. / 24.; T X_Y = bracket(X, Y); - return X + Y + _2 * X_Y + _12 * bracket(X - Y, X_Y) - _24 * bracket(Y, - bracket(X, X_Y)); + return T(X + Y + _2 * X_Y + _12 * bracket(X - Y, X_Y) - _24 * bracket(Y, bracket(X, X_Y))); } /** From 3052afe42b9f916916beda3826942852d647f48b Mon Sep 17 00:00:00 2001 From: Frank Date: Mon, 8 Feb 2016 17:32:25 -0800 Subject: [PATCH 05/90] Explicit template of insert --- examples/SFMExample.cpp | 2 +- examples/SFMExampleExpressions.cpp | 2 +- examples/SelfCalibrationExample.cpp | 2 +- examples/VisualISAM2Example.cpp | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/examples/SFMExample.cpp b/examples/SFMExample.cpp index eaabb3ea9..893454936 100644 --- a/examples/SFMExample.cpp +++ b/examples/SFMExample.cpp @@ -99,7 +99,7 @@ int main(int argc, char* argv[]) { for (size_t i = 0; i < poses.size(); ++i) initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)))); for (size_t j = 0; j < points.size(); ++j) - initialEstimate.insert(Symbol('l', j), points[j] + Point3(-0.25, 0.20, 0.15)); + initialEstimate.insert(Symbol('l', j), points[j] + Point3(-0.25, 0.20, 0.15)); initialEstimate.print("Initial Estimates:\n"); /* Optimize the graph and print results */ diff --git a/examples/SFMExampleExpressions.cpp b/examples/SFMExampleExpressions.cpp index 8a59100da..30db9144d 100644 --- a/examples/SFMExampleExpressions.cpp +++ b/examples/SFMExampleExpressions.cpp @@ -88,7 +88,7 @@ int main(int argc, char* argv[]) { for (size_t i = 0; i < poses.size(); ++i) initial.insert(Symbol('x', i), poses[i].compose(delta)); for (size_t j = 0; j < points.size(); ++j) - initial.insert(Symbol('l', j), points[j] + Point3(-0.25, 0.20, 0.15)); + initial.insert(Symbol('l', j), points[j] + Point3(-0.25, 0.20, 0.15)); cout << "initial error = " << graph.error(initial) << endl; /* Optimize the graph and print results */ diff --git a/examples/SelfCalibrationExample.cpp b/examples/SelfCalibrationExample.cpp index 066b16e8a..1c5d155dc 100644 --- a/examples/SelfCalibrationExample.cpp +++ b/examples/SelfCalibrationExample.cpp @@ -84,7 +84,7 @@ int main(int argc, char* argv[]) { for (size_t i = 0; i < poses.size(); ++i) initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)))); for (size_t j = 0; j < points.size(); ++j) - initialEstimate.insert(Symbol('l', j), points[j] + Point3(-0.25, 0.20, 0.15)); + initialEstimate.insert(Symbol('l', j), points[j] + Point3(-0.25, 0.20, 0.15)); /* Optimize the graph and print results */ Values result = DoglegOptimizer(graph, initialEstimate).optimize(); diff --git a/examples/VisualISAM2Example.cpp b/examples/VisualISAM2Example.cpp index c0b4b5b0a..78bc463ec 100644 --- a/examples/VisualISAM2Example.cpp +++ b/examples/VisualISAM2Example.cpp @@ -113,7 +113,7 @@ int main(int argc, char* argv[]) { // Add initial guesses to all observed landmarks // Intentionally initialize the variables off from the ground truth for (size_t j = 0; j < points.size(); ++j) - initialEstimate.insert(Symbol('l', j), points[j] +Point3(-0.25, 0.20, 0.15)); + initialEstimate.insert(Symbol('l', j), points[j] +Point3(-0.25, 0.20, 0.15)); } else { // Update iSAM with the new factors From 3394e85ef7b82905cfb4ad6884bef6898009085d Mon Sep 17 00:00:00 2001 From: Frank Date: Mon, 8 Feb 2016 17:34:42 -0800 Subject: [PATCH 06/90] Now allows for a flag to compile Point3 as derived from Vector3 --- gtsam/geometry/Point3.cpp | 31 ++++---- gtsam/geometry/Point3.h | 100 +++++++++++++++++++++++-- gtsam/geometry/Pose3.cpp | 4 +- gtsam/geometry/Pose3.h | 6 +- gtsam/geometry/Rot3.h | 37 +++++---- gtsam/geometry/Unit3.cpp | 6 +- gtsam/geometry/Unit3.h | 2 + python/handwritten/geometry/Point3.cpp | 4 + python/handwritten/geometry/Rot3.cpp | 17 +++-- 9 files changed, 159 insertions(+), 48 deletions(-) diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp index 29d59d9e3..7b1a26e2c 100644 --- a/gtsam/geometry/Point3.cpp +++ b/gtsam/geometry/Point3.cpp @@ -15,25 +15,25 @@ */ #include +#include using namespace std; namespace gtsam { /* ************************************************************************* */ -bool Point3::equals(const Point3 & q, double tol) const { - return (fabs(x_ - q.x()) < tol && fabs(y_ - q.y()) < tol - && fabs(z_ - q.z()) < tol); +bool Point3::equals(const Point3 &q, double tol) const { + return (fabs(x() - q.x()) < tol && fabs(y() - q.y()) < tol && + fabs(z() - q.z()) < tol); } /* ************************************************************************* */ - void Point3::print(const string& s) const { cout << s << *this << endl; } +#ifndef GTSAM_USE_VECTOR3_POINTS /* ************************************************************************* */ - bool Point3::operator==(const Point3& q) const { return x_ == q.x_ && y_ == q.y_ && z_ == q.z_; } @@ -57,18 +57,19 @@ Point3 Point3::operator*(double s) const { Point3 Point3::operator/(double s) const { return Point3(x_ / s, y_ / s, z_ / s); } +#endif /* ************************************************************************* */ double Point3::distance(const Point3 &p2, OptionalJacobian<1, 3> H1, OptionalJacobian<1, 3> H2) const { double d = (p2 - *this).norm(); if (H1) { - *H1 << x_ - p2.x(), y_ - p2.y(), z_ - p2.z(); + *H1 << x() - p2.x(), y() - p2.y(), z() - p2.z(); *H1 = *H1 *(1. / d); } if (H2) { - *H2 << -x_ + p2.x(), -y_ + p2.y(), -z_ + p2.z(); + *H2 << -x() + p2.x(), -y() + p2.y(), -z() + p2.z(); *H2 = *H2 *(1. / d); } return d; @@ -100,8 +101,8 @@ Point3 Point3::cross(const Point3 &q, OptionalJacobian<3, 3> H_p, OptionalJacobi *H_q << skewSymmetric(vector()); } - return Point3(y_ * q.z_ - z_ * q.y_, z_ * q.x_ - x_ * q.z_, - x_ * q.y_ - y_ * q.x_); + return Point3(y() * q.z() - z() * q.y(), z() * q.x() - x() * q.z(), + x() * q.y() - y() * q.x()); } /* ************************************************************************* */ @@ -113,15 +114,15 @@ double Point3::dot(const Point3 &q, OptionalJacobian<1, 3> H_p, OptionalJacobian *H_q << vector().transpose(); } - return (x_ * q.x_ + y_ * q.y_ + z_ * q.z_); + return (x() * q.x() + y() * q.y() + z() * q.z()); } /* ************************************************************************* */ double Point3::norm(OptionalJacobian<1,3> H) const { - double r = sqrt(x_ * x_ + y_ * y_ + z_ * z_); + double r = std::sqrt(x() * x() + y() * y() + z() * z()); if (H) { if (fabs(r) > 1e-10) - *H << x_ / r, y_ / r, z_ / r; + *H << x() / r, y() / r, z() / r; else *H << 1, 1, 1; // really infinity, why 1 ? } @@ -133,10 +134,10 @@ Point3 Point3::normalize(OptionalJacobian<3,3> H) const { Point3 normalized = *this / norm(); if (H) { // 3*3 Derivative - double x2 = x_ * x_, y2 = y_ * y_, z2 = z_ * z_; - double xy = x_ * y_, xz = x_ * z_, yz = y_ * z_; + double x2 = x() * x(), y2 = y() * y(), z2 = z() * z(); + double xy = x() * y(), xz = x() * z(), yz = y() * z(); *H << y2 + z2, -xy, -xz, /**/-xy, x2 + z2, -yz, /**/-xz, -yz, x2 + y2; - *H /= pow(x2 + y2 + z2, 1.5); + *H /= std::pow(x2 + y2 + z2, 1.5); } return normalized; } diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 978d7b963..467703d46 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -22,18 +22,104 @@ #pragma once #include +#include #include #include -#include namespace gtsam { - /** +//#define GTSAM_USE_VECTOR3_POINTS +#ifdef GTSAM_USE_VECTOR3_POINTS +/** + * A 3D point is just a Vector3 with some additional methods + * @addtogroup geometry + * \nosubgrouping + */ +class GTSAM_EXPORT Point3 : public Vector3 { + + public: + enum { dimension = 3 }; + + /// @name Standard Constructors + /// @{ + + /// Default constructor creates a zero-Point3 + Point3() { setZero(); } + + /// Construct from x, y, and z coordinates + Point3(double x, double y, double z): Vector3(x,y, z) {} + + /// Construct from other vector + template + inline Point3(const Eigen::MatrixBase& v): Vector3(v) {} + + /// @} + /// @name Testable + /// @{ + + /** print with optional string */ + void print(const std::string& s = "") const; + + /** equals with an tolerance */ + bool equals(const Point3& p, double tol = 1e-9) const; + + /// @} + /// @name Group + /// @{ + + /// identity for group operation + inline static Point3 identity() { return Point3();} + + /// @} + /// @name Vector Space + /// @{ + + /** distance between two points */ + double distance(const Point3& p2, OptionalJacobian<1, 3> H1 = boost::none, + OptionalJacobian<1, 3> H2 = boost::none) const; + + /** Distance of the point from the origin, with Jacobian */ + double norm(OptionalJacobian<1,3> H = boost::none) const; + + /** normalize, with optional Jacobian */ + Point3 normalize(OptionalJacobian<3, 3> H = boost::none) const; + + /** cross product @return this x q */ + Point3 cross(const Point3 &q, OptionalJacobian<3, 3> H_p = boost::none, // + OptionalJacobian<3, 3> H_q = boost::none) const; + + /** dot product @return this * q*/ + double dot(const Point3 &q, OptionalJacobian<1, 3> H_p = boost::none, // + OptionalJacobian<1, 3> H_q = boost::none) const; + + /// @} + /// @name Standard Interface + /// @{ + + /** return vectorized form (column-wise)*/ + const Vector3& vector() const { return *this; } + + /// @} + + /// Output stream operator + GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Point3& p); + + private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Vector3); + } + }; +#else +/** * A 3D point * @addtogroup geometry * \nosubgrouping */ - class GTSAM_EXPORT Point3 { +class GTSAM_EXPORT Point3 { private: @@ -181,13 +267,15 @@ namespace gtsam { /// @} }; +/// Syntactic sugar for multiplying coordinates by a scalar s*p +inline Point3 operator*(double s, const Point3& p) { return p*s;} + +#endif + // Convenience typedef typedef std::pair Point3Pair; std::ostream &operator<<(std::ostream &os, const gtsam::Point3Pair &p); -/// Syntactic sugar for multiplying coordinates by a scalar s*p -inline Point3 operator*(double s, const Point3& p) { return p*s;} - template<> struct traits : public internal::VectorSpace {}; diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 9954240fd..44eb5c226 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -297,7 +297,7 @@ Point3 Pose3::transform_to(const Point3& p, OptionalJacobian<3,6> Dpose, // Only get transpose once, to avoid multiple allocations, // as well as multiple conversions in the Quaternion case const Matrix3 Rt = R_.transpose(); - const Point3 q(Rt*(p - t_).vector()); + const Point3 q(Rt*(p.vector() - t_.vector())); if (Dpose) { const double wx = q.x(), wy = q.y(), wz = q.z(); (*Dpose) << diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index f82e25424..fa4b2e51a 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -65,10 +65,12 @@ public: R_(R), t_(t) { } - /** Construct from R,t, where t \in vector3 */ +#ifndef GTSAM_USE_VECTOR3_POINTS + /** Construct from R,t */ Pose3(const Rot3& R, const Vector3& t) : R_(R), t_(t) { } +#endif /** Construct from Pose2 */ explicit Pose3(const Pose2& pose2); diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 264be1537..b26475578 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -93,7 +93,7 @@ namespace gtsam { * See: http://stackoverflow.com/questions/27094132/cannot-understand-if-this-is-circular-dependency-or-clang#tab-top */ template - inline Rot3(const Eigen::MatrixBase& R) { + inline explicit Rot3(const Eigen::MatrixBase& R) { #ifdef GTSAM_USE_QUATERNIONS quaternion_=Matrix3(R); #else @@ -105,7 +105,7 @@ namespace gtsam { * Constructor from a rotation matrix * Overload version for Matrix3 to avoid casting in quaternion mode. */ - inline Rot3(const Matrix3& R) { + inline explicit Rot3(const Matrix3& R) { #ifdef GTSAM_USE_QUATERNIONS quaternion_=R; #else @@ -171,6 +171,7 @@ namespace gtsam { return Rot3(q); } +#ifndef GTSAM_USE_VECTOR3_POINTS /** * Convert from axis/angle representation * @param axis is the rotation axis, unit length @@ -181,9 +182,10 @@ namespace gtsam { #ifdef GTSAM_USE_QUATERNIONS return gtsam::Quaternion(Eigen::AngleAxis(angle, axis)); #else - return SO3::AxisAngle(axis,angle); + return Rot3(SO3::AxisAngle(axis,angle)); #endif } +#endif /** * Convert from axis/angle representation @@ -192,7 +194,11 @@ namespace gtsam { * @return incremental rotation */ static Rot3 AxisAngle(const Point3& axis, double angle) { - return AxisAngle(axis.vector(),angle); +#ifdef GTSAM_USE_QUATERNIONS + return gtsam::Quaternion(Eigen::AngleAxis(angle, axis.vector())); +#else + return Rot3(SO3::AxisAngle(axis.vector(),angle)); +#endif } /** @@ -315,7 +321,7 @@ namespace gtsam { #ifdef GTSAM_USE_QUATERNIONS return traits::Expmap(v); #else - return traits::Expmap(v); + return Rot3(traits::Expmap(v)); #endif } @@ -352,6 +358,14 @@ namespace gtsam { Point3 rotate(const Point3& p, OptionalJacobian<3,3> H1 = boost::none, OptionalJacobian<3,3> H2 = boost::none) const; + /// rotate point from rotated coordinate frame to world = R*p + Point3 operator*(const Point3& p) const; + + /// rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$ + Point3 unrotate(const Point3& p, OptionalJacobian<3,3> H1 = boost::none, + OptionalJacobian<3,3> H2=boost::none) const; + +#ifndef GTSAM_USE_VECTOR3_POINTS /// operator* for Vector3 inline Vector3 operator*(const Vector3& v) const { return rotate(Point3(v)).vector(); @@ -362,19 +376,12 @@ namespace gtsam { OptionalJacobian<3, 3> H2 = boost::none) const { return rotate(Point3(v), H1, H2).vector(); } - - /// rotate point from rotated coordinate frame to world = R*p - Point3 operator*(const Point3& p) const; - - /// rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$ - Point3 unrotate(const Point3& p, OptionalJacobian<3,3> H1 = boost::none, - OptionalJacobian<3,3> H2=boost::none) const; - /// unrotate for Vector3 Vector3 unrotate(const Vector3& v, OptionalJacobian<3, 3> H1 = boost::none, OptionalJacobian<3, 3> H2 = boost::none) const { return unrotate(Point3(v), H1, H2).vector(); } +#endif /// @} /// @name Group Action on Unit3 @@ -539,7 +546,7 @@ namespace gtsam { template<> struct traits : public internal::LieGroup {}; - + template<> struct traits : public internal::LieGroup {}; } diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index aaf0aa953..203a08fb3 100644 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -35,6 +35,7 @@ #include #include #include +#include using namespace std; @@ -252,11 +253,10 @@ Unit3 Unit3::retract(const Vector2& v) const { // Treat case of very small v differently if (theta < std::numeric_limits::epsilon()) { - return Unit3(cos(theta) * p_ + xi_hat); + return Unit3(Vector3(std::cos(theta) * p_ + xi_hat)); } - Vector3 exp_p_xi_hat = - cos(theta) * p_ + xi_hat * (sin(theta) / theta); + Vector3 exp_p_xi_hat = std::cos(theta) * p_ + xi_hat * (sin(theta) / theta); return Unit3(exp_p_xi_hat); } diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h index 428211c6b..1ce790a1b 100644 --- a/gtsam/geometry/Unit3.h +++ b/gtsam/geometry/Unit3.h @@ -65,10 +65,12 @@ public: p_(1.0, 0.0, 0.0) { } +#ifndef GTSAM_USE_VECTOR3_POINTS /// Construct from point explicit Unit3(const Point3& p) : p_(p.vector().normalized()) { } +#endif /// Construct from a vector3 explicit Unit3(const Vector3& p) : diff --git a/python/handwritten/geometry/Point3.cpp b/python/handwritten/geometry/Point3.cpp index 5a3ba8fb5..1c4e6f714 100644 --- a/python/handwritten/geometry/Point3.cpp +++ b/python/handwritten/geometry/Point3.cpp @@ -43,10 +43,14 @@ class_("Point3") .def("norm", &Point3::norm) .def("normalize", &Point3::normalize) .def("print", &Point3::print, print_overloads(args("s"))) +#ifndef GTSAM_USE_VECTOR3_POINTS .def("vector", &Point3::vector) .def("x", &Point3::x) .def("y", &Point3::y) .def("z", &Point3::z) +#else + .def("vector", &Point3::vector, return_value_policy()) +#endif .def(self * other()) .def(other() * self) .def(self + self) diff --git a/python/handwritten/geometry/Rot3.cpp b/python/handwritten/geometry/Rot3.cpp index 68643fe2c..f4b868b50 100644 --- a/python/handwritten/geometry/Rot3.cpp +++ b/python/handwritten/geometry/Rot3.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -37,8 +37,11 @@ static Rot3 Quaternion_1(double w, double x, double y, double z) // Prototypes used to perform overloading // See: http://www.boost.org/doc/libs/1_59_0/libs/python/doc/tutorial/doc/html/python/functions.html -gtsam::Rot3 (*AxisAngle_0)(const Vector3&, double) = &Rot3::AxisAngle; -gtsam::Rot3 (*AxisAngle_1)(const gtsam::Point3&, double) = &Rot3::AxisAngle; +gtsam::Rot3 (*AxisAngle_0)(const gtsam::Point3&, double) = &Rot3::AxisAngle; +gtsam::Rot3 (*AxisAngle_1)(const gtsam::Unit3&, double) = &Rot3::AxisAngle; +#ifndef GTSAM_USE_VECTOR3_POINTS +gtsam::Rot3 (*AxisAngle_2)(const Vector3&, double) = &Rot3::AxisAngle; +#endif gtsam::Rot3 (*Rodrigues_0)(const Vector3&) = &Rot3::Rodrigues; gtsam::Rot3 (*Rodrigues_1)(double, double, double) = &Rot3::Rodrigues; gtsam::Rot3 (*RzRyRx_0)(double, double, double) = &Rot3::RzRyRx; @@ -58,8 +61,6 @@ void exportRot3(){ .def("Quaternion", Quaternion_0, arg("q"), "Creates a Rot3 from an array [w,x,y,z] representing a quaternion") .def("Quaternion", Quaternion_1, (arg("w"),arg("x"),arg("y"),arg("z")) ) .staticmethod("Quaternion") - .def("AxisAngle", AxisAngle_0) - .def("AxisAngle", AxisAngle_1) .staticmethod("AxisAngle") .def("Expmap", &Rot3::Expmap) .staticmethod("Expmap") @@ -69,6 +70,12 @@ void exportRot3(){ .staticmethod("Logmap") .def("LogmapDerivative", &Rot3::LogmapDerivative) .staticmethod("LogmapDerivative") +#ifdef GTSAM_USE_VECTOR3_POINTS + .def("AxisAngle", AxisAngle_0) + .def("AxisAngle", AxisAngle_1) +#else + .def("AxisAngle", AxisAngle_2) +#endif .def("Rodrigues", Rodrigues_0) .def("Rodrigues", Rodrigues_1) .staticmethod("Rodrigues") From 6eeeb3fef1bd389af8c8913ae4bd6e7e9a005c03 Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 8 Feb 2016 18:58:43 -0800 Subject: [PATCH 07/90] Fixed forgotten compose --- gtsam_unstable/timing/timeInertialNavFactor_GlobalVelocity.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam_unstable/timing/timeInertialNavFactor_GlobalVelocity.cpp b/gtsam_unstable/timing/timeInertialNavFactor_GlobalVelocity.cpp index 39481f929..001d75eb2 100644 --- a/gtsam_unstable/timing/timeInertialNavFactor_GlobalVelocity.cpp +++ b/gtsam_unstable/timing/timeInertialNavFactor_GlobalVelocity.cpp @@ -75,7 +75,7 @@ int main() { Rot3 R2(0.473618898, 0.119523052, 0.872582019, 0.609241153, 0.67099888, -0.422594037, -0.636011287, 0.731761397, 0.244979388); - Point3 t2 = t1.compose( Point3(Vel1*measurement_dt) ); + Point3 t2 = t1 + Point3(Vel1*measurement_dt); Pose3 Pose2(R2, t2); Vector dv = measurement_dt * (R1.matrix() * measurement_acc + world_g); Vector3 Vel2 = Vel1 + dv; From 50e8fa9b5468ba1cf792b0b5a1063fa1be73314f Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 8 Feb 2016 18:58:57 -0800 Subject: [PATCH 08/90] Fix timing script --- timing/timeSFMBAL.h | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/timing/timeSFMBAL.h b/timing/timeSFMBAL.h index e9449af7b..cd859326c 100644 --- a/timing/timeSFMBAL.h +++ b/timing/timeSFMBAL.h @@ -49,8 +49,12 @@ SfM_data preamble(int argc, char* argv[]) { // Load BAL file SfM_data db; - string defaultFilename = findExampleDataFile("dubrovnik-16-22106-pre"); - bool success = readBAL(argc > 1 ? argv[argc - 1] : defaultFilename, db); + string filename; + if (argc > 1) + filename = argv[argc - 1]; + else + filename = findExampleDataFile("dubrovnik-16-22106-pre"); + bool success = readBAL(argv[argc - 1], db); if (!success) throw runtime_error("Could not access file!"); return db; } From 760a375a32326dff80eed084668e366b4955eb06 Mon Sep 17 00:00:00 2001 From: Paul Date: Tue, 9 Feb 2016 10:30:05 -0500 Subject: [PATCH 09/90] External define for depricated functions --- CMakeLists.txt | 4 ---- gtsam/config.h.in | 3 ++- 2 files changed, 2 insertions(+), 5 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 75c24f76d..3123dbea2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -322,10 +322,6 @@ if(GTSAM_ENABLE_CONSISTENCY_CHECKS) add_definitions(-DGTSAM_EXTRA_CONSISTENCY_CHECKS) endif() -if(GTSAM_ALLOW_DEPRECATED_SINCE_V4) - add_definitions(-DGTSAM_ALLOW_DEPRECATED_SINCE_V4) -endif() - ############################################################################### # Add components diff --git a/gtsam/config.h.in b/gtsam/config.h.in index 20073152e..a408b254c 100644 --- a/gtsam/config.h.in +++ b/gtsam/config.h.in @@ -60,4 +60,5 @@ // Option for not throwing the CheiralityException for points that are behind a camera #cmakedefine GTSAM_THROW_CHEIRALITY_EXCEPTION - +// Make sure dependent projects that want it can see deprecated functions +#cmakedefine GTSAM_ALLOW_DEPRECATED_SINCE_V4 From dcbba523f216e6685adefbcbf2ab8f7c0a0729a1 Mon Sep 17 00:00:00 2001 From: Kartik Mohta Date: Tue, 9 Feb 2016 21:39:20 +0000 Subject: [PATCH 10/90] Use system Eigen if version >= 3.2.5 since that includes our patches --- CMakeLists.txt | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 3123dbea2..07d8d49de 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -215,6 +215,15 @@ endif() ### http://eigen.tuxfamily.org/bz/show_bug.cgi?id=705 (Fix MKL LLT return code) option(GTSAM_USE_SYSTEM_EIGEN "Find and use system-installed Eigen. If 'off', use the one bundled with GTSAM" OFF) +# Use system Eigen if version >= 3.2.5 since that includes our patches +find_package(Eigen3 QUIET) +if(EIGEN3_FOUND AND (${EIGEN3_VERSION} VERSION_GREATER "3.2.4")) + set(GTSAM_USE_SYSTEM_EIGEN ON) + set(GTSAM_SYSTEM_EIGEN_MKL_ERROR OFF) +else() + set(GTSAM_SYSTEM_EIGEN_MKL_ERROR ON) +endif() + # Switch for using system Eigen or GTSAM-bundled Eigen if(GTSAM_USE_SYSTEM_EIGEN) find_package(Eigen3 REQUIRED) @@ -224,7 +233,7 @@ if(GTSAM_USE_SYSTEM_EIGEN) set(GTSAM_EIGEN_INCLUDE_PREFIX "${EIGEN3_INCLUDE_DIR}") # check if MKL is also enabled - can have one or the other, but not both! - if(EIGEN_USE_MKL_ALL) + if(EIGEN_USE_MKL_ALL AND GTSAM_SYSTEM_EIGEN_MKL_ERROR) message(FATAL_ERROR "MKL cannot be used together with system-installed Eigen, as MKL support relies on patches which are not yet in the system-installed Eigen. Disable either GTSAM_USE_SYSTEM_EIGEN or GTSAM_WITH_EIGEN_MKL") endif() else() From cc0b7cfdc1ccd2fc9a6195f01b6a07ae691d6c50 Mon Sep 17 00:00:00 2001 From: Chris Beall Date: Tue, 9 Feb 2016 20:00:38 -0500 Subject: [PATCH 11/90] convert tabs to spaces. See https://bitbucket.org/gtborg/gtsam/wiki/C++%20Coding%20Conventions --- examples/RangeISAMExample_plaza2.cpp | 2 +- gtsam/base/Matrix.cpp | 2 +- .../inference/EliminateableFactorGraph-inst.h | 18 +++---- gtsam/inference/EliminateableFactorGraph.h | 18 +++---- gtsam/linear/JacobianFactor.cpp | 6 +-- gtsam/nonlinear/NonlinearOptimizer.cpp | 4 +- gtsam/nonlinear/NonlinearOptimizerParams.cpp | 54 ++++++++++--------- gtsam/nonlinear/NonlinearOptimizerParams.h | 6 +-- 8 files changed, 56 insertions(+), 54 deletions(-) diff --git a/examples/RangeISAMExample_plaza2.cpp b/examples/RangeISAMExample_plaza2.cpp index 4d116c7ec..01ce8b08b 100644 --- a/examples/RangeISAMExample_plaza2.cpp +++ b/examples/RangeISAMExample_plaza2.cpp @@ -83,7 +83,7 @@ vector readTriples() { while (is) { double t, sender, range; - size_t receiver; + size_t receiver; is >> t >> sender >> receiver >> range; triples.push_back(RangeTriple(t, receiver, range)); } diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp index c6af89486..740699d4b 100644 --- a/gtsam/base/Matrix.cpp +++ b/gtsam/base/Matrix.cpp @@ -189,7 +189,7 @@ void print(const Matrix& A, const string &s, ostream& stream) { 0, // flags " ", // coeffSeparator ";\n", // rowSeparator - " \t", // rowPrefix + " \t", // rowPrefix "", // rowSuffix "[\n", // matPrefix "\n ]" // matSuffix diff --git a/gtsam/inference/EliminateableFactorGraph-inst.h b/gtsam/inference/EliminateableFactorGraph-inst.h index b4fc3b3a6..98a3545f6 100644 --- a/gtsam/inference/EliminateableFactorGraph-inst.h +++ b/gtsam/inference/EliminateableFactorGraph-inst.h @@ -28,8 +28,8 @@ namespace gtsam { template boost::shared_ptr::BayesNetType> EliminateableFactorGraph::eliminateSequential( - OptionalOrdering ordering, const Eliminate& function, - OptionalVariableIndex variableIndex, OptionalOrderingType orderingType) const + OptionalOrdering ordering, const Eliminate& function, + OptionalVariableIndex variableIndex, OptionalOrderingType orderingType) const { if(ordering && variableIndex) { gttic(eliminateSequential); @@ -65,8 +65,8 @@ namespace gtsam { template boost::shared_ptr::BayesTreeType> EliminateableFactorGraph::eliminateMultifrontal( - OptionalOrdering ordering, const Eliminate& function, - OptionalVariableIndex variableIndex, OptionalOrderingType orderingType) const + OptionalOrdering ordering, const Eliminate& function, + OptionalVariableIndex variableIndex, OptionalOrderingType orderingType) const { if(ordering && variableIndex) { gttic(eliminateMultifrontal); @@ -86,16 +86,16 @@ namespace gtsam { // If no VariableIndex provided, compute one and call this function again IMPORTANT: we check // for no variable index first so that it's always computed if we need to call COLAMD because // no Ordering is provided. - return eliminateMultifrontal(ordering, function, VariableIndex(asDerived()), orderingType); + return eliminateMultifrontal(ordering, function, VariableIndex(asDerived()), orderingType); } else /*if(!ordering)*/ { // If no Ordering provided, compute one and call this function again. We are guaranteed to // have a VariableIndex already here because we computed one if needed in the previous 'else' // block. - if (orderingType == Ordering::METIS) - return eliminateMultifrontal(Ordering::Metis(asDerived()), function, variableIndex, orderingType); - else - return eliminateMultifrontal(Ordering::Colamd(*variableIndex), function, variableIndex, orderingType); + if (orderingType == Ordering::METIS) + return eliminateMultifrontal(Ordering::Metis(asDerived()), function, variableIndex, orderingType); + else + return eliminateMultifrontal(Ordering::Colamd(*variableIndex), function, variableIndex, orderingType); } } diff --git a/gtsam/inference/EliminateableFactorGraph.h b/gtsam/inference/EliminateableFactorGraph.h index f5431db3d..891f22e61 100644 --- a/gtsam/inference/EliminateableFactorGraph.h +++ b/gtsam/inference/EliminateableFactorGraph.h @@ -94,8 +94,8 @@ namespace gtsam { /// Typedef for an optional variable index as an argument to elimination functions typedef boost::optional OptionalVariableIndex; - /// Typedef for an optional ordering type - typedef boost::optional OptionalOrderingType; + /// Typedef for an optional ordering type + typedef boost::optional OptionalOrderingType; /** Do sequential elimination of all variables to produce a Bayes net. If an ordering is not * provided, the ordering provided by COLAMD will be used. @@ -104,10 +104,10 @@ namespace gtsam { * \code * boost::shared_ptr result = graph.eliminateSequential(EliminateCholesky); * \endcode - * - * Example - METIS ordering for elimination - * \code - * boost::shared_ptr result = graph.eliminateSequential(OrderingType::METIS); + * + * Example - METIS ordering for elimination + * \code + * boost::shared_ptr result = graph.eliminateSequential(OrderingType::METIS); * * Example - Full QR elimination in specified order: * \code @@ -125,7 +125,7 @@ namespace gtsam { OptionalOrdering ordering = boost::none, const Eliminate& function = EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex = boost::none, - OptionalOrderingType orderingType = boost::none) const; + OptionalOrderingType orderingType = boost::none) const; /** Do multifrontal elimination of all variables to produce a Bayes tree. If an ordering is not * provided, the ordering will be computed using either COLAMD or METIS, dependeing on @@ -151,8 +151,8 @@ namespace gtsam { boost::shared_ptr eliminateMultifrontal( OptionalOrdering ordering = boost::none, const Eliminate& function = EliminationTraitsType::DefaultEliminate, - OptionalVariableIndex variableIndex = boost::none, - OptionalOrderingType orderingType = boost::none) const; + OptionalVariableIndex variableIndex = boost::none, + OptionalOrderingType orderingType = boost::none) const; /** Do sequential elimination of some variables, in \c ordering provided, to produce a Bayes net * and a remaining factor graph. This computes the factorization \f$ p(X) = p(A|B) p(B) \f$, diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index d4df57298..47b6ec90b 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -349,11 +349,11 @@ void JacobianFactor::print(const string& s, const KeyFormatter& formatter) const { static const Eigen::IOFormat matlab( Eigen::StreamPrecision, // precision - 0, // flags + 0, // flags " ", // coeffSeparator ";\n", // rowSeparator - "\t", // rowPrefix - "", // rowSuffix + "\t", // rowPrefix + "", // rowSuffix "[\n", // matPrefix "\n ]" // matSuffix ); diff --git a/gtsam/nonlinear/NonlinearOptimizer.cpp b/gtsam/nonlinear/NonlinearOptimizer.cpp index 77d26d361..2c752815e 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearOptimizer.cpp @@ -110,8 +110,8 @@ VectorValues NonlinearOptimizer::solve(const GaussianFactorGraph &gfg, delta = gfg.optimize(*params.ordering, params.getEliminationFunction()); } else if (params.isSequential()) { // Sequential QR or Cholesky (decided by params.getEliminationFunction()) - delta = gfg.eliminateSequential(*params.ordering, params.getEliminationFunction(), - boost::none, params.orderingType)->optimize(); + delta = gfg.eliminateSequential(*params.ordering, + params.getEliminationFunction(), boost::none, params.orderingType)->optimize(); } else if (params.isIterative()) { // Conjugate Gradient -> needs params.iterativeParams diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.cpp b/gtsam/nonlinear/NonlinearOptimizerParams.cpp index 5a163ffb9..91edd8f93 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.cpp +++ b/gtsam/nonlinear/NonlinearOptimizerParams.cpp @@ -110,14 +110,14 @@ void NonlinearOptimizerParams::print(const std::string& str) const { switch (orderingType){ case Ordering::COLAMD: - std::cout << " ordering: COLAMD\n"; - break; + std::cout << " ordering: COLAMD\n"; + break; case Ordering::METIS: - std::cout << " ordering: METIS\n"; - break; + std::cout << " ordering: METIS\n"; + break; default: - std::cout << " ordering: custom\n"; - break; + std::cout << " ordering: custom\n"; + break; } std::cout.flush(); @@ -165,29 +165,31 @@ NonlinearOptimizerParams::LinearSolverType NonlinearOptimizerParams::linearSolve } /* ************************************************************************* */ -std::string NonlinearOptimizerParams::orderingTypeTranslator(Ordering::OrderingType type) const{ - switch (type) { - case Ordering::METIS: - return "METIS"; - case Ordering::COLAMD: - return "COLAMD"; - default: - if (ordering) - return "CUSTOM"; - else - throw std::invalid_argument( - "Invalid ordering type: You must provide an ordering for a custom ordering type. See setOrdering"); - } +std::string NonlinearOptimizerParams::orderingTypeTranslator( + Ordering::OrderingType type) const { + switch (type) { + case Ordering::METIS: + return "METIS"; + case Ordering::COLAMD: + return "COLAMD"; + default: + if (ordering) + return "CUSTOM"; + else + throw std::invalid_argument( + "Invalid ordering type: You must provide an ordering for a custom ordering type. See setOrdering"); + } } /* ************************************************************************* */ -Ordering::OrderingType NonlinearOptimizerParams::orderingTypeTranslator(const std::string& type) const{ - if (type == "METIS") - return Ordering::METIS; - if (type == "COLAMD") - return Ordering::COLAMD; - throw std::invalid_argument( - "Invalid ordering type: You must provide an ordering for a custom ordering type. See setOrdering"); +Ordering::OrderingType NonlinearOptimizerParams::orderingTypeTranslator( + const std::string& type) const { + if (type == "METIS") + return Ordering::METIS; + if (type == "COLAMD") + return Ordering::COLAMD; + throw std::invalid_argument( + "Invalid ordering type: You must provide an ordering for a custom ordering type. See setOrdering"); } diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.h b/gtsam/nonlinear/NonlinearOptimizerParams.h index 10de6994f..ca75bb02a 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.h +++ b/gtsam/nonlinear/NonlinearOptimizerParams.h @@ -154,16 +154,16 @@ public: void setOrdering(const Ordering& ordering) { this->ordering = ordering; - this->orderingType = Ordering::CUSTOM; + this->orderingType = Ordering::CUSTOM; } std::string getOrderingType() const { - return orderingTypeTranslator(orderingType); + return orderingTypeTranslator(orderingType); } // Note that if you want to use a custom ordering, you must set the ordering directly, this will switch to custom type void setOrderingType(const std::string& ordering){ - orderingType = orderingTypeTranslator(ordering); + orderingType = orderingTypeTranslator(ordering); } private: From ae58516e23cdd1d896e38d1196bb33f20e3e6067 Mon Sep 17 00:00:00 2001 From: Frank Date: Tue, 9 Feb 2016 17:27:50 -0800 Subject: [PATCH 12/90] Fix some more timing scripts --- timing/timeCalibratedCamera.cpp | 9 ++------- timing/timePinholeCamera.cpp | 9 ++------- timing/timeStereoCamera.cpp | 9 ++------- 3 files changed, 6 insertions(+), 21 deletions(-) diff --git a/timing/timeCalibratedCamera.cpp b/timing/timeCalibratedCamera.cpp index 2d0576aea..088ec7b1f 100644 --- a/timing/timeCalibratedCamera.cpp +++ b/timing/timeCalibratedCamera.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -27,12 +27,7 @@ int main() { int n = 100000; - const Pose3 pose1(Matrix3((Matrix(3,3) << - 1., 0., 0., - 0.,-1., 0., - 0., 0.,-1. - ).finished()), - Point3(0,0,0.5)); + const Pose3 pose1(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5)); const CalibratedCamera camera(pose1); const Point3 point1(-0.08,-0.08, 0.0); diff --git a/timing/timePinholeCamera.cpp b/timing/timePinholeCamera.cpp index 6f7e5e972..458f88db1 100644 --- a/timing/timePinholeCamera.cpp +++ b/timing/timePinholeCamera.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -28,12 +28,7 @@ int main() { int n = 1e6; - const Pose3 pose1((Matrix)(Matrix(3,3) << - 1., 0., 0., - 0.,-1., 0., - 0., 0.,-1. - ).finished(), - Point3(0,0,0.5)); + const Pose3 pose1(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5)); static Cal3Bundler K(500, 1e-3, 2.0*1e-3); const PinholeCamera camera(pose1,K); diff --git a/timing/timeStereoCamera.cpp b/timing/timeStereoCamera.cpp index ab8e2fff9..c609f8885 100644 --- a/timing/timeStereoCamera.cpp +++ b/timing/timeStereoCamera.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -27,12 +27,7 @@ int main() { int n = 100000; - const Pose3 pose1((Matrix)(Matrix(3,3) << - 1., 0., 0., - 0.,-1., 0., - 0., 0.,-1. - ).finished(), - Point3(0,0,0.5)); + const Pose3 pose1(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5)); const Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5)); const StereoCamera camera(pose1, K); From 90e7a9a19440dfecfbd326979953d4c7a977a2f2 Mon Sep 17 00:00:00 2001 From: Frank Date: Tue, 9 Feb 2016 18:01:47 -0800 Subject: [PATCH 13/90] Made all methods with derivatives available as free functions --- gtsam/geometry/Point3.cpp | 108 +++++++++++++------------ gtsam/geometry/Point3.h | 34 +++++++- gtsam/geometry/Unit3.cpp | 4 +- gtsam/geometry/tests/testPoint3.cpp | 4 +- python/handwritten/geometry/Point3.cpp | 2 +- 5 files changed, 92 insertions(+), 60 deletions(-) diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp index 29d59d9e3..8b44583a6 100644 --- a/gtsam/geometry/Point3.cpp +++ b/gtsam/geometry/Point3.cpp @@ -33,45 +33,54 @@ void Point3::print(const string& s) const { } /* ************************************************************************* */ - bool Point3::operator==(const Point3& q) const { return x_ == q.x_ && y_ == q.y_ && z_ == q.z_; } -/* ************************************************************************* */ Point3 Point3::operator+(const Point3& q) const { return Point3(x_ + q.x_, y_ + q.y_, z_ + q.z_); } -/* ************************************************************************* */ Point3 Point3::operator-(const Point3& q) const { return Point3(x_ - q.x_, y_ - q.y_, z_ - q.z_); } -/* ************************************************************************* */ Point3 Point3::operator*(double s) const { return Point3(x_ * s, y_ * s, z_ * s); } -/* ************************************************************************* */ Point3 Point3::operator/(double s) const { return Point3(x_ / s, y_ / s, z_ / s); } /* ************************************************************************* */ -double Point3::distance(const Point3 &p2, OptionalJacobian<1, 3> H1, +double Point3::distance(const Point3 &q, OptionalJacobian<1, 3> H1, OptionalJacobian<1, 3> H2) const { - double d = (p2 - *this).norm(); - if (H1) { - *H1 << x_ - p2.x(), y_ - p2.y(), z_ - p2.z(); - *H1 = *H1 *(1. / d); - } + return gtsam::distance(*this,q,H1,H2); +} - if (H2) { - *H2 << -x_ + p2.x(), -y_ + p2.y(), -z_ + p2.z(); - *H2 = *H2 *(1. / d); - } - return d; +double Point3::norm(OptionalJacobian<1,3> H) const { + return gtsam::norm(*this, H); +} + +Point3 Point3::normalized(OptionalJacobian<3,3> H) const { + return gtsam::normalize(*this, H); +} + +Point3 Point3::cross(const Point3 &q, OptionalJacobian<3, 3> H1, + OptionalJacobian<3, 3> H2) const { + return gtsam::cross(*this, q, H1, H2); +} + +double Point3::dot(const Point3 &q, OptionalJacobian<1, 3> H1, + OptionalJacobian<1, 3> H2) const { + return gtsam::dot(*this, q, H1, H2); +} + +/* ************************************************************************* */ +ostream &operator<<(ostream &os, const Point3& p) { + os << '[' << p.x() << ", " << p.y() << ", " << p.z() << "]\';"; + return os; } /* ************************************************************************* */ @@ -92,59 +101,56 @@ Point3 Point3::sub(const Point3 &q, OptionalJacobian<3,3> H1, #endif /* ************************************************************************* */ -Point3 Point3::cross(const Point3 &q, OptionalJacobian<3, 3> H_p, OptionalJacobian<3, 3> H_q) const { - if (H_p) { - *H_p << skewSymmetric(-q.vector()); +double distance(const Point3 &p1, const Point3 &q, OptionalJacobian<1, 3> H1, + OptionalJacobian<1, 3> H2) { + double d = (q - p1).norm(); + if (H1) { + *H1 << p1.x() - q.x(), p1.y() - q.y(), p1.z() - q.z(); + *H1 = *H1 *(1. / d); } - if (H_q) { - *H_q << skewSymmetric(vector()); + if (H2) { + *H2 << -p1.x() + q.x(), -p1.y() + q.y(), -p1.z() + q.z(); + *H2 = *H2 *(1. / d); } - - return Point3(y_ * q.z_ - z_ * q.y_, z_ * q.x_ - x_ * q.z_, - x_ * q.y_ - y_ * q.x_); + return d; } -/* ************************************************************************* */ -double Point3::dot(const Point3 &q, OptionalJacobian<1, 3> H_p, OptionalJacobian<1, 3> H_q) const { - if (H_p) { - *H_p << q.vector().transpose(); - } - if (H_q) { - *H_q << vector().transpose(); - } - - return (x_ * q.x_ + y_ * q.y_ + z_ * q.z_); -} - -/* ************************************************************************* */ -double Point3::norm(OptionalJacobian<1,3> H) const { - double r = sqrt(x_ * x_ + y_ * y_ + z_ * z_); +double norm(const Point3 &p, OptionalJacobian<1, 3> H) { + double r = sqrt(p.x() * p.x() + p.y() * p.y() + p.z() * p.z()); if (H) { if (fabs(r) > 1e-10) - *H << x_ / r, y_ / r, z_ / r; + *H << p.x() / r, p.y() / r, p.z() / r; else - *H << 1, 1, 1; // really infinity, why 1 ? + *H << 1, 1, 1; // really infinity, why 1 ? } return r; } -/* ************************************************************************* */ -Point3 Point3::normalize(OptionalJacobian<3,3> H) const { - Point3 normalized = *this / norm(); +Point3 normalize(const Point3 &p, OptionalJacobian<3, 3> H) { + Point3 normalized = p / norm(p); if (H) { // 3*3 Derivative - double x2 = x_ * x_, y2 = y_ * y_, z2 = z_ * z_; - double xy = x_ * y_, xz = x_ * z_, yz = y_ * z_; - *H << y2 + z2, -xy, -xz, /**/-xy, x2 + z2, -yz, /**/-xz, -yz, x2 + y2; + double x2 = p.x() * p.x(), y2 = p.y() * p.y(), z2 = p.z() * p.z(); + double xy = p.x() * p.y(), xz = p.x() * p.z(), yz = p.y() * p.z(); + *H << y2 + z2, -xy, -xz, /**/ -xy, x2 + z2, -yz, /**/ -xz, -yz, x2 + y2; *H /= pow(x2 + y2 + z2, 1.5); } return normalized; } -/* ************************************************************************* */ -ostream &operator<<(ostream &os, const Point3& p) { - os << '[' << p.x() << ", " << p.y() << ", " << p.z() << "]\';"; - return os; +Point3 cross(const Point3 &p, const Point3 &q, OptionalJacobian<3, 3> H1, + OptionalJacobian<3, 3> H2) { + if (H1) *H1 << skewSymmetric(-q.x(), -q.y(), -q.z()); + if (H2) *H2 << skewSymmetric(p.x(), p.y(), p.z()); + return Point3(p.y() * q.z() - p.z() * q.y(), p.z() * q.x() - p.x() * q.z(), + p.x() * q.y() - p.y() * q.x()); +} + +double dot(const Point3 &p, const Point3 &q, OptionalJacobian<1, 3> H1, + OptionalJacobian<1, 3> H2) { + if (H1) *H1 << q.x(), q.y(), q.z(); + if (H2) *H2 << p.x(), p.y(), p.z(); + return p.x() * q.x() + p.y() * q.y() + p.z() * q.z(); } /* ************************************************************************* */ diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 978d7b963..90bfb7cea 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -104,14 +104,14 @@ namespace gtsam { Point3 operator / (double s) const; /** distance between two points */ - double distance(const Point3& p2, OptionalJacobian<1, 3> H1 = boost::none, + double distance(const Point3& q, OptionalJacobian<1, 3> H1 = boost::none, OptionalJacobian<1, 3> H2 = boost::none) const; /** Distance of the point from the origin, with Jacobian */ double norm(OptionalJacobian<1,3> H = boost::none) const; /** normalize, with optional Jacobian */ - Point3 normalize(OptionalJacobian<3, 3> H = boost::none) const; + Point3 normalized(OptionalJacobian<3, 3> H = boost::none) const; /** cross product @return this x q */ Point3 cross(const Point3 &q, OptionalJacobian<3, 3> H_p = boost::none, // @@ -155,7 +155,8 @@ namespace gtsam { Point3 retract(const Vector3& v) const { return compose(Point3(v));} static Vector3 Logmap(const Point3& p) { return p.vector();} static Point3 Expmap(const Vector3& v) { return Point3(v);} - inline double dist(const Point3& p2) const { return (p2 - *this).norm(); } + inline double dist(const Point3& q) const { return (q - *this).norm(); } + Point3 normalize(OptionalJacobian<3, 3> H = boost::none) const { return normalized(H);} Point3 add(const Point3& q, OptionalJacobian<3, 3> H1 = boost::none, OptionalJacobian<3, 3> H2 = boost::none) const; Point3 sub(const Point3& q, OptionalJacobian<3, 3> H1 = boost::none, @@ -194,6 +195,31 @@ struct traits : public internal::VectorSpace {}; template<> struct traits : public internal::VectorSpace {}; +/// distance between two points +double distance(const Point3& p1, const Point3& q, + OptionalJacobian<1, 3> H1 = boost::none, + OptionalJacobian<1, 3> H2 = boost::none); + +/// Distance of the point from the origin, with Jacobian +double norm(const Point3& p, OptionalJacobian<1, 3> H = boost::none); + +/// normalize, with optional Jacobian +Point3 normalize(const Point3& p, OptionalJacobian<3, 3> H = boost::none); + +/// cross product @return this x q +Point3 cross(const Point3& p, const Point3& q, + OptionalJacobian<3, 3> H_p = boost::none, + OptionalJacobian<3, 3> H_q = boost::none); + +/// dot product +double dot(const Point3& p, const Point3& q, + OptionalJacobian<1, 3> H_p = boost::none, + OptionalJacobian<1, 3> H_q = boost::none); + +// Convenience typedef +typedef std::pair Point3Pair; +std::ostream &operator<<(std::ostream &os, const gtsam::Point3Pair &p); + template struct Range; @@ -203,7 +229,7 @@ struct Range { double operator()(const Point3& p, const Point3& q, OptionalJacobian<1, 3> H1 = boost::none, OptionalJacobian<1, 3> H2 = boost::none) { - return p.distance(q, H1, H2); + return distance(p, q, H1, H2); } }; diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index aaf0aa953..d80fe2915 100644 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -44,7 +44,7 @@ namespace gtsam { Unit3 Unit3::FromPoint3(const Point3& point, OptionalJacobian<2,3> H) { // 3*3 Derivative of representation with respect to point is 3*3: Matrix3 D_p_point; - Point3 normalized = point.normalize(H ? &D_p_point : 0); + Point3 normalized = normalize(point, H ? &D_p_point : 0); Unit3 direction; direction.p_ = normalized.vector(); if (H) @@ -108,7 +108,7 @@ const Matrix32& Unit3::basis(OptionalJacobian<6, 2> H) const { // Normalize result to get a unit vector: b1 = B1 / |B1|. Matrix33 H_b1_B1; - Point3 b1 = B1.normalize(H ? &H_b1_B1 : 0); + Point3 b1 = normalize(B1, H ? &H_b1_B1 : 0); // Get the second basis vector b2, which is orthogonal to n and b1, by crossing them. // No need to normalize this, p and b1 are orthogonal unit vectors. diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp index d0123af83..37ad105c7 100644 --- a/gtsam/geometry/tests/testPoint3.cpp +++ b/gtsam/geometry/tests/testPoint3.cpp @@ -96,9 +96,9 @@ TEST (Point3, normalize) { Matrix actualH; Point3 point(1, -2, 3); // arbitrary point Point3 expected(point / sqrt(14.0)); - EXPECT(assert_equal(expected, point.normalize(actualH), 1e-8)); + EXPECT(assert_equal(expected, normalize(point, actualH), 1e-8)); Matrix expectedH = numericalDerivative11( - boost::bind(&Point3::normalize, _1, boost::none), point); + boost::bind(gtsam::normalize, _1, boost::none), point); EXPECT(assert_equal(expectedH, actualH, 1e-8)); } diff --git a/python/handwritten/geometry/Point3.cpp b/python/handwritten/geometry/Point3.cpp index 5a3ba8fb5..19f5a4602 100644 --- a/python/handwritten/geometry/Point3.cpp +++ b/python/handwritten/geometry/Point3.cpp @@ -41,7 +41,7 @@ class_("Point3") .def("dot", &Point3::dot) .def("equals", &Point3::equals, equals_overloads(args("q","tol"))) .def("norm", &Point3::norm) - .def("normalize", &Point3::normalize) + .def("normalized", &Point3::normalized) .def("print", &Point3::print, print_overloads(args("s"))) .def("vector", &Point3::vector) .def("x", &Point3::x) From 9a7dba75c49cd79c1432b6d3380c1e0f6cb0eb0f Mon Sep 17 00:00:00 2001 From: Kartik Mohta Date: Tue, 9 Feb 2016 22:11:28 -0500 Subject: [PATCH 14/90] CMakeLists.txt: Fix eigen version check logic --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 07d8d49de..d63bf5c43 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -217,7 +217,7 @@ option(GTSAM_USE_SYSTEM_EIGEN "Find and use system-installed Eigen. If 'off', us # Use system Eigen if version >= 3.2.5 since that includes our patches find_package(Eigen3 QUIET) -if(EIGEN3_FOUND AND (${EIGEN3_VERSION} VERSION_GREATER "3.2.4")) +if(EIGEN3_FOUND AND (EIGEN3_VERSION VERSION_GREATER 3.2.4)) set(GTSAM_USE_SYSTEM_EIGEN ON) set(GTSAM_SYSTEM_EIGEN_MKL_ERROR OFF) else() From 659ffeda02b9c40431b8c9fedf3569c3a4c67c8b Mon Sep 17 00:00:00 2001 From: Kartik Mohta Date: Tue, 9 Feb 2016 22:12:01 -0500 Subject: [PATCH 15/90] Replace GTSAM_USE_SYSTEM_EIGEN check for householder_qr_inplace_blocked The check was based on whether we are using the included version of Eigen or the system version, but since Eigen v3.2.5, our patches are included in the upstream Eigen, so check for Eigen version instead. --- gtsam/base/Matrix.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp index c6af89486..eae3dc48b 100644 --- a/gtsam/base/Matrix.cpp +++ b/gtsam/base/Matrix.cpp @@ -704,11 +704,9 @@ void inplace_QR(Matrix& A){ HCoeffsType hCoeffs(size); RowVectorType temp(cols); -#ifdef GTSAM_USE_SYSTEM_EIGEN - // System-Eigen is used, and MKL is off +#if !EIGEN_VERSION_AT_LEAST(3,2,5) Eigen::internal::householder_qr_inplace_blocked(A, hCoeffs, 48, temp.data()); #else - // Patched Eigen is used, and MKL is either on or off Eigen::internal::householder_qr_inplace_blocked::run(A, hCoeffs, 48, temp.data()); #endif From 0a7fd27f28bc155c198618ea3ebd00067e451b11 Mon Sep 17 00:00:00 2001 From: Frank Date: Wed, 10 Feb 2016 17:48:52 -0800 Subject: [PATCH 16/90] Working on getting a simple typedef to compile - as well as dealing with Point3() now creating uninitialized memory. --- gtsam/base/serializationTestHelpers.h | 27 +-- gtsam/geometry/EssentialMatrix.cpp | 2 +- gtsam/geometry/OrientedPlane3.cpp | 4 +- gtsam/geometry/Point3.cpp | 27 +-- gtsam/geometry/Point3.h | 173 +++--------------- gtsam/geometry/Pose3.cpp | 52 +++--- gtsam/geometry/Pose3.h | 10 +- gtsam/geometry/Rot3.cpp | 4 +- gtsam/geometry/Rot3.h | 36 +--- gtsam/geometry/Rot3M.cpp | 10 +- gtsam/geometry/Unit3.cpp | 21 +-- gtsam/geometry/tests/testPinholeCamera.cpp | 12 +- gtsam/geometry/tests/testPinholePose.cpp | 13 +- gtsam/geometry/tests/testPoint3.cpp | 18 +- gtsam/geometry/tests/testPose3.cpp | 14 +- gtsam/geometry/tests/testRot3.cpp | 4 +- gtsam/geometry/tests/testSimpleCamera.cpp | 10 +- gtsam/geometry/tests/testStereoCamera.cpp | 2 +- gtsam/geometry/tests/testUnit3.cpp | 2 +- gtsam/geometry/triangulation.h | 2 +- gtsam/navigation/GPSFactor.cpp | 8 +- gtsam/navigation/GPSFactor.h | 5 +- gtsam/navigation/NavState.cpp | 13 +- gtsam/navigation/NavState.h | 7 +- gtsam/navigation/PreintegrationBase.cpp | 4 +- gtsam/navigation/Scenario.h | 2 +- gtsam/nonlinear/NonlinearFactorGraph.cpp | 6 +- gtsam/slam/InitializePose3.cpp | 2 +- gtsam/slam/dataset.cpp | 6 +- gtsam/slam/dataset.h | 3 +- gtsam_unstable/geometry/Event.h | 2 +- gtsam_unstable/geometry/Similarity3.cpp | 4 +- .../geometry/tests/testSimilarity3.cpp | 6 +- 33 files changed, 155 insertions(+), 356 deletions(-) diff --git a/gtsam/base/serializationTestHelpers.h b/gtsam/base/serializationTestHelpers.h index f408427d8..c55e5067b 100644 --- a/gtsam/base/serializationTestHelpers.h +++ b/gtsam/base/serializationTestHelpers.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -11,7 +11,7 @@ /** * @file serializationTestHelpers.h - * @brief + * @brief * @author Alex Cunningham * @author Richard Roberts * @date Feb 7, 2012 @@ -30,6 +30,11 @@ const bool verbose = false; namespace gtsam { namespace serializationTestHelpers { +// templated default object creation so we only need to declare one friend (if applicable) +template +T create() { + return T(); +} // Templated round-trip serialization template @@ -44,7 +49,7 @@ void roundtrip(const T& input, T& output) { // This version requires equality operator template bool equality(const T& input = T()) { - T output; + T output = create(); roundtrip(input,output); return input==output; } @@ -52,7 +57,7 @@ bool equality(const T& input = T()) { // This version requires Testable template bool equalsObj(const T& input = T()) { - T output; + T output = create(); roundtrip(input,output); return assert_equal(input, output); } @@ -60,7 +65,7 @@ bool equalsObj(const T& input = T()) { // De-referenced version for pointers, requires equals method template bool equalsDereferenced(const T& input) { - T output; + T output = create(); roundtrip(input,output); return input->equals(*output); } @@ -79,7 +84,7 @@ void roundtripXML(const T& input, T& output) { // This version requires equality operator template bool equalityXML(const T& input = T()) { - T output; + T output = create(); roundtripXML(input,output); return input==output; } @@ -87,7 +92,7 @@ bool equalityXML(const T& input = T()) { // This version requires Testable template bool equalsXML(const T& input = T()) { - T output; + T output = create(); roundtripXML(input,output); return assert_equal(input, output); } @@ -95,7 +100,7 @@ bool equalsXML(const T& input = T()) { // This version is for pointers, requires equals method template bool equalsDereferencedXML(const T& input = T()) { - T output; + T output = create(); roundtripXML(input,output); return input->equals(*output); } @@ -114,7 +119,7 @@ void roundtripBinary(const T& input, T& output) { // This version requires equality operator template bool equalityBinary(const T& input = T()) { - T output; + T output = create(); roundtripBinary(input,output); return input==output; } @@ -122,7 +127,7 @@ bool equalityBinary(const T& input = T()) { // This version requires Testable template bool equalsBinary(const T& input = T()) { - T output; + T output = create(); roundtripBinary(input,output); return assert_equal(input, output); } @@ -130,7 +135,7 @@ bool equalsBinary(const T& input = T()) { // This version is for pointers, requires equals method template bool equalsDereferencedBinary(const T& input = T()) { - T output; + T output = create(); roundtripBinary(input,output); return input->equals(*output); } diff --git a/gtsam/geometry/EssentialMatrix.cpp b/gtsam/geometry/EssentialMatrix.cpp index 7b90edc39..699705fa5 100644 --- a/gtsam/geometry/EssentialMatrix.cpp +++ b/gtsam/geometry/EssentialMatrix.cpp @@ -107,7 +107,7 @@ ostream& operator <<(ostream& os, const EssentialMatrix& E) { Rot3 R = E.rotation(); Unit3 d = E.direction(); os.precision(10); - os << R.xyz().transpose() << " " << d.point3().vector().transpose() << " "; + os << R.xyz().transpose() << " " << d.point3().transpose() << " "; return os; } diff --git a/gtsam/geometry/OrientedPlane3.cpp b/gtsam/geometry/OrientedPlane3.cpp index 95290497d..bb9925e23 100644 --- a/gtsam/geometry/OrientedPlane3.cpp +++ b/gtsam/geometry/OrientedPlane3.cpp @@ -39,7 +39,7 @@ OrientedPlane3 OrientedPlane3::transform(const Pose3& xr, OptionalJacobian<3, 3> Unit3 n_rotated = xr.rotation().unrotate(n_, D_rotated_plane, D_rotated_pose); Vector3 unit_vec = n_rotated.unitVector(); - double pred_d = n_.unitVector().dot(xr.translation().vector()) + d_; + double pred_d = n_.unitVector().dot(xr.translation()) + d_; if (Hr) { *Hr = zeros(3, 6); @@ -47,7 +47,7 @@ OrientedPlane3 OrientedPlane3::transform(const Pose3& xr, OptionalJacobian<3, 3> Hr->block<1, 3>(2, 3) = unit_vec; } if (Hp) { - Vector2 hpp = n_.basis().transpose() * xr.translation().vector(); + Vector2 hpp = n_.basis().transpose() * xr.translation(); *Hp = Z_3x3; Hp->block<2, 2>(0, 0) = D_rotated_pose; Hp->block<1, 2>(2, 0) = hpp; diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp index 181ff3e97..01b5ed132 100644 --- a/gtsam/geometry/Point3.cpp +++ b/gtsam/geometry/Point3.cpp @@ -21,40 +21,16 @@ using namespace std; namespace gtsam { -/* ************************************************************************* */ +#ifndef GTSAM_USE_VECTOR3_POINTS bool Point3::equals(const Point3 &q, double tol) const { return (fabs(x() - q.x()) < tol && fabs(y() - q.y()) < tol && fabs(z() - q.z()) < tol); } -/* ************************************************************************* */ void Point3::print(const string& s) const { cout << s << *this << endl; } -#ifndef GTSAM_USE_VECTOR3_POINTS -/* ************************************************************************* */ -bool Point3::operator==(const Point3& q) const { - return x_ == q.x_ && y_ == q.y_ && z_ == q.z_; -} - -Point3 Point3::operator+(const Point3& q) const { - return Point3(x_ + q.x_, y_ + q.y_, z_ + q.z_); -} - -Point3 Point3::operator-(const Point3& q) const { - return Point3(x_ - q.x_, y_ - q.y_, z_ - q.z_); -} - -Point3 Point3::operator*(double s) const { - return Point3(x_ * s, y_ * s, z_ * s); -} - -Point3 Point3::operator/(double s) const { - return Point3(x_ / s, y_ / s, z_ / s); -} -#endif - /* ************************************************************************* */ double Point3::distance(const Point3 &q, OptionalJacobian<1, 3> H1, OptionalJacobian<1, 3> H2) const { @@ -102,6 +78,7 @@ Point3 Point3::sub(const Point3 &q, OptionalJacobian<3,3> H1, } #endif +#endif /* ************************************************************************* */ double distance(const Point3 &p1, const Point3 &q, OptionalJacobian<1, 3> H1, OptionalJacobian<1, 3> H2) { diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 75b8a7829..ba06e1d85 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -30,6 +30,12 @@ namespace gtsam { //#define GTSAM_USE_VECTOR3_POINTS #ifdef GTSAM_USE_VECTOR3_POINTS + + // As of GTSAM4, we just typedef Point3 to Vector3 + typedef Vector3 Point3; + +#else + /** * A 3D point is just a Vector3 with some additional methods * @addtogroup geometry @@ -38,13 +44,17 @@ namespace gtsam { class GTSAM_EXPORT Point3 : public Vector3 { public: + enum { dimension = 3 }; /// @name Standard Constructors /// @{ - /// Default constructor creates a zero-Point3 - Point3() { setZero(); } +#ifndef GTSAM_ALLOW_DEPRECATED_SINCE_V4 + /// Default constructor now creates *uninitialized* point !!!! + Point3() {} +#endif + /// Construct from x, y, and z coordinates Point3(double x, double y, double z): Vector3(x,y, z) {} @@ -68,7 +78,7 @@ class GTSAM_EXPORT Point3 : public Vector3 { /// @{ /// identity for group operation - inline static Point3 identity() { return Point3();} + inline static Point3 identity() { return Point3(0.0, 0.0, 0.0); } /// @} /// @name Vector Space @@ -82,7 +92,7 @@ class GTSAM_EXPORT Point3 : public Vector3 { double norm(OptionalJacobian<1,3> H = boost::none) const; /** normalize, with optional Jacobian */ - Point3 normalize(OptionalJacobian<3, 3> H = boost::none) const; + Point3 normalized(OptionalJacobian<3, 3> H = boost::none) const; /** cross product @return this x q */ Point3 cross(const Point3 &q, OptionalJacobian<3, 3> H_p = boost::none, // @@ -104,136 +114,10 @@ class GTSAM_EXPORT Point3 : public Vector3 { /// Output stream operator GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Point3& p); - private: - - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Vector3); - } - }; -#else -/** - * A 3D point - * @addtogroup geometry - * \nosubgrouping - */ -class GTSAM_EXPORT Point3 { - - private: - - double x_, y_, z_; - - public: - enum { dimension = 3 }; - - /// @name Standard Constructors - /// @{ - - /// Default constructor creates a zero-Point3 - Point3(): x_(0), y_(0), z_(0) {} - - /// Construct from x, y, and z coordinates - Point3(double x, double y, double z): x_(x), y_(y), z_(z) {} - - /// @} - /// @name Advanced Constructors - /// @{ - - /// Construct from 3-element vector - explicit Point3(const Vector3& v) { - x_ = v(0); - y_ = v(1); - z_ = v(2); - } - - /// @} - /// @name Testable - /// @{ - - /** print with optional string */ - void print(const std::string& s = "") const; - - /** equals with an tolerance */ - bool equals(const Point3& p, double tol = 1e-9) const; - - /// @} - /// @name Group - /// @{ - - /// identity for group operation - inline static Point3 identity() { return Point3();} - - /// inverse - Point3 operator - () const { return Point3(-x_,-y_,-z_);} - - /// add vector on right - inline Point3 operator +(const Vector3& v) const { - return Point3(x_ + v[0], y_ + v[1], z_ + v[2]); - } - - /// add - Point3 operator + (const Point3& q) const; - - /// subtract - Point3 operator - (const Point3& q) const; - - /// @} - /// @name Vector Space - /// @{ - - ///multiply with a scalar - Point3 operator * (double s) const; - - ///divide by a scalar - Point3 operator / (double s) const; - - /** distance between two points */ - double distance(const Point3& q, OptionalJacobian<1, 3> H1 = boost::none, - OptionalJacobian<1, 3> H2 = boost::none) const; - - /** Distance of the point from the origin, with Jacobian */ - double norm(OptionalJacobian<1,3> H = boost::none) const; - - /** normalize, with optional Jacobian */ - Point3 normalized(OptionalJacobian<3, 3> H = boost::none) const; - - /** cross product @return this x q */ - Point3 cross(const Point3 &q, OptionalJacobian<3, 3> H_p = boost::none, // - OptionalJacobian<3, 3> H_q = boost::none) const; - - /** dot product @return this * q*/ - double dot(const Point3 &q, OptionalJacobian<1, 3> H_p = boost::none, // - OptionalJacobian<1, 3> H_q = boost::none) const; - - /// @} - /// @name Standard Interface - /// @{ - - /// equality - bool operator ==(const Point3& q) const; - - /** return vectorized form (column-wise)*/ - Vector3 vector() const { return Vector3(x_,y_,z_); } - - /// get x - inline double x() const {return x_;} - - /// get y - inline double y() const {return y_;} - - /// get z - inline double z() const {return z_;} - - /// @} - - /// Output stream operator - GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Point3& p); - #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 /// @name Deprecated /// @{ + Point3() { setZero(); } Point3 inverse() const { return -(*this);} Point3 compose(const Point3& q) const { return (*this)+q;} Point3 between(const Point3& q) const { return q-(*this);} @@ -252,37 +136,26 @@ class GTSAM_EXPORT Point3 { private: - /// @name Advanced Interface - /// @{ - /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) - { - ar & BOOST_SERIALIZATION_NVP(x_); - ar & BOOST_SERIALIZATION_NVP(y_); - ar & BOOST_SERIALIZATION_NVP(z_); + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Vector3); } - - /// @} }; -/// Syntactic sugar for multiplying coordinates by a scalar s*p -inline Point3 operator*(double s, const Point3& p) { return p*s;} - -#endif - -// Convenience typedef -typedef std::pair Point3Pair; -std::ostream &operator<<(std::ostream &os, const gtsam::Point3Pair &p); - template<> struct traits : public internal::VectorSpace {}; template<> struct traits : public internal::VectorSpace {}; +#endif + +// Convenience typedef +typedef std::pair Point3Pair; +std::ostream &operator<<(std::ostream &os, const gtsam::Point3Pair &p); + /// distance between two points double distance(const Point3& p1, const Point3& q, OptionalJacobian<1, 3> H1 = boost::none, diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 44eb5c226..ccf39f153 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -48,8 +48,7 @@ Pose3 Pose3::inverse() const { // Experimental - unit tests of derivatives based on it do not check out yet Matrix6 Pose3::AdjointMap() const { const Matrix3 R = R_.matrix(); - const Vector3 t = t_.vector(); - Matrix3 A = skewSymmetric(t) * R; + Matrix3 A = skewSymmetric(t_.x(), t_.y(), t_.z()) * R; Matrix6 adj; adj << R, Z_3x3, A, R; return adj; @@ -101,12 +100,12 @@ Vector6 Pose3::adjointTranspose(const Vector6& xi, const Vector6& y, void Pose3::print(const string& s) const { cout << s; R_.print("R:\n"); - t_.print("t: "); + traits::Print(t_, "t: "); } /* ************************************************************************* */ bool Pose3::equals(const Pose3& pose, double tol) const { - return R_.equals(pose.R_, tol) && t_.equals(pose.t_, tol); + return R_.equals(pose.R_, tol) && traits::Equals(t_, pose.t_, tol); } /* ************************************************************************* */ @@ -115,14 +114,14 @@ Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> H) { if (H) *H = ExpmapDerivative(xi); // get angular velocity omega and translational velocity v from twist xi - Point3 omega(xi(0), xi(1), xi(2)), v(xi(3), xi(4), xi(5)); + Vector3 omega(xi(0), xi(1), xi(2)), v(xi(3), xi(4), xi(5)); - Rot3 R = Rot3::Expmap(omega.vector()); + Rot3 R = Rot3::Expmap(omega); double theta2 = omega.dot(omega); if (theta2 > std::numeric_limits::epsilon()) { - Point3 t_parallel = omega * omega.dot(v); // translation parallel to axis - Point3 omega_cross_v = omega.cross(v); // points towards axis - Point3 t = (omega_cross_v - R * omega_cross_v + t_parallel) / theta2; + Vector3 t_parallel = omega * omega.dot(v); // translation parallel to axis + Vector3 omega_cross_v = omega.cross(v); // points towards axis + Vector3 t = (omega_cross_v - R * omega_cross_v + t_parallel) / theta2; return Pose3(R, t); } else { return Pose3(R, v); @@ -132,19 +131,20 @@ Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> H) { /* ************************************************************************* */ Vector6 Pose3::Logmap(const Pose3& p, OptionalJacobian<6, 6> H) { if (H) *H = LogmapDerivative(p); - Vector3 w = Rot3::Logmap(p.rotation()), T = p.translation().vector(); - double t = w.norm(); + const Vector3 w = Rot3::Logmap(p.rotation()); + const Vector3 T = p.translation(); + const double t = w.norm(); if (t < 1e-10) { Vector6 log; log << w, T; return log; } else { - Matrix3 W = skewSymmetric(w / t); + const Matrix3 W = skewSymmetric(w / t); // Formula from Agrawal06iros, equation (14) // simplified with Mathematica, and multiplying in T to avoid matrix math - double Tan = tan(0.5 * t); - Vector3 WT = W * T; - Vector3 u = T - (0.5 * t) * WT + (1 - t / (2. * Tan)) * (W * WT); + const double Tan = tan(0.5 * t); + const Vector3 WT = W * T; + const Vector3 u = T - (0.5 * t) * WT + (1 - t / (2. * Tan)) * (W * WT); Vector6 log; log << w, u; return log; @@ -178,7 +178,7 @@ Vector6 Pose3::ChartAtOrigin::Local(const Pose3& T, ChartJacobian H) { H->topLeftCorner<3,3>() = DR; } Vector6 xi; - xi << omega, T.translation().vector(); + xi << omega, T.translation(); return xi; #endif } @@ -264,7 +264,7 @@ const Point3& Pose3::translation(OptionalJacobian<3, 6> H) const { Matrix4 Pose3::matrix() const { static const Matrix14 A14 = (Matrix14() << 0.0, 0.0, 0.0, 1.0).finished(); Matrix4 mat; - mat << R_.matrix(), t_.vector(), A14; + mat << R_.matrix(), t_, A14; return mat; } @@ -288,7 +288,7 @@ Point3 Pose3::transform_from(const Point3& p, OptionalJacobian<3,6> Dpose, if (Dpoint) { *Dpoint = R; } - return Point3(R * p.vector()) + t_; + return R_ * p + t_; } /* ************************************************************************* */ @@ -297,7 +297,7 @@ Point3 Pose3::transform_to(const Point3& p, OptionalJacobian<3,6> Dpose, // Only get transpose once, to avoid multiple allocations, // as well as multiple conversions in the Quaternion case const Matrix3 Rt = R_.transpose(); - const Point3 q(Rt*(p.vector() - t_.vector())); + const Point3 q(Rt*(p - t_)); if (Dpose) { const double wx = q.x(), wy = q.y(), wz = q.z(); (*Dpose) << @@ -321,7 +321,7 @@ double Pose3::range(const Point3& point, OptionalJacobian<1, 6> H1, return local.norm(); } else { Matrix13 D_r_local; - const double r = local.norm(D_r_local); + const double r = norm(local, D_r_local); if (H1) *H1 = D_r_local * D_local_pose; if (H2) *H2 = D_r_local * D_local_point; return r; @@ -361,10 +361,10 @@ boost::optional align(const vector& pairs) { return boost::none; // we need at least three pairs // calculate centroids - Vector3 cp = Vector3::Zero(), cq = Vector3::Zero(); + Point3 cp(0,0,0), cq(0,0,0); BOOST_FOREACH(const Point3Pair& pair, pairs) { - cp += pair.first.vector(); - cq += pair.second.vector(); + cp += pair.first; + cq += pair.second; } double f = 1.0 / n; cp *= f; @@ -373,10 +373,10 @@ boost::optional align(const vector& pairs) { // Add to form H matrix Matrix3 H = Z_3x3; BOOST_FOREACH(const Point3Pair& pair, pairs) { - Vector3 dp = pair.first.vector() - cp; - Vector3 dq = pair.second.vector() - cq; + Point3 dp = pair.first - cp; + Point3 dq = pair.second - cq; H += dp * dq.transpose(); - } + } // Compute SVD Matrix U, V; diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index fa4b2e51a..ba4b9737b 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -52,8 +52,7 @@ public: /// @{ /** Default constructor is origin */ - Pose3() { - } + Pose3() : R_(traits::Identity()), t_(traits::Identity()) {} /** Copy constructor */ Pose3(const Pose3& pose) : @@ -65,13 +64,6 @@ public: R_(R), t_(t) { } -#ifndef GTSAM_USE_VECTOR3_POINTS - /** Construct from R,t */ - Pose3(const Rot3& R, const Vector3& t) : - R_(R), t_(t) { - } -#endif - /** Construct from Pose2 */ explicit Pose3(const Pose2& pose2); diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index ef384c3ef..696c9df68 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -82,7 +82,7 @@ Unit3 Rot3::operator*(const Unit3& p) const { Point3 Rot3::unrotate(const Point3& p, OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const { const Matrix3& Rt = transpose(); - Point3 q(Rt * p.vector()); // q = Rt*p + Point3 q(Rt * p); // q = Rt*p const double wx = q.x(), wy = q.y(), wz = q.z(); if (H1) *H1 << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0; diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index b26475578..30385bd8c 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -171,22 +171,6 @@ namespace gtsam { return Rot3(q); } -#ifndef GTSAM_USE_VECTOR3_POINTS - /** - * Convert from axis/angle representation - * @param axis is the rotation axis, unit length - * @param angle rotation angle - * @return incremental rotation - */ - static Rot3 AxisAngle(const Vector3& axis, double angle) { -#ifdef GTSAM_USE_QUATERNIONS - return gtsam::Quaternion(Eigen::AngleAxis(angle, axis)); -#else - return Rot3(SO3::AxisAngle(axis,angle)); -#endif - } -#endif - /** * Convert from axis/angle representation * @param axisw is the rotation axis, unit length @@ -197,7 +181,7 @@ namespace gtsam { #ifdef GTSAM_USE_QUATERNIONS return gtsam::Quaternion(Eigen::AngleAxis(angle, axis.vector())); #else - return Rot3(SO3::AxisAngle(axis.vector(),angle)); + return Rot3(SO3::AxisAngle(axis,angle)); #endif } @@ -365,24 +349,6 @@ namespace gtsam { Point3 unrotate(const Point3& p, OptionalJacobian<3,3> H1 = boost::none, OptionalJacobian<3,3> H2=boost::none) const; -#ifndef GTSAM_USE_VECTOR3_POINTS - /// operator* for Vector3 - inline Vector3 operator*(const Vector3& v) const { - return rotate(Point3(v)).vector(); - } - - /// rotate for Vector3 - Vector3 rotate(const Vector3& v, OptionalJacobian<3, 3> H1 = boost::none, - OptionalJacobian<3, 3> H2 = boost::none) const { - return rotate(Point3(v), H1, H2).vector(); - } - /// unrotate for Vector3 - Vector3 unrotate(const Vector3& v, OptionalJacobian<3, 3> H1 = boost::none, - OptionalJacobian<3, 3> H2 = boost::none) const { - return unrotate(Point3(v), H1, H2).vector(); - } -#endif - /// @} /// @name Group Action on Unit3 /// @{ diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index c3636000b..03cde1a01 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -36,9 +36,9 @@ Rot3::Rot3() : rot_(I_3x3) {} /* ************************************************************************* */ Rot3::Rot3(const Point3& col1, const Point3& col2, const Point3& col3) { - rot_.col(0) = col1.vector(); - rot_.col(1) = col2.vector(); - rot_.col(2) = col3.vector(); + rot_.col(0) = (Vector3)col1; + rot_.col(1) = (Vector3)col2; + rot_.col(2) = (Vector3)col3; } /* ************************************************************************* */ @@ -121,7 +121,7 @@ Point3 Rot3::rotate(const Point3& p, OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const { if (H1) *H1 = rot_ * skewSymmetric(-p.x(), -p.y(), -p.z()); if (H2) *H2 = rot_; - return Point3(rot_ * p.vector()); + return rot_ * p; } /* ************************************************************************* */ diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index 69d4276dd..a64f99eb1 100644 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -45,9 +45,8 @@ namespace gtsam { Unit3 Unit3::FromPoint3(const Point3& point, OptionalJacobian<2,3> H) { // 3*3 Derivative of representation with respect to point is 3*3: Matrix3 D_p_point; - Point3 normalized = normalize(point, H ? &D_p_point : 0); Unit3 direction; - direction.p_ = normalized.vector(); + direction.p_ = normalize(point, H ? &D_p_point : 0); if (H) *H << direction.basis().transpose() * D_p_point; return direction; @@ -90,22 +89,18 @@ const Matrix32& Unit3::basis(OptionalJacobian<6, 2> H) const { const Point3 n(p_); // Get the axis of rotation with the minimum projected length of the point - Point3 axis; + Point3 axis(0, 0, 1); double mx = fabs(n.x()), my = fabs(n.y()), mz = fabs(n.z()); if ((mx <= my) && (mx <= mz)) { axis = Point3(1.0, 0.0, 0.0); } else if ((my <= mx) && (my <= mz)) { axis = Point3(0.0, 1.0, 0.0); - } else if ((mz <= mx) && (mz <= my)) { - axis = Point3(0.0, 0.0, 1.0); - } else { - assert(false); } // Choose the direction of the first basis vector b1 in the tangent plane by crossing n with // the chosen axis. Matrix33 H_B1_n; - Point3 B1 = n.cross(axis, H ? &H_B1_n : 0); + Point3 B1 = gtsam::cross(n, axis, H ? &H_B1_n : 0); // Normalize result to get a unit vector: b1 = B1 / |B1|. Matrix33 H_b1_B1; @@ -114,7 +109,7 @@ const Matrix32& Unit3::basis(OptionalJacobian<6, 2> H) const { // Get the second basis vector b2, which is orthogonal to n and b1, by crossing them. // No need to normalize this, p and b1 are orthogonal unit vectors. Matrix33 H_b2_n, H_b2_b1; - Point3 b2 = n.cross(b1, H ? &H_b2_n : 0, H ? &H_b2_b1 : 0); + Point3 b2 = gtsam::cross(n, b1, H ? &H_b2_n : 0, H ? &H_b2_b1 : 0); // Create the basis by stacking b1 and b2. B_.reset(Matrix32()); @@ -176,7 +171,7 @@ double Unit3::dot(const Unit3& q, OptionalJacobian<1,2> H_p, OptionalJacobian<1, // Compute the dot product of the Point3s. Matrix13 H_dot_pn, H_dot_qn; - double d = pn.dot(qn, H_p ? &H_dot_pn : 0, H_q ? &H_dot_qn : 0); + double d = gtsam::dot(pn, qn, H_p ? &H_dot_pn : 0, H_q ? &H_dot_qn : 0); if (H_p) { (*H_p) << H_dot_pn * H_pn_p; @@ -209,7 +204,7 @@ Vector2 Unit3::errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p, OptionalJ // 2D error here is projecting q into the tangent plane of this (p). Matrix62 H_B_p; Matrix23 Bt = basis(H_p ? &H_B_p : 0).transpose(); - Vector2 xi = Bt * qn.vector(); + Vector2 xi = Bt * qn; if (H_p) { // Derivatives of each basis vector. @@ -217,8 +212,8 @@ Vector2 Unit3::errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p, OptionalJ const Matrix32& H_b2_p = H_B_p.block<3, 2>(3, 0); // Derivatives of the two entries of xi wrt the basis vectors. - Matrix13 H_xi1_b1 = qn.vector().transpose(); - Matrix13 H_xi2_b2 = qn.vector().transpose(); + Matrix13 H_xi1_b1 = qn.transpose(); + Matrix13 H_xi2_b2 = qn.transpose(); // Assemble dxi/dp = dxi/dB * dB/dp. Matrix12 H_xi1_p = H_xi1_b1 * H_b1_p; diff --git a/gtsam/geometry/tests/testPinholeCamera.cpp b/gtsam/geometry/tests/testPinholeCamera.cpp index ce9932879..4682a6507 100644 --- a/gtsam/geometry/tests/testPinholeCamera.cpp +++ b/gtsam/geometry/tests/testPinholeCamera.cpp @@ -103,16 +103,16 @@ TEST( PinholeCamera, level2) TEST( PinholeCamera, lookat) { // Create a level camera, looking in Y-direction - Point3 C(10.0,0.0,0.0); - Camera camera = Camera::Lookat(C, Point3(), Point3(0.0,0.0,1.0)); + Point3 C(10,0,0); + Camera camera = Camera::Lookat(C, Point3(0,0,0), Point3(0,0,1)); // expected Point3 xc(0,1,0),yc(0,0,-1),zc(-1,0,0); Pose3 expected(Rot3(xc,yc,zc),C); - EXPECT(assert_equal( camera.pose(), expected)); + EXPECT(assert_equal(camera.pose(), expected)); - Point3 C2(30.0,0.0,10.0); - Camera camera2 = Camera::Lookat(C2, Point3(), Point3(0.0,0.0,1.0)); + Point3 C2(30,0,10); + Camera camera2 = Camera::Lookat(C2, Point3(), Point3(0,0,1)); Matrix R = camera2.pose().rotation().matrix(); Matrix I = trans(R)*R; @@ -278,7 +278,7 @@ TEST( PinholeCamera, range0) { double result = camera.range(point1, D1, D2); Matrix Hexpected1 = numericalDerivative21(range0, camera, point1); Matrix Hexpected2 = numericalDerivative22(range0, camera, point1); - EXPECT_DOUBLES_EQUAL(point1.distance(camera.pose().translation()), result, + EXPECT_DOUBLES_EQUAL(distance(point1, camera.pose().translation()), result, 1e-9); EXPECT(assert_equal(Hexpected1, D1, 1e-7)); EXPECT(assert_equal(Hexpected2, D2, 1e-7)); diff --git a/gtsam/geometry/tests/testPinholePose.cpp b/gtsam/geometry/tests/testPinholePose.cpp index 3c79a45e3..c5137e3b3 100644 --- a/gtsam/geometry/tests/testPinholePose.cpp +++ b/gtsam/geometry/tests/testPinholePose.cpp @@ -74,16 +74,16 @@ TEST(PinholeCamera, Pose) { TEST( PinholePose, lookat) { // Create a level camera, looking in Y-direction - Point3 C(10.0,0.0,0.0); - Camera camera = Camera::Lookat(C, Point3(), Point3(0.0,0.0,1.0)); + Point3 C(10,0,0); + Camera camera = Camera::Lookat(C, Point3(0,0,0), Point3(0,0,1)); // expected Point3 xc(0,1,0),yc(0,0,-1),zc(-1,0,0); Pose3 expected(Rot3(xc,yc,zc),C); EXPECT(assert_equal( camera.pose(), expected)); - Point3 C2(30.0,0.0,10.0); - Camera camera2 = Camera::Lookat(C2, Point3(), Point3(0.0,0.0,1.0)); + Point3 C2(30,0,10); + Camera camera2 = Camera::Lookat(C2, Point3(0,0,0), Point3(0,0,1)); Matrix R = camera2.pose().rotation().matrix(); Matrix I = trans(R)*R; @@ -120,7 +120,7 @@ TEST( PinholePose, backprojectInfinity) /* ************************************************************************* */ TEST( PinholePose, backproject2) { - Point3 origin; + Point3 origin(0,0,0); Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera1 looking down Camera camera(Pose3(rot, origin), K); @@ -212,8 +212,7 @@ TEST( PinholePose, range0) { double result = camera.range(point1, D1, D2); Matrix expectedDcamera = numericalDerivative21(range0, camera, point1); Matrix expectedDpoint = numericalDerivative22(range0, camera, point1); - EXPECT_DOUBLES_EQUAL(point1.distance(camera.pose().translation()), result, - 1e-9); + EXPECT_DOUBLES_EQUAL(distance(point1, camera.pose().translation()), result, 1e-9); EXPECT(assert_equal(expectedDcamera, D1, 1e-7)); EXPECT(assert_equal(expectedDpoint, D2, 1e-7)); } diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp index 37ad105c7..c7fb44716 100644 --- a/gtsam/geometry/tests/testPoint3.cpp +++ b/gtsam/geometry/tests/testPoint3.cpp @@ -71,14 +71,14 @@ TEST( Point3, arithmetic) { /* ************************************************************************* */ TEST( Point3, equals) { - CHECK(P.equals(P)); - Point3 Q; - CHECK(!P.equals(Q)); + CHECK(traits::Equals(P,P)); + Point3 Q(0,0,0); + CHECK(!traits::Equals(P,Q)); } /* ************************************************************************* */ TEST( Point3, dot) { - Point3 origin, ones(1, 1, 1); + Point3 origin(0,0,0), ones(1, 1, 1); CHECK(origin.dot(Point3(1, 1, 0)) == 0); CHECK(ones.dot(Point3(1, 1, 0)) == 2); } @@ -111,20 +111,20 @@ TEST (Point3, norm) { Matrix actualH; Point3 point(3,4,5); // arbitrary point double expected = sqrt(50); - EXPECT_DOUBLES_EQUAL(expected, point.norm(actualH), 1e-8); + EXPECT_DOUBLES_EQUAL(expected, norm(point, actualH), 1e-8); Matrix expectedH = numericalDerivative11(norm_proxy, point); EXPECT(assert_equal(expectedH, actualH, 1e-8)); } /* ************************************************************************* */ double testFunc(const Point3& P, const Point3& Q) { - return P.distance(Q); + return distance(P,Q); } TEST (Point3, distance) { Point3 P(1., 12.8, -32.), Q(52.7, 4.9, -13.3); Matrix H1, H2; - double d = P.distance(Q, H1, H2); + double d = distance(P, Q, H1, H2); double expectedDistance = 55.542686; Matrix numH1 = numericalDerivative21(testFunc, P, Q); Matrix numH2 = numericalDerivative22(testFunc, P, Q); @@ -137,9 +137,9 @@ TEST (Point3, distance) { TEST(Point3, cross) { Matrix aH1, aH2; boost::function f = - boost::bind(&Point3::cross, _1, _2, boost::none, boost::none); + boost::bind(>sam::cross, _1, _2, boost::none, boost::none); const Point3 omega(0, 1, 0), theta(4, 6, 8); - omega.cross(theta, aH1, aH2); + cross(omega, theta, aH1, aH2); EXPECT(assert_equal(numericalDerivative21(f, omega, theta), aH1)); EXPECT(assert_equal(numericalDerivative22(f, omega, theta), aH2)); } diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 56e1e022c..61550d540 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -62,7 +62,7 @@ TEST( Pose3, retract_first_order) Pose3 id; Vector v = zero(6); v(0) = 0.3; - EXPECT(assert_equal(Pose3(R, Point3()), id.retract(v),1e-2)); + EXPECT(assert_equal(Pose3(R, Point3(0,0,0)), id.retract(v),1e-2)); v(3)=0.2;v(4)=0.7;v(5)=-2; EXPECT(assert_equal(Pose3(R, P),id.retract(v),1e-2)); } @@ -72,7 +72,7 @@ TEST( Pose3, retract_expmap) { Vector v = zero(6); v(0) = 0.3; Pose3 pose = Pose3::Expmap(v); - EXPECT(assert_equal(Pose3(R, Point3()), pose, 1e-2)); + EXPECT(assert_equal(Pose3(R, Point3(0,0,0)), pose, 1e-2)); EXPECT(assert_equal(v,Pose3::Logmap(pose),1e-2)); } @@ -82,7 +82,7 @@ TEST( Pose3, expmap_a_full) Pose3 id; Vector v = zero(6); v(0) = 0.3; - EXPECT(assert_equal(expmap_default(id, v), Pose3(R, Point3()))); + EXPECT(assert_equal(expmap_default(id, v), Pose3(R, Point3(0,0,0)))); v(3)=0.2;v(4)=0.394742;v(5)=-2.08998; EXPECT(assert_equal(Pose3(R, P),expmap_default(id, v),1e-5)); } @@ -93,7 +93,7 @@ TEST( Pose3, expmap_a_full2) Pose3 id; Vector v = zero(6); v(0) = 0.3; - EXPECT(assert_equal(expmap_default(id, v), Pose3(R, Point3()))); + EXPECT(assert_equal(expmap_default(id, v), Pose3(R, Point3(0,0,0)))); v(3)=0.2;v(4)=0.394742;v(5)=-2.08998; EXPECT(assert_equal(Pose3(R, P),expmap_default(id, v),1e-5)); } @@ -388,7 +388,7 @@ TEST( Pose3, transform_to_translate) /* ************************************************************************* */ TEST( Pose3, transform_to_rotate) { - Pose3 transform(Rot3::Rodrigues(0,0,-1.570796), Point3()); + Pose3 transform(Rot3::Rodrigues(0,0,-1.570796), Point3(0,0,0)); Point3 actual = transform.transform_to(Point3(2,1,10)); Point3 expected(-1,2,10); EXPECT(assert_equal(expected, actual, 0.001)); @@ -406,7 +406,7 @@ TEST( Pose3, transform_to) /* ************************************************************************* */ TEST( Pose3, transform_from) { - Point3 actual = T3.transform_from(Point3()); + Point3 actual = T3.transform_from(Point3(0,0,0)); Point3 expected = Point3(1.,2.,3.); EXPECT(assert_equal(expected, actual)); } diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index ce5d3268c..f96198b94 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -542,8 +542,8 @@ TEST(Rot3, quaternion) { Point3 expected1 = R1*p1; Point3 expected2 = R2*p2; - Point3 actual1 = Point3(q1*p1.vector()); - Point3 actual2 = Point3(q2*p2.vector()); + Point3 actual1 = Point3(q1*p1); + Point3 actual2 = Point3(q2*p2); EXPECT(assert_equal(expected1, actual1)); EXPECT(assert_equal(expected2, actual2)); diff --git a/gtsam/geometry/tests/testSimpleCamera.cpp b/gtsam/geometry/tests/testSimpleCamera.cpp index 8db8113a1..71dcabd4e 100644 --- a/gtsam/geometry/tests/testSimpleCamera.cpp +++ b/gtsam/geometry/tests/testSimpleCamera.cpp @@ -63,16 +63,16 @@ TEST( SimpleCamera, level2) TEST( SimpleCamera, lookat) { // Create a level camera, looking in Y-direction - Point3 C(10.0,0.0,0.0); - SimpleCamera camera = SimpleCamera::Lookat(C, Point3(), Point3(0.0,0.0,1.0)); + Point3 C(10,0,0); + SimpleCamera camera = SimpleCamera::Lookat(C, Point3(0,0,0), Point3(0,0,1)); // expected Point3 xc(0,1,0),yc(0,0,-1),zc(-1,0,0); Pose3 expected(Rot3(xc,yc,zc),C); CHECK(assert_equal( camera.pose(), expected)); - Point3 C2(30.0,0.0,10.0); - SimpleCamera camera2 = SimpleCamera::Lookat(C2, Point3(), Point3(0.0,0.0,1.0)); + Point3 C2(30,0,10); + SimpleCamera camera2 = SimpleCamera::Lookat(C2, Point3(0,0,0), Point3(0,0,1)); Matrix R = camera2.pose().rotation().matrix(); Matrix I = trans(R)*R; @@ -100,7 +100,7 @@ TEST( SimpleCamera, backproject) /* ************************************************************************* */ TEST( SimpleCamera, backproject2) { - Point3 origin; + Point3 origin(0,0,0); Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera looking down SimpleCamera camera(Pose3(rot, origin), K); diff --git a/gtsam/geometry/tests/testStereoCamera.cpp b/gtsam/geometry/tests/testStereoCamera.cpp index 6508965ec..497599a6a 100644 --- a/gtsam/geometry/tests/testStereoCamera.cpp +++ b/gtsam/geometry/tests/testStereoCamera.cpp @@ -46,7 +46,7 @@ TEST( StereoCamera, project) // point X Y Z in meters Point3 p(0, 0, 5); StereoPoint2 result = stereoCam.project(p); - CHECK(assert_equal(StereoPoint2(320,320-150,240),result)); + CHECK(assert_equal(StereoPoint2(320, 320 - 150, 240), result)); // point X Y Z in meters Point3 p2(1, 1, 5); diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp index dbe315807..07789acaa 100644 --- a/gtsam/geometry/tests/testUnit3.cpp +++ b/gtsam/geometry/tests/testUnit3.cpp @@ -377,7 +377,7 @@ TEST(Unit3, retract_expmap) { TEST(Unit3, Random) { boost::mt19937 rng(42); // Check that means are all zero at least - Point3 expectedMean, actualMean; + Point3 expectedMean(0,0,0), actualMean(0,0,0); for (size_t i = 0; i < 100; i++) actualMean = actualMean + Unit3::Random(rng).point3(); actualMean = actualMean / 100; diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index c6e613b14..15029b8ad 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -436,7 +436,7 @@ TriangulationResult triangulateSafe(const std::vector& cameras, BOOST_FOREACH(const CAMERA& camera, cameras) { const Pose3& pose = camera.pose(); if (params.landmarkDistanceThreshold > 0 - && pose.translation().distance(point) + && distance(pose.translation(), point) > params.landmarkDistanceThreshold) return TriangulationResult::Degenerate(); #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION diff --git a/gtsam/navigation/GPSFactor.cpp b/gtsam/navigation/GPSFactor.cpp index 2f4cb9bc3..b2cb5ed82 100644 --- a/gtsam/navigation/GPSFactor.cpp +++ b/gtsam/navigation/GPSFactor.cpp @@ -25,14 +25,14 @@ namespace gtsam { //*************************************************************************** void GPSFactor::print(const string& s, const KeyFormatter& keyFormatter) const { cout << s << "GPSFactor on " << keyFormatter(key()) << "\n"; - nT_.print(" prior mean: "); + cout << " prior mean: " << nT_ << "\n"; noiseModel_->print(" noise model: "); } //*************************************************************************** bool GPSFactor::equals(const NonlinearFactor& expected, double tol) const { const This* e = dynamic_cast(&expected); - return e != NULL && Base::equals(*e, tol) && nT_.equals(e->nT_, tol); + return e != NULL && Base::equals(*e, tol) && traits::Equals(nT_, e->nT_, tol); } //*************************************************************************** @@ -43,7 +43,7 @@ Vector GPSFactor::evaluateError(const Pose3& p, H->block < 3, 3 > (0, 0) << zeros(3, 3); H->block < 3, 3 > (0, 3) << p.rotation().matrix(); } - return p.translation().vector() -nT_.vector(); + return p.translation() -nT_; } //*************************************************************************** @@ -66,7 +66,7 @@ pair GPSFactor::EstimateState(double t1, const Point3& NED1, // Construct initial pose Pose3 nTb(nRb, nT); // nTb - return make_pair(nTb, nV.vector()); + return make_pair(nTb, nV); } //*************************************************************************** diff --git a/gtsam/navigation/GPSFactor.h b/gtsam/navigation/GPSFactor.h index cd5fa71d2..46e194460 100644 --- a/gtsam/navigation/GPSFactor.h +++ b/gtsam/navigation/GPSFactor.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -48,8 +48,7 @@ public: typedef GPSFactor This; /** default constructor - only use for serialization */ - GPSFactor() { - } + GPSFactor(): nT_(0, 0, 0) {} virtual ~GPSFactor() { } diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index 860eaa85c..7e5d85cde 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -89,7 +89,7 @@ void NavState::print(const string& s) const { //------------------------------------------------------------------------------ bool NavState::equals(const NavState& other, double tol) const { - return R_.equals(other.R_, tol) && t_.equals(other.t_, tol) + return R_.equals(other.R_, tol) && traits::Equals(t_, other.t_, tol) && equal_with_abs_tol(v_, other.v_, tol); } @@ -121,11 +121,6 @@ Point3 NavState::operator*(const Point3& b_t) const { return Point3(R_ * b_t + t_); } -//------------------------------------------------------------------------------ -Velocity3 NavState::operator*(const Velocity3& b_v) const { - return Velocity3(R_ * b_v + v_); -} - //------------------------------------------------------------------------------ NavState NavState::ChartAtOrigin::Retract(const Vector9& xi, OptionalJacobian<9, 9> H) { @@ -189,7 +184,7 @@ Vector9 NavState::Logmap(const NavState& nTb, OptionalJacobian<9, 9> H) { throw runtime_error("NavState::Logmap derivative not implemented yet"); TIE(nRb, n_p, n_v, nTb); - Vector3 n_t = n_p.vector(); + Vector3 n_t = n_p; // NOTE(frank): See Pose3::Logmap Vector9 xi; @@ -300,7 +295,7 @@ Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder, dP(xi) << ((-dt2) * omega_cross_vel); // NOTE(luca): we got rid of the 2 wrt INS paper dV(xi) << ((-2.0 * dt) * omega_cross_vel); if (secondOrder) { - const Vector3 omega_cross2_t = omega.cross(omega.cross(n_t.vector())); + const Vector3 omega_cross2_t = omega.cross(omega.cross(n_t)); dP(xi) -= (0.5 * dt2) * omega_cross2_t; dV(xi) -= dt * omega_cross2_t; } diff --git a/gtsam/navigation/NavState.h b/gtsam/navigation/NavState.h index 4c8b50776..02da46317 100644 --- a/gtsam/navigation/NavState.h +++ b/gtsam/navigation/NavState.h @@ -49,7 +49,7 @@ public: /// Default constructor NavState() : - v_(Vector3::Zero()) { + t_(0,0,0), v_(Vector3::Zero()) { } /// Construct from attitude, position, velocity NavState(const Rot3& R, const Point3& t, const Velocity3& v) : @@ -97,7 +97,7 @@ public: } /// Return position as Vector3 Vector3 t() const { - return t_.vector(); + return t_; } /// Return velocity as Vector3. Computation-free. const Vector3& v() const { @@ -147,9 +147,6 @@ public: /// Act on position alone, n_t = nRb * b_t + n_t Point3 operator*(const Point3& b_t) const; - /// Act on velocity alone, n_v = nRb * b_v + n_v - Velocity3 operator*(const Velocity3& b_v) const; - /// @} /// @name Manifold /// @{ diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index c3f203849..7209391f1 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -94,7 +94,7 @@ pair PreintegrationBase::correctMeasurementsBySensorPose( if (D_correctedOmega_unbiasedOmega) *D_correctedOmega_unbiasedOmega = bRs; // Centrifugal acceleration - const Vector3 b_arm = p().body_P_sensor->translation().vector(); + const Vector3 b_arm = p().body_P_sensor->translation(); if (!b_arm.isZero()) { // Subtract out the the centripetal acceleration from the unbiased one // to get linear acceleration vector in the body frame: @@ -188,7 +188,7 @@ void PreintegrationBase::integrateMeasurement(const Vector3& measuredAcc, if (p().body_P_sensor) { // More complicated derivatives in case of non-trivial sensor pose *C *= D_correctedOmega_omega; - if (!p().body_P_sensor->translation().vector().isZero()) + if (!p().body_P_sensor->translation().isZero()) *C += *B* D_correctedAcc_omega; *B *= D_correctedAcc_acc; // NOTE(frank): needs to be last } diff --git a/gtsam/navigation/Scenario.h b/gtsam/navigation/Scenario.h index ad684f5f8..e4b380ff9 100644 --- a/gtsam/navigation/Scenario.h +++ b/gtsam/navigation/Scenario.h @@ -80,7 +80,7 @@ class AcceleratingScenario : public Scenario { AcceleratingScenario(const Rot3& nRb, const Point3& p0, const Vector3& v0, const Vector3& a_n, const Vector3& omega_b = Vector3::Zero()) - : nRb_(nRb), p0_(p0.vector()), v0_(v0), a_n_(a_n), omega_b_(omega_b) {} + : nRb_(nRb), p0_(p0), v0_(v0), a_n_(a_n), omega_b_(omega_b) {} Pose3 pose(double t) const override { return Pose3(nRb_.expmap(omega_b_ * t), p0_ + v0_ * t + a_n_ * t * t / 2.0); diff --git a/gtsam/nonlinear/NonlinearFactorGraph.cpp b/gtsam/nonlinear/NonlinearFactorGraph.cpp index b81332f5e..f05504d6b 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.cpp +++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -84,9 +84,9 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values, } else if (const GenericValue* p = dynamic_cast*>(&value)) { t << p->value().x(), p->value().y(), 0; } else if (const GenericValue* p = dynamic_cast*>(&value)) { - t = p->value().translation().vector(); + t = p->value().translation(); } else if (const GenericValue* p = dynamic_cast*>(&value)) { - t = p->value().vector(); + t = p->value(); } else { return boost::none; } diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp index 26580f41e..ddc7b18d6 100644 --- a/gtsam/slam/InitializePose3.cpp +++ b/gtsam/slam/InitializePose3.cpp @@ -327,7 +327,7 @@ Values computePoses(NonlinearFactorGraph& pose3graph, Values& initialRot) { BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, initialRot){ Key key = key_value.key; const Rot3& rot = initialRot.at(key); - Pose3 initializedPose = Pose3(rot, Point3()); + Pose3 initializedPose = Pose3(rot, Point3(0,0,0)); initialPose.insert(key, initializedPose); } // add prior diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 50516afe4..c79fb5823 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -1,5 +1,5 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -808,7 +808,7 @@ bool writeBAL(const string& filename, SfM_data &data) { Cal3Bundler cameraCalibration = data.cameras[i].calibration(); Pose3 poseOpenGL = gtsam2openGL(poseGTSAM); os << Rot3::Logmap(poseOpenGL.rotation()) << endl; - os << poseOpenGL.translation().vector() << endl; + os << poseOpenGL.translation() << endl; os << cameraCalibration.fx() << endl; os << cameraCalibration.k1() << endl; os << cameraCalibration.k2() << endl; @@ -880,7 +880,7 @@ bool writeBALfromValues(const string& filename, const SfM_data &data, dataValues.tracks[j].r = 1.0; dataValues.tracks[j].g = 0.0; dataValues.tracks[j].b = 0.0; - dataValues.tracks[j].p = Point3(); + dataValues.tracks[j].p = Point3(0,0,0); } } diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 54e27229c..dc7804c2d 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -147,6 +147,7 @@ typedef std::pair SIFT_Index; /// Define the structure for the 3D points struct SfM_Track { + SfM_Track():p(0,0,0) {} Point3 p; ///< 3D position of the point float r, g, b; ///< RGB color of the 3D point std::vector measurements; ///< The 2D image projections (id,(u,v)) diff --git a/gtsam_unstable/geometry/Event.h b/gtsam_unstable/geometry/Event.h index 5bd453424..77b02d9fb 100644 --- a/gtsam_unstable/geometry/Event.h +++ b/gtsam_unstable/geometry/Event.h @@ -35,7 +35,7 @@ public: /// Default Constructor Event() : - time_(0) { + time_(0), location_(0,0,0) { } /// Constructor from time and location diff --git a/gtsam_unstable/geometry/Similarity3.cpp b/gtsam_unstable/geometry/Similarity3.cpp index 17d54b04d..516e1fb5b 100644 --- a/gtsam_unstable/geometry/Similarity3.cpp +++ b/gtsam_unstable/geometry/Similarity3.cpp @@ -23,11 +23,11 @@ namespace gtsam { Similarity3::Similarity3() : - R_(), t_(), s_(1) { + t_(0,0,0), s_(1) { } Similarity3::Similarity3(double s) : - s_(s) { + t_(0,0,0), s_(s) { } Similarity3::Similarity3(const Rot3& R, const Point3& t, double s) : diff --git a/gtsam_unstable/geometry/tests/testSimilarity3.cpp b/gtsam_unstable/geometry/tests/testSimilarity3.cpp index 9c7620d70..713ea3054 100644 --- a/gtsam_unstable/geometry/tests/testSimilarity3.cpp +++ b/gtsam_unstable/geometry/tests/testSimilarity3.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -71,7 +71,7 @@ TEST(Similarity3, Constructors) { TEST(Similarity3, Getters) { Similarity3 sim3_default; EXPECT(assert_equal(Rot3(), sim3_default.rotation())); - EXPECT(assert_equal(Point3(), sim3_default.translation())); + EXPECT(assert_equal(Point3(0,0,0), sim3_default.translation())); EXPECT_DOUBLES_EQUAL(1.0, sim3_default.scale(), 1e-9); Similarity3 sim3(Rot3::Ypr(1, 2, 3), Point3(4, 5, 6), 7); @@ -158,7 +158,7 @@ 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)); + EXPECT(assert_equal(Similarity3(R, Point3(0,0,0), 1), id.retract(v), 1e-2)); // v(3) = 0.2; // v(4) = 0.7; // v(5) = -2; From 94ccf989857964dc7e517b2cea100721b20110fb Mon Sep 17 00:00:00 2001 From: Frank Date: Thu, 11 Feb 2016 16:52:02 -0800 Subject: [PATCH 17/90] Avoid default constructor in tests --- gtsam/geometry/tests/testPinholeCamera.cpp | 8 ++++---- gtsam/geometry/tests/testPose3.cpp | 2 +- gtsam/navigation/tests/testImuFactor.cpp | 4 ++-- .../navigation/tests/testImuFactorSerialization.cpp | 2 +- gtsam/slam/tests/testInitializePose3.cpp | 12 ++++++------ gtsam/slam/tests/testSmartProjectionPoseFactor.cpp | 4 ++-- gtsam_unstable/dynamics/tests/testPoseRTV.cpp | 2 +- gtsam_unstable/geometry/tests/testEvent.cpp | 4 ++-- .../slam/tests/testInvDepthFactorVariant1.cpp | 6 +++--- .../slam/tests/testInvDepthFactorVariant2.cpp | 8 ++++---- tests/testNonlinearEquality.cpp | 2 +- 11 files changed, 27 insertions(+), 27 deletions(-) diff --git a/gtsam/geometry/tests/testPinholeCamera.cpp b/gtsam/geometry/tests/testPinholeCamera.cpp index 4682a6507..4fbdbcbe1 100644 --- a/gtsam/geometry/tests/testPinholeCamera.cpp +++ b/gtsam/geometry/tests/testPinholeCamera.cpp @@ -112,7 +112,7 @@ TEST( PinholeCamera, lookat) EXPECT(assert_equal(camera.pose(), expected)); Point3 C2(30,0,10); - Camera camera2 = Camera::Lookat(C2, Point3(), Point3(0,0,1)); + Camera camera2 = Camera::Lookat(C2, Point3(0,0,0), Point3(0,0,1)); Matrix R = camera2.pose().rotation().matrix(); Matrix I = trans(R)*R; @@ -149,7 +149,7 @@ TEST( PinholeCamera, backprojectInfinity) /* ************************************************************************* */ TEST( PinholeCamera, backproject2) { - Point3 origin; + Point3 origin(0,0,0); Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera1 looking down Camera camera(Pose3(rot, origin), K); @@ -165,7 +165,7 @@ TEST( PinholeCamera, backproject2) /* ************************************************************************* */ TEST( PinholeCamera, backprojectInfinity2) { - Point3 origin; + Point3 origin(0,0,0); Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera1 looking down Camera camera(Pose3(rot, origin), K); @@ -180,7 +180,7 @@ TEST( PinholeCamera, backprojectInfinity2) /* ************************************************************************* */ TEST( PinholeCamera, backprojectInfinity3) { - Point3 origin; + Point3 origin(0,0,0); Rot3 rot(1., 0., 0., 0., 1., 0., 0., 0., 1.); // identity Camera camera(Pose3(rot, origin), K); diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 61550d540..af8e7c6a0 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -306,7 +306,7 @@ TEST( Pose3, Dtransform_from1_b) TEST( Pose3, Dtransform_from1_c) { - Point3 origin; + Point3 origin(0,0,0); Pose3 T0(R,origin); Matrix actualDtransform_from1; T0.transform_from(P, actualDtransform_from1, boost::none); diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index f1d761cb0..28ad037c0 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -590,7 +590,7 @@ TEST(ImuFactor, PredictRotation) { // Predict NavState actual = pim.predict(NavState(), bias); - NavState expected(Rot3::Ypr(M_PI / 10, 0, 0), Point3(), Z_3x1); + NavState expected(Rot3::Ypr(M_PI / 10, 0, 0), Point3(0,0,0), Z_3x1); EXPECT(assert_equal(expected, actual)); } @@ -708,7 +708,7 @@ TEST(ImuFactor, bodyPSensorWithBias) { auto p = testing::Params(); p->n_gravity = Vector3(0, 0, -kGravity); - p->body_P_sensor = Pose3(Rot3::Ypr(0, 0, M_PI), Point3()); + p->body_P_sensor = Pose3(Rot3::Ypr(0, 0, M_PI), Point3(0,0,0)); p->accelerometerCovariance = 1e-7 * I_3x3; p->gyroscopeCovariance = 1e-8 * I_3x3; p->integrationCovariance = 1e-9 * I_3x3; diff --git a/gtsam/navigation/tests/testImuFactorSerialization.cpp b/gtsam/navigation/tests/testImuFactorSerialization.cpp index a7796d1b2..9f9781d2c 100644 --- a/gtsam/navigation/tests/testImuFactorSerialization.cpp +++ b/gtsam/navigation/tests/testImuFactorSerialization.cpp @@ -43,7 +43,7 @@ TEST(ImuFactor, serialization) { // Create default parameters with Z-down and above noise paramaters auto p = PreintegrationParams::MakeSharedD(9.81); - p->body_P_sensor = Pose3(Rot3::Ypr(0, 0, M_PI), Point3()); + p->body_P_sensor = Pose3(Rot3::Ypr(0, 0, M_PI), Point3(0,0,0)); p->accelerometerCovariance = 1e-7 * I_3x3; p->gyroscopeCovariance = 1e-8 * I_3x3; p->integrationCovariance = 1e-9 * I_3x3; diff --git a/gtsam/slam/tests/testInitializePose3.cpp b/gtsam/slam/tests/testInitializePose3.cpp index 9fd8474eb..e6f08286a 100644 --- a/gtsam/slam/tests/testInitializePose3.cpp +++ b/gtsam/slam/tests/testInitializePose3.cpp @@ -148,9 +148,9 @@ TEST( InitializePose3, iterationGradient ) { Rot3 Rpert = Rot3::Expmap(Vector3(0.01, 0.01, 0.01)); Values givenPoses; givenPoses.insert(x0,simple::pose0); - givenPoses.insert(x1,(simple::pose0).compose( Pose3(Rpert,Point3()) )); - givenPoses.insert(x2, (simple::pose0).compose( Pose3(Rpert.inverse(),Point3()) )); - givenPoses.insert(x3, (simple::pose0).compose( Pose3(Rpert,Point3()) )); + givenPoses.insert(x1,(simple::pose0).compose( Pose3(Rpert,Point3(0,0,0)) )); + givenPoses.insert(x2, (simple::pose0).compose( Pose3(Rpert.inverse(),Point3(0,0,0)) )); + givenPoses.insert(x3, (simple::pose0).compose( Pose3(Rpert,Point3(0,0,0)) )); size_t maxIter = 1; // test gradient at the first iteration bool setRefFrame = false; @@ -189,9 +189,9 @@ TEST( InitializePose3, orientationsGradient ) { Rot3 Rpert = Rot3::Expmap(Vector3(0.01, 0.01, 0.01)); Values givenPoses; givenPoses.insert(x0,simple::pose0); - givenPoses.insert(x1,(simple::pose0).compose( Pose3(Rpert,Point3()) )); - givenPoses.insert(x2, (simple::pose0).compose( Pose3(Rpert.inverse(),Point3()) )); - givenPoses.insert(x3, (simple::pose0).compose( Pose3(Rpert,Point3()) )); + givenPoses.insert(x1,(simple::pose0).compose( Pose3(Rpert,Point3(0,0,0)) )); + givenPoses.insert(x2, (simple::pose0).compose( Pose3(Rpert.inverse(),Point3(0,0,0)) )); + givenPoses.insert(x3, (simple::pose0).compose( Pose3(Rpert,Point3(0,0,0)) )); // do 10 gradient iterations bool setRefFrame = false; Values orientations = InitializePose3::computeOrientationsGradient(pose3Graph, givenPoses, 10, setRefFrame); diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index 1c1bc3c03..5430e1088 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -959,8 +959,8 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac views.push_back(x3); // Two different cameras, at the same position, but different rotations - Pose3 pose2 = level_pose * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3()); - Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3()); + Pose3 pose2 = level_pose * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); + Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); Camera cam2(pose2, sharedK2); Camera cam3(pose3, sharedK2); diff --git a/gtsam_unstable/dynamics/tests/testPoseRTV.cpp b/gtsam_unstable/dynamics/tests/testPoseRTV.cpp index db2f8f7f8..855131042 100644 --- a/gtsam_unstable/dynamics/tests/testPoseRTV.cpp +++ b/gtsam_unstable/dynamics/tests/testPoseRTV.cpp @@ -29,7 +29,7 @@ TEST( testPoseRTV, constructors ) { EXPECT(assert_equal(Pose3(rot, pt), state1.pose())); PoseRTV state2; - EXPECT(assert_equal(Point3(), state2.t())); + EXPECT(assert_equal(Point3(0,0,0), state2.t())); EXPECT(assert_equal(Rot3(), state2.R())); EXPECT(assert_equal(kZero3, state2.v())); EXPECT(assert_equal(Pose3(), state2.pose())); diff --git a/gtsam_unstable/geometry/tests/testEvent.cpp b/gtsam_unstable/geometry/tests/testEvent.cpp index 0842e2146..ec8ca1f4b 100644 --- a/gtsam_unstable/geometry/tests/testEvent.cpp +++ b/gtsam_unstable/geometry/tests/testEvent.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -34,7 +34,7 @@ static SharedNoiseModel model(noiseModel::Isotropic::Sigma(1,0.5*ms)); static const double timeOfEvent = 25; static const Event exampleEvent(timeOfEvent, 1, 0, 0); -static const Point3 microphoneAt0; +static const Point3 microphoneAt0(0,0,0); //***************************************************************************** TEST( Event, Constructor ) { diff --git a/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp b/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp index 3aa758701..96043fb50 100644 --- a/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp +++ b/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp @@ -48,7 +48,7 @@ TEST( InvDepthFactorVariant1, optimize) { Vector6 expected((Vector(6) << x, y, z, theta, phi, rho).finished()); - + // Create a factor graph with two inverse depth factors and two pose priors Key poseKey1(1); Key poseKey2(2); @@ -89,14 +89,14 @@ TEST( InvDepthFactorVariant1, optimize) { // cout << endl << endl; // Calculate world coordinates of landmark versions - Point3 world_landmarkBefore; + Point3 world_landmarkBefore(0,0,0); { Vector6 landmarkBefore = values.at(landmarkKey); double x = landmarkBefore(0), y = landmarkBefore(1), z = landmarkBefore(2); double theta = landmarkBefore(3), phi = landmarkBefore(4), rho = landmarkBefore(5); world_landmarkBefore = Point3(x, y, z) + Point3(cos(theta)*cos(phi)/rho, sin(theta)*cos(phi)/rho, sin(phi)/rho); } - Point3 world_landmarkAfter; + Point3 world_landmarkAfter(0,0,0); { Vector6 landmarkAfter = result.at(landmarkKey); double x = landmarkAfter(0), y = landmarkAfter(1), z = landmarkAfter(2); diff --git a/gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp b/gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp index d20fc000c..7ac0faa1e 100644 --- a/gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp +++ b/gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp @@ -46,7 +46,7 @@ TEST( InvDepthFactorVariant2, optimize) { Vector3 expected((Vector(3) << theta, phi, rho).finished()); - + // Create a factor graph with two inverse depth factors and two pose priors Key poseKey1(1); Key poseKey2(2); @@ -72,7 +72,7 @@ TEST( InvDepthFactorVariant2, optimize) { LevenbergMarquardtParams params; Values result = LevenbergMarquardtOptimizer(graph, values, params).optimize(); Vector3 actual = result.at(landmarkKey); - + // values.at(poseKey1).print("Pose1 Before:\n"); // result.at(poseKey1).print("Pose1 After:\n"); // pose1.print("Pose1 Truth:\n"); @@ -87,13 +87,13 @@ TEST( InvDepthFactorVariant2, optimize) { // std::cout << std::endl << std::endl; // Calculate world coordinates of landmark versions - Point3 world_landmarkBefore; + Point3 world_landmarkBefore(0,0,0); { Vector3 landmarkBefore = values.at(landmarkKey); double theta = landmarkBefore(0), phi = landmarkBefore(1), rho = landmarkBefore(2); world_landmarkBefore = referencePoint + Point3(cos(theta)*cos(phi)/rho, sin(theta)*cos(phi)/rho, sin(phi)/rho); } - Point3 world_landmarkAfter; + Point3 world_landmarkAfter(0,0,0); { Vector3 landmarkAfter = result.at(landmarkKey); double theta = landmarkAfter(0), phi = landmarkAfter(1), rho = landmarkAfter(2); diff --git a/tests/testNonlinearEquality.cpp b/tests/testNonlinearEquality.cpp index a3c99ece7..130dc1bc8 100644 --- a/tests/testNonlinearEquality.cpp +++ b/tests/testNonlinearEquality.cpp @@ -531,7 +531,7 @@ TEST (testNonlinearEqualityConstraint, stereo_constrained ) { // create initial estimates Rot3 faceDownY( (Matrix) (Matrix(3, 3) << 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, -1.0, 0.0).finished()); - Pose3 pose1(faceDownY, Point3()); // origin, left camera + Pose3 pose1(faceDownY, Point3(0,0,0)); // origin, left camera SimpleCamera camera1(pose1, K); Pose3 pose2(faceDownY, Point3(2.0, 0.0, 0.0)); // 2 units to the left SimpleCamera camera2(pose2, K); From 2060b09a2b391079d4a88975375dcdced5e11510 Mon Sep 17 00:00:00 2001 From: Frank Date: Thu, 11 Feb 2016 19:03:11 -0800 Subject: [PATCH 18/90] Avoid calling default constructors and/or vector --- gtsam/geometry/Point3.h | 11 +++++++++-- gtsam/slam/dataset.cpp | 6 ++++-- gtsam_unstable/dynamics/PoseRTV.cpp | 10 +++++----- gtsam_unstable/dynamics/PoseRTV.h | 4 ++-- gtsam_unstable/geometry/BearingS2.cpp | 4 ++-- gtsam_unstable/geometry/Event.h | 7 +++---- gtsam_unstable/geometry/Similarity3.cpp | 15 +++++++-------- gtsam_unstable/slam/SmartStereoProjectionFactor.h | 8 ++++---- wrap/Class.cpp | 2 +- wrap/tests/expected/geometry_wrapper.cpp | 2 +- 10 files changed, 38 insertions(+), 31 deletions(-) diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index ba06e1d85..0cf771844 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -52,7 +52,9 @@ class GTSAM_EXPORT Point3 : public Vector3 { #ifndef GTSAM_ALLOW_DEPRECATED_SINCE_V4 /// Default constructor now creates *uninitialized* point !!!! - Point3() {} + Point3() { + throw std::runtime_error("Default constructor called!"); + } #endif @@ -106,9 +108,14 @@ class GTSAM_EXPORT Point3 : public Vector3 { /// @name Standard Interface /// @{ - /** return vectorized form (column-wise)*/ + /// return as Vector3 const Vector3& vector() const { return *this; } + /// return as transposed vector + Eigen::DenseBase::ConstTransposeReturnType transpose() const { + return this->Vector3::transpose(); + } + /// @} /// Output stream operator diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index c79fb5823..d037f7535 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -779,7 +779,7 @@ bool writeBAL(const string& filename, SfM_data &data) { os << endl; for (size_t j = 0; j < data.number_tracks(); j++) { // for each 3D point j - SfM_Track track = data.tracks[j]; + const SfM_Track& track = data.tracks[j]; for (size_t k = 0; k < track.number_measurements(); k++) { // for each observation of the 3D point j size_t i = track.measurements[k].first; // camera id @@ -808,7 +808,9 @@ bool writeBAL(const string& filename, SfM_data &data) { Cal3Bundler cameraCalibration = data.cameras[i].calibration(); Pose3 poseOpenGL = gtsam2openGL(poseGTSAM); os << Rot3::Logmap(poseOpenGL.rotation()) << endl; - os << poseOpenGL.translation() << endl; + os << poseOpenGL.translation().x() << endl; + os << poseOpenGL.translation().y() << endl; + os << poseOpenGL.translation().z() << endl; os << cameraCalibration.fx() << endl; os << cameraCalibration.k1() << endl; os << cameraCalibration.k2() << endl; diff --git a/gtsam_unstable/dynamics/PoseRTV.cpp b/gtsam_unstable/dynamics/PoseRTV.cpp index b3953dd23..b6fc61411 100644 --- a/gtsam_unstable/dynamics/PoseRTV.cpp +++ b/gtsam_unstable/dynamics/PoseRTV.cpp @@ -37,7 +37,7 @@ PoseRTV::PoseRTV(const Vector& rtv) : Vector PoseRTV::vector() const { Vector rtv(9); rtv.head(3) = rotation().xyz(); - rtv.segment(3,3) = translation().vector(); + rtv.segment(3,3) = translation(); rtv.tail(3) = velocity(); return rtv; } @@ -52,7 +52,7 @@ bool PoseRTV::equals(const PoseRTV& other, double tol) const { void PoseRTV::print(const string& s) const { cout << s << ":" << endl; gtsam::print((Vector)R().xyz(), " R:rpy"); - t().print(" T"); + cout << " T" << t().transpose() << endl; gtsam::print((Vector)velocity(), " V"); } @@ -103,7 +103,7 @@ PoseRTV PoseRTV::flyingDynamics( Rot3 yaw_correction_bn = Rot3::Yaw(yaw2); Point3 forward(forward_accel, 0.0, 0.0); Vector Acc_n = - yaw_correction_bn.rotate(forward).vector() // applies locally forward force in the global frame + yaw_correction_bn.rotate(forward) // applies locally forward force in the global frame - drag * (Vector(3) << v1.x(), v1.y(), 0.0).finished() // drag term dependent on v1 + delta(3, 2, loss_lift - lift_control); // falling due to lift lost from pitch @@ -172,7 +172,7 @@ double PoseRTV::range(const PoseRTV& other, const Point3 t1 = pose().translation(H1 ? &D_t1_pose : 0); const Point3 t2 = other.pose().translation(H2 ? &D_t2_other : 0); Matrix13 D_d_t1, D_d_t2; - double d = t1.distance(t2, H1 ? &D_d_t1 : 0, H2 ? &D_d_t2 : 0); + double d = distance(t1, t2, H1 ? &D_d_t1 : 0, H2 ? &D_d_t2 : 0); if (H1) *H1 << D_d_t1 * D_t1_pose, 0,0,0; if (H2) *H2 << D_d_t2 * D_t2_other, 0,0,0; return d; @@ -188,7 +188,7 @@ PoseRTV PoseRTV::transformed_from(const Pose3& trans, ChartJacobian Dglobal, // Note that we rotate the velocity Matrix3 D_newvel_R, D_newvel_v; - Velocity3 newvel = trans.rotation().rotate(Point3(velocity()), D_newvel_R, D_newvel_v).vector(); + Velocity3 newvel = trans.rotation().rotate(Point3(velocity()), D_newvel_R, D_newvel_v); if (Dglobal) { Dglobal->setZero(); diff --git a/gtsam_unstable/dynamics/PoseRTV.h b/gtsam_unstable/dynamics/PoseRTV.h index b59ea4614..bf92ab9c4 100644 --- a/gtsam_unstable/dynamics/PoseRTV.h +++ b/gtsam_unstable/dynamics/PoseRTV.h @@ -66,7 +66,7 @@ public: // Access to vector for ease of use with Matlab // and avoidance of Point3 Vector vector() const; - Vector translationVec() const { return pose().translation().vector(); } + Vector translationVec() const { return pose().translation(); } const Velocity3& velocityVec() const { return velocity(); } // testable @@ -126,7 +126,7 @@ public: /// @return a vector for Matlab compatibility inline Vector translationIntegrationVec(const PoseRTV& x2, double dt) const { - return translationIntegration(x2, dt).vector(); + return translationIntegration(x2, dt); } /** diff --git a/gtsam_unstable/geometry/BearingS2.cpp b/gtsam_unstable/geometry/BearingS2.cpp index 71278789c..9dfed612c 100644 --- a/gtsam_unstable/geometry/BearingS2.cpp +++ b/gtsam_unstable/geometry/BearingS2.cpp @@ -34,7 +34,7 @@ BearingS2 BearingS2::fromDownwardsObservation(const Pose3& A, const Point3& B) { 0.,1.,0., -1.,0.,0.).finished(); // p_rel_c = Cbc*Cnb*(PosObj - Pos); - Vector p_rel_c = Cbc*Cnb*(B.vector() - A.translation().vector()); + Vector p_rel_c = Cbc*Cnb*(B - A.translation()); // FIXME: the matlab code checks for p_rel_c(0) greater than @@ -50,7 +50,7 @@ BearingS2 BearingS2::fromForwardObservation(const Pose3& A, const Point3& B) { // Cnb = DCMnb(Att); Matrix Cnb = A.rotation().matrix().transpose(); - Vector p_rel_c = Cnb*(B.vector() - A.translation().vector()); + Vector p_rel_c = Cnb*(B - A.translation()); // FIXME: the matlab code checks for p_rel_c(0) greater than diff --git a/gtsam_unstable/geometry/Event.h b/gtsam_unstable/geometry/Event.h index 77b02d9fb..615b1d467 100644 --- a/gtsam_unstable/geometry/Event.h +++ b/gtsam_unstable/geometry/Event.h @@ -60,14 +60,13 @@ public: /** print with optional string */ void print(const std::string& s = "") const { - std::cout << s << "time = " << time_; - location_.print("; location"); + std::cout << s << "time = " << time_ << "location = " << location_.transpose(); } /** equals with an tolerance */ bool equals(const Event& other, double tol = 1e-9) const { return std::abs(time_ - other.time_) < tol - && location_.equals(other.location_, tol); + && traits::Equals(location_, other.location_, tol); } /// Updates a with tangent space delta @@ -86,7 +85,7 @@ public: OptionalJacobian<1, 3> H2 = boost::none) const { static const double Speed = 330; Matrix13 D1, D2; - double distance = location_.distance(microphone, D1, D2); + double distance = gtsam::distance(location_, microphone, D1, D2); if (H1) // derivative of toa with respect to event *H1 << 1.0, D1 / Speed; diff --git a/gtsam_unstable/geometry/Similarity3.cpp b/gtsam_unstable/geometry/Similarity3.cpp index 516e1fb5b..9768f4fa4 100644 --- a/gtsam_unstable/geometry/Similarity3.cpp +++ b/gtsam_unstable/geometry/Similarity3.cpp @@ -43,7 +43,7 @@ Similarity3::Similarity3(const Matrix4& T) : } bool Similarity3::equals(const Similarity3& other, double tol) const { - return R_.equals(other.R_, tol) && t_.equals(other.t_, tol) + return R_.equals(other.R_, tol) && traits::Equals(t_, other.t_, tol) && s_ < (other.s_ + tol) && s_ > (other.s_ - tol); } @@ -55,8 +55,7 @@ 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; + std::cout << "t: " << translation().transpose() << "s: " << scale() << std::endl; } Similarity3 Similarity3::identity() { @@ -79,7 +78,7 @@ Point3 Similarity3::transform_from(const Point3& p, // // For this derivative, see LieGroups.pdf const Matrix3 sR = s_ * R_.matrix(); const Matrix3 DR = sR * skewSymmetric(-p.x(), -p.y(), -p.z()); - *H1 << DR, sR, sR * p.vector(); + *H1 << DR, sR, sR * p; } if (H2) *H2 = s_ * R_.matrix(); // just 3*3 sub-block of matrix() @@ -103,7 +102,7 @@ Matrix4 Similarity3::wedge(const Vector7& xi) { Matrix7 Similarity3::AdjointMap() const { // http://www.ethaneade.org/latex2html/lie/node30.html const Matrix3 R = R_.matrix(); - const Vector3 t = t_.vector(); + const Vector3 t = t_; const Matrix3 A = s_ * skewSymmetric(t) * R; Matrix7 adj; adj << R, Z_3x3, Matrix31::Zero(), // 3*7 @@ -153,7 +152,7 @@ Vector7 Similarity3::Logmap(const Similarity3& T, OptionalJacobian<7, 7> Hm) { const Vector3 w = Rot3::Logmap(T.R_); const double lambda = log(T.s_); Vector7 result; - result << w, GetV(w, lambda).inverse() * T.t_.vector(), lambda; + result << w, GetV(w, lambda).inverse() * T.t_, lambda; if (Hm) { throw std::runtime_error("Similarity3::Logmap: derivative not implemented"); } @@ -173,13 +172,13 @@ Similarity3 Similarity3::Expmap(const Vector7& v, OptionalJacobian<7, 7> Hm) { std::ostream &operator<<(std::ostream &os, const Similarity3& p) { os << "[" << p.rotation().xyz().transpose() << " " - << p.translation().vector().transpose() << " " << p.scale() << "]\';"; + << p.translation().transpose() << " " << p.scale() << "]\';"; return os; } const Matrix4 Similarity3::matrix() const { Matrix4 T; - T.topRows<3>() << R_.matrix(), t_.vector(); + T.topRows<3>() << R_.matrix(), t_; T.bottomRows<1>() << 0, 0, 0, 1.0 / s_; return T; } diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index 6651c005f..7cbeaae65 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -1,12 +1,12 @@ /* ---------------------------------------------------------------------------- - + * 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 - + * -------------------------------------------------------------------------- */ /** @@ -253,7 +253,7 @@ public: reprojections.push_back(cameras[i].backproject(measured_[i])); } - Point3 pw_sum; + Point3 pw_sum(0,0,0); BOOST_FOREACH(const Point3& pw, reprojections) { pw_sum = pw_sum + pw; } diff --git a/wrap/Class.cpp b/wrap/Class.cpp index e62e31bc1..d7a3b6ee4 100644 --- a/wrap/Class.cpp +++ b/wrap/Class.cpp @@ -567,7 +567,7 @@ void Class::deserialization_fragments(FileWriter& proxyFile, // string serialized = unwrap< string >(in[0]); // istringstream in_archive_stream(serialized); // boost::archive::text_iarchive in_archive(in_archive_stream); - // Shared output(new Point3()); + // Shared output(new Point3(0,0,0)); // in_archive >> *output; // out[0] = wrap_shared_ptr(output,"Point3", false); //} diff --git a/wrap/tests/expected/geometry_wrapper.cpp b/wrap/tests/expected/geometry_wrapper.cpp index 226030a0d..6646394f6 100644 --- a/wrap/tests/expected/geometry_wrapper.cpp +++ b/wrap/tests/expected/geometry_wrapper.cpp @@ -326,7 +326,7 @@ void gtsamPoint3_string_deserialize_19(int nargout, mxArray *out[], int nargin, string serialized = unwrap< string >(in[0]); istringstream in_archive_stream(serialized); boost::archive::text_iarchive in_archive(in_archive_stream); - Shared output(new gtsam::Point3()); + Shared output(new gtsam::Point3(0,0,0)); in_archive >> *output; out[0] = wrap_shared_ptr(output,"gtsam.Point3", false); } From 26d74a1f6e3133d923243bd26358868e837ca120 Mon Sep 17 00:00:00 2001 From: Frank Date: Thu, 11 Feb 2016 19:04:36 -0800 Subject: [PATCH 19/90] Modified ExpressionNode so avoids calling constructors altogether --- gtsam/nonlinear/internal/ExpressionNode.h | 84 ++++++++++++----------- 1 file changed, 43 insertions(+), 41 deletions(-) diff --git a/gtsam/nonlinear/internal/ExpressionNode.h b/gtsam/nonlinear/internal/ExpressionNode.h index 928387eb9..d2a130830 100644 --- a/gtsam/nonlinear/internal/ExpressionNode.h +++ b/gtsam/nonlinear/internal/ExpressionNode.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -269,9 +269,13 @@ public: // Inner Record Class struct Record: public CallRecordImplementor::dimension> { - A1 value1; - ExecutionTrace trace1; typename Jacobian::type dTdA1; + ExecutionTrace trace1; + A1 value1; + + /// Construct record by calling argument expression + Record(const Values& values, const ExpressionNode& expression1, ExecutionTraceStorage* ptr) + : value1(expression1.traceExecution(values, trace1, ptr + upAligned(sizeof(Record)))) {} /// Print to std::cout void print(const std::string& indent) const { @@ -305,20 +309,15 @@ public: ExecutionTraceStorage* ptr) const { assert(reinterpret_cast(ptr) % TraceAlignment == 0); - // Create the record at the start of the traceStorage and advance the pointer - Record* record = new (ptr) Record(); - ptr += upAligned(sizeof(Record)); - - // Record the traces for all arguments - // After this, the traceStorage pointer is set to after what was written + // Create a Record in the memory pointed to by ptr + // Calling the construct will record the traces for all arguments // Write an Expression execution trace in record->trace // Iff Constant or Leaf, this will not write to traceStorage, only to trace. // Iff the expression is functional, write all Records in traceStorage buffer // Return value of type T is recorded in record->value - record->value1 = expression1_->traceExecution(values, record->trace1, ptr); + Record* record = new (ptr) Record(values, *expression1_, ptr); - // We have written in the buffer, the next caller expects we advance the pointer - ptr += expression1_->traceSize(); + // Our trace parameter is set to point to the Record trace.setFunction(record); // Finally, the function call fills in the Jacobian dTdA1 @@ -384,14 +383,21 @@ public: // Inner Record Class struct Record: public CallRecordImplementor::dimension> { - A1 value1; - ExecutionTrace trace1; typename Jacobian::type dTdA1; - - A2 value2; - ExecutionTrace trace2; typename Jacobian::type dTdA2; + ExecutionTrace trace1; + ExecutionTrace trace2; + + A1 value1; + A2 value2; + + /// Construct record by calling argument expressions + Record(const Values& values, const ExpressionNode& expression1, + const ExpressionNode& expression2, ExecutionTraceStorage* ptr) + : value1(expression1.traceExecution(values, trace1, ptr += upAligned(sizeof(Record)))), + value2(expression2.traceExecution(values, trace2, ptr += expression1.traceSize())) {} + /// Print to std::cout void print(const std::string& indent) const { std::cout << indent << "BinaryExpression::Record {" << std::endl; @@ -418,12 +424,7 @@ public: virtual T traceExecution(const Values& values, ExecutionTrace& trace, ExecutionTraceStorage* ptr) const { assert(reinterpret_cast(ptr) % TraceAlignment == 0); - Record* record = new (ptr) Record(); - ptr += upAligned(sizeof(Record)); - record->value1 = expression1_->traceExecution(values, record->trace1, ptr); - ptr += expression1_->traceSize(); - record->value2 = expression2_->traceExecution(values, record->trace2, ptr); - ptr += expression2_->traceSize(); + Record* record = new (ptr) Record(values, *expression1_, *expression2_, ptr); trace.setFunction(record); return function_(record->value1, record->value2, record->dTdA1, record->dTdA2); } @@ -492,18 +493,26 @@ public: // Inner Record Class struct Record: public CallRecordImplementor::dimension> { - A1 value1; - ExecutionTrace trace1; typename Jacobian::type dTdA1; - - A2 value2; - ExecutionTrace trace2; typename Jacobian::type dTdA2; - - A3 value3; - ExecutionTrace trace3; typename Jacobian::type dTdA3; + ExecutionTrace trace1; + ExecutionTrace trace2; + ExecutionTrace trace3; + + A1 value1; + A2 value2; + A3 value3; + + /// Construct record by calling 3 argument expressions + Record(const Values& values, const ExpressionNode& expression1, + const ExpressionNode& expression2, + const ExpressionNode& expression3, ExecutionTraceStorage* ptr) + : value1(expression1.traceExecution(values, trace1, ptr += upAligned(sizeof(Record)))), + value2(expression2.traceExecution(values, trace2, ptr += expression1.traceSize())), + value3(expression3.traceExecution(values, trace3, ptr += expression2.traceSize())) {} + /// Print to std::cout void print(const std::string& indent) const { std::cout << indent << "TernaryExpression::Record {" << std::endl; @@ -531,19 +540,12 @@ public: /// Construct an execution trace for reverse AD, see UnaryExpression for explanation virtual T traceExecution(const Values& values, ExecutionTrace& trace, - ExecutionTraceStorage* ptr) const { + ExecutionTraceStorage* ptr) const { assert(reinterpret_cast(ptr) % TraceAlignment == 0); - Record* record = new (ptr) Record(); - ptr += upAligned(sizeof(Record)); - record->value1 = expression1_->traceExecution(values, record->trace1, ptr); - ptr += expression1_->traceSize(); - record->value2 = expression2_->traceExecution(values, record->trace2, ptr); - ptr += expression2_->traceSize(); - record->value3 = expression3_->traceExecution(values, record->trace3, ptr); - ptr += expression3_->traceSize(); + Record* record = new (ptr) Record(values, *expression1_, *expression2_, *expression3_, ptr); trace.setFunction(record); return function_(record->value1, record->value2, record->value3, - record->dTdA1, record->dTdA2, record->dTdA3); + record->dTdA1, record->dTdA2, record->dTdA3); } }; From 5c3dd7914b7ab93e6c70ff1cc282f2fabfed0c3f Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 11 Feb 2016 22:36:03 -0800 Subject: [PATCH 20/90] Removed checks on sizeof(Record) as non-portable. --- tests/testExpressionFactor.cpp | 28 ---------------------------- 1 file changed, 28 deletions(-) diff --git a/tests/testExpressionFactor.cpp b/tests/testExpressionFactor.cpp index bef69edb6..57f03cca6 100644 --- a/tests/testExpressionFactor.cpp +++ b/tests/testExpressionFactor.cpp @@ -186,25 +186,8 @@ TEST(ExpressionFactor, Binary) { values.insert(1, Cal3_S2()); values.insert(2, Point2(0, 0)); - // traceRaw will fill raw with [Trace | Binary::Record] - EXPECT_LONGS_EQUAL(8, sizeof(double)); - EXPECT_LONGS_EQUAL(16, sizeof(Point2)); - EXPECT_LONGS_EQUAL(40, sizeof(Cal3_S2)); - EXPECT_LONGS_EQUAL(16, sizeof(internal::ExecutionTrace)); - EXPECT_LONGS_EQUAL(16, sizeof(internal::ExecutionTrace)); - EXPECT_LONGS_EQUAL(2*5*8, sizeof(internal::Jacobian::type)); - EXPECT_LONGS_EQUAL(2*2*8, sizeof(internal::Jacobian::type)); - size_t expectedRecordSize = sizeof(Cal3_S2) - + sizeof(internal::ExecutionTrace) - + +sizeof(internal::Jacobian::type) + sizeof(Point2) - + sizeof(internal::ExecutionTrace) - + sizeof(internal::Jacobian::type); - EXPECT_LONGS_EQUAL(expectedRecordSize + 8, sizeof(Binary::Record)); - // Check size size_t size = binary.traceSize(); - CHECK(size); - EXPECT_LONGS_EQUAL(expectedRecordSize + 8, size); // Use Variable Length Array, allocated on stack by gcc // Note unclear for Clang: http://clang.llvm.org/compatibility.html#vla internal::ExecutionTraceStorage traceStorage[size]; @@ -261,18 +244,7 @@ TEST(ExpressionFactor, Shallow) { // traceExecution of shallow tree typedef internal::UnaryExpression Unary; typedef internal::BinaryExpression Binary; - size_t expectedTraceSize = sizeof(Unary::Record) + sizeof(Binary::Record); - EXPECT_LONGS_EQUAL(96, sizeof(Unary::Record)); -#ifdef GTSAM_USE_QUATERNIONS - EXPECT_LONGS_EQUAL(352, sizeof(Binary::Record)); - LONGS_EQUAL(96+352, expectedTraceSize); -#else - EXPECT_LONGS_EQUAL(384, sizeof(Binary::Record)); - LONGS_EQUAL(96+384, expectedTraceSize); -#endif size_t size = expression.traceSize(); - CHECK(size); - EXPECT_LONGS_EQUAL(expectedTraceSize, size); internal::ExecutionTraceStorage traceStorage[size]; internal::ExecutionTrace trace; Point2 value = expression.traceExecution(values, trace, traceStorage); From 7fd838611e951e01a8e28ce1ee4aa4146d7c3763 Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 11 Feb 2016 23:27:09 -0800 Subject: [PATCH 21/90] Fixed typo --- wrap/tests/expected/geometry_wrapper.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/wrap/tests/expected/geometry_wrapper.cpp b/wrap/tests/expected/geometry_wrapper.cpp index 6646394f6..226030a0d 100644 --- a/wrap/tests/expected/geometry_wrapper.cpp +++ b/wrap/tests/expected/geometry_wrapper.cpp @@ -326,7 +326,7 @@ void gtsamPoint3_string_deserialize_19(int nargout, mxArray *out[], int nargin, string serialized = unwrap< string >(in[0]); istringstream in_archive_stream(serialized); boost::archive::text_iarchive in_archive(in_archive_stream); - Shared output(new gtsam::Point3(0,0,0)); + Shared output(new gtsam::Point3()); in_archive >> *output; out[0] = wrap_shared_ptr(output,"gtsam.Point3", false); } From 56dbf487ee70b169a75829448224d9e4757f1944 Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 11 Feb 2016 23:27:58 -0800 Subject: [PATCH 22/90] Fixed more default constructor calls --- gtsam/navigation/MagFactor.h | 10 +++++----- gtsam/navigation/tests/testScenarios.cpp | 4 ++-- gtsam/slam/EssentialMatrixFactor.h | 13 ++++--------- gtsam/slam/tests/testGeneralSFMFactor.cpp | 4 ++-- .../slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp | 2 +- .../slam/tests/testSmartProjectionCameraFactor.cpp | 13 ++++++------- gtsam_unstable/dynamics/VelocityConstraint.h | 2 +- gtsam_unstable/slam/tests/testTOAFactor.cpp | 2 +- 8 files changed, 22 insertions(+), 28 deletions(-) diff --git a/gtsam/navigation/MagFactor.h b/gtsam/navigation/MagFactor.h index fc1e69190..bee3e8944 100644 --- a/gtsam/navigation/MagFactor.h +++ b/gtsam/navigation/MagFactor.h @@ -76,7 +76,7 @@ public: boost::optional H = boost::none) const { // measured bM = nRb� * nM + b Point3 hx = unrotate(nRb, nM_, H) + bias_; - return (hx - measured_).vector(); + return (hx - measured_); } }; @@ -114,7 +114,7 @@ public: boost::optional H = boost::none) const { // measured bM = nRb� * nM + b Point3 hx = nRb.unrotate(nM_, H, boost::none) + bias_; - return (hx - measured_).vector(); + return (hx - measured_); } }; @@ -155,7 +155,7 @@ public: Point3 hx = bRn_.rotate(nM, boost::none, H1) + bias; if (H2) *H2 = eye(3); - return (hx - measured_).vector(); + return (hx - measured_); } }; @@ -197,7 +197,7 @@ public: Unit3 rotated = bRn_.rotate(direction, boost::none, H2); Point3 hx = scale * rotated.point3() + bias; if (H1) - *H1 = rotated.point3().vector(); + *H1 = rotated.point3(); if (H2) // H2 is 2*2, but we need 3*2 { Matrix H; @@ -206,7 +206,7 @@ public: } if (H3) *H3 = eye(3); - return (hx - measured_).vector(); + return (hx - measured_); } }; diff --git a/gtsam/navigation/tests/testScenarios.cpp b/gtsam/navigation/tests/testScenarios.cpp index c2fdb963d..b5d312a86 100644 --- a/gtsam/navigation/tests/testScenarios.cpp +++ b/gtsam/navigation/tests/testScenarios.cpp @@ -140,7 +140,7 @@ TEST(ScenarioRunner, Loop) { /* ************************************************************************* */ namespace initial { const Rot3 nRb; -const Point3 P0; +const Point3 P0(0,0,0); const Vector3 V0(0, 0, 0); } @@ -259,7 +259,7 @@ namespace initial3 { // Rotation only // Set up body pointing towards y axis. The body itself has Z axis pointing down const Rot3 nRb(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1)); -const Point3 P0; +const Point3 P0(0,0,0); const Vector3 V0(0, 0, 0); } diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h index da22225e5..e9a117745 100644 --- a/gtsam/slam/EssentialMatrixFactor.h +++ b/gtsam/slam/EssentialMatrixFactor.h @@ -19,7 +19,7 @@ namespace gtsam { */ class EssentialMatrixFactor: public NoiseModelFactor1 { - Vector vA_, vB_; ///< Homogeneous versions, in ideal coordinates + Vector3 vA_, vB_; ///< Homogeneous versions, in ideal coordinates typedef NoiseModelFactor1 Base; typedef EssentialMatrixFactor This; @@ -107,9 +107,7 @@ public: */ EssentialMatrixFactor2(Key key1, Key key2, const Point2& pA, const Point2& pB, const SharedNoiseModel& model) : - Base(model, key1, key2) { - dP1_ = Point3(pA.x(), pA.y(), 1); - pn_ = pB; + Base(model, key1, key2), dP1_(EssentialMatrix::Homogeneous(pA)), pn_(pB) { f_ = 1.0; } @@ -125,11 +123,8 @@ public: template EssentialMatrixFactor2(Key key1, Key key2, const Point2& pA, const Point2& pB, const SharedNoiseModel& model, boost::shared_ptr K) : - Base(model, key1, key2) { - assert(K); - Point2 p1 = K->calibrate(pA); - dP1_ = Point3(p1.x(), p1.y(), 1); // d*P1 = (x,y,1) - pn_ = K->calibrate(pB); + Base(model, key1, key2), dP1_( + EssentialMatrix::Homogeneous(K->calibrate(pA))), pn_(K->calibrate(pB)) { f_ = 0.5 * (K->fx() + K->fy()); } diff --git a/gtsam/slam/tests/testGeneralSFMFactor.cpp b/gtsam/slam/tests/testGeneralSFMFactor.cpp index 07abb557f..48e2e8b2f 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor.cpp @@ -154,7 +154,7 @@ TEST( GeneralSFMFactor, error ) { Point3 t1(0, 0, -6); Pose3 x1(R, t1); values.insert(X(1), GeneralCamera(x1)); - Point3 l1; + Point3 l1(0,0,0); values.insert(L(1), l1); EXPECT( assert_equal(((Vector ) Vector2(-3., 0.)), @@ -451,7 +451,7 @@ TEST(GeneralSFMFactor, BinaryJacobianFactor) { Point3 t1(0, 0, -6); Pose3 x1(R, t1); values.insert(X(1), GeneralCamera(x1)); - Point3 l1; + Point3 l1(0,0,0); values.insert(L(1), l1); vector models; diff --git a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp index a349a4992..9fda7d745 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp @@ -107,7 +107,7 @@ TEST( GeneralSFMFactor_Cal3Bundler, error ) { Point3 t1(0, 0, -6); Pose3 x1(R, t1); values.insert(X(1), GeneralCamera(x1)); - Point3 l1; + Point3 l1(0,0,0); values.insert(L(1), l1); EXPECT(assert_equal(Vector2(-3., 0.), factor->unwhitenedError(values))); } diff --git a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp index 467aefe91..e441c37ff 100644 --- a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp @@ -610,11 +610,11 @@ TEST( SmartProjectionCameraFactor, noiselessBundler ) { double expectedError = 0.0; DOUBLES_EQUAL(expectedError, actualError, 1e-3); - Point3 oldPoint; // this takes the point stored in the factor (we are not interested in this) + Point3 oldPoint(0,0,0); // this takes the point stored in the factor (we are not interested in this) if (factor1->point()) oldPoint = *(factor1->point()); - Point3 expectedPoint; + Point3 expectedPoint(0,0,0); if (factor1->point(values)) expectedPoint = *(factor1->point(values)); @@ -636,10 +636,9 @@ TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor ) { smartGraph.push_back(factor1); double expectedError = factor1->error(values); double expectedErrorGraph = smartGraph.error(values); - Point3 expectedPoint; + Point3 expectedPoint(0,0,0); if (factor1->point()) expectedPoint = *(factor1->point()); - // cout << "expectedPoint " << expectedPoint.vector() << endl; // COMMENTS: // 1) triangulation introduces small errors, then for a fair comparison we use expectedPoint as @@ -677,7 +676,7 @@ TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor1 ) { smartGraph.push_back(factor1); Matrix expectedHessian = smartGraph.linearize(values)->hessian().first; Vector expectedInfoVector = smartGraph.linearize(values)->hessian().second; - Point3 expectedPoint; + Point3 expectedPoint(0,0,0); if (factor1->point()) expectedPoint = *(factor1->point()); @@ -720,7 +719,7 @@ TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor1 ) { // smartGraph.push_back(factor1); // GaussianFactorGraph::shared_ptr gfgSmart = smartGraph.linearize(values); // -// Point3 expectedPoint; +// Point3 expectedPoint(0,0,0); // if(factor1->point()) // expectedPoint = *(factor1->point()); // @@ -773,7 +772,7 @@ TEST( SmartProjectionCameraFactor, computeImplicitJacobian ) { cameras.push_back(level_camera_right); factor1->error(values); // this is important to have a triangulation of the point - Point3 point; + Point3 point(0,0,0); if (factor1->point()) point = *(factor1->point()); vector Fblocks; diff --git a/gtsam_unstable/dynamics/VelocityConstraint.h b/gtsam_unstable/dynamics/VelocityConstraint.h index 4ce4d5758..d9674b415 100644 --- a/gtsam_unstable/dynamics/VelocityConstraint.h +++ b/gtsam_unstable/dynamics/VelocityConstraint.h @@ -108,7 +108,7 @@ private: const Velocity3& v1 = x1.v(), v2 = x2.v(); const Point3& p1 = x1.t(), p2 = x2.t(); - Point3 hx; + Point3 hx(0,0,0); switch(mode) { case dynamics::TRAPEZOIDAL: hx = p1 + Point3((v1 + v2) * dt *0.5); break; case dynamics::EULER_START: hx = p1 + Point3(v1 * dt); break; diff --git a/gtsam_unstable/slam/tests/testTOAFactor.cpp b/gtsam_unstable/slam/tests/testTOAFactor.cpp index 879f7095e..042a15108 100644 --- a/gtsam_unstable/slam/tests/testTOAFactor.cpp +++ b/gtsam_unstable/slam/tests/testTOAFactor.cpp @@ -44,7 +44,7 @@ static SharedNoiseModel model(noiseModel::Isotropic::Sigma(1, 0.5 * ms)); static const double timeOfEvent = 25; static const Event exampleEvent(timeOfEvent, 1, 0, 0); -static const Point3 microphoneAt0; +static const Point3 microphoneAt0(0,0,0); //***************************************************************************** TEST( TOAFactor, NewWay ) { From fe1607c46494ed61d7acaf53120fe2e2c40593c7 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 12 Feb 2016 00:05:25 -0800 Subject: [PATCH 23/90] Enable typedef for Jenkins testing --- gtsam/geometry/Point3.h | 19 ++++++------------- 1 file changed, 6 insertions(+), 13 deletions(-) diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 0cf771844..524bb9bbf 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -28,10 +28,11 @@ namespace gtsam { -//#define GTSAM_USE_VECTOR3_POINTS +#define GTSAM_USE_VECTOR3_POINTS #ifdef GTSAM_USE_VECTOR3_POINTS - // As of GTSAM4, we just typedef Point3 to Vector3 + /// As of GTSAM 4, in order to make GTSAM more lean, + /// it is now possible to just typedef Point3 to Vector3 typedef Vector3 Point3; #else @@ -51,13 +52,10 @@ class GTSAM_EXPORT Point3 : public Vector3 { /// @{ #ifndef GTSAM_ALLOW_DEPRECATED_SINCE_V4 - /// Default constructor now creates *uninitialized* point !!!! - Point3() { - throw std::runtime_error("Default constructor called!"); - } + /// Default constructor no longer initializes, just like Vector3 + Point3() {} #endif - /// Construct from x, y, and z coordinates Point3(double x, double y, double z): Vector3(x,y, z) {} @@ -111,11 +109,6 @@ class GTSAM_EXPORT Point3 : public Vector3 { /// return as Vector3 const Vector3& vector() const { return *this; } - /// return as transposed vector - Eigen::DenseBase::ConstTransposeReturnType transpose() const { - return this->Vector3::transpose(); - } - /// @} /// Output stream operator @@ -124,7 +117,7 @@ class GTSAM_EXPORT Point3 : public Vector3 { #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 /// @name Deprecated /// @{ - Point3() { setZero(); } + Point3() { setZero(); } // initializes to zero, in contrast to new behavior Point3 inverse() const { return -(*this);} Point3 compose(const Point3& q) const { return (*this)+q;} Point3 between(const Point3& q) const { return q-(*this);} From 1c920967d968f78238d2562c7f5e883f1bd42b88 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 12 Feb 2016 00:06:07 -0800 Subject: [PATCH 24/90] No more use of vector() or default constructor --- gtsam/nonlinear/tests/testAdaptAutoDiff.cpp | 2 +- gtsam/nonlinear/tests/testExpression.cpp | 4 ++-- gtsam/sam/tests/testRangeFactor.cpp | 12 ++++++------ gtsam/slam/EssentialMatrixFactor.h | 4 ++-- gtsam/slam/PoseTranslationPrior.h | 6 +++--- gtsam/slam/RotateFactor.h | 6 +++--- gtsam_unstable/dynamics/FullIMUFactor.h | 4 ++-- gtsam_unstable/dynamics/VelocityConstraint.h | 2 +- .../dynamics/tests/testSimpleHelicopter.cpp | 3 ++- gtsam_unstable/geometry/tests/testSimilarity3.cpp | 2 +- gtsam_unstable/slam/BiasedGPSFactor.h | 8 ++++---- .../slam/InertialNavFactor_GlobalVelocity.h | 2 +- gtsam_unstable/slam/InvDepthFactorVariant2.h | 2 +- .../tests/testInertialNavFactor_GlobalVelocity.cpp | 12 ++++++------ tests/simulated3D.h | 4 ++-- tests/testExpressionFactor.cpp | 2 +- 16 files changed, 38 insertions(+), 37 deletions(-) diff --git a/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp index d210be789..af6954341 100644 --- a/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp +++ b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp @@ -167,7 +167,7 @@ Camera camera(Pose3(Rot3().retract(Vector3(0.1, 0.2, 0.3)), Point3(0, 5, 0)), Cal3Bundler0(1, 0, 0)); Point3 point(10, 0, -5); // negative Z-axis convention of Snavely! Vector9 P = Camera().localCoordinates(camera); -Vector3 X = point.vector(); +Vector3 X = point; Vector2 expectedMeasurement(1.2431567, 1.2525694); Matrix E1 = numericalDerivative21(adapted, P, X); Matrix E2 = numericalDerivative22(adapted, P, X); diff --git a/gtsam/nonlinear/tests/testExpression.cpp b/gtsam/nonlinear/tests/testExpression.cpp index 75af5f634..6c9588d38 100644 --- a/gtsam/nonlinear/tests/testExpression.cpp +++ b/gtsam/nonlinear/tests/testExpression.cpp @@ -81,7 +81,7 @@ double f2(const Point3& p, OptionalJacobian<1, 3> H) { return 0.0; } Vector f3(const Point3& p, OptionalJacobian H) { - return p.vector(); + return p; } Expression p(1); set expected = list_of(1); @@ -108,7 +108,7 @@ TEST(Expression, NullaryMethod) { // Create expression Expression p(67); - Expression norm(p, &Point3::norm); + Expression norm(>sam::norm, p); // Create Values Values values; diff --git a/gtsam/sam/tests/testRangeFactor.cpp b/gtsam/sam/tests/testRangeFactor.cpp index 706c20e78..73ff34d2a 100644 --- a/gtsam/sam/tests/testRangeFactor.cpp +++ b/gtsam/sam/tests/testRangeFactor.cpp @@ -361,10 +361,10 @@ TEST( RangeFactor, Camera) { namespace gtsam{ template <> -struct Range { +struct Range { typedef double result_type; - double operator()(const Vector3& v1, const Vector3& v2, - OptionalJacobian<1, 3> H1, OptionalJacobian<1, 3> H2) { + double operator()(const Vector4& v1, const Vector4& v2, + OptionalJacobian<1, 4> H1, OptionalJacobian<1, 4> H2) { return (v2 - v1).norm(); // derivatives not implemented } @@ -376,11 +376,11 @@ TEST(RangeFactor, NonGTSAM) { Key poseKey(1); Key pointKey(2); double measurement(10.0); - RangeFactor factor(poseKey, pointKey, measurement, model); + RangeFactor factor(poseKey, pointKey, measurement, model); // Set the linearization point - Vector3 pose(1.0, 2.0, 00); - Vector3 point(-4.0, 11.0, 0); + Vector4 pose(1.0, 2.0, 00, 0); + Vector4 point(-4.0, 11.0, 0, 0); // The expected error is ||(5.0, 9.0)|| - 10.0 = 0.295630141 meter / UnitCovariance Vector expectedError = (Vector(1) << 0.295630141).finished(); diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h index e9a117745..e668d27d3 100644 --- a/gtsam/slam/EssentialMatrixFactor.h +++ b/gtsam/slam/EssentialMatrixFactor.h @@ -139,7 +139,7 @@ public: const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { Base::print(s); std::cout << " EssentialMatrixFactor2 with measurements\n (" - << dP1_.vector().transpose() << ")' and (" << pn_.vector().transpose() + << dP1_.transpose() << ")' and (" << pn_.vector().transpose() << ")'" << std::endl; } @@ -191,7 +191,7 @@ public: if (Dd) // efficient backwards computation: // (2*3) * (3*3) * (3*1) - *Dd = -f_ * (Dpn_dP2 * (DP2_point * _1T2.vector())); + *Dd = -f_ * (Dpn_dP2 * (DP2_point * _1T2)); } Point2 reprojectionError = pn - pn_; diff --git a/gtsam/slam/PoseTranslationPrior.h b/gtsam/slam/PoseTranslationPrior.h index e9914955c..a24421b34 100644 --- a/gtsam/slam/PoseTranslationPrior.h +++ b/gtsam/slam/PoseTranslationPrior.h @@ -70,19 +70,19 @@ public: (*H).middleCols(transInterval.first, tDim) = R.matrix(); } - return newTrans.vector() - measured_.vector(); + return traits::Local(measured_, newTrans); } /** equals specialized to this factor */ virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This *e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) && measured_.equals(e->measured_, tol); + return e != NULL && Base::equals(*e, tol) && traits::Equals(measured_, e->measured_, tol); } /** print contents */ void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { Base::print(s + "PoseTranslationPrior", keyFormatter); - measured_.print("Measured Translation"); + traits::Print(measured_, "Measured Translation"); } private: diff --git a/gtsam/slam/RotateFactor.h b/gtsam/slam/RotateFactor.h index ea8811d17..2ac217ac1 100644 --- a/gtsam/slam/RotateFactor.h +++ b/gtsam/slam/RotateFactor.h @@ -44,9 +44,9 @@ public: virtual void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { Base::print(s); - std::cout << "RotateFactor:" << std::endl; - p_.print("p"); - z_.print("z"); + std::cout << "RotateFactor:]\n"; + std::cout << "p: " << p_.transpose() << std::endl; + std::cout << "z: " << z_.transpose() << std::endl; } /// vector of errors returns 2D vector diff --git a/gtsam_unstable/dynamics/FullIMUFactor.h b/gtsam_unstable/dynamics/FullIMUFactor.h index 66538e802..1aa85b6fe 100644 --- a/gtsam_unstable/dynamics/FullIMUFactor.h +++ b/gtsam_unstable/dynamics/FullIMUFactor.h @@ -87,7 +87,7 @@ public: Vector9 z; z.head(3).operator=(accel_); // Strange syntax to work around ambiguous operator error with clang z.segment(3, 3).operator=(gyro_); // Strange syntax to work around ambiguous operator error with clang - z.tail(3).operator=(x2.t().vector()); // Strange syntax to work around ambiguous operator error with clang + z.tail(3).operator=(x2.t()); // Strange syntax to work around ambiguous operator error with clang if (H1) *H1 = numericalDerivative21( boost::bind(This::predict_proxy, _1, _2, dt_), x1, x2, 1e-5); if (H2) *H2 = numericalDerivative22( @@ -109,7 +109,7 @@ private: static Vector9 predict_proxy(const PoseRTV& x1, const PoseRTV& x2, double dt) { Vector9 hx; hx.head(6).operator=(x1.imuPrediction(x2, dt)); // Strange syntax to work around ambiguous operator error with clang - hx.tail(3).operator=(x1.translationIntegration(x2, dt).vector()); // Strange syntax to work around ambiguous operator error with clang + hx.tail(3).operator=(x1.translationIntegration(x2, dt)); // Strange syntax to work around ambiguous operator error with clang return hx; } }; diff --git a/gtsam_unstable/dynamics/VelocityConstraint.h b/gtsam_unstable/dynamics/VelocityConstraint.h index d9674b415..ed3797015 100644 --- a/gtsam_unstable/dynamics/VelocityConstraint.h +++ b/gtsam_unstable/dynamics/VelocityConstraint.h @@ -115,7 +115,7 @@ private: case dynamics::EULER_END : hx = p1 + Point3(v2 * dt); break; default: assert(false); break; } - return p2.vector() - hx.vector(); + return p2 - hx; } }; diff --git a/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp b/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp index 1fb2cf39e..8b224f510 100644 --- a/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp +++ b/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp @@ -89,7 +89,8 @@ Vector newtonEuler(const Vector& Vb, const Vector& Fb, const Matrix& Inertia) { TEST( DiscreteEulerPoincareHelicopter, evaluateError) { Vector Fu = computeFu(gamma2, u2); - Vector fGravity_g1 = zero(6); subInsert(fGravity_g1, g1.rotation().unrotate(Point3(0.0, 0.0, -mass*9.81)).vector(), 3); // gravity force in g1 frame + Vector fGravity_g1 = zero(6); + subInsert(fGravity_g1, g1.rotation().unrotate(Point3(0.0, 0.0, -mass*9.81)), 3); // gravity force in g1 frame Vector Fb = Fu+fGravity_g1; Vector dV = newtonEuler(V1_g1, Fb, Inertia); diff --git a/gtsam_unstable/geometry/tests/testSimilarity3.cpp b/gtsam_unstable/geometry/tests/testSimilarity3.cpp index 713ea3054..e2f2a2a86 100644 --- a/gtsam_unstable/geometry/tests/testSimilarity3.cpp +++ b/gtsam_unstable/geometry/tests/testSimilarity3.cpp @@ -64,7 +64,7 @@ TEST(Similarity3, Constructors) { Similarity3 sim3_Construct1; Similarity3 sim3_Construct2(s); Similarity3 sim3_Construct3(R, P, s); - Similarity3 sim4_Construct4(R.matrix(), P.vector(), s); + Similarity3 sim4_Construct4(R.matrix(), P, s); } //****************************************************************************** diff --git a/gtsam_unstable/slam/BiasedGPSFactor.h b/gtsam_unstable/slam/BiasedGPSFactor.h index 6f39ce1b6..df1873765 100644 --- a/gtsam_unstable/slam/BiasedGPSFactor.h +++ b/gtsam_unstable/slam/BiasedGPSFactor.h @@ -58,15 +58,15 @@ namespace gtsam { virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { std::cout << s << "BiasedGPSFactor(" << keyFormatter(this->key1()) << "," - << keyFormatter(this->key2()) << ")\n"; - measured_.print(" measured: "); + << keyFormatter(this->key2()) << ")\n" + << " measured: " << measured_.transpose() << std::endl; this->noiseModel_->print(" noise model: "); } /** equals */ virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This *e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) && this->measured_.equals(e->measured_, tol); + return e != NULL && Base::equals(*e, tol) && traits::Equals(this->measured_, e->measured_, tol); } /** implement functions needed to derive from Factor */ @@ -82,7 +82,7 @@ namespace gtsam { H2->resize(3,3); // jacobian wrt bias (*H2) << Matrix3::Identity(); } - return pose.translation().vector() + bias.vector() - measured_.vector(); + return pose.translation() + bias - measured_; } /** return the measured */ diff --git a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h index d8fceeb68..30d3a216d 100644 --- a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h +++ b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h @@ -187,7 +187,7 @@ public: Vector GyroCorrected(Bias1.correctGyroscope(measurement_gyro_)); body_omega_body = body_R_sensor * GyroCorrected; Matrix body_omega_body__cross = skewSymmetric(body_omega_body); - body_a_body = body_R_sensor * AccCorrected - body_omega_body__cross * body_omega_body__cross * body_P_sensor_->translation().vector(); + body_a_body = body_R_sensor * AccCorrected - body_omega_body__cross * body_omega_body__cross * body_P_sensor_->translation(); } else { body_a_body = AccCorrected; } diff --git a/gtsam_unstable/slam/InvDepthFactorVariant2.h b/gtsam_unstable/slam/InvDepthFactorVariant2.h index 879e1e1dd..fdba78445 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant2.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant2.h @@ -79,7 +79,7 @@ public: && Base::equals(p, tol) && this->measured_.equals(e->measured_, tol) && this->K_->equals(*e->K_, tol) - && this->referencePoint_.equals(e->referencePoint_, tol); + && traits::Equals(this->referencePoint_, e->referencePoint_, tol); } Vector inverseDepthError(const Pose3& pose, const Vector3& landmark) const { diff --git a/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp b/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp index efe1df640..75535eebc 100644 --- a/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp +++ b/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp @@ -233,7 +233,7 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, ///* VADIM - START ************************************************************************* */ //Vector3 predictionRq(const Vector3 angles, const Vector3 q) { -// return (Rot3().RzRyRx(angles) * q).vector(); +// return (Rot3().RzRyRx(angles) * q); //} // //TEST (InertialNavFactor_GlobalVelocity, Rotation_Deriv ) { @@ -435,7 +435,7 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, Vector measurement_acc = Vector3(0.2, 0.1, -0.3 + 9.81) + omega__cross * omega__cross * body_P_sensor.rotation().inverse().matrix() - * body_P_sensor.translation().vector(); // Measured in ENU orientation + * body_P_sensor.translation(); // Measured in ENU orientation InertialNavFactor_GlobalVelocity f( PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, @@ -474,7 +474,7 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, Vector measurement_acc = Vector3(0.2, 0.1, -0.3 + 9.81) + omega__cross * omega__cross * body_P_sensor.rotation().inverse().matrix() - * body_P_sensor.translation().vector(); // Measured in ENU orientation + * body_P_sensor.translation(); // Measured in ENU orientation InertialNavFactor_GlobalVelocity f( PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, @@ -512,7 +512,7 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, Vector measurement_acc = Vector3(0.0, 0.0, 0.0 + 9.81) + omega__cross * omega__cross * body_P_sensor.rotation().inverse().matrix() - * body_P_sensor.translation().vector(); // Measured in ENU orientation + * body_P_sensor.translation(); // Measured in ENU orientation InertialNavFactor_GlobalVelocity f( PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, @@ -554,7 +554,7 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, Vector3(-6.763926150509185, 6.501390843381716, +2.300389940090343) + omega__cross * omega__cross * body_P_sensor.rotation().inverse().matrix() - * body_P_sensor.translation().vector(); // Measured in ENU orientation + * body_P_sensor.translation(); // Measured in ENU orientation InertialNavFactor_GlobalVelocity f( PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, @@ -605,7 +605,7 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, Vector3(-6.763926150509185, 6.501390843381716, +2.300389940090343) + omega__cross * omega__cross * body_P_sensor.rotation().inverse().matrix() - * body_P_sensor.translation().vector(); // Measured in ENU orientation + * body_P_sensor.translation(); // Measured in ENU orientation InertialNavFactor_GlobalVelocity factor( PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, diff --git a/tests/simulated3D.h b/tests/simulated3D.h index cf69a8fa3..84d9ec8cd 100644 --- a/tests/simulated3D.h +++ b/tests/simulated3D.h @@ -91,7 +91,7 @@ struct PointPrior3D: public NoiseModelFactor1 { */ Vector evaluateError(const Point3& x, boost::optional H = boost::none) const { - return prior(x, H).vector() - measured_.vector(); + return prior(x, H) - measured_; } }; @@ -122,7 +122,7 @@ struct Simulated3DMeasurement: public NoiseModelFactor2 { */ Vector evaluateError(const Point3& x1, const Point3& x2, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { - return mea(x1, x2, H1, H2).vector() - measured_.vector(); + return mea(x1, x2, H1, H2) - measured_; } }; diff --git a/tests/testExpressionFactor.cpp b/tests/testExpressionFactor.cpp index 57f03cca6..bda23b3f6 100644 --- a/tests/testExpressionFactor.cpp +++ b/tests/testExpressionFactor.cpp @@ -139,7 +139,7 @@ TEST(ExpressionFactor, Unary) { typedef Eigen::Matrix Matrix93; Vector9 wide(const Point3& p, OptionalJacobian<9,3> H) { Vector9 v; - v << p.vector(), p.vector(), p.vector(); + v << p, p, p; if (H) *H << eye(3), eye(3), eye(3); return v; } From 1a69a71c2c228dc1ebbd6df7a12db66689546e01 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 12 Feb 2016 00:15:49 -0800 Subject: [PATCH 25/90] Fixed print issues --- gtsam/geometry/Pose3.cpp | 6 ++++-- gtsam/geometry/tests/testPoint3.cpp | 2 ++ 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index ccf39f153..f1cb482bb 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -100,7 +100,7 @@ Vector6 Pose3::adjointTranspose(const Vector6& xi, const Vector6& y, void Pose3::print(const string& s) const { cout << s; R_.print("R:\n"); - traits::Print(t_, "t: "); + cout << '[' << t_.x() << ", " << t_.y() << ", " << t_.z() << "]\';"; } /* ************************************************************************* */ @@ -394,7 +394,9 @@ boost::optional align(const vector& pairs) { /* ************************************************************************* */ std::ostream &operator<<(std::ostream &os, const Pose3& pose) { - os << pose.rotation() << "\n" << pose.translation() << endl; + os << pose.rotation() << "\n"; + const Point3& t = pose.translation(); + os << '[' << t.x() << ", " << t.y() << ", " << t.z() << "]\';\n"; return os; } diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp index c7fb44716..a4e2b139e 100644 --- a/gtsam/geometry/tests/testPoint3.cpp +++ b/gtsam/geometry/tests/testPoint3.cpp @@ -84,12 +84,14 @@ TEST( Point3, dot) { } /* ************************************************************************* */ +#ifndef GTSAM_USE_VECTOR3_POINTS TEST( Point3, stream) { Point3 p(1, 2, -3); std::ostringstream os; os << p; EXPECT(os.str() == "[1, 2, -3]';"); } +#endif //************************************************************************* TEST (Point3, normalize) { From 8dd42c8ce5016e74f28bf2b0902966c1d3bf4b71 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 12 Feb 2016 00:53:05 -0800 Subject: [PATCH 26/90] Fixed compilation error in deprecated path --- gtsam/geometry/Rot3.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 30385bd8c..057fdf558 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -456,7 +456,9 @@ namespace gtsam { #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 /// @name Deprecated /// @{ +#ifndef GTSAM_USE_VECTOR3_POINTS static Rot3 rodriguez(const Vector3& axis, double angle) { return AxisAngle(axis, angle); } +#endif static Rot3 rodriguez(const Point3& axis, double angle) { return AxisAngle(axis, angle); } static Rot3 rodriguez(const Unit3& axis, double angle) { return AxisAngle(axis, angle); } static Rot3 rodriguez(const Vector3& w) { return Rodrigues(w); } From b96aa633706fa63ee9c97dc33b6f026477d6b688 Mon Sep 17 00:00:00 2001 From: Enrique Fernandez Date: Fri, 12 Feb 2016 15:39:28 -0500 Subject: [PATCH 27/90] Add Gaussian::Covariance NOTE about inverse --- gtsam/linear/NoiseModel.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index cb77902d0..776c60375 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -120,7 +120,10 @@ Gaussian::shared_ptr Gaussian::Covariance(const Matrix& covariance, if (variances) return Diagonal::Variances(*variances, true); else { - // TODO: can we do this more efficiently and still get an upper triangular nmatrix?? + // NOTE: if cov = L'*L, then the square root information R can be found by + // QR, as L.inverse() = Q*R, with Q some rotation matrix. However, R has + // annoying sign flips with respect the simpler Information(inv(cov)), + // hence we choose the simpler path here: return Information(covariance.inverse(), false); } } From 3a7c8542b0b10d01e60c3ae60662b411271584a5 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 12 Feb 2016 13:14:44 -0800 Subject: [PATCH 28/90] Fixed compile error --- matlab.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/matlab.h b/matlab.h index 4a9ac2309..521e7a469 100644 --- a/matlab.h +++ b/matlab.h @@ -103,7 +103,7 @@ Matrix extractPoint3(const Values& values) { Matrix result(points.size(), 3); size_t j = 0; BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, points) - result.row(j++) = key_value.value.vector(); + result.row(j++) = key_value.value; return result; } @@ -131,7 +131,7 @@ Matrix extractPose3(const Values& values) { result.row(j).segment(0, 3) << key_value.value.rotation().matrix().row(0); result.row(j).segment(3, 3) << key_value.value.rotation().matrix().row(1); result.row(j).segment(6, 3) << key_value.value.rotation().matrix().row(2); - result.row(j).tail(3) = key_value.value.translation().vector(); + result.row(j).tail(3) = key_value.value.translation(); j++; } return result; From 77e2be26c8ec98d733e73d179d88c1d273c43070 Mon Sep 17 00:00:00 2001 From: Enrique Fernandez Date: Fri, 12 Feb 2016 16:19:00 -0500 Subject: [PATCH 29/90] Disable GTSAM_BUILD_PYTHON by default --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 3123dbea2..0d81a57fd 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -64,7 +64,7 @@ option(GTSAM_WITH_TBB "Use Intel Threaded Building Blocks (TB option(GTSAM_WITH_EIGEN_MKL "Eigen will use Intel MKL if available" ON) option(GTSAM_WITH_EIGEN_MKL_OPENMP "Eigen, when using Intel MKL, will also use OpenMP for multithreading if available" ON) option(GTSAM_THROW_CHEIRALITY_EXCEPTION "Throw exception when a triangulated point is behind a camera" ON) -option(GTSAM_BUILD_PYTHON "Enable/Disable building & installation of Python module" ON) +option(GTSAM_BUILD_PYTHON "Enable/Disable building & installation of Python module" OFF) option(GTSAM_ALLOW_DEPRECATED_SINCE_V4 "Allow use of methods/functions deprecated in GTSAM 4" ON) # Options relating to MATLAB wrapper From 20a75db5b51f38a9b4c1b1575ae88d3bb70968f5 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 13 Feb 2016 18:58:03 +0000 Subject: [PATCH 30/90] GTSAM-Concepts.md edited online with Bitbucket --- GTSAM-Concepts.md | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/GTSAM-Concepts.md b/GTSAM-Concepts.md index 3c10451be..5c7434a8d 100644 --- a/GTSAM-Concepts.md +++ b/GTSAM-Concepts.md @@ -19,10 +19,10 @@ To optimize over continuous types, we assume they are manifolds. This is central [Manifolds](http://en.wikipedia.org/wiki/Manifold#Charts.2C_atlases.2C_and_transition_maps) and [charts](http://en.wikipedia.org/wiki/Manifold#Charts.2C_atlases.2C_and_transition_maps) are intimately linked concepts. We are only interested here in [differentiable manifolds](http://en.wikipedia.org/wiki/Differentiable_manifold#Definition), continuous spaces that can be locally approximated *at any point* using a local vector space, called the [tangent space](http://en.wikipedia.org/wiki/Tangent_space). A *chart* is an invertible map from the manifold to that tangent space. In GTSAM, all properties and operations needed to use a type must be defined through template specialization of the struct `gtsam::traits`. Concept checks are used to check that all required functions are implemented. -In detail, we ask the following are defined in the traits object: +In detail, we ask the following are defined in the traits object (although, not all are needed for optimization): * values: - * `enum { dimension = D};`, an enum that indicates the dimensionality *n* of the manifold. In Eigen-fashion, we also support manifolds whose dimenionality is only defined at runtime, by specifying the value -1. + * `enum { dimension = D};`, an enum that indicates the dimensionality *n* of the manifold. In Eigen-fashion, we also support manifolds whose dimensionality is only defined at runtime, by specifying the value -1. * types: * `TangentVector`, type that lives in tangent space. This will almost always be an `Eigen::Matrix`. * `ChartJacobian`, a typedef for `OptionalJacobian`. @@ -33,7 +33,7 @@ In detail, we ask the following are defined in the traits object: * `gtsam::traits::lie_group_tag` -- Everything in this list is expected, plus the functions defined under **Groups**, and **Lie Groups** below. * `gtsam::traits::vector_space_tag` -- Everything in this list is expected, plus the functions defined under **Groups**, and **Lie Groups** below. * valid expressions: - * `size_t dim = traits::getDimension(p);` static function should be defined. This is mostly useful if the size is not known at compile time. + * `size_t dim = traits::GetDimension(p);` static function should be defined. This is mostly useful if the size is not known at compile time. * `v = traits::Local(p,q)`, the chart, from manifold to tangent space, think of it as *q (-) p*, where *p* and *q* are elements of the manifold and the result, *v* is an element of the vector space. * `p = traits::Retract(p,v)`, the inverse chart, from tangent space to manifold, think of it as *p (+) v*, where *p* is an element of the manifold and the result, *v* is an element of the vector space. * invariants From 3df163837abb23dd2ad75336f486e4df5c378d2f Mon Sep 17 00:00:00 2001 From: Kartik Mohta Date: Sat, 13 Feb 2016 15:08:54 -0500 Subject: [PATCH 31/90] Respect user's choice for using system Eigen, default to included copy --- CMakeLists.txt | 12 ++---------- 1 file changed, 2 insertions(+), 10 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index d63bf5c43..5105b7423 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -215,15 +215,6 @@ endif() ### http://eigen.tuxfamily.org/bz/show_bug.cgi?id=705 (Fix MKL LLT return code) option(GTSAM_USE_SYSTEM_EIGEN "Find and use system-installed Eigen. If 'off', use the one bundled with GTSAM" OFF) -# Use system Eigen if version >= 3.2.5 since that includes our patches -find_package(Eigen3 QUIET) -if(EIGEN3_FOUND AND (EIGEN3_VERSION VERSION_GREATER 3.2.4)) - set(GTSAM_USE_SYSTEM_EIGEN ON) - set(GTSAM_SYSTEM_EIGEN_MKL_ERROR OFF) -else() - set(GTSAM_SYSTEM_EIGEN_MKL_ERROR ON) -endif() - # Switch for using system Eigen or GTSAM-bundled Eigen if(GTSAM_USE_SYSTEM_EIGEN) find_package(Eigen3 REQUIRED) @@ -233,7 +224,8 @@ if(GTSAM_USE_SYSTEM_EIGEN) set(GTSAM_EIGEN_INCLUDE_PREFIX "${EIGEN3_INCLUDE_DIR}") # check if MKL is also enabled - can have one or the other, but not both! - if(EIGEN_USE_MKL_ALL AND GTSAM_SYSTEM_EIGEN_MKL_ERROR) + # Note: Eigen >= v3.2.5 includes our patches + if(EIGEN_USE_MKL_ALL AND (EIGEN3_VERSION VERSION_LESS 3.2.5)) message(FATAL_ERROR "MKL cannot be used together with system-installed Eigen, as MKL support relies on patches which are not yet in the system-installed Eigen. Disable either GTSAM_USE_SYSTEM_EIGEN or GTSAM_WITH_EIGEN_MKL") endif() else() From 96714269a32c60d8e57a563740161eabbdb3d270 Mon Sep 17 00:00:00 2001 From: Kartik Mohta Date: Sat, 13 Feb 2016 16:34:27 -0500 Subject: [PATCH 32/90] Change Eigen MKL warning message --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 5105b7423..602ed01fd 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -226,7 +226,7 @@ if(GTSAM_USE_SYSTEM_EIGEN) # check if MKL is also enabled - can have one or the other, but not both! # Note: Eigen >= v3.2.5 includes our patches if(EIGEN_USE_MKL_ALL AND (EIGEN3_VERSION VERSION_LESS 3.2.5)) - message(FATAL_ERROR "MKL cannot be used together with system-installed Eigen, as MKL support relies on patches which are not yet in the system-installed Eigen. Disable either GTSAM_USE_SYSTEM_EIGEN or GTSAM_WITH_EIGEN_MKL") + message(FATAL_ERROR "MKL requires at least Eigen 3.2.5, and your system appears to have an older version. Disable GTSAM_USE_SYSTEM_EIGEN to use GTSAM's copy of Eigen, or disable GTSAM_WITH_EIGEN_MKL") endif() else() # Use bundled Eigen include path. From a283938b47e26de4f757b3663dd678fbf1a1a233 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 16 Feb 2016 10:23:02 -0800 Subject: [PATCH 33/90] A minimal traits example --- tests/testNonlinearOptimizer.cpp | 29 +++++++++++++++++++++++++++++ 1 file changed, 29 insertions(+) diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index f927f45ae..f6cdd6ee5 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -462,6 +462,35 @@ TEST( NonlinearOptimizer, logfile ) // EXPECT(actual.str()==expected.str()); } +/* ************************************************************************* */ +// Minimal traits example +struct MyType : public Vector3 { + using Vector3::Vector3; +}; + +namespace gtsam { +template <> +struct traits { + static bool Equals(const MyType&, const MyType&, double tol) {return true;} + static void Print(const MyType&, const string&) {} + static int GetDimension(const MyType&) { return 3;} + static MyType Retract(const MyType&, const Vector3&) {return MyType();} + static Vector3 Local(const MyType&, const MyType&) {return Vector3();} +}; +} + +TEST(NonlinearOptimizer, Traits) { + NonlinearFactorGraph fg; + fg += PriorFactor(0, MyType(0, 0, 0), noiseModel::Isotropic::Sigma(3, 1)); + + Values init; + init.insert(0, MyType(0,0,0)); + + LevenbergMarquardtOptimizer optimizer(fg, init); + Values actual = optimizer.optimize(); + EXPECT(assert_equal(init, actual)); +} + /* ************************************************************************* */ int main() { TestResult tr; From aebdd2dc2f7c1046c0ad80a688adcf5fdbc4b2fa Mon Sep 17 00:00:00 2001 From: chrisbeall Date: Tue, 16 Feb 2016 21:06:57 -0500 Subject: [PATCH 34/90] Eigen 3.2.8 - mostly minor bugfixes http://eigen.tuxfamily.org/index.php?title=ChangeLog#Eigen_3.2.8 --- gtsam/3rdparty/Eigen/.hg_archival.txt | 4 +- gtsam/3rdparty/Eigen/.hgtags | 1 + gtsam/3rdparty/Eigen/CMakeLists.txt | 66 ++-- gtsam/3rdparty/Eigen/Eigen/CholmodSupport | 2 +- gtsam/3rdparty/Eigen/Eigen/SPQRSupport | 2 +- gtsam/3rdparty/Eigen/Eigen/UmfPackSupport | 2 +- .../Eigen/Eigen/src/Core/CwiseUnaryView.h | 2 +- .../3rdparty/Eigen/Eigen/src/Core/DenseBase.h | 2 +- .../Eigen/Eigen/src/Core/GeneralProduct.h | 11 +- gtsam/3rdparty/Eigen/Eigen/src/Core/MapBase.h | 4 + .../Eigen/Eigen/src/Core/MathFunctions.h | 6 +- .../Eigen/Eigen/src/Core/SolveTriangular.h | 8 +- gtsam/3rdparty/Eigen/Eigen/src/Core/Visitor.h | 7 +- .../Eigen/src/Core/arch/SSE/PacketMath.h | 44 +-- .../src/Core/products/GeneralMatrixMatrix.h | 8 +- .../Core/products/TriangularSolverMatrix.h | 4 +- .../Eigen/Eigen/src/Core/util/Macros.h | 287 +++++++++++++++++- .../Eigen/Eigen/src/Core/util/StaticAssert.h | 2 +- .../Eigen/src/Eigenvalues/ComplexSchur_MKL.h | 1 - .../Eigen/src/Eigenvalues/RealSchur_MKL.h | 4 - .../Eigen/Eigen/src/Geometry/AngleAxis.h | 7 + .../Eigen/src/Geometry/ParametrizedLine.h | 2 +- .../Eigen/Eigen/src/Geometry/Transform.h | 49 ++- .../Eigen/Eigen/src/Geometry/Translation.h | 2 +- .../ConjugateGradient.h | 2 + gtsam/3rdparty/Eigen/Eigen/src/LU/FullPivLU.h | 2 +- .../Eigen/Eigen/src/OrderingMethods/Amd.h | 2 +- .../Eigen/src/OrderingMethods/Eigen_Colamd.h | 31 +- .../Eigen/src/QR/ColPivHouseholderQR_MKL.h | 1 - .../3rdparty/Eigen/Eigen/src/SVD/JacobiSVD.h | 2 +- .../Eigen/Eigen/src/SVD/JacobiSVD_MKL.h | 4 +- .../Eigen/Eigen/src/SparseCore/SparseBlock.h | 6 +- .../src/SparseCore/SparseCwiseBinaryOp.h | 5 +- .../Eigen/Eigen/src/SparseCore/SparseView.h | 6 +- .../Eigen/Eigen/src/StlSupport/StdDeque.h | 18 +- .../Eigen/Eigen/src/StlSupport/StdList.h | 18 +- gtsam/3rdparty/Eigen/doc/CMakeLists.txt | 1 + gtsam/3rdparty/Eigen/doc/Manual.dox | 2 + .../Eigen/doc/PreprocessorDirectives.dox | 1 + .../Eigen/doc/SparseLinearSystems.dox | 6 +- .../Eigen/doc/SparseQuickReference.dox | 62 ++-- gtsam/3rdparty/Eigen/doc/TutorialSparse.dox | 24 +- .../Eigen/doc/UnalignedArrayAssert.dox | 15 +- gtsam/3rdparty/Eigen/eigen3.pc.in | 7 +- gtsam/3rdparty/Eigen/test/CMakeLists.txt | 2 + gtsam/3rdparty/Eigen/test/dynalloc.cpp | 39 ++- gtsam/3rdparty/Eigen/test/product.h | 37 ++- gtsam/3rdparty/Eigen/test/product_large.cpp | 23 ++ .../Eigen/test/product_notemporary.cpp | 9 + gtsam/3rdparty/Eigen/test/ref.cpp | 13 +- gtsam/3rdparty/Eigen/test/visitor.cpp | 5 + .../Eigen/unsupported/Eigen/AlignedVector3 | 2 +- 52 files changed, 631 insertions(+), 241 deletions(-) diff --git a/gtsam/3rdparty/Eigen/.hg_archival.txt b/gtsam/3rdparty/Eigen/.hg_archival.txt index 8c88afc32..c4115d7f6 100644 --- a/gtsam/3rdparty/Eigen/.hg_archival.txt +++ b/gtsam/3rdparty/Eigen/.hg_archival.txt @@ -1,4 +1,4 @@ repo: 8a21fd850624c931e448cbcfb38168cb2717c790 -node: b30b87236a1b1552af32ac34075ee5696a9b5a33 +node: 07105f7124f9aef00a68c85e0fc606e65d3d6c15 branch: 3.2 -tag: 3.2.7 +tag: 3.2.8 diff --git a/gtsam/3rdparty/Eigen/.hgtags b/gtsam/3rdparty/Eigen/.hgtags index c4ccd33fa..8f0097f20 100644 --- a/gtsam/3rdparty/Eigen/.hgtags +++ b/gtsam/3rdparty/Eigen/.hgtags @@ -30,3 +30,4 @@ ffa86ffb557094721ca71dcea6aed2651b9fd610 3.2.0 10219c95fe653d4962aa9db4946f6fbea96dd740 3.2.4 bdd17ee3b1b3a166cd5ec36dcad4fc1f3faf774a 3.2.5 c58038c56923e0fd86de3ded18e03df442e66dfb 3.2.6 +b30b87236a1b1552af32ac34075ee5696a9b5a33 3.2.7 diff --git a/gtsam/3rdparty/Eigen/CMakeLists.txt b/gtsam/3rdparty/Eigen/CMakeLists.txt index ed3e67fe9..77e9f2d35 100644 --- a/gtsam/3rdparty/Eigen/CMakeLists.txt +++ b/gtsam/3rdparty/Eigen/CMakeLists.txt @@ -1,6 +1,5 @@ project(Eigen) - -cmake_minimum_required(VERSION 2.8.2) +cmake_minimum_required(VERSION 2.8.5) # guard against in-source builds @@ -55,6 +54,7 @@ endif(EIGEN_HG_CHANGESET) include(CheckCXXCompilerFlag) +include(GNUInstallDirs) set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) @@ -288,25 +288,26 @@ option(EIGEN_TEST_C++0x "Enables all C++0x features." OFF) include_directories(${CMAKE_CURRENT_SOURCE_DIR} ${CMAKE_CURRENT_BINARY_DIR}) -# the user modifiable install path for header files -set(EIGEN_INCLUDE_INSTALL_DIR ${EIGEN_INCLUDE_INSTALL_DIR} CACHE PATH "The directory where we install the header files (optional)") - -# set the internal install path for header files which depends on wether the user modifiable -# EIGEN_INCLUDE_INSTALL_DIR has been set by the user or not. -if(EIGEN_INCLUDE_INSTALL_DIR) - set(INCLUDE_INSTALL_DIR - ${EIGEN_INCLUDE_INSTALL_DIR} - CACHE INTERNAL - "The directory where we install the header files (internal)" - ) +# Backward compatibility support for EIGEN_INCLUDE_INSTALL_DIR +if(EIGEN_INCLUDE_INSTALL_DIR AND NOT INCLUDE_INSTALL_DIR) + set(INCLUDE_INSTALL_DIR ${EIGEN_INCLUDE_INSTALL_DIR} + CACHE PATH "The directory relative to CMAKE_PREFIX_PATH where Eigen header files are installed") else() set(INCLUDE_INSTALL_DIR - "include/eigen3" - CACHE INTERNAL - "The directory where we install the header files (internal)" - ) + "${CMAKE_INSTALL_INCLUDEDIR}/eigen3" + CACHE PATH "The directory relative to CMAKE_PREFIX_PATH where Eigen header files are installed" + ) endif() +set(CMAKEPACKAGE_INSTALL_DIR + "${CMAKE_INSTALL_LIBDIR}/cmake/eigen3" + CACHE PATH "The directory relative to CMAKE_PREFIX_PATH where Eigen3Config.cmake is installed" + ) +set(PKGCONFIG_INSTALL_DIR + "${CMAKE_INSTALL_DATADIR}/pkgconfig" + CACHE PATH "The directory relative to CMAKE_PREFIX_PATH where eigen3.pc is installed" + ) + # similar to set_target_properties but append the property instead of overwriting it macro(ei_add_target_property target prop value) @@ -324,21 +325,9 @@ install(FILES ) if(EIGEN_BUILD_PKGCONFIG) - SET(path_separator ":") - STRING(REPLACE ${path_separator} ";" pkg_config_libdir_search "$ENV{PKG_CONFIG_LIBDIR}") - message(STATUS "searching for 'pkgconfig' directory in PKG_CONFIG_LIBDIR ( $ENV{PKG_CONFIG_LIBDIR} ), ${CMAKE_INSTALL_PREFIX}/share, and ${CMAKE_INSTALL_PREFIX}/lib") - FIND_PATH(pkg_config_libdir pkgconfig ${pkg_config_libdir_search} ${CMAKE_INSTALL_PREFIX}/share ${CMAKE_INSTALL_PREFIX}/lib ${pkg_config_libdir_search}) - if(pkg_config_libdir) - SET(pkg_config_install_dir ${pkg_config_libdir}) - message(STATUS "found ${pkg_config_libdir}/pkgconfig" ) - else(pkg_config_libdir) - SET(pkg_config_install_dir ${CMAKE_INSTALL_PREFIX}/share) - message(STATUS "pkgconfig not found; installing in ${pkg_config_install_dir}" ) - endif(pkg_config_libdir) - - configure_file(eigen3.pc.in eigen3.pc) + configure_file(eigen3.pc.in eigen3.pc @ONLY) install(FILES ${CMAKE_CURRENT_BINARY_DIR}/eigen3.pc - DESTINATION ${pkg_config_install_dir}/pkgconfig + DESTINATION ${PKGCONFIG_INSTALL_DIR} ) endif(EIGEN_BUILD_PKGCONFIG) @@ -401,12 +390,15 @@ if(cmake_generator_tolower MATCHES "makefile") message(STATUS "--------------+--------------------------------------------------------------") message(STATUS "Command | Description") message(STATUS "--------------+--------------------------------------------------------------") - message(STATUS "make install | Install to ${CMAKE_INSTALL_PREFIX}. To change that:") - message(STATUS " | cmake . -DCMAKE_INSTALL_PREFIX=yourpath") - message(STATUS " | Eigen headers will then be installed to:") - message(STATUS " | ${CMAKE_INSTALL_PREFIX}/${INCLUDE_INSTALL_DIR}") - message(STATUS " | To install Eigen headers to a separate location, do:") - message(STATUS " | cmake . -DEIGEN_INCLUDE_INSTALL_DIR=yourpath") + message(STATUS "make install | Install Eigen. Headers will be installed to:") + message(STATUS " | /") + message(STATUS " | Using the following values:") + message(STATUS " | CMAKE_INSTALL_PREFIX: ${CMAKE_INSTALL_PREFIX}") + message(STATUS " | INCLUDE_INSTALL_DIR: ${INCLUDE_INSTALL_DIR}") + message(STATUS " | Change the install location of Eigen headers using:") + message(STATUS " | cmake . -DCMAKE_INSTALL_PREFIX=yourprefix") + message(STATUS " | Or:") + message(STATUS " | cmake . -DINCLUDE_INSTALL_DIR=yourdir") message(STATUS "make doc | Generate the API documentation, requires Doxygen & LaTeX") message(STATUS "make check | Build and run the unit-tests. Read this page:") message(STATUS " | http://eigen.tuxfamily.org/index.php?title=Tests") diff --git a/gtsam/3rdparty/Eigen/Eigen/CholmodSupport b/gtsam/3rdparty/Eigen/Eigen/CholmodSupport index 745b884e7..88c29a646 100644 --- a/gtsam/3rdparty/Eigen/Eigen/CholmodSupport +++ b/gtsam/3rdparty/Eigen/Eigen/CholmodSupport @@ -12,7 +12,7 @@ extern "C" { /** \ingroup Support_modules * \defgroup CholmodSupport_Module CholmodSupport module * - * This module provides an interface to the Cholmod library which is part of the suitesparse package. + * This module provides an interface to the Cholmod library which is part of the suitesparse package. * It provides the two following main factorization classes: * - class CholmodSupernodalLLT: a supernodal LLT Cholesky factorization. * - class CholmodDecomposiiton: a general L(D)LT Cholesky factorization with automatic or explicit runtime selection of the underlying factorization method (supernodal or simplicial). diff --git a/gtsam/3rdparty/Eigen/Eigen/SPQRSupport b/gtsam/3rdparty/Eigen/Eigen/SPQRSupport index 77016442e..7f1eb4770 100644 --- a/gtsam/3rdparty/Eigen/Eigen/SPQRSupport +++ b/gtsam/3rdparty/Eigen/Eigen/SPQRSupport @@ -10,7 +10,7 @@ /** \ingroup Support_modules * \defgroup SPQRSupport_Module SuiteSparseQR module * - * This module provides an interface to the SPQR library, which is part of the suitesparse package. + * This module provides an interface to the SPQR library, which is part of the suitesparse package. * * \code * #include diff --git a/gtsam/3rdparty/Eigen/Eigen/UmfPackSupport b/gtsam/3rdparty/Eigen/Eigen/UmfPackSupport index 984f64a84..7b1b66064 100644 --- a/gtsam/3rdparty/Eigen/Eigen/UmfPackSupport +++ b/gtsam/3rdparty/Eigen/Eigen/UmfPackSupport @@ -12,7 +12,7 @@ extern "C" { /** \ingroup Support_modules * \defgroup UmfPackSupport_Module UmfPackSupport module * - * This module provides an interface to the UmfPack library which is part of the suitesparse package. + * This module provides an interface to the UmfPack library which is part of the suitesparse package. * It provides the following factorization class: * - class UmfPackLU: a multifrontal sequential LU factorization. * diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/CwiseUnaryView.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/CwiseUnaryView.h index b2638d326..f3b2ffeb6 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/CwiseUnaryView.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/CwiseUnaryView.h @@ -38,7 +38,7 @@ struct traits > typedef typename remove_all::type _MatrixTypeNested; enum { Flags = (traits<_MatrixTypeNested>::Flags & (HereditaryBits | LvalueBit | LinearAccessBit | DirectAccessBit)), - CoeffReadCost = traits<_MatrixTypeNested>::CoeffReadCost + functor_traits::Cost, + CoeffReadCost = EIGEN_ADD_COST(traits<_MatrixTypeNested>::CoeffReadCost, functor_traits::Cost), MatrixTypeInnerStride = inner_stride_at_compile_time::ret, // need to cast the sizeof's from size_t to int explicitly, otherwise: // "error: no integral type can represent all of the enumerator values diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseBase.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseBase.h index 354c739f7..4b371b075 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseBase.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseBase.h @@ -40,7 +40,7 @@ static inline void check_DenseIndex_is_signed() { */ template class DenseBase #ifndef EIGEN_PARSED_BY_DOXYGEN - : public internal::special_scalar_op_base::Scalar, + : public internal::special_scalar_op_base::Scalar, typename NumTraits::Scalar>::Real, DenseCoeffsBase > #else diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/GeneralProduct.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/GeneralProduct.h index 0eae52990..29ac522d2 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/GeneralProduct.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/GeneralProduct.h @@ -425,15 +425,18 @@ template<> struct gemv_selector ResScalar actualAlpha = alpha * LhsBlasTraits::extractScalarFactor(prod.lhs()) * RhsBlasTraits::extractScalarFactor(prod.rhs()); + // make sure Dest is a compile-time vector type (bug 1166) + typedef typename conditional::type ActualDest; + enum { // FIXME find a way to allow an inner stride on the result if packet_traits::size==1 // on, the other hand it is good for the cache to pack the vector anyways... - EvalToDestAtCompileTime = Dest::InnerStrideAtCompileTime==1, + EvalToDestAtCompileTime = (ActualDest::InnerStrideAtCompileTime==1), ComplexByReal = (NumTraits::IsComplex) && (!NumTraits::IsComplex), - MightCannotUseDest = (Dest::InnerStrideAtCompileTime!=1) || ComplexByReal + MightCannotUseDest = (ActualDest::InnerStrideAtCompileTime!=1) || ComplexByReal }; - gemv_static_vector_if static_dest; + gemv_static_vector_if static_dest; bool alphaIsCompatible = (!ComplexByReal) || (numext::imag(actualAlpha)==RealScalar(0)); bool evalToDest = EvalToDestAtCompileTime && alphaIsCompatible; @@ -522,7 +525,7 @@ template<> struct gemv_selector actualLhs.rows(), actualLhs.cols(), actualLhs.data(), actualLhs.outerStride(), actualRhsPtr, 1, - dest.data(), dest.innerStride(), + dest.data(), dest.col(0).innerStride(), //NOTE if dest is not a vector at compile-time, then dest.innerStride() might be wrong. (bug 1166) actualAlpha); } }; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/MapBase.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/MapBase.h index a9828f7f4..81efc4a6d 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/MapBase.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/MapBase.h @@ -149,6 +149,10 @@ template class MapBase checkSanity(); } + #ifdef EIGEN_MAPBASE_PLUGIN + #include EIGEN_MAPBASE_PLUGIN + #endif + protected: void checkSanity() const diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/MathFunctions.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/MathFunctions.h index adf2f9c51..4e17ecd4b 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/MathFunctions.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/MathFunctions.h @@ -707,21 +707,21 @@ struct scalar_fuzzy_impl : scalar_fuzzy_default_impl:: template inline bool isMuchSmallerThan(const Scalar& x, const OtherScalar& y, - typename NumTraits::Real precision = NumTraits::dummy_precision()) + const typename NumTraits::Real &precision = NumTraits::dummy_precision()) { return scalar_fuzzy_impl::template isMuchSmallerThan(x, y, precision); } template inline bool isApprox(const Scalar& x, const Scalar& y, - typename NumTraits::Real precision = NumTraits::dummy_precision()) + const typename NumTraits::Real &precision = NumTraits::dummy_precision()) { return scalar_fuzzy_impl::isApprox(x, y, precision); } template inline bool isApproxOrLessThan(const Scalar& x, const Scalar& y, - typename NumTraits::Real precision = NumTraits::dummy_precision()) + const typename NumTraits::Real &precision = NumTraits::dummy_precision()) { return scalar_fuzzy_impl::isApproxOrLessThan(x, y, precision); } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/SolveTriangular.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/SolveTriangular.h index ef17f288e..83565ddd8 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/SolveTriangular.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/SolveTriangular.h @@ -116,17 +116,17 @@ template struct triangular_solver_unroller { enum { IsLower = ((Mode&Lower)==Lower), - I = IsLower ? Index : Size - Index - 1, - S = IsLower ? 0 : I+1 + RowIndex = IsLower ? Index : Size - Index - 1, + S = IsLower ? 0 : RowIndex+1 }; static void run(const Lhs& lhs, Rhs& rhs) { if (Index>0) - rhs.coeffRef(I) -= lhs.row(I).template segment(S).transpose() + rhs.coeffRef(RowIndex) -= lhs.row(RowIndex).template segment(S).transpose() .cwiseProduct(rhs.template segment(S)).sum(); if(!(Mode & UnitDiag)) - rhs.coeffRef(I) /= lhs.coeff(I,I); + rhs.coeffRef(RowIndex) /= lhs.coeff(RowIndex,RowIndex); triangular_solver_unroller::run(lhs,rhs); } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/Visitor.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/Visitor.h index 64867b7a2..dd94eb8f0 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/Visitor.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/Visitor.h @@ -76,14 +76,17 @@ template template void DenseBase::visit(Visitor& visitor) const { + typedef typename internal::remove_all::type ThisNested; + typename Derived::Nested thisNested(derived()); + enum { unroll = SizeAtCompileTime != Dynamic && CoeffReadCost != Dynamic && (SizeAtCompileTime == 1 || internal::functor_traits::Cost != Dynamic) && SizeAtCompileTime * CoeffReadCost + (SizeAtCompileTime-1) * internal::functor_traits::Cost <= EIGEN_UNROLLING_LIMIT }; - return internal::visitor_impl::run(derived(), visitor); + >::run(thisNested, visitor); } namespace internal { diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/SSE/PacketMath.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/SSE/PacketMath.h index fc8ae50fe..bef898b1f 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/SSE/PacketMath.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/arch/SSE/PacketMath.h @@ -235,63 +235,27 @@ template<> EIGEN_STRONG_INLINE Packet4i pload(const int* from) { E return _mm_loadu_ps(from); #endif } - template<> EIGEN_STRONG_INLINE Packet2d ploadu(const double* from) { EIGEN_DEBUG_UNALIGNED_LOAD return _mm_loadu_pd(from); } - template<> EIGEN_STRONG_INLINE Packet4i ploadu(const int* from) { EIGEN_DEBUG_UNALIGNED_LOAD return _mm_loadu_si128(reinterpret_cast(from)); } #else -// Fast unaligned loads. Note that here we cannot directly use intrinsics: this would -// require pointer casting to incompatible pointer types and leads to invalid code -// because of the strict aliasing rule. The "dummy" stuff are required to enforce -// a correct instruction dependency. -// TODO: do the same for MSVC (ICC is compatible) // NOTE: with the code below, MSVC's compiler crashes! -#if defined(__GNUC__) && defined(__i386__) - // bug 195: gcc/i386 emits weird x87 fldl/fstpl instructions for _mm_load_sd - #define EIGEN_AVOID_CUSTOM_UNALIGNED_LOADS 1 -#elif defined(__clang__) - // bug 201: Segfaults in __mm_loadh_pd with clang 2.8 - #define EIGEN_AVOID_CUSTOM_UNALIGNED_LOADS 1 -#else - #define EIGEN_AVOID_CUSTOM_UNALIGNED_LOADS 0 -#endif - template<> EIGEN_STRONG_INLINE Packet4f ploadu(const float* from) { EIGEN_DEBUG_UNALIGNED_LOAD -#if EIGEN_AVOID_CUSTOM_UNALIGNED_LOADS return _mm_loadu_ps(from); -#else - __m128d res; - res = _mm_load_sd((const double*)(from)) ; - res = _mm_loadh_pd(res, (const double*)(from+2)) ; - return _mm_castpd_ps(res); -#endif } +#endif + template<> EIGEN_STRONG_INLINE Packet2d ploadu(const double* from) { EIGEN_DEBUG_UNALIGNED_LOAD -#if EIGEN_AVOID_CUSTOM_UNALIGNED_LOADS return _mm_loadu_pd(from); -#else - __m128d res; - res = _mm_load_sd(from) ; - res = _mm_loadh_pd(res,from+1); - return res; -#endif } template<> EIGEN_STRONG_INLINE Packet4i ploadu(const int* from) { EIGEN_DEBUG_UNALIGNED_LOAD -#if EIGEN_AVOID_CUSTOM_UNALIGNED_LOADS - return _mm_loadu_si128(reinterpret_cast(from)); -#else - __m128d res; - res = _mm_load_sd((const double*)(from)) ; - res = _mm_loadh_pd(res, (const double*)(from+2)) ; - return _mm_castpd_si128(res); -#endif + return _mm_loadu_si128(reinterpret_cast(from)); } -#endif + template<> EIGEN_STRONG_INLINE Packet4f ploaddup(const float* from) { diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixMatrix.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixMatrix.h index 3f5ffcf51..cfd2f0095 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixMatrix.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixMatrix.h @@ -140,8 +140,10 @@ static void run(Index rows, Index cols, Index depth, // Release all the sub blocks B'_j of B' for the current thread, // i.e., we simply decrement the number of users by 1 for(Index j=0; j GeneralProduct(const Lhs& lhs, const Rhs& rhs) : Base(lhs,rhs) { +#if !(defined(EIGEN_NO_STATIC_ASSERT) && defined(EIGEN_NO_DEBUG)) typedef internal::scalar_product_op BinOp; EIGEN_CHECK_BINARY_COMPATIBILIY(BinOp,LhsScalar,RhsScalar); +#endif } template void scaleAndAddTo(Dest& dst, const Scalar& alpha) const { eigen_assert(dst.rows()==m_lhs.rows() && dst.cols()==m_rhs.cols()); + if(m_lhs.cols()==0 || m_lhs.rows()==0 || m_rhs.cols()==0) + return; typename internal::add_const_on_value_type::type lhs = LhsBlasTraits::extract(m_lhs); typename internal::add_const_on_value_type::type rhs = RhsBlasTraits::extract(m_rhs); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularSolverMatrix.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularSolverMatrix.h index 04240ab50..03a23abc2 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularSolverMatrix.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularSolverMatrix.h @@ -115,8 +115,9 @@ EIGEN_DONT_INLINE void triangular_solve_matrixx || (EIGEN_WORLD_VERSION>=x && \ (EIGEN_MAJOR_VERSION>y || (EIGEN_MAJOR_VERSION>=y && \ EIGEN_MINOR_VERSION>=z)))) + + +// Compiler identification, EIGEN_COMP_* + +/// \internal EIGEN_COMP_GNUC set to 1 for all compilers compatible with GCC #ifdef __GNUC__ + #define EIGEN_COMP_GNUC 1 +#else + #define EIGEN_COMP_GNUC 0 +#endif + +/// \internal EIGEN_COMP_CLANG set to 1 if the compiler is clang (alias for __clang__) +#if defined(__clang__) + #define EIGEN_COMP_CLANG 1 +#else + #define EIGEN_COMP_CLANG 0 +#endif + + +/// \internal EIGEN_COMP_LLVM set to 1 if the compiler backend is llvm +#if defined(__llvm__) + #define EIGEN_COMP_LLVM 1 +#else + #define EIGEN_COMP_LLVM 0 +#endif + +/// \internal EIGEN_COMP_ICC set to __INTEL_COMPILER if the compiler is Intel compiler, 0 otherwise +#if defined(__INTEL_COMPILER) + #define EIGEN_COMP_ICC __INTEL_COMPILER +#else + #define EIGEN_COMP_ICC 0 +#endif + +/// \internal EIGEN_COMP_MINGW set to 1 if the compiler is mingw +#if defined(__MINGW32__) + #define EIGEN_COMP_MINGW 1 +#else + #define EIGEN_COMP_MINGW 0 +#endif + +/// \internal EIGEN_COMP_SUNCC set to 1 if the compiler is Solaris Studio +#if defined(__SUNPRO_CC) + #define EIGEN_COMP_SUNCC 1 +#else + #define EIGEN_COMP_SUNCC 0 +#endif + +/// \internal EIGEN_COMP_MSVC set to _MSC_VER if the compiler is Microsoft Visual C++, 0 otherwise. +#if defined(_MSC_VER) + #define EIGEN_COMP_MSVC _MSC_VER +#else + #define EIGEN_COMP_MSVC 0 +#endif + +/// \internal EIGEN_COMP_MSVC_STRICT set to 1 if the compiler is really Microsoft Visual C++ and not ,e.g., ICC +#if EIGEN_COMP_MSVC && !(EIGEN_COMP_ICC) + #define EIGEN_COMP_MSVC_STRICT _MSC_VER +#else + #define EIGEN_COMP_MSVC_STRICT 0 +#endif + +/// \internal EIGEN_COMP_IBM set to 1 if the compiler is IBM XL C++ +#if defined(__IBMCPP__) || defined(__xlc__) + #define EIGEN_COMP_IBM 1 +#else + #define EIGEN_COMP_IBM 0 +#endif + +/// \internal EIGEN_COMP_PGI set to 1 if the compiler is Portland Group Compiler +#if defined(__PGI) + #define EIGEN_COMP_PGI 1 +#else + #define EIGEN_COMP_PGI 0 +#endif + +/// \internal EIGEN_COMP_ARM set to 1 if the compiler is ARM Compiler +#if defined(__CC_ARM) || defined(__ARMCC_VERSION) + #define EIGEN_COMP_ARM 1 +#else + #define EIGEN_COMP_ARM 0 +#endif + + +/// \internal EIGEN_GNUC_STRICT set to 1 if the compiler is really GCC and not a compatible compiler (e.g., ICC, clang, mingw, etc.) +#if EIGEN_COMP_GNUC && !(EIGEN_COMP_CLANG || EIGEN_COMP_ICC || EIGEN_COMP_MINGW || EIGEN_COMP_PGI || EIGEN_COMP_IBM || EIGEN_COMP_ARM ) + #define EIGEN_COMP_GNUC_STRICT 1 +#else + #define EIGEN_COMP_GNUC_STRICT 0 +#endif + + +#if EIGEN_COMP_GNUC #define EIGEN_GNUC_AT_LEAST(x,y) ((__GNUC__==x && __GNUC_MINOR__>=y) || __GNUC__>x) + #define EIGEN_GNUC_AT_MOST(x,y) ((__GNUC__==x && __GNUC_MINOR__<=y) || __GNUC__= 201103L) || \ (defined(_MSC_VER) && _MSC_VER >= 1600)) #define EIGEN_HAVE_RVALUE_REFERENCES #endif diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/StaticAssert.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/StaticAssert.h index bac5d9fe9..e53d2b878 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/StaticAssert.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/StaticAssert.h @@ -26,7 +26,7 @@ #ifndef EIGEN_NO_STATIC_ASSERT - #if defined(__GXX_EXPERIMENTAL_CXX0X__) || (defined(_MSC_VER) && (_MSC_VER >= 1600)) + #if __has_feature(cxx_static_assert) || (defined(__cplusplus) && __cplusplus >= 201103L) || (EIGEN_COMP_MSVC >= 1600) // if native static_assert is enabled, let's use it #define EIGEN_STATIC_ASSERT(X,MSG) static_assert(X,#MSG); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/ComplexSchur_MKL.h b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/ComplexSchur_MKL.h index 4af8be30f..ef5d8d336 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/ComplexSchur_MKL.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/ComplexSchur_MKL.h @@ -45,7 +45,6 @@ ComplexSchur >& \ ComplexSchur >::compute(const Matrix& matrix, bool computeU) \ { \ typedef Matrix MatrixType; \ - typedef MatrixType::Scalar Scalar; \ typedef MatrixType::RealScalar RealScalar; \ typedef std::complex ComplexScalar; \ \ diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/RealSchur_MKL.h b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/RealSchur_MKL.h index 26f72775c..469d801a8 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/RealSchur_MKL.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/RealSchur_MKL.h @@ -44,10 +44,6 @@ template<> inline \ RealSchur >& \ RealSchur >::compute(const Matrix& matrix, bool computeU) \ { \ - typedef Matrix MatrixType; \ - typedef MatrixType::Scalar Scalar; \ - typedef MatrixType::RealScalar RealScalar; \ -\ eigen_assert(matrix.cols() == matrix.rows()); \ \ lapack_int n = matrix.cols(), sdim, info; \ diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/AngleAxis.h b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/AngleAxis.h index bbf6a7ed8..a8d3cdcfa 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/AngleAxis.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/AngleAxis.h @@ -83,10 +83,17 @@ public: template inline explicit AngleAxis(const MatrixBase& m) { *this = m; } + /** \returns the value of the rotation angle in radian */ Scalar angle() const { return m_angle; } + /** \returns a read-write reference to the stored angle in radian */ Scalar& angle() { return m_angle; } + /** \returns the rotation axis */ const Vector3& axis() const { return m_axis; } + /** \returns a read-write reference to the stored rotation axis. + * + * \warning The rotation axis must remain a \b unit vector. + */ Vector3& axis() { return m_axis; } /** Concatenates two rotations */ diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/ParametrizedLine.h b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/ParametrizedLine.h index 77fa228e6..cf3252df9 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/ParametrizedLine.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/ParametrizedLine.h @@ -129,7 +129,7 @@ public: * determined by \a prec. * * \sa MatrixBase::isApprox() */ - bool isApprox(const ParametrizedLine& other, typename NumTraits::Real prec = NumTraits::dummy_precision()) const + bool isApprox(const ParametrizedLine& other, const typename NumTraits::Real& prec = NumTraits::dummy_precision()) const { return m_origin.isApprox(other.m_origin, prec) && m_direction.isApprox(other.m_direction, prec); } protected: diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Transform.h b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Transform.h index e786e5356..0186f3b82 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Transform.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Transform.h @@ -102,15 +102,15 @@ template struct transform_make_affine; * * However, unlike a plain matrix, the Transform class provides many features * simplifying both its assembly and usage. In particular, it can be composed - * with any other transformations (Transform,Translation,RotationBase,Matrix) + * with any other transformations (Transform,Translation,RotationBase,DiagonalMatrix) * and can be directly used to transform implicit homogeneous vectors. All these * operations are handled via the operator*. For the composition of transformations, * its principle consists to first convert the right/left hand sides of the product * to a compatible (Dim+1)^2 matrix and then perform a pure matrix product. * Of course, internally, operator* tries to perform the minimal number of operations * according to the nature of each terms. Likewise, when applying the transform - * to non homogeneous vectors, the latters are automatically promoted to homogeneous - * one before doing the matrix product. The convertions to homogeneous representations + * to points, the latters are automatically promoted to homogeneous vectors + * before doing the matrix product. The conventions to homogeneous representations * are performed as follow: * * \b Translation t (Dim)x(1): @@ -124,7 +124,7 @@ template struct transform_make_affine; * R & 0\\ * 0\,...\,0 & 1 * \end{array} \right) \f$ - * + * + * \b Scaling \b DiagonalMatrix S (Dim)x(Dim): + * \f$ \left( \begin{array}{cc} + * S & 0\\ + * 0\,...\,0 & 1 + * \end{array} \right) \f$ * - * \b Column \b vector v (Dim)x(1): + * \b Column \b point v (Dim)x(1): * \f$ \left( \begin{array}{c} * v\\ * 1 * \end{array} \right) \f$ * - * \b Set \b of \b column \b vectors V1...Vn (Dim)x(n): + * \b Set \b of \b column \b points V1...Vn (Dim)x(n): * \f$ \left( \begin{array}{ccc} * v_1 & ... & v_n\\ * 1 & ... & 1 @@ -384,26 +390,39 @@ public: /** \returns a writable expression of the translation vector of the transformation */ inline TranslationPart translation() { return TranslationPart(m_matrix,0,Dim); } - /** \returns an expression of the product between the transform \c *this and a matrix expression \a other + /** \returns an expression of the product between the transform \c *this and a matrix expression \a other. * - * The right hand side \a other might be either: - * \li a vector of size Dim, + * The right-hand-side \a other can be either: * \li an homogeneous vector of size Dim+1, - * \li a set of vectors of size Dim x Dynamic, - * \li a set of homogeneous vectors of size Dim+1 x Dynamic, - * \li a linear transformation matrix of size Dim x Dim, - * \li an affine transformation matrix of size Dim x Dim+1, + * \li a set of homogeneous vectors of size Dim+1 x N, * \li a transformation matrix of size Dim+1 x Dim+1. + * + * Moreover, if \c *this represents an affine transformation (i.e., Mode!=Projective), then \a other can also be: + * \li a point of size Dim (computes: \code this->linear() * other + this->translation()\endcode), + * \li a set of N points as a Dim x N matrix (computes: \code (this->linear() * other).colwise() + this->translation()\endcode), + * + * In all cases, the return type is a matrix or vector of same sizes as the right-hand-side \a other. + * + * If you want to interpret \a other as a linear or affine transformation, then first convert it to a Transform<> type, + * or do your own cooking. + * + * Finally, if you want to apply Affine transformations to vectors, then explicitly apply the linear part only: + * \code + * Affine3f A; + * Vector3f v1, v2; + * v2 = A.linear() * v1; + * \endcode + * */ // note: this function is defined here because some compilers cannot find the respective declaration template - EIGEN_STRONG_INLINE const typename internal::transform_right_product_impl::ResultType + EIGEN_STRONG_INLINE const typename OtherDerived::PlainObject operator * (const EigenBase &other) const { return internal::transform_right_product_impl::run(*this,other.derived()); } /** \returns the product expression of a transformation matrix \a a times a transform \a b * - * The left hand side \a other might be either: + * The left hand side \a other can be either: * \li a linear transformation matrix of size Dim x Dim, * \li an affine transformation matrix of size Dim x Dim+1, * \li a general transformation matrix of size Dim+1 x Dim+1. diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Translation.h b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Translation.h index 7fda179cc..2e7798686 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Translation.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Translation.h @@ -162,7 +162,7 @@ public: * determined by \a prec. * * \sa MatrixBase::isApprox() */ - bool isApprox(const Translation& other, typename NumTraits::Real prec = NumTraits::dummy_precision()) const + bool isApprox(const Translation& other, const typename NumTraits::Real& prec = NumTraits::dummy_precision()) const { return m_coeffs.isApprox(other.m_coeffs, prec); } }; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/ConjugateGradient.h b/gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/ConjugateGradient.h index 1a7e569c8..7dd4010c3 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/ConjugateGradient.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/ConjugateGradient.h @@ -139,6 +139,8 @@ struct traits > * By default the iterations start with x=0 as an initial guess of the solution. * One can control the start using the solveWithGuess() method. * + * ConjugateGradient can also be used in a matrix-free context, see the following \link MatrixfreeSolverExample example \endlink. + * * \sa class SimplicialCholesky, DiagonalPreconditioner, IdentityPreconditioner */ template< typename _MatrixType, int _UpLo, typename _Preconditioner> diff --git a/gtsam/3rdparty/Eigen/Eigen/src/LU/FullPivLU.h b/gtsam/3rdparty/Eigen/Eigen/src/LU/FullPivLU.h index 26bc71447..e38470463 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/LU/FullPivLU.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/LU/FullPivLU.h @@ -688,7 +688,7 @@ struct solve_retval, Rhs> */ const Index rows = dec().rows(), cols = dec().cols(), - nonzero_pivots = dec().nonzeroPivots(); + nonzero_pivots = dec().rank(); eigen_assert(rhs().rows() == rows); const Index smalldim = (std::min)(rows, cols); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/OrderingMethods/Amd.h b/gtsam/3rdparty/Eigen/Eigen/src/OrderingMethods/Amd.h index 70550b8a9..658b954c7 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/OrderingMethods/Amd.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/OrderingMethods/Amd.h @@ -8,7 +8,7 @@ NOTE: this routine has been adapted from the CSparse library: Copyright (c) 2006, Timothy A. Davis. -http://www.cise.ufl.edu/research/sparse/CSparse +http://www.suitesparse.com CSparse is free software; you can redistribute it and/or modify it under the terms of the GNU Lesser General Public diff --git a/gtsam/3rdparty/Eigen/Eigen/src/OrderingMethods/Eigen_Colamd.h b/gtsam/3rdparty/Eigen/Eigen/src/OrderingMethods/Eigen_Colamd.h index 44548f660..359fd4417 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/OrderingMethods/Eigen_Colamd.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/OrderingMethods/Eigen_Colamd.h @@ -41,12 +41,8 @@ // // The colamd/symamd library is available at // -// http://www.cise.ufl.edu/research/sparse/colamd/ +// http://www.suitesparse.com -// This is the http://www.cise.ufl.edu/research/sparse/colamd/colamd.h -// file. It is required by the colamd.c, colamdmex.c, and symamdmex.c -// files, and by any C code that calls the routines whose prototypes are -// listed below, or that uses the colamd/symamd definitions listed below. #ifndef EIGEN_COLAMD_H #define EIGEN_COLAMD_H @@ -102,9 +98,6 @@ namespace internal { /* === Definitions ========================================================== */ /* ========================================================================== */ -#define COLAMD_MAX(a,b) (((a) > (b)) ? (a) : (b)) -#define COLAMD_MIN(a,b) (((a) < (b)) ? (a) : (b)) - #define ONES_COMPLEMENT(r) (-(r)-1) /* -------------------------------------------------------------------------- */ @@ -516,7 +509,7 @@ static Index init_rows_cols /* returns true if OK, or false otherwise */ Col [col].start = p [col] ; Col [col].length = p [col+1] - p [col] ; - if (Col [col].length < 0) + if ((Col [col].length) < 0) // extra parentheses to work-around gcc bug 10200 { /* column pointers must be non-decreasing */ stats [COLAMD_STATUS] = COLAMD_ERROR_col_length_negative ; @@ -739,8 +732,8 @@ static void init_scoring /* === Extract knobs ==================================================== */ - dense_row_count = COLAMD_MAX (0, COLAMD_MIN (knobs [COLAMD_DENSE_ROW] * n_col, n_col)) ; - dense_col_count = COLAMD_MAX (0, COLAMD_MIN (knobs [COLAMD_DENSE_COL] * n_row, n_row)) ; + dense_row_count = std::max(0, (std::min)(Index(knobs [COLAMD_DENSE_ROW] * n_col), n_col)) ; + dense_col_count = std::max(0, (std::min)(Index(knobs [COLAMD_DENSE_COL] * n_row), n_row)) ; COLAMD_DEBUG1 (("colamd: densecount: %d %d\n", dense_row_count, dense_col_count)) ; max_deg = 0 ; n_col2 = n_col ; @@ -804,7 +797,7 @@ static void init_scoring else { /* keep track of max degree of remaining rows */ - max_deg = COLAMD_MAX (max_deg, deg) ; + max_deg = (std::max)(max_deg, deg) ; } } COLAMD_DEBUG1 (("colamd: Dense and null rows killed: %d\n", n_row - n_row2)) ; @@ -842,7 +835,7 @@ static void init_scoring /* add row's external degree */ score += Row [row].shared1.degree - 1 ; /* guard against integer overflow */ - score = COLAMD_MIN (score, n_col) ; + score = (std::min)(score, n_col) ; } /* determine pruned column length */ col_length = (Index) (new_cp - &A [Col [c].start]) ; @@ -914,7 +907,7 @@ static void init_scoring head [score] = c ; /* see if this score is less than current min */ - min_score = COLAMD_MIN (min_score, score) ; + min_score = (std::min)(min_score, score) ; } @@ -1040,7 +1033,7 @@ static Index find_ordering /* return the number of garbage collections */ /* === Garbage_collection, if necessary ============================= */ - needed_memory = COLAMD_MIN (pivot_col_score, n_col - k) ; + needed_memory = (std::min)(pivot_col_score, n_col - k) ; if (pfree + needed_memory >= Alen) { pfree = Eigen::internal::garbage_collection (n_row, n_col, Row, Col, A, &A [pfree]) ; @@ -1099,7 +1092,7 @@ static Index find_ordering /* return the number of garbage collections */ /* clear tag on pivot column */ Col [pivot_col].shared1.thickness = pivot_col_thickness ; - max_deg = COLAMD_MAX (max_deg, pivot_row_degree) ; + max_deg = (std::max)(max_deg, pivot_row_degree) ; /* === Kill all rows used to construct pivot row ==================== */ @@ -1273,7 +1266,7 @@ static Index find_ordering /* return the number of garbage collections */ /* add set difference */ cur_score += row_mark - tag_mark ; /* integer overflow... */ - cur_score = COLAMD_MIN (cur_score, n_col) ; + cur_score = (std::min)(cur_score, n_col) ; } /* recompute the column's length */ @@ -1386,7 +1379,7 @@ static Index find_ordering /* return the number of garbage collections */ cur_score -= Col [col].shared1.thickness ; /* make sure score is less or equal than the max score */ - cur_score = COLAMD_MIN (cur_score, max_score) ; + cur_score = (std::min)(cur_score, max_score) ; COLAMD_ASSERT (cur_score >= 0) ; /* store updated score */ @@ -1409,7 +1402,7 @@ static Index find_ordering /* return the number of garbage collections */ head [cur_score] = col ; /* see if this score is less than current min */ - min_score = COLAMD_MIN (min_score, cur_score) ; + min_score = (std::min)(min_score, cur_score) ; } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/QR/ColPivHouseholderQR_MKL.h b/gtsam/3rdparty/Eigen/Eigen/src/QR/ColPivHouseholderQR_MKL.h index b1332be5e..448feb49b 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/QR/ColPivHouseholderQR_MKL.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/QR/ColPivHouseholderQR_MKL.h @@ -49,7 +49,6 @@ ColPivHouseholderQR MatrixType; \ - typedef MatrixType::Scalar Scalar; \ typedef MatrixType::RealScalar RealScalar; \ Index rows = matrix.rows();\ Index cols = matrix.cols();\ diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SVD/JacobiSVD.h b/gtsam/3rdparty/Eigen/Eigen/src/SVD/JacobiSVD.h index 1b2977419..89ace381e 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SVD/JacobiSVD.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SVD/JacobiSVD.h @@ -816,7 +816,7 @@ void JacobiSVD::allocate(Index rows, Index cols, u if(m_cols>m_rows) m_qr_precond_morecols.allocate(*this); if(m_rows>m_cols) m_qr_precond_morerows.allocate(*this); - if(m_cols!=m_cols) m_scaledMatrix.resize(rows,cols); + if(m_rows!=m_cols) m_scaledMatrix.resize(rows,cols); } template diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SVD/JacobiSVD_MKL.h b/gtsam/3rdparty/Eigen/Eigen/src/SVD/JacobiSVD_MKL.h index f76a7082a..ab6d96261 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SVD/JacobiSVD_MKL.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SVD/JacobiSVD_MKL.h @@ -45,8 +45,8 @@ JacobiSVD, ColPiv JacobiSVD, ColPivHouseholderQRPreconditioner>::compute(const Matrix& matrix, unsigned int computationOptions) \ { \ typedef Matrix MatrixType; \ - typedef MatrixType::Scalar Scalar; \ - typedef MatrixType::RealScalar RealScalar; \ + /*typedef MatrixType::Scalar Scalar;*/ \ + /*typedef MatrixType::RealScalar RealScalar;*/ \ allocate(matrix.rows(), matrix.cols(), computationOptions); \ \ /*const RealScalar precision = RealScalar(2) * NumTraits::epsilon();*/ \ diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseBlock.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseBlock.h index 0c90bafbe..4f4983508 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseBlock.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseBlock.h @@ -364,10 +364,11 @@ public: protected: + EIGEN_INHERIT_ASSIGNMENT_OPERATORS(BlockImpl) + typename SparseMatrixType::Nested m_matrix; Index m_outerStart; const internal::variable_if_dynamic m_outerSize; - }; //---------- @@ -528,7 +529,8 @@ public: const internal::variable_if_dynamic m_startCol; const internal::variable_if_dynamic m_blockRows; const internal::variable_if_dynamic m_blockCols; - + private: + Index nonZeros() const; }; } // end namespace Eigen diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseCwiseBinaryOp.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseCwiseBinaryOp.h index 4ca912833..546273759 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseCwiseBinaryOp.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseCwiseBinaryOp.h @@ -55,10 +55,9 @@ class CwiseBinaryOpImpl EIGEN_SPARSE_PUBLIC_INTERFACE(Derived) CwiseBinaryOpImpl() { - typedef typename internal::traits::StorageKind LhsStorageKind; - typedef typename internal::traits::StorageKind RhsStorageKind; EIGEN_STATIC_ASSERT(( - (!internal::is_same::value) + (!internal::is_same::StorageKind, + typename internal::traits::StorageKind>::value) || ((Lhs::Flags&RowMajorBit) == (Rhs::Flags&RowMajorBit))), THE_STORAGE_ORDER_OF_BOTH_SIDES_MUST_MATCH); } diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseView.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseView.h index fd8450463..2820b39b8 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseView.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseView.h @@ -35,9 +35,9 @@ class SparseView : public SparseMatrixBase > public: EIGEN_SPARSE_PUBLIC_INTERFACE(SparseView) - SparseView(const MatrixType& mat, const Scalar& m_reference = Scalar(0), - typename NumTraits::Real m_epsilon = NumTraits::dummy_precision()) : - m_matrix(mat), m_reference(m_reference), m_epsilon(m_epsilon) {} + explicit SparseView(const MatrixType& mat, const Scalar& reference = Scalar(0), + const RealScalar &epsilon = NumTraits::dummy_precision()) + : m_matrix(mat), m_reference(reference), m_epsilon(epsilon) {} class InnerIterator; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/StlSupport/StdDeque.h b/gtsam/3rdparty/Eigen/Eigen/src/StlSupport/StdDeque.h index aaf66330b..69a46b2b8 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/StlSupport/StdDeque.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/StlSupport/StdDeque.h @@ -13,32 +13,24 @@ #include "details.h" -// Define the explicit instantiation (e.g. necessary for the Intel compiler) -#if defined(__INTEL_COMPILER) || defined(__GNUC__) - #define EIGEN_EXPLICIT_STL_DEQUE_INSTANTIATION(...) template class std::deque<__VA_ARGS__, EIGEN_ALIGNED_ALLOCATOR<__VA_ARGS__> >; -#else - #define EIGEN_EXPLICIT_STL_DEQUE_INSTANTIATION(...) -#endif - /** * This section contains a convenience MACRO which allows an easy specialization of * std::deque such that for data types with alignment issues the correct allocator * is used automatically. */ #define EIGEN_DEFINE_STL_DEQUE_SPECIALIZATION(...) \ -EIGEN_EXPLICIT_STL_DEQUE_INSTANTIATION(__VA_ARGS__) \ namespace std \ { \ - template \ - class deque<__VA_ARGS__, _Ay> \ + template<> \ + class deque<__VA_ARGS__, std::allocator<__VA_ARGS__> > \ : public deque<__VA_ARGS__, EIGEN_ALIGNED_ALLOCATOR<__VA_ARGS__> > \ { \ typedef deque<__VA_ARGS__, EIGEN_ALIGNED_ALLOCATOR<__VA_ARGS__> > deque_base; \ public: \ typedef __VA_ARGS__ value_type; \ - typedef typename deque_base::allocator_type allocator_type; \ - typedef typename deque_base::size_type size_type; \ - typedef typename deque_base::iterator iterator; \ + typedef deque_base::allocator_type allocator_type; \ + typedef deque_base::size_type size_type; \ + typedef deque_base::iterator iterator; \ explicit deque(const allocator_type& a = allocator_type()) : deque_base(a) {} \ template \ deque(InputIterator first, InputIterator last, const allocator_type& a = allocator_type()) : deque_base(first, last, a) {} \ diff --git a/gtsam/3rdparty/Eigen/Eigen/src/StlSupport/StdList.h b/gtsam/3rdparty/Eigen/Eigen/src/StlSupport/StdList.h index 3c742430c..050c2373e 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/StlSupport/StdList.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/StlSupport/StdList.h @@ -12,32 +12,24 @@ #include "details.h" -// Define the explicit instantiation (e.g. necessary for the Intel compiler) -#if defined(__INTEL_COMPILER) || defined(__GNUC__) - #define EIGEN_EXPLICIT_STL_LIST_INSTANTIATION(...) template class std::list<__VA_ARGS__, EIGEN_ALIGNED_ALLOCATOR<__VA_ARGS__> >; -#else - #define EIGEN_EXPLICIT_STL_LIST_INSTANTIATION(...) -#endif - /** * This section contains a convenience MACRO which allows an easy specialization of * std::list such that for data types with alignment issues the correct allocator * is used automatically. */ #define EIGEN_DEFINE_STL_LIST_SPECIALIZATION(...) \ -EIGEN_EXPLICIT_STL_LIST_INSTANTIATION(__VA_ARGS__) \ namespace std \ { \ - template \ - class list<__VA_ARGS__, _Ay> \ + template<> \ + class list<__VA_ARGS__, std::allocator<__VA_ARGS__> > \ : public list<__VA_ARGS__, EIGEN_ALIGNED_ALLOCATOR<__VA_ARGS__> > \ { \ typedef list<__VA_ARGS__, EIGEN_ALIGNED_ALLOCATOR<__VA_ARGS__> > list_base; \ public: \ typedef __VA_ARGS__ value_type; \ - typedef typename list_base::allocator_type allocator_type; \ - typedef typename list_base::size_type size_type; \ - typedef typename list_base::iterator iterator; \ + typedef list_base::allocator_type allocator_type; \ + typedef list_base::size_type size_type; \ + typedef list_base::iterator iterator; \ explicit list(const allocator_type& a = allocator_type()) : list_base(a) {} \ template \ list(InputIterator first, InputIterator last, const allocator_type& a = allocator_type()) : list_base(first, last, a) {} \ diff --git a/gtsam/3rdparty/Eigen/doc/CMakeLists.txt b/gtsam/3rdparty/Eigen/doc/CMakeLists.txt index 2fc2a0dfc..02790ee43 100644 --- a/gtsam/3rdparty/Eigen/doc/CMakeLists.txt +++ b/gtsam/3rdparty/Eigen/doc/CMakeLists.txt @@ -89,6 +89,7 @@ add_dependencies(doc-unsupported-prerequisites unsupported_snippets unsupported_ add_custom_target(doc ALL COMMAND doxygen COMMAND doxygen Doxyfile-unsupported + COMMAND ${CMAKE_COMMAND} -E copy ${Eigen_BINARY_DIR}/doc/html/group__TopicUnalignedArrayAssert.html ${Eigen_BINARY_DIR}/doc/html/TopicUnalignedArrayAssert.html COMMAND ${CMAKE_COMMAND} -E rename html eigen-doc COMMAND ${CMAKE_COMMAND} -E remove eigen-doc/eigen-doc.tgz COMMAND ${CMAKE_COMMAND} -E tar cfz eigen-doc.tgz eigen-doc diff --git a/gtsam/3rdparty/Eigen/doc/Manual.dox b/gtsam/3rdparty/Eigen/doc/Manual.dox index 55057d213..52427f066 100644 --- a/gtsam/3rdparty/Eigen/doc/Manual.dox +++ b/gtsam/3rdparty/Eigen/doc/Manual.dox @@ -121,6 +121,8 @@ namespace Eigen { \ingroup Sparse_chapter */ /** \addtogroup TopicSparseSystems \ingroup Sparse_chapter */ +/** \addtogroup MatrixfreeSolverExample + \ingroup Sparse_chapter */ /** \addtogroup Sparse_Reference \ingroup Sparse_chapter */ diff --git a/gtsam/3rdparty/Eigen/doc/PreprocessorDirectives.dox b/gtsam/3rdparty/Eigen/doc/PreprocessorDirectives.dox index 8a2968ebb..cfaba35d8 100644 --- a/gtsam/3rdparty/Eigen/doc/PreprocessorDirectives.dox +++ b/gtsam/3rdparty/Eigen/doc/PreprocessorDirectives.dox @@ -91,6 +91,7 @@ following macros are supported; none of them are defined by default. - \b EIGEN_MATRIX_PLUGIN - filename of plugin for extending the Matrix class. - \b EIGEN_MATRIXBASE_PLUGIN - filename of plugin for extending the MatrixBase class. - \b EIGEN_PLAINOBJECTBASE_PLUGIN - filename of plugin for extending the PlainObjectBase class. + - \b EIGEN_MAPBASE_PLUGIN - filename of plugin for extending the MapBase class. - \b EIGEN_QUATERNIONBASE_PLUGIN - filename of plugin for extending the QuaternionBase class. - \b EIGEN_SPARSEMATRIX_PLUGIN - filename of plugin for extending the SparseMatrix class. - \b EIGEN_SPARSEMATRIXBASE_PLUGIN - filename of plugin for extending the SparseMatrixBase class. diff --git a/gtsam/3rdparty/Eigen/doc/SparseLinearSystems.dox b/gtsam/3rdparty/Eigen/doc/SparseLinearSystems.dox index c00be10d3..051a02ed9 100644 --- a/gtsam/3rdparty/Eigen/doc/SparseLinearSystems.dox +++ b/gtsam/3rdparty/Eigen/doc/SparseLinearSystems.dox @@ -35,17 +35,17 @@ They are summarized in the following table: Requires the PaStiX package, \b CeCILL-C optimized for tough problems and symmetric patterns CholmodSupernodalLLT\link CholmodSupport_Module CholmodSupport \endlinkDirect LLt factorizationSPDFill-in reducing, Leverage fast dense algebra - Requires the SuiteSparse package, \b GPL + Requires the SuiteSparse package, \b GPL UmfPackLU\link UmfPackSupport_Module UmfPackSupport \endlinkDirect LU factorizationSquareFill-in reducing, Leverage fast dense algebra - Requires the SuiteSparse package, \b GPL + Requires the SuiteSparse package, \b GPL SuperLU\link SuperLUSupport_Module SuperLUSupport \endlinkDirect LU factorizationSquareFill-in reducing, Leverage fast dense algebra Requires the SuperLU library, (BSD-like) SPQR\link SPQRSupport_Module SPQRSupport \endlink QR factorization Any, rectangularfill-in reducing, multithreaded, fast dense algebra - requires the SuiteSparse package, \b GPL recommended for linear least-squares problems, has a rank-revealing feature + requires the SuiteSparse package, \b GPL recommended for linear least-squares problems, has a rank-revealing feature Here \c SPD means symmetric positive definite. diff --git a/gtsam/3rdparty/Eigen/doc/SparseQuickReference.dox b/gtsam/3rdparty/Eigen/doc/SparseQuickReference.dox index d04ac35c5..e0a30edcc 100644 --- a/gtsam/3rdparty/Eigen/doc/SparseQuickReference.dox +++ b/gtsam/3rdparty/Eigen/doc/SparseQuickReference.dox @@ -21,7 +21,7 @@ i.e either row major or column major. The default is column major. Most arithmet Resize/Reserve \code - sm1.resize(m,n); //Change sm1 to a m x n matrix. + sm1.resize(m,n); // Change sm1 to a m x n matrix. sm1.reserve(nnz); // Allocate room for nnz nonzeros elements. \endcode @@ -151,10 +151,10 @@ It is easy to perform arithmetic operations on sparse matrices provided that the Permutation \code -perm.indices(); // Reference to the vector of indices +perm.indices(); // Reference to the vector of indices sm1.twistedBy(perm); // Permute rows and columns -sm2 = sm1 * perm; //Permute the columns -sm2 = perm * sm1; // Permute the columns +sm2 = sm1 * perm; // Permute the columns +sm2 = perm * sm1; // Permute the columns \endcode @@ -181,9 +181,9 @@ sm2 = perm * sm1; // Permute the columns \section sparseotherops Other supported operations - + + - + - - + + - + + - - - + + + - - +
Operations Code Notes
Code Notes
Sub-matrices
Sub-matrices \code sm1.block(startRow, startCol, rows, cols); @@ -193,25 +193,31 @@ sm2 = perm * sm1; // Permute the columns sm1.bottomLeftCorner( rows, cols); sm1.bottomRightCorner( rows, cols); \endcode - +Contrary to dense matrices, here all these methods are read-only.\n +See \ref TutorialSparse_SubMatrices and below for read-write sub-matrices. +
Range
Range
\code - sm1.innerVector(outer); - sm1.innerVectors(start, size); - sm1.leftCols(size); - sm2.rightCols(size); - sm1.middleRows(start, numRows); - sm1.middleCols(start, numCols); - sm1.col(j); + sm1.innerVector(outer); // RW + sm1.innerVectors(start, size); // RW + sm1.leftCols(size); // RW + sm2.rightCols(size); // RO because sm2 is row-major + sm1.middleRows(start, numRows); // RO becasue sm1 is column-major + sm1.middleCols(start, numCols); // RW + sm1.col(j); // RW \endcode A inner vector is either a row (for row-major) or a column (for column-major). As stated earlier, the evaluation can be done in a matrix with different storage order +A inner vector is either a row (for row-major) or a column (for column-major).\n +As stated earlier, for a read-write sub-matrix (RW), the evaluation can be done in a matrix with different storage order. +
Triangular and selfadjoint views
Triangular and selfadjoint views \code sm2 = sm1.triangularview(); @@ -222,26 +228,30 @@ sm2 = perm * sm1; // Permute the columns \code \endcode
Triangular solve
Triangular solve
\code dv2 = sm1.triangularView().solve(dv1); - dv2 = sm1.topLeftCorner(size, size).triangularView().solve(dv1); + dv2 = sm1.topLeftCorner(size, size) + .triangularView().solve(dv1); \endcode For general sparse solve, Use any suitable module described at \ref TopicSparseSystems
Low-level API
Low-level API \code -sm1.valuePtr(); // Pointer to the values -sm1.innerIndextr(); // Pointer to the indices. -sm1.outerIndexPtr(); //Pointer to the beginning of each inner vector +sm1.valuePtr(); // Pointer to the values +sm1.innerIndextr(); // Pointer to the indices. +sm1.outerIndexPtr(); // Pointer to the beginning of each inner vector \endcode If the matrix is not in compressed form, makeCompressed() should be called before. Note that these functions are mostly provided for interoperability purposes with external libraries. A better access to the values of the matrix is done by using the InnerIterator class as described in \link TutorialSparse the Tutorial Sparse \endlink section +If the matrix is not in compressed form, makeCompressed() should be called before.\n +Note that these functions are mostly provided for interoperability purposes with external libraries.\n +A better access to the values of the matrix is done by using the InnerIterator class as described in \link TutorialSparse the Tutorial Sparse \endlink section
*/ diff --git a/gtsam/3rdparty/Eigen/doc/TutorialSparse.dox b/gtsam/3rdparty/Eigen/doc/TutorialSparse.dox index fa2a3ad8b..ee206cc42 100644 --- a/gtsam/3rdparty/Eigen/doc/TutorialSparse.dox +++ b/gtsam/3rdparty/Eigen/doc/TutorialSparse.dox @@ -241,11 +241,11 @@ In the following \em sm denotes a sparse matrix, \em sv a sparse vector, \em dm sm1.real() sm1.imag() -sm1 0.5*sm1 sm1+sm2 sm1-sm2 sm1.cwiseProduct(sm2) \endcode -However, a strong restriction is that the storage orders must match. For instance, in the following example: +However, a strong restriction is that the storage orders must match. For instance, in the following example: \code sm4 = sm1 + sm2 + sm3; \endcode -sm1, sm2, and sm3 must all be row-major or all column major. +sm1, sm2, and sm3 must all be row-major or all column-major. On the other hand, there is no restriction on the target matrix sm4. For instance, this means that for computing \f$ A^T + A \f$, the matrix \f$ A^T \f$ must be evaluated into a temporary matrix of compatible storage order: \code @@ -307,6 +307,26 @@ sm2 = sm1.transpose() * P; \endcode +\subsection TutorialSparse_SubMatrices Block operations + +Regarding read-access, sparse matrices expose the same API than for dense matrices to access to sub-matrices such as blocks, columns, and rows. See \ref TutorialBlockOperations for a detailed introduction. +However, for performance reasons, writing to a sub-sparse-matrix is much more limited, and currently only contiguous sets of columns (resp. rows) of a column-major (resp. row-major) SparseMatrix are writable. Moreover, this information has to be known at compile-time, leaving out methods such as block(...) and corner*(...). The available API for write-access to a SparseMatrix are summarized below: +\code +SparseMatrix sm1; +sm1.col(j) = ...; +sm1.leftCols(ncols) = ...; +sm1.middleCols(j,ncols) = ...; +sm1.rightCols(ncols) = ...; + +SparseMatrix sm2; +sm2.row(i) = ...; +sm2.topRows(nrows) = ...; +sm2.middleRows(i,nrows) = ...; +sm2.bottomRows(nrows) = ...; +\endcode + +In addition, sparse matrices expose the SparseMatrixBase::innerVector() and SparseMatrixBase::innerVectors() methods, which are aliases to the col/middleCols methods for a column-major storage, and to the row/middleRows methods for a row-major storage. + \subsection TutorialSparse_TriangularSelfadjoint Triangular and selfadjoint views Just as with dense matrices, the triangularView() function can be used to address a triangular part of the matrix, and perform triangular solves with a dense right hand side: diff --git a/gtsam/3rdparty/Eigen/doc/UnalignedArrayAssert.dox b/gtsam/3rdparty/Eigen/doc/UnalignedArrayAssert.dox index 8c97d7874..b0d6e18f2 100644 --- a/gtsam/3rdparty/Eigen/doc/UnalignedArrayAssert.dox +++ b/gtsam/3rdparty/Eigen/doc/UnalignedArrayAssert.dox @@ -7,8 +7,8 @@ Hello! You are seeing this webpage because your program terminated on an asserti my_program: path/to/eigen/Eigen/src/Core/DenseStorage.h:44: Eigen::internal::matrix_array::internal::matrix_array() [with T = double, int Size = 2, int MatrixOptions = 2, bool Align = true]: -Assertion `(reinterpret_cast(array) & 0xf) == 0 && "this assertion -is explained here: http://eigen.tuxfamily.org/dox/UnalignedArrayAssert.html +Assertion `(reinterpret_cast(array) & (sizemask)) == 0 && "this assertion +is explained here: http://eigen.tuxfamily.org/dox/group__TopicUnalignedArrayAssert.html **** READ THIS WEB PAGE !!! ****"' failed. @@ -46,9 +46,9 @@ then you need to read this separate page: \ref TopicStructHavingEigenMembers "St Note that here, Eigen::Vector2d is only used as an example, more generally the issue arises for all \ref TopicFixedSizeVectorizable "fixed-size vectorizable Eigen types". -\section c2 Cause 2: STL Containers +\section c2 Cause 2: STL Containers or manual memory allocation -If you use STL Containers such as std::vector, std::map, ..., with Eigen objects, or with classes containing Eigen objects, like this, +If you use STL Containers such as std::vector, std::map, ..., with %Eigen objects, or with classes containing %Eigen objects, like this, \code std::vector my_vector; @@ -60,6 +60,8 @@ then you need to read this separate page: \ref TopicStlContainers "Using STL Con Note that here, Eigen::Matrix2f is only used as an example, more generally the issue arises for all \ref TopicFixedSizeVectorizable "fixed-size vectorizable Eigen types" and \ref TopicStructHavingEigenMembers "structures having such Eigen objects as member". +The same issue will be exhibited by any classes/functions by-passing operator new to allocate memory, that is, by performing custom memory allocation followed by calls to the placement new operator. This is for instance typically the case of \c std::make_shared or \c std::allocate_shared for which is the solution is to use an \ref aligned_allocator "aligned allocator" as detailed in the \ref TopicStlContainers "solution for STL containers". + \section c3 Cause 3: Passing Eigen objects by value If some function in your code is getting an Eigen object passed by value, like this, @@ -107,7 +109,10 @@ Two possibilities: 128-bit alignment code and thus preserves ABI compatibility, but completely disables vectorization. -For more information, see this FAQ. +If you want to know why defining EIGEN_DONT_VECTORIZE does not by itself disable 128-bit alignment and the assertion, here's the explanation: + +It doesn't disable the assertion, because otherwise code that runs fine without vectorization would suddenly crash when enabling vectorization. +It doesn't disable 128bit alignment, because that would mean that vectorized and non-vectorized code are not mutually ABI-compatible. This ABI compatibility is very important, even for people who develop only an in-house application, as for instance one may want to have in the same application a vectorized path and a non-vectorized path. */ diff --git a/gtsam/3rdparty/Eigen/eigen3.pc.in b/gtsam/3rdparty/Eigen/eigen3.pc.in index c5855de33..3368a3aa1 100644 --- a/gtsam/3rdparty/Eigen/eigen3.pc.in +++ b/gtsam/3rdparty/Eigen/eigen3.pc.in @@ -1,6 +1,9 @@ +prefix=@CMAKE_INSTALL_PREFIX@ +exec_prefix=${prefix} + Name: Eigen3 Description: A C++ template library for linear algebra: vectors, matrices, and related algorithms Requires: -Version: ${EIGEN_VERSION_NUMBER} +Version: @EIGEN_VERSION_NUMBER@ Libs: -Cflags: -I${INCLUDE_INSTALL_DIR} +Cflags: -I${prefix}/@INCLUDE_INSTALL_DIR@ diff --git a/gtsam/3rdparty/Eigen/test/CMakeLists.txt b/gtsam/3rdparty/Eigen/test/CMakeLists.txt index 3739268d2..c0d8a4e28 100644 --- a/gtsam/3rdparty/Eigen/test/CMakeLists.txt +++ b/gtsam/3rdparty/Eigen/test/CMakeLists.txt @@ -202,7 +202,9 @@ ei_add_test(geo_alignedbox) ei_add_test(stdvector) ei_add_test(stdvector_overload) ei_add_test(stdlist) +ei_add_test(stdlist_overload) ei_add_test(stddeque) +ei_add_test(stddeque_overload) ei_add_test(resize) ei_add_test(sparse_vector) ei_add_test(sparse_basic) diff --git a/gtsam/3rdparty/Eigen/test/dynalloc.cpp b/gtsam/3rdparty/Eigen/test/dynalloc.cpp index 7e41bfa97..ef92c0507 100644 --- a/gtsam/3rdparty/Eigen/test/dynalloc.cpp +++ b/gtsam/3rdparty/Eigen/test/dynalloc.cpp @@ -87,6 +87,32 @@ template void check_dynaligned() delete obj; } +template void check_custom_new_delete() +{ + { + T* t = new T; + delete t; + } + + { + std::size_t N = internal::random(1,10); + T* t = new T[N]; + delete[] t; + } + +#ifdef EIGEN_ALIGN + { + T* t = static_cast((T::operator new)(sizeof(T))); + (T::operator delete)(t, sizeof(T)); + } + + { + T* t = static_cast((T::operator new)(sizeof(T))); + (T::operator delete)(t); + } +#endif +} + void test_dynalloc() { // low level dynamic memory allocation @@ -94,7 +120,9 @@ void test_dynalloc() CALL_SUBTEST(check_aligned_malloc()); CALL_SUBTEST(check_aligned_new()); CALL_SUBTEST(check_aligned_stack_alloc()); - + + // check static allocation, who knows ? + #if EIGEN_ALIGN_STATICALLY for (int i=0; i() ); @@ -102,10 +130,13 @@ void test_dynalloc() CALL_SUBTEST(check_dynaligned() ); CALL_SUBTEST(check_dynaligned() ); CALL_SUBTEST(check_dynaligned() ); + + CALL_SUBTEST( check_custom_new_delete() ); + CALL_SUBTEST( check_custom_new_delete() ); + CALL_SUBTEST( check_custom_new_delete() ); + CALL_SUBTEST( check_custom_new_delete() ); } - - // check static allocation, who knows ? - #if EIGEN_ALIGN_STATICALLY + { MyStruct foo0; VERIFY(size_t(foo0.avec.data())%ALIGNMENT==0); MyClassA fooA; VERIFY(size_t(fooA.avec.data())%ALIGNMENT==0); diff --git a/gtsam/3rdparty/Eigen/test/product.h b/gtsam/3rdparty/Eigen/test/product.h index 0b3abe402..0d054ff46 100644 --- a/gtsam/3rdparty/Eigen/test/product.h +++ b/gtsam/3rdparty/Eigen/test/product.h @@ -136,9 +136,27 @@ template void product(const MatrixType& m) VERIFY_IS_APPROX(res.col(r).noalias() = square.adjoint() * square.col(r), (square.adjoint() * square.col(r)).eval()); VERIFY_IS_APPROX(res.col(r).noalias() = square * square.col(r), (square * square.col(r)).eval()); + // vector at runtime (see bug 1166) + { + RowSquareMatrixType ref(square); + ColSquareMatrixType ref2(square2); + ref = res = square; + VERIFY_IS_APPROX(res.block(0,0,1,rows).noalias() = m1.col(0).transpose() * square.transpose(), (ref.row(0) = m1.col(0).transpose() * square.transpose())); + VERIFY_IS_APPROX(res.block(0,0,1,rows).noalias() = m1.block(0,0,rows,1).transpose() * square.transpose(), (ref.row(0) = m1.col(0).transpose() * square.transpose())); + VERIFY_IS_APPROX(res.block(0,0,1,rows).noalias() = m1.col(0).transpose() * square, (ref.row(0) = m1.col(0).transpose() * square)); + VERIFY_IS_APPROX(res.block(0,0,1,rows).noalias() = m1.block(0,0,rows,1).transpose() * square, (ref.row(0) = m1.col(0).transpose() * square)); + ref2 = res2 = square2; + VERIFY_IS_APPROX(res2.block(0,0,1,cols).noalias() = m1.row(0) * square2.transpose(), (ref2.row(0) = m1.row(0) * square2.transpose())); + VERIFY_IS_APPROX(res2.block(0,0,1,cols).noalias() = m1.block(0,0,1,cols) * square2.transpose(), (ref2.row(0) = m1.row(0) * square2.transpose())); + VERIFY_IS_APPROX(res2.block(0,0,1,cols).noalias() = m1.row(0) * square2, (ref2.row(0) = m1.row(0) * square2)); + VERIFY_IS_APPROX(res2.block(0,0,1,cols).noalias() = m1.block(0,0,1,cols) * square2, (ref2.row(0) = m1.row(0) * square2)); + } + // inner product - Scalar x = square2.row(c) * square2.col(c2); - VERIFY_IS_APPROX(x, square2.row(c).transpose().cwiseProduct(square2.col(c2)).sum()); + { + Scalar x = square2.row(c) * square2.col(c2); + VERIFY_IS_APPROX(x, square2.row(c).transpose().cwiseProduct(square2.col(c2)).sum()); + } // outer product VERIFY_IS_APPROX(m1.col(c) * m1.row(r), m1.block(0,c,rows,1) * m1.block(r,0,1,cols)); @@ -146,5 +164,18 @@ template void product(const MatrixType& m) VERIFY_IS_APPROX(m1.block(0,c,rows,1) * m1.row(r), m1.block(0,c,rows,1) * m1.block(r,0,1,cols)); VERIFY_IS_APPROX(m1.col(c) * m1.block(r,0,1,cols), m1.block(0,c,rows,1) * m1.block(r,0,1,cols)); VERIFY_IS_APPROX(m1.leftCols(1) * m1.row(r), m1.block(0,0,rows,1) * m1.block(r,0,1,cols)); - VERIFY_IS_APPROX(m1.col(c) * m1.topRows(1), m1.block(0,c,rows,1) * m1.block(0,0,1,cols)); + VERIFY_IS_APPROX(m1.col(c) * m1.topRows(1), m1.block(0,c,rows,1) * m1.block(0,0,1,cols)); + + // Aliasing + { + ColVectorType x(cols); x.setRandom(); + ColVectorType z(x); + ColVectorType y(cols); y.setZero(); + ColSquareMatrixType A(cols,cols); A.setRandom(); + // CwiseBinaryOp + VERIFY_IS_APPROX(x = y + A*x, A*z); + x = z; + // CwiseUnaryOp + VERIFY_IS_APPROX(x = Scalar(1.)*(A*x), A*z); + } } diff --git a/gtsam/3rdparty/Eigen/test/product_large.cpp b/gtsam/3rdparty/Eigen/test/product_large.cpp index 03d7bd8ed..6bb4d1ad1 100644 --- a/gtsam/3rdparty/Eigen/test/product_large.cpp +++ b/gtsam/3rdparty/Eigen/test/product_large.cpp @@ -9,6 +9,27 @@ #include "product.h" +template +void test_aliasing() +{ + int rows = internal::random(1,12); + int cols = internal::random(1,12); + typedef Matrix MatrixType; + typedef Matrix VectorType; + VectorType x(cols); x.setRandom(); + VectorType z(x); + VectorType y(rows); y.setZero(); + MatrixType A(rows,cols); A.setRandom(); + // CwiseBinaryOp + VERIFY_IS_APPROX(x = y + A*x, A*z); + x = z; + // CwiseUnaryOp + VERIFY_IS_APPROX(x = T(1.)*(A*x), A*z); + x = z; + VERIFY_IS_APPROX(x = y+(-(A*x)), -A*z); + x = z; +} + void test_product_large() { for(int i = 0; i < g_repeat; i++) { @@ -17,6 +38,8 @@ void test_product_large() CALL_SUBTEST_3( product(MatrixXi(internal::random(1,EIGEN_TEST_MAX_SIZE), internal::random(1,EIGEN_TEST_MAX_SIZE))) ); CALL_SUBTEST_4( product(MatrixXcf(internal::random(1,EIGEN_TEST_MAX_SIZE/2), internal::random(1,EIGEN_TEST_MAX_SIZE/2))) ); CALL_SUBTEST_5( product(Matrix(internal::random(1,EIGEN_TEST_MAX_SIZE), internal::random(1,EIGEN_TEST_MAX_SIZE))) ); + + CALL_SUBTEST_1( test_aliasing() ); } #if defined EIGEN_TEST_PART_6 diff --git a/gtsam/3rdparty/Eigen/test/product_notemporary.cpp b/gtsam/3rdparty/Eigen/test/product_notemporary.cpp index 258d238e2..5599d268d 100644 --- a/gtsam/3rdparty/Eigen/test/product_notemporary.cpp +++ b/gtsam/3rdparty/Eigen/test/product_notemporary.cpp @@ -58,10 +58,19 @@ template void product_notemporary(const MatrixType& m) r1 = internal::random(8,rows-r0); VERIFY_EVALUATION_COUNT( m3 = (m1 * m2.adjoint()), 1); + VERIFY_EVALUATION_COUNT( m3 = (m1 * m2.adjoint()).transpose(), 1); VERIFY_EVALUATION_COUNT( m3.noalias() = m1 * m2.adjoint(), 0); + VERIFY_EVALUATION_COUNT( m3 = s1 * (m1 * m2.transpose()), 1); + VERIFY_EVALUATION_COUNT( m3 = m3 + s1 * (m1 * m2.transpose()), 1); VERIFY_EVALUATION_COUNT( m3.noalias() = s1 * (m1 * m2.transpose()), 0); + VERIFY_EVALUATION_COUNT( m3 = m3 + (m1 * m2.adjoint()), 1); + VERIFY_EVALUATION_COUNT( m3 = m3 + (m1 * m2.adjoint()).transpose(), 1); + VERIFY_EVALUATION_COUNT( m3.noalias() = m3 + m1 * m2.transpose(), 1); // 0 in 3.3 + VERIFY_EVALUATION_COUNT( m3.noalias() += m3 + m1 * m2.transpose(), 1); // 0 in 3.3 + VERIFY_EVALUATION_COUNT( m3.noalias() -= m3 + m1 * m2.transpose(), 1); // 0 in 3.3 + VERIFY_EVALUATION_COUNT( m3.noalias() = s1 * m1 * s2 * m2.adjoint(), 0); VERIFY_EVALUATION_COUNT( m3.noalias() = s1 * m1 * s2 * (m1*s3+m2*s2).adjoint(), 1); VERIFY_EVALUATION_COUNT( m3.noalias() = (s1 * m1).adjoint() * s2 * m2, 0); diff --git a/gtsam/3rdparty/Eigen/test/ref.cpp b/gtsam/3rdparty/Eigen/test/ref.cpp index 44bbd3bf1..8297e263a 100644 --- a/gtsam/3rdparty/Eigen/test/ref.cpp +++ b/gtsam/3rdparty/Eigen/test/ref.cpp @@ -34,6 +34,18 @@ inline void on_temporary_creation(int) { // test Ref.h +// Deal with i387 extended precision +#if EIGEN_ARCH_i386 && !(EIGEN_ARCH_x86_64) + +#if EIGEN_COMP_GNUC_STRICT && EIGEN_GNUC_AT_LEAST(4,4) +#pragma GCC optimize ("-ffloat-store") +#else +#undef VERIFY_IS_EQUAL +#define VERIFY_IS_EQUAL(X,Y) VERIFY_IS_APPROX(X,Y) +#endif + +#endif + template void ref_matrix(const MatrixType& m) { typedef typename MatrixType::Index Index; @@ -71,7 +83,6 @@ template void ref_matrix(const MatrixType& m) rm2 = m2.block(i,j,brows,bcols); VERIFY_IS_EQUAL(m1, m2); - ConstRefDynMat rm3 = m1.block(i,j,brows,bcols); m1.block(i,j,brows,bcols) *= 2; m2.block(i,j,brows,bcols) *= 2; diff --git a/gtsam/3rdparty/Eigen/test/visitor.cpp b/gtsam/3rdparty/Eigen/test/visitor.cpp index 39a5d6b5f..844170ec6 100644 --- a/gtsam/3rdparty/Eigen/test/visitor.cpp +++ b/gtsam/3rdparty/Eigen/test/visitor.cpp @@ -55,6 +55,11 @@ template void matrixVisitor(const MatrixType& p) VERIFY_IS_APPROX(maxc, eigen_maxc); VERIFY_IS_APPROX(minc, m.minCoeff()); VERIFY_IS_APPROX(maxc, m.maxCoeff()); + + eigen_maxc = (m.adjoint()*m).maxCoeff(&eigen_maxrow,&eigen_maxcol); + eigen_maxc = (m.adjoint()*m).eval().maxCoeff(&maxrow,&maxcol); + VERIFY(maxrow == eigen_maxrow); + VERIFY(maxcol == eigen_maxcol); } template void vectorVisitor(const VectorType& w) diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/AlignedVector3 b/gtsam/3rdparty/Eigen/unsupported/Eigen/AlignedVector3 index 7b45e6cce..29d5c90fb 100644 --- a/gtsam/3rdparty/Eigen/unsupported/Eigen/AlignedVector3 +++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/AlignedVector3 @@ -177,7 +177,7 @@ template class AlignedVector3 } template - inline bool isApprox(const MatrixBase& other, RealScalar eps=NumTraits::dummy_precision()) const + inline bool isApprox(const MatrixBase& other, const RealScalar& eps=NumTraits::dummy_precision()) const { return m_coeffs.template head<3>().isApprox(other,eps); } From 52fff13b6fa802890d4e3b11f423c85f410e975c Mon Sep 17 00:00:00 2001 From: Frank Date: Wed, 17 Feb 2016 12:19:03 -0800 Subject: [PATCH 35/90] Added configuration variable for typedef only --- CMakeLists.txt | 8 +++++++- gtsam/config.h.in | 3 +++ gtsam/geometry/Point3.h | 2 +- 3 files changed, 11 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 3123dbea2..836acde81 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -66,6 +66,7 @@ option(GTSAM_WITH_EIGEN_MKL_OPENMP "Eigen, when using Intel MKL, will also option(GTSAM_THROW_CHEIRALITY_EXCEPTION "Throw exception when a triangulated point is behind a camera" ON) option(GTSAM_BUILD_PYTHON "Enable/Disable building & installation of Python module" ON) option(GTSAM_ALLOW_DEPRECATED_SINCE_V4 "Allow use of methods/functions deprecated in GTSAM 4" ON) +option(GTSAM_USE_VECTOR3_POINTS "Symply typdef Point3 to eigen::Vector3d" OFF) # Options relating to MATLAB wrapper # TODO: Check for matlab mex binary before handling building of binaries @@ -85,7 +86,11 @@ if(GTSAM_INSTALL_MATLAB_TOOLBOX AND GTSAM_BUILD_STATIC_LIBRARY) endif() if(GTSAM_BUILD_PYTHON AND GTSAM_ALLOW_DEPRECATED_SINCE_V4) - message(FATAL_ERROR "GTSAM_BUILD_PYTHON and GTSAM_ALLOW_DEPRECATED_SINCE_V4 are both enabled. The python module cannot be compiled with deprecated functions turned on. Turn one of the two options off.") + message(FATAL_ERROR "GTSAM_BUILD_PYTHON and GTSAM_ALLOW_DEPRECATED_SINCE_V4 are both enabled. The python module cannot be compiled with deprecated functions turned on. Turn one of the two options off.") +endif() + +if(GTSAM_INSTALL_MATLAB_TOOLBOX AND GTSAM_USE_VECTOR3_POINTS) + message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX and GTSAM_USE_VECTOR3_POINTS are both enabled. For now, the MATLAB toolbox cannot deal with this yet. Please turn one of the two options off.") endif() # Flags for choosing default packaging tools @@ -477,6 +482,7 @@ print_config_flag(${GTSAM_ENABLE_CONSISTENCY_CHECKS} "Runtime consistency chec print_config_flag(${GTSAM_ROT3_EXPMAP} "Rot3 retract is full ExpMap ") print_config_flag(${GTSAM_POSE3_EXPMAP} "Pose3 retract is full ExpMap ") print_config_flag(${GTSAM_ALLOW_DEPRECATED_SINCE_V4} "Deprecated in GTSAM 4 allowed ") +print_config_flag(${GTSAM_USE_VECTOR3_POINTS} "Point3 is typedef to Vector3 ") message(STATUS "MATLAB toolbox flags ") print_config_flag(${GTSAM_INSTALL_MATLAB_TOOLBOX} "Install matlab toolbox ") diff --git a/gtsam/config.h.in b/gtsam/config.h.in index a408b254c..cb195dc03 100644 --- a/gtsam/config.h.in +++ b/gtsam/config.h.in @@ -62,3 +62,6 @@ // Make sure dependent projects that want it can see deprecated functions #cmakedefine GTSAM_ALLOW_DEPRECATED_SINCE_V4 + +// Publish flag about Eigen typedef +#cmakedefine GTSAM_USE_VECTOR3_POINTS diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 524bb9bbf..9c660f083 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -21,6 +21,7 @@ #pragma once +#include #include #include #include @@ -28,7 +29,6 @@ namespace gtsam { -#define GTSAM_USE_VECTOR3_POINTS #ifdef GTSAM_USE_VECTOR3_POINTS /// As of GTSAM 4, in order to make GTSAM more lean, From a10f462fef3c279bbb807958d7224c8a4a84890b Mon Sep 17 00:00:00 2001 From: Frank Date: Wed, 17 Feb 2016 12:36:57 -0800 Subject: [PATCH 36/90] Fixed warning --- gtsam/linear/NoiseModel.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index cb77902d0..584f758a1 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -82,13 +82,13 @@ Gaussian::shared_ptr Gaussian::SqrtInformation(const Matrix& R, bool smart) { size_t m = R.rows(), n = R.cols(); if (m != n) throw invalid_argument("Gaussian::SqrtInformation: R not square"); - boost::optional diagonal = boost::none; - if (smart) - diagonal = checkIfDiagonal(R); - if (diagonal) - return Diagonal::Sigmas(diagonal->array().inverse(), true); - else - return shared_ptr(new Gaussian(R.rows(), R)); + if (smart) { + boost::optional diagonal = checkIfDiagonal(R); + if (diagonal) + return Diagonal::Sigmas(diagonal->array().inverse(), true); + } + // NOTE(frank): only reaches here if !smart && !diagonal + return shared_ptr(new Gaussian(R.rows(), R)); } /* ************************************************************************* */ From 7bcdcbd8053609174d3100e7128631dba9714a5b Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 17 Feb 2016 16:05:50 -0800 Subject: [PATCH 37/90] Fixed compilation problems (and used some c++11 for loops) --- matlab.h | 28 +++++++++++++--------------- 1 file changed, 13 insertions(+), 15 deletions(-) diff --git a/matlab.h b/matlab.h index 521e7a469..72889dc4b 100644 --- a/matlab.h +++ b/matlab.h @@ -28,8 +28,6 @@ #include #include -#include - #include namespace gtsam { @@ -92,7 +90,7 @@ Matrix extractPoint2(const Values& values) { size_t j = 0; Values::ConstFiltered points = values.filter(); Matrix result(points.size(), 2); - BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, points) + for(const auto& key_value: points) result.row(j++) = key_value.value.vector(); return result; } @@ -102,7 +100,7 @@ Matrix extractPoint3(const Values& values) { Values::ConstFiltered points = values.filter(); Matrix result(points.size(), 3); size_t j = 0; - BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, points) + for(const auto& key_value: points) result.row(j++) = key_value.value; return result; } @@ -112,7 +110,7 @@ Matrix extractPose2(const Values& values) { Values::ConstFiltered poses = values.filter(); Matrix result(poses.size(), 3); size_t j = 0; - BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, poses) + for(const auto& key_value: poses) result.row(j++) << key_value.value.x(), key_value.value.y(), key_value.value.theta(); return result; } @@ -127,7 +125,7 @@ Matrix extractPose3(const Values& values) { Values::ConstFiltered poses = values.filter(); Matrix result(poses.size(), 12); size_t j = 0; - BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, poses) { + for(const auto& key_value: poses) { result.row(j).segment(0, 3) << key_value.value.rotation().matrix().row(0); result.row(j).segment(3, 3) << key_value.value.rotation().matrix().row(1); result.row(j).segment(6, 3) << key_value.value.rotation().matrix().row(2); @@ -142,8 +140,8 @@ void perturbPoint2(Values& values, double sigma, int32_t seed = 42u) { noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(2, sigma); Sampler sampler(model, seed); - BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, values.filter()) { - values.update(key_value.key, key_value.value + Point2(sampler.sample())); + for(const auto& key_value: values.filter()) { + values.update(key_value.key, key_value.value + Point2(sampler.sample())); } } @@ -153,8 +151,8 @@ void perturbPose2(Values& values, double sigmaT, double sigmaR, int32_t seed = noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas( Vector3(sigmaT, sigmaT, sigmaR)); Sampler sampler(model, seed); - BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, values.filter()) { - values.update(key_value.key, key_value.value.retract(sampler.sample())); + for(const auto& key_value: values.filter()) { + values.update(key_value.key, key_value.value.retract(sampler.sample())); } } @@ -163,8 +161,8 @@ void perturbPoint3(Values& values, double sigma, int32_t seed = 42u) { noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3, sigma); Sampler sampler(model, seed); - BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, values.filter()) { - values.update(key_value.key, key_value.value + Point3(sampler.sample())); + for(const auto& key_value: values.filter()) { + values.update(key_value.key, key_value.value + Point3(sampler.sample())); } } @@ -204,13 +202,13 @@ Matrix reprojectionErrors(const NonlinearFactorGraph& graph, const Values& values) { // first count size_t K = 0, k = 0; - BOOST_FOREACH(const NonlinearFactor::shared_ptr& f, graph) + for(const NonlinearFactor::shared_ptr& f: graph) if (boost::dynamic_pointer_cast >( f)) ++K; // now fill Matrix errors(2, K); - BOOST_FOREACH(const NonlinearFactor::shared_ptr& f, graph) { + for(const NonlinearFactor::shared_ptr& f: graph) { boost::shared_ptr > p = boost::dynamic_pointer_cast >( f); @@ -232,7 +230,7 @@ Values localToWorld(const Values& local, const Pose2& base, keys = local.keys(); // Loop over all keys - BOOST_FOREACH(Key key, keys) { + for(Key key: keys) { try { // if value is a Pose2, compose it with base pose Pose2 pose = local.at(key); From df4efbf2d761f145a9f4cc8e6803a1220639034b Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 17 Feb 2016 17:42:20 -0800 Subject: [PATCH 38/90] Fixed python wrapper issue --- python/handwritten/geometry/Rot3.cpp | 7 ------- 1 file changed, 7 deletions(-) diff --git a/python/handwritten/geometry/Rot3.cpp b/python/handwritten/geometry/Rot3.cpp index f4b868b50..685a37ca9 100644 --- a/python/handwritten/geometry/Rot3.cpp +++ b/python/handwritten/geometry/Rot3.cpp @@ -39,9 +39,6 @@ static Rot3 Quaternion_1(double w, double x, double y, double z) // See: http://www.boost.org/doc/libs/1_59_0/libs/python/doc/tutorial/doc/html/python/functions.html gtsam::Rot3 (*AxisAngle_0)(const gtsam::Point3&, double) = &Rot3::AxisAngle; gtsam::Rot3 (*AxisAngle_1)(const gtsam::Unit3&, double) = &Rot3::AxisAngle; -#ifndef GTSAM_USE_VECTOR3_POINTS -gtsam::Rot3 (*AxisAngle_2)(const Vector3&, double) = &Rot3::AxisAngle; -#endif gtsam::Rot3 (*Rodrigues_0)(const Vector3&) = &Rot3::Rodrigues; gtsam::Rot3 (*Rodrigues_1)(double, double, double) = &Rot3::Rodrigues; gtsam::Rot3 (*RzRyRx_0)(double, double, double) = &Rot3::RzRyRx; @@ -70,12 +67,8 @@ void exportRot3(){ .staticmethod("Logmap") .def("LogmapDerivative", &Rot3::LogmapDerivative) .staticmethod("LogmapDerivative") -#ifdef GTSAM_USE_VECTOR3_POINTS .def("AxisAngle", AxisAngle_0) .def("AxisAngle", AxisAngle_1) -#else - .def("AxisAngle", AxisAngle_2) -#endif .def("Rodrigues", Rodrigues_0) .def("Rodrigues", Rodrigues_1) .staticmethod("Rodrigues") From a9b0d81be47f28d871e7f648700d4af0012af4b4 Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 17 Feb 2016 18:56:44 -0800 Subject: [PATCH 39/90] Yet another overload fix in python wrapper --- gtsam/geometry/Point3.h | 9 +++++++++ python/handwritten/geometry/Point3.cpp | 6 +++--- 2 files changed, 12 insertions(+), 3 deletions(-) diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 9c660f083..8b31316c7 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -109,6 +109,15 @@ class GTSAM_EXPORT Point3 : public Vector3 { /// return as Vector3 const Vector3& vector() const { return *this; } + /// get x + inline double x() const {return (*this)[0];} + + /// get y + inline double y() const {return (*this)[1];} + + /// get z + inline double z() const {return (*this)[2];} + /// @} /// Output stream operator diff --git a/python/handwritten/geometry/Point3.cpp b/python/handwritten/geometry/Point3.cpp index f83b0c516..c87421075 100644 --- a/python/handwritten/geometry/Point3.cpp +++ b/python/handwritten/geometry/Point3.cpp @@ -43,13 +43,13 @@ class_("Point3") .def("norm", &Point3::norm) .def("normalized", &Point3::normalized) .def("print", &Point3::print, print_overloads(args("s"))) -#ifndef GTSAM_USE_VECTOR3_POINTS +#ifdef GTSAM_USE_VECTOR3_POINTS + .def("vector", &Point3::vector, return_value_policy()) +#else .def("vector", &Point3::vector) .def("x", &Point3::x) .def("y", &Point3::y) .def("z", &Point3::z) -#else - .def("vector", &Point3::vector, return_value_policy()) #endif .def(self * other()) .def(other() * self) From 2ab4fb2670f8a740881cdeaf80160fb2f9918e0b Mon Sep 17 00:00:00 2001 From: Kartik Mohta Date: Thu, 18 Feb 2016 04:55:31 +0000 Subject: [PATCH 40/90] Add gtsam/sam to gtsam_doc_subdirs --- doc/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/doc/CMakeLists.txt b/doc/CMakeLists.txt index 4cb6d4a40..634d3cf71 100644 --- a/doc/CMakeLists.txt +++ b/doc/CMakeLists.txt @@ -28,6 +28,7 @@ if (GTSAM_BUILD_DOCS) gtsam/discrete gtsam/linear gtsam/nonlinear + gtsam/sam gtsam/slam gtsam ) From 3a50a0e19eda38ce41837d8f787517c2597e67b8 Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 18 Feb 2016 08:50:11 -0800 Subject: [PATCH 41/90] spurious vector --- gtsam/navigation/tests/testPoseVelocityBias.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/navigation/tests/testPoseVelocityBias.cpp b/gtsam/navigation/tests/testPoseVelocityBias.cpp index cc2a47498..0b897bc6e 100644 --- a/gtsam/navigation/tests/testPoseVelocityBias.cpp +++ b/gtsam/navigation/tests/testPoseVelocityBias.cpp @@ -31,7 +31,7 @@ using namespace gtsam; Vector9 error(const PoseVelocityBias& pvb1, const PoseVelocityBias& pvb2) { Matrix3 R1 = pvb1.pose.rotation().matrix(); // Ri.transpose() translate the error from the global frame into pose1's frame - const Vector3 fp = R1.transpose() * (pvb2.pose.translation() - pvb1.pose.translation()).vector(); + const Vector3 fp = R1.transpose() * (pvb2.pose.translation() - pvb1.pose.translation()); const Vector3 fv = R1.transpose() * (pvb2.velocity - pvb1.velocity); const Rot3 R1BetweenR2 = pvb1.pose.rotation().between(pvb2.pose.rotation()); const Vector3 fR = Rot3::Logmap(R1BetweenR2); From 6d4cf0c1156c8fc1990dc76b52b5ecd2d388b914 Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 18 Feb 2016 09:55:59 -0800 Subject: [PATCH 42/90] Fix compile issue --- python/handwritten/geometry/Point3.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/python/handwritten/geometry/Point3.cpp b/python/handwritten/geometry/Point3.cpp index c87421075..57f0fb75e 100644 --- a/python/handwritten/geometry/Point3.cpp +++ b/python/handwritten/geometry/Point3.cpp @@ -43,10 +43,8 @@ class_("Point3") .def("norm", &Point3::norm) .def("normalized", &Point3::normalized) .def("print", &Point3::print, print_overloads(args("s"))) -#ifdef GTSAM_USE_VECTOR3_POINTS +#ifndef GTSAM_USE_VECTOR3_POINTS .def("vector", &Point3::vector, return_value_policy()) -#else - .def("vector", &Point3::vector) .def("x", &Point3::x) .def("y", &Point3::y) .def("z", &Point3::z) From f6eb67e8693dd73d5367e3df96c876ba15d04c32 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 18 Feb 2016 18:54:15 +0000 Subject: [PATCH 43/90] README.md edited online with Bitbucket --- README.md | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/README.md b/README.md index ccdc7f07e..b02c126d9 100644 --- a/README.md +++ b/README.md @@ -38,6 +38,13 @@ Optional prerequisites - used automatically if findable by CMake: - [Intel Threaded Building Blocks (TBB)](http://www.threadingbuildingblocks.org/) (Ubuntu: `sudo apt-get install libtbb-dev`) - [Intel Math Kernel Library (MKL)](http://software.intel.com/en-us/intel-mkl) +GTSAM 4 Compatibility +--------------------- + +GTSAM 4 will introduce several new features, most notably Expressions and a python toolbox. We will also deprecate some legacy functionality and wrongly named methods, but by default the flag GTSAM_ALLOW_DEPRECATED_SINCE_V4 is enabled, allowing anyone to just pull V4 and compile. To build the python toolbox, however, you will have to explicitly disable that flag. + +Also, GTSAM 4 introduces traits, a C++ technique that allows optimizing with non-GTSAM types. That opens the door to retiring geometric types such as Point2 and Point3 to pure Eigen types, which we will also do. + Additional Information ---------------------- From 61113b19603f150d700a605c2da760fa8d13fb2d Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 18 Feb 2016 19:16:04 +0000 Subject: [PATCH 44/90] README.md edited online with Bitbucket --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index b02c126d9..040f1134f 100644 --- a/README.md +++ b/README.md @@ -43,7 +43,7 @@ GTSAM 4 Compatibility GTSAM 4 will introduce several new features, most notably Expressions and a python toolbox. We will also deprecate some legacy functionality and wrongly named methods, but by default the flag GTSAM_ALLOW_DEPRECATED_SINCE_V4 is enabled, allowing anyone to just pull V4 and compile. To build the python toolbox, however, you will have to explicitly disable that flag. -Also, GTSAM 4 introduces traits, a C++ technique that allows optimizing with non-GTSAM types. That opens the door to retiring geometric types such as Point2 and Point3 to pure Eigen types, which we will also do. +Also, GTSAM 4 introduces traits, a C++ technique that allows optimizing with non-GTSAM types. That opens the door to retiring geometric types such as Point2 and Point3 to pure Eigen types, which we will also do. A significant change which will not trigger a compile error is that zero-initializing of Point2 and Point3 will be deprecated, so please be aware that this might render functions using their default constructor incorrect. Additional Information ---------------------- From dd0691d7b083404d479090c787844075f7c0578a Mon Sep 17 00:00:00 2001 From: Kartik Mohta Date: Thu, 18 Feb 2016 16:25:04 -0500 Subject: [PATCH 45/90] Add missing directories to gtsam_doc_subdirs for doc generation --- doc/CMakeLists.txt | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/doc/CMakeLists.txt b/doc/CMakeLists.txt index 634d3cf71..7c43a8989 100644 --- a/doc/CMakeLists.txt +++ b/doc/CMakeLists.txt @@ -23,13 +23,17 @@ if (GTSAM_BUILD_DOCS) # GTSAM core subfolders set(gtsam_doc_subdirs gtsam/base + gtsam/discrete gtsam/geometry gtsam/inference - gtsam/discrete gtsam/linear + gtsam/navigation gtsam/nonlinear gtsam/sam + gtsam/sfm gtsam/slam + gtsam/smart + gtsam/symbolic gtsam ) From 0372a959ee8756a17b232c3d675d4cfd26dbbd75 Mon Sep 17 00:00:00 2001 From: Frank Date: Wed, 24 Feb 2016 11:01:19 -0800 Subject: [PATCH 46/90] Many small improvements, bug-fixes, and tests --- CMakeLists.txt | 36 ++- GTSAM-Concepts.md | 8 +- cmake/GtsamBuildTypes.cmake | 135 +++++---- cmake/GtsamTesting.cmake | 6 +- gtsam.h | 3 +- gtsam/3rdparty/CMakeLists.txt | 4 +- gtsam/3rdparty/ceres/fixed_array.h | 2 +- gtsam/3rdparty/ceres/jet.h | 2 +- gtsam/3rdparty/metis/CMakeLists.txt | 6 +- gtsam/CMakeLists.txt | 4 +- gtsam/base/Lie.h | 9 + gtsam/base/Matrix.h | 52 ++-- gtsam/base/Testable.h | 12 +- gtsam/base/Vector.cpp | 2 +- gtsam/base/Vector.h | 30 +- gtsam/base/VectorSpace.h | 37 ++- gtsam/base/VerticalBlockMatrix.h | 18 +- gtsam/base/numericalDerivative.h | 14 +- gtsam/base/tests/testFastContainers.cpp | 6 +- gtsam/base/types.h | 5 +- gtsam/config.h.in | 9 +- .../tests/testAlgebraicDecisionTree.cpp | 10 +- gtsam/geometry/Cal3_S2.h | 5 + gtsam/geometry/EssentialMatrix.cpp | 11 + gtsam/geometry/EssentialMatrix.h | 5 + gtsam/geometry/OrientedPlane3.cpp | 20 +- gtsam/geometry/OrientedPlane3.h | 9 + gtsam/geometry/PinholePose.h | 5 + gtsam/geometry/Point2.cpp | 6 + gtsam/geometry/Point2.h | 11 +- gtsam/geometry/Point3.cpp | 2 +- gtsam/geometry/Point3.h | 14 +- gtsam/geometry/Pose2.cpp | 4 +- gtsam/geometry/Pose3.cpp | 84 ++++-- gtsam/geometry/Pose3.h | 35 ++- gtsam/geometry/Rot3.cpp | 57 +++- gtsam/geometry/Rot3.h | 10 +- gtsam/geometry/Rot3M.cpp | 6 +- gtsam/geometry/SO3.cpp | 13 +- gtsam/geometry/StereoPoint2.h | 7 +- gtsam/geometry/Unit3.cpp | 52 ++-- gtsam/geometry/Unit3.h | 7 - gtsam/geometry/tests/testEssentialMatrix.cpp | 49 ++- gtsam/geometry/tests/testOrientedPlane3.cpp | 24 ++ gtsam/geometry/tests/testPoint3.cpp | 76 ++++- gtsam/geometry/tests/testPose3.cpp | 123 ++++++-- gtsam/geometry/tests/testRot3.cpp | 5 +- gtsam/geometry/tests/testUnit3.cpp | 2 +- gtsam/geometry/triangulation.cpp | 8 +- .../inference/EliminateableFactorGraph-inst.h | 46 ++- gtsam/inference/FactorGraph-inst.h | 25 +- gtsam/inference/FactorGraph.h | 13 +- gtsam/inference/Key.h | 19 +- gtsam/inference/LabeledSymbol.cpp | 2 +- gtsam/inference/LabeledSymbol.h | 6 +- gtsam/inference/Ordering.cpp | 10 +- gtsam/inference/Symbol.cpp | 8 +- gtsam/inference/Symbol.h | 67 ++-- gtsam/inference/VariableIndex.h | 8 +- gtsam/inference/inference-inst.h | 2 +- gtsam/inference/tests/testOrdering.cpp | 2 +- gtsam/linear/GaussianBayesNet.cpp | 15 +- gtsam/linear/GaussianConditional.cpp | 20 +- gtsam/linear/GaussianFactorGraph.cpp | 28 +- gtsam/linear/GaussianFactorGraph.h | 6 +- gtsam/linear/HessianFactor.cpp | 4 +- gtsam/linear/IterativeSolver.h | 2 +- gtsam/linear/JacobianFactor.cpp | 101 ++++--- gtsam/linear/JacobianFactor.h | 6 +- gtsam/linear/NoiseModel.cpp | 257 +++++++++------- gtsam/linear/NoiseModel.h | 130 +++++--- gtsam/linear/SubgraphPreconditioner.cpp | 11 +- gtsam/linear/VectorValues.h | 4 +- gtsam/linear/tests/testHessianFactor.cpp | 14 +- gtsam/linear/tests/testJacobianFactor.cpp | 120 +++++--- gtsam/linear/tests/testNoiseModel.cpp | 155 +++++++++- gtsam/navigation/GPSFactor.cpp | 28 +- gtsam/navigation/GPSFactor.h | 88 +++++- gtsam/navigation/ImuBias.h | 7 +- gtsam/navigation/ImuFactor.cpp | 78 ++++- gtsam/navigation/ImuFactor.h | 86 +++++- gtsam/navigation/NavState.cpp | 2 + gtsam/navigation/NavState.h | 2 + gtsam/navigation/PreintegratedRotation.cpp | 3 +- gtsam/navigation/PreintegrationBase.cpp | 43 ++- gtsam/navigation/PreintegrationBase.h | 18 +- gtsam/navigation/tests/imuFactorTesting.h | 2 +- .../tests/testCombinedImuFactor.cpp | 15 +- gtsam/navigation/tests/testGPSFactor.cpp | 50 ++- gtsam/navigation/tests/testImuFactor.cpp | 54 +++- gtsam/navigation/tests/testNavState.cpp | 2 + .../tests/testPreintegrationBase.cpp | 19 +- gtsam/navigation/tests/testScenarios.cpp | 2 +- gtsam/nonlinear/Expression-inl.h | 40 ++- gtsam/nonlinear/Expression.h | 97 +++++- gtsam/nonlinear/ExpressionFactor.h | 9 +- gtsam/nonlinear/ExtendedKalmanFilter-inl.h | 55 ++-- gtsam/nonlinear/ExtendedKalmanFilter.h | 141 +++++---- gtsam/nonlinear/GaussNewtonOptimizer.cpp | 7 +- gtsam/nonlinear/ISAM2.cpp | 33 +- gtsam/nonlinear/ISAM2.h | 2 +- gtsam/nonlinear/LinearContainerFactor.h | 8 +- gtsam/nonlinear/Marginals.cpp | 45 +-- gtsam/nonlinear/Marginals.h | 23 +- gtsam/nonlinear/NonlinearFactor.h | 7 +- gtsam/nonlinear/NonlinearFactorGraph.cpp | 36 ++- gtsam/nonlinear/NonlinearFactorGraph.h | 20 +- gtsam/nonlinear/NonlinearOptimizer.cpp | 6 +- gtsam/nonlinear/NonlinearOptimizer.h | 7 +- gtsam/nonlinear/Values-inl.h | 8 +- gtsam/nonlinear/expressionTesting.h | 6 +- gtsam/nonlinear/factorTesting.h | 19 +- gtsam/nonlinear/internal/CallRecord.h | 38 ++- gtsam/nonlinear/internal/ExecutionTrace.h | 19 +- gtsam/nonlinear/internal/ExpressionNode.h | 213 ++++++++++++- gtsam/nonlinear/tests/testAdaptAutoDiff.cpp | 3 - gtsam/nonlinear/tests/testExpression.cpp | 285 +++++++++++++++--- gtsam/slam/InitializePose3.cpp | 4 +- gtsam/slam/JacobianFactorSVD.h | 17 +- gtsam/slam/OrientedPlane3Factor.h | 1 + gtsam/slam/RotateFactor.h | 34 ++- gtsam/slam/SmartProjectionFactor.h | 6 +- gtsam/slam/dataset.cpp | 4 +- gtsam/slam/lago.cpp | 6 +- gtsam/slam/tests/testDataset.cpp | 6 +- .../slam/tests/testEssentialMatrixFactor.cpp | 65 +++- gtsam/slam/tests/testRotateFactor.cpp | 54 +++- .../symbolic/tests/testSymbolicBayesTree.cpp | 16 +- .../tests/testSymbolicFactorGraph.cpp | 20 +- gtsam/symbolic/tests/testSymbolicISAM.cpp | 3 +- .../nonlinear/BatchFixedLagSmoother.cpp | 6 + gtsam_unstable/slam/tests/testTOAFactor.cpp | 6 +- python/gtsam/__init__.py | 2 +- python/gtsam_examples/ImuFactorExample.py | 82 +++-- python/gtsam_examples/Pose2SLAMExample.py | 68 +++++ .../gtsam_examples/PreintegrationExample.py | 4 +- python/gtsam_utils/plot.py | 27 +- python/handwritten/CMakeLists.txt | 16 +- python/handwritten/exportgtsam.cpp | 8 +- python/handwritten/geometry/Point3.cpp | 3 +- python/handwritten/geometry/Pose3.cpp | 74 ++--- python/handwritten/geometry/Rot3.cpp | 2 +- .../handwritten/geometry/export_geometry.cpp | 35 +++ python/handwritten/linear/NoiseModel.cpp | 6 +- python/handwritten/navigation/ImuFactor.cpp | 49 ++- .../nonlinear/LevenbergMarquardtOptimizer.cpp | 45 ++- python/handwritten/nonlinear/Values.cpp | 4 + python/handwritten/slam/PriorFactor.cpp | 2 + python/handwritten/slam/export_slam.cpp | 36 +++ tests/testExpressionFactor.cpp | 2 +- tests/testGaussianBayesTreeB.cpp | 3 +- tests/testNonlinearEquality.cpp | 72 ++--- 152 files changed, 3223 insertions(+), 1332 deletions(-) create mode 100644 python/gtsam_examples/Pose2SLAMExample.py create mode 100644 python/handwritten/geometry/export_geometry.cpp create mode 100644 python/handwritten/slam/export_slam.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 95cfa9d41..8c4229ed5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -66,7 +66,8 @@ option(GTSAM_WITH_EIGEN_MKL_OPENMP "Eigen, when using Intel MKL, will also option(GTSAM_THROW_CHEIRALITY_EXCEPTION "Throw exception when a triangulated point is behind a camera" ON) option(GTSAM_BUILD_PYTHON "Enable/Disable building & installation of Python module" OFF) option(GTSAM_ALLOW_DEPRECATED_SINCE_V4 "Allow use of methods/functions deprecated in GTSAM 4" ON) -option(GTSAM_USE_VECTOR3_POINTS "Symply typdef Point3 to eigen::Vector3d" OFF) +option(GTSAM_USE_VECTOR3_POINTS "Simply typdef Point3 to eigen::Vector3d" OFF) +option(GTSAM_SUPPORT_NESTED_DISSECTION "Support Metis-based nested dissection" ON) # Options relating to MATLAB wrapper # TODO: Check for matlab mex binary before handling building of binaries @@ -190,10 +191,10 @@ if(MKL_FOUND AND GTSAM_WITH_EIGEN_MKL) set(EIGEN_USE_MKL_ALL 1) # This will go into config.h - it makes Eigen use MKL include_directories(${MKL_INCLUDE_DIR}) list(APPEND GTSAM_ADDITIONAL_LIBRARIES ${MKL_LIBRARIES}) - + # --no-as-needed is required with gcc according to the MKL link advisor if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU") - set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -Wl,--no-as-needed") + set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -Wl,--no-as-needed") endif() else() set(GTSAM_USE_EIGEN_MKL 0) @@ -224,17 +225,17 @@ option(GTSAM_USE_SYSTEM_EIGEN "Find and use system-installed Eigen. If 'off', us if(GTSAM_USE_SYSTEM_EIGEN) find_package(Eigen3 REQUIRED) include_directories(AFTER "${EIGEN3_INCLUDE_DIR}") - + # Use generic Eigen include paths e.g. set(GTSAM_EIGEN_INCLUDE_PREFIX "${EIGEN3_INCLUDE_DIR}") - + # check if MKL is also enabled - can have one or the other, but not both! # Note: Eigen >= v3.2.5 includes our patches if(EIGEN_USE_MKL_ALL AND (EIGEN3_VERSION VERSION_LESS 3.2.5)) message(FATAL_ERROR "MKL requires at least Eigen 3.2.5, and your system appears to have an older version. Disable GTSAM_USE_SYSTEM_EIGEN to use GTSAM's copy of Eigen, or disable GTSAM_WITH_EIGEN_MKL") endif() else() - # Use bundled Eigen include path. + # Use bundled Eigen include path. # Clear any variables set by FindEigen3 if(EIGEN3_INCLUDE_DIR) set(EIGEN3_INCLUDE_DIR NOTFOUND CACHE STRING "" FORCE) @@ -242,11 +243,11 @@ else() # Add the bundled version of eigen to the include path so that it can still be included # with #include include_directories(BEFORE "gtsam/3rdparty/Eigen/") - + # set full path to be used by external projects # this will be added to GTSAM_INCLUDE_DIR by gtsam_extra.cmake.in set(GTSAM_EIGEN_INCLUDE_PREFIX "${CMAKE_INSTALL_PREFIX}/include/gtsam/3rdparty/Eigen/") - + endif() ############################################################################### @@ -289,18 +290,24 @@ endif() # Include boost - use 'BEFORE' so that a specific boost specified to CMake # takes precedence over a system-installed one. -# Use 'SYSTEM' to ignore compiler warnings in Boost headers include_directories(BEFORE SYSTEM ${Boost_INCLUDE_DIR}) +if(GTSAM_SUPPORT_NESTED_DISSECTION) + set(METIS_INCLUDE_DIRECTORIES + gtsam/3rdparty/metis/include + gtsam/3rdparty/metis/libmetis + gtsam/3rdparty/metis/GKlib) +else() + set(METIS_INCLUDE_DIRECTORIES) +endif() + # Add includes for source directories 'BEFORE' boost and any system include # paths so that the compiler uses GTSAM headers in our source directory instead # of any previously installed GTSAM headers. include_directories(BEFORE gtsam/3rdparty/UFconfig gtsam/3rdparty/CCOLAMD/Include - gtsam/3rdparty/metis/include - gtsam/3rdparty/metis/libmetis - gtsam/3rdparty/metis/GKlib + ${METIS_INCLUDE_DIRECTORIES} ${PROJECT_SOURCE_DIR} ${PROJECT_BINARY_DIR} # So we can include generated config header files CppUnitLite) @@ -362,9 +369,9 @@ if (GTSAM_BUILD_PYTHON) # NOTE: The automatic generation of python wrapper from the gtsampy.h interface is # not working yet, so we're using a handwritten wrapper files on python/handwritten. - # Once the python wrapping from the interface file is working, you can _swap_ the + # Once the python wrapping from the interface file is working, you can _swap_ the # comments on the next lines - + # wrap_and_install_python(gtsampy.h "${GTSAM_ADDITIONAL_LIBRARIES}" "") add_subdirectory(python) @@ -484,6 +491,7 @@ print_config_flag(${GTSAM_ROT3_EXPMAP} "Rot3 retract is full Exp print_config_flag(${GTSAM_POSE3_EXPMAP} "Pose3 retract is full ExpMap ") print_config_flag(${GTSAM_ALLOW_DEPRECATED_SINCE_V4} "Deprecated in GTSAM 4 allowed ") print_config_flag(${GTSAM_USE_VECTOR3_POINTS} "Point3 is typedef to Vector3 ") +print_config_flag(${GTSAM_SUPPORT_NESTED_DISSECTION} "Metis-based Nested Dissection ") message(STATUS "MATLAB toolbox flags ") print_config_flag(${GTSAM_INSTALL_MATLAB_TOOLBOX} "Install matlab toolbox ") diff --git a/GTSAM-Concepts.md b/GTSAM-Concepts.md index 5c7434a8d..366a58a09 100644 --- a/GTSAM-Concepts.md +++ b/GTSAM-Concepts.md @@ -18,14 +18,14 @@ To optimize over continuous types, we assume they are manifolds. This is central [Manifolds](http://en.wikipedia.org/wiki/Manifold#Charts.2C_atlases.2C_and_transition_maps) and [charts](http://en.wikipedia.org/wiki/Manifold#Charts.2C_atlases.2C_and_transition_maps) are intimately linked concepts. We are only interested here in [differentiable manifolds](http://en.wikipedia.org/wiki/Differentiable_manifold#Definition), continuous spaces that can be locally approximated *at any point* using a local vector space, called the [tangent space](http://en.wikipedia.org/wiki/Tangent_space). A *chart* is an invertible map from the manifold to that tangent space. -In GTSAM, all properties and operations needed to use a type must be defined through template specialization of the struct `gtsam::traits`. Concept checks are used to check that all required functions are implemented. -In detail, we ask the following are defined in the traits object (although, not all are needed for optimization): +In GTSAM, all properties and operations needed to use a type must be defined through template specialization of the struct `gtsam::traits`. Concept checks are used to check that all required functions are implemented. +In detail, we ask that the following items are defined in the traits object (although, not all are needed for optimization): * values: * `enum { dimension = D};`, an enum that indicates the dimensionality *n* of the manifold. In Eigen-fashion, we also support manifolds whose dimensionality is only defined at runtime, by specifying the value -1. -* types: +* types: * `TangentVector`, type that lives in tangent space. This will almost always be an `Eigen::Matrix`. - * `ChartJacobian`, a typedef for `OptionalJacobian`. + * `ChartJacobian`, a typedef for `OptionalJacobian`. * `ManifoldType`, a pointer back to the type. * `structure_category`, a tag type that defines what requirements the type fulfills, and therefore what requirements this traits class must fulfill. It should be defined to be one of the following: * `gtsam::traits::manifold_tag` -- Everything in this list is expected diff --git a/cmake/GtsamBuildTypes.cmake b/cmake/GtsamBuildTypes.cmake index 43ae36929..838d5eb09 100644 --- a/cmake/GtsamBuildTypes.cmake +++ b/cmake/GtsamBuildTypes.cmake @@ -3,56 +3,76 @@ list(APPEND CMAKE_PREFIX_PATH "${CMAKE_INSTALL_PREFIX}") # Default to Release mode -if(NOT FIRST_PASS_DONE AND NOT CMAKE_BUILD_TYPE AND NOT MSVC AND NOT XCODE_VERSION) - set(CMAKE_BUILD_TYPE "Release" CACHE STRING - "Choose the type of build, options are: None Debug Release Timing Profiling RelWithDebInfo." - FORCE) +if(NOT CMAKE_BUILD_TYPE AND NOT MSVC AND NOT XCODE_VERSION) + set(GTSAM_CMAKE_BUILD_TYPE "Release" CACHE STRING + "Choose the type of build, options are: None Debug Release Timing Profiling RelWithDebInfo.") + set(CMAKE_BUILD_TYPE ${GTSAM_CMAKE_BUILD_TYPE}) endif() # Add option for using build type postfixes to allow installing multiple build modes option(GTSAM_BUILD_TYPE_POSTFIXES "Enable/Disable appending the build type to the name of compiled libraries" ON) -# Add debugging flags but only on the first pass -if(NOT FIRST_PASS_DONE) - if(MSVC) - set(CMAKE_C_FLAGS_DEBUG "/D_DEBUG /MDd /Zi /Ob0 /Od /RTC1 /W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN /DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Flags used by the compiler during debug builds." FORCE) - set(CMAKE_CXX_FLAGS_DEBUG "/D_DEBUG /MDd /Zi /Ob0 /Od /RTC1 /W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN /DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Flags used by the compiler during debug builds." FORCE) - set(CMAKE_C_FLAGS_RELWITHDEBINFO "/MD /O2 /DNDEBUG /W3 /GR /EHsc /MP /Zi /d2Zi+ /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during relwithdebinfo builds." FORCE) - set(CMAKE_CXX_FLAGS_RELWITHDEBINFO "/MD /O2 /DNDEBUG /W3 /GR /EHsc /MP /Zi /d2Zi+ /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during relwithdebinfo builds." FORCE) - set(CMAKE_C_FLAGS_RELEASE "/MD /O2 /DNDEBUG /W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during release builds." FORCE) - set(CMAKE_CXX_FLAGS_RELEASE "/MD /O2 /DNDEBUG /W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during release builds." FORCE) - set(CMAKE_C_FLAGS_TIMING "${CMAKE_C_FLAGS_RELEASE} /DENABLE_TIMING" CACHE STRING "Flags used by the compiler during timing builds." FORCE) - set(CMAKE_CXX_FLAGS_TIMING "${CMAKE_CXX_FLAGS_RELEASE} /DENABLE_TIMING" CACHE STRING "Flags used by the compiler during timing builds." FORCE) - set(CMAKE_EXE_LINKER_FLAGS_TIMING "${CMAKE_EXE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during timing builds." FORCE) - set(CMAKE_SHARED_LINKER_FLAGS_TIMING "${CMAKE_SHARED_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during timing builds." FORCE) - set(CMAKE_MODULE_LINKER_FLAGS_TIMING "${CMAKE_MODULE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during timing builds." FORCE) - mark_as_advanced(CMAKE_C_FLAGS_TIMING CMAKE_CXX_FLAGS_TIMING CMAKE_EXE_LINKER_FLAGS_TIMING CMAKE_SHARED_LINKER_FLAGS_TIMING CMAKE_MODULE_LINKER_FLAGS_TIMING) - set(CMAKE_C_FLAGS_PROFILING "/MD /O2 /DNDEBUG /W3 /GR /EHsc /MP /Zi /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during profiling builds." FORCE) - set(CMAKE_CXX_FLAGS_PROFILING "/MD /O2 /DNDEBUG /W3 /GR /EHsc /MP /Zi /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during profiling builds." FORCE) - set(CMAKE_EXE_LINKER_FLAGS_PROFILING "${CMAKE_EXE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during profiling builds." FORCE) - set(CMAKE_SHARED_LINKER_FLAGS_PROFILING "${CMAKE_SHARED_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during profiling builds." FORCE) - set(CMAKE_MODULE_LINKER_FLAGS_PROFILING "${CMAKE_MODULE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during profiling builds." FORCE) - mark_as_advanced(CMAKE_C_FLAGS_PROFILING CMAKE_CXX_FLAGS_PROFILING CMAKE_EXE_LINKER_FLAGS_PROFILING CMAKE_SHARED_LINKER_FLAGS_PROFILING CMAKE_MODULE_LINKER_FLAGS_PROFILING) - else() - set(CMAKE_C_FLAGS_DEBUG "${CMAKE_C_FLAGS_DEBUG} -std=c11 -fno-inline -Wall -DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Flags used by the compiler during debug builds." FORCE) - set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -std=c++11 -fno-inline -Wall -DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Flags used by the compiler during debug builds." FORCE) - set(CMAKE_C_FLAGS_RELWITHDEBINFO "-std=c11 -g -O3 -Wall -DNDEBUG" CACHE STRING "Flags used by the compiler during relwithdebinfo builds." FORCE) - set(CMAKE_CXX_FLAGS_RELWITHDEBINFO "-std=c++11 -g -O3 -Wall -DNDEBUG" CACHE STRING "Flags used by the compiler during relwithdebinfo builds." FORCE) - set(CMAKE_C_FLAGS_RELEASE "-std=c11 -O3 -Wall -DNDEBUG -Wall" CACHE STRING "Flags used by the compiler during release builds." FORCE) - set(CMAKE_CXX_FLAGS_RELEASE "-std=c++11 -O3 -Wall -DNDEBUG -Wall" CACHE STRING "Flags used by the compiler during release builds." FORCE) - set(CMAKE_C_FLAGS_TIMING "${CMAKE_C_FLAGS_RELEASE} -DENABLE_TIMING" CACHE STRING "Flags used by the compiler during timing builds." FORCE) - set(CMAKE_CXX_FLAGS_TIMING "${CMAKE_CXX_FLAGS_RELEASE} -DENABLE_TIMING" CACHE STRING "Flags used by the compiler during timing builds." FORCE) - set(CMAKE_EXE_LINKER_FLAGS_TIMING "${CMAKE_EXE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during timing builds." FORCE) - set(CMAKE_SHARED_LINKER_FLAGS_TIMING "${CMAKE_EXE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during timing builds." FORCE) - mark_as_advanced(CMAKE_C_FLAGS_TIMING CMAKE_CXX_FLAGS_TIMING CMAKE_EXE_LINKER_FLAGS_TIMING CMAKE_SHARED_LINKER_FLAGS_TIMING) - set(CMAKE_C_FLAGS_PROFILING "-std=c11 -g -O3 -Wall -DNDEBUG" CACHE STRING "Flags used by the compiler during profiling builds." FORCE) - set(CMAKE_CXX_FLAGS_PROFILING "-std=c++11 -g -O3 -Wall -DNDEBUG" CACHE STRING "Flags used by the compiler during profiling builds." FORCE) - set(CMAKE_EXE_LINKER_FLAGS_PROFILING "${CMAKE_EXE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during profiling builds." FORCE) - set(CMAKE_SHARED_LINKER_FLAGS_PROFILING "${CMAKE__LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during profiling builds." FORCE) - mark_as_advanced(CMAKE_C_FLAGS_PROFILING CMAKE_CXX_FLAGS_PROFILING CMAKE_EXE_LINKER_FLAGS_PROFILING CMAKE_SHARED_LINKER_FLAGS_PROFILING) - endif() +# Set custom compilation flags. +# NOTE: We set all the CACHE variables with a GTSAM prefix, and then set a normal local variable below +# so that we don't "pollute" the global variable namespace in the cmake cache. +if(MSVC) + set(GTSAM_CMAKE_C_FLAGS_DEBUG "/D_DEBUG /MDd /Zi /Ob0 /Od /RTC1 /W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN /DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Flags used by the compiler during debug builds.") + set(GTSAM_CMAKE_CXX_FLAGS_DEBUG "/D_DEBUG /MDd /Zi /Ob0 /Od /RTC1 /W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN /DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Flags used by the compiler during debug builds.") + set(GTSAM_CMAKE_C_FLAGS_RELWITHDEBINFO "/MD /O2 /DNDEBUG /W3 /GR /EHsc /MP /Zi /d2Zi+ /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during relwithdebinfo builds.") + set(GTSAM_CMAKE_CXX_FLAGS_RELWITHDEBINFO "/MD /O2 /DNDEBUG /W3 /GR /EHsc /MP /Zi /d2Zi+ /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during relwithdebinfo builds.") + set(GTSAM_CMAKE_C_FLAGS_RELEASE "/MD /O2 /DNDEBUG /W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during release builds.") + set(GTSAM_CMAKE_CXX_FLAGS_RELEASE "/MD /O2 /DNDEBUG /W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during release builds.") + set(GTSAM_CMAKE_C_FLAGS_PROFILING "${GTSAM_CMAKE_C_FLAGS_RELEASE} /Zi" CACHE STRING "Flags used by the compiler during profiling builds.") + set(GTSAM_CMAKE_CXX_FLAGS_PROFILING "${GTSAM_CMAKE_CXX_FLAGS_RELEASE} /Zi" CACHE STRING "Flags used by the compiler during profiling builds.") + set(GTSAM_CMAKE_C_FLAGS_TIMING "${GTSAM_CMAKE_C_FLAGS_RELEASE} /DENABLE_TIMING" CACHE STRING "Flags used by the compiler during timing builds.") + set(GTSAM_CMAKE_CXX_FLAGS_TIMING "${GTSAM_CMAKE_CXX_FLAGS_RELEASE} /DENABLE_TIMING" CACHE STRING "Flags used by the compiler during timing builds.") +else() + set(GTSAM_CMAKE_C_FLAGS_DEBUG "-std=c11 -Wall -g -fno-inline -DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Flags used by the compiler during debug builds.") + set(GTSAM_CMAKE_CXX_FLAGS_DEBUG "-std=c++11 -Wall -g -fno-inline -DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Flags used by the compiler during debug builds.") + set(GTSAM_CMAKE_C_FLAGS_RELWITHDEBINFO "-std=c11 -Wall -g -O3 -DNDEBUG" CACHE STRING "Flags used by the compiler during relwithdebinfo builds.") + set(GTSAM_CMAKE_CXX_FLAGS_RELWITHDEBINFO "-std=c++11 -Wall -g -O3 -DNDEBUG" CACHE STRING "Flags used by the compiler during relwithdebinfo builds.") + set(GTSAM_CMAKE_C_FLAGS_RELEASE "-std=c11 -Wall -O3 -DNDEBUG" CACHE STRING "Flags used by the compiler during release builds.") + set(GTSAM_CMAKE_CXX_FLAGS_RELEASE "-std=c++11 -Wall -O3 -DNDEBUG" CACHE STRING "Flags used by the compiler during release builds.") + set(GTSAM_CMAKE_C_FLAGS_PROFILING "${GTSAM_CMAKE_C_FLAGS_RELEASE}" CACHE STRING "Flags used by the compiler during profiling builds.") + set(GTSAM_CMAKE_CXX_FLAGS_PROFILING "${GTSAM_CMAKE_CXX_FLAGS_RELEASE}" CACHE STRING "Flags used by the compiler during profiling builds.") + set(GTSAM_CMAKE_C_FLAGS_TIMING "${GTSAM_CMAKE_C_FLAGS_RELEASE} -DENABLE_TIMING" CACHE STRING "Flags used by the compiler during timing builds.") + set(GTSAM_CMAKE_CXX_FLAGS_TIMING "${GTSAM_CMAKE_CXX_FLAGS_RELEASE} -DENABLE_TIMING" CACHE STRING "Flags used by the compiler during timing builds.") endif() +set(GTSAM_CMAKE_SHARED_LINKER_FLAGS_TIMING "${CMAKE_SHARED_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during timing builds.") +set(GTSAM_CMAKE_MODULE_LINKER_FLAGS_TIMING "${CMAKE_MODULE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during timing builds.") +set(GTSAM_CMAKE_EXE_LINKER_FLAGS_TIMING "${CMAKE_EXE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during timing builds.") + +set(GTSAM_CMAKE_SHARED_LINKER_FLAGS_PROFILING "${CMAKE_SHARED_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during profiling builds.") +set(GTSAM_CMAKE_MODULE_LINKER_FLAGS_PROFILING "${CMAKE_MODULE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during profiling builds.") +set(GTSAM_CMAKE_EXE_LINKER_FLAGS_PROFILING "${CMAKE_EXE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during profiling builds.") + +mark_as_advanced(GTSAM_CMAKE_C_FLAGS_TIMING GTSAM_CMAKE_CXX_FLAGS_TIMING GTSAM_CMAKE_EXE_LINKER_FLAGS_TIMING + GTSAM_CMAKE_SHARED_LINKER_FLAGS_TIMING GTSAM_CMAKE_MODULE_LINKER_FLAGS_TIMING + GTSAM_CMAKE_C_FLAGS_PROFILING GTSAM_CMAKE_CXX_FLAGS_PROFILING GTSAM_CMAKE_EXE_LINKER_FLAGS_PROFILING + GTSAM_CMAKE_SHARED_LINKER_FLAGS_PROFILING GTSAM_CMAKE_MODULE_LINKER_FLAGS_PROFILING) + +# Apply the gtsam specific build flags as normal variables. This makes it so that they only +# apply to the gtsam part of the build if gtsam is built as a subproject +set(CMAKE_C_FLAGS_DEBUG ${GTSAM_CMAKE_C_FLAGS_DEBUG}) +set(CMAKE_CXX_FLAGS_DEBUG ${GTSAM_CMAKE_CXX_FLAGS_DEBUG}) +set(CMAKE_C_FLAGS_RELWITHDEBINFO ${GTSAM_CMAKE_C_FLAGS_RELWITHDEBINFO}) +set(CMAKE_CXX_FLAGS_RELWITHDEBINFO ${GTSAM_CMAKE_CXX_FLAGS_RELWITHDEBINFO}) +set(CMAKE_C_FLAGS_RELEASE ${GTSAM_CMAKE_C_FLAGS_RELEASE}) +set(CMAKE_CXX_FLAGS_RELEASE ${GTSAM_CMAKE_CXX_FLAGS_RELEASE}) +set(CMAKE_C_FLAGS_PROFILING ${GTSAM_CMAKE_C_FLAGS_PROFILING}) +set(CMAKE_CXX_FLAGS_PROFILING ${GTSAM_CMAKE_CXX_FLAGS_PROFILING}) +set(CMAKE_C_FLAGS_TIMING ${GTSAM_CMAKE_C_FLAGS_TIMING}) +set(CMAKE_CXX_FLAGS_TIMING ${GTSAM_CMAKE_CXX_FLAGS_TIMING}) + +set(CMAKE_SHARED_LINKER_FLAGS_TIMING ${GTSAM_CMAKE_SHARED_LINKER_FLAGS_TIMING}) +set(CMAKE_MODULE_LINKER_FLAGS_TIMING ${GTSAM_CMAKE_MODULE_LINKER_FLAGS_TIMING}) +set(CMAKE_EXE_LINKER_FLAGS_TIMING ${GTSAM_CMAKE_EXE_LINKER_FLAGS_TIMING}) + +set(CMAKE_SHARED_LINKER_FLAGS_PROFILING ${GTSAM_CMAKE_SHARED_LINKER_FLAGS_PROFILING}) +set(CMAKE_MODULE_LINKER_FLAGS_PROFILING ${GTSAM_CMAKE_MODULE_LINKER_FLAGS_PROFILING}) +set(CMAKE_EXE_LINKER_FLAGS_PROFILING ${GTSAM_CMAKE_EXE_LINKER_FLAGS_PROFILING}) + # Clang uses a template depth that is less than standard and is too small if(${CMAKE_CXX_COMPILER_ID} STREQUAL "Clang") # Apple Clang before 5.0 does not support -ftemplate-depth. @@ -63,10 +83,10 @@ endif() # Set up build type library postfixes if(GTSAM_BUILD_TYPE_POSTFIXES) - foreach(build_type Debug Timing Profiling RelWithDebInfo MinSizeRel) - string(TOUPPER "${build_type}" build_type_toupper) - set(CMAKE_${build_type_toupper}_POSTFIX ${build_type}) - endforeach() + foreach(build_type Debug Timing Profiling RelWithDebInfo MinSizeRel) + string(TOUPPER "${build_type}" build_type_toupper) + set(CMAKE_${build_type_toupper}_POSTFIX ${build_type}) + endforeach() endif() # Make common binary output directory when on Windows @@ -78,17 +98,16 @@ endif() # Set up build type list for cmake-gui if(NOT "${CMAKE_BUILD_TYPE}" STREQUAL "") - if(${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} VERSION_GREATER 2.8 OR ${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} VERSION_EQUAL 2.8) - set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS None Debug Release Timing Profiling RelWithDebInfo MinSizeRel) - endif() + if(${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} VERSION_GREATER 2.8 OR ${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} VERSION_EQUAL 2.8) + set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS None Debug Release Timing Profiling RelWithDebInfo MinSizeRel) + endif() endif() # Set up build types for MSVC and XCode -if(NOT FIRST_PASS_DONE) - set(CMAKE_CONFIGURATION_TYPES Debug Release Timing Profiling RelWithDebInfo MinSizeRel - CACHE STRING "Build types available to MSVC and XCode" FORCE) - mark_as_advanced(FORCE CMAKE_CONFIGURATION_TYPES) -endif() +set(GTSAM_CMAKE_CONFIGURATION_TYPES Debug Release Timing Profiling RelWithDebInfo MinSizeRel + CACHE STRING "Build types available to MSVC and XCode") +mark_as_advanced(FORCE GTSAM_CMAKE_CONFIGURATION_TYPES) +set(CMAKE_CONFIGURATION_TYPES ${GTSAM_CMAKE_CONFIGURATION_TYPES}) # Check build types string(TOLOWER "${CMAKE_BUILD_TYPE}" cmake_build_type_tolower) @@ -100,13 +119,9 @@ if( NOT cmake_build_type_tolower STREQUAL "" AND NOT cmake_build_type_tolower STREQUAL "profiling" AND NOT cmake_build_type_tolower STREQUAL "relwithdebinfo" AND NOT cmake_build_type_tolower STREQUAL "minsizerel") - message(FATAL_ERROR "Unknown build type \"${CMAKE_BUILD_TYPE}\". Allowed values are None, Debug, Release, Timing, Profiling, RelWithDebInfo (case-insensitive).") + message(FATAL_ERROR "Unknown build type \"${CMAKE_BUILD_TYPE}\". Allowed values are None, Debug, Release, Timing, Profiling, RelWithDebInfo, MinSizeRel (case-insensitive).") endif() -# Mark that first pass is done -set(FIRST_PASS_DONE TRUE CACHE INTERNAL "Internally used to mark whether cmake has been run multiple times") -mark_as_advanced(FIRST_PASS_DONE) - # Enable Visual Studio solution folders set_property(GLOBAL PROPERTY USE_FOLDERS On) diff --git a/cmake/GtsamTesting.cmake b/cmake/GtsamTesting.cmake index 3e5cadd32..e13888c00 100644 --- a/cmake/GtsamTesting.cmake +++ b/cmake/GtsamTesting.cmake @@ -164,9 +164,9 @@ macro(gtsamAddTestsGlob_impl groupName globPatterns excludedFiles linkLibraries) add_test(NAME ${script_name} COMMAND ${script_name}) add_dependencies(check.${groupName} ${script_name}) add_dependencies(check ${script_name}) - add_dependencies(all.tests ${script_name}) + add_dependencies(all.tests ${script_name}) if(NOT MSVC AND NOT XCODE_VERSION) - add_custom_target(${script_name}.run ${EXECUTABLE_OUTPUT_PATH}${script_name}) + add_custom_target(${script_name}.run ${EXECUTABLE_OUTPUT_PATH}${script_name} DEPENDS ${script_name}) endif() # Add TOPSRCDIR @@ -254,7 +254,7 @@ macro(gtsamAddExesGlob_impl globPatterns excludedFiles linkLibraries groupName b # Add target dependencies add_dependencies(${groupName} ${script_name}) if(NOT MSVC AND NOT XCODE_VERSION) - add_custom_target(${script_name}.run ${EXECUTABLE_OUTPUT_PATH}${script_name}) + add_custom_target(${script_name}.run ${EXECUTABLE_OUTPUT_PATH}${script_name} DEPENDS ${script_name}) endif() # Add TOPSRCDIR diff --git a/gtsam.h b/gtsam.h index c42cf7031..3d225529e 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1742,7 +1742,8 @@ virtual class NonlinearFactor { virtual class NoiseModelFactor: gtsam::NonlinearFactor { void equals(const gtsam::NoiseModelFactor& other, double tol) const; - gtsam::noiseModel::Base* get_noiseModel() const; + gtsam::noiseModel::Base* get_noiseModel() const; // deprecated by below + gtsam::noiseModel::Base* noiseModel() const; Vector unwhitenedError(const gtsam::Values& x) const; Vector whitenedError(const gtsam::Values& x) const; }; diff --git a/gtsam/3rdparty/CMakeLists.txt b/gtsam/3rdparty/CMakeLists.txt index 4adbfb250..8534a8d7e 100644 --- a/gtsam/3rdparty/CMakeLists.txt +++ b/gtsam/3rdparty/CMakeLists.txt @@ -43,7 +43,9 @@ if(NOT GTSAM_USE_SYSTEM_EIGEN) endif() option(GTSAM_BUILD_METIS_EXECUTABLES "Build metis library executables" OFF) -add_subdirectory(metis) +if(GTSAM_SUPPORT_NESTED_DISSECTION) + add_subdirectory(metis) +endif() add_subdirectory(ceres) diff --git a/gtsam/3rdparty/ceres/fixed_array.h b/gtsam/3rdparty/ceres/fixed_array.h index 455fce383..d85021dbd 100644 --- a/gtsam/3rdparty/ceres/fixed_array.h +++ b/gtsam/3rdparty/ceres/fixed_array.h @@ -33,7 +33,7 @@ #define CERES_PUBLIC_INTERNAL_FIXED_ARRAY_H_ #include -#include +#include #include #include diff --git a/gtsam/3rdparty/ceres/jet.h b/gtsam/3rdparty/ceres/jet.h index 4a7683f50..d45dab9e5 100644 --- a/gtsam/3rdparty/ceres/jet.h +++ b/gtsam/3rdparty/ceres/jet.h @@ -162,7 +162,7 @@ #include #include -#include +#include #include namespace ceres { diff --git a/gtsam/3rdparty/metis/CMakeLists.txt b/gtsam/3rdparty/metis/CMakeLists.txt index 6ba17787f..dd21338a4 100644 --- a/gtsam/3rdparty/metis/CMakeLists.txt +++ b/gtsam/3rdparty/metis/CMakeLists.txt @@ -20,7 +20,7 @@ if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU") endif() set(GKLIB_PATH ${PROJECT_SOURCE_DIR}/GKlib CACHE PATH "path to GKlib") -set(SHARED FALSE CACHE BOOL "build a shared library") +set(METIS_SHARED TRUE CACHE BOOL "build a shared library") if(MSVC) set(METIS_INSTALL FALSE) @@ -29,11 +29,11 @@ else() endif() # Configure libmetis library. -if(SHARED) +if(METIS_SHARED) set(METIS_LIBRARY_TYPE SHARED) else() set(METIS_LIBRARY_TYPE STATIC) -endif(SHARED) +endif(METIS_SHARED) include(${GKLIB_PATH}/GKlibSystem.cmake) # Add include directories. diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index cab5e8639..d3229427e 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -88,7 +88,9 @@ configure_file("${PROJECT_SOURCE_DIR}/cmake/dllexport.h.in" "dllexport.h") list(APPEND gtsam_srcs "${PROJECT_BINARY_DIR}/gtsam/config.h" "${PROJECT_BINARY_DIR}/gtsam/dllexport.h") install(FILES "${PROJECT_BINARY_DIR}/gtsam/config.h" "${PROJECT_BINARY_DIR}/gtsam/dllexport.h" DESTINATION include/gtsam) -list(APPEND GTSAM_ADDITIONAL_LIBRARIES metis) +if(GTSAM_SUPPORT_NESTED_DISSECTION) + list(APPEND GTSAM_ADDITIONAL_LIBRARIES metis) +endif() # Versions set(gtsam_version ${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSION_PATCH}) diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index 23ac0f4df..bb49a84d6 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.h @@ -327,6 +327,15 @@ T expm(const Vector& x, int K=7) { return T(expm(xhat,K)); } +/** + * Linear interpolation between X and Y by coefficient t in [0, 1]. + */ +template +T interpolate(const T& X, const T& Y, double t) { + assert(t >= 0 && t <= 1); + return traits::Compose(X, traits::Expmap(t * traits::Logmap(traits::Between(X, Y)))); +} + } // namespace gtsam /** diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index 37a0d28b8..b0b292c56 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -46,29 +46,29 @@ typedef Eigen::Matrix M // Create handy typedefs and constants for square-size matrices // MatrixMN, MatrixN = MatrixNN, I_NxN, and Z_NxN, for M,N=1..9 -#define GTSAM_MAKE_TYPEDEFS(SIZE, SUFFIX) \ -typedef Eigen::Matrix Matrix##SUFFIX; \ -typedef Eigen::Matrix Matrix1##SUFFIX; \ -typedef Eigen::Matrix Matrix2##SUFFIX; \ -typedef Eigen::Matrix Matrix3##SUFFIX; \ -typedef Eigen::Matrix Matrix4##SUFFIX; \ -typedef Eigen::Matrix Matrix5##SUFFIX; \ -typedef Eigen::Matrix Matrix6##SUFFIX; \ -typedef Eigen::Matrix Matrix7##SUFFIX; \ -typedef Eigen::Matrix Matrix8##SUFFIX; \ -typedef Eigen::Matrix Matrix9##SUFFIX; \ -static const Eigen::MatrixBase::IdentityReturnType I_##SUFFIX##x##SUFFIX = Matrix##SUFFIX::Identity(); \ -static const Eigen::MatrixBase::ConstantReturnType Z_##SUFFIX##x##SUFFIX = Matrix##SUFFIX::Zero(); +#define GTSAM_MAKE_MATRIX_DEFS(N) \ +typedef Eigen::Matrix Matrix##N; \ +typedef Eigen::Matrix Matrix1##N; \ +typedef Eigen::Matrix Matrix2##N; \ +typedef Eigen::Matrix Matrix3##N; \ +typedef Eigen::Matrix Matrix4##N; \ +typedef Eigen::Matrix Matrix5##N; \ +typedef Eigen::Matrix Matrix6##N; \ +typedef Eigen::Matrix Matrix7##N; \ +typedef Eigen::Matrix Matrix8##N; \ +typedef Eigen::Matrix Matrix9##N; \ +static const Eigen::MatrixBase::IdentityReturnType I_##N##x##N = Matrix##N::Identity(); \ +static const Eigen::MatrixBase::ConstantReturnType Z_##N##x##N = Matrix##N::Zero(); -GTSAM_MAKE_TYPEDEFS(1,1); -GTSAM_MAKE_TYPEDEFS(2,2); -GTSAM_MAKE_TYPEDEFS(3,3); -GTSAM_MAKE_TYPEDEFS(4,4); -GTSAM_MAKE_TYPEDEFS(5,5); -GTSAM_MAKE_TYPEDEFS(6,6); -GTSAM_MAKE_TYPEDEFS(7,7); -GTSAM_MAKE_TYPEDEFS(8,8); -GTSAM_MAKE_TYPEDEFS(9,9); +GTSAM_MAKE_MATRIX_DEFS(1); +GTSAM_MAKE_MATRIX_DEFS(2); +GTSAM_MAKE_MATRIX_DEFS(3); +GTSAM_MAKE_MATRIX_DEFS(4); +GTSAM_MAKE_MATRIX_DEFS(5); +GTSAM_MAKE_MATRIX_DEFS(6); +GTSAM_MAKE_MATRIX_DEFS(7); +GTSAM_MAKE_MATRIX_DEFS(8); +GTSAM_MAKE_MATRIX_DEFS(9); // Matrix expressions for accessing parts of matrices typedef Eigen::Block SubMatrix; @@ -135,7 +135,7 @@ inline bool operator==(const Matrix& A, const Matrix& B) { } /** - * inequality + * inequality */ inline bool operator!=(const Matrix& A, const Matrix& B) { return !(A==B); @@ -371,7 +371,7 @@ GTSAM_EXPORT Matrix inverse(const Matrix& A); * m*n matrix -> m*m Q, m*n R * @param A a matrix * @return rotation matrix Q, upper triangular R - */ + */ GTSAM_EXPORT std::pair qr(const Matrix& A); /** @@ -434,7 +434,7 @@ GTSAM_EXPORT Vector backSubstituteUpper(const Vector& b, const Matrix& U, bool u * @param b an RHS vector * @param unit, set true if unit triangular * @return the solution x of L*x=b - */ + */ GTSAM_EXPORT Vector backSubstituteLower(const Matrix& L, const Vector& b, bool unit=false); /** diff --git a/gtsam/base/Testable.h b/gtsam/base/Testable.h index 92ccb9156..73fb320e1 100644 --- a/gtsam/base/Testable.h +++ b/gtsam/base/Testable.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -13,20 +13,20 @@ * @file Testable.h * @brief Concept check for values that can be used in unit tests * @author Frank Dellaert - * + * * The necessary functions to implement for Testable are defined * below with additional details as to the interface. * The concept checking function will check whether or not * the function exists in derived class and throw compile-time errors. - * + * * print with optional string naming the object * void print(const std::string& name) const = 0; - * + * * equality up to tolerance * tricky to implement, see NoiseModelFactor1 for an example * equals is not supposed to print out *anything*, just return true|false * bool equals(const Derived& expected, double tol) const = 0; - * + * */ // \callgraph @@ -123,7 +123,7 @@ namespace gtsam { double tol_; equals_star(double tol = 1e-9) : tol_(tol) {} bool operator()(const boost::shared_ptr& expected, const boost::shared_ptr& actual) { - if (!actual || !expected) return true; + if (!actual && !expected) return true; return actual && expected && traits::Equals(*actual,*expected, tol_); } }; diff --git a/gtsam/base/Vector.cpp b/gtsam/base/Vector.cpp index ed6373f5b..61a42fead 100644 --- a/gtsam/base/Vector.cpp +++ b/gtsam/base/Vector.cpp @@ -292,7 +292,7 @@ double weightedPseudoinverse(const Vector& a, const Vector& weights, // Basically, instead of doing a normal QR step with the weighted // pseudoinverse, we enforce the constraint by turning // ax + AS = b into x + (A/a)S = b/a, for the first row where a!=0 - pseudo = delta(m, i, 1 / a[i]); + pseudo = delta(m, i, 1.0 / a[i]); return inf; } } diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index 20477a205..f87703e2b 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -41,13 +41,25 @@ typedef Eigen::VectorXd Vector; typedef Eigen::Matrix Vector1; typedef Eigen::Vector2d Vector2; typedef Eigen::Vector3d Vector3; -typedef Eigen::Matrix Vector4; -typedef Eigen::Matrix Vector5; -typedef Eigen::Matrix Vector6; -typedef Eigen::Matrix Vector7; -typedef Eigen::Matrix Vector8; -typedef Eigen::Matrix Vector9; -typedef Eigen::Matrix Vector10; + +static const Eigen::MatrixBase::ConstantReturnType Z_2x1 = Vector2::Zero(); +static const Eigen::MatrixBase::ConstantReturnType Z_3x1 = Vector3::Zero(); + +// Create handy typedefs and constants for vectors with N>3 +// VectorN and Z_Nx1, for N=1..9 +#define GTSAM_MAKE_VECTOR_DEFS(N) \ + typedef Eigen::Matrix Vector##N; \ + static const Eigen::MatrixBase::ConstantReturnType Z_##N##x1 = Vector##N::Zero(); + +GTSAM_MAKE_VECTOR_DEFS(4); +GTSAM_MAKE_VECTOR_DEFS(5); +GTSAM_MAKE_VECTOR_DEFS(6); +GTSAM_MAKE_VECTOR_DEFS(7); +GTSAM_MAKE_VECTOR_DEFS(8); +GTSAM_MAKE_VECTOR_DEFS(9); +GTSAM_MAKE_VECTOR_DEFS(10); +GTSAM_MAKE_VECTOR_DEFS(11); +GTSAM_MAKE_VECTOR_DEFS(12); typedef Eigen::VectorBlock SubVector; typedef Eigen::VectorBlock ConstSubVector; @@ -89,7 +101,7 @@ inline Vector zero(size_t n) { return Vector::Zero(n);} * @param n size */ inline Vector ones(size_t n) { return Vector::Ones(n); } - + /** * check if all zero */ diff --git a/gtsam/base/VectorSpace.h b/gtsam/base/VectorSpace.h index c356dcc07..cc28ac893 100644 --- a/gtsam/base/VectorSpace.h +++ b/gtsam/base/VectorSpace.h @@ -43,7 +43,7 @@ struct VectorSpaceImpl { ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { if (H1) *H1 = Jacobian::Identity(); if (H2) *H2 = Jacobian::Identity(); - return origin + (Class)v; + return origin + v; } /// @} @@ -117,7 +117,7 @@ struct VectorSpaceImpl { ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { if (H1) *H1 = Eye(origin); if (H2) *H2 = Eye(origin); - return origin + Class(v); + return origin + v; } /// @} @@ -159,12 +159,34 @@ struct VectorSpaceImpl { /// @} }; -/// A helper that implements the traits interface for GTSAM vector space types. -/// To use this for your gtsam type, define: -/// template<> struct traits : public VectorSpaceTraits { }; +/// Requirements on type to pass it to Manifold template below +template +struct HasVectorSpacePrereqs { + + enum { dim = Class::dimension }; + + Class p, q; + Vector v; + + BOOST_CONCEPT_USAGE(HasVectorSpacePrereqs) { + p = Class::identity(); // identity + q = p + p; // addition + q = p - p; // subtraction + v = p.vector(); // conversion to vector + q = p + v; // addition of a vector on the right + } +}; + +/// A helper that implements the traits interface for *classes* that define vector spaces +/// To use this for your class, define: +/// template<> struct traits : public VectorSpaceTraits {}; +/// The class needs to support the requirements defined by HasVectorSpacePrereqs above template struct VectorSpaceTraits: VectorSpaceImpl { + // Check that Class has the necessary machinery + BOOST_CONCEPT_ASSERT((HasVectorSpacePrereqs)); + typedef vector_space_tag structure_category; /// @name Group @@ -180,12 +202,11 @@ struct VectorSpaceTraits: VectorSpaceImpl { /// @} }; -/// Both VectorSpaceTRaits and Testable +/// VectorSpace provides both Testable and VectorSpaceTraits template struct VectorSpace: Testable, VectorSpaceTraits {}; -/// A helper that implements the traits interface for GTSAM lie groups. -/// To use this for your gtsam type, define: +/// A helper that implements the traits interface for scalar vector spaces. Usage: /// template<> struct traits : public ScalarTraits { }; template struct ScalarTraits : VectorSpaceImpl { diff --git a/gtsam/base/VerticalBlockMatrix.h b/gtsam/base/VerticalBlockMatrix.h index c6a3eb034..9cd566ecf 100644 --- a/gtsam/base/VerticalBlockMatrix.h +++ b/gtsam/base/VerticalBlockMatrix.h @@ -165,24 +165,24 @@ namespace gtsam { return variableColOffsets_[actualBlock]; } + /** Get the apparent first row of the underlying matrix for all operations */ + const DenseIndex& rowStart() const { return rowStart_; } + /** Get or set the apparent first row of the underlying matrix for all operations */ DenseIndex& rowStart() { return rowStart_; } + /** Get the apparent last row (exclusive, i.e. rows() == rowEnd() - rowStart()) of the underlying matrix for all operations */ + const DenseIndex& rowEnd() const { return rowEnd_; } + /** Get or set the apparent last row (exclusive, i.e. rows() == rowEnd() - rowStart()) of the underlying matrix for all operations */ DenseIndex& rowEnd() { return rowEnd_; } + /** Get the apparent first block for all operations */ + const DenseIndex& firstBlock() const { return blockStart_; } + /** Get or set the apparent first block for all operations */ DenseIndex& firstBlock() { return blockStart_; } - /** Get the apparent first row of the underlying matrix for all operations */ - DenseIndex rowStart() const { return rowStart_; } - - /** Get the apparent last row (exclusive, i.e. rows() == rowEnd() - rowStart()) of the underlying matrix for all operations */ - DenseIndex rowEnd() const { return rowEnd_; } - - /** Get the apparent first block for all operations */ - DenseIndex firstBlock() const { return blockStart_; } - /** Access to full matrix (*including* any portions excluded by rowStart(), rowEnd(), and firstBlock()) */ const Matrix& matrix() const { return matrix_; } diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h index a7dc37d55..9a42db3ce 100644 --- a/gtsam/base/numericalDerivative.h +++ b/gtsam/base/numericalDerivative.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -131,11 +131,11 @@ typename internal::FixedSizeMatrix::type numericalDerivative11(boost::funct typedef typename TraitsX::TangentVector TangentX; // get value at x, and corresponding chart - Y hx = h(x); + const Y hx = h(x); // Bit of a hack for now to find number of rows - TangentY zeroY = TraitsY::Local(hx, hx); - size_t m = zeroY.size(); + const TangentY zeroY = TraitsY::Local(hx, hx); + const size_t m = zeroY.size(); // Prepare a tangent vector to perturb x with, only works for fixed size TangentX dx; @@ -143,12 +143,12 @@ typename internal::FixedSizeMatrix::type numericalDerivative11(boost::funct // Fill in Jacobian H Matrix H = zeros(m, N); - double factor = 1.0 / (2.0 * delta); + const double factor = 1.0 / (2.0 * delta); for (int j = 0; j < N; j++) { dx(j) = delta; - TangentY dy1 = TraitsY::Local(hx, h(TraitsX::Retract(x, dx))); + const TangentY dy1 = TraitsY::Local(hx, h(TraitsX::Retract(x, dx))); dx(j) = -delta; - TangentY dy2 = TraitsY::Local(hx, h(TraitsX::Retract(x, dx))); + const TangentY dy2 = TraitsY::Local(hx, h(TraitsX::Retract(x, dx))); dx(j) = 0; H.col(j) << (dy1 - dy2) * factor; } diff --git a/gtsam/base/tests/testFastContainers.cpp b/gtsam/base/tests/testFastContainers.cpp index 19870fdeb..4909f4ecb 100644 --- a/gtsam/base/tests/testFastContainers.cpp +++ b/gtsam/base/tests/testFastContainers.cpp @@ -22,11 +22,11 @@ using namespace gtsam; /* ************************************************************************* */ TEST( testFastContainers, KeySet ) { - FastVector init_vector; + KeyVector init_vector; init_vector += 2, 3, 4, 5; - FastSet actSet(init_vector); - FastSet expSet; expSet += 2, 3, 4, 5; + KeySet actSet(init_vector); + KeySet expSet; expSet += 2, 3, 4, 5; EXPECT(actSet == expSet); } diff --git a/gtsam/base/types.h b/gtsam/base/types.h index 84c94e73d..c4775b672 100644 --- a/gtsam/base/types.h +++ b/gtsam/base/types.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -25,6 +25,7 @@ #include // for GTSAM_USE_TBB #include +#include #ifdef GTSAM_USE_TBB #include @@ -53,7 +54,7 @@ namespace gtsam { /// Integer nonlinear key type - typedef size_t Key; + typedef std::uint64_t Key; /// The index type for Eigen objects typedef ptrdiff_t DenseIndex; diff --git a/gtsam/config.h.in b/gtsam/config.h.in index cb195dc03..f9a576d14 100644 --- a/gtsam/config.h.in +++ b/gtsam/config.h.in @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -32,11 +32,11 @@ #cmakedefine GTSAM_USE_QUATERNIONS // Whether GTSAM is compiled to use Pose3::EXPMAP as the default coordinates mode for Pose3's retract and localCoordinates (otherwise, Pose3::FIRST_ORDER will be used) -#cmakedefine GTSAM_POSE3_EXPMAP +#cmakedefine GTSAM_POSE3_EXPMAP // Whether GTSAM is compiled to use Rot3::EXPMAP as the default coordinates mode for Rot3's retract and localCoordinates (otherwise, Pose3::CAYLEY will be used) #ifndef GTSAM_USE_QUATERNIONS - #cmakedefine GTSAM_ROT3_EXPMAP + #cmakedefine GTSAM_ROT3_EXPMAP #endif // Whether we are using TBB (if TBB was found and GTSAM_WITH_TBB is enabled in CMake) @@ -65,3 +65,6 @@ // Publish flag about Eigen typedef #cmakedefine GTSAM_USE_VECTOR3_POINTS + +// Support Metis-based nested dissection +#cmakedefine GTSAM_SUPPORT_NESTED_DISSECTION diff --git a/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp b/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp index 970a18b42..d6902bbef 100644 --- a/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp +++ b/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp @@ -421,16 +421,16 @@ TEST(ADT, conversion) ADT fDiscreteKey(X & Y, "0.2 0.5 0.3 0.6"); dot(fDiscreteKey, "conversion-f1"); - std::map ordering; - ordering[0] = 5; - ordering[1] = 2; + std::map keyMap; + keyMap[0] = 5; + keyMap[1] = 2; - AlgebraicDecisionTree fIndexKey(fDiscreteKey, ordering); + AlgebraicDecisionTree fIndexKey(fDiscreteKey, keyMap); // f1.print("f1"); // f2.print("f2"); dot(fIndexKey, "conversion-f2"); - Assignment x00, x01, x02, x10, x11, x12; + Assignment x00, x01, x02, x10, x11, x12; x00[5] = 0, x00[2] = 0; x01[5] = 0, x01[2] = 1; x10[5] = 1, x10[2] = 0; diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index 4e62ca7e9..ac4b68ccd 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/gtsam/geometry/Cal3_S2.h @@ -95,6 +95,11 @@ public: return fy_; } + /// aspect ratio + inline double aspectRatio() const { + return fx_/fy_; + } + /// skew inline double skew() const { return s_; diff --git a/gtsam/geometry/EssentialMatrix.cpp b/gtsam/geometry/EssentialMatrix.cpp index 699705fa5..02ede9228 100644 --- a/gtsam/geometry/EssentialMatrix.cpp +++ b/gtsam/geometry/EssentialMatrix.cpp @@ -12,6 +12,17 @@ using namespace std; namespace gtsam { +/* ************************************************************************* */ +EssentialMatrix EssentialMatrix::FromRotationAndDirection(const Rot3& aRb, const Unit3& aTb, + OptionalJacobian<5, 3> H1, + OptionalJacobian<5, 2> H2) { + if (H1) + *H1 << I_3x3, Matrix23::Zero(); + if (H2) + *H2 << Matrix32::Zero(), I_2x2; + return EssentialMatrix(aRb, aTb); +} + /* ************************************************************************* */ EssentialMatrix EssentialMatrix::FromPose3(const Pose3& aPb, OptionalJacobian<5, 6> H) { diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h index 697bd462d..9dec574eb 100644 --- a/gtsam/geometry/EssentialMatrix.h +++ b/gtsam/geometry/EssentialMatrix.h @@ -52,6 +52,11 @@ public: Base(aRb, aTb), E_(direction().skew() * rotation().matrix()) { } + /// Named constructor with derivatives + static EssentialMatrix FromRotationAndDirection(const Rot3& aRb, const Unit3& aTb, + OptionalJacobian<5, 3> H1 = boost::none, + OptionalJacobian<5, 2> H2 = boost::none); + /// Named constructor converting a Pose3 with scale to EssentialMatrix (no scale) static EssentialMatrix FromPose3(const Pose3& _1P2_, OptionalJacobian<5, 6> H = boost::none); diff --git a/gtsam/geometry/OrientedPlane3.cpp b/gtsam/geometry/OrientedPlane3.cpp index bb9925e23..aa023a09a 100644 --- a/gtsam/geometry/OrientedPlane3.cpp +++ b/gtsam/geometry/OrientedPlane3.cpp @@ -57,13 +57,31 @@ OrientedPlane3 OrientedPlane3::transform(const Pose3& xr, OptionalJacobian<3, 3> return OrientedPlane3(unit_vec(0), unit_vec(1), unit_vec(2), pred_d); } - /* ************************************************************************* */ Vector3 OrientedPlane3::error(const OrientedPlane3& plane) const { Vector2 n_error = -n_.localCoordinates(plane.n_); return Vector3(n_error(0), n_error(1), d_ - plane.d_); } +/* ************************************************************************* */ +Vector3 OrientedPlane3::errorVector(const OrientedPlane3& other, OptionalJacobian<3, 3> H1, + OptionalJacobian<3, 3> H2) const { + Matrix22 H_n_error_this, H_n_error_other; + Vector2 n_error = n_.errorVector(other.normal(), H1 ? &H_n_error_this : 0, + H2 ? &H_n_error_other : 0); + + double d_error = d_ - other.d_; + + if (H1) { + *H1 << H_n_error_this, Vector2::Zero(), 0, 0, 1; + } + if (H2) { + *H2 << H_n_error_other, Vector2::Zero(), 0, 0, -1; + } + + return Vector3(n_error(0), n_error(1), d_error); +} + /* ************************************************************************* */ OrientedPlane3 OrientedPlane3::retract(const Vector3& v) const { return OrientedPlane3(n_.retract(Vector2(v(0), v(1))), d_ + v(2)); diff --git a/gtsam/geometry/OrientedPlane3.h b/gtsam/geometry/OrientedPlane3.h index 5c9a5cdef..e425a4b81 100644 --- a/gtsam/geometry/OrientedPlane3.h +++ b/gtsam/geometry/OrientedPlane3.h @@ -114,6 +114,15 @@ public: */ Vector3 error(const OrientedPlane3& plane) const; + /** Computes the error between the two planes, with derivatives. + * This uses Unit3::errorVector, as opposed to the other .error() in this class, which uses + * Unit3::localCoordinates. This one has correct derivatives. + * NOTE(hayk): The derivatives are zero when normals are exactly orthogonal. + * @param the other plane + */ + Vector3 errorVector(const OrientedPlane3& other, OptionalJacobian<3, 3> H1 = boost::none, // + OptionalJacobian<3, 3> H2 = boost::none) const; + /// Dimensionality of tangent space = 3 DOF inline static size_t Dim() { return 3; diff --git a/gtsam/geometry/PinholePose.h b/gtsam/geometry/PinholePose.h index ac453e048..ac889c9d7 100644 --- a/gtsam/geometry/PinholePose.h +++ b/gtsam/geometry/PinholePose.h @@ -328,6 +328,11 @@ public: virtual ~PinholePose() { } + /// return shared pointer to calibration + const boost::shared_ptr& sharedCalibration() const { + return K_; + } + /// return calibration virtual const CALIBRATION& calibration() const { return *K_; diff --git a/gtsam/geometry/Point2.cpp b/gtsam/geometry/Point2.cpp index 4b2111efc..7066e527b 100644 --- a/gtsam/geometry/Point2.cpp +++ b/gtsam/geometry/Point2.cpp @@ -140,4 +140,10 @@ ostream &operator<<(ostream &os, const Point2& p) { return os; } +/* ************************************************************************* */ +ostream &operator<<(ostream &os, const gtsam::Point2Pair &p) { + os << p.first << " <-> " << p.second; + return os; +} + } // namespace gtsam diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index 56809ae53..3099a8bb3 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -50,7 +50,7 @@ public: /// @{ /// construct from 2D vector - Point2(const Vector2& v) { + explicit Point2(const Vector2& v) { x_ = v(0); y_ = v(1); } @@ -112,6 +112,11 @@ public: /// inverse inline Point2 operator- () const {return Point2(-x_,-y_);} + /// add vector on right + inline Point2 operator +(const Vector2& v) const { + return Point2(x_ + v[0], y_ + v[1]); + } + /// add inline Point2 operator + (const Point2& q) const {return Point2(x_+q.x_,y_+q.y_);} @@ -194,6 +199,10 @@ private: /// @} }; +// Convenience typedef +typedef std::pair Point2Pair; +std::ostream &operator<<(std::ostream &os, const gtsam::Point2Pair &p); + // For MATLAB wrapper typedef std::vector Point2Vector; diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp index 01b5ed132..df0f78283 100644 --- a/gtsam/geometry/Point3.cpp +++ b/gtsam/geometry/Point3.cpp @@ -73,7 +73,7 @@ Point3 Point3::add(const Point3 &q, OptionalJacobian<3,3> H1, Point3 Point3::sub(const Point3 &q, OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const { if (H1) *H1 = I_3x3; - if (H2) *H2 = I_3x3; + if (H2) *H2 = -I_3x3; return *this - q; } #endif diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 8b31316c7..fd254e51c 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -51,17 +51,12 @@ class GTSAM_EXPORT Point3 : public Vector3 { /// @name Standard Constructors /// @{ -#ifndef GTSAM_ALLOW_DEPRECATED_SINCE_V4 - /// Default constructor no longer initializes, just like Vector3 - Point3() {} +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 + // Deprecated default constructor initializes to zero, in contrast to new behavior below + Point3() { setZero(); } #endif - /// Construct from x, y, and z coordinates - Point3(double x, double y, double z): Vector3(x,y, z) {} - - /// Construct from other vector - template - inline Point3(const Eigen::MatrixBase& v): Vector3(v) {} + using Vector3::Vector3; /// @} /// @name Testable @@ -126,7 +121,6 @@ class GTSAM_EXPORT Point3 : public Vector3 { #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 /// @name Deprecated /// @{ - Point3() { setZero(); } // initializes to zero, in contrast to new behavior Point3 inverse() const { return -(*this);} Point3 compose(const Point3& q) const { return (*this)+q;} Point3 between(const Point3& q) const { return q-(*this);} diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index 89f6b3754..1aa8f060a 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -216,7 +216,7 @@ Point2 Pose2::transform_from(const Point2& point, OptionalJacobian<2, 2> Htranslation = Hpose.cols<2>(0); OptionalJacobian<2, 1> Hrotation = Hpose.cols<1>(2); const Point2 q = r_.rotate(point, Hrotation, Hpoint); - if (Htranslation) *Htranslation = Hpoint ? *Hpoint : r_.matrix(); + if (Htranslation) *Htranslation = (Hpoint ? *Hpoint : r_.matrix()); return q + t_; } diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index f1cb482bb..d170282fe 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -36,6 +36,14 @@ Pose3::Pose3(const Pose2& pose2) : Point3(pose2.x(), pose2.y(), 0)) { } +/* ************************************************************************* */ +Pose3 Pose3::Create(const Rot3& R, const Point3& t, OptionalJacobian<6, 3> H1, + OptionalJacobian<6, 3> H2) { + if (H1) *H1 << I_3x3, Z_3x3; + if (H2) *H2 << Z_3x3, R.transpose(); + return Pose3(R, t); +} + /* ************************************************************************* */ Pose3 Pose3::inverse() const { Rot3 Rt = R_.inverse(); @@ -193,8 +201,8 @@ Vector6 Pose3::ChartAtOrigin::Local(const Pose3& T, ChartJacobian H) { * The closed-form formula is similar to formula 102 in Barfoot14tro) */ static Matrix3 computeQforExpmapDerivative(const Vector6& xi) { - const Vector3 w = xi.head<3>(); - const Vector3 v = xi.tail<3>(); + const auto w = xi.head<3>(); + const auto v = xi.tail<3>(); const Matrix3 V = skewSymmetric(v); const Matrix3 W = skewSymmetric(w); @@ -260,9 +268,18 @@ const Point3& Pose3::translation(OptionalJacobian<3, 6> H) const { return t_; } +/* ************************************************************************* */ + +const Rot3& Pose3::rotation(OptionalJacobian<3, 6> H) const { + if (H) { + *H << I_3x3, Z_3x3; + } + return R_; +} + /* ************************************************************************* */ Matrix4 Pose3::matrix() const { - static const Matrix14 A14 = (Matrix14() << 0.0, 0.0, 0.0, 1.0).finished(); + static const auto A14 = Eigen::RowVector4d(0,0,0,1); Matrix4 mat; mat << R_.matrix(), t_, A14; return mat; @@ -275,6 +292,14 @@ Pose3 Pose3::transform_to(const Pose3& pose) const { return Pose3(cRv, t); } +/* ************************************************************************* */ +Pose3 Pose3::transform_pose_to(const Pose3& pose, OptionalJacobian<6, 6> H1, + OptionalJacobian<6, 6> H2) const { + if (H1) *H1 = -pose.inverse().AdjointMap() * AdjointMap(); + if (H2) *H2 = I_6x6; + return inverse() * pose; +} + /* ************************************************************************* */ Point3 Pose3::transform_from(const Point3& p, OptionalJacobian<3,6> Dpose, OptionalJacobian<3,3> Dpoint) const { @@ -355,41 +380,54 @@ Unit3 Pose3::bearing(const Point3& point, OptionalJacobian<2, 6> H1, } /* ************************************************************************* */ -boost::optional align(const vector& pairs) { - const size_t n = pairs.size(); +boost::optional Pose3::Align(const std::vector& abPointPairs) { + const size_t n = abPointPairs.size(); if (n < 3) - return boost::none; // we need at least three pairs + return boost::none; // we need at least three pairs // calculate centroids - Point3 cp(0,0,0), cq(0,0,0); - BOOST_FOREACH(const Point3Pair& pair, pairs) { - cp += pair.first; - cq += pair.second; + Point3 aCentroid(0,0,0), bCentroid(0,0,0); + for(const Point3Pair& abPair: abPointPairs) { + aCentroid += abPair.first; + bCentroid += abPair.second; } double f = 1.0 / n; - cp *= f; - cq *= f; + aCentroid *= f; + bCentroid *= f; // Add to form H matrix Matrix3 H = Z_3x3; - BOOST_FOREACH(const Point3Pair& pair, pairs) { - Point3 dp = pair.first - cp; - Point3 dq = pair.second - cq; - H += dp * dq.transpose(); + for(const Point3Pair& abPair: abPointPairs) { + Point3 da = abPair.first - aCentroid; + Point3 db = abPair.second - bCentroid; + H += db * da.transpose(); } -// Compute SVD - Matrix U, V; - Vector S; - svd(H, U, S, V); + // Compute SVD + Eigen::JacobiSVD svd(H, Eigen::ComputeThinU | Eigen::ComputeThinV); + Matrix U = svd.matrixU(); + Vector S = svd.singularValues(); + Matrix V = svd.matrixV(); + + // Check rank + if (S[1] < 1e-10) + return boost::none; // Recover transform with correction from Eggert97machinevisionandapplications Matrix3 UVtranspose = U * V.transpose(); Matrix3 detWeighting = I_3x3; detWeighting(2, 2) = UVtranspose.determinant(); - Rot3 R(Matrix3(V * detWeighting * U.transpose())); - Point3 t = Point3(cq) - R * Point3(cp); - return Pose3(R, t); + Rot3 aRb(Matrix(V * detWeighting * U.transpose())); + Point3 aTb = Point3(aCentroid) - aRb * Point3(bCentroid); + return Pose3(aRb, aTb); +} + +boost::optional align(const vector& baPointPairs) { + vector abPointPairs; + BOOST_FOREACH (const Point3Pair& baPair, baPointPairs) { + abPointPairs.push_back(make_pair(baPair.second, baPair.first)); + } + return Pose3::Align(abPointPairs); } /* ************************************************************************* */ diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index ba4b9737b..ee1206119 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -73,6 +73,19 @@ public: T(2, 2)), t_(T(0, 3), T(1, 3), T(2, 3)) { } + /// Named constructor with derivatives + static Pose3 Create(const Rot3& R, const Point3& t, + OptionalJacobian<6, 3> H1 = boost::none, + OptionalJacobian<6, 3> H2 = boost::none); + + /** + * Create Pose3 by aligning two point pairs + * A pose aTb is estimated between pairs (a_point, b_point) such that a_point = aTb * b_point + * Meant to replace the deprecated function 'align', which orders the pairs the opposite way. + * Note this allows for noise on the points but in that case the mapping will not be exact. + */ + static boost::optional Align(const std::vector& abPointPairs); + /// @} /// @name Testable /// @{ @@ -213,9 +226,7 @@ public: /// @{ /// get rotation - const Rot3& rotation() const { - return R_; - } + const Rot3& rotation(OptionalJacobian<3, 6> H = boost::none) const; /// get translation const Point3& translation(OptionalJacobian<3, 6> H = boost::none) const; @@ -238,9 +249,15 @@ public: /** convert to 4*4 matrix */ Matrix4 matrix() const; - /** receives a pose in world coordinates and transforms it to local coordinates */ + /** receives a pose in local coordinates and transforms it to world coordinates + * @deprecated: This is actually equivalent to transform_from, so it is WRONG! Use + * transform_pose_to instead. */ Pose3 transform_to(const Pose3& pose) const; + /** receives a pose in world coordinates and transforms it to local coordinates */ + Pose3 transform_pose_to(const Pose3& pose, OptionalJacobian<6, 6> H1 = boost::none, + OptionalJacobian<6, 6> H2 = boost::none) const; + /** * Calculate range to a landmark * @param point 3D location of landmark @@ -291,7 +308,7 @@ public: GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Pose3& p); -private: + private: /** Serialization function */ friend class boost::serialization::access; template @@ -317,11 +334,11 @@ inline Matrix wedge(const Vector& xi) { } /** - * Calculate pose between a vector of 3D point correspondences (p,q) - * where q = Pose3::transform_from(p) = t + R*p + * Calculate pose between a vector of 3D point correspondences (b_point, a_point) + * where a_point = Pose3::transform_from(b_point) = t + R*b_point + * @deprecated: use Pose3::Align with point pairs ordered the opposite way */ -typedef std::pair Point3Pair; -GTSAM_EXPORT boost::optional align(const std::vector& pairs); +GTSAM_EXPORT boost::optional align(const std::vector& baPointPairs); // For MATLAB wrapper typedef std::vector Pose3Vector; diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index 696c9df68..c2e711a17 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -42,6 +42,58 @@ Rot3 Rot3::Random(boost::mt19937& rng) { return AxisAngle(axis, angle); } + + +/* ************************************************************************* */ +Rot3 Rot3::AlignPair(const Unit3& axis, const Unit3& a_p, const Unit3& b_p) { + // if a_p is already aligned with b_p, return the identity rotation + if (std::abs(a_p.dot(b_p)) > 0.999999999) { + return Rot3(); + } + + // Check axis was not degenerate cross product + const Vector3 z = axis.unitVector(); + if (z.hasNaN()) + throw std::runtime_error("AlignSinglePair: axis has Nans"); + + // Now, calculate rotation that takes b_p to a_p + const Matrix3 P = I_3x3 - z * z.transpose(); // orthogonal projector + const Vector3 a_po = P * a_p.unitVector(); // point in a orthogonal to axis + const Vector3 b_po = P * b_p.unitVector(); // point in b orthogonal to axis + const Vector3 x = a_po.normalized(); // x-axis in axis-orthogonal plane, along a_p vector + const Vector3 y = z.cross(x); // y-axis in axis-orthogonal plane + const double u = x.dot(b_po); // x-coordinate for b_po + const double v = y.dot(b_po); // y-coordinate for b_po + double angle = std::atan2(v, u); + return Rot3::AxisAngle(z, -angle); +} + +/* ************************************************************************* */ +Rot3 Rot3::AlignTwoPairs(const Unit3& a_p, const Unit3& b_p, // + const Unit3& a_q, const Unit3& b_q) { + // there are three frames in play: + // a: the first frame in which p and q are measured + // b: the second frame in which p and q are measured + // i: intermediate, after aligning first pair + + // First, find rotation around that aligns a_p and b_p + Rot3 i_R_b = AlignPair(a_p.cross(b_p), a_p, b_p); + + // Rotate points in frame b to the intermediate frame, + // in which we expect the point p to be aligned now + Unit3 i_q = i_R_b * b_q; + assert(assert_equal(a_p, i_R_b * b_p, 1e-6)); + + // Now align second pair: we need to align i_q to a_q + Rot3 a_R_i = AlignPair(a_p, a_q, i_q); + assert(assert_equal(a_p, a_R_i * a_p, 1e-6)); + assert(assert_equal(a_q, a_R_i * i_q, 1e-6)); + + // The desired rotation is the product of both + Rot3 a_R_b = a_R_i * i_R_b; + return a_R_b; +} + /* ************************************************************************* */ bool Rot3::equals(const Rot3 & R, double tol) const { return equal_with_abs_tol(matrix(), R.matrix(), tol); @@ -172,10 +224,7 @@ ostream &operator<<(ostream &os, const Rot3& R) { /* ************************************************************************* */ Rot3 Rot3::slerp(double t, const Rot3& other) const { - // amazingly simple in GTSAM :-) - assert(t>=0 && t<=1); - Vector3 omega = Logmap(between(other)); - return compose(Expmap(t * omega)); + return interpolate(*this, other, t); } /* ************************************************************************* */ diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 057fdf558..79b13b93e 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -215,6 +215,13 @@ namespace gtsam { return Rodrigues(Vector3(wx, wy, wz)); } + /// Determine a rotation to bring two vectors into alignment, using the rotation axis provided + static Rot3 AlignPair(const Unit3& axis, const Unit3& a_p, const Unit3& b_p); + + /// Calculate rotation from two pairs of homogeneous points using two successive rotations + static Rot3 AlignTwoPairs(const Unit3& a_p, const Unit3& b_p, // + const Unit3& a_q, const Unit3& b_q); + /// @} /// @name Testable /// @{ @@ -456,9 +463,6 @@ namespace gtsam { #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 /// @name Deprecated /// @{ -#ifndef GTSAM_USE_VECTOR3_POINTS - static Rot3 rodriguez(const Vector3& axis, double angle) { return AxisAngle(axis, angle); } -#endif static Rot3 rodriguez(const Point3& axis, double angle) { return AxisAngle(axis, angle); } static Rot3 rodriguez(const Unit3& axis, double angle) { return AxisAngle(axis, angle); } static Rot3 rodriguez(const Vector3& w) { return Rodrigues(w); } diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index 03cde1a01..529c64973 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -36,9 +36,9 @@ Rot3::Rot3() : rot_(I_3x3) {} /* ************************************************************************* */ Rot3::Rot3(const Point3& col1, const Point3& col2, const Point3& col3) { - rot_.col(0) = (Vector3)col1; - rot_.col(1) = (Vector3)col2; - rot_.col(2) = (Vector3)col3; + rot_.col(0) = col1; + rot_.col(1) = col2; + rot_.col(2) = col3; } /* ************************************************************************* */ diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index 459f15561..bd111d9b1 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -29,11 +29,12 @@ namespace so3 { void ExpmapFunctor::init(bool nearZeroApprox) { nearZero = nearZeroApprox || (theta2 <= std::numeric_limits::epsilon()); - if (nearZero) return; - theta = std::sqrt(theta2); // rotation angle - sin_theta = std::sin(theta); - const double s2 = std::sin(theta / 2.0); - one_minus_cos = 2.0 * s2 * s2; // numerically better than [1 - cos(theta)] + if (!nearZero) { + theta = std::sqrt(theta2); // rotation angle + sin_theta = std::sin(theta); + const double s2 = std::sin(theta / 2.0); + one_minus_cos = 2.0 * s2 * s2; // numerically better than [1 - cos(theta)] + } } ExpmapFunctor::ExpmapFunctor(const Vector3& omega, bool nearZeroApprox) @@ -42,7 +43,6 @@ ExpmapFunctor::ExpmapFunctor(const Vector3& omega, bool nearZeroApprox) W << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0; init(nearZeroApprox); if (!nearZero) { - theta = std::sqrt(theta2); K = W / theta; KK = K * K; } @@ -55,7 +55,6 @@ ExpmapFunctor::ExpmapFunctor(const Vector3& axis, double angle, bool nearZeroApp W = K * angle; init(nearZeroApprox); if (!nearZero) { - theta = angle; KK = K * K; } } diff --git a/gtsam/geometry/StereoPoint2.h b/gtsam/geometry/StereoPoint2.h index 1b9559e67..961cc041b 100644 --- a/gtsam/geometry/StereoPoint2.h +++ b/gtsam/geometry/StereoPoint2.h @@ -50,7 +50,7 @@ public: } /// construct from 3D vector - StereoPoint2(const Vector3& v) : + explicit StereoPoint2(const Vector3& v) : uL_(v(0)), uR_(v(1)), v_(v(2)) {} /// @} @@ -80,6 +80,11 @@ public: return StereoPoint2(-uL_, -uR_, -v_); } + /// add vector on right + inline StereoPoint2 operator +(const Vector3& v) const { + return StereoPoint2(uL_ + v[0], uR_ + v[1], v_ + v[2]); + } + /// add inline StereoPoint2 operator +(const StereoPoint2& b) const { return StereoPoint2(uL_ + b.uL_, uR_ + b.uR_, v_ + b.v_); diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index a64f99eb1..dacb5c3fd 100644 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -60,7 +60,7 @@ Unit3 Unit3::Random(boost::mt19937 & rng) { // around 1.46, instead of drawing directly using boost::uniform_on_sphere(rng). boost::variate_generator > generator( rng, randomDirection); - vector d = generator(); + const vector d = generator(); return Unit3(d[0], d[1], d[2]); } @@ -100,16 +100,16 @@ const Matrix32& Unit3::basis(OptionalJacobian<6, 2> H) const { // Choose the direction of the first basis vector b1 in the tangent plane by crossing n with // the chosen axis. Matrix33 H_B1_n; - Point3 B1 = gtsam::cross(n, axis, H ? &H_B1_n : 0); + Point3 B1 = gtsam::cross(n, axis, H ? &H_B1_n : nullptr); // Normalize result to get a unit vector: b1 = B1 / |B1|. Matrix33 H_b1_B1; - Point3 b1 = normalize(B1, H ? &H_b1_B1 : 0); + Point3 b1 = normalize(B1, H ? &H_b1_B1 : nullptr); // Get the second basis vector b2, which is orthogonal to n and b1, by crossing them. // No need to normalize this, p and b1 are orthogonal unit vectors. Matrix33 H_b2_n, H_b2_b1; - Point3 b2 = gtsam::cross(n, b1, H ? &H_b2_n : 0, H ? &H_b2_b1 : 0); + Point3 b2 = gtsam::cross(n, b1, H ? &H_b2_n : nullptr, H ? &H_b2_b1 : nullptr); // Create the basis by stacking b1 and b2. B_.reset(Matrix32()); @@ -118,8 +118,8 @@ const Matrix32& Unit3::basis(OptionalJacobian<6, 2> H) const { if (H) { // Chain rule tomfoolery to compute the derivative. const Matrix32& H_n_p = *B_; - Matrix32 H_b1_p = H_b1_B1 * H_B1_n * H_n_p; - Matrix32 H_b2_p = H_b2_n * H_n_p + H_b2_b1 * H_b1_p; + const Matrix32 H_b1_p = H_b1_B1 * H_B1_n * H_n_p; + const Matrix32 H_b2_p = H_b2_n * H_n_p + H_b2_b1 * H_b1_p; // Cache the derivative and fill the result. H_B_.reset(Matrix62()); @@ -164,14 +164,14 @@ Matrix3 Unit3::skew() const { double Unit3::dot(const Unit3& q, OptionalJacobian<1,2> H_p, OptionalJacobian<1,2> H_q) const { // Get the unit vectors of each, and the derivative. Matrix32 H_pn_p; - Point3 pn = point3(H_p ? &H_pn_p : 0); + Point3 pn = point3(H_p ? &H_pn_p : nullptr); Matrix32 H_qn_q; - Point3 qn = q.point3(H_q ? &H_qn_q : 0); + const Point3 qn = q.point3(H_q ? &H_qn_q : nullptr); // Compute the dot product of the Point3s. Matrix13 H_dot_pn, H_dot_qn; - double d = gtsam::dot(pn, qn, H_p ? &H_dot_pn : 0, H_q ? &H_dot_qn : 0); + double d = gtsam::dot(pn, qn, H_p ? &H_dot_pn : nullptr, H_q ? &H_dot_qn : nullptr); if (H_p) { (*H_p) << H_dot_pn * H_pn_p; @@ -187,10 +187,9 @@ double Unit3::dot(const Unit3& q, OptionalJacobian<1,2> H_p, OptionalJacobian<1, /* ************************************************************************* */ Vector2 Unit3::error(const Unit3& q, OptionalJacobian<2,2> H_q) const { // 2D error is equal to B'*q, as B is 3x2 matrix and q is 3x1 - Matrix23 Bt = basis().transpose(); - Vector2 xi = Bt * q.p_; + const Vector2 xi = basis().transpose() * q.p_; if (H_q) { - *H_q = Bt * q.basis(); + *H_q = basis().transpose() * q.basis(); } return xi; } @@ -199,11 +198,11 @@ Vector2 Unit3::error(const Unit3& q, OptionalJacobian<2,2> H_q) const { Vector2 Unit3::errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p, OptionalJacobian<2, 2> H_q) const { // Get the point3 of this, and the derivative. Matrix32 H_qn_q; - Point3 qn = q.point3(H_q ? &H_qn_q : 0); + const Point3 qn = q.point3(H_q ? &H_qn_q : nullptr); // 2D error here is projecting q into the tangent plane of this (p). Matrix62 H_B_p; - Matrix23 Bt = basis(H_p ? &H_B_p : 0).transpose(); + Matrix23 Bt = basis(H_p ? &H_B_p : nullptr).transpose(); Vector2 xi = Bt * qn; if (H_p) { @@ -212,18 +211,18 @@ Vector2 Unit3::errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p, OptionalJ const Matrix32& H_b2_p = H_B_p.block<3, 2>(3, 0); // Derivatives of the two entries of xi wrt the basis vectors. - Matrix13 H_xi1_b1 = qn.transpose(); - Matrix13 H_xi2_b2 = qn.transpose(); + const Matrix13 H_xi1_b1 = qn.transpose(); + const Matrix13 H_xi2_b2 = qn.transpose(); // Assemble dxi/dp = dxi/dB * dB/dp. - Matrix12 H_xi1_p = H_xi1_b1 * H_b1_p; - Matrix12 H_xi2_p = H_xi2_b2 * H_b2_p; + const Matrix12 H_xi1_p = H_xi1_b1 * H_b1_p; + const Matrix12 H_xi2_p = H_xi2_b2 * H_b2_p; *H_p << H_xi1_p, H_xi2_p; } if (H_q) { // dxi/dq is given by dxi/dqu * dqu/dq, where qu is the unit vector of q. - Matrix23 H_xi_qu = Bt; + const Matrix23 H_xi_qu = Bt; *H_q = H_xi_qu * H_qn_q; } @@ -232,26 +231,27 @@ Vector2 Unit3::errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p, OptionalJ /* ************************************************************************* */ double Unit3::distance(const Unit3& q, OptionalJacobian<1,2> H) const { - Matrix2 H_; - Vector2 xi = error(q, H_); - double theta = xi.norm(); + Matrix2 H_xi_q; + const Vector2 xi = error(q, H ? &H_xi_q : nullptr); + const double theta = xi.norm(); if (H) - *H = (xi.transpose() / theta) * H_; + *H = (xi.transpose() / theta) * H_xi_q; return theta; } /* ************************************************************************* */ Unit3 Unit3::retract(const Vector2& v) const { // Compute the 3D xi_hat vector - Vector3 xi_hat = basis() * v; - double theta = xi_hat.norm(); + const Vector3 xi_hat = basis() * v; + const double theta = xi_hat.norm(); // Treat case of very small v differently if (theta < std::numeric_limits::epsilon()) { return Unit3(Vector3(std::cos(theta) * p_ + xi_hat)); } - Vector3 exp_p_xi_hat = std::cos(theta) * p_ + xi_hat * (sin(theta) / theta); + const Vector3 exp_p_xi_hat = + std::cos(theta) * p_ + xi_hat * (sin(theta) / theta); return Unit3(exp_p_xi_hat); } diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h index 1ce790a1b..9f21bb82a 100644 --- a/gtsam/geometry/Unit3.h +++ b/gtsam/geometry/Unit3.h @@ -65,14 +65,7 @@ public: p_(1.0, 0.0, 0.0) { } -#ifndef GTSAM_USE_VECTOR3_POINTS /// Construct from point - explicit Unit3(const Point3& p) : - p_(p.vector().normalized()) { - } -#endif - - /// Construct from a vector3 explicit Unit3(const Vector3& p) : p_(p.normalized()) { } diff --git a/gtsam/geometry/tests/testEssentialMatrix.cpp b/gtsam/geometry/tests/testEssentialMatrix.cpp index bfff0a182..79759678d 100644 --- a/gtsam/geometry/tests/testEssentialMatrix.cpp +++ b/gtsam/geometry/tests/testEssentialMatrix.cpp @@ -20,20 +20,41 @@ GTSAM_CONCEPT_MANIFOLD_INST(EssentialMatrix) //************************************************************************* // Create two cameras and corresponding essential matrix E -Rot3 c1Rc2 = Rot3::Yaw(M_PI_2); -Point3 c1Tc2(0.1, 0, 0); -EssentialMatrix trueE(c1Rc2, Unit3(c1Tc2)); +Rot3 trueRotation = Rot3::Yaw(M_PI_2); +Point3 trueTranslation(0.1, 0, 0); +Unit3 trueDirection(trueTranslation); +EssentialMatrix trueE(trueRotation, trueDirection); //************************************************************************* TEST (EssentialMatrix, equality) { - EssentialMatrix actual(c1Rc2, Unit3(c1Tc2)), expected(c1Rc2, Unit3(c1Tc2)); + EssentialMatrix actual(trueRotation, trueDirection), expected(trueRotation, trueDirection); EXPECT(assert_equal(expected, actual)); } +//************************************************************************* +TEST(EssentialMatrix, FromRotationAndDirection) { + Matrix actualH1, actualH2; + EXPECT(assert_equal( + trueE, EssentialMatrix::FromRotationAndDirection(trueRotation, trueDirection, actualH1, actualH2), + 1e-8)); + + Matrix expectedH1 = numericalDerivative11( + boost::bind(EssentialMatrix::FromRotationAndDirection, _1, trueDirection, boost::none, + boost::none), + trueRotation); + EXPECT(assert_equal(expectedH1, actualH1, 1e-7)); + + Matrix expectedH2 = numericalDerivative11( + boost::bind(EssentialMatrix::FromRotationAndDirection, trueRotation, _1, boost::none, + boost::none), + trueDirection); + EXPECT(assert_equal(expectedH2, actualH2, 1e-7)); +} + //************************************************************************* TEST (EssentialMatrix, FromPose3) { - EssentialMatrix expected(c1Rc2, Unit3(c1Tc2)); - Pose3 pose(c1Rc2, c1Tc2); + EssentialMatrix expected(trueRotation, trueDirection); + Pose3 pose(trueRotation, trueTranslation); EssentialMatrix actual = EssentialMatrix::FromPose3(pose); EXPECT(assert_equal(expected, actual)); } @@ -50,7 +71,7 @@ TEST(EssentialMatrix, localCoordinates0) { TEST (EssentialMatrix, localCoordinates) { // Pose between two cameras - Pose3 pose(c1Rc2, c1Tc2); + Pose3 pose(trueRotation, trueTranslation); EssentialMatrix hx = EssentialMatrix::FromPose3(pose); Vector actual = hx.localCoordinates(EssentialMatrix::FromPose3(pose)); EXPECT(assert_equal(zero(5), actual, 1e-8)); @@ -70,15 +91,15 @@ TEST (EssentialMatrix, retract0) { //************************************************************************* TEST (EssentialMatrix, retract1) { - EssentialMatrix expected(c1Rc2.retract(Vector3(0.1, 0, 0)), Unit3(c1Tc2)); + EssentialMatrix expected(trueRotation.retract(Vector3(0.1, 0, 0)), trueDirection); EssentialMatrix actual = trueE.retract((Vector(5) << 0.1, 0, 0, 0, 0).finished()); EXPECT(assert_equal(expected, actual)); } //************************************************************************* TEST (EssentialMatrix, retract2) { - EssentialMatrix expected(c1Rc2, - Unit3(c1Tc2).retract(Vector2(0.1, 0))); + EssentialMatrix expected(trueRotation, + trueDirection.retract(Vector2(0.1, 0))); EssentialMatrix actual = trueE.retract((Vector(5) << 0, 0, 0, 0.1, 0).finished()); EXPECT(assert_equal(expected, actual)); } @@ -124,8 +145,8 @@ TEST (EssentialMatrix, rotate) { Rot3 bRc(bX, bZ, -bY), cRb = bRc.inverse(); // Let's compute the ground truth E in body frame: - Rot3 b1Rb2 = bRc * c1Rc2 * cRb; - Point3 b1Tb2 = bRc * c1Tc2; + Rot3 b1Rb2 = bRc * trueRotation * cRb; + Point3 b1Tb2 = bRc * trueTranslation; EssentialMatrix bodyE(b1Rb2, Unit3(b1Tb2)); EXPECT(assert_equal(bodyE, bRc * trueE, 1e-8)); EXPECT(assert_equal(bodyE, trueE.rotate(bRc), 1e-8)); @@ -149,7 +170,7 @@ TEST (EssentialMatrix, rotate) { //************************************************************************* TEST (EssentialMatrix, FromPose3_a) { Matrix actualH; - Pose3 pose(c1Rc2, c1Tc2); // Pose between two cameras + Pose3 pose(trueRotation, trueTranslation); // Pose between two cameras EXPECT(assert_equal(trueE, EssentialMatrix::FromPose3(pose, actualH), 1e-8)); Matrix expectedH = numericalDerivative11( boost::bind(EssentialMatrix::FromPose3, _1, boost::none), pose); @@ -171,7 +192,7 @@ TEST (EssentialMatrix, FromPose3_b) { //************************************************************************* TEST (EssentialMatrix, streaming) { - EssentialMatrix expected(c1Rc2, Unit3(c1Tc2)), actual; + EssentialMatrix expected(trueRotation, trueDirection), actual; stringstream ss; ss << expected; ss >> actual; diff --git a/gtsam/geometry/tests/testOrientedPlane3.cpp b/gtsam/geometry/tests/testOrientedPlane3.cpp index 180abb0d6..d3a107570 100644 --- a/gtsam/geometry/tests/testOrientedPlane3.cpp +++ b/gtsam/geometry/tests/testOrientedPlane3.cpp @@ -139,6 +139,30 @@ TEST(OrientedPlane3, localCoordinates_retract) { } } +//******************************************************************************* +TEST (OrientedPlane3, error2) { + OrientedPlane3 plane1(-1, 0.1, 0.2, 5); + OrientedPlane3 plane2(-1.1, 0.2, 0.3, 5.4); + + // Hard-coded regression values, to ensure the result doesn't change. + EXPECT(assert_equal(zero(3), plane1.errorVector(plane1), 1e-8)); + EXPECT(assert_equal(Vector3(-0.0677674148, -0.0760543588, -0.4), plane1.errorVector(plane2), 1e-5)); + + // Test the jacobians of transform + Matrix33 actualH1, expectedH1, actualH2, expectedH2; + + Vector3 actual = plane1.errorVector(plane2, actualH1, actualH2); + EXPECT(assert_equal(plane1.normal().errorVector(plane2.normal()), Vector2(actual[0], actual[1]))); + EXPECT(assert_equal(plane1.distance() - plane2.distance(), actual[2])); + + boost::function f = // + boost::bind(&OrientedPlane3::errorVector, _1, _2, boost::none, boost::none); + expectedH1 = numericalDerivative21(f, plane1, plane2); + expectedH2 = numericalDerivative22(f, plane1, plane2); + EXPECT(assert_equal(expectedH1, actualH1, 1e-9)); + EXPECT(assert_equal(expectedH2, actualH2, 1e-9)); +} + /* ************************************************************************* */ int main() { srand(time(NULL)); diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp index a4e2b139e..e457da887 100644 --- a/gtsam/geometry/tests/testPoint3.cpp +++ b/gtsam/geometry/tests/testPoint3.cpp @@ -81,6 +81,71 @@ TEST( Point3, dot) { Point3 origin(0,0,0), ones(1, 1, 1); CHECK(origin.dot(Point3(1, 1, 0)) == 0); CHECK(ones.dot(Point3(1, 1, 0)) == 2); + + Point3 p(1, 0.2, 0.3); + Point3 q = p + Point3(0.5, 0.2, -3.0); + Point3 r = p + Point3(0.8, 0, 0); + Point3 t = p + Point3(0, 0.3, -0.4); + EXPECT(assert_equal(1.130000, p.dot(p), 1e-8)); + EXPECT(assert_equal(0.770000, p.dot(q), 1e-5)); + EXPECT(assert_equal(1.930000, p.dot(r), 1e-5)); + EXPECT(assert_equal(1.070000, p.dot(t), 1e-5)); + + // Use numerical derivatives to calculate the expected Jacobians + Matrix H1, H2; + boost::function f = boost::bind(&Point3::dot, _1, _2, // + boost::none, boost::none); + { + p.dot(q, H1, H2); + EXPECT(assert_equal(numericalDerivative21(f, p, q), H1, 1e-9)); + EXPECT(assert_equal(numericalDerivative22(f, p, q), H2, 1e-9)); + } + { + p.dot(r, H1, H2); + EXPECT(assert_equal(numericalDerivative21(f, p, r), H1, 1e-9)); + EXPECT(assert_equal(numericalDerivative22(f, p, r), H2, 1e-9)); + } + { + p.dot(t, H1, H2); + EXPECT(assert_equal(numericalDerivative21(f, p, t), H1, 1e-9)); + EXPECT(assert_equal(numericalDerivative22(f, p, t), H2, 1e-9)); + } +} + +/* ************************************************************************* */ +TEST(Point3, cross) { + Matrix aH1, aH2; + boost::function f = + boost::bind(>sam::cross, _1, _2, boost::none, boost::none); + const Point3 omega(0, 1, 0), theta(4, 6, 8); + cross(omega, theta, aH1, aH2); + EXPECT(assert_equal(numericalDerivative21(f, omega, theta), aH1)); + EXPECT(assert_equal(numericalDerivative22(f, omega, theta), aH2)); +} + +/* ************************************************************************* */ +TEST( Point3, cross2) { + Point3 p(1, 0.2, 0.3); + Point3 q = p + Point3(0.5, 0.2, -3.0); + Point3 r = p + Point3(0.8, 0, 0); + EXPECT(assert_equal(Point3(0, 0, 0), p.cross(p), 1e-8)); + EXPECT(assert_equal(Point3(-0.66, 3.15, 0.1), p.cross(q), 1e-5)); + EXPECT(assert_equal(Point3(0, 0.24, -0.16), p.cross(r), 1e-5)); + + // Use numerical derivatives to calculate the expected Jacobians + Matrix H1, H2; + boost::function f = boost::bind(&Point3::cross, _1, _2, // + boost::none, boost::none); + { + p.cross(q, H1, H2); + EXPECT(assert_equal(numericalDerivative21(f, p, q), H1, 1e-9)); + EXPECT(assert_equal(numericalDerivative22(f, p, q), H2, 1e-9)); + } + { + p.cross(r, H1, H2); + EXPECT(assert_equal(numericalDerivative21(f, p, r), H1, 1e-9)); + EXPECT(assert_equal(numericalDerivative22(f, p, r), H2, 1e-9)); + } } /* ************************************************************************* */ @@ -135,17 +200,6 @@ TEST (Point3, distance) { EXPECT(assert_equal(numH2, H2, 1e-8)); } -/* ************************************************************************* */ -TEST(Point3, cross) { - Matrix aH1, aH2; - boost::function f = - boost::bind(>sam::cross, _1, _2, boost::none, boost::none); - const Point3 omega(0, 1, 0), theta(4, 6, 8); - cross(omega, theta, aH1, aH2); - EXPECT(assert_equal(numericalDerivative21(f, omega, theta), aH1)); - EXPECT(assert_equal(numericalDerivative22(f, omega, theta), aH2)); -} - /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index af8e7c6a0..1aba34bd6 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -31,12 +31,13 @@ using namespace gtsam; GTSAM_CONCEPT_TESTABLE_INST(Pose3) GTSAM_CONCEPT_LIE_INST(Pose3) -static Point3 P(0.2,0.7,-2); -static Rot3 R = Rot3::Rodrigues(0.3,0,0); -static Pose3 T(R,Point3(3.5,-8.2,4.2)); -static Pose3 T2(Rot3::Rodrigues(0.3,0.2,0.1),Point3(3.5,-8.2,4.2)); -static Pose3 T3(Rot3::Rodrigues(-90, 0, 0), Point3(1, 2, 3)); -const double tol=1e-5; +static const Point3 P(0.2,0.7,-2); +static const Rot3 R = Rot3::Rodrigues(0.3,0,0); +static const Point3 P2(3.5,-8.2,4.2); +static const Pose3 T(R,P2); +static const Pose3 T2(Rot3::Rodrigues(0.3,0.2,0.1),P2); +static const Pose3 T3(Rot3::Rodrigues(-90, 0, 0), Point3(1, 2, 3)); +static const double tol=1e-5; /* ************************************************************************* */ TEST( Pose3, equals) @@ -197,6 +198,17 @@ TEST(Pose3, translation) { EXPECT(assert_equal(numericalH, actualH, 1e-6)); } +/* ************************************************************************* */ +// Check rotation and its pushforward +TEST(Pose3, rotation) { + Matrix actualH; + EXPECT(assert_equal(R, T.rotation(actualH), 1e-8)); + + Matrix numericalH = numericalDerivative11( + boost::bind(&Pose3::rotation, _1, boost::none), T); + EXPECT(assert_equal(numericalH, actualH, 1e-6)); +} + /* ************************************************************************* */ TEST(Pose3, Adjoint_compose_full) { @@ -403,6 +415,41 @@ TEST( Pose3, transform_to) EXPECT(assert_equal(expected, actual, 0.001)); } +Pose3 transform_pose_to_(const Pose3& pose, const Pose3& pose2) { return pose.transform_pose_to(pose2); } + +/* ************************************************************************* */ +TEST( Pose3, transform_pose_to) +{ + Pose3 origin = T.transform_pose_to(T); + EXPECT(assert_equal(Pose3{}, origin)); +} + +/* ************************************************************************* */ +TEST( Pose3, transform_pose_to_with_derivatives) +{ + Matrix actH1, actH2; + Pose3 res = T.transform_pose_to(T2,actH1,actH2); + EXPECT(assert_equal(res, T.inverse().compose(T2))); + + Matrix expH1 = numericalDerivative21(transform_pose_to_, T, T2), + expH2 = numericalDerivative22(transform_pose_to_, T, T2); + EXPECT(assert_equal(expH1, actH1, 1e-8)); + EXPECT(assert_equal(expH2, actH2, 1e-8)); +} + +/* ************************************************************************* */ +TEST( Pose3, transform_pose_to_with_derivatives2) +{ + Matrix actH1, actH2; + Pose3 res = T.transform_pose_to(T3,actH1,actH2); + EXPECT(assert_equal(res, T.inverse().compose(T3))); + + Matrix expH1 = numericalDerivative21(transform_pose_to_, T, T3), + expH2 = numericalDerivative22(transform_pose_to_, T, T3); + EXPECT(assert_equal(expH1, actH1, 1e-8)); + EXPECT(assert_equal(expH2, actH2, 1e-8)); +} + /* ************************************************************************* */ TEST( Pose3, transform_from) { @@ -476,7 +523,7 @@ TEST(Pose3, Retract_LocalCoordinates) { Vector6 d; d << 1,2,3,4,5,6; d/=10; - R = Rot3::Retract(d.head<3>()); + const Rot3 R = Rot3::Retract(d.head<3>()); Pose3 t = Pose3::Retract(d); EXPECT(assert_equal(d, Pose3::LocalCoordinates(t))); } @@ -506,6 +553,8 @@ TEST(Pose3, retract_localCoordinates2) EXPECT(assert_equal(t2, t1.retract(d12))); Vector d21 = t2.localCoordinates(t1); EXPECT(assert_equal(t1, t2.retract(d21))); + // TODO(hayk): This currently fails! + // EXPECT(assert_equal(d12, -d21)); } /* ************************************************************************* */ TEST(Pose3, manifold_expmap) @@ -637,16 +686,26 @@ TEST( Pose3, range_pose ) Unit3 bearing_proxy(const Pose3& pose, const Point3& point) { return pose.bearing(point); } -TEST( Pose3, bearing ) -{ +TEST(Pose3, Bearing) { Matrix expectedH1, actualH1, expectedH2, actualH2; - EXPECT(assert_equal(Unit3(1,0,0),x1.bearing(l1, actualH1, actualH2),1e-9)); + EXPECT(assert_equal(Unit3(1, 0, 0), x1.bearing(l1, actualH1, actualH2), 1e-9)); // Check numerical derivatives expectedH1 = numericalDerivative21(bearing_proxy, x1, l1); expectedH2 = numericalDerivative22(bearing_proxy, x1, l1); - EXPECT(assert_equal(expectedH1,actualH1)); - EXPECT(assert_equal(expectedH2,actualH2)); + EXPECT(assert_equal(expectedH1, actualH1)); + EXPECT(assert_equal(expectedH2, actualH2)); +} + +TEST(Pose3, Bearing2) { + Matrix expectedH1, actualH1, expectedH2, actualH2; + EXPECT(assert_equal(Unit3(0,0.6,-0.8), x2.bearing(l4, actualH1, actualH2), 1e-9)); + + // Check numerical derivatives + expectedH1 = numericalDerivative21(bearing_proxy, x2, l4); + expectedH2 = numericalDerivative22(bearing_proxy, x2, l4); + EXPECT(assert_equal(expectedH1, actualH1)); + EXPECT(assert_equal(expectedH2, actualH2)); } /* ************************************************************************* */ @@ -671,21 +730,21 @@ TEST( Pose3, adjointMap) { } /* ************************************************************************* */ -TEST(Pose3, align_1) { +TEST(Pose3, Align1) { Pose3 expected(Rot3(), Point3(10,10,0)); vector correspondences; - Point3Pair pq1(make_pair(Point3(0,0,0), Point3(10,10,0))); - Point3Pair pq2(make_pair(Point3(20,10,0), Point3(30,20,0))); - Point3Pair pq3(make_pair(Point3(10,20,0), Point3(20,30,0))); - correspondences += pq1, pq2, pq3; + Point3Pair ab1(make_pair(Point3(10,10,0), Point3(0,0,0))); + Point3Pair ab2(make_pair(Point3(30,20,0), Point3(20,10,0))); + Point3Pair ab3(make_pair(Point3(20,30,0), Point3(10,20,0))); + correspondences += ab1, ab2, ab3; - boost::optional actual = align(correspondences); + boost::optional actual = Pose3::Align(correspondences); EXPECT(assert_equal(expected, *actual)); } /* ************************************************************************* */ -TEST(Pose3, align_2) { +TEST(Pose3, Align2) { Point3 t(20,10,5); Rot3 R = Rot3::RzRyRx(0.3, 0.2, 0.1); Pose3 expected(R, t); @@ -695,12 +754,12 @@ TEST(Pose3, align_2) { Point3 q1 = expected.transform_from(p1), q2 = expected.transform_from(p2), q3 = expected.transform_from(p3); - Point3Pair pq1(make_pair(p1, q1)); - Point3Pair pq2(make_pair(p2, q2)); - Point3Pair pq3(make_pair(p3, q3)); - correspondences += pq1, pq2, pq3; + Point3Pair ab1(make_pair(q1, p1)); + Point3Pair ab2(make_pair(q2, p2)); + Point3Pair ab3(make_pair(q3, p3)); + correspondences += ab1, ab2, ab3; - boost::optional actual = align(correspondences); + boost::optional actual = Pose3::Align(correspondences); EXPECT(assert_equal(expected, *actual, 1e-5)); } @@ -843,6 +902,22 @@ TEST(Pose3 , ChartDerivatives) { } } +/* ************************************************************************* */ +TEST(Pose3, interpolate) { + EXPECT(assert_equal(T2, interpolate(T2,T3, 0.0))); + EXPECT(assert_equal(T3, interpolate(T2,T3, 1.0))); +} + +/* ************************************************************************* */ +TEST(Pose3, Create) { + Matrix63 actualH1, actualH2; + Pose3 actual = Pose3::Create(R, P2, actualH1, actualH2); + EXPECT(assert_equal(T, actual)); + boost::function create = boost::bind(Pose3::Create,_1,_2,boost::none,boost::none); + EXPECT(assert_equal(numericalDerivative21(create, R, P2), actualH1, 1e-9)); + EXPECT(assert_equal(numericalDerivative22(create, R, P2), actualH2, 1e-9)); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index f96198b94..5364d72f6 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -117,11 +117,13 @@ TEST( Rot3, Rodrigues2) { Vector axis = Vector3(0., 1., 0.); // rotation around Y double angle = 3.14 / 4.0; - Rot3 actual = Rot3::AxisAngle(axis, angle); Rot3 expected(0.707388, 0, 0.706825, 0, 1, 0, -0.706825, 0, 0.707388); + Rot3 actual = Rot3::AxisAngle(axis, angle); CHECK(assert_equal(expected,actual,1e-5)); + Rot3 actual2 = Rot3::Rodrigues(angle*axis); + CHECK(assert_equal(expected,actual2,1e-5)); } /* ************************************************************************* */ @@ -242,6 +244,7 @@ TEST(Rot3, retract_localCoordinates2) EXPECT(assert_equal(t2, t1.retract(d12))); Vector d21 = t2.localCoordinates(t1); EXPECT(assert_equal(t1, t2.retract(d21))); + EXPECT(assert_equal(d12, -d21)); } /* ************************************************************************* */ TEST(Rot3, manifold_expmap) diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp index 07789acaa..396c8b0f3 100644 --- a/gtsam/geometry/tests/testUnit3.cpp +++ b/gtsam/geometry/tests/testUnit3.cpp @@ -303,7 +303,7 @@ TEST(Unit3, localCoordinates) { } //******************************************************************************* -// Wrapper to make basis return a vector6 so we can test numerical derivatives. +// Wrapper to make basis return a Vector6 so we can test numerical derivatives. Vector6 BasisTest(const Unit3& p, OptionalJacobian<6, 2> H) { Matrix32 B = p.basis(H); Vector6 B_vec; diff --git a/gtsam/geometry/triangulation.cpp b/gtsam/geometry/triangulation.cpp index c92aa8237..7ec9edbb3 100644 --- a/gtsam/geometry/triangulation.cpp +++ b/gtsam/geometry/triangulation.cpp @@ -56,11 +56,10 @@ Vector4 triangulateHomogeneousDLT( Point3 triangulateDLT(const std::vector& projection_matrices, const std::vector& measurements, double rank_tol) { - Vector4 v = triangulateHomogeneousDLT(projection_matrices, measurements, - rank_tol); + Vector4 v = triangulateHomogeneousDLT(projection_matrices, measurements, rank_tol); // Create 3D point from homogeneous coordinates - return Point3(sub((v / v(3)), 0, 3)); + return Point3(v.head<3>() / v[3]); } /// @@ -91,5 +90,4 @@ Point3 optimize(const NonlinearFactorGraph& graph, const Values& values, return result.at(landmarkKey); } -} // \namespace gtsam - +} // \namespace gtsam diff --git a/gtsam/inference/EliminateableFactorGraph-inst.h b/gtsam/inference/EliminateableFactorGraph-inst.h index 98a3545f6..43bcffb09 100644 --- a/gtsam/inference/EliminateableFactorGraph-inst.h +++ b/gtsam/inference/EliminateableFactorGraph-inst.h @@ -48,16 +48,20 @@ namespace gtsam { // If no VariableIndex provided, compute one and call this function again IMPORTANT: we check // for no variable index first so that it's always computed if we need to call COLAMD because // no Ordering is provided. - return eliminateSequential(ordering, function, VariableIndex(asDerived()), orderingType); + VariableIndex computedVariableIndex(asDerived()); + return eliminateSequential(ordering, function, computedVariableIndex, orderingType); } else /*if(!ordering)*/ { // If no Ordering provided, compute one and call this function again. We are guaranteed to // have a VariableIndex already here because we computed one if needed in the previous 'else' // block. - if (orderingType == Ordering::METIS) - return eliminateSequential(Ordering::Metis(asDerived()), function, variableIndex, orderingType); - else - return eliminateSequential(Ordering::Colamd(*variableIndex), function, variableIndex, orderingType); + if (orderingType == Ordering::METIS) { + Ordering computedOrdering = Ordering::Metis(asDerived()); + return eliminateSequential(computedOrdering, function, variableIndex, orderingType); + } else { + Ordering computedOrdering = Ordering::Colamd(*variableIndex); + return eliminateSequential(computedOrdering, function, variableIndex, orderingType); + } } } @@ -86,16 +90,20 @@ namespace gtsam { // If no VariableIndex provided, compute one and call this function again IMPORTANT: we check // for no variable index first so that it's always computed if we need to call COLAMD because // no Ordering is provided. - return eliminateMultifrontal(ordering, function, VariableIndex(asDerived()), orderingType); + VariableIndex computedVariableIndex(asDerived()); + return eliminateMultifrontal(ordering, function, computedVariableIndex, orderingType); } else /*if(!ordering)*/ { // If no Ordering provided, compute one and call this function again. We are guaranteed to // have a VariableIndex already here because we computed one if needed in the previous 'else' // block. - if (orderingType == Ordering::METIS) - return eliminateMultifrontal(Ordering::Metis(asDerived()), function, variableIndex, orderingType); - else - return eliminateMultifrontal(Ordering::Colamd(*variableIndex), function, variableIndex, orderingType); + if (orderingType == Ordering::METIS) { + Ordering computedOrdering = Ordering::Metis(asDerived()); + return eliminateMultifrontal(computedOrdering, function, variableIndex, orderingType); + } else { + Ordering computedOrdering = Ordering::Colamd(*variableIndex); + return eliminateMultifrontal(computedOrdering, function, variableIndex, orderingType); + } } } @@ -112,7 +120,8 @@ namespace gtsam { return etree.eliminate(function); } else { // If no variable index is provided, compute one and call this function again - return eliminatePartialSequential(ordering, function, VariableIndex(asDerived())); + VariableIndex computedVariableIndex(asDerived()); + return eliminatePartialSequential(ordering, function, computedVariableIndex); } } @@ -132,7 +141,8 @@ namespace gtsam { return eliminatePartialSequential(ordering, function, variableIndex); } else { // If no variable index is provided, compute one and call this function again - return eliminatePartialSequential(variables, function, VariableIndex(asDerived())); + VariableIndex computedVariableIndex(asDerived()); + return eliminatePartialSequential(variables, function, computedVariableIndex); } } @@ -150,7 +160,8 @@ namespace gtsam { return junctionTree.eliminate(function); } else { // If no variable index is provided, compute one and call this function again - return eliminatePartialMultifrontal(ordering, function, VariableIndex(asDerived())); + VariableIndex computedVariableIndex(asDerived()); + return eliminatePartialMultifrontal(ordering, function, computedVariableIndex); } } @@ -170,7 +181,8 @@ namespace gtsam { return eliminatePartialMultifrontal(ordering, function, variableIndex); } else { // If no variable index is provided, compute one and call this function again - return eliminatePartialMultifrontal(variables, function, VariableIndex(asDerived())); + VariableIndex computedVariableIndex(asDerived()); + return eliminatePartialMultifrontal(variables, function, computedVariableIndex); } } @@ -287,7 +299,8 @@ namespace gtsam { } } else { // If no variable index is provided, compute one and call this function again - return marginalMultifrontalBayesTree(variables, marginalizedVariableOrdering, function, VariableIndex(asDerived())); + VariableIndex computedVariableIndex(asDerived()); + return marginalMultifrontalBayesTree(variables, marginalizedVariableOrdering, function, computedVariableIndex); } } @@ -312,7 +325,8 @@ namespace gtsam { else { // If no variable index is provided, compute one and call this function again - return marginal(variables, function, VariableIndex(asDerived())); + VariableIndex computedVariableIndex(asDerived()); + return marginal(variables, function, computedVariableIndex); } } diff --git a/gtsam/inference/FactorGraph-inst.h b/gtsam/inference/FactorGraph-inst.h index c629d336a..52ba23155 100644 --- a/gtsam/inference/FactorGraph-inst.h +++ b/gtsam/inference/FactorGraph-inst.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -74,11 +74,26 @@ namespace gtsam { /* ************************************************************************* */ template KeySet FactorGraph::keys() const { - KeySet allKeys; - BOOST_FOREACH(const sharedFactor& factor, factors_) + KeySet keys; + BOOST_FOREACH(const sharedFactor& factor, this->factors_) { + if(factor) + keys.insert(factor->begin(), factor->end()); + } + return keys; + } + + /* ************************************************************************* */ + template + KeyVector FactorGraph::keyVector() const { + KeyVector keys; + keys.reserve(2 * size()); // guess at size + BOOST_FOREACH (const sharedFactor& factor, factors_) if (factor) - allKeys.insert(factor->begin(), factor->end()); - return allKeys; + keys.insert(keys.end(), factor->begin(), factor->end()); + std::sort(keys.begin(), keys.end()); + auto last = std::unique(keys.begin(), keys.end()); + keys.erase(last, keys.end()); + return keys; } /* ************************************************************************* */ diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index e97860eaa..fcafc9b42 100644 --- a/gtsam/inference/FactorGraph.h +++ b/gtsam/inference/FactorGraph.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -114,7 +114,7 @@ namespace gtsam { /// @} /// @name Advanced Constructors /// @{ - + // TODO: are these needed? ///** @@ -319,10 +319,10 @@ namespace gtsam { void replace(size_t index, sharedFactor factor) { at(index) = factor; } /** Erase factor and rearrange other factors to take up the empty space */ - void erase(iterator item) { factors_.erase(item); } + iterator erase(iterator item) { return factors_.erase(item); } /** Erase factors and rearrange other factors to take up the empty space */ - void erase(iterator first, iterator last) { factors_.erase(first, last); } + iterator erase(iterator first, iterator last) { return factors_.erase(first, last); } /// @} /// @name Advanced Interface @@ -331,9 +331,12 @@ namespace gtsam { /** return the number of non-null factors */ size_t nrFactors() const; - /** Potentially very slow function to return all keys involved */ + /** Potentially slow function to return all keys involved, sorted, as a set */ KeySet keys() const; + /** Potentially slow function to return all keys involved, sorted, as a vector */ + KeyVector keyVector() const; + /** MATLAB interface utility: Checks whether a factor index idx exists in the graph and is a live pointer */ inline bool exists(size_t idx) const { return idx < size() && at(idx); } diff --git a/gtsam/inference/Key.h b/gtsam/inference/Key.h index d2b342575..2c3eb84c6 100644 --- a/gtsam/inference/Key.h +++ b/gtsam/inference/Key.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -11,7 +11,7 @@ /** * @file Key.h - * @brief + * @brief * @author Richard Roberts * @date Feb 20, 2012 */ @@ -78,14 +78,19 @@ GTSAM_EXPORT void PrintKeySet(const KeySet& keys, const std::string& s = "", // Define Key to be Testable by specializing gtsam::traits template struct traits; -template<> struct traits { - static void Print(const Key& key, const std::string& str = "") { - PrintKey(key, str); + +template <> +struct traits { + static void Print(const Key& val, const std::string& str = "") { + PrintKey(val, str); } - static bool Equals(const Key& key1, const Key& key2, double tol = 1e-8) { - return key1 == key2; + static bool Equals(const Key& val1, const Key& val2, double tol = 1e-8) { + return val1 == val2; } }; } // namespace gtsam + + + diff --git a/gtsam/inference/LabeledSymbol.cpp b/gtsam/inference/LabeledSymbol.cpp index b9e93ceb1..c82062246 100644 --- a/gtsam/inference/LabeledSymbol.cpp +++ b/gtsam/inference/LabeledSymbol.cpp @@ -37,7 +37,7 @@ LabeledSymbol::LabeledSymbol(const LabeledSymbol& key) : c_(key.c_), label_(key.label_), j_(key.j_) {} /* ************************************************************************* */ -LabeledSymbol::LabeledSymbol(unsigned char c, unsigned char label, size_t j) +LabeledSymbol::LabeledSymbol(unsigned char c, unsigned char label, std::uint64_t j) : c_(c), label_(label), j_(j) {} /* ************************************************************************* */ diff --git a/gtsam/inference/LabeledSymbol.h b/gtsam/inference/LabeledSymbol.h index 452c98434..8c521b067 100644 --- a/gtsam/inference/LabeledSymbol.h +++ b/gtsam/inference/LabeledSymbol.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -34,7 +34,7 @@ namespace gtsam { class GTSAM_EXPORT LabeledSymbol { protected: unsigned char c_, label_; - size_t j_; + std::uint64_t j_; public: /** Default constructor */ @@ -44,7 +44,7 @@ public: LabeledSymbol(const LabeledSymbol& key); /** Constructor */ - LabeledSymbol(unsigned char c, unsigned char label, size_t j); + LabeledSymbol(unsigned char c, unsigned char label, std::uint64_t j); /** Constructor that decodes an integer gtsam::Key */ LabeledSymbol(gtsam::Key key); diff --git a/gtsam/inference/Ordering.cpp b/gtsam/inference/Ordering.cpp index 9f4a81d05..3e3b40f8f 100644 --- a/gtsam/inference/Ordering.cpp +++ b/gtsam/inference/Ordering.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -23,7 +23,10 @@ #include #include + +#ifdef GTSAM_SUPPORT_NESTED_DISSECTION #include +#endif using namespace std; @@ -198,6 +201,7 @@ Ordering Ordering::ColamdConstrained(const VariableIndex& variableIndex, /* ************************************************************************* */ Ordering Ordering::Metis(const MetisIndex& met) { +#ifdef GTSAM_SUPPORT_NESTED_DISSECTION gttic(Ordering_METIS); vector xadj = met.xadj(); @@ -227,6 +231,10 @@ Ordering Ordering::Metis(const MetisIndex& met) { result[j] = met.intToKey(perm[j]); } return result; +#else + throw runtime_error("GTSAM was built without support for Metis-based " + "nested dissection"); +#endif } /* ************************************************************************* */ diff --git a/gtsam/inference/Symbol.cpp b/gtsam/inference/Symbol.cpp index f8b37d429..ccabcb07e 100644 --- a/gtsam/inference/Symbol.cpp +++ b/gtsam/inference/Symbol.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -33,9 +33,9 @@ static const size_t indexBits = keyBits - chrBits; static const Key chrMask = Key(UCHAR_MAX) << indexBits; // For some reason, std::numeric_limits::max() fails static const Key indexMask = ~chrMask; -Symbol::Symbol(Key key) { - c_ = (unsigned char) ((key & chrMask) >> indexBits); - j_ = key & indexMask; +Symbol::Symbol(Key key) : + c_((unsigned char) ((key & chrMask) >> indexBits)), + j_ (key & indexMask) { } Key Symbol::key() const { diff --git a/gtsam/inference/Symbol.h b/gtsam/inference/Symbol.h index 68d927baf..102761273 100644 --- a/gtsam/inference/Symbol.h +++ b/gtsam/inference/Symbol.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -21,6 +21,7 @@ #include #include #include +#include namespace gtsam { @@ -32,8 +33,8 @@ namespace gtsam { */ class GTSAM_EXPORT Symbol { protected: - unsigned char c_; - size_t j_; + const unsigned char c_; + const std::uint64_t j_; public: @@ -48,7 +49,7 @@ public: } /** Constructor */ - Symbol(unsigned char c, size_t j) : + Symbol(unsigned char c, std::uint64_t j) : c_(c), j_(j) { } @@ -73,7 +74,7 @@ public: } /** Retrieve key index */ - size_t index() const { + std::uint64_t index() const { return j_; } @@ -124,41 +125,41 @@ private: }; /** Create a symbol key from a character and index, i.e. x5. */ -inline Key symbol(unsigned char c, size_t j) { return (Key)Symbol(c,j); } +inline Key symbol(unsigned char c, std::uint64_t j) { return (Key)Symbol(c,j); } /** Return the character portion of a symbol key. */ inline unsigned char symbolChr(Key key) { return Symbol(key).chr(); } /** Return the index portion of a symbol key. */ -inline size_t symbolIndex(Key key) { return Symbol(key).index(); } +inline std::uint64_t symbolIndex(Key key) { return Symbol(key).index(); } namespace symbol_shorthand { -inline Key A(size_t j) { return Symbol('a', j); } -inline Key B(size_t j) { return Symbol('b', j); } -inline Key C(size_t j) { return Symbol('c', j); } -inline Key D(size_t j) { return Symbol('d', j); } -inline Key E(size_t j) { return Symbol('e', j); } -inline Key F(size_t j) { return Symbol('f', j); } -inline Key G(size_t j) { return Symbol('g', j); } -inline Key H(size_t j) { return Symbol('h', j); } -inline Key I(size_t j) { return Symbol('i', j); } -inline Key J(size_t j) { return Symbol('j', j); } -inline Key K(size_t j) { return Symbol('k', j); } -inline Key L(size_t j) { return Symbol('l', j); } -inline Key M(size_t j) { return Symbol('m', j); } -inline Key N(size_t j) { return Symbol('n', j); } -inline Key O(size_t j) { return Symbol('o', j); } -inline Key P(size_t j) { return Symbol('p', j); } -inline Key Q(size_t j) { return Symbol('q', j); } -inline Key R(size_t j) { return Symbol('r', j); } -inline Key S(size_t j) { return Symbol('s', j); } -inline Key T(size_t j) { return Symbol('t', j); } -inline Key U(size_t j) { return Symbol('u', j); } -inline Key V(size_t j) { return Symbol('v', j); } -inline Key W(size_t j) { return Symbol('w', j); } -inline Key X(size_t j) { return Symbol('x', j); } -inline Key Y(size_t j) { return Symbol('y', j); } -inline Key Z(size_t j) { return Symbol('z', j); } +inline Key A(std::uint64_t j) { return Symbol('a', j); } +inline Key B(std::uint64_t j) { return Symbol('b', j); } +inline Key C(std::uint64_t j) { return Symbol('c', j); } +inline Key D(std::uint64_t j) { return Symbol('d', j); } +inline Key E(std::uint64_t j) { return Symbol('e', j); } +inline Key F(std::uint64_t j) { return Symbol('f', j); } +inline Key G(std::uint64_t j) { return Symbol('g', j); } +inline Key H(std::uint64_t j) { return Symbol('h', j); } +inline Key I(std::uint64_t j) { return Symbol('i', j); } +inline Key J(std::uint64_t j) { return Symbol('j', j); } +inline Key K(std::uint64_t j) { return Symbol('k', j); } +inline Key L(std::uint64_t j) { return Symbol('l', j); } +inline Key M(std::uint64_t j) { return Symbol('m', j); } +inline Key N(std::uint64_t j) { return Symbol('n', j); } +inline Key O(std::uint64_t j) { return Symbol('o', j); } +inline Key P(std::uint64_t j) { return Symbol('p', j); } +inline Key Q(std::uint64_t j) { return Symbol('q', j); } +inline Key R(std::uint64_t j) { return Symbol('r', j); } +inline Key S(std::uint64_t j) { return Symbol('s', j); } +inline Key T(std::uint64_t j) { return Symbol('t', j); } +inline Key U(std::uint64_t j) { return Symbol('u', j); } +inline Key V(std::uint64_t j) { return Symbol('v', j); } +inline Key W(std::uint64_t j) { return Symbol('w', j); } +inline Key X(std::uint64_t j) { return Symbol('x', j); } +inline Key Y(std::uint64_t j) { return Symbol('y', j); } +inline Key Z(std::uint64_t j) { return Symbol('z', j); } } /// traits diff --git a/gtsam/inference/VariableIndex.h b/gtsam/inference/VariableIndex.h index 3b80f0d01..ad8d6720b 100644 --- a/gtsam/inference/VariableIndex.h +++ b/gtsam/inference/VariableIndex.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -32,7 +32,7 @@ namespace gtsam { /** * The VariableIndex class computes and stores the block column structure of a - * factor graph. The factor graph stores a collection of factors, each of + * factor graph. The factor graph stores a collection of factors, each of * which involves a set of variables. In contrast, the VariableIndex is built * from a factor graph prior to elimination, and stores the list of factors * that involve each variable. This information is stored as a deque of @@ -45,7 +45,7 @@ public: typedef boost::shared_ptr shared_ptr; typedef FastVector Factors; typedef Factors::iterator Factor_iterator; - typedef Factors::const_iterator Factor_const_iterator; + typedef Factors::const_iterator Factor_const_iterator; protected: typedef FastMap KeyMap; @@ -81,7 +81,7 @@ public: * The number of variable entries. This is one greater than the variable * with the highest index. */ - Key size() const { return index_.size(); } + size_t size() const { return index_.size(); } /** The number of factors in the original factor graph */ size_t nFactors() const { return nFactors_; } diff --git a/gtsam/inference/inference-inst.h b/gtsam/inference/inference-inst.h index 434b705f2..1e3f4cdfc 100644 --- a/gtsam/inference/inference-inst.h +++ b/gtsam/inference/inference-inst.h @@ -61,7 +61,7 @@ namespace gtsam { { // Call eliminate on the node and add the result to the parent's gathered factors typename TREE::sharedFactor childFactor = node->eliminate(result, eliminationFunction, myData.childFactors); - if(!childFactor->empty()) + if(childFactor && !childFactor->empty()) myData.parentData->childFactors.push_back(childFactor); } }; diff --git a/gtsam/inference/tests/testOrdering.cpp b/gtsam/inference/tests/testOrdering.cpp index d789da9b0..c209d086b 100644 --- a/gtsam/inference/tests/testOrdering.cpp +++ b/gtsam/inference/tests/testOrdering.cpp @@ -71,7 +71,7 @@ TEST(Ordering, grouped_constrained_ordering) { SymbolicFactorGraph sfg = example::symbolicChain(); // constrained version - push one set to the end - FastMap constraints; + FastMap constraints; constraints[2] = 1; constraints[4] = 1; constraints[5] = 2; diff --git a/gtsam/linear/GaussianBayesNet.cpp b/gtsam/linear/GaussianBayesNet.cpp index 6db13adb6..948daf2ad 100644 --- a/gtsam/linear/GaussianBayesNet.cpp +++ b/gtsam/linear/GaussianBayesNet.cpp @@ -86,6 +86,8 @@ namespace gtsam { VectorValues GaussianBayesNet::backSubstitute(const VectorValues& rhs) const { VectorValues result; + // TODO this looks pretty sketchy. result is passed as the parents argument + // as it's filled up by solving the gaussian conditionals. BOOST_REVERSE_FOREACH(const sharedConditional& cg, *this) { result.insert(cg->solveOtherRHS(result, rhs)); } @@ -138,17 +140,18 @@ namespace gtsam { // return grad; //} - /* ************************************************************************* */ - pair GaussianBayesNet::matrix() const - { + /* ************************************************************************* */ + pair GaussianBayesNet::matrix() const { GaussianFactorGraph factorGraph(*this); KeySet keys = factorGraph.keys(); // add frontal keys in order Ordering ordering; BOOST_FOREACH (const sharedConditional& cg, *this) - BOOST_FOREACH (Key key, cg->frontals()) { - ordering.push_back(key); - keys.erase(key); + if (cg) { + BOOST_FOREACH (Key key, cg->frontals()) { + ordering.push_back(key); + keys.erase(key); + } } // add remaining keys in case Bayes net is incomplete BOOST_FOREACH (Key key, keys) diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index 6bd853764..feb477a4e 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -71,7 +71,7 @@ namespace gtsam { model_->print(" Noise model: "); else cout << " No noise model" << endl; - } + } /* ************************************************************************* */ bool GaussianConditional::equals(const GaussianFactor& f, double tol) const @@ -120,24 +120,24 @@ namespace gtsam { VectorValues GaussianConditional::solve(const VectorValues& x) const { // Concatenate all vector values that correspond to parent variables - Vector xS = x.vector(FastVector(beginParents(), endParents())); + const Vector xS = x.vector(FastVector(beginParents(), endParents())); // Update right-hand-side - xS = getb() - get_S() * xS; + const Vector rhs = get_d() - get_S() * xS; // Solve matrix - Vector soln = get_R().triangularView().solve(xS); + const Vector solution = get_R().triangularView().solve(rhs); // Check for indeterminant solution - if(soln.hasNaN()) throw IndeterminantLinearSystemException(keys().front()); - - // TODO, do we not have to scale by sigmas here? Copy/paste bug + if (solution.hasNaN()) { + throw IndeterminantLinearSystemException(keys().front()); + } // Insert solution into a VectorValues VectorValues result; DenseIndex vectorPosition = 0; - for(const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) { - result.insert(*frontal, soln.segment(vectorPosition, getDim(frontal))); + for (const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) { + result.insert(*frontal, solution.segment(vectorPosition, getDim(frontal))); vectorPosition += getDim(frontal); } diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index a39b1d91e..19d6bd607 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -30,6 +30,8 @@ #include #include +#include + using namespace std; using namespace gtsam; @@ -68,27 +70,6 @@ namespace gtsam { return spec; } - /* ************************************************************************* */ - vector GaussianFactorGraph::getkeydim() const { - // First find dimensions of each variable - vector dims; - BOOST_FOREACH(const sharedFactor& factor, *this) { - for (GaussianFactor::const_iterator pos = factor->begin(); - pos != factor->end(); ++pos) { - if (dims.size() <= *pos) - dims.resize(*pos + 1, 0); - dims[*pos] = factor->getDim(pos); - } - } - // Find accumulated dimensions for variables - vector dims_accumulated; - dims_accumulated.resize(dims.size()+1,0); - dims_accumulated[0]=0; - for (size_t i=1; i optionalOrdering) const { // combine all factors and get upper-triangular part of Hessian - HessianFactor combined(*this, Scatter(*this, optionalOrdering)); + Scatter scatter(*this, optionalOrdering); + HessianFactor combined(*this, scatter); Matrix result = combined.info(); // Fill in lower-triangular part of Hessian result.triangularView() = result.transpose(); diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index 02bc95511..ae552adcd 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -17,7 +17,7 @@ * @author Alireza Fathi * @author Richard Roberts * @author Frank Dellaert - */ + */ #pragma once @@ -141,8 +141,6 @@ namespace gtsam { /* return a map of (Key, dimension) */ std::map getKeyDimMap() const; - std::vector getkeydim() const; - /** unnormalized error */ double error(const VectorValues& x) const { double total_error = 0.; diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 2b275b60f..9277f3903 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -455,8 +455,8 @@ std::pair, // Build joint factor HessianFactor::shared_ptr jointFactor; try { - jointFactor = boost::make_shared(factors, - Scatter(factors, keys)); + Scatter scatter(factors, keys); + jointFactor = boost::make_shared(factors, scatter); } catch (std::invalid_argument&) { throw InvalidDenseElimination( "EliminateCholesky was called with a request to eliminate variables that are not\n" diff --git a/gtsam/linear/IterativeSolver.h b/gtsam/linear/IterativeSolver.h index 442a5fc6b..92dcbfa07 100644 --- a/gtsam/linear/IterativeSolver.h +++ b/gtsam/linear/IterativeSolver.h @@ -111,7 +111,7 @@ public: * Handy data structure for iterative solvers * key to (index, dimension, colstart) */ -class GTSAM_EXPORT KeyInfoEntry: public boost::tuple { +class GTSAM_EXPORT KeyInfoEntry: public boost::tuple { public: diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 47b6ec90b..fc1a7c841 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -527,15 +527,16 @@ void JacobianFactor::updateHessian(const FastVector& infoKeys, // Loop over blocks of A, including RHS with j==n vector slots(n+1); for (DenseIndex j = 0; j <= n; ++j) { + Eigen::Block Ab_j = Ab_(j); const DenseIndex J = (j == n) ? N : Slot(infoKeys, keys_[j]); slots[j] = J; // Fill off-diagonal blocks with Ai'*Aj for (DenseIndex i = 0; i < j; ++i) { const DenseIndex I = slots[i]; // because i, - boost::shared_ptr > JacobianFactor::eliminate( +std::pair JacobianFactor::eliminate( const Ordering& keys) { GaussianFactorGraph graph; graph.add(*this); @@ -710,8 +711,7 @@ void JacobianFactor::setModel(bool anyConstrained, const Vector& sigmas) { } /* ************************************************************************* */ -std::pair, - boost::shared_ptr > EliminateQR( +std::pair EliminateQR( const GaussianFactorGraph& factors, const Ordering& keys) { gttic(EliminateQR); // Combine and sort variable blocks in elimination order @@ -721,77 +721,84 @@ std::pair, } catch (std::invalid_argument&) { throw InvalidDenseElimination( "EliminateQR was called with a request to eliminate variables that are not\n" - "involved in the provided factors."); + "involved in the provided factors."); } // Do dense elimination + // The following QR variants eliminate to fully triangular or trapezoidal SharedDiagonal noiseModel; - if (jointFactor->model_) - jointFactor->model_ = jointFactor->model_->QR(jointFactor->Ab_.matrix()); - else - inplace_QR(jointFactor->Ab_.matrix()); - - // Zero below the diagonal - jointFactor->Ab_.matrix().triangularView().setZero(); + VerticalBlockMatrix& Ab = jointFactor->Ab_; + if (jointFactor->model_) { + // The noiseModel QR can, in the case of constraints, yield a "staggered" QR where + // some rows have more leading zeros than in an upper triangular matrix. + // In either case, QR will put zeros below the "diagonal". + jointFactor->model_ = jointFactor->model_->QR(Ab.matrix()); + } else { + // The inplace variant will have no valid rows anymore below m==n + // and only entries above the diagonal are valid. + inplace_QR(Ab.matrix()); + // We zero below the diagonal to agree with the result from noieModel QR + Ab.matrix().triangularView().setZero(); + size_t m = Ab.rows(), n = Ab.cols() - 1; + size_t maxRank = min(m, n); + jointFactor->model_ = noiseModel::Unit::Create(maxRank); + } // Split elimination result into conditional and remaining factor - GaussianConditional::shared_ptr conditional = // + GaussianConditional::shared_ptr conditional = // jointFactor->splitConditional(keys.size()); return make_pair(conditional, jointFactor); } /* ************************************************************************* */ -GaussianConditional::shared_ptr JacobianFactor::splitConditional( - size_t nrFrontals) { +GaussianConditional::shared_ptr JacobianFactor::splitConditional(size_t nrFrontals) { gttic(JacobianFactor_splitConditional); - if (nrFrontals > size()) + if (!model_) { throw std::invalid_argument( - "Requesting to split more variables than exist using JacobianFactor::splitConditional"); + "JacobianFactor::splitConditional cannot be given a NULL noise model"); + } + if (nrFrontals > size()) { + throw std::invalid_argument( + "JacobianFactor::splitConditional was requested to split off more variables than exist."); + } + + // Convert nr of keys to number of scalar columns DenseIndex frontalDim = Ab_.range(0, nrFrontals).cols(); + // Check that the noise model has at least this dimension + // If this is *not* the case, we do not have enough information on the frontal variables. + if ((DenseIndex)model_->dim() < frontalDim) + throw IndeterminantLinearSystemException(this->keys().front()); + // Restrict the matrix to be in the first nrFrontals variables and create the conditional - gttic(cond_Rd); const DenseIndex originalRowEnd = Ab_.rowEnd(); Ab_.rowEnd() = Ab_.rowStart() + frontalDim; SharedDiagonal conditionalNoiseModel; - if (model_) { - if ((DenseIndex) model_->dim() < frontalDim) - throw IndeterminantLinearSystemException(this->keys().front()); - conditionalNoiseModel = noiseModel::Diagonal::Sigmas( - model_->sigmas().segment(Ab_.rowStart(), Ab_.rows())); - } - GaussianConditional::shared_ptr conditional = boost::make_shared< - GaussianConditional>(Base::keys_, nrFrontals, Ab_, conditionalNoiseModel); - const DenseIndex maxRemainingRows = std::min(Ab_.cols(), originalRowEnd) - - Ab_.rowStart() - frontalDim; - const DenseIndex remainingRows = - model_ ? std::min(model_->sigmas().size() - frontalDim, - maxRemainingRows) : - maxRemainingRows; + conditionalNoiseModel = + noiseModel::Diagonal::Sigmas(model_->sigmas().segment(Ab_.rowStart(), Ab_.rows())); + GaussianConditional::shared_ptr conditional = + boost::make_shared(Base::keys_, nrFrontals, Ab_, conditionalNoiseModel); + + const DenseIndex maxRemainingRows = + std::min(Ab_.cols(), originalRowEnd) - Ab_.rowStart() - frontalDim; + const DenseIndex remainingRows = std::min(model_->sigmas().size() - frontalDim, maxRemainingRows); Ab_.rowStart() += frontalDim; Ab_.rowEnd() = Ab_.rowStart() + remainingRows; Ab_.firstBlock() += nrFrontals; - gttoc(cond_Rd); // Take lower-right block of Ab to get the new factor - gttic(remaining_factor); keys_.erase(begin(), begin() + nrFrontals); // Set sigmas with the right model - if (model_) { - if (model_->isConstrained()) - model_ = noiseModel::Constrained::MixedSigmas( - model_->sigmas().tail(remainingRows)); - else - model_ = noiseModel::Diagonal::Sigmas( - model_->sigmas().tail(remainingRows)); - assert(model_->dim() == (size_t)Ab_.rows()); - } - gttoc(remaining_factor); + if (model_->isConstrained()) + model_ = noiseModel::Constrained::MixedSigmas(model_->sigmas().tail(remainingRows)); + else + model_ = noiseModel::Diagonal::Sigmas(model_->sigmas().tail(remainingRows)); + assert(model_->dim() == (size_t)Ab_.rows()); return conditional; } -} +} // namespace gtsam diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index ea9958474..3795f096e 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -314,7 +314,7 @@ namespace gtsam { JacobianFactor whiten() const; /** Eliminate the requested variables. */ - std::pair, boost::shared_ptr > + std::pair, shared_ptr> eliminate(const Ordering& keys); /** set noiseModel correctly */ @@ -331,13 +331,15 @@ namespace gtsam { * @return The conditional and remaining factor * * \addtogroup LinearSolving */ - friend GTSAM_EXPORT std::pair, boost::shared_ptr > + friend GTSAM_EXPORT std::pair, shared_ptr> EliminateQR(const GaussianFactorGraph& factors, const Ordering& keys); /** * splits a pre-factorized factor into a conditional, and changes the current * factor to be the remaining component. Performs same operation as eliminate(), * but without running QR. + * NOTE: looks at dimension of noise model to determine how many rows to keep. + * @param nrFrontals number of keys to eliminate */ boost::shared_ptr splitConditional(size_t nrFrontals); diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index dc12216e3..15b2dcf81 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -21,6 +21,7 @@ #include #include +#include #include #include #include @@ -58,7 +59,7 @@ boost::optional checkIfDiagonal(const Matrix M) { for (i = 0; i < m; i++) if (!full) for (j = i + 1; j < n; j++) - if (fabs(M(i, j)) > 1e-9) { + if (std::abs(M(i, j)) > 1e-9) { full = true; break; } @@ -87,7 +88,7 @@ Gaussian::shared_ptr Gaussian::SqrtInformation(const Matrix& R, bool smart) { if (diagonal) return Diagonal::Sigmas(diagonal->array().inverse(), true); } - // NOTE(frank): only reaches here if !smart && !diagonal + // NOTE(frank): only reaches here if !(smart && diagonal) return shared_ptr(new Gaussian(R.rows(), R)); } @@ -190,8 +191,8 @@ SharedDiagonal Gaussian::QR(Matrix& Ab) const { // get size(A) and maxRank // TODO: really no rank problems ? - // size_t m = Ab.rows(), n = Ab.cols()-1; - // size_t maxRank = min(m,n); + size_t m = Ab.rows(), n = Ab.cols()-1; + size_t maxRank = min(m,n); // pre-whiten everything (cheaply if possible) WhitenInPlace(Ab); @@ -200,12 +201,13 @@ SharedDiagonal Gaussian::QR(Matrix& Ab) const { // Eigen QR - much faster than older householder approach inplace_QR(Ab); + Ab.triangularView().setZero(); // hand-coded householder implementation // TODO: necessary to isolate last column? // householder(Ab, maxRank); - return SharedDiagonal(); + return noiseModel::Unit::Create(maxRank); } void Gaussian::WhitenSystem(vector& A, Vector& b) const { @@ -414,81 +416,141 @@ Constrained::shared_ptr Constrained::unit() const { // Special version of QR for Constrained calls slower but smarter code // that deals with possibly zero sigmas // It is Gram-Schmidt orthogonalization rather than Householder -// Previously Diagonal::QR + +// Check whether column a triggers a constraint and corresponding variable is deterministic +// Return constraint_row with maximum element in case variable plays in multiple constraints +template +boost::optional check_if_constraint(VECTOR a, const Vector& invsigmas, size_t m) { + boost::optional constraint_row; + // not zero, so roundoff errors will not be counted + // TODO(frank): that's a fairly crude way of dealing with roundoff errors :-( + double max_element = 1e-9; + for (size_t i = 0; i < m; i++) { + if (!std::isinf(invsigmas[i])) + continue; + double abs_ai = std::abs(a(i,0)); + if (abs_ai > max_element) { + max_element = abs_ai; + constraint_row.reset(i); + } + } + return constraint_row; +} + SharedDiagonal Constrained::QR(Matrix& Ab) const { - bool verbose = false; - if (verbose) cout << "\nStarting Constrained::QR" << endl; + static const double kInfinity = std::numeric_limits::infinity(); // get size(A) and maxRank - size_t m = Ab.rows(), n = Ab.cols()-1; - size_t maxRank = min(m,n); + size_t m = Ab.rows(); + const size_t n = Ab.cols() - 1; + const size_t maxRank = min(m, n); // create storage for [R d] - typedef boost::tuple Triple; + typedef boost::tuple Triple; list Rd; - Vector pseudo(m); // allocate storage for pseudo-inverse + Matrix rd(1, n + 1); // and for row of R Vector invsigmas = sigmas_.array().inverse(); - Vector weights = invsigmas.array().square(); // calculate weights once + Vector weights = invsigmas.array().square(); // calculate weights once // We loop over all columns, because the columns that can be eliminated // are not necessarily contiguous. For each one, estimate the corresponding // scalar variable x as d-rS, with S the separator (remaining columns). // Then update A and b by substituting x with d-rS, zero-ing out x's column. - for (size_t j=0; j a = Ab.block(0, j, m, 1); - // Calculate weighted pseudo-inverse and corresponding precision - gttic(constrained_QR_weightedPseudoinverse); - double precision = weightedPseudoinverse(a, weights, pseudo); - gttoc(constrained_QR_weightedPseudoinverse); + // Check whether we need to handle as a constraint + boost::optional constraint_row = check_if_constraint(a, invsigmas, m); - // If precision is zero, no information on this column - // This is actually not limited to constraints, could happen in Gaussian::QR - // In that case, we're probably hosed. TODO: make sure Householder is rank-revealing - if (precision < 1e-8) continue; + if (constraint_row) { + // Handle this as a constraint, as the i^th row has zero sigma with non-zero entry A(i,j) - gttic(constrained_QR_create_rd); - // create solution [r d], rhs is automatically r(n) - Vector rd(n+1); // uninitialized ! - rd(j)=1.0; // put 1 on diagonal - for (size_t j2=j+1; j2=maxRank) break; + // exit after rank exhausted + if (Rd.size() >= maxRank) + break; - // update Ab, expensive, using outer product - gttic(constrained_QR_update_Ab); - Ab.middleCols(j+1,n-j) -= a * rd.segment(j+1, n-j).transpose(); - gttoc(constrained_QR_update_Ab); + // The constraint row will be zeroed out, so we can save work by swapping in the + // last valid row and decreasing m. This will save work on subsequent down-dates, too. + m -= 1; + if (*constraint_row != m) { + Ab.row(*constraint_row) = Ab.row(m); + weights(*constraint_row) = weights(m); + invsigmas(*constraint_row) = invsigmas(m); + } + + // get a reduced a-column which is now shorter + Eigen::Block a_reduced = Ab.block(0, j, m, 1); + a_reduced *= (1.0/rd(0, j)); // NOTE(frank): this is the 1/a[i] = 1/rd(0,j) factor we need! + + // Rank-1 down-date of Ab, expensive, using outer product + Ab.block(0, j + 1, m, n - j).noalias() -= a_reduced * rd.middleCols(j + 1, n - j); + } else { + // Treat in normal Gram-Schmidt way + // Calculate weighted pseudo-inverse and corresponding precision + + // Form psuedo-inverse inv(a'inv(Sigma)a)a'inv(Sigma) + // For diagonal Sigma, inv(Sigma) = diag(precisions) + double precision = 0; + Vector pseudo(m); // allocate storage for pseudo-inverse + for (size_t i = 0; i < m; i++) { + double ai = a(i, 0); + if (std::abs(ai) > 1e-9) { // also catches remaining sigma==0 rows + pseudo[i] = weights[i] * ai; + precision += pseudo[i] * ai; + } else + pseudo[i] = 0; + } + + if (precision > 1e-8) { + pseudo /= precision; + + // create solution [r d], rhs is automatically r(n) + rd(0, j) = 1.0; // put 1 on diagonal + rd.block(0, j + 1, 1, n - j) = pseudo.transpose() * Ab.block(0, j + 1, m, n - j); + + // construct solution (r, d, sigma) + Rd.push_back(boost::make_tuple(j, rd, precision)); + } else { + // If precision is zero, no information on this column + // This is actually not limited to constraints, could happen in Gaussian::QR + // In that case, we're probably hosed. TODO: make sure Householder is rank-revealing + continue; // but even if not, no need to update if a==zeros + } + + // exit after rank exhausted + if (Rd.size() >= maxRank) + break; + + // Rank-1 down-date of Ab, expensive, using outer product + Ab.block(0, j + 1, m, n - j).noalias() -= a * rd.middleCols(j + 1, n - j); + } } // Create storage for precisions Vector precisions(Rd.size()); - gttic(constrained_QR_write_back_into_Ab); // Write back result in Ab, imperative as we are - // TODO: test that is correct if a column was skipped !!!! - size_t i = 0; // start with first row + size_t i = 0; // start with first row bool mixed = false; - BOOST_FOREACH(const Triple& t, Rd) { - const size_t& j = t.get<0>(); - const Vector& rd = t.get<1>(); - precisions(i) = t.get<2>(); - if (constrained(i)) mixed = true; - for (size_t j2=0; j2(); + const Matrix& rd = t.get<1>(); + precisions(i) = t.get<2>(); + if (std::isinf(precisions(i))) + mixed = true; + Ab.block(i, j, 1, n + 1 - j) = rd.block(0, j, 1, n + 1 - j); + i += 1; } - gttoc(constrained_QR_write_back_into_Ab); // Must include mu, as the defaults might be higher, resulting in non-convergence return mixed ? Constrained::MixedPrecisions(mu_, precisions) : Diagonal::Precisions(precisions); @@ -498,13 +560,13 @@ SharedDiagonal Constrained::QR(Matrix& Ab) const { // Isotropic /* ************************************************************************* */ Isotropic::shared_ptr Isotropic::Sigma(size_t dim, double sigma, bool smart) { - if (smart && fabs(sigma-1.0)<1e-9) return Unit::Create(dim); + if (smart && std::abs(sigma-1.0)<1e-9) return Unit::Create(dim); return shared_ptr(new Isotropic(dim, sigma)); } /* ************************************************************************* */ Isotropic::shared_ptr Isotropic::Variance(size_t dim, double variance, bool smart) { - if (smart && fabs(variance-1.0)<1e-9) return Unit::Create(dim); + if (smart && std::abs(variance-1.0)<1e-9) return Unit::Create(dim); return shared_ptr(new Isotropic(dim, sqrt(variance))); } @@ -561,21 +623,19 @@ void Unit::print(const std::string& name) const { namespace mEstimator { -/** produce a weight vector according to an error vector and the implemented - * robust function */ -Vector Base::weight(const Vector &error) const { +Vector Base::weight(const Vector& error) const { const size_t n = error.rows(); Vector w(n); - for ( size_t i = 0 ; i < n ; ++i ) + for (size_t i = 0; i < n; ++i) w(i) = weight(error(i)); return w; } -/** The following three functions reweight block matrices and a vector - * according to their weight implementation */ +// The following three functions re-weight block matrices and a vector +// according to their weight implementation void Base::reweight(Vector& error) const { - if(reweight_ == Block) { + if (reweight_ == Block) { const double w = sqrtWeight(error.norm()); error *= w; } else { @@ -583,7 +643,7 @@ void Base::reweight(Vector& error) const { } } -/** Reweight n block matrices with one error vector */ +// Reweight n block matrices with one error vector void Base::reweight(vector &A, Vector &error) const { if ( reweight_ == Block ) { const double w = sqrtWeight(error.norm()); @@ -601,7 +661,7 @@ void Base::reweight(vector &A, Vector &error) const { } } -/** Reweight one block matrix with one error vector */ +// Reweight one block matrix with one error vector void Base::reweight(Matrix &A, Vector &error) const { if ( reweight_ == Block ) { const double w = sqrtWeight(error.norm()); @@ -615,7 +675,7 @@ void Base::reweight(Matrix &A, Vector &error) const { } } -/** Reweight two block matrix with one error vector */ +// Reweight two block matrix with one error vector void Base::reweight(Matrix &A1, Matrix &A2, Vector &error) const { if ( reweight_ == Block ) { const double w = sqrtWeight(error.norm()); @@ -631,7 +691,7 @@ void Base::reweight(Matrix &A1, Matrix &A2, Vector &error) const { } } -/** Reweight three block matrix with one error vector */ +// Reweight three block matrix with one error vector void Base::reweight(Matrix &A1, Matrix &A2, Matrix &A3, Vector &error) const { if ( reweight_ == Block ) { const double w = sqrtWeight(error.norm()); @@ -659,11 +719,9 @@ void Null::print(const std::string &s="") const Null::shared_ptr Null::Create() { return shared_ptr(new Null()); } -Fair::Fair(double c, const ReweightScheme reweight) - : Base(reweight), c_(c) { - if ( c_ <= 0 ) { - cout << "mEstimator Fair takes only positive double in constructor. forced to 1.0" << endl; - c_ = 1.0; +Fair::Fair(double c, const ReweightScheme reweight) : Base(reweight), c_(c) { + if (c_ <= 0) { + throw runtime_error("mEstimator Fair takes only positive double in constructor."); } } @@ -671,16 +729,13 @@ Fair::Fair(double c, const ReweightScheme reweight) // Fair /* ************************************************************************* */ -double Fair::weight(double error) const -{ return 1.0 / (1.0 + fabs(error)/c_); } - void Fair::print(const std::string &s="") const { cout << s << "fair (" << c_ << ")" << endl; } bool Fair::equals(const Base &expected, double tol) const { const Fair* p = dynamic_cast (&expected); if (p == NULL) return false; - return fabs(c_ - p->c_ ) < tol; + return std::abs(c_ - p->c_ ) < tol; } Fair::shared_ptr Fair::Create(double c, const ReweightScheme reweight) @@ -690,18 +745,12 @@ Fair::shared_ptr Fair::Create(double c, const ReweightScheme reweight) // Huber /* ************************************************************************* */ -Huber::Huber(double k, const ReweightScheme reweight) - : Base(reweight), k_(k) { - if ( k_ <= 0 ) { - cout << "mEstimator Huber takes only positive double in constructor. forced to 1.0" << endl; - k_ = 1.0; +Huber::Huber(double k, const ReweightScheme reweight) : Base(reweight), k_(k) { + if (k_ <= 0) { + throw runtime_error("mEstimator Huber takes only positive double in constructor."); } } -double Huber::weight(double error) const { - return (fabs(error) > k_) ? k_ / fabs(error) : 1.0; -} - void Huber::print(const std::string &s="") const { cout << s << "huber (" << k_ << ")" << endl; } @@ -709,7 +758,7 @@ void Huber::print(const std::string &s="") const { bool Huber::equals(const Base &expected, double tol) const { const Huber* p = dynamic_cast(&expected); if (p == NULL) return false; - return fabs(k_ - p->k_) < tol; + return std::abs(k_ - p->k_) < tol; } Huber::shared_ptr Huber::Create(double c, const ReweightScheme reweight) { @@ -720,18 +769,12 @@ Huber::shared_ptr Huber::Create(double c, const ReweightScheme reweight) { // Cauchy /* ************************************************************************* */ -Cauchy::Cauchy(double k, const ReweightScheme reweight) - : Base(reweight), k_(k) { - if ( k_ <= 0 ) { - cout << "mEstimator Cauchy takes only positive double in constructor. forced to 1.0" << endl; - k_ = 1.0; +Cauchy::Cauchy(double k, const ReweightScheme reweight) : Base(reweight), k_(k), ksquared_(k * k) { + if (k <= 0) { + throw runtime_error("mEstimator Cauchy takes only positive double in constructor."); } } -double Cauchy::weight(double error) const { - return k_*k_ / (k_*k_ + error*error); -} - void Cauchy::print(const std::string &s="") const { cout << s << "cauchy (" << k_ << ")" << endl; } @@ -739,7 +782,7 @@ void Cauchy::print(const std::string &s="") const { bool Cauchy::equals(const Base &expected, double tol) const { const Cauchy* p = dynamic_cast(&expected); if (p == NULL) return false; - return fabs(k_ - p->k_) < tol; + return std::abs(ksquared_ - p->ksquared_) < tol; } Cauchy::shared_ptr Cauchy::Create(double c, const ReweightScheme reweight) { @@ -749,18 +792,7 @@ Cauchy::shared_ptr Cauchy::Create(double c, const ReweightScheme reweight) { /* ************************************************************************* */ // Tukey /* ************************************************************************* */ -Tukey::Tukey(double c, const ReweightScheme reweight) - : Base(reweight), c_(c) { -} - -double Tukey::weight(double error) const { - if (fabs(error) <= c_) { - double xc2 = (error/c_)*(error/c_); - double one_xc22 = (1.0-xc2)*(1.0-xc2); - return one_xc22; - } - return 0.0; -} +Tukey::Tukey(double c, const ReweightScheme reweight) : Base(reweight), c_(c), csquared_(c * c) {} void Tukey::print(const std::string &s="") const { std::cout << s << ": Tukey (" << c_ << ")" << std::endl; @@ -769,7 +801,7 @@ void Tukey::print(const std::string &s="") const { bool Tukey::equals(const Base &expected, double tol) const { const Tukey* p = dynamic_cast(&expected); if (p == NULL) return false; - return fabs(c_ - p->c_) < tol; + return std::abs(c_ - p->c_) < tol; } Tukey::shared_ptr Tukey::Create(double c, const ReweightScheme reweight) { @@ -779,14 +811,7 @@ Tukey::shared_ptr Tukey::Create(double c, const ReweightScheme reweight) { /* ************************************************************************* */ // Welsh /* ************************************************************************* */ -Welsh::Welsh(double c, const ReweightScheme reweight) - : Base(reweight), c_(c) { -} - -double Welsh::weight(double error) const { - double xc2 = (error/c_)*(error/c_); - return std::exp(-xc2); -} +Welsh::Welsh(double c, const ReweightScheme reweight) : Base(reweight), c_(c), csquared_(c * c) {} void Welsh::print(const std::string &s="") const { std::cout << s << ": Welsh (" << c_ << ")" << std::endl; @@ -795,7 +820,7 @@ void Welsh::print(const std::string &s="") const { bool Welsh::equals(const Base &expected, double tol) const { const Welsh* p = dynamic_cast(&expected); if (p == NULL) return false; - return fabs(c_ - p->c_) < tol; + return std::abs(c_ - p->c_) < tol; } Welsh::shared_ptr Welsh::Create(double c, const ReweightScheme reweight) { diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index db01152f6..c76db1b55 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -232,9 +232,11 @@ namespace gtsam { /** * Apply appropriately weighted QR factorization to the system [A b] * Q' * [A b] = [R d] - * Dimensions: (r*m) * m*(n+1) = r*(n+1) + * Dimensions: (r*m) * m*(n+1) = r*(n+1), where r = min(m,n). + * This routine performs an in-place factorization on Ab. + * Below-diagonal elements are set to zero by this routine. * @param Ab is the m*(n+1) augmented system matrix [A b] - * @return in-place QR factorization [R d]. Below-diagonal is undefined !!!!! + * @return Empty SharedDiagonal() noise model: R,d are whitened */ virtual boost::shared_ptr QR(Matrix& Ab) const; @@ -482,6 +484,12 @@ namespace gtsam { /** * Apply QR factorization to the system [A b], taking into account constraints + * Q' * [A b] = [R d] + * Dimensions: (r*m) * m*(n+1) = r*(n+1), where r = min(m,n). + * This routine performs an in-place factorization on Ab. + * Below-diagonal elements are set to zero by this routine. + * @param Ab is the m*(n+1) augmented system matrix [A b] + * @return diagonal noise model can be all zeros, mixed, or not-constrained */ virtual Diagonal::shared_ptr QR(Matrix& Ab) const; @@ -618,8 +626,24 @@ namespace gtsam { } }; - // TODO: should not really exist - /// The mEstimator namespace contains all robust error functions (not models) + /** + * The mEstimator name space contains all robust error functions. + * It mirrors the exposition at + * http://research.microsoft.com/en-us/um/people/zhang/INRIA/Publis/Tutorial-Estim/node24.html + * which talks about minimizing \sum \rho(r_i), where \rho is a residual function of choice. + * + * To illustrate, let's consider the least-squares (L2), L1, and Huber estimators as examples: + * + * Name Symbol Least-Squares L1-norm Huber + * Residual \rho(x) 0.5*x^2 |x| 0.5*x^2 if x shared_ptr; Fair(double c = 1.3998, const ReweightScheme reweight = Block); - virtual ~Fair() {} - virtual double weight(double error) const; - virtual void print(const std::string &s) const; - virtual bool equals(const Base& expected, double tol=1e-8) const; + double weight(double error) const { + return 1.0 / (1.0 + fabs(error) / c_); + } + void print(const std::string &s) const; + bool equals(const Base& expected, double tol=1e-8) const; static shared_ptr Create(double c, const ReweightScheme reweight = Block) ; - protected: - double c_; - private: /** Serialization function */ friend class boost::serialization::access; @@ -721,19 +746,20 @@ namespace gtsam { /// Huber implements the "Huber" robust error model (Zhang97ivc) class GTSAM_EXPORT Huber : public Base { + protected: + double k_; + public: typedef boost::shared_ptr shared_ptr; - virtual ~Huber() {} Huber(double k = 1.345, const ReweightScheme reweight = Block); - virtual double weight(double error) const; - virtual void print(const std::string &s) const; - virtual bool equals(const Base& expected, double tol=1e-8) const; + double weight(double error) const { + return (error < k_) ? (1.0) : (k_ / fabs(error)); + } + void print(const std::string &s) const; + bool equals(const Base& expected, double tol=1e-8) const; static shared_ptr Create(double k, const ReweightScheme reweight = Block) ; - protected: - double k_; - private: /** Serialization function */ friend class boost::serialization::access; @@ -750,19 +776,20 @@ namespace gtsam { /// oberlaender@fzi.de /// Thanks Jan! class GTSAM_EXPORT Cauchy : public Base { + protected: + double k_, ksquared_; + public: typedef boost::shared_ptr shared_ptr; - virtual ~Cauchy() {} Cauchy(double k = 0.1, const ReweightScheme reweight = Block); - virtual double weight(double error) const; - virtual void print(const std::string &s) const; - virtual bool equals(const Base& expected, double tol=1e-8) const; + double weight(double error) const { + return ksquared_ / (ksquared_ + error*error); + } + void print(const std::string &s) const; + bool equals(const Base& expected, double tol=1e-8) const; static shared_ptr Create(double k, const ReweightScheme reweight = Block) ; - protected: - double k_; - private: /** Serialization function */ friend class boost::serialization::access; @@ -775,19 +802,24 @@ namespace gtsam { /// Tukey implements the "Tukey" robust error model (Zhang97ivc) class GTSAM_EXPORT Tukey : public Base { + protected: + double c_, csquared_; + public: typedef boost::shared_ptr shared_ptr; Tukey(double c = 4.6851, const ReweightScheme reweight = Block); - virtual ~Tukey() {} - virtual double weight(double error) const; - virtual void print(const std::string &s) const; - virtual bool equals(const Base& expected, double tol=1e-8) const; + double weight(double error) const { + if (std::fabs(error) <= c_) { + double xc2 = error*error/csquared_; + return (1.0-xc2)*(1.0-xc2); + } + return 0.0; + } + void print(const std::string &s) const; + bool equals(const Base& expected, double tol=1e-8) const; static shared_ptr Create(double k, const ReweightScheme reweight = Block) ; - protected: - double c_; - private: /** Serialization function */ friend class boost::serialization::access; @@ -800,19 +832,21 @@ namespace gtsam { /// Welsh implements the "Welsh" robust error model (Zhang97ivc) class GTSAM_EXPORT Welsh : public Base { + protected: + double c_, csquared_; + public: typedef boost::shared_ptr shared_ptr; Welsh(double c = 2.9846, const ReweightScheme reweight = Block); - virtual ~Welsh() {} - virtual double weight(double error) const; - virtual void print(const std::string &s) const; - virtual bool equals(const Base& expected, double tol=1e-8) const; + double weight(double error) const { + double xc2 = (error*error)/csquared_; + return std::exp(-xc2); + } + void print(const std::string &s) const; + bool equals(const Base& expected, double tol=1e-8) const; static shared_ptr Create(double k, const ReweightScheme reweight = Block) ; - protected: - double c_; - private: /** Serialization function */ friend class boost::serialization::access; @@ -884,7 +918,23 @@ namespace gtsam { } ///\namespace mEstimator - /// Base class for robust error models + /** + * Base class for robust error models + * The robust M-estimators above simply tell us how to re-weight the residual, and are + * isotropic kernels, in that they do not allow for correlated noise. They also have no way + * to scale the residual values, e.g., dividing by a single standard deviation. + * Hence, the actual robust noise model below does this scaling/whitening in sequence, by + * passing both a standard noise model and a robust estimator. + * + * Taking as an example noise = Isotropic::Create(d, sigma), we first divide the residuals + * uw = |Ax-b| by sigma by "whitening" the system (A,b), obtaining r = |Ax-b|/sigma, and + * then we pass the now whitened residual 'r' through the robust M-estimator. + * This is currently done by multiplying with sqrt(w), because the residuals will be squared + * again in error, yielding 0.5 \sum w(r)*r^2. + * + * In other words, while sigma is expressed in the native residual units, a parameter like + * k in the Huber norm is expressed in whitened units, i.e., "nr of sigmas". + */ class GTSAM_EXPORT Robust : public Base { public: typedef boost::shared_ptr shared_ptr; diff --git a/gtsam/linear/SubgraphPreconditioner.cpp b/gtsam/linear/SubgraphPreconditioner.cpp index ee2447d2f..ebe5bcc9b 100644 --- a/gtsam/linear/SubgraphPreconditioner.cpp +++ b/gtsam/linear/SubgraphPreconditioner.cpp @@ -508,9 +508,8 @@ Errors SubgraphPreconditioner::operator*(const VectorValues& y) const { void SubgraphPreconditioner::multiplyInPlace(const VectorValues& y, Errors& e) const { Errors::iterator ei = e.begin(); - for ( Key i = 0 ; i < y.size() ; ++i, ++ei ) { + for (size_t i = 0; i < y.size(); ++i, ++ei) *ei = y[i]; - } // Add A2 contribution VectorValues x = Rc1()->backSubstitute(y); // x=inv(R1)*y @@ -523,9 +522,9 @@ VectorValues SubgraphPreconditioner::operator^(const Errors& e) const { Errors::const_iterator it = e.begin(); VectorValues y = zero(); - for ( Key i = 0 ; i < y.size() ; ++i, ++it ) - y[i] = *it ; - transposeMultiplyAdd2(1.0,it,e.end(),y); + for (size_t i = 0; i < y.size(); ++i, ++it) + y[i] = *it; + transposeMultiplyAdd2(1.0, it, e.end(), y); return y; } @@ -535,7 +534,7 @@ void SubgraphPreconditioner::transposeMultiplyAdd (double alpha, const Errors& e, VectorValues& y) const { Errors::const_iterator it = e.begin(); - for ( Key i = 0 ; i < y.size() ; ++i, ++it ) { + for (size_t i = 0; i < y.size(); ++i, ++it) { const Vector& ei = *it; axpy(alpha, ei, y[i]); } diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index d04d9faac..0c752728a 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -135,7 +135,7 @@ namespace gtsam { /// @{ /** Number of variables stored. */ - Key size() const { return values_.size(); } + size_t size() const { return values_.size(); } /** Return the dimension of variable \c j. */ size_t dim(Key j) const { return at(j).rows(); } diff --git a/gtsam/linear/tests/testHessianFactor.cpp b/gtsam/linear/tests/testHessianFactor.cpp index 10fb34988..61083a926 100644 --- a/gtsam/linear/tests/testHessianFactor.cpp +++ b/gtsam/linear/tests/testHessianFactor.cpp @@ -314,22 +314,26 @@ TEST(HessianFactor, CombineAndEliminate1) { Ordering ordering = list_of(1); GaussianConditional::shared_ptr expectedConditional; JacobianFactor::shared_ptr expectedFactor; - boost::tie(expectedConditional, expectedFactor) = // - jacobian.eliminate(ordering); + boost::tie(expectedConditional, expectedFactor) = jacobian.eliminate(ordering); + CHECK(expectedFactor); // Eliminate GaussianConditional::shared_ptr actualConditional; HessianFactor::shared_ptr actualHessian; boost::tie(actualConditional, actualHessian) = // EliminateCholesky(gfg, ordering); + actualConditional->setModel(false,Vector3(1,1,1)); // add a unit model for comparison EXPECT(assert_equal(*expectedConditional, *actualConditional, 1e-6)); +#ifdef TEST_ERROR_EQUIVALENCE + // these tests disabled because QR cuts off the all zeros + error row EXPECT_DOUBLES_EQUAL(expectedFactor->error(v), actualHessian->error(v), 1e-9); EXPECT( assert_equal(expectedFactor->augmentedInformation(), actualHessian->augmentedInformation(), 1e-9)); EXPECT(assert_equal(HessianFactor(*expectedFactor), *actualHessian, 1e-6)); -} +#endif + } /* ************************************************************************* */ TEST(HessianFactor, CombineAndEliminate2) { @@ -381,8 +385,11 @@ TEST(HessianFactor, CombineAndEliminate2) { HessianFactor::shared_ptr actualHessian; boost::tie(actualConditional, actualHessian) = // EliminateCholesky(gfg, ordering); + actualConditional->setModel(false,Vector3(1,1,1)); // add a unit model for comparison EXPECT(assert_equal(*expectedConditional, *actualConditional, 1e-6)); +#ifdef TEST_ERROR_EQUIVALENCE + // these tests disabled because QR cuts off the all zeros + error row VectorValues v; v.insert(1, Vector3(1, 2, 3)); EXPECT_DOUBLES_EQUAL(expectedFactor->error(v), actualHessian->error(v), 1e-9); @@ -390,6 +397,7 @@ TEST(HessianFactor, CombineAndEliminate2) { assert_equal(expectedFactor->augmentedInformation(), actualHessian->augmentedInformation(), 1e-9)); EXPECT(assert_equal(HessianFactor(*expectedFactor), *actualHessian, 1e-6)); +#endif } /* ************************************************************************* */ diff --git a/gtsam/linear/tests/testJacobianFactor.cpp b/gtsam/linear/tests/testJacobianFactor.cpp index 17ceaf5f0..d57f896ef 100644 --- a/gtsam/linear/tests/testJacobianFactor.cpp +++ b/gtsam/linear/tests/testJacobianFactor.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -19,6 +19,7 @@ #include #include +#include #include #include #include @@ -161,37 +162,46 @@ TEST(JabobianFactor, Hessian_conversion) { EXPECT(assert_equal(expected, JacobianFactor(hessian), 1e-3)); } +/* ************************************************************************* */ +namespace simple_graph { + +Key keyX(10), keyY(8), keyZ(12); + +double sigma1 = 0.1; +Matrix A11 = Matrix::Identity(2, 2); +Vector2 b1(2, -1); +JacobianFactor factor1(keyX, A11, b1, noiseModel::Isotropic::Sigma(2, sigma1)); + +double sigma2 = 0.5; +Matrix A21 = -2 * Matrix::Identity(2, 2); +Matrix A22 = 3 * Matrix::Identity(2, 2); +Vector2 b2(4, -5); +JacobianFactor factor2(keyX, A21, keyY, A22, b2, noiseModel::Isotropic::Sigma(2, sigma2)); + +double sigma3 = 1.0; +Matrix A32 = -4 * Matrix::Identity(2, 2); +Matrix A33 = 5 * Matrix::Identity(2, 2); +Vector2 b3(3, -6); +JacobianFactor factor3(keyY, A32, keyZ, A33, b3, noiseModel::Isotropic::Sigma(2, sigma3)); + +GaussianFactorGraph factors(list_of(factor1)(factor2)(factor3)); +Ordering ordering(list_of(keyX)(keyY)(keyZ)); +} + /* ************************************************************************* */ TEST( JacobianFactor, construct_from_graph) { - GaussianFactorGraph factors; - - double sigma1 = 0.1; - Matrix A11 = Matrix::Identity(2,2); - Vector b1(2); b1 << 2, -1; - factors.add(JacobianFactor(10, A11, b1, noiseModel::Isotropic::Sigma(2, sigma1))); - - double sigma2 = 0.5; - Matrix A21 = -2 * Matrix::Identity(2,2); - Matrix A22 = 3 * Matrix::Identity(2,2); - Vector b2(2); b2 << 4, -5; - factors.add(JacobianFactor(10, A21, 8, A22, b2, noiseModel::Isotropic::Sigma(2, sigma2))); - - double sigma3 = 1.0; - Matrix A32 = -4 * Matrix::Identity(2,2); - Matrix A33 = 5 * Matrix::Identity(2,2); - Vector b3(2); b3 << 3, -6; - factors.add(JacobianFactor(8, A32, 12, A33, b3, noiseModel::Isotropic::Sigma(2, sigma3))); + using namespace simple_graph; Matrix A1(6,2); A1 << A11, A21, Matrix::Zero(2,2); Matrix A2(6,2); A2 << Matrix::Zero(2,2), A22, A32; Matrix A3(6,2); A3 << Matrix::Zero(4,2), A33; Vector b(6); b << b1, b2, b3; Vector sigmas(6); sigmas << sigma1, sigma1, sigma2, sigma2, sigma3, sigma3; - JacobianFactor expected(10, A1, 8, A2, 12, A3, b, noiseModel::Diagonal::Sigmas(sigmas)); + JacobianFactor expected(keyX, A1, keyY, A2, keyZ, A3, b, noiseModel::Diagonal::Sigmas(sigmas)); // The ordering here specifies the order in which the variables will appear in the combined factor - JacobianFactor actual(factors, Ordering(list_of(10)(8)(12))); + JacobianFactor actual(factors, ordering); EXPECT(assert_equal(expected, actual)); } @@ -466,7 +476,7 @@ TEST(JacobianFactor, eliminate2 ) +0.00,-0.20,+0.00,-0.80 ).finished()/oldSigma; Vector d = Vector2(0.2,-0.14)/oldSigma; - GaussianConditional expectedCG(2, d, R11, 11, S12); + GaussianConditional expectedCG(2, d, R11, 11, S12, noiseModel::Unit::Create(2)); EXPECT(assert_equal(expectedCG, *actual.first, 1e-4)); @@ -478,7 +488,7 @@ TEST(JacobianFactor, eliminate2 ) 0.00, 1.00, +0.00, -1.00 ).finished()/sigma; Vector b1 = Vector2(0.0, 0.894427); - JacobianFactor expectedLF(11, Bl1x1, b1); + JacobianFactor expectedLF(11, Bl1x1, b1, noiseModel::Unit::Create(2)); EXPECT(assert_equal(expectedLF, *actual.second,1e-3)); } @@ -516,7 +526,7 @@ TEST(JacobianFactor, EliminateQR) EXPECT(assert_equal(2.0 * Ab, actualDense)); // Expected augmented matrix, both GaussianConditional (first 6 rows) and remaining factor (next 4 rows) - Matrix R = 2.0 * (Matrix(11, 11) << + Matrix R = 2.0 * (Matrix(10, 11) << -12.1244, -5.1962, -5.2786, -8.6603, -10.5573, -5.9385, -11.3820, -7.2581, -8.7427, -13.4440, -5.3611, 0., 4.6904, 5.0254, 5.5432, 5.5737, 3.0153, -3.0153, -3.5635, -3.9290, -2.7412, 2.1625, 0., 0., -13.8160, -8.7166, -10.2245, -8.8666, -8.7632, -5.2544, -6.9192, -10.5537, -9.3250, @@ -526,24 +536,24 @@ TEST(JacobianFactor, EliminateQR) 0., 0., 0., 0., 0., 0., -11.1139, -0.6521, -2.1943, -7.5529, -0.9081, 0., 0., 0., 0., 0., 0., 0., -4.6479, -1.9367, -6.5170, -3.7685, 0., 0., 0., 0., 0., 0., 0., 0., 8.2503, 3.3757, 6.8476, - 0., 0., 0., 0., 0., 0., 0., 0., 0., -5.7095, -0.0090, - 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., -7.1635).finished(); + 0., 0., 0., 0., 0., 0., 0., 0., 0., -5.7095, -0.0090).finished(); - GaussianConditional expectedFragment( - list_of(3)(5)(7)(9)(11), 3, VerticalBlockMatrix(list_of(2)(2)(2)(2)(2)(1), R)); + GaussianConditional expectedFragment(list_of(3)(5)(7)(9)(11), 3, + VerticalBlockMatrix(list_of(2)(2)(2)(2)(2)(1), R.topRows(6)), + noiseModel::Unit::Create(6)); // Eliminate (3 frontal variables, 6 scalar columns) using QR !!!! GaussianFactorGraph::EliminationResult actual = EliminateQR(factors, list_of(3)(5)(7)); const JacobianFactor &actualJF = dynamic_cast(*actual.second); EXPECT(assert_equal(expectedFragment, *actual.first, 0.001)); - EXPECT(assert_equal(size_t(2), actualJF.keys().size())); + EXPECT_LONGS_EQUAL(size_t(2), actualJF.keys().size()); EXPECT(assert_equal(Key(9), actualJF.keys()[0])); EXPECT(assert_equal(Key(11), actualJF.keys()[1])); - EXPECT(assert_equal(Matrix(R.block(6, 6, 5, 2)), actualJF.getA(actualJF.begin()), 0.001)); - EXPECT(assert_equal(Matrix(R.block(6, 8, 5, 2)), actualJF.getA(actualJF.begin()+1), 0.001)); - EXPECT(assert_equal(Vector(R.col(10).segment(6, 5)), actualJF.getb(), 0.001)); - EXPECT(!actualJF.get_model()); + EXPECT(assert_equal(Matrix(R.block(6, 6, 4, 2)), actualJF.getA(actualJF.begin()), 0.001)); + EXPECT(assert_equal(Matrix(R.block(6, 8, 4, 2)), actualJF.getA(actualJF.begin()+1), 0.001)); + EXPECT(assert_equal(Vector(R.col(10).segment(6, 4)), actualJF.getb(), 0.001)); + EXPECT(assert_equal(*noiseModel::Diagonal::Sigmas(Vector4::Ones()), *actualJF.get_model(), 0.001)); } /* ************************************************************************* */ @@ -557,8 +567,8 @@ TEST ( JacobianFactor, constraint_eliminate1 ) pair actual = lc.eliminate(list_of(1)); - // verify linear factor - EXPECT(actual.second->size() == 0); + // verify linear factor is a null ptr + EXPECT(actual.second->empty()); // verify conditional Gaussian Vector sigmas = Vector2(0.0, 0.0); @@ -574,14 +584,10 @@ TEST ( JacobianFactor, constraint_eliminate2 ) Vector b(2); b(0)=3.0; b(1)=4.0; // A1 - invertible - Matrix A1(2,2); - A1(0,0) = 1.0 ; A1(0,1) = 2.0; - A1(1,0) = 2.0 ; A1(1,1) = 1.0; + Matrix2 A1; A1 << 2,4, 2,1; // A2 - not invertible - Matrix A2(2,2); - A2(0,0) = 1.0 ; A2(0,1) = 2.0; - A2(1,0) = 2.0 ; A2(1,1) = 4.0; + Matrix2 A2; A2 << 2,4, 2,4; JacobianFactor lc(1, A1, 2, A2, b, noiseModel::Constrained::All(2)); @@ -598,18 +604,38 @@ TEST ( JacobianFactor, constraint_eliminate2 ) EXPECT(assert_equal(expectedLF, *actual.second)); // verify CG - Matrix R = (Matrix(2, 2) << - 1.0, 2.0, - 0.0, 1.0).finished(); - Matrix S = (Matrix(2, 2) << - 1.0, 2.0, - 0.0, 0.0).finished(); + Matrix2 R; R << 1,2, 0,1; + Matrix2 S; S << 1,2, 0,0; Vector d = Vector2(3.0, 0.6666); Vector sigmas = Vector2(0.0, 0.0); GaussianConditional expectedCG(1, d, R, 2, S, noiseModel::Diagonal::Sigmas(sigmas)); EXPECT(assert_equal(expectedCG, *actual.first, 1e-4)); } +/* ************************************************************************* */ +TEST(JacobianFactor, OverdeterminedEliminate) { + Matrix Ab(9, 4); + Ab << 0, 1, 0, 0, // + 0, 0, 1, 0, // + Matrix74::Ones(); + + // Call Gaussian version + Vector9 sigmas = Vector9::Ones(); + SharedDiagonal diagonal = noiseModel::Diagonal::Sigmas(sigmas); + + JacobianFactor factor(0, Ab.leftCols(3), Ab.col(3), diagonal); + GaussianFactorGraph::EliminationResult actual = factor.eliminate(list_of(0)); + + Matrix expectedRd(3, 4); + expectedRd << -2.64575131, -2.64575131, -2.64575131, -2.64575131, // + 0.0, -1, 0, 0, // + 0.0, 0.0, -1, 0; + GaussianConditional expectedConditional(0, expectedRd.col(3), expectedRd.leftCols(3), + noiseModel::Unit::Create(3)); + EXPECT(assert_equal(expectedConditional, *actual.first, 1e-4)); + EXPECT(actual.second->empty()); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */ diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index d6857c6ff..7ac4846f9 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -222,45 +222,172 @@ namespace exampleQR { SharedDiagonal diagonal = noiseModel::Diagonal::Sigmas(sigmas); } +/* ************************************************************************* */ TEST( NoiseModel, QR ) { Matrix Ab1 = exampleQR::Ab; Matrix Ab2 = exampleQR::Ab; // otherwise overwritten ! - // Expected result - Vector expectedSigmas = (Vector(4) << 0.0894427, 0.0894427, 0.223607, 0.223607).finished(); - SharedDiagonal expectedModel = noiseModel::Diagonal::Sigmas(expectedSigmas); - // Call Gaussian version SharedDiagonal actual1 = exampleQR::diagonal->QR(Ab1); - EXPECT(!actual1); + EXPECT(actual1->isUnit()); EXPECT(linear_dependent(exampleQR::Rd,Ab1,1e-4)); // Ab was modified in place !!! - // Call Constrained version - SharedDiagonal constrained = noiseModel::Constrained::MixedSigmas(exampleQR::sigmas); - SharedDiagonal actual2 = constrained->QR(Ab2); - SharedDiagonal expectedModel2 = noiseModel::Diagonal::Sigmas(expectedSigmas); - EXPECT(assert_equal(*expectedModel2,*actual2,1e-6)); + // Expected result for constrained version + Vector expectedSigmas = (Vector(4) << 0.0894427, 0.0894427, 0.223607, 0.223607).finished(); + SharedDiagonal expectedModel = noiseModel::Diagonal::Sigmas(expectedSigmas); Matrix expectedRd2 = (Matrix(4, 7) << 1., 0., -0.2, 0., -0.8, 0., 0.2, 0., 1., 0.,-0.2, 0., -0.8,-0.14, 0., 0., 1., 0., -1., 0., 0.0, 0., 0., 0., 1., 0., -1., 0.2).finished(); - EXPECT(linear_dependent(expectedRd2,Ab2,1e-6)); // Ab was modified in place !!! + + // Call Constrained version + SharedDiagonal constrained = noiseModel::Constrained::MixedSigmas(exampleQR::sigmas); + SharedDiagonal actual2 = constrained->QR(Ab2); + EXPECT(assert_equal(*expectedModel, *actual2, 1e-6)); + EXPECT(linear_dependent(expectedRd2, Ab2, 1e-6)); // Ab was modified in place !!! } /* ************************************************************************* */ +TEST(NoiseModel, OverdeterminedQR) { + Matrix Ab1(9, 4); + Ab1 << 0, 1, 0, 0, // + 0, 0, 1, 0, // + Matrix74::Ones(); + Matrix Ab2 = Ab1; // otherwise overwritten ! + + // Call Gaussian version + Vector9 sigmas = Vector9::Ones() ; + SharedDiagonal diagonal = noiseModel::Diagonal::Sigmas(sigmas); + SharedDiagonal actual1 = diagonal->QR(Ab1); + EXPECT(actual1->isUnit()); + Matrix expectedRd(9,4); + expectedRd << -2.64575131, -2.64575131, -2.64575131, -2.64575131, // + 0.0, -1, 0, 0, // + 0.0, 0.0, -1, 0, // + Matrix64::Zero(); + EXPECT(assert_equal(expectedRd, Ab1, 1e-4)); // Ab was modified in place !!! + + // Expected result for constrained version + Vector3 expectedSigmas(0.377964473, 1, 1); + SharedDiagonal expectedModel = noiseModel::Diagonal::Sigmas(expectedSigmas); + + // Call Constrained version + SharedDiagonal constrained = noiseModel::Constrained::MixedSigmas(sigmas); + SharedDiagonal actual2 = constrained->QR(Ab2); + EXPECT(assert_equal(*expectedModel, *actual2, 1e-6)); + expectedRd.row(0) *= 0.377964473; // not divided by sigma! + EXPECT(assert_equal(-expectedRd, Ab2, 1e-6)); // Ab was modified in place !!! +} + +/* ************************************************************************* */ +TEST( NoiseModel, MixedQR ) +{ + // Call Constrained version, with first and third row treated as constraints + // Naming the 6 variables u,v,w,x,y,z, we have + // u = -z + // w = -x + // And let's have simple priors on variables + Matrix Ab(5,6+1); + Ab << + 1,0,0,0,0,1, 0, // u+z = 0 + 0,0,0,0,1,0, 0, // y^2 + 0,0,1,1,0,0, 0, // w+x = 0 + 0,1,0,0,0,0, 0, // v^2 + 0,0,0,0,0,1, 0; // z^2 + Vector mixed_sigmas = (Vector(5) << 0, 1, 0, 1, 1).finished(); + SharedDiagonal constrained = noiseModel::Constrained::MixedSigmas(mixed_sigmas); + + // Expected result + Vector expectedSigmas = (Vector(5) << 0, 1, 0, 1, 1).finished(); + SharedDiagonal expectedModel = noiseModel::Diagonal::Sigmas(expectedSigmas); + Matrix expectedRd(5, 6+1); + expectedRd << 1, 0, 0, 0, 0, 1, 0, // + 0, 1, 0, 0, 0, 0, 0, // + 0, 0, 1, 1, 0, 0, 0, // + 0, 0, 0, 0, 1, 0, 0, // + 0, 0, 0, 0, 0, 1, 0; // + + SharedDiagonal actual = constrained->QR(Ab); + EXPECT(assert_equal(*expectedModel,*actual,1e-6)); + EXPECT(linear_dependent(expectedRd,Ab,1e-6)); // Ab was modified in place !!! +} + +/* ************************************************************************* */ +TEST( NoiseModel, MixedQR2 ) +{ + // Let's have three variables x,y,z, but x=z and y=z + // Hence, all non-constraints are really measurements on z + Matrix Ab(11,3+1); + Ab << + 1,0,0, 0, // + 0,1,0, 0, // + 0,0,1, 0, // + -1,0,1, 0, // x=z + 1,0,0, 0, // + 0,1,0, 0, // + 0,0,1, 0, // + 0,-1,1, 0, // y=z + 1,0,0, 0, // + 0,1,0, 0, // + 0,0,1, 0; // + + Vector sigmas(11); + sigmas.setOnes(); + sigmas[3] = 0; + sigmas[7] = 0; + SharedDiagonal constrained = noiseModel::Constrained::MixedSigmas(sigmas); + + // Expected result + Vector3 expectedSigmas(0,0,1.0/3); + SharedDiagonal expectedModel = noiseModel::Constrained::MixedSigmas(expectedSigmas); + Matrix expectedRd(11, 3+1); + expectedRd.setZero(); + expectedRd.row(0) << -1, 0, 1, 0; // x=z + expectedRd.row(1) << 0, -1, 1, 0; // y=z + expectedRd.row(2) << 0, 0, 1, 0; // z=0 +/- 1/3 + + SharedDiagonal actual = constrained->QR(Ab); + EXPECT(assert_equal(*expectedModel,*actual,1e-6)); + EXPECT(assert_equal(expectedRd,Ab,1e-6)); // Ab was modified in place !!! +} + +/* ************************************************************************* */ +TEST( NoiseModel, FullyConstrained ) +{ + Matrix Ab(3,7); + Ab << + 1,0,0,0,0,1, 2, // u+z = 2 + 0,0,1,1,0,0, 4, // w+x = 4 + 0,1,0,1,1,1, 8; // v+x+y+z=8 + SharedDiagonal constrained = noiseModel::Constrained::All(3); + + // Expected result + SharedDiagonal expectedModel = noiseModel::Diagonal::Sigmas(Vector3 (0,0,0)); + Matrix expectedRd(3, 7); + expectedRd << 1, 0, 0, 0, 0, 1, 2, // + 0, 1, 0, 1, 1, 1, 8, // + 0, 0, 1, 1, 0, 0, 4; // + + SharedDiagonal actual = constrained->QR(Ab); + EXPECT(assert_equal(*expectedModel,*actual,1e-6)); + EXPECT(linear_dependent(expectedRd,Ab,1e-6)); // Ab was modified in place !!! +} + +/* ************************************************************************* */ +// This matches constraint_eliminate2 in testJacobianFactor TEST(NoiseModel, QRNan ) { SharedDiagonal constrained = noiseModel::Constrained::All(2); - Matrix Ab = (Matrix(2, 5) << 1., 2., 1., 2., 3., 2., 1., 2., 4., 4.).finished(); + Matrix Ab = (Matrix25() << 2, 4, 2, 4, 6, 2, 1, 2, 4, 4).finished(); SharedDiagonal expected = noiseModel::Constrained::All(2); - Matrix expectedAb = (Matrix(2, 5) << 1., 2., 1., 2., 3., 0., 1., 0., 0., 2.0/3).finished(); + Matrix expectedAb = (Matrix25() << 1, 2, 1, 2, 3, 0, 1, 0, 0, 2.0/3).finished(); SharedDiagonal actual = constrained->QR(Ab); EXPECT(assert_equal(*expected,*actual)); - EXPECT(assert_equal(expectedAb,Ab)); + EXPECT(linear_dependent(expectedAb,Ab)); } /* ************************************************************************* */ diff --git a/gtsam/navigation/GPSFactor.cpp b/gtsam/navigation/GPSFactor.cpp index b2cb5ed82..782c98148 100644 --- a/gtsam/navigation/GPSFactor.cpp +++ b/gtsam/navigation/GPSFactor.cpp @@ -25,7 +25,7 @@ namespace gtsam { //*************************************************************************** void GPSFactor::print(const string& s, const KeyFormatter& keyFormatter) const { cout << s << "GPSFactor on " << keyFormatter(key()) << "\n"; - cout << " prior mean: " << nT_ << "\n"; + cout << " GPS measurement: " << nT_ << "\n"; noiseModel_->print(" noise model: "); } @@ -38,12 +38,7 @@ bool GPSFactor::equals(const NonlinearFactor& expected, double tol) const { //*************************************************************************** Vector GPSFactor::evaluateError(const Pose3& p, boost::optional H) const { - if (H) { - H->resize(3, 6); - H->block < 3, 3 > (0, 0) << zeros(3, 3); - H->block < 3, 3 > (0, 3) << p.rotation().matrix(); - } - return p.translation() -nT_; + return p.translation(H) -nT_; } //*************************************************************************** @@ -68,6 +63,25 @@ pair GPSFactor::EstimateState(double t1, const Point3& NED1, return make_pair(nTb, nV); } +//*************************************************************************** +void GPSFactor2::print(const string& s, const KeyFormatter& keyFormatter) const { + cout << s << "GPSFactor2 on " << keyFormatter(key()) << "\n"; + nT_.print(" GPS measurement: "); + noiseModel_->print(" noise model: "); +} + +//*************************************************************************** +bool GPSFactor2::equals(const NonlinearFactor& expected, double tol) const { + const This* e = dynamic_cast(&expected); + return e != NULL && Base::equals(*e, tol) && nT_.equals(e->nT_, tol); +} + +//*************************************************************************** +Vector GPSFactor2::evaluateError(const NavState& p, + boost::optional H) const { + return p.position(H).vector() -nT_.vector(); +} + //*************************************************************************** }/// namespace gtsam diff --git a/gtsam/navigation/GPSFactor.h b/gtsam/navigation/GPSFactor.h index 46e194460..2b5ea1f2f 100644 --- a/gtsam/navigation/GPSFactor.h +++ b/gtsam/navigation/GPSFactor.h @@ -18,6 +18,7 @@ #pragma once #include +#include #include namespace gtsam { @@ -37,7 +38,7 @@ private: typedef NoiseModelFactor1 Base; - Point3 nT_; ///< Position measurement in + Point3 nT_; ///< Position measurement in cartesian coordinates public: @@ -50,14 +51,13 @@ public: /** default constructor - only use for serialization */ GPSFactor(): nT_(0, 0, 0) {} - virtual ~GPSFactor() { - } + virtual ~GPSFactor() {} /** * @brief Constructor from a measurement in a Cartesian frame. * Use GeographicLib to convert from geographic (latitude and longitude) coordinates * @param key of the Pose3 variable that will be constrained - * @param gpsIn measurement already in coordinates + * @param gpsIn measurement already in correct coordinates * @param model Gaussian noise model */ GPSFactor(Key key, const Point3& gpsIn, const SharedNoiseModel& model) : @@ -70,18 +70,14 @@ public: gtsam::NonlinearFactor::shared_ptr(new This(*this))); } - /** implement functions needed for Testable */ - - /** print */ + /// print virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; - /** equals */ + /// equals virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const; - /** implement functions needed to derive from Factor */ - - /** vector of errors */ + /// vector of errors Vector evaluateError(const Pose3& p, boost::optional H = boost::none) const; @@ -89,8 +85,8 @@ public: return nT_; } - /* - * Convenience funcion to estimate state at time t, given two GPS + /** + * Convenience function to estimate state at time t, given two GPS * readings (in local NED Cartesian frame) bracketing t * Assumes roll is zero, calculates yaw and pitch from NED1->NED2 vector. */ @@ -99,7 +95,71 @@ public: private: - /** Serialization function */ + /// Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + ar + & boost::serialization::make_nvp("NoiseModelFactor1", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(nT_); + } +}; + +/** + * Version of GPSFactor for NavState + * @addtogroup Navigation + */ +class GTSAM_EXPORT GPSFactor2: public NoiseModelFactor1 { + +private: + + typedef NoiseModelFactor1 Base; + + Point3 nT_; ///< Position measurement in cartesian coordinates + +public: + + /// shorthand for a smart pointer to a factor + typedef boost::shared_ptr shared_ptr; + + /// Typedef to this class + typedef GPSFactor2 This; + + /// default constructor - only use for serialization + GPSFactor2():nT_(0, 0, 0) {} + + virtual ~GPSFactor2() {} + + /// Constructor from a measurement in a Cartesian frame. + GPSFactor2(Key key, const Point3& gpsIn, const SharedNoiseModel& model) : + Base(model, key), nT_(gpsIn) { + } + + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } + + /// print + virtual void print(const std::string& s, const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const; + + /// equals + virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const; + + /// vector of errors + Vector evaluateError(const NavState& p, + boost::optional H = boost::none) const; + + inline const Point3 & measurementIn() const { + return nT_; + } + +private: + + /// Serialization function friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { diff --git a/gtsam/navigation/ImuBias.h b/gtsam/navigation/ImuBias.h index 11799f310..2ad3c17dd 100644 --- a/gtsam/navigation/ImuBias.h +++ b/gtsam/navigation/ImuBias.h @@ -44,7 +44,7 @@ public: biasAcc_(biasAcc), biasGyro_(biasGyro) { } - ConstantBias(const Vector6& v) : + explicit ConstantBias(const Vector6& v) : biasAcc_(v.head<3>()), biasGyro_(v.tail<3>()) { } @@ -114,6 +114,11 @@ public: return ConstantBias(-biasAcc_, -biasGyro_); } + /** addition of vector on right */ + ConstantBias operator+(const Vector6& v) const { + return ConstantBias(biasAcc_ + v.head<3>(), biasGyro_ + v.tail<3>()); + } + /** addition */ ConstantBias operator+(const ConstantBias& b) const { return ConstantBias(biasAcc_ + b.biasAcc_, biasGyro_ + b.biasGyro_); diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index 145359d91..6d36539e4 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -72,9 +72,30 @@ void PreintegratedImuMeasurements::integrateMeasurement( preintMeasCov_.noalias() += B * (aCov / dt) * B.transpose(); preintMeasCov_.noalias() += C * (wCov / dt) * C.transpose(); - // NOTE(frank): (Gi*dt)*(C/dt)*(Gi'*dt) - static const Matrix93 Gi = (Matrix93() << Z_3x3, I_3x3, Z_3x3).finished(); - preintMeasCov_.noalias() += Gi * (iCov * dt) * Gi.transpose(); + // NOTE(frank): (Gi*dt)*(C/dt)*(Gi'*dt), with Gi << Z_3x3, I_3x3, Z_3x3 + preintMeasCov_.block<3,3>(3,3).noalias() += iCov * dt; +} + +//------------------------------------------------------------------------------ +void PreintegratedImuMeasurements::integrateMeasurements(const Matrix& measuredAccs, + const Matrix& measuredOmegas, + const Matrix& dts) { + assert(measuredAccs.rows() == 3 && measuredOmegas.rows() == 3 && dts.rows() == 1); + assert(dts.cols() >= 1); + assert(measuredAccs.cols() == dts.cols()); + assert(measuredOmegas.cols() == dts.cols()); + size_t n = static_cast(dts.cols()); + for (size_t j = 0; j < n; j++) { + integrateMeasurement(measuredAccs.col(j), measuredOmegas.col(j), dts(0,j)); + } +} + +//------------------------------------------------------------------------------ +void PreintegratedImuMeasurements::mergeWith(const PreintegratedImuMeasurements& pim12, // + Matrix9* H1, Matrix9* H2) { + PreintegrationBase::mergeWith(pim12, H1, H2); + preintMeasCov_ = + *H1 * preintMeasCov_ * H1->transpose() + *H2 * pim12.preintMeasCov_ * H2->transpose(); } //------------------------------------------------------------------------------ @@ -120,9 +141,7 @@ NonlinearFactor::shared_ptr ImuFactor::clone() const { //------------------------------------------------------------------------------ std::ostream& operator<<(std::ostream& os, const ImuFactor& f) { - os << " preintegrated measurements:\n" << f._PIM_; - ; - // Print standard deviations on covariance only + f._PIM_.print("preintegrated measurements:\n"); os << " noise model sigmas: " << f.noiseModel_->sigmas().transpose(); return os; } @@ -172,9 +191,6 @@ PreintegratedImuMeasurements ImuFactor::Merge( Matrix9 H1, H2; pim02.mergeWith(pim12, &H1, &H2); - pim02.preintMeasCov_ = H1 * pim01.preintMeasCov_ * H1.transpose() + - H2 * pim12.preintMeasCov_ * H2.transpose(); - return pim02; } @@ -229,6 +245,50 @@ void ImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i, vel_j = pvb.velocity; } #endif +//------------------------------------------------------------------------------ +// ImuFactor2 methods +//------------------------------------------------------------------------------ +ImuFactor2::ImuFactor2(Key state_i, Key state_j, Key bias, const PreintegratedImuMeasurements& pim) + : Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), state_i, state_j, bias), + _PIM_(pim) {} + +//------------------------------------------------------------------------------ +NonlinearFactor::shared_ptr ImuFactor2::clone() const { + return boost::static_pointer_cast( + NonlinearFactor::shared_ptr(new This(*this))); +} + +//------------------------------------------------------------------------------ +std::ostream& operator<<(std::ostream& os, const ImuFactor2& f) { + f._PIM_.print("preintegrated measurements:\n"); + os << " noise model sigmas: " << f.noiseModel_->sigmas().transpose(); + return os; +} + +//------------------------------------------------------------------------------ +void ImuFactor2::print(const string& s, const KeyFormatter& keyFormatter) const { + cout << s << "ImuFactor2(" << keyFormatter(this->key1()) << "," << keyFormatter(this->key2()) + << "," << keyFormatter(this->key3()) << ")\n"; + cout << *this << endl; +} + +//------------------------------------------------------------------------------ +bool ImuFactor2::equals(const NonlinearFactor& other, double tol) const { + const This *e = dynamic_cast(&other); + const bool base = Base::equals(*e, tol); + const bool pim = _PIM_.equals(e->_PIM_, tol); + return e != nullptr && base && pim; +} + +//------------------------------------------------------------------------------ +Vector ImuFactor2::evaluateError(const NavState& state_i, const NavState& state_j, + const imuBias::ConstantBias& bias_i, // + boost::optional H1, + boost::optional H2, + boost::optional H3) const { + return _PIM_.computeError(state_i, state_j, bias_i, H1, H2, H3); +} + //------------------------------------------------------------------------------ } diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index 73a2f05d2..010550eb1 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -64,6 +64,7 @@ namespace gtsam { class PreintegratedImuMeasurements: public PreintegrationBase { friend class ImuFactor; + friend class ImuFactor2; protected: @@ -114,12 +115,18 @@ public: * @param measuredOmega Measured angular velocity (as given by the sensor) * @param dt Time interval between this and the last IMU measurement */ - void integrateMeasurement(const Vector3& measuredAcc, - const Vector3& measuredOmega, double dt); + void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, double dt); + + /// Add multiple measurements, in matrix columns + void integrateMeasurements(const Matrix& measuredAccs, const Matrix& measuredOmegas, + const Matrix& dts); /// Return pre-integrated measurement covariance Matrix preintMeasCov() const { return preintMeasCov_; } + /// Merge in a different set of measurements and update bias derivatives accordingly + void mergeWith(const PreintegratedImuMeasurements& pim, Matrix9* H1, Matrix9* H2); + #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 /// @deprecated constructor /// NOTE(frank): assumes Z-Down convention, only second order integration supported @@ -162,8 +169,6 @@ private: */ class ImuFactor: public NoiseModelFactor5 { -public: - private: typedef ImuFactor This; @@ -220,7 +225,7 @@ public: /// vector of errors Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, - const imuBias::ConstantBias& bias, boost::optional H1 = + const imuBias::ConstantBias& bias_i, boost::optional H1 = boost::none, boost::optional H2 = boost::none, boost::optional H3 = boost::none, boost::optional H4 = boost::none, boost::optional H5 = boost::none) const; @@ -265,10 +270,81 @@ private: }; // class ImuFactor +/** + * ImuFactor2 is a ternary factor that uses NavStates rather than Pose/Velocity. + * @addtogroup SLAM + */ +class ImuFactor2 : public NoiseModelFactor3 { +private: + + typedef ImuFactor2 This; + typedef NoiseModelFactor3 Base; + + PreintegratedImuMeasurements _PIM_; + +public: + + /** Default constructor - only use for serialization */ + ImuFactor2() {} + + /** + * Constructor + * @param state_i Previous state key + * @param state_j Current state key + * @param bias Previous bias key + */ + ImuFactor2(Key state_i, Key state_j, Key bias, + const PreintegratedImuMeasurements& preintegratedMeasurements); + + virtual ~ImuFactor2() { + } + + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const; + + /// @name Testable + /// @{ + GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const ImuFactor2&); + virtual void print(const std::string& s, const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const; + virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const; + /// @} + + /** Access the preintegrated measurements. */ + + const PreintegratedImuMeasurements& preintegratedMeasurements() const { + return _PIM_; + } + + /** implement functions needed to derive from Factor */ + + /// vector of errors + Vector evaluateError(const NavState& state_i, const NavState& state_j, + const imuBias::ConstantBias& bias_i, // + boost::optional H1 = boost::none, + boost::optional H2 = boost::none, + boost::optional H3 = boost::none) const; + +private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + ar & boost::serialization::make_nvp("NoiseModelFactor3", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(_PIM_); + } +}; +// class ImuFactor2 + template <> struct traits : public Testable {}; template <> struct traits : public Testable {}; +template <> +struct traits : public Testable {}; + } /// namespace gtsam diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index 7e5d85cde..f01a55577 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -239,6 +239,7 @@ Matrix7 NavState::wedge(const Vector9& xi) { #define D_v_v(H) (H)->block<3,3>(6,6) //------------------------------------------------------------------------------ +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 NavState NavState::update(const Vector3& b_acceleration, const Vector3& b_omega, const double dt, OptionalJacobian<9, 9> F, OptionalJacobian<9, 3> G1, OptionalJacobian<9, 3> G2) const { @@ -281,6 +282,7 @@ NavState NavState::update(const Vector3& b_acceleration, const Vector3& b_omega, } return newState; } +#endif //------------------------------------------------------------------------------ Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder, diff --git a/gtsam/navigation/NavState.h b/gtsam/navigation/NavState.h index 02da46317..eabd1f39b 100644 --- a/gtsam/navigation/NavState.h +++ b/gtsam/navigation/NavState.h @@ -203,11 +203,13 @@ public: /// @name Dynamics /// @{ +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 /// Integrate forward in time given angular velocity and acceleration in body frame /// Uses second order integration for position, returns derivatives except dt. NavState update(const Vector3& b_acceleration, const Vector3& b_omega, const double dt, OptionalJacobian<9, 9> F, OptionalJacobian<9, 3> G1, OptionalJacobian<9, 3> G2) const; +#endif /// Compute tangent space contribution due to Coriolis forces Vector9 coriolis(double dt, const Vector3& omega, bool secondOrder = false, diff --git a/gtsam/navigation/PreintegratedRotation.cpp b/gtsam/navigation/PreintegratedRotation.cpp index df38df0b8..ec91d69b8 100644 --- a/gtsam/navigation/PreintegratedRotation.cpp +++ b/gtsam/navigation/PreintegratedRotation.cpp @@ -63,7 +63,8 @@ void PreintegratedRotation::print(const string& s) const { bool PreintegratedRotation::equals(const PreintegratedRotation& other, double tol) const { - return p_->equals(*other.p_,tol) && deltaRij_.equals(other.deltaRij_, tol) + return this->matchesParamsWith(other) + && deltaRij_.equals(other.deltaRij_, tol) && fabs(deltaTij_ - other.deltaTij_) < tol && equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol); } diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index 7209391f1..fefd4b23c 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -61,8 +61,8 @@ void PreintegrationBase::print(const string& s) const { //------------------------------------------------------------------------------ bool PreintegrationBase::equals(const PreintegrationBase& other, double tol) const { - const bool params_match = p_->equals(*other.p_, tol); - return params_match && fabs(deltaTij_ - other.deltaTij_) < tol + return p_->equals(*other.p_, tol) + && fabs(deltaTij_ - other.deltaTij_) < tol && biasHat_.equals(other.biasHat_, tol) && equal_with_abs_tol(preintegrated_, other.preintegrated_, tol) && equal_with_abs_tol(preintegrated_H_biasAcc_, other.preintegrated_H_biasAcc_, tol) @@ -72,9 +72,9 @@ bool PreintegrationBase::equals(const PreintegrationBase& other, //------------------------------------------------------------------------------ pair PreintegrationBase::correctMeasurementsBySensorPose( const Vector3& unbiasedAcc, const Vector3& unbiasedOmega, - OptionalJacobian<3, 3> D_correctedAcc_unbiasedAcc, - OptionalJacobian<3, 3> D_correctedAcc_unbiasedOmega, - OptionalJacobian<3, 3> D_correctedOmega_unbiasedOmega) const { + OptionalJacobian<3, 3> correctedAcc_H_unbiasedAcc, + OptionalJacobian<3, 3> correctedAcc_H_unbiasedOmega, + OptionalJacobian<3, 3> correctedOmega_H_unbiasedOmega) const { assert(p().body_P_sensor); // Compensate for sensor-body displacement if needed: we express the quantities @@ -89,9 +89,9 @@ pair PreintegrationBase::correctMeasurementsBySensorPose( const Vector3 correctedOmega = bRs * unbiasedOmega; // Jacobians - if (D_correctedAcc_unbiasedAcc) *D_correctedAcc_unbiasedAcc = bRs; - if (D_correctedAcc_unbiasedOmega) *D_correctedAcc_unbiasedOmega = Z_3x3; - if (D_correctedOmega_unbiasedOmega) *D_correctedOmega_unbiasedOmega = bRs; + if (correctedAcc_H_unbiasedAcc) *correctedAcc_H_unbiasedAcc = bRs; + if (correctedAcc_H_unbiasedOmega) *correctedAcc_H_unbiasedOmega = Z_3x3; + if (correctedOmega_H_unbiasedOmega) *correctedOmega_H_unbiasedOmega = bRs; // Centrifugal acceleration const Vector3 b_arm = p().body_P_sensor->translation(); @@ -103,9 +103,9 @@ pair PreintegrationBase::correctMeasurementsBySensorPose( correctedAcc -= body_Omega_body * b_velocity_bs; // Update derivative: centrifugal causes the correlation between acc and omega!!! - if (D_correctedAcc_unbiasedOmega) { + if (correctedAcc_H_unbiasedOmega) { double wdp = correctedOmega.dot(b_arm); - *D_correctedAcc_unbiasedOmega = -(diag(Vector3::Constant(wdp)) + *correctedAcc_H_unbiasedOmega = -(diag(Vector3::Constant(wdp)) + correctedOmega * b_arm.transpose()) * bRs.matrix() + 2 * b_arm * unbiasedOmega.transpose(); } @@ -366,27 +366,26 @@ Vector9 PreintegrationBase::Compose(const Vector9& zeta01, //------------------------------------------------------------------------------ void PreintegrationBase::mergeWith(const PreintegrationBase& pim12, Matrix9* H1, Matrix9* H2) { - if (!matchesParamsWith(pim12)) - throw std::domain_error( - "Cannot merge pre-integrated measurements with different params"); + if (!matchesParamsWith(pim12)) { + throw std::domain_error("Cannot merge pre-integrated measurements with different params"); + } - if (params()->body_P_sensor) - throw std::domain_error( - "Cannot merge pre-integrated measurements with sensor pose yet"); + if (params()->body_P_sensor) { + throw std::domain_error("Cannot merge pre-integrated measurements with sensor pose yet"); + } - const double& t01 = deltaTij(); - const double& t12 = pim12.deltaTij(); + const double t01 = deltaTij(); + const double t12 = pim12.deltaTij(); deltaTij_ = t01 + t12; - Vector9 zeta01 = preintegrated(); - Vector9 zeta12 = pim12.preintegrated(); + const Vector9 zeta01 = preintegrated(); + Vector9 zeta12 = pim12.preintegrated(); // will be modified. - // TODO(frank): adjust zeta12 due to bias difference const imuBias::ConstantBias bias_incr_for_12 = biasHat() - pim12.biasHat(); zeta12 += pim12.preintegrated_H_biasOmega_ * bias_incr_for_12.gyroscope() + pim12.preintegrated_H_biasAcc_ * bias_incr_for_12.accelerometer(); - preintegrated_ << PreintegrationBase::Compose(zeta01, zeta12, t12, H1, H2); + preintegrated_ = PreintegrationBase::Compose(zeta01, zeta12, t12, H1, H2); preintegrated_H_biasAcc_ = (*H1) * preintegrated_H_biasAcc_ + (*H2) * pim12.preintegrated_H_biasAcc_; diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index f8639cf79..9d751e92d 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -77,6 +77,7 @@ class PreintegrationBase { /** * Preintegrated navigation state, from frame i to frame j + * Order is: theta, position, velocity * Note: relative position does not take into account velocity at time i, see deltap+, in [2] * Note: velocity is now also in frame i, as opposed to deltaVij in [2] */ @@ -94,6 +95,9 @@ public: /// @name Constructors /// @{ + /// @name Constructors + /// @{ + /** * Constructor, initializes the variables in the base class * @param p Parameters, typically fixed in a single application @@ -111,7 +115,7 @@ public: /// check parameters equality: checks whether shared pointer points to same Params object. bool matchesParamsWith(const PreintegrationBase& other) const { - return p_ == other.p_; + return p_.get() == other.p_.get(); } /// shared pointer to params @@ -134,7 +138,7 @@ public: /// @name Instance variables access /// @{ const imuBias::ConstantBias& biasHat() const { return biasHat_; } - const double& deltaTij() const { return deltaTij_; } + double deltaTij() const { return deltaTij_; } const Vector9& preintegrated() const { return preintegrated_; } @@ -167,9 +171,9 @@ public: /// Ignore D_correctedOmega_measuredAcc as it is trivially zero std::pair correctMeasurementsBySensorPose( const Vector3& unbiasedAcc, const Vector3& unbiasedOmega, - OptionalJacobian<3, 3> D_correctedAcc_unbiasedAcc = boost::none, - OptionalJacobian<3, 3> D_correctedAcc_unbiasedOmega = boost::none, - OptionalJacobian<3, 3> D_correctedOmega_unbiasedOmega = boost::none) const; + OptionalJacobian<3, 3> correctedAcc_H_unbiasedAcc = boost::none, + OptionalJacobian<3, 3> correctedAcc_H_unbiasedOmega = boost::none, + OptionalJacobian<3, 3> correctedOmega_H_unbiasedOmega = boost::none) const; // Update integrated vector on tangent manifold preintegrated with acceleration // Static, functional version. @@ -215,7 +219,7 @@ public: OptionalJacobian<9, 6> H3 = boost::none, OptionalJacobian<9, 3> H4 = boost::none, OptionalJacobian<9, 6> H5 = boost::none) const; - // Compose the two preintegrated vectors + // Compose the two pre-integrated 9D-vectors zeta01 and zeta02, with derivatives static Vector9 Compose(const Vector9& zeta01, const Vector9& zeta12, double deltaT12, OptionalJacobian<9, 9> H1 = boost::none, @@ -244,6 +248,8 @@ public: /// @} #endif + /// @} + private: /** Serialization function */ friend class boost::serialization::access; diff --git a/gtsam/navigation/tests/imuFactorTesting.h b/gtsam/navigation/tests/imuFactorTesting.h index 3247b56ee..5aa83e87e 100644 --- a/gtsam/navigation/tests/imuFactorTesting.h +++ b/gtsam/navigation/tests/imuFactorTesting.h @@ -28,8 +28,8 @@ using symbol_shorthand::X; using symbol_shorthand::V; using symbol_shorthand::B; +static const Vector3 kZero = Z_3x1; typedef imuBias::ConstantBias Bias; -static const Vector3 Z_3x1 = Vector3::Zero(); static const Bias kZeroBiasHat, kZeroBias; static const Vector3 kZeroOmegaCoriolis(0, 0, 0); diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index 743fc9baf..fa2e9d832 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -136,20 +136,19 @@ TEST(CombinedImuFactor, FirstOrderPreIntegratedMeasurements) { auto p = testing::Params(); testing::SomeMeasurements measurements; - boost::function preintegrated = - [=](const Vector3& a, const Vector3& w) { - PreintegratedImuMeasurements pim(p, Bias(a, w)); - testing::integrateMeasurements(measurements, &pim); - return pim.preintegrated(); - }; + auto preintegrated = [=](const Vector3& a, const Vector3& w) { + PreintegratedImuMeasurements pim(p, Bias(a, w)); + testing::integrateMeasurements(measurements, &pim); + return pim.preintegrated(); + }; // Actual pre-integrated values PreintegratedCombinedMeasurements pim(p); testing::integrateMeasurements(measurements, &pim); - EXPECT(assert_equal(numericalDerivative21(preintegrated, Z_3x1, Z_3x1), + EXPECT(assert_equal(numericalDerivative21(preintegrated, Z_3x1, Z_3x1), pim.preintegrated_H_biasAcc())); - EXPECT(assert_equal(numericalDerivative22(preintegrated, Z_3x1, Z_3x1), + EXPECT(assert_equal(numericalDerivative22(preintegrated, Z_3x1, Z_3x1), pim.preintegrated_H_biasOmega(), 1e-3)); } diff --git a/gtsam/navigation/tests/testGPSFactor.cpp b/gtsam/navigation/tests/testGPSFactor.cpp index 6149c1651..de318b3e4 100644 --- a/gtsam/navigation/tests/testGPSFactor.cpp +++ b/gtsam/navigation/tests/testGPSFactor.cpp @@ -29,19 +29,24 @@ using namespace gtsam; using namespace GeographicLib; // ************************************************************************* -TEST( GPSFactor, Constructors ) { +namespace example { +// ENU Origin is where the plane was in hold next to runway +const double lat0 = 33.86998, lon0 = -84.30626, h0 = 274; - // Convert from GPS to ENU - // ENU Origin is where the plane was in hold next to runway - const double lat0 = 33.86998, lon0 = -84.30626, h0 = 274; - LocalCartesian enu(lat0, lon0, h0, Geocentric::WGS84); +// Convert from GPS to ENU +LocalCartesian origin_ENU(lat0, lon0, h0, Geocentric::WGS84); - // Dekalb-Peachtree Airport runway 2L - const double lat = 33.87071, lon = -84.30482, h = 274; +// Dekalb-Peachtree Airport runway 2L +const double lat = 33.87071, lon = -84.30482, h = 274; +} + +// ************************************************************************* +TEST( GPSFactor, Constructor ) { + using namespace example; // From lat-lon to geocentric double E, N, U; - enu.Forward(lat, lon, h, E, N, U); + origin_ENU.Forward(lat, lon, h, E, N, U); EXPECT_DOUBLES_EQUAL(133.24, E, 1e-2); EXPECT_DOUBLES_EQUAL(80.98, N, 1e-2); EXPECT_DOUBLES_EQUAL(0, U, 1e-2); @@ -67,6 +72,35 @@ TEST( GPSFactor, Constructors ) { EXPECT(assert_equal(expectedH, actualH, 1e-8)); } +// ************************************************************************* +TEST( GPSFactor2, Constructor ) { + using namespace example; + + // From lat-lon to geocentric + double E, N, U; + origin_ENU.Forward(lat, lon, h, E, N, U); + + // Factor + Key key(1); + SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25); + GPSFactor2 factor(key, Point3(E, N, U), model); + + // Create a linearization point at zero error + NavState T(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(E, N, U), Vector3::Zero()); + EXPECT(assert_equal(zero(3),factor.evaluateError(T),1e-5)); + + // Calculate numerical derivatives + Matrix expectedH = numericalDerivative11( + boost::bind(&GPSFactor2::evaluateError, &factor, _1, boost::none), T); + + // Use the factor to calculate the derivative + Matrix actualH; + factor.evaluateError(T, actualH); + + // Verify we get the expected error + EXPECT(assert_equal(expectedH, actualH, 1e-8)); +} + //*************************************************************************** TEST(GPSData, init) { diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 28ad037c0..54ca50797 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -94,9 +94,9 @@ TEST(ImuFactor, PreintegratedMeasurements) { // Actual pre-integrated values PreintegratedImuMeasurements actual(testing::Params()); - EXPECT(assert_equal(Z_3x1, actual.theta())); - EXPECT(assert_equal(Z_3x1, actual.deltaPij())); - EXPECT(assert_equal(Z_3x1, actual.deltaVij())); + EXPECT(assert_equal(kZero, actual.theta())); + EXPECT(assert_equal(kZero, actual.deltaPij())); + EXPECT(assert_equal(kZero, actual.deltaVij())); DOUBLES_EQUAL(0.0, actual.deltaTij(), 1e-9); actual.integrateMeasurement(measuredAcc, measuredOmega, deltaT); @@ -185,8 +185,25 @@ TEST(ImuFactor, PreintegrationBaseMethods) { boost::bind(&PreintegrationBase::predict, pim, state1, _1, boost::none, boost::none), kZeroBias); EXPECT(assert_equal(eH2, aH2)); - return; +} +/* ************************************************************************* */ +TEST(ImuFactor, MultipleMeasurements) { + using namespace common; + + PreintegratedImuMeasurements expected(testing::Params(), kZeroBiasHat); + expected.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + expected.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + + Matrix32 acc,gyro; + Matrix12 dts; + acc << measuredAcc, measuredAcc; + gyro << measuredOmega, measuredOmega; + dts << deltaT, deltaT; + PreintegratedImuMeasurements actual(testing::Params(), kZeroBiasHat); + actual.integrateMeasurements(acc,gyro,dts); + + EXPECT(assert_equal(expected,actual)); } /* ************************************************************************* */ @@ -436,9 +453,9 @@ TEST(ImuFactor, FirstOrderPreIntegratedMeasurements) { PreintegratedImuMeasurements pim(testing::Params()); testing::integrateMeasurements(measurements, &pim); - EXPECT(assert_equal(numericalDerivative21(preintegrated, Z_3x1, Z_3x1), + EXPECT(assert_equal(numericalDerivative21(preintegrated, kZero, kZero), pim.preintegrated_H_biasAcc())); - EXPECT(assert_equal(numericalDerivative22(preintegrated, Z_3x1, Z_3x1), + EXPECT(assert_equal(numericalDerivative22(preintegrated, kZero, kZero), pim.preintegrated_H_biasOmega(), 1e-3)); } @@ -780,7 +797,7 @@ struct ImuFactorMergeTest { ImuFactorMergeTest() : p_(PreintegratedImuMeasurements::Params::MakeSharedU(kGravity)), - forward_(Z_3x1, Vector3(kVelocity, 0, 0)), + forward_(kZero, Vector3(kVelocity, 0, 0)), loop_(Vector3(0, -kAngularVelocity, 0), Vector3(kVelocity, 0, 0)) { // arbitrary noise values p_->gyroscopeCovariance = I_3x3 * 0.01; @@ -867,6 +884,29 @@ TEST(ImuFactor, MergeWithCoriolis) { mergeTest.TestScenarios(result_, name_, kZeroBias, kZeroBias, 1e-4); } +/* ************************************************************************* */ +// Same values as pre-integration test but now testing covariance +TEST(ImuFactor, CheckCovariance) { + // Measurements + Vector3 measuredAcc(0.1, 0.0, 0.0); + Vector3 measuredOmega(M_PI / 100.0, 0.0, 0.0); + double deltaT = 0.5; + + PreintegratedImuMeasurements actual(testing::Params()); + actual.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + Matrix9 expected; + expected << 1.0577e-08, 0, 0, 0, 0, 0, 0, 0, 0, // + 0, 1.0577e-08, 0, 0, 0, 0, 0, 0, 0, // + 0, 0, 1.0577e-08, 0, 0, 0, 0, 0, 0, // + 0, 0, 0, 5.00868e-05, 0, 0, 3.47222e-07, 0, 0, // + 0, 0, 0, 0, 5.00868e-05, 0, 0, 3.47222e-07, 0, // + 0, 0, 0, 0, 0, 5.00868e-05, 0, 0, 3.47222e-07, // + 0, 0, 0, 3.47222e-07, 0, 0, 1.38889e-06, 0, 0, // + 0, 0, 0, 0, 3.47222e-07, 0, 0, 1.38889e-06, 0, // + 0, 0, 0, 0, 0, 3.47222e-07, 0, 0, 1.38889e-06; + EXPECT(assert_equal(expected, actual.preintMeasCov())); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/navigation/tests/testNavState.cpp b/gtsam/navigation/tests/testNavState.cpp index 2fbb9fee3..fb6045d33 100644 --- a/gtsam/navigation/tests/testNavState.cpp +++ b/gtsam/navigation/tests/testNavState.cpp @@ -193,6 +193,7 @@ TEST( NavState, Lie ) { } /* ************************************************************************* */ +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 TEST(NavState, Update) { Vector3 omega(M_PI / 100.0, 0.0, 0.0); Vector3 acc(0.1, 0.0, 0.0); @@ -220,6 +221,7 @@ TEST(NavState, Update) { EXPECT(assert_equal(numericalDerivative32(update, kState1, acc, omega, 1e-7), aG1, 1e-7)); EXPECT(assert_equal(numericalDerivative33(update, kState1, acc, omega, 1e-7), aG2, 1e-7)); } +#endif /* ************************************************************************* */ static const double dt = 2.0; diff --git a/gtsam/navigation/tests/testPreintegrationBase.cpp b/gtsam/navigation/tests/testPreintegrationBase.cpp index b733181ac..527a6da7b 100644 --- a/gtsam/navigation/tests/testPreintegrationBase.cpp +++ b/gtsam/navigation/tests/testPreintegrationBase.cpp @@ -120,25 +120,24 @@ TEST(PreintegrationBase, Compose) { } /* ************************************************************************* */ - TEST(PreintegrationBase, MergedBiasDerivatives) { +TEST(PreintegrationBase, MergedBiasDerivatives) { testing::SomeMeasurements measurements; - boost::function f = - [=](const Vector3& a, const Vector3& w) { - PreintegrationBase pim02(testing::Params(), Bias(a, w)); - testing::integrateMeasurements(measurements, &pim02); - testing::integrateMeasurements(measurements, &pim02); - return pim02.preintegrated(); - }; + auto f = [=](const Vector3& a, const Vector3& w) { + PreintegrationBase pim02(testing::Params(), Bias(a, w)); + testing::integrateMeasurements(measurements, &pim02); + testing::integrateMeasurements(measurements, &pim02); + return pim02.preintegrated(); + }; // Expected merge result PreintegrationBase expected_pim02(testing::Params()); testing::integrateMeasurements(measurements, &expected_pim02); testing::integrateMeasurements(measurements, &expected_pim02); - EXPECT(assert_equal(numericalDerivative21(f, Z_3x1, Z_3x1), + EXPECT(assert_equal(numericalDerivative21(f, Z_3x1, Z_3x1), expected_pim02.preintegrated_H_biasAcc())); - EXPECT(assert_equal(numericalDerivative22(f, Z_3x1, Z_3x1), + EXPECT(assert_equal(numericalDerivative22(f, Z_3x1, Z_3x1), expected_pim02.preintegrated_H_biasOmega(), 1e-7)); } diff --git a/gtsam/navigation/tests/testScenarios.cpp b/gtsam/navigation/tests/testScenarios.cpp index b5d312a86..5097e9e66 100644 --- a/gtsam/navigation/tests/testScenarios.cpp +++ b/gtsam/navigation/tests/testScenarios.cpp @@ -75,7 +75,7 @@ TEST(ScenarioRunner, Spin) { /* ************************************************************************* */ namespace forward { const double v = 2; // m/s -ConstantTwistScenario scenario(Vector3::Zero(), Vector3(v, 0, 0)); +ConstantTwistScenario scenario(Z_3x1, Vector3(v, 0, 0)); } /* ************************************************************************* */ TEST(ScenarioRunner, Forward) { diff --git a/gtsam/nonlinear/Expression-inl.h b/gtsam/nonlinear/Expression-inl.h index 70a872d15..e59201ca7 100644 --- a/gtsam/nonlinear/Expression-inl.h +++ b/gtsam/nonlinear/Expression-inl.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -27,11 +27,6 @@ namespace gtsam { -template -void Expression::print(const std::string& s) const { - root_->print(s); -} - template Expression::Expression(const T& value) : root_(new internal::ConstantExpression(value)) { @@ -48,7 +43,7 @@ Expression::Expression(const Symbol& symbol) : } template -Expression::Expression(unsigned char c, size_t j) : +Expression::Expression(unsigned char c, std::uint64_t j) : root_(new internal::LeafExpression(Symbol(c, j))) { } @@ -128,6 +123,11 @@ void Expression::dims(std::map& map) const { root_->dims(map); } +template +void Expression::print(const std::string& s) const { + root_->print(s); +} + template T Expression::value(const Values& values, boost::optional&> H) const { @@ -259,4 +259,30 @@ std::vector > createUnknowns(size_t n, char c, size_t start) { return unknowns; } +template +ScalarMultiplyExpression::ScalarMultiplyExpression(double s, const Expression& e) + : Expression(boost::make_shared>(s, e)) {} + +template +SumExpression::SumExpression(const std::vector>& expressions) + : Expression(boost::make_shared>(expressions)) {} + +template +SumExpression SumExpression::operator+(const Expression& e) const { + SumExpression copy = *this; + boost::static_pointer_cast>(copy.root_)->add(e); + return copy; +} + +template +SumExpression& SumExpression::operator+=(const Expression& e) { + boost::static_pointer_cast>(this->root_)->add(e); + return *this; +} + +template +size_t SumExpression::nrTerms() const { + return boost::static_pointer_cast>(this->root_)->nrTerms(); +} + } // namespace gtsam diff --git a/gtsam/nonlinear/Expression.h b/gtsam/nonlinear/Expression.h index 0b348ece9..6ed6bb4a5 100644 --- a/gtsam/nonlinear/Expression.h +++ b/gtsam/nonlinear/Expression.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -22,6 +22,7 @@ #include #include #include +#include #include #include @@ -52,11 +53,14 @@ public: /// Define type so we can apply it as a meta-function typedef Expression type; -private: +protected: // Paul's trick shared pointer, polymorphic root of entire expression tree boost::shared_ptr > root_; + /// Construct with a custom root + Expression(const boost::shared_ptr >& root) : root_(root) {} + public: // Expressions wrap trees of functions that can evaluate their own derivatives. @@ -85,9 +89,6 @@ public: typename MakeOptionalJacobian::type)> type; }; - /// Print - void print(const std::string& s) const; - /// Construct a constant expression Expression(const T& value); @@ -98,7 +99,7 @@ public: Expression(const Symbol& symbol); /// Construct a leaf expression, creating Symbol - Expression(unsigned char c, size_t j); + Expression(unsigned char c, std::uint64_t j); /// Construct a unary function expression template @@ -147,6 +148,9 @@ public: /// Return dimensions for each argument, as a map void dims(std::map& map) const; + /// Print + void print(const std::string& s) const; + /** * @brief Return value and optional derivatives, reverse AD version * Notes: this is not terribly efficient, and H should have correct size. @@ -170,7 +174,7 @@ public: /// Return size needed for memory buffer in traceExecution size_t traceSize() const; -private: +protected: /// Default constructor, for serialization Expression() {} @@ -199,6 +203,85 @@ private: friend class ::ExpressionFactorShallowTest; }; +/** + * A ScalarMultiplyExpression is a specialization of Expression that multiplies with a scalar + * It optimizes the Jacobian calculation for this specific case + */ +template +class ScalarMultiplyExpression : public Expression { + // Check that T is a vector space + BOOST_CONCEPT_ASSERT((gtsam::IsVectorSpace)); + + public: + explicit ScalarMultiplyExpression(double s, const Expression& e); +}; + +/** + * A SumExpression is a specialization of Expression that just sums the arguments + * It optimizes the Jacobian calculation for this specific case + */ +template +class SumExpression : public Expression { + // Check that T is a vector space + BOOST_CONCEPT_ASSERT((gtsam::IsVectorSpace)); + + public: + explicit SumExpression(const std::vector>& expressions); + + // Syntactic sugar to allow e1 + e2 + e3... + SumExpression operator+(const Expression& e) const; + SumExpression& operator+=(const Expression& e); + + size_t nrTerms() const; +}; + +/** + * Create an expression out of a linear function f:T->A with (constant) Jacobian dTdA + * TODO(frank): create a more efficient version like ScalarMultiplyExpression. This version still + * does a malloc every linearize. + */ +template +Expression linearExpression( + const boost::function& f, const Expression& expression, + const Eigen::Matrix::dimension, traits::dimension>& dTdA) { + // Use lambda to endow f with a linear Jacobian + typename Expression::template UnaryFunction::type g = + [=](const A& value, typename MakeOptionalJacobian::type H) { + if (H) + *H << dTdA; + return f(value); + }; + return Expression(g, expression); +} + +/** + * Construct an expression that executes the scalar multiplication with an input expression + * The type T must be a vector space + * Example: + * Expression a(0), b = 12 * a; + */ +template +ScalarMultiplyExpression operator*(double s, const Expression& e) { + return ScalarMultiplyExpression(s, e); +} + +/** + * Construct an expression that sums two input expressions of the same type T + * The type T must be a vector space + * Example: + * Expression a(0), b(1), c = a + b; + */ +template +SumExpression operator+(const Expression& e1, const Expression& e2) { + return SumExpression({e1, e2}); +} + +/// Construct an expression that subtracts one expression from another +template +SumExpression operator-(const Expression& e1, const Expression& e2) { + return e1 + (-1.0) * e2; +} + /** * Construct a product expression, assumes T::compose(T) -> T * Example: diff --git a/gtsam/nonlinear/ExpressionFactor.h b/gtsam/nonlinear/ExpressionFactor.h index 21e17a648..04836a1cb 100644 --- a/gtsam/nonlinear/ExpressionFactor.h +++ b/gtsam/nonlinear/ExpressionFactor.h @@ -43,8 +43,8 @@ protected: Expression expression_; ///< the expression that is AD enabled FastVector dims_; ///< dimensions of the Jacobian matrices -public: + public: typedef boost::shared_ptr > shared_ptr; /// Constructor @@ -61,9 +61,10 @@ public: const T& measured() const { return measured_; } /// print relies on Testable traits being defined for T - void print(const std::string& s, const KeyFormatter& keyFormatter) const { + void print(const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { NoiseModelFactor::print(s, keyFormatter); - traits::Print(measured_, s + ".measured_"); + traits::Print(measured_, "ExpressionFactor with measurement: "); } /// equals relies on Testable traits being defined for T @@ -80,7 +81,7 @@ public: * both the function evaluation and its derivative(s) in H. */ virtual Vector unwhitenedError(const Values& x, - boost::optional&> H = boost::none) const { + boost::optional&> H = boost::none) const { if (H) { const T value = expression_.valueAndDerivatives(x, keys_, dims_, *H); // NOTE(hayk): Doing the reverse, AKA Local(measured_, value) is not correct here diff --git a/gtsam/nonlinear/ExtendedKalmanFilter-inl.h b/gtsam/nonlinear/ExtendedKalmanFilter-inl.h index 9251aac07..e0c4da94e 100644 --- a/gtsam/nonlinear/ExtendedKalmanFilter-inl.h +++ b/gtsam/nonlinear/ExtendedKalmanFilter-inl.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -29,8 +29,8 @@ namespace gtsam { template typename ExtendedKalmanFilter::T ExtendedKalmanFilter::solve_( const GaussianFactorGraph& linearFactorGraph, - const Values& linearizationPoints, Key lastKey, - JacobianFactor::shared_ptr& newPrior) const + const Values& linearizationPoint, Key lastKey, + JacobianFactor::shared_ptr* newPrior) { // Compute the marginal on the last key // Solve the linear factor graph, converting it into a linear Bayes Network @@ -42,7 +42,7 @@ namespace gtsam { // Extract the current estimate of x1,P1 VectorValues result = marginal->solve(VectorValues()); - const T& current = linearizationPoints.at(lastKey); + const T& current = linearizationPoint.at(lastKey); T x = traits::Retract(current, result[lastKey]); // Create a Jacobian Factor from the root node of the produced Bayes Net. @@ -51,7 +51,7 @@ namespace gtsam { // and the key/index needs to be reset to 0, the first key in the next iteration. assert(marginal->nrFrontals() == 1); assert(marginal->nrParents() == 0); - newPrior = boost::make_shared( + *newPrior = boost::make_shared( marginal->keys().front(), marginal->getA(marginal->begin()), marginal->getb() - marginal->getA(marginal->begin()) * result[lastKey], @@ -80,20 +80,8 @@ namespace gtsam { /* ************************************************************************* */ template typename ExtendedKalmanFilter::T ExtendedKalmanFilter::predict( - const MotionFactor& motionFactor) { - - // TODO: This implementation largely ignores the actual factor symbols. - // Calling predict() then update() with drastically - // different keys will still compute as if a common key-set was used - - // Create Keys - Key x0 = motionFactor.key1(); - Key x1 = motionFactor.key2(); - - // Create a set of linearization points - Values linearizationPoints; - linearizationPoints.insert(x0, x_); - linearizationPoints.insert(x1, x_); // TODO should this really be x_ ? + const NoiseModelFactor& motionFactor) { + const auto keys = motionFactor.keys(); // Create a Gaussian Factor Graph GaussianFactorGraph linearFactorGraph; @@ -102,12 +90,14 @@ namespace gtsam { linearFactorGraph.push_back(priorFactor_); // Linearize motion model and add it to the Kalman Filter graph - linearFactorGraph.push_back( - motionFactor.linearize(linearizationPoints)); + Values linearizationPoint; + linearizationPoint.insert(keys[0], x_); + linearizationPoint.insert(keys[1], x_); // TODO should this really be x_ ? + linearFactorGraph.push_back(motionFactor.linearize(linearizationPoint)); // Solve the factor graph and update the current state estimate // and the posterior for the next iteration. - x_ = solve_(linearFactorGraph, linearizationPoints, x1, priorFactor_); + x_ = solve_(linearFactorGraph, linearizationPoint, keys[1], &priorFactor_); return x_; } @@ -115,18 +105,8 @@ namespace gtsam { /* ************************************************************************* */ template typename ExtendedKalmanFilter::T ExtendedKalmanFilter::update( - const MeasurementFactor& measurementFactor) { - - // TODO: This implementation largely ignores the actual factor symbols. - // Calling predict() then update() with drastically - // different keys will still compute as if a common key-set was used - - // Create Keys - Key x0 = measurementFactor.key(); - - // Create a set of linearization points - Values linearizationPoints; - linearizationPoints.insert(x0, x_); + const NoiseModelFactor& measurementFactor) { + const auto keys = measurementFactor.keys(); // Create a Gaussian Factor Graph GaussianFactorGraph linearFactorGraph; @@ -135,12 +115,13 @@ namespace gtsam { linearFactorGraph.push_back(priorFactor_); // Linearize measurement factor and add it to the Kalman Filter graph - linearFactorGraph.push_back( - measurementFactor.linearize(linearizationPoints)); + Values linearizationPoint; + linearizationPoint.insert(keys[0], x_); + linearFactorGraph.push_back(measurementFactor.linearize(linearizationPoint)); // Solve the factor graph and update the current state estimate // and the prior factor for the next iteration - x_ = solve_(linearFactorGraph, linearizationPoints, x0, priorFactor_); + x_ = solve_(linearFactorGraph, linearizationPoint, keys[0], &priorFactor_); return x_; } diff --git a/gtsam/nonlinear/ExtendedKalmanFilter.h b/gtsam/nonlinear/ExtendedKalmanFilter.h index 4adad676e..77bb1ca6c 100644 --- a/gtsam/nonlinear/ExtendedKalmanFilter.h +++ b/gtsam/nonlinear/ExtendedKalmanFilter.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -24,76 +24,85 @@ namespace gtsam { +/** + * This is a generic Extended Kalman Filter class implemented using nonlinear factors. GTSAM + * basically does SRIF with Cholesky to solve the filter problem, making this an efficient, + *numerically + * stable Kalman Filter implementation. + * + * The Kalman Filter relies on two models: a motion model that predicts the next state using + * the current state, and a measurement model that predicts the measurement value at a given + * state. Because these two models are situation-dependent, base classes for each have been + * provided above, from which the user must derive a class and incorporate the actual modeling + * equations. + * + * The class provides a "predict" and "update" function to perform these steps independently. + * TODO: a "predictAndUpdate" that combines both steps for some computational savings. + * \nosubgrouping + */ + +template +class ExtendedKalmanFilter { + // Check that VALUE type is a testable Manifold + BOOST_CONCEPT_ASSERT((IsTestable)); + BOOST_CONCEPT_ASSERT((IsManifold)); + + public: + typedef boost::shared_ptr > shared_ptr; + typedef VALUE T; + + //@deprecated: any NoiseModelFactor will do, as long as they have the right keys + typedef NoiseModelFactor2 MotionFactor; + typedef NoiseModelFactor1 MeasurementFactor; + + protected: + T x_; // linearization point + JacobianFactor::shared_ptr priorFactor_; // Gaussian density on x_ + + static T solve_(const GaussianFactorGraph& linearFactorGraph, const Values& linearizationPoints, + Key x, JacobianFactor::shared_ptr* newPrior); + + public: + /// @name Standard Constructors + /// @{ + + ExtendedKalmanFilter(Key key_initial, T x_initial, noiseModel::Gaussian::shared_ptr P_initial); + + /// @} + /// @name Testable + /// @{ + + /// print + void print(const std::string& s = "") const { + std::cout << s << "\n"; + x_.print(s + "x"); + priorFactor_->print(s + "density"); + } + + /// @} + /// @name Interface + /// @{ + /** - * This is a generic Extended Kalman Filter class implemented using nonlinear factors. GTSAM - * basically does SRIF with Cholesky to solve the filter problem, making this an efficient, numerically - * stable Kalman Filter implementation. - * - * The Kalman Filter relies on two models: a motion model that predicts the next state using - * the current state, and a measurement model that predicts the measurement value at a given - * state. Because these two models are situation-dependent, base classes for each have been - * provided above, from which the user must derive a class and incorporate the actual modeling - * equations. - * - * The class provides a "predict" and "update" function to perform these steps independently. - * TODO: a "predictAndUpdate" that combines both steps for some computational savings. - * \nosubgrouping + * Calculate predictive density P(x_) ~ \int P(x_min) P(x_min, x_) + * The motion model should be given as a factor with key1 for x_min and key2_ for x */ + T predict(const NoiseModelFactor& motionFactor); - template - class ExtendedKalmanFilter { + /** + * Calculate posterior density P(x_) ~ L(z|x) P(x) + * The likelihood L(z|x) should be given as a unary factor on x + */ + T update(const NoiseModelFactor& measurementFactor); - // Check that VALUE type is a testable Manifold - BOOST_CONCEPT_ASSERT((IsTestable)); - BOOST_CONCEPT_ASSERT((IsManifold)); + /// Return current predictive (if called after predict)/posterior (if called after update) + const JacobianFactor::shared_ptr Density() const { + return priorFactor_; + } - public: + /// @} +}; - typedef boost::shared_ptr > shared_ptr; - typedef VALUE T; - typedef NoiseModelFactor2 MotionFactor; - typedef NoiseModelFactor1 MeasurementFactor; - - protected: - T x_; // linearization point - JacobianFactor::shared_ptr priorFactor_; // density - - T solve_(const GaussianFactorGraph& linearFactorGraph, - const Values& linearizationPoints, - Key x, JacobianFactor::shared_ptr& newPrior) const; - - public: - - /// @name Standard Constructors - /// @{ - - ExtendedKalmanFilter(Key key_initial, T x_initial, - noiseModel::Gaussian::shared_ptr P_initial); - - /// @} - /// @name Testable - /// @{ - - /// print - void print(const std::string& s="") const { - std::cout << s << "\n"; - x_.print(s+"x"); - priorFactor_->print(s+"density"); - } - - /// @} - /// @name Advanced Interface - /// @{ - - ///TODO: comment - T predict(const MotionFactor& motionFactor); - - ///TODO: comment - T update(const MeasurementFactor& measurementFactor); - - /// @} - }; - -} // namespace +} // namespace #include diff --git a/gtsam/nonlinear/GaussNewtonOptimizer.cpp b/gtsam/nonlinear/GaussNewtonOptimizer.cpp index e420567ec..d12c56b6f 100644 --- a/gtsam/nonlinear/GaussNewtonOptimizer.cpp +++ b/gtsam/nonlinear/GaussNewtonOptimizer.cpp @@ -11,7 +11,7 @@ /** * @file GaussNewtonOptimizer.cpp - * @brief + * @brief * @author Richard Roberts * @date Feb 26, 2012 */ @@ -26,14 +26,19 @@ namespace gtsam { /* ************************************************************************* */ void GaussNewtonOptimizer::iterate() { + gttic(GaussNewtonOptimizer_Iterate); const NonlinearOptimizerState& current = state_; // Linearize graph + gttic(GaussNewtonOptimizer_Linearize); GaussianFactorGraph::shared_ptr linear = graph_.linearize(current.values); + gttoc(GaussNewtonOptimizer_Linearize); // Solve Factor Graph + gttic(GaussNewtonOptimizer_Solve); const VectorValues delta = solve(*linear, current.values, params_); + gttoc(GaussNewtonOptimizer_Solve); // Maybe show output if(params_.verbosity >= NonlinearOptimizerParams::DELTA) delta.print("delta"); diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 52af4fee3..af37ca954 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -171,14 +171,14 @@ bool ISAM2::equals(const ISAM2& other, double tol) const { } /* ************************************************************************* */ -FastSet ISAM2::getAffectedFactors(const FastList& keys) const { +KeySet ISAM2::getAffectedFactors(const KeyList& keys) const { static const bool debug = false; if(debug) cout << "Getting affected factors for "; if(debug) { BOOST_FOREACH(const Key key, keys) { cout << key << " "; } } if(debug) cout << endl; NonlinearFactorGraph allAffected; - FastSet indices; + KeySet indices; BOOST_FOREACH(const Key key, keys) { const VariableIndex::Factors& factors(variableIndex_[key]); indices.insert(factors.begin(), factors.end()); @@ -197,7 +197,7 @@ GaussianFactorGraph::shared_ptr ISAM2::relinearizeAffectedFactors(const FastList& affectedKeys, const KeySet& relinKeys) const { gttic(getAffectedFactors); - FastSet candidates = getAffectedFactors(affectedKeys); + KeySet candidates = getAffectedFactors(affectedKeys); gttoc(getAffectedFactors); NonlinearFactorGraph nonlinearAffectedFactors; @@ -210,7 +210,7 @@ ISAM2::relinearizeAffectedFactors(const FastList& affectedKeys, const KeySe gttic(check_candidates_and_linearize); GaussianFactorGraph::shared_ptr linearized = boost::make_shared(); - BOOST_FOREACH(size_t idx, candidates) { + BOOST_FOREACH(Key idx, candidates) { bool inside = true; bool useCachedLinear = params_.cacheLinearizedFactors; BOOST_FOREACH(Key key, nonlinearFactors_[idx]->keys()) { @@ -464,9 +464,8 @@ boost::shared_ptr ISAM2::recalculate(const KeySet& markedKeys, const Ke constraintGroups = *constrainKeys; } else { constraintGroups = FastMap(); - const int group = observedKeys.size() < affectedFactorsVarIndex.size() - ? 1 : 0; - BOOST_FOREACH(Key var, observedKeys) + const int group = observedKeys.size() < affectedFactorsVarIndex.size() ? 1 : 0; + BOOST_FOREACH (Key var, observedKeys) constraintGroups.insert(make_pair(var, group)); } @@ -766,7 +765,7 @@ void ISAM2::marginalizeLeaves(const FastList& leafKeysList, map > marginalFactors; // Keep track of factors that get summarized by removing cliques - FastSet factorIndicesToRemove; + KeySet factorIndicesToRemove; // Keep track of variables removed in subtrees KeySet leafKeysRemoved; @@ -828,10 +827,10 @@ void ISAM2::marginalizeLeaves(const FastList& leafKeysList, // get the childrens' marginals from any existing children, plus // the marginals from the marginalFactors multimap, which come from any // subtrees already marginalized out. - + // Add child marginals and remove marginalized subtrees GaussianFactorGraph graph; - FastSet factorsInSubtreeRoot; + KeySet factorsInSubtreeRoot; Cliques subtreesToRemove; BOOST_FOREACH(const sharedClique& child, clique->children) { // Remove subtree if child depends on any marginalized keys @@ -867,14 +866,14 @@ void ISAM2::marginalizeLeaves(const FastList& leafKeysList, // These are the factors that involve *marginalized* frontal variables in this clique // but do not involve frontal variables of any of its children. // TODO: reuse cached linear factors - FastSet factorsFromMarginalizedInClique_step1; + KeySet factorsFromMarginalizedInClique_step1; BOOST_FOREACH(Key frontal, clique->conditional()->frontals()) { if(leafKeys.exists(frontal)) factorsFromMarginalizedInClique_step1.insert(variableIndex_[frontal].begin(), variableIndex_[frontal].end()); } // Remove any factors in subtrees that we're removing at this step BOOST_FOREACH(const sharedClique& removedChild, childrenRemoved) { BOOST_FOREACH(Key indexInClique, removedChild->conditional()->frontals()) { - BOOST_FOREACH(size_t factorInvolving, variableIndex_[indexInClique]) { + BOOST_FOREACH(Key factorInvolving, variableIndex_[indexInClique]) { factorsFromMarginalizedInClique_step1.erase(factorInvolving); } } } // Create factor graph from factor indices BOOST_FOREACH(size_t i, factorsFromMarginalizedInClique_step1) { @@ -989,15 +988,15 @@ void ISAM2::updateDelta(bool forceFullSolve) const gttic(Wildfire_update); lastBacksubVariableCount = Impl::UpdateGaussNewtonDelta(roots_, deltaReplacedMask_, deltaNewton_, effectiveWildfireThreshold); gttoc(Wildfire_update); - + // Compute steepest descent step const VectorValues gradAtZero = this->gradientAtZero(); // Compute gradient Impl::UpdateRgProd(roots_, deltaReplacedMask_, gradAtZero, RgProd_); // Update RgProd const VectorValues dx_u = Impl::ComputeGradientSearch(gradAtZero, RgProd_); // Compute gradient search point - + // Clear replaced keys mask because now we've updated deltaNewton_ and RgProd_ deltaReplacedMask_.clear(); - + // Compute dogleg point DoglegOptimizerImpl::IterationResult doglegResult(DoglegOptimizerImpl::Iterate( *doglegDelta_, doglegParams.adaptationMode, dx_u, deltaNewton_, *this, nonlinearFactors_, @@ -1076,11 +1075,11 @@ VectorValues ISAM2::gradientAtZero() const { // Create result VectorValues g; - + // Sum up contributions for each clique BOOST_FOREACH(const ISAM2::sharedClique& root, this->roots()) gradientAtZeroTreeAdder(root, g); - + return g; } diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index fdc0021e5..e8165aad9 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -640,7 +640,7 @@ public: protected: - FastSet getAffectedFactors(const FastList& keys) const; + FastSet getAffectedFactors(const FastList& keys) const; GaussianFactorGraph::shared_ptr relinearizeAffectedFactors(const FastList& affectedKeys, const KeySet& relinKeys) const; GaussianFactorGraph getCachedBoundaryFactors(Cliques& orphans); diff --git a/gtsam/nonlinear/LinearContainerFactor.h b/gtsam/nonlinear/LinearContainerFactor.h index 3ae8d4c98..307c7f001 100644 --- a/gtsam/nonlinear/LinearContainerFactor.h +++ b/gtsam/nonlinear/LinearContainerFactor.h @@ -2,7 +2,7 @@ * @file LinearContainerFactor.h * * @brief Wrap Jacobian and Hessian linear factors to allow simple injection into a nonlinear graph - * + * * @date Jul 6, 2012 * @author Alex Cunningham */ @@ -36,8 +36,14 @@ protected: /** direct copy constructor */ LinearContainerFactor(const GaussianFactor::shared_ptr& factor, const boost::optional& linearizationPoint); + // Some handy typedefs + typedef NonlinearFactor Base; + typedef LinearContainerFactor This; + public: + typedef boost::shared_ptr shared_ptr; + /** Primary constructor: store a linear factor with optional linearization point */ LinearContainerFactor(const JacobianFactor& factor, const Values& linearizationPoint = Values()); diff --git a/gtsam/nonlinear/Marginals.cpp b/gtsam/nonlinear/Marginals.cpp index 0dca18a1f..40d943656 100644 --- a/gtsam/nonlinear/Marginals.cpp +++ b/gtsam/nonlinear/Marginals.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -11,7 +11,7 @@ /** * @file Marginals.cpp - * @brief + * @brief * @author Richard Roberts * @date May 14, 2012 */ @@ -26,7 +26,8 @@ using namespace std; namespace gtsam { /* ************************************************************************* */ -Marginals::Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization) +Marginals::Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization, + EliminateableFactorGraph::OptionalOrdering ordering) { gttic(MarginalsConstructor); @@ -39,9 +40,9 @@ Marginals::Marginals(const NonlinearFactorGraph& graph, const Values& solution, // Compute BayesTree factorization_ = factorization; if(factorization_ == CHOLESKY) - bayesTree_ = *graph_.eliminateMultifrontal(boost::none, EliminatePreferCholesky); + bayesTree_ = *graph_.eliminateMultifrontal(ordering, EliminatePreferCholesky); else if(factorization_ == QR) - bayesTree_ = *graph_.eliminateMultifrontal(boost::none, EliminateQR); + bayesTree_ = *graph_.eliminateMultifrontal(ordering, EliminateQR); } /* ************************************************************************* */ @@ -53,24 +54,29 @@ void Marginals::print(const std::string& str, const KeyFormatter& keyFormatter) } /* ************************************************************************* */ -Matrix Marginals::marginalCovariance(Key variable) const { - return marginalInformation(variable).inverse(); +GaussianFactor::shared_ptr Marginals::marginalFactor(Key variable) const { + gttic(marginalFactor); + + // Compute marginal factor + if(factorization_ == CHOLESKY) + return bayesTree_.marginalFactor(variable, EliminatePreferCholesky); + else if(factorization_ == QR) + return bayesTree_.marginalFactor(variable, EliminateQR); + else + throw std::runtime_error("Marginals::marginalFactor: Unknown factorization"); } /* ************************************************************************* */ Matrix Marginals::marginalInformation(Key variable) const { - gttic(marginalInformation); - - // Compute marginal - GaussianFactor::shared_ptr marginalFactor; - if(factorization_ == CHOLESKY) - marginalFactor = bayesTree_.marginalFactor(variable, EliminatePreferCholesky); - else if(factorization_ == QR) - marginalFactor = bayesTree_.marginalFactor(variable, EliminateQR); // Get information matrix (only store upper-right triangle) - gttic(AsMatrix); - return marginalFactor->information(); + gttic(marginalInformation); + return marginalFactor(variable)->information(); +} + +/* ************************************************************************* */ +Matrix Marginals::marginalCovariance(Key variable) const { + return marginalInformation(variable).inverse(); } /* ************************************************************************* */ @@ -129,6 +135,11 @@ JointMarginal Marginals::jointMarginalInformation(const std::vector& variab } } +/* ************************************************************************* */ +VectorValues Marginals::optimize() const { + return bayesTree_.optimize(); +} + /* ************************************************************************* */ void JointMarginal::print(const std::string& s, const KeyFormatter& formatter) const { cout << s << "Joint marginal on keys "; diff --git a/gtsam/nonlinear/Marginals.h b/gtsam/nonlinear/Marginals.h index dd585e902..ad723015d 100644 --- a/gtsam/nonlinear/Marginals.h +++ b/gtsam/nonlinear/Marginals.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -52,25 +52,32 @@ public: * @param graph The factor graph defining the full joint density on all variables. * @param solution The linearization point about which to compute Gaussian marginals (usually the MLE as obtained from a NonlinearOptimizer). * @param factorization The linear decomposition mode - either Marginals::CHOLESKY (faster and suitable for most problems) or Marginals::QR (slower but more numerically stable for poorly-conditioned problems). + * @param ordering An optional variable ordering for elimination. */ - Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization = CHOLESKY); + Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization = CHOLESKY, + EliminateableFactorGraph::OptionalOrdering ordering = boost::none); /** print */ void print(const std::string& str = "Marginals: ", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + /** Compute the marginal factor of a single variable */ + GaussianFactor::shared_ptr marginalFactor(Key variable) const; + + /** Compute the marginal information matrix of a single variable. + * Use LLt(const Matrix&) or RtR(const Matrix&) to obtain the square-root information matrix. */ + Matrix marginalInformation(Key variable) const; + /** Compute the marginal covariance of a single variable */ Matrix marginalCovariance(Key variable) const; - /** Compute the marginal information matrix of a single variable. You can - * use LLt(const Matrix&) or RtR(const Matrix&) to obtain the square-root information - * matrix. */ - Matrix marginalInformation(Key variable) const; - /** Compute the joint marginal covariance of several variables */ JointMarginal jointMarginalCovariance(const std::vector& variables) const; /** Compute the joint marginal information of several variables */ JointMarginal jointMarginalInformation(const std::vector& variables) const; + + /** Optimize the bayes tree */ + VectorValues optimize() const; }; /** @@ -80,7 +87,7 @@ class GTSAM_EXPORT JointMarginal { protected: SymmetricBlockMatrix blockMatrix_; - std::vector keys_; + KeyVector keys_; FastMap indices_; public: diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 5b46742e7..ec77b2bd6 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -207,7 +207,12 @@ public: return noiseModel_->dim(); } - /** access to the noise model */ + /// access to the noise model + const SharedNoiseModel& noiseModel() const { + return noiseModel_; + } + + /// @deprecated access to the noise model SharedNoiseModel get_noiseModel() const { return noiseModel_; } diff --git a/gtsam/nonlinear/NonlinearFactorGraph.cpp b/gtsam/nonlinear/NonlinearFactorGraph.cpp index f05504d6b..77809b51e 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.cpp +++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp @@ -43,8 +43,8 @@ namespace gtsam { template class FactorGraph; /* ************************************************************************* */ -double NonlinearFactorGraph::probPrime(const Values& c) const { - return exp(-0.5 * error(c)); +double NonlinearFactorGraph::probPrime(const Values& values) const { + return exp(-0.5 * error(values)); } /* ************************************************************************* */ @@ -58,6 +58,24 @@ void NonlinearFactorGraph::print(const std::string& str, const KeyFormatter& key } } +/* ************************************************************************* */ +void NonlinearFactorGraph::printErrors(const Values& values, const std::string& str, + const KeyFormatter& keyFormatter) const { + cout << str << "size: " << size() << endl + << endl; + for (size_t i = 0; i < factors_.size(); i++) { + stringstream ss; + ss << "Factor " << i << ": "; + if (factors_[i] == NULL) { + cout << "NULL" << endl; + } else { + factors_[i]->print(ss.str(), keyFormatter); + cout << "error = " << factors_[i]->error(values) << endl; + } + cout << endl; + } +} + /* ************************************************************************* */ bool NonlinearFactorGraph::equals(const NonlinearFactorGraph& other, double tol) const { return Base::equals(other, tol); @@ -224,27 +242,17 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values, } /* ************************************************************************* */ -double NonlinearFactorGraph::error(const Values& c) const { +double NonlinearFactorGraph::error(const Values& values) const { gttic(NonlinearFactorGraph_error); double total_error = 0.; // iterate over all the factors_ to accumulate the log probabilities BOOST_FOREACH(const sharedFactor& factor, this->factors_) { if(factor) - total_error += factor->error(c); + total_error += factor->error(values); } return total_error; } -/* ************************************************************************* */ -KeySet NonlinearFactorGraph::keys() const { - KeySet keys; - BOOST_FOREACH(const sharedFactor& factor, this->factors_) { - if(factor) - keys.insert(factor->begin(), factor->end()); - } - return keys; -} - /* ************************************************************************* */ Ordering NonlinearFactorGraph::orderingCOLAMD() const { diff --git a/gtsam/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h index ecd3b9b56..3702d8a8f 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.h +++ b/gtsam/nonlinear/NonlinearFactorGraph.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -67,7 +67,7 @@ namespace gtsam { * A non-linear factor graph is a graph of non-Gaussian, i.e. non-linear factors, * which derive from NonlinearFactor. The values structures are typically (in SAM) more general * than just vectors, e.g., Rot3 or Pose3, which are objects in non-linear manifolds. - * Linearizing the non-linear factor graph creates a linear factor graph on the + * Linearizing the non-linear factor graph creates a linear factor graph on the * tangent vector space at the linearization point. Because the tangent space is a true * vector space, the config type will be an VectorValues in that linearized factor graph. */ @@ -94,8 +94,13 @@ namespace gtsam { template NonlinearFactorGraph(const FactorGraph& graph) : Base(graph) {} - /** print just calls base class */ - void print(const std::string& str = "NonlinearFactorGraph: ", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + /** print */ + void print(const std::string& str = "NonlinearFactorGraph: ", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + + /** print errors along with factors*/ + void printErrors(const Values& values, const std::string& str = "NonlinearFactorGraph: ", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; /** Test equality */ bool equals(const NonlinearFactorGraph& other, double tol = 1e-9) const; @@ -105,14 +110,11 @@ namespace gtsam { const GraphvizFormatting& graphvizFormatting = GraphvizFormatting(), const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; - /** return keys as an ordered set - ordering is by key value */ - KeySet keys() const; - /** unnormalized error, \f$ 0.5 \sum_i (h_i(X_i)-z)^2/\sigma^2 \f$ in the most common case */ - double error(const Values& c) const; + double error(const Values& values) const; /** Unnormalized probability. O(n) */ - double probPrime(const Values& c) const; + double probPrime(const Values& values) const; /** * Create a symbolic factor graph diff --git a/gtsam/nonlinear/NonlinearOptimizer.cpp b/gtsam/nonlinear/NonlinearOptimizer.cpp index 2c752815e..bb7b9717b 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearOptimizer.cpp @@ -184,6 +184,10 @@ bool checkConvergence(double relativeErrorTreshold, double absoluteErrorTreshold } /* ************************************************************************* */ - +GTSAM_EXPORT bool checkConvergence(const NonlinearOptimizerParams& params, double currentError, + double newError) { + return checkConvergence(params.relativeErrorTol, params.absoluteErrorTol, params.errorTol, + currentError, newError, params.verbosity); +} } diff --git a/gtsam/nonlinear/NonlinearOptimizer.h b/gtsam/nonlinear/NonlinearOptimizer.h index 7b9fdba41..cb4cbebc1 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.h +++ b/gtsam/nonlinear/NonlinearOptimizer.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -191,7 +191,7 @@ public: /// @} -protected: +protected: /** A default implementation of the optimization loop, which calls iterate() * until checkConvergence returns true. */ @@ -214,4 +214,7 @@ GTSAM_EXPORT bool checkConvergence(double relativeErrorTreshold, double absoluteErrorTreshold, double errorThreshold, double currentError, double newError, NonlinearOptimizerParams::Verbosity verbosity = NonlinearOptimizerParams::SILENT); +GTSAM_EXPORT bool checkConvergence(const NonlinearOptimizerParams& params, double currentError, + double newError); + } // gtsam diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h index 05e58accb..0008252c4 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -284,7 +284,9 @@ namespace gtsam { try { return dynamic_cast&>(value).value(); } catch (std::bad_cast &) { - throw ValuesIncorrectType(j, typeid(value), typeid(ValueType)); + // NOTE(abe): clang warns about potential side effects if done in typeid + const Value* value = item->second; + throw ValuesIncorrectType(j, typeid(*value), typeid(ValueType)); } } @@ -300,7 +302,9 @@ namespace gtsam { try { return dynamic_cast&>(value).value(); } catch (std::bad_cast &) { - throw ValuesIncorrectType(j, typeid(value), typeid(ValueType)); + // NOTE(abe): clang warns about potential side effects if done in typeid + const Value* value = item->second; + throw ValuesIncorrectType(j, typeid(*value), typeid(ValueType)); } } else { return boost::none; diff --git a/gtsam/nonlinear/expressionTesting.h b/gtsam/nonlinear/expressionTesting.h index b15592c00..a5326995c 100644 --- a/gtsam/nonlinear/expressionTesting.h +++ b/gtsam/nonlinear/expressionTesting.h @@ -28,14 +28,14 @@ namespace gtsam { namespace internal { // CPPUnitLite-style test for linearization of an ExpressionFactor template -bool testExpressionJacobians(TestResult& result_, const std::string& name_, +bool testExpressionJacobians(const std::string& name_, const gtsam::Expression& expression, const gtsam::Values& values, double nd_step, double tolerance) { // Create factor size_t size = traits::dimension; ExpressionFactor f(noiseModel::Unit::Create(size), expression.value(values), expression); - return testFactorJacobians(result_, name_, f, values, nd_step, tolerance); + return testFactorJacobians(name_, f, values, nd_step, tolerance); } } // namespace internal } // namespace gtsam @@ -46,4 +46,4 @@ bool testExpressionJacobians(TestResult& result_, const std::string& name_, /// \param numerical_derivative_step The step to use when computing the finite difference Jacobians /// \param tolerance The numerical tolerance to use when comparing Jacobians. #define EXPECT_CORRECT_EXPRESSION_JACOBIANS(expression, values, numerical_derivative_step, tolerance) \ - { EXPECT(gtsam::internal::testExpressionJacobians(result_, name_, expression, values, numerical_derivative_step, tolerance)); } + { EXPECT(gtsam::internal::testExpressionJacobians(name_, expression, values, numerical_derivative_step, tolerance)); } diff --git a/gtsam/nonlinear/factorTesting.h b/gtsam/nonlinear/factorTesting.h index 2a9d70cb4..a256f8fe2 100644 --- a/gtsam/nonlinear/factorTesting.h +++ b/gtsam/nonlinear/factorTesting.h @@ -22,9 +22,7 @@ #include #include -#include -#include -#include +#include namespace gtsam { @@ -33,7 +31,7 @@ namespace gtsam { * The benefit of this method is that it does not need to know what types are * involved to evaluate the factor. If all the machinery of gtsam is working * correctly, we should get the correct numerical derivatives out the other side. - * NOTE(frank): factor that have non vector-space measurements use between or LocalCoordinates + * NOTE(frank): factors that have non vector-space measurements use between or LocalCoordinates * to evaluate the error, and their derivatives will only be correct for near-zero errors. * This is fixable but expensive, and does not matter in practice as most factors will sit near * zero errors anyway. However, it means that below will only be exact for the correct measurement. @@ -45,10 +43,11 @@ JacobianFactor linearizeNumerically(const NoiseModelFactor& factor, std::vector > jacobians; // Get size - Eigen::VectorXd e = factor.whitenedError(values); + const Eigen::VectorXd e = factor.whitenedError(values); const size_t rows = e.size(); // Loop over all variables + const double one_over_2delta = 1.0 / (2.0 * delta); VectorValues dX = values.zeroVectors(); BOOST_FOREACH(Key key, factor) { // Compute central differences using the values struct. @@ -59,12 +58,12 @@ JacobianFactor linearizeNumerically(const NoiseModelFactor& factor, dx[col] = delta; dX[key] = dx; Values eval_values = values.retract(dX); - Eigen::VectorXd left = factor.whitenedError(eval_values); + const Eigen::VectorXd left = factor.whitenedError(eval_values); dx[col] = -delta; dX[key] = dx; eval_values = values.retract(dX); - Eigen::VectorXd right = factor.whitenedError(eval_values); - J.col(col) = (left - right) * (1.0 / (2.0 * delta)); + const Eigen::VectorXd right = factor.whitenedError(eval_values); + J.col(col) = (left - right) * one_over_2delta; } jacobians.push_back(std::make_pair(key,J)); } @@ -75,7 +74,7 @@ JacobianFactor linearizeNumerically(const NoiseModelFactor& factor, namespace internal { // CPPUnitLite-style test for linearization of a factor -bool testFactorJacobians(TestResult& result_, const std::string& name_, +bool testFactorJacobians(const std::string& name_, const NoiseModelFactor& factor, const gtsam::Values& values, double delta, double tolerance) { @@ -111,6 +110,6 @@ bool testFactorJacobians(TestResult& result_, const std::string& name_, /// \param numerical_derivative_step The step to use when computing the numerical derivative Jacobians /// \param tolerance The numerical tolerance to use when comparing Jacobians. #define EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, numerical_derivative_step, tolerance) \ - { EXPECT(gtsam::internal::testFactorJacobians(result_, name_, factor, values, numerical_derivative_step, tolerance)); } + { EXPECT(gtsam::internal::testFactorJacobians(name_, factor, values, numerical_derivative_step, tolerance)); } } // namespace gtsam diff --git a/gtsam/nonlinear/internal/CallRecord.h b/gtsam/nonlinear/internal/CallRecord.h index 90aa8a8be..6616f0a83 100644 --- a/gtsam/nonlinear/internal/CallRecord.h +++ b/gtsam/nonlinear/internal/CallRecord.h @@ -31,34 +31,39 @@ namespace internal { * it just passes dense Eigen matrices through. */ template -struct ConvertToVirtualFunctionSupportedMatrixType { +struct ConvertToDynamicIf { template static Eigen::Matrix convert( - const Eigen::MatrixBase & x) { + const Eigen::MatrixBase& x) { return x; } }; template<> -struct ConvertToVirtualFunctionSupportedMatrixType { +struct ConvertToDynamicIf { template static const Eigen::Matrix convert( const Eigen::MatrixBase & x) { return x; } - // special treatment of matrices that don't need conversion - template - static const Eigen::Matrix & convert( - const Eigen::Matrix & x) { + // Most common case: just pass through matrices that are already of the right fixed-size type + template + static const Eigen::Matrix& convert( + const Eigen::Matrix& x) { return x; } }; /** - * The CallRecord is an abstract base class for the any class that stores + * The CallRecord is an abstract base class for any class that stores * the Jacobians of applying a function with respect to each of its arguments, * as well as an execution trace for each of its arguments. + * + * The complicated structure of this class is to get around the limitations of mixing inheritance + * (needed so that a trace can keep Record pointers) and templating (needed for efficient matrix + * multiplication). The "hack" is to implement N differently sized reverse AD methods, and select + * the appropriate version with the dispatch method reverseAD2 below. */ template struct CallRecord { @@ -72,19 +77,18 @@ struct CallRecord { // Called *once* by the main AD entry point, ExecutionTrace::startReverseAD1 // This function then calls ExecutionTrace::reverseAD for every argument // which will in turn call the reverseAD method below. - // This non-virtual function _startReverseAD3, implemented in derived + // Calls virtual function _startReverseAD3, implemented in derived inline void startReverseAD2(JacobianMap& jacobians) const { _startReverseAD3(jacobians); } // Dispatch the reverseAD2 calls issued by ExecutionTrace::reverseAD1 - // Here we convert to dynamic if the - template - inline void reverseAD2(const Eigen::MatrixBase & dFdT, - JacobianMap& jacobians) const { - _reverseAD3( - ConvertToVirtualFunctionSupportedMatrixType< - (Derived::RowsAtCompileTime > 5)>::convert(dFdT), jacobians); + // Here we convert dFdT to a dynamic Matrix if the # rows>5, because _reverseAD3 is only + // specialized for fixed-size matrices up to 5 rows. + // The appropriate _reverseAD3 method is selected by method overloading. + template + inline void reverseAD2(const Eigen::MatrixBase& dFdT, JacobianMap& jacobians) const { + _reverseAD3(ConvertToDynamicIf<(Derived::RowsAtCompileTime > 5)>::convert(dFdT), jacobians); } // This overload supports matrices with both rows and columns dynamically sized. @@ -144,6 +148,8 @@ private: derived().print(indent); } + // Called from base class non-virtual inline method startReverseAD2 + // Calls non-virtual function startReverseAD4, implemented in Derived (ExpressionNode::Record) virtual void _startReverseAD3(JacobianMap& jacobians) const { derived().startReverseAD4(jacobians); } diff --git a/gtsam/nonlinear/internal/ExecutionTrace.h b/gtsam/nonlinear/internal/ExecutionTrace.h index 315261293..ed811764a 100644 --- a/gtsam/nonlinear/internal/ExecutionTrace.h +++ b/gtsam/nonlinear/internal/ExecutionTrace.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -90,6 +90,7 @@ void handleLeafCase(const Eigen::MatrixBase& dTdA, template class ExecutionTrace { static const int Dim = traits::dimension; + typedef Eigen::Matrix JacobianTT; enum { Constant, Leaf, Function } kind; @@ -97,21 +98,26 @@ class ExecutionTrace { Key key; CallRecord* ptr; } content; -public: + + public: + /// Pointer always starts out as a Constant ExecutionTrace() : kind(Constant) { } + /// Change pointer to a Leaf Record void setLeaf(Key key) { kind = Leaf; content.key = key; } + /// Take ownership of pointer to a Function Record void setFunction(CallRecord* record) { kind = Function; content.ptr = record; } + /// Print void print(const std::string& indent = "") const { if (kind == Constant) @@ -122,6 +128,7 @@ public: content.ptr->print(indent + " "); } } + /// Return record pointer, quite unsafe, used only for testing template boost::optional record() { @@ -132,11 +139,11 @@ public: return p ? boost::optional(p) : boost::none; } } + /** * *** This is the main entry point for reverse AD, called from Expression *** * Called only once, either inserts I into Jacobians (Leaf) or starts AD (Function) */ - typedef Eigen::Matrix JacobianTT; void startReverseAD1(JacobianMap& jacobians) const { if (kind == Leaf) { // This branch will only be called on trivial Leaf expressions, i.e. Priors @@ -147,7 +154,11 @@ public: // Inside startReverseAD2 the correctly dimensioned pipeline is chosen. content.ptr->startReverseAD2(jacobians); } - // Either add to Jacobians (Leaf) or propagate (Function) + + /// Either add to Jacobians (Leaf) or propagate (Function) + // This is a crucial method in selecting the pipeline dimension: since it is templated method + // it will call the templated Record method reverseAD2. Hence, at this point in the pipeline + // there are as many reverseAD1 and reverseAD2 versions as there are different Jacobian sizes. template void reverseAD1(const Eigen::MatrixBase & dTdA, JacobianMap& jacobians) const { diff --git a/gtsam/nonlinear/internal/ExpressionNode.h b/gtsam/nonlinear/internal/ExpressionNode.h index d2a130830..966f43da8 100644 --- a/gtsam/nonlinear/internal/ExpressionNode.h +++ b/gtsam/nonlinear/internal/ExpressionNode.h @@ -234,7 +234,7 @@ class UnaryExpression: public ExpressionNode { /// Constructor with a unary function f, and input argument e1 UnaryExpression(Function f, const Expression& e1) : expression1_(e1.root()), function_(f) { - ExpressionNode::traceSize_ = upAligned(sizeof(Record)) + e1.traceSize(); + this->traceSize_ = upAligned(sizeof(Record)) + e1.traceSize(); } friend class Expression; @@ -298,8 +298,8 @@ public: } /// Given df/dT, multiply in dT/dA and continue reverse AD process - template - void reverseAD4(const SomeMatrix & dFdT, JacobianMap& jacobians) const { + template + void reverseAD4(const MatrixType & dFdT, JacobianMap& jacobians) const { trace1.reverseAD1(dFdT * dTdA1, jacobians); } }; @@ -339,7 +339,7 @@ class BinaryExpression: public ExpressionNode { BinaryExpression(Function f, const Expression& e1, const Expression& e2) : expression1_(e1.root()), expression2_(e2.root()), function_(f) { - ExpressionNode::traceSize_ = // + this->traceSize_ = // upAligned(sizeof(Record)) + e1.traceSize() + e2.traceSize(); } @@ -406,15 +406,15 @@ public: std::cout << indent << "}" << std::endl; } - /// Start the reverse AD process, see comments in Base + /// Start the reverse AD process, see comments in UnaryExpression void startReverseAD4(JacobianMap& jacobians) const { trace1.reverseAD1(dTdA1, jacobians); trace2.reverseAD1(dTdA2, jacobians); } /// Given df/dT, multiply in dT/dA and continue reverse AD process - template - void reverseAD4(const SomeMatrix & dFdT, JacobianMap& jacobians) const { + template + void reverseAD4(const MatrixType & dFdT, JacobianMap& jacobians) const { trace1.reverseAD1(dFdT * dTdA1, jacobians); trace2.reverseAD1(dFdT * dTdA2, jacobians); } @@ -446,7 +446,7 @@ class TernaryExpression: public ExpressionNode { const Expression& e2, const Expression& e3) : expression1_(e1.root()), expression2_(e2.root()), expression3_(e3.root()), // function_(f) { - ExpressionNode::traceSize_ = upAligned(sizeof(Record)) + // + this->traceSize_ = upAligned(sizeof(Record)) + // e1.traceSize() + e2.traceSize() + e3.traceSize(); } @@ -530,8 +530,8 @@ public: } /// Given df/dT, multiply in dT/dA and continue reverse AD process - template - void reverseAD4(const SomeMatrix & dFdT, JacobianMap& jacobians) const { + template + void reverseAD4(const MatrixType & dFdT, JacobianMap& jacobians) const { trace1.reverseAD1(dFdT * dTdA1, jacobians); trace2.reverseAD1(dFdT * dTdA2, jacobians); trace3.reverseAD1(dFdT * dTdA3, jacobians); @@ -549,5 +549,194 @@ public: } }; -} // namespace internal -} // namespace gtsam +//----------------------------------------------------------------------------- +/// Expression for scalar multiplication +template +class ScalarMultiplyNode : public ExpressionNode { + // Check that T is a vector space + BOOST_CONCEPT_ASSERT((gtsam::IsVectorSpace)); + + double scalar_; + boost::shared_ptr > expression_; + + public: + /// Constructor with a unary function f, and input argument e1 + ScalarMultiplyNode(double s, const Expression& e) : scalar_(s), expression_(e.root()) { + this->traceSize_ = upAligned(sizeof(Record)) + e.traceSize(); + } + + /// Destructor + virtual ~ScalarMultiplyNode() {} + + /// Print + virtual void print(const std::string& indent = "") const { + std::cout << indent << "ScalarMultiplyNode" << std::endl; + expression_->print(indent + " "); + } + + /// Return value + virtual T value(const Values& values) const { + return scalar_ * expression_->value(values); + } + + /// Return keys that play in this expression + virtual std::set keys() const { + return expression_->keys(); + } + + /// Return dimensions for each argument + virtual void dims(std::map& map) const { + expression_->dims(map); + } + + // Inner Record Class + struct Record : public CallRecordImplementor::dimension> { + static const int Dim = traits::dimension; + typedef Eigen::Matrix JacobianTT; + + double scalar_dTdA; + ExecutionTrace trace; + + /// Print to std::cout + void print(const std::string& indent) const { + std::cout << indent << "ScalarMultiplyNode::Record {" << std::endl; + std::cout << indent << "D(" << typeid(T).name() << ")/D(" << typeid(T).name() + << ") = " << scalar_dTdA << std::endl; + trace.print(); + std::cout << indent << "}" << std::endl; + } + + /// Start the reverse AD process + void startReverseAD4(JacobianMap& jacobians) const { + trace.reverseAD1(scalar_dTdA * JacobianTT::Identity(), jacobians); + } + + /// Given df/dT, multiply in dT/dA and continue reverse AD process + template + void reverseAD4(const MatrixType& dFdT, JacobianMap& jacobians) const { + trace.reverseAD1(dFdT * scalar_dTdA, jacobians); + } + }; + + /// Construct an execution trace for reverse AD + virtual T traceExecution(const Values& values, ExecutionTrace& trace, + ExecutionTraceStorage* ptr) const { + assert(reinterpret_cast(ptr) % TraceAlignment == 0); + Record* record = new (ptr) Record(); + ptr += upAligned(sizeof(Record)); + T value = expression_->traceExecution(values, record->trace, ptr); + ptr += expression_->traceSize(); + trace.setFunction(record); + record->scalar_dTdA = scalar_; + return scalar_ * value; + } +}; + +//----------------------------------------------------------------------------- +/// Sum Expression +template +class SumExpressionNode : public ExpressionNode { + typedef ExpressionNode NodeT; + std::vector> expressions_; + + public: + explicit SumExpressionNode(const std::vector>& expressions) { + this->traceSize_ = upAligned(sizeof(Record)); + for (const Expression& e : expressions) + add(e); + } + + void add(const Expression& e) { + expressions_.push_back(e.root()); + this->traceSize_ += e.traceSize(); + } + + /// Destructor + virtual ~SumExpressionNode() {} + + size_t nrTerms() const { + return expressions_.size(); + } + + /// Print + virtual void print(const std::string& indent = "") const { + std::cout << indent << "SumExpressionNode" << std::endl; + for (const auto& node : expressions_) + node->print(indent + " "); + } + + /// Return value + virtual T value(const Values& values) const { + auto it = expressions_.begin(); + T sum = (*it)->value(values); + for (++it; it != expressions_.end(); ++it) + sum = sum + (*it)->value(values); + return sum; + } + + /// Return keys that play in this expression + virtual std::set keys() const { + std::set keys; + for (const auto& node : expressions_) { + std::set myKeys = node->keys(); + keys.insert(myKeys.begin(), myKeys.end()); + } + return keys; + } + + /// Return dimensions for each argument + virtual void dims(std::map& map) const { + for (const auto& node : expressions_) + node->dims(map); + } + + // Inner Record Class + struct Record : public CallRecordImplementor::dimension> { + std::vector> traces_; + + explicit Record(size_t nrArguments) : traces_(nrArguments) {} + + /// Print to std::cout + void print(const std::string& indent) const { + std::cout << indent << "SumExpressionNode::Record {" << std::endl; + for (const auto& trace : traces_) + trace.print(indent); + std::cout << indent << "}" << std::endl; + } + + /// If the SumExpression is the root, we just start as many pipelines as there are terms. + void startReverseAD4(JacobianMap& jacobians) const { + for (const auto& trace : traces_) + // NOTE(frank): equivalent to trace.reverseAD1(dTdA, jacobians) with dTdA=Identity + trace.startReverseAD1(jacobians); + } + + /// If we are not the root, we simply pass on the adjoint matrix dFdT to all terms + template + void reverseAD4(const MatrixType& dFdT, JacobianMap& jacobians) const { + for (const auto& trace : traces_) + // NOTE(frank): equivalent to trace.reverseAD1(dFdT * dTdA, jacobians) with dTdA=Identity + trace.reverseAD1(dFdT, jacobians); + } + }; + + /// Construct an execution trace for reverse AD + virtual T traceExecution(const Values& values, ExecutionTrace& trace, + ExecutionTraceStorage* ptr) const { + assert(reinterpret_cast(ptr) % TraceAlignment == 0); + size_t nrArguments = expressions_.size(); + Record* record = new (ptr) Record(nrArguments); + ptr += upAligned(sizeof(Record)); + size_t i = 0; + T sum = traits::Identity(); + for (const auto& node : expressions_) { + sum = sum + node->traceExecution(values, record->traces_[i++], ptr); + ptr += node->traceSize(); + } + trace.setFunction(record); + return sum; + } +}; + +} // namespace internal +} // namespace gtsam diff --git a/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp index af6954341..aa50ce73f 100644 --- a/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp +++ b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp @@ -238,9 +238,6 @@ TEST(AdaptAutoDiff, SnavelyExpression) { EXPECT_LONGS_EQUAL( sizeof(internal::BinaryExpression::Record), expression.traceSize()); -#ifdef GTSAM_USE_QUATERNIONS - EXPECT_LONGS_EQUAL(336, expression.traceSize()); -#endif set expected = list_of(1)(2); EXPECT(expected == expression.keys()); } diff --git a/gtsam/nonlinear/tests/testExpression.cpp b/gtsam/nonlinear/tests/testExpression.cpp index 6c9588d38..bbf19869d 100644 --- a/gtsam/nonlinear/tests/testExpression.cpp +++ b/gtsam/nonlinear/tests/testExpression.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -18,8 +18,9 @@ */ #include -#include #include +#include +#include #include #include @@ -31,10 +32,16 @@ using boost::assign::map_list_of; using namespace std; using namespace gtsam; +typedef Expression double_; +typedef Expression Point3_; +typedef Expression Vector3_; +typedef Expression Pose3_; +typedef Expression Rot3_; + /* ************************************************************************* */ -template +template Point2 uncalibrate(const CAL& K, const Point2& p, OptionalJacobian<2, 5> Dcal, - OptionalJacobian<2, 2> Dp) { + OptionalJacobian<2, 2> Dp) { return K.uncalibrate(p, Dcal, Dp); } @@ -43,7 +50,7 @@ static const Rot3 someR = Rot3::RzRyRx(1, 2, 3); /* ************************************************************************* */ // Constant TEST(Expression, Constant) { - Expression R(someR); + Rot3_ R(someR); Values values; Rot3 actual = R.value(values); EXPECT(assert_equal(someR, actual)); @@ -53,7 +60,7 @@ TEST(Expression, Constant) { /* ************************************************************************* */ // Leaf TEST(Expression, Leaf) { - Expression R(100); + Rot3_ R(100); Values values; values.insert(100, someR); @@ -67,7 +74,7 @@ TEST(Expression, Leaves) { Values values; Point3 somePoint(1, 2, 3); values.insert(Symbol('p', 10), somePoint); - std::vector > points = createUnknowns(10, 'p', 1); + std::vector points = createUnknowns(10, 'p', 1); EXPECT(assert_equal(somePoint, points.back().value(values))); } @@ -83,9 +90,10 @@ double f2(const Point3& p, OptionalJacobian<1, 3> H) { Vector f3(const Point3& p, OptionalJacobian H) { return p; } -Expression p(1); +Point3_ p(1); set expected = list_of(1); -} +} // namespace unary + TEST(Expression, Unary1) { using namespace unary; Expression e(f1, p); @@ -93,19 +101,20 @@ TEST(Expression, Unary1) { } TEST(Expression, Unary2) { using namespace unary; - Expression e(f2, p); + double_ e(f2, p); EXPECT(expected == e.keys()); } + /* ************************************************************************* */ // Unary(Leaf), dynamic TEST(Expression, Unary3) { using namespace unary; -// Expression e(f3, p); + // Expression e(f3, p); } -/* ************************************************************************* */ -//Nullary Method -TEST(Expression, NullaryMethod) { +/* ************************************************************************* */ +// Nullary Method +TEST(Expression, NullaryMethod) { // Create expression Expression p(67); Expression norm(>sam::norm, p); @@ -129,30 +138,34 @@ TEST(Expression, NullaryMethod) { expected << 3.0 / sqrt(50.0), 4.0 / sqrt(50.0), 5.0 / sqrt(50.0); EXPECT(assert_equal(expected, H[0])); } + /* ************************************************************************* */ // Binary(Leaf,Leaf) namespace binary { // Create leaves -double doubleF(const Pose3& pose, // - const Point3& point, OptionalJacobian<1, 6> H1, OptionalJacobian<1, 3> H2) { +double doubleF(const Pose3& pose, // + const Point3& point, OptionalJacobian<1, 6> H1, OptionalJacobian<1, 3> H2) { return 0.0; } -Expression x(1); -Expression p(2); -Expression p_cam(x, &Pose3::transform_to, p); +Pose3_ x(1); +Point3_ p(2); +Point3_ p_cam(x, &Pose3::transform_to, p); } + /* ************************************************************************* */ // Check that creating an expression to double compiles TEST(Expression, BinaryToDouble) { using namespace binary; - Expression p_cam(doubleF, x, p); + double_ p_cam(doubleF, x, p); } + /* ************************************************************************* */ // keys TEST(Expression, BinaryKeys) { set expected = list_of(1)(2); EXPECT(expected == binary::p_cam.keys()); } + /* ************************************************************************* */ // dimensions TEST(Expression, BinaryDimensions) { @@ -160,6 +173,7 @@ TEST(Expression, BinaryDimensions) { binary::p_cam.dims(actual); EXPECT(actual == expected); } + /* ************************************************************************* */ // dimensions TEST(Expression, BinaryTraceSize) { @@ -167,6 +181,7 @@ TEST(Expression, BinaryTraceSize) { size_t expectedTraceSize = sizeof(Binary::Record); EXPECT_LONGS_EQUAL(expectedTraceSize, binary::p_cam.traceSize()); } + /* ************************************************************************* */ // Binary(Leaf,Unary(Binary(Leaf,Leaf))) namespace tree { @@ -179,12 +194,14 @@ Point2 (*f)(const Point3&, OptionalJacobian<2, 3>) = &PinholeBase::Project; Expression projection(f, p_cam); Expression uv_hat(uncalibrate, K, projection); } + /* ************************************************************************* */ // keys TEST(Expression, TreeKeys) { set expected = list_of(1)(2)(3); EXPECT(expected == tree::uv_hat.keys()); } + /* ************************************************************************* */ // dimensions TEST(Expression, TreeDimensions) { @@ -192,32 +209,30 @@ TEST(Expression, TreeDimensions) { tree::uv_hat.dims(actual); EXPECT(actual == expected); } + /* ************************************************************************* */ // TraceSize TEST(Expression, TreeTraceSize) { typedef internal::BinaryExpression Binary1; - EXPECT_LONGS_EQUAL(internal::upAligned(sizeof(Binary1::Record)), - tree::p_cam.traceSize()); + EXPECT_LONGS_EQUAL(internal::upAligned(sizeof(Binary1::Record)), tree::p_cam.traceSize()); typedef internal::UnaryExpression Unary; - EXPECT_LONGS_EQUAL( - internal::upAligned(sizeof(Unary::Record)) + tree::p_cam.traceSize(), - tree::projection.traceSize()); + EXPECT_LONGS_EQUAL(internal::upAligned(sizeof(Unary::Record)) + tree::p_cam.traceSize(), + tree::projection.traceSize()); EXPECT_LONGS_EQUAL(0, tree::K.traceSize()); typedef internal::BinaryExpression Binary2; - EXPECT_LONGS_EQUAL( - internal::upAligned(sizeof(Binary2::Record)) + tree::K.traceSize() - + tree::projection.traceSize(), tree::uv_hat.traceSize()); + EXPECT_LONGS_EQUAL(internal::upAligned(sizeof(Binary2::Record)) + tree::K.traceSize() + + tree::projection.traceSize(), + tree::uv_hat.traceSize()); } + /* ************************************************************************* */ - TEST(Expression, compose1) { - // Create expression - Expression R1(1), R2(2); - Expression R3 = R1 * R2; + Rot3_ R1(1), R2(2); + Rot3_ R3 = R1 * R2; // Check keys set expected = list_of(1)(2); @@ -227,10 +242,9 @@ TEST(Expression, compose1) { /* ************************************************************************* */ // Test compose with arguments referring to the same rotation TEST(Expression, compose2) { - // Create expression - Expression R1(1), R2(1); - Expression R3 = R1 * R2; + Rot3_ R1(1), R2(1); + Rot3_ R3 = R1 * R2; // Check keys set expected = list_of(1); @@ -240,10 +254,9 @@ TEST(Expression, compose2) { /* ************************************************************************* */ // Test compose with one arguments referring to constant rotation TEST(Expression, compose3) { - // Create expression - Expression R1(Rot3::identity()), R2(3); - Expression R3 = R1 * R2; + Rot3_ R1(Rot3::identity()), R2(3); + Rot3_ R3 = R1 * R2; // Check keys set expected = list_of(3); @@ -252,9 +265,8 @@ TEST(Expression, compose3) { /* ************************************************************************* */ // Test with ternary function -Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3, - OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2, - OptionalJacobian<3, 3> H3) { +Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3, OptionalJacobian<3, 3> H1, + OptionalJacobian<3, 3> H2, OptionalJacobian<3, 3> H3) { // return dummy derivatives (not correct, but that's ok for testing here) if (H1) *H1 = eye(3); @@ -266,20 +278,203 @@ Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3, } TEST(Expression, ternary) { - // Create expression - Expression A(1), B(2), C(3); - Expression ABC(composeThree, A, B, C); + Rot3_ A(1), B(2), C(3); + Rot3_ ABC(composeThree, A, B, C); // Check keys set expected = list_of(1)(2)(3); EXPECT(expected == ABC.keys()); } +/* ************************************************************************* */ +TEST(Expression, ScalarMultiply) { + const Key key(67); + const Point3_ sum_ = 23 * Point3_(key); + + set expected_keys = list_of(key); + EXPECT(expected_keys == sum_.keys()); + + map actual_dims, expected_dims = map_list_of(key, 3); + sum_.dims(actual_dims); + EXPECT(actual_dims == expected_dims); + + // Check dims as map + std::map map; + sum_.dims(map); + LONGS_EQUAL(1, map.size()); + + Values values; + values.insert(key, Point3(1, 0, 2)); + + // Check value + const Point3 expected(23, 0, 46); + EXPECT(assert_equal(expected, sum_.value(values))); + + // Check value + Jacobians + std::vector H(1); + EXPECT(assert_equal(expected, sum_.value(values, H))); + EXPECT(assert_equal(23 * I_3x3, H[0])); +} + +/* ************************************************************************* */ +TEST(Expression, Sum) { + const Key key(67); + const Point3_ sum_ = Point3_(key) + Point3_(Point3(1, 1, 1)); + + set expected_keys = list_of(key); + EXPECT(expected_keys == sum_.keys()); + + map actual_dims, expected_dims = map_list_of(key, 3); + sum_.dims(actual_dims); + EXPECT(actual_dims == expected_dims); + + // Check dims as map + std::map map; + sum_.dims(map); + LONGS_EQUAL(1, map.size()); + + Values values; + values.insert(key, Point3(2, 2, 2)); + + // Check value + const Point3 expected(3, 3, 3); + EXPECT(assert_equal(expected, sum_.value(values))); + + // Check value + Jacobians + std::vector H(1); + EXPECT(assert_equal(expected, sum_.value(values, H))); + EXPECT(assert_equal(I_3x3, H[0])); +} + +/* ************************************************************************* */ +TEST(Expression, TripleSum) { + const Key key(67); + const Point3_ p1_(Point3(1, 1, 1)), p2_(key); + const SumExpression sum_ = p1_ + p2_ + p1_; + + LONGS_EQUAL(3, sum_.nrTerms()); + LONGS_EQUAL(1, sum_.keys().size()); + + Values values; + values.insert(key, Point3(2, 2, 2)); + + // Check value + const Point3 expected(4, 4, 4); + EXPECT(assert_equal(expected, sum_.value(values))); + + // Check value + Jacobians + std::vector H(1); + EXPECT(assert_equal(expected, sum_.value(values, H))); + EXPECT(assert_equal(I_3x3, H[0])); +} + +/* ************************************************************************* */ +TEST(Expression, SumOfUnaries) { + const Key key(67); + const double_ norm_(Point3_(key), &Point3::norm); + const double_ sum_ = norm_ + norm_; + + Values values; + values.insert(key, Point3(6, 0, 0)); + + // Check value + EXPECT_DOUBLES_EQUAL(12, sum_.value(values), 1e-9); + + // Check value + Jacobians + std::vector H(1); + EXPECT_DOUBLES_EQUAL(12, sum_.value(values, H), 1e-9); + EXPECT(assert_equal(Vector3(2, 0, 0).transpose(), H[0])); +} + +/* ************************************************************************* */ +TEST(Expression, UnaryOfSum) { + const Key key1(42), key2(67); + const Point3_ sum_ = Point3_(key1) + Point3_(key2); + const double_ norm_(sum_, &Point3::norm); + + map actual_dims, expected_dims = map_list_of(key1, 3)(key2, 3); + norm_.dims(actual_dims); + EXPECT(actual_dims == expected_dims); + + Values values; + values.insert(key1, Point3(1, 0, 0)); + values.insert(key2, Point3(0, 1, 0)); + + // Check value + EXPECT_DOUBLES_EQUAL(sqrt(2), norm_.value(values), 1e-9); + + // Check value + Jacobians + std::vector H(2); + EXPECT_DOUBLES_EQUAL(sqrt(2), norm_.value(values, H), 1e-9); + EXPECT(assert_equal(0.5 * sqrt(2) * Vector3(1, 1, 0).transpose(), H[0])); + EXPECT(assert_equal(0.5 * sqrt(2) * Vector3(1, 1, 0).transpose(), H[1])); +} + +/* ************************************************************************* */ +TEST(Expression, WeightedSum) { + const Key key1(42), key2(67); + const Point3_ weighted_sum_ = 17 * Point3_(key1) + 23 * Point3_(key2); + + map actual_dims, expected_dims = map_list_of(key1, 3)(key2, 3); + weighted_sum_.dims(actual_dims); + EXPECT(actual_dims == expected_dims); + + Values values; + values.insert(key1, Point3(1, 0, 0)); + values.insert(key2, Point3(0, 1, 0)); + + // Check value + const Point3 expected = 17 * Point3(1, 0, 0) + 23 * Point3(0, 1, 0); + EXPECT(assert_equal(expected, weighted_sum_.value(values))); + + // Check value + Jacobians + std::vector H(2); + EXPECT(assert_equal(expected, weighted_sum_.value(values, H))); + EXPECT(assert_equal(17 * I_3x3, H[0])); + EXPECT(assert_equal(23 * I_3x3, H[1])); +} + +/* ************************************************************************* */ +TEST(Expression, Subtract) { + const Vector3 p = Vector3::Random(), q = Vector3::Random(); + Values values; + values.insert(0, p); + values.insert(1, q); + const Vector3_ expression = Vector3_(0) - Vector3_(1); + set expected_keys = {0, 1}; + EXPECT(expression.keys() == expected_keys); + + // Check value + Jacobians + std::vector H(2); + EXPECT(assert_equal(p - q, expression.value(values, H))); + EXPECT(assert_equal(I_3x3, H[0])); + EXPECT(assert_equal(-I_3x3, H[1])); +} + +/* ************************************************************************* */ +TEST(Expression, LinearExpression) { + const Key key(67); + const boost::function f = boost::bind(&Point3::vector, _1); + const Matrix3 kIdentity = I_3x3; + const Expression linear_ = linearExpression(f, Point3_(key), kIdentity); + + Values values; + values.insert(key, Point3(1, 0, 2)); + + // Check value + const Vector3 expected(1, 0, 2); + EXPECT(assert_equal(expected, linear_.value(values))); + + // Check value + Jacobians + std::vector H(1); + EXPECT(assert_equal(expected, linear_.value(values, H))); + EXPECT(assert_equal(I_3x3, H[0])); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ - diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp index ddc7b18d6..b93fe1fc1 100644 --- a/gtsam/slam/InitializePose3.cpp +++ b/gtsam/slam/InitializePose3.cpp @@ -125,7 +125,7 @@ NonlinearFactorGraph buildPose3graph(const NonlinearFactorGraph& graph) { if (pose3Prior) pose3Graph.add( BetweenFactor(keyAnchor, pose3Prior->keys()[0], - pose3Prior->prior(), pose3Prior->get_noiseModel())); + pose3Prior->prior(), pose3Prior->noiseModel())); } return pose3Graph; } @@ -327,7 +327,7 @@ Values computePoses(NonlinearFactorGraph& pose3graph, Values& initialRot) { BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, initialRot){ Key key = key_value.key; const Rot3& rot = initialRot.at(key); - Pose3 initializedPose = Pose3(rot, Point3(0,0,0)); + Pose3 initializedPose = Pose3(rot, Point3(0, 0, 0)); initialPose.insert(key, initializedPose); } // add prior diff --git a/gtsam/slam/JacobianFactorSVD.h b/gtsam/slam/JacobianFactorSVD.h index 86636c38f..dabb7600d 100644 --- a/gtsam/slam/JacobianFactorSVD.h +++ b/gtsam/slam/JacobianFactorSVD.h @@ -9,7 +9,20 @@ namespace gtsam { /** - * JacobianFactor for Schur complement + * JacobianFactor for Schur complement that uses the "Nullspace Trick" by Mourikis + * + * This trick is equivalent to the Schur complement, but can be faster. + * In essence, the linear factor |E*dp + F*dX - b|, where p is point and X are poses, + * is multiplied by Enull, a matrix that spans the left nullspace of E, i.e., + * The mx3 matrix is analyzed with SVD as E = [Erange Enull]*S*V (mxm * mx3 * 3x3) + * where Enull is an m x (m-3) matrix + * Then Enull'*E*dp = 0, and + * |Enull'*E*dp + Enull'*F*dX - Enull'*b| == |Enull'*F*dX - Enull'*b| + * Normally F is m x 6*numKeys, and Enull'*F yields an (m-3) x 6*numKeys matrix. + * + * The code below assumes that F is block diagonal and is given as a vector of ZDim*D blocks. + * Example: m = 4 (2 measurements), Enull = 4*1, F = 4*12 (for D=6) + * Then Enull'*F = 1*4 * 4*12 = 1*12, but each 1*6 piece can be computed as a 1x2 * 2x6 mult */ template class JacobianFactorSVD: public RegularJacobianFactor { @@ -51,7 +64,7 @@ public: const SharedDiagonal& model = SharedDiagonal()) : Base() { size_t numKeys = Enull.rows() / ZDim; - size_t m2 = ZDim * numKeys - 3; + size_t m2 = ZDim * numKeys - 3; // TODO: is this not just Enull.rows()? // PLAIN NULL SPACE TRICK // Matrix Q = Enull * Enull.transpose(); // BOOST_FOREACH(const KeyMatrixZD& it, Fblocks) diff --git a/gtsam/slam/OrientedPlane3Factor.h b/gtsam/slam/OrientedPlane3Factor.h index ab77ec612..1f56adc45 100644 --- a/gtsam/slam/OrientedPlane3Factor.h +++ b/gtsam/slam/OrientedPlane3Factor.h @@ -30,6 +30,7 @@ public: /// Constructor OrientedPlane3Factor() { } + virtual ~OrientedPlane3Factor() {} /// Constructor with measured plane coefficients (a,b,c,d), noise model, pose symbol OrientedPlane3Factor(const Vector&z, const SharedGaussian& noiseModel, diff --git a/gtsam/slam/RotateFactor.h b/gtsam/slam/RotateFactor.h index 2ac217ac1..9d0975df3 100644 --- a/gtsam/slam/RotateFactor.h +++ b/gtsam/slam/RotateFactor.h @@ -61,12 +61,12 @@ public: }; /** - * Factor on unknown rotation R that relates two directions p_i = iRc * z_c + * Factor on unknown rotation iRc that relates two directions c * Directions provide less constraints than a full rotation */ class RotateDirectionsFactor: public NoiseModelFactor1 { - Unit3 p_, z_; ///< Predicted and measured directions, p = iRc * z + Unit3 i_p_, c_z_; ///< Predicted and measured directions, i_p = iRc * c_z typedef NoiseModelFactor1 Base; typedef RotateDirectionsFactor This; @@ -74,9 +74,17 @@ class RotateDirectionsFactor: public NoiseModelFactor1 { public: /// Constructor - RotateDirectionsFactor(Key key, const Unit3& p, const Unit3& z, + RotateDirectionsFactor(Key key, const Unit3& i_p, const Unit3& c_z, const SharedNoiseModel& model) : - Base(model, key), p_(p), z_(z) { + Base(model, key), i_p_(i_p), c_z_(c_z) { + } + + /// Initialize rotation iRc such that i_p = iRc * c_z + static Rot3 Initialize(const Unit3& i_p, const Unit3& c_z) { + gtsam::Quaternion iRc; + // setFromTwoVectors sets iRc to (a) quaternion which transform c_z into i_p + iRc.setFromTwoVectors(c_z.unitVector(), i_p.unitVector()); + return Rot3(iRc); } /// @return a deep copy of this factor @@ -89,23 +97,21 @@ public: const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { Base::print(s); std::cout << "RotateDirectionsFactor:" << std::endl; - p_.print("p"); - z_.print("z"); + i_p_.print("p"); + c_z_.print("z"); } /// vector of errors returns 2D vector - Vector evaluateError(const Rot3& R, - boost::optional H = boost::none) const { - Unit3 q = R * z_; - Vector e = p_.error(q, H); + Vector evaluateError(const Rot3& iRc, boost::optional H = boost::none) const { + Unit3 i_q = iRc * c_z_; + Vector error = i_p_.error(i_q, H); if (H) { Matrix DR; - R.rotate(z_, DR); + iRc.rotate(c_z_, DR); *H = (*H) * DR; } - return e; + return error; } - }; -} // gtsam +} // namespace gtsam diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index 09fe84caa..00c19198b 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -1,12 +1,12 @@ /* ---------------------------------------------------------------------------- - + * 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 - + * -------------------------------------------------------------------------- */ /** diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index d037f7535..8636138f2 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -432,7 +432,7 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate, boost::shared_ptr > factor = boost::dynamic_pointer_cast >(factor_); if (factor){ - SharedNoiseModel model = factor->get_noiseModel(); + SharedNoiseModel model = factor->noiseModel(); boost::shared_ptr gaussianModel = boost::dynamic_pointer_cast(model); if (!gaussianModel){ @@ -455,7 +455,7 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate, boost::dynamic_pointer_cast< BetweenFactor >(factor_); if (factor3D){ - SharedNoiseModel model = factor3D->get_noiseModel(); + SharedNoiseModel model = factor3D->noiseModel(); boost::shared_ptr gaussianModel = boost::dynamic_pointer_cast(model); diff --git a/gtsam/slam/lago.cpp b/gtsam/slam/lago.cpp index b2528b9d8..e46b8ee71 100644 --- a/gtsam/slam/lago.cpp +++ b/gtsam/slam/lago.cpp @@ -146,7 +146,7 @@ static void getDeltaThetaAndNoise(NonlinearFactor::shared_ptr factor, deltaTheta = (Vector(1) << pose2Between->measured().theta()).finished(); // Retrieve the noise model for the relative rotation - SharedNoiseModel model = pose2Between->get_noiseModel(); + SharedNoiseModel model = pose2Between->noiseModel(); boost::shared_ptr diagonalModel = boost::dynamic_pointer_cast(model); if (!diagonalModel) @@ -213,7 +213,7 @@ static NonlinearFactorGraph buildPose2graph(const NonlinearFactorGraph& graph) { if (pose2Prior) pose2Graph.add( BetweenFactor(keyAnchor, pose2Prior->keys()[0], - pose2Prior->prior(), pose2Prior->get_noiseModel())); + pose2Prior->prior(), pose2Prior->noiseModel())); } return pose2Graph; } @@ -329,7 +329,7 @@ Values computePoses(const NonlinearFactorGraph& pose2graph, // Retrieve the noise model for the relative rotation boost::shared_ptr diagonalModel = boost::dynamic_pointer_cast( - pose2Between->get_noiseModel()); + pose2Between->noiseModel()); linearPose2graph.add(key1, J1, key2, I3, b, diagonalModel); } else { diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index 4e8e3dbf3..1dd4e8abc 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -413,8 +413,8 @@ TEST( dataSet, writeBAL_Dubrovnik) CHECK(readBAL(filenameToWrite, writtenData)); // Check that what we read is the same as what we wrote - EXPECT(assert_equal(readData.number_cameras(),writtenData.number_cameras())); - EXPECT(assert_equal(readData.number_tracks(),writtenData.number_tracks())); + EXPECT_LONGS_EQUAL(readData.number_cameras(),writtenData.number_cameras()); + EXPECT_LONGS_EQUAL(readData.number_tracks(),writtenData.number_tracks()); for (size_t i = 0; i < readData.number_cameras(); i++){ PinholeCamera expectedCamera = writtenData.cameras[i]; @@ -437,7 +437,7 @@ TEST( dataSet, writeBAL_Dubrovnik) // check measurements for (size_t k = 0; k < actualTrack.number_measurements(); k++){ - EXPECT(assert_equal(expectedTrack.measurements[k].first,actualTrack.measurements[k].first)); + EXPECT_LONGS_EQUAL(expectedTrack.measurements[k].first,actualTrack.measurements[k].first); EXPECT(assert_equal(expectedTrack.measurements[k].second,actualTrack.measurements[k].second)); } } diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index 3bcc3eccd..8ac6c48b6 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -8,6 +8,8 @@ #include #include +#include +#include #include #include #include @@ -37,7 +39,9 @@ bool readOK = readBAL(filename, data); Rot3 c1Rc2 = data.cameras[1].pose().rotation(); Point3 c1Tc2 = data.cameras[1].pose().translation(); PinholeCamera camera2(data.cameras[1].pose(), Cal3_S2()); -EssentialMatrix trueE(c1Rc2, Unit3(c1Tc2)); +Rot3 trueRotation(c1Rc2); +Unit3 trueDirection(c1Tc2); +EssentialMatrix trueE(trueRotation, trueDirection); double baseline = 0.1; // actual baseline of the camera Point2 pA(size_t i) { @@ -84,8 +88,9 @@ TEST (EssentialMatrixFactor, testData) { //************************************************************************* TEST (EssentialMatrixFactor, factor) { + Key key(1); for (size_t i = 0; i < 5; i++) { - EssentialMatrixFactor factor(1, pA(i), pB(i), model1); + EssentialMatrixFactor factor(key, pA(i), pB(i), model1); // Check evaluation Vector expected(1); @@ -106,6 +111,62 @@ TEST (EssentialMatrixFactor, factor) { } } +//************************************************************************* +TEST(EssentialMatrixFactor, ExpressionFactor) { + Key key(1); + for (size_t i = 0; i < 5; i++) { + boost::function)> f = + boost::bind(&EssentialMatrix::error, _1, vA(i), vB(i), _2); + Expression E_(key); // leaf expression + Expression expr(f, E_); // unary expression + + // Test the derivatives using Paul's magic + Values values; + values.insert(key, trueE); + EXPECT_CORRECT_EXPRESSION_JACOBIANS(expr, values, 1e-5, 1e-9); + + // Create the factor + ExpressionFactor factor(model1, 0, expr); + + // Check evaluation + Vector expected(1); + expected << 0; + vector Hactual(1); + Vector actual = factor.unwhitenedError(values, Hactual); + EXPECT(assert_equal(expected, actual, 1e-7)); + } +} + +//************************************************************************* +TEST(EssentialMatrixFactor, ExpressionFactorRotationOnly) { + Key key(1); + for (size_t i = 0; i < 5; i++) { + boost::function)> f = + boost::bind(&EssentialMatrix::error, _1, vA(i), vB(i), _2); + boost::function, + OptionalJacobian<5, 2>)> g; + Expression R_(key); + Expression d_(trueDirection); + Expression E_(&EssentialMatrix::FromRotationAndDirection, R_, d_); + Expression expr(f, E_); + + // Test the derivatives using Paul's magic + Values values; + values.insert(key, trueRotation); + EXPECT_CORRECT_EXPRESSION_JACOBIANS(expr, values, 1e-5, 1e-9); + + // Create the factor + ExpressionFactor factor(model1, 0, expr); + + // Check evaluation + Vector expected(1); + expected << 0; + vector Hactual(1); + Vector actual = factor.unwhitenedError(values, Hactual); + EXPECT(assert_equal(expected, actual, 1e-7)); + } +} + //************************************************************************* TEST (EssentialMatrixFactor, minimization) { // Here we want to optimize directly on essential matrix constraints diff --git a/gtsam/slam/tests/testRotateFactor.cpp b/gtsam/slam/tests/testRotateFactor.cpp index 1283987d4..a8f48b050 100644 --- a/gtsam/slam/tests/testRotateFactor.cpp +++ b/gtsam/slam/tests/testRotateFactor.cpp @@ -20,26 +20,28 @@ using namespace std; using namespace boost::assign; using namespace gtsam; +static const double kDegree = M_PI / 180; + //************************************************************************* // Create some test data // Let's assume IMU is aligned with aero (X-forward,Z down) // And camera is looking forward. -Point3 cameraX(0, 1, 0), cameraY(0, 0, 1), cameraZ(1, 0, 0); -Rot3 iRc(cameraX, cameraY, cameraZ); +static const Point3 cameraX(0, 1, 0), cameraY(0, 0, 1), cameraZ(1, 0, 0); +static const Rot3 iRc(cameraX, cameraY, cameraZ); // Now, let's create some rotations around IMU frame -Unit3 p1(1, 0, 0), p2(0, 1, 0), p3(0, 0, 1); -Rot3 i1Ri2 = Rot3::AxisAngle(p1, 1), // +static const Unit3 p1(1, 0, 0), p2(0, 1, 0), p3(0, 0, 1); +static const Rot3 i1Ri2 = Rot3::AxisAngle(p1, 1), // i2Ri3 = Rot3::AxisAngle(p2, 1), // i3Ri4 = Rot3::AxisAngle(p3, 1); // The corresponding rotations in the camera frame -Rot3 c1Zc2 = iRc.inverse() * i1Ri2 * iRc, // +static const Rot3 c1Zc2 = iRc.inverse() * i1Ri2 * iRc, // c2Zc3 = iRc.inverse() * i2Ri3 * iRc, // c3Zc4 = iRc.inverse() * i3Ri4 * iRc; // The corresponding rotated directions in the camera frame -Unit3 z1 = iRc.inverse() * p1, // +static const Unit3 z1 = iRc.inverse() * p1, // z2 = iRc.inverse() * p2, // z3 = iRc.inverse() * p3; @@ -98,8 +100,7 @@ TEST (RotateFactor, minimization) { // Check error at initial estimate Values initial; - double degree = M_PI / 180; - Rot3 initialE = iRc.retract(degree * Vector3(20, -20, 20)); + Rot3 initialE = iRc.retract(kDegree * Vector3(20, -20, 20)); initial.insert(1, initialE); #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) @@ -172,8 +173,7 @@ TEST (RotateDirectionsFactor, minimization) { // Check error at initial estimate Values initial; - double degree = M_PI / 180; - Rot3 initialE = iRc.retract(degree * Vector3(20, -20, 20)); + Rot3 initialE = iRc.retract(kDegree * Vector3(20, -20, 20)); initial.insert(1, initialE); #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) @@ -184,7 +184,6 @@ TEST (RotateDirectionsFactor, minimization) { // Optimize LevenbergMarquardtParams parameters; - //parameters.setVerbosity("ERROR"); LevenbergMarquardtOptimizer optimizer(graph, initial, parameters); Values result = optimizer.optimize(); @@ -196,6 +195,39 @@ TEST (RotateDirectionsFactor, minimization) { EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4); } +//************************************************************************* +TEST(RotateDirectionsFactor, Initialization) { + // Create a gravity vector in a nav frame that has Z up + const Point3 n_gravity(0, 0, -10); + const Unit3 n_p(-n_gravity); + + // NOTE(frank): avoid singularities by using 85/275 instead of 90/270 + std::vector angles = {0, 45, 85, 135, 180, 225, 275, 315}; + for (double yaw : angles) { + const Rot3 nRy = Rot3::Yaw(yaw * kDegree); + for (double pitch : angles) { + const Rot3 yRp = Rot3::Pitch(pitch * kDegree); + for (double roll : angles) { + const Rot3 pRb = Rot3::Roll(roll * kDegree); + + // Rotation from body to nav frame: + const Rot3 nRb = nRy * yRp * pRb; + const Vector3 rpy = nRb.rpy() / kDegree; + + // Simulate measurement of IMU in body frame: + const Point3 b_acc = nRb.unrotate(-n_gravity); + const Unit3 b_z(b_acc); + + // Check initialization + const Rot3 actual_nRb = RotateDirectionsFactor::Initialize(n_p, b_z); + const Vector3 actual_rpy = actual_nRb.rpy() / kDegree; + EXPECT_DOUBLES_EQUAL(rpy.x(), actual_rpy.x(), 1e-5); + EXPECT_DOUBLES_EQUAL(rpy.y(), actual_rpy.y(), 1e-5); + } + } + } +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/symbolic/tests/testSymbolicBayesTree.cpp b/gtsam/symbolic/tests/testSymbolicBayesTree.cpp index 93c38b324..82d8fb3ec 100644 --- a/gtsam/symbolic/tests/testSymbolicBayesTree.cpp +++ b/gtsam/symbolic/tests/testSymbolicBayesTree.cpp @@ -231,8 +231,8 @@ TEST( BayesTree, shortcutCheck ) (SymbolicFactor(_E_, _B_)) (SymbolicFactor(_F_, _E_)) (SymbolicFactor(_G_, _F_)); - SymbolicBayesTree bayesTree = *chain.eliminateMultifrontal( - Ordering(list_of(_G_)(_F_)(_E_)(_D_)(_C_)(_B_)(_A_))); + Ordering ordering(list_of(_G_)(_F_)(_E_)(_D_)(_C_)(_B_)(_A_)); + SymbolicBayesTree bayesTree = *chain.eliminateMultifrontal(ordering); //bayesTree.saveGraph("BT1.dot"); @@ -337,8 +337,8 @@ TEST( BayesTree, removeTop3 ) (SymbolicFactor(X(4), L(5))) (SymbolicFactor(X(2), X(4))) (SymbolicFactor(X(3), X(2))); - SymbolicBayesTree bayesTree = *graph.eliminateMultifrontal( - Ordering(list_of (X(3)) (X(2)) (X(4)) (L(5)) )); + Ordering ordering(list_of (X(3)) (X(2)) (X(4)) (L(5)) ); + SymbolicBayesTree bayesTree = *graph.eliminateMultifrontal(ordering); // remove all SymbolicBayesNet bn; @@ -361,8 +361,8 @@ TEST( BayesTree, removeTop4 ) (SymbolicFactor(X(4), L(5))) (SymbolicFactor(X(2), X(4))) (SymbolicFactor(X(3), X(2))); - SymbolicBayesTree bayesTree = *graph.eliminateMultifrontal( - Ordering(list_of (X(3)) (X(2)) (X(4)) (L(5)) )); + Ordering ordering(list_of (X(3)) (X(2)) (X(4)) (L(5)) ); + SymbolicBayesTree bayesTree = *graph.eliminateMultifrontal(ordering); // remove all SymbolicBayesNet bn; @@ -386,8 +386,8 @@ TEST( BayesTree, removeTop5 ) (SymbolicFactor(X(4), L(5))) (SymbolicFactor(X(2), X(4))) (SymbolicFactor(X(3), X(2))); - SymbolicBayesTree bayesTree = *graph.eliminateMultifrontal( - Ordering(list_of (X(3)) (X(2)) (X(4)) (L(5)) )); + Ordering ordering(list_of (X(3)) (X(2)) (X(4)) (L(5)) ); + SymbolicBayesTree bayesTree = *graph.eliminateMultifrontal(ordering); // Remove nonexistant SymbolicBayesNet bn; diff --git a/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp b/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp index a5ad7519c..a26d2f581 100644 --- a/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp +++ b/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -23,10 +23,28 @@ #include #include +#include + using namespace std; using namespace gtsam; using namespace boost::assign; +/* ************************************************************************* */ +TEST(SymbolicFactorGraph, keys1) { + KeySet expected; + expected += 0, 1, 2, 3, 4; + KeySet actual = simpleTestGraph1.keys(); + EXPECT(expected == actual); +} + +/* ************************************************************************* */ +TEST(SymbolicFactorGraph, keys2) { + KeySet expected; + expected += 0, 1, 2, 3, 4, 5; + KeySet actual = simpleTestGraph2.keys(); + EXPECT(expected == actual); +} + /* ************************************************************************* */ TEST(SymbolicFactorGraph, eliminateFullSequential) { diff --git a/gtsam/symbolic/tests/testSymbolicISAM.cpp b/gtsam/symbolic/tests/testSymbolicISAM.cpp index 52dfbacb8..d192a6064 100644 --- a/gtsam/symbolic/tests/testSymbolicISAM.cpp +++ b/gtsam/symbolic/tests/testSymbolicISAM.cpp @@ -87,7 +87,8 @@ TEST( SymbolicISAM, iSAM ) fullGraph += SymbolicFactor(_B_, _S_); // This ordering is chosen to match the one chosen by COLAMD during the ISAM update - SymbolicBayesTree expected = *fullGraph.eliminateMultifrontal(Ordering(list_of(_X_)(_B_)(_S_)(_E_)(_L_)(_T_))); + Ordering ordering(list_of(_X_)(_B_)(_S_)(_E_)(_L_)(_T_)); + SymbolicBayesTree expected = *fullGraph.eliminateMultifrontal(ordering); // Add factor on B and S SymbolicISAM actual = *asiaGraph.eliminateMultifrontal(); diff --git a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp index d74a0c302..470701d97 100644 --- a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp @@ -536,6 +536,12 @@ NonlinearFactorGraph BatchFixedLagSmoother::calculateMarginalFactors( PrintKeySet(allKeys, "BatchFixedLagSmoother::calculateMarginalFactors All Keys: "); + if (!std::includes(allKeys.begin(), allKeys.end(), marginalizeKeys.begin(), + marginalizeKeys.end())) { + throw std::runtime_error("Some keys found in marginalizeKeys do not" + " occur in the graph."); + } + // Calculate the set of RemainingKeys = AllKeys \Intersect marginalizeKeys KeySet remainingKeys; std::set_difference(allKeys.begin(), allKeys.end(), marginalizeKeys.begin(), diff --git a/gtsam_unstable/slam/tests/testTOAFactor.cpp b/gtsam_unstable/slam/tests/testTOAFactor.cpp index 042a15108..52786702d 100644 --- a/gtsam_unstable/slam/tests/testTOAFactor.cpp +++ b/gtsam_unstable/slam/tests/testTOAFactor.cpp @@ -19,6 +19,7 @@ #include #include +#include #include #include @@ -31,7 +32,6 @@ using namespace gtsam; // typedefs typedef Eigen::Matrix Vector1; -typedef Expression Double_; typedef Expression Point3_; typedef Expression Event_; @@ -52,7 +52,7 @@ TEST( TOAFactor, NewWay ) { Event_ eventExpression(key); Point3_ microphoneConstant(microphoneAt0); // constant expression double measurement = 7; - Double_ expression(&Event::toa, eventExpression, microphoneConstant); + double_ expression(&Event::toa, eventExpression, microphoneConstant); ExpressionFactor factor(model, measurement, expression); } @@ -92,7 +92,7 @@ TEST( TOAFactor, WholeEnchilada ) { Event_ eventExpression(key); for (size_t i = 0; i < K; i++) { Point3_ microphone_i(microphones[i]); // constant expression - Double_ predictTOA(&Event::toa, eventExpression, microphone_i); + double_ predictTOA(&Event::toa, eventExpression, microphone_i); graph.addExpressionFactor(predictTOA, simulatedTOA[i], model); } diff --git a/python/gtsam/__init__.py b/python/gtsam/__init__.py index f9e40517d..2e95ea33a 100644 --- a/python/gtsam/__init__.py +++ b/python/gtsam/__init__.py @@ -1 +1 @@ -from ._libgtsam_python import * +from gtsampy import * diff --git a/python/gtsam_examples/ImuFactorExample.py b/python/gtsam_examples/ImuFactorExample.py index ca5e524ee..0c80c2fb5 100644 --- a/python/gtsam_examples/ImuFactorExample.py +++ b/python/gtsam_examples/ImuFactorExample.py @@ -18,6 +18,8 @@ BIAS_KEY = int(gtsam.Symbol('b', 0)) V = lambda j: int(gtsam.Symbol('v', j)) X = lambda i: int(gtsam.Symbol('x', i)) +np.set_printoptions(precision=3, suppress=True) + class ImuFactorExample(PreintegrationExample): def __init__(self): @@ -25,14 +27,18 @@ class ImuFactorExample(PreintegrationExample): self.priorNoise = gtsam.noiseModel.Isotropic.Sigma(6, 0.1) self.velNoise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1) + # Choose one of these twists to change scenario: + zero_twist = (np.zeros(3), np.zeros(3)) forward_twist = (np.zeros(3), self.velocity) loop_twist = (np.array([0, -math.radians(30), 0]), self.velocity) + sick_twist = (np.array([math.radians(30), -math.radians(30), 0]), self.velocity) accBias = np.array([-0.3, 0.1, 0.2]) gyroBias = np.array([0.1, 0.3, -0.1]) bias = gtsam.ConstantBias(accBias, gyroBias) - super(ImuFactorExample, self).__init__(loop_twist, bias) + dt = 1e-2 + super(ImuFactorExample, self).__init__(sick_twist, bias, dt) def addPrior(self, i, graph): state = self.scenario.navState(i) @@ -42,41 +48,13 @@ class ImuFactorExample(PreintegrationExample): def run(self): graph = gtsam.NonlinearFactorGraph() - i = 0 # state index - # initialize data structure for pre-integrated IMU measurements pim = gtsam.PreintegratedImuMeasurements(self.params, self.actualBias) - # simulate the loop - T = 12 - actual_state_i = self.scenario.navState(0) - for k, t in enumerate(np.arange(0, T, self.dt)): - # get measurements and add them to PIM - measuredOmega = self.runner.measuredAngularVelocity(t) - measuredAcc = self.runner.measuredSpecificForce(t) - pim.integrateMeasurement(measuredAcc, measuredOmega, self.dt) - - # Plot IMU many times - if k % 10 == 0: - self.plotImu(t, measuredOmega, measuredAcc) - - # Plot every second - if k % 100 == 0: - self.plotGroundTruthPose(t) - - # create IMU factor every second - if (k + 1) % 100 == 0: - factor = gtsam.ImuFactor(X(i), V(i), X(i + 1), V(i + 1), BIAS_KEY, pim) - graph.push_back(factor) - pim.resetIntegration() - actual_state_i = self.scenario.navState(t + self.dt) - i += 1 - - # add priors on beginning and end - num_poses = i + 1 - self.addPrior(0, graph) - self.addPrior(num_poses - 1, graph) + H9 = gtsam.OptionalJacobian9(); + T = 12 + num_poses = T + 1 # assumes 1 factor per second initial = gtsam.Values() initial.insert(BIAS_KEY, self.actualBias) for i in range(num_poses): @@ -84,11 +62,51 @@ class ImuFactorExample(PreintegrationExample): initial.insert(X(i), state_i.pose()) initial.insert(V(i), state_i.velocity()) + # simulate the loop + i = 0 # state index + actual_state_i = self.scenario.navState(0) + for k, t in enumerate(np.arange(0, T, self.dt)): + # get measurements and add them to PIM + measuredOmega = self.runner.measuredAngularVelocity(t) + measuredAcc = self.runner.measuredSpecificForce(t) + pim.integrateMeasurement(measuredAcc, measuredOmega, self.dt, H9, H9) + + # Plot IMU many times + if k % 10 == 0: + self.plotImu(t, measuredOmega, measuredAcc) + + # Plot every second + if k % int(1 / self.dt) == 0: + self.plotGroundTruthPose(t) + + # create IMU factor every second + if (k + 1) % int(1 / self.dt) == 0: + factor = gtsam.ImuFactor(X(i), V(i), X(i + 1), V(i + 1), BIAS_KEY, pim) + graph.push_back(factor) + if True: + print(factor) + H2 = gtsam.OptionalJacobian96(); + print(pim.predict(actual_state_i, self.actualBias, H9, H2)) + pim.resetIntegration() + actual_state_i = self.scenario.navState(t + self.dt) + i += 1 + + # add priors on beginning and end + self.addPrior(0, graph) + self.addPrior(num_poses - 1, graph) + # optimize using Levenberg-Marquardt optimization params = gtsam.LevenbergMarquardtParams() params.setVerbosityLM("SUMMARY") optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params) result = optimizer.optimize() + + # Calculate and print marginal covariances + marginals = gtsam.Marginals(graph, result) + print("Covariance on bias:\n", marginals.marginalCovariance(BIAS_KEY)) + for i in range(num_poses): + print("Covariance on pose {}:\n{}\n".format(i, marginals.marginalCovariance(X(i)))) + print("Covariance on vel {}:\n{}\n".format(i, marginals.marginalCovariance(V(i)))) # Plot resulting poses i = 0 diff --git a/python/gtsam_examples/Pose2SLAMExample.py b/python/gtsam_examples/Pose2SLAMExample.py new file mode 100644 index 000000000..7bb1296d7 --- /dev/null +++ b/python/gtsam_examples/Pose2SLAMExample.py @@ -0,0 +1,68 @@ +from __future__ import print_function +import gtsam +import math +import numpy as np + +def Vector3(x, y, z): return np.array([x, y, z]) + +# 1. Create a factor graph container and add factors to it +graph = gtsam.NonlinearFactorGraph() + +# 2a. Add a prior on the first pose, setting it to the origin +# A prior factor consists of a mean and a noise model (covariance matrix) +priorNoise = gtsam.noiseModel.Diagonal.Sigmas(Vector3(0.3, 0.3, 0.1)) +graph.add(gtsam.PriorFactorPose2(1, gtsam.Pose2(0, 0, 0), priorNoise)) + +# For simplicity, we will use the same noise model for odometry and loop closures +model = gtsam.noiseModel.Diagonal.Sigmas(Vector3(0.2, 0.2, 0.1)) + +# 2b. Add odometry factors +# Create odometry (Between) factors between consecutive poses +graph.add(gtsam.BetweenFactorPose2(1, 2, gtsam.Pose2(2, 0, 0), model)) +graph.add(gtsam.BetweenFactorPose2(2, 3, gtsam.Pose2(2, 0, math.pi / 2), model)) +graph.add(gtsam.BetweenFactorPose2(3, 4, gtsam.Pose2(2, 0, math.pi / 2), model)) +graph.add(gtsam.BetweenFactorPose2(4, 5, gtsam.Pose2(2, 0, math.pi / 2), model)) + +# 2c. Add the loop closure constraint +# This factor encodes the fact that we have returned to the same pose. In real +# systems, these constraints may be identified in many ways, such as appearance-based +# techniques with camera images. We will use another Between Factor to enforce this constraint: +graph.add(gtsam.BetweenFactorPose2(5, 2, gtsam.Pose2(2, 0, math.pi / 2), model)) +graph.print("\nFactor Graph:\n") # print + +# 3. Create the data structure to hold the initialEstimate estimate to the +# solution. For illustrative purposes, these have been deliberately set to incorrect values +initialEstimate = gtsam.Values() +initialEstimate.insert(1, gtsam.Pose2(0.5, 0.0, 0.2)) +initialEstimate.insert(2, gtsam.Pose2(2.3, 0.1, -0.2)) +initialEstimate.insert(3, gtsam.Pose2(4.1, 0.1, math.pi / 2)) +initialEstimate.insert(4, gtsam.Pose2(4.0, 2.0, math.pi)) +initialEstimate.insert(5, gtsam.Pose2(2.1, 2.1, -math.pi / 2)) +initialEstimate.print("\nInitial Estimate:\n") # print + +# 4. Optimize the initial values using a Gauss-Newton nonlinear optimizer +# The optimizer accepts an optional set of configuration parameters, +# controlling things like convergence criteria, the type of linear +# system solver to use, and the amount of information displayed during +# optimization. We will set a few parameters as a demonstration. +parameters = gtsam.GaussNewtonParams() + +# Stop iterating once the change in error between steps is less than this value +parameters.relativeErrorTol = 1e-5 +# Do not perform more than N iteration steps +parameters.maxIterations = 100 +# Create the optimizer ... +optimizer = gtsam.GaussNewtonOptimizer(graph, initialEstimate, parameters) +# ... and optimize +result = optimizer.optimize() +result.print("Final Result:\n") + +# 5. Calculate and print marginal covariances for all variables +marginals = gtsam.Marginals(graph, result) +print("x1 covariance:\n", marginals.marginalCovariance(1)) +print("x2 covariance:\n", marginals.marginalCovariance(2)) +print("x3 covariance:\n", marginals.marginalCovariance(3)) +print("x4 covariance:\n", marginals.marginalCovariance(4)) +print("x5 covariance:\n", marginals.marginalCovariance(5)) + + diff --git a/python/gtsam_examples/PreintegrationExample.py b/python/gtsam_examples/PreintegrationExample.py index 7095dc59e..6b0d83b10 100644 --- a/python/gtsam_examples/PreintegrationExample.py +++ b/python/gtsam_examples/PreintegrationExample.py @@ -27,7 +27,7 @@ class PreintegrationExample(object): params.integrationCovariance = 0.0000001 ** 2 * np.identity(3, np.float) return params - def __init__(self, twist=None, bias=None): + def __init__(self, twist=None, bias=None, dt=1e-2): """Initialize with given twist, a pair(angularVelocityVector, velocityVector).""" # setup interactive plotting @@ -43,7 +43,7 @@ class PreintegrationExample(object): V = np.array([2, 0, 0]) self.scenario = gtsam.ConstantTwistScenario(W, V) - self.dt = 1e-2 + self.dt = dt self.maxDim = 5 self.labels = list('xyz') diff --git a/python/gtsam_utils/plot.py b/python/gtsam_utils/plot.py index 84a388bbb..01ec85009 100644 --- a/python/gtsam_utils/plot.py +++ b/python/gtsam_utils/plot.py @@ -1,12 +1,13 @@ import numpy as np import matplotlib.pyplot as plt -from mpl_toolkits.mplot3d import Axes3D as _Axes3D + +def plotPoint3OnAxes(ax, point, linespec): + ax.plot([point.x()], [point.y()], [point.z()], linespec) def plotPoint3(fignum, point, linespec): fig = plt.figure(fignum) ax = fig.gca(projection='3d') - ax.plot([point.x()], [point.y()], [point.z()], linespec) - + plotPoint3OnAxes(ax, point, linespec) def plot3DPoints(fignum, values, linespec, marginals=None): # PLOT3DPOINTS Plots the Point3's in a values, with optional covariances @@ -29,11 +30,7 @@ def plot3DPoints(fignum, values, linespec, marginals=None): continue # I guess it's not a Point3 -def plotPose3(fignum, pose, axisLength=0.1): - # get figure object - fig = plt.figure(fignum) - ax = fig.gca(projection='3d') - +def plotPose3OnAxes(ax, pose, axisLength=0.1): # get rotation and translation (center) gRp = pose.rotation().matrix() # rotation from pose to global C = pose.translation().vector() @@ -42,18 +39,24 @@ def plotPose3(fignum, pose, axisLength=0.1): xAxis = C + gRp[:, 0] * axisLength L = np.append(C[np.newaxis], xAxis[np.newaxis], axis=0) ax.plot(L[:, 0], L[:, 1], L[:, 2], 'r-') - + yAxis = C + gRp[:, 1] * axisLength L = np.append(C[np.newaxis], yAxis[np.newaxis], axis=0) ax.plot(L[:, 0], L[:, 1], L[:, 2], 'g-') - + zAxis = C + gRp[:, 2] * axisLength L = np.append(C[np.newaxis], zAxis[np.newaxis], axis=0) ax.plot(L[:, 0], L[:, 1], L[:, 2], 'b-') # # plot the covariance # if (nargin>2) && (~isempty(P)) - # pPp = P(4:6,4:6); % covariance matrix in pose coordinate frame + # pPp = P(4:6,4:6); % covariance matrix in pose coordinate frame # gPp = gRp*pPp*gRp'; % convert the covariance matrix to global coordinate frame - # gtsam.covarianceEllipse3D(C,gPp); + # gtsam.covarianceEllipse3D(C,gPp); # end + +def plotPose3(fignum, pose, axisLength=0.1): + # get figure object + fig = plt.figure(fignum) + ax = fig.gca(projection='3d') + plotPose3OnAxes(ax, pose, axisLength) diff --git a/python/handwritten/CMakeLists.txt b/python/handwritten/CMakeLists.txt index 9d4f9151a..00900206c 100644 --- a/python/handwritten/CMakeLists.txt +++ b/python/handwritten/CMakeLists.txt @@ -10,10 +10,13 @@ endforeach() # Create the library add_library(gtsam_python SHARED exportgtsam.cpp ${gtsam_python_srcs}) +string(TOUPPER "${CMAKE_BUILD_TYPE}" build_type_toupper) set_target_properties(gtsam_python PROPERTIES - OUTPUT_NAME gtsam_python - SKIP_BUILD_RPATH TRUE - CLEAN_DIRECT_OUTPUT 1 + OUTPUT_NAME gtsampy + PREFIX "" + ${build_type_toupper}_POSTFIX "" + SKIP_BUILD_RPATH TRUE + CLEAN_DIRECT_OUTPUT 1 ) target_link_libraries(gtsam_python ${Boost_PYTHON${BOOST_PYTHON_VERSION_SUFFIX_UPPERCASE}_LIBRARY} @@ -21,10 +24,11 @@ target_link_libraries(gtsam_python # Cause the library to be output in the correct directory. # TODO: Change below to work on different systems (currently works only with Linux) +set(output_path ${CMAKE_CURRENT_BINARY_DIR}/../gtsam/_libgtsam_python.so) add_custom_command( - OUTPUT ${CMAKE_BINARY_DIR}/python/gtsam/_libgtsam_python.so + OUTPUT ${output_path} DEPENDS gtsam_python - COMMAND ${CMAKE_COMMAND} -E copy $ ${CMAKE_BINARY_DIR}/python/gtsam/_libgtsam_python.so + COMMAND ${CMAKE_COMMAND} -E copy $ ${output_path} COMMENT "Copying extension module to python/gtsam/_libgtsam_python.so" ) -add_custom_target(copy_gtsam_python_module ALL DEPENDS ${CMAKE_BINARY_DIR}/python/gtsam/_libgtsam_python.so) \ No newline at end of file +add_custom_target(copy_gtsam_python_module ALL DEPENDS ${output_path}) \ No newline at end of file diff --git a/python/handwritten/exportgtsam.cpp b/python/handwritten/exportgtsam.cpp index 7e8c22a82..94dc10e56 100644 --- a/python/handwritten/exportgtsam.cpp +++ b/python/handwritten/exportgtsam.cpp @@ -16,8 +16,6 @@ **/ #include -#include - #include // base @@ -33,6 +31,7 @@ void exportPose3(); void exportPinholeBaseK(); void exportPinholeCamera(); void exportCal3_S2(); +void export_geometry(); // inference void exportSymbol(); @@ -51,6 +50,7 @@ void exportISAM2(); void exportPriorFactors(); void exportBetweenFactors(); void exportGenericProjectionFactor(); +void export_slam(); // navigation void exportImuFactor(); @@ -62,7 +62,7 @@ void registerNumpyEigenConversions(); //-----------------------------------// -BOOST_PYTHON_MODULE(_libgtsam_python){ +BOOST_PYTHON_MODULE(gtsampy){ // NOTE: We need to call import_array1() instead of import_array() to support both python 2 // and 3. The reason is that BOOST_PYTHON_MODULE puts all its contents in a function @@ -85,6 +85,7 @@ BOOST_PYTHON_MODULE(_libgtsam_python){ exportPinholeBaseK(); exportPinholeCamera(); exportCal3_S2(); + export_geometry(); exportSymbol(); @@ -99,6 +100,7 @@ BOOST_PYTHON_MODULE(_libgtsam_python){ exportPriorFactors(); exportBetweenFactors(); exportGenericProjectionFactor(); + export_slam(); exportImuFactor(); exportScenario(); diff --git a/python/handwritten/geometry/Point3.cpp b/python/handwritten/geometry/Point3.cpp index 57f0fb75e..de5c8e556 100644 --- a/python/handwritten/geometry/Point3.cpp +++ b/python/handwritten/geometry/Point3.cpp @@ -27,6 +27,7 @@ using namespace gtsam; BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, Point3::print, 0, 1) BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, Point3::equals, 1, 2) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(norm_overloads, Point3::norm, 0, 1) void exportPoint3(){ @@ -40,7 +41,7 @@ class_("Point3") .def("distance", &Point3::distance) .def("dot", &Point3::dot) .def("equals", &Point3::equals, equals_overloads(args("q","tol"))) - .def("norm", &Point3::norm) + .def("norm", &Point3::norm, norm_overloads(args("OptionalJacobian<1,3>"))) .def("normalized", &Point3::normalized) .def("print", &Point3::print, print_overloads(args("s"))) #ifndef GTSAM_USE_VECTOR3_POINTS diff --git a/python/handwritten/geometry/Pose3.cpp b/python/handwritten/geometry/Pose3.cpp index dfdfe8ad1..f778ea4e0 100644 --- a/python/handwritten/geometry/Pose3.cpp +++ b/python/handwritten/geometry/Pose3.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -32,6 +32,7 @@ BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, Pose3::print, 0, 1) BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, Pose3::equals, 1, 2) BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(transform_to_overloads, Pose3::transform_to, 1, 3) BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(transform_from_overloads, Pose3::transform_from, 1, 3) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(rotation_overloads, Pose3::rotation, 0, 1) BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(translation_overloads, Pose3::translation, 0, 1) BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(compose_overloads, Pose3::compose, 2, 3) BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(between_overloads, Pose3::between, 2, 3) @@ -54,39 +55,38 @@ void exportPose3(){ double (Pose3::*range2)(const Pose3 &, OptionalJacobian<1,6>, OptionalJacobian<1,6>) const = &Pose3::range; class_("Pose3") - .def(init<>()) - .def(init()) - .def(init()) - .def(init()) - .def(init()) - .def(init()) - .def("print", &Pose3::print, print_overloads(args("s"))) - .def("equals", &Pose3::equals, equals_overloads(args("pose","tol"))) - .def("identity", &Pose3::identity) - .staticmethod("identity") - .def("bearing", &Pose3::bearing) - .def("matrix", &Pose3::matrix) - .def("transform_from", &Pose3::transform_from, - transform_from_overloads(args("point", "H1", "H2"))) - .def("transform_to", transform_to1, - transform_to_overloads(args("point", "H1", "H2"))) - .def("transform_to", transform_to2) - .def("x", &Pose3::x) - .def("y", &Pose3::y) - .def("z", &Pose3::z) - .def("translation", &Pose3::translation, - translation_overloads()[return_value_policy()]) - .def("rotation", &Pose3::rotation, return_value_policy()) - .def(self * self) // __mult__ - .def(self * other()) // __mult__ - .def(self_ns::str(self)) // __str__ - .def(repr(self)) // __repr__ - .def("compose", compose1) - .def("compose", compose2, compose_overloads()) - .def("between", between1) - .def("between", between2, between_overloads()) - .def("range", range1, range_overloads()) - .def("range", range2, range_overloads()) - .def("bearing", &Pose3::bearing, bearing_overloads()) - ; -} \ No newline at end of file + .def(init<>()) + .def(init()) + .def(init()) + .def(init()) + .def(init()) + .def(init()) + .def("print", &Pose3::print, print_overloads(args("s"))) + .def("equals", &Pose3::equals, equals_overloads(args("pose", "tol"))) + .def("identity", &Pose3::identity) + .staticmethod("identity") + .def("bearing", &Pose3::bearing) + .def("matrix", &Pose3::matrix) + .def("transform_from", &Pose3::transform_from, + transform_from_overloads(args("point", "H1", "H2"))) + .def("transform_to", transform_to1, transform_to_overloads(args("point", "H1", "H2"))) + .def("transform_to", transform_to2) + .def("x", &Pose3::x) + .def("y", &Pose3::y) + .def("z", &Pose3::z) + .def("rotation", &Pose3::rotation, + rotation_overloads()[return_value_policy()]) + .def("translation", &Pose3::translation, + translation_overloads()[return_value_policy()]) + .def(self * self) // __mult__ + .def(self * other()) // __mult__ + .def(self_ns::str(self)) // __str__ + .def(repr(self)) // __repr__ + .def("compose", compose1) + .def("compose", compose2, compose_overloads()) + .def("between", between1) + .def("between", between2, between_overloads()) + .def("range", range1, range_overloads()) + .def("range", range2, range_overloads()) + .def("bearing", &Pose3::bearing, bearing_overloads()); +} diff --git a/python/handwritten/geometry/Rot3.cpp b/python/handwritten/geometry/Rot3.cpp index 685a37ca9..b840718a3 100644 --- a/python/handwritten/geometry/Rot3.cpp +++ b/python/handwritten/geometry/Rot3.cpp @@ -58,7 +58,6 @@ void exportRot3(){ .def("Quaternion", Quaternion_0, arg("q"), "Creates a Rot3 from an array [w,x,y,z] representing a quaternion") .def("Quaternion", Quaternion_1, (arg("w"),arg("x"),arg("y"),arg("z")) ) .staticmethod("Quaternion") - .staticmethod("AxisAngle") .def("Expmap", &Rot3::Expmap) .staticmethod("Expmap") .def("ExpmapDerivative", &Rot3::ExpmapDerivative) @@ -69,6 +68,7 @@ void exportRot3(){ .staticmethod("LogmapDerivative") .def("AxisAngle", AxisAngle_0) .def("AxisAngle", AxisAngle_1) + .staticmethod("AxisAngle") .def("Rodrigues", Rodrigues_0) .def("Rodrigues", Rodrigues_1) .staticmethod("Rodrigues") diff --git a/python/handwritten/geometry/export_geometry.cpp b/python/handwritten/geometry/export_geometry.cpp new file mode 100644 index 000000000..8c04f23dc --- /dev/null +++ b/python/handwritten/geometry/export_geometry.cpp @@ -0,0 +1,35 @@ +/* ---------------------------------------------------------------------------- + + * 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 export_geometry + * @brief wraps geometry classes + * @author Ellon Paiva Mendes (LAAS-CNRS) + * @author Frank Dellaert + **/ + +#include + +#define NO_IMPORT_ARRAY +#include + +#include + +using namespace boost::python; +using namespace gtsam; +using namespace std; + +void export_geometry() { + class_("Unit3") + .def(init<>()) + .def(init()) + .def(init()); +} diff --git a/python/handwritten/linear/NoiseModel.cpp b/python/handwritten/linear/NoiseModel.cpp index 3453184bd..3612f7e14 100644 --- a/python/handwritten/linear/NoiseModel.cpp +++ b/python/handwritten/linear/NoiseModel.cpp @@ -110,6 +110,7 @@ void exportNoiseModels(){ .def("Covariance",&Gaussian::Covariance, Gaussian_Covariance_overloads()) .staticmethod("Covariance") ; + register_ptr_to_python< boost::shared_ptr >(); class_, bases >("Diagonal", no_init) .def("Sigmas",&Diagonal::Sigmas, Diagonal_Sigmas_overloads()) @@ -119,6 +120,7 @@ void exportNoiseModels(){ .def("Precisions",&Diagonal::Precisions, Diagonal_Precisions_overloads()) .staticmethod("Precisions") ; + register_ptr_to_python< boost::shared_ptr >(); class_, bases >("Isotropic", no_init) .def("Sigma",&Isotropic::Sigma, Isotropic_Sigma_overloads()) @@ -128,10 +130,12 @@ void exportNoiseModels(){ .def("Precision",&Isotropic::Precision, Isotropic_Precision_overloads()) .staticmethod("Precision") ; + register_ptr_to_python< boost::shared_ptr >(); class_, bases >("Unit", no_init) .def("Create",&Unit::Create) .staticmethod("Create") ; -} \ No newline at end of file + register_ptr_to_python< boost::shared_ptr >(); +} diff --git a/python/handwritten/navigation/ImuFactor.cpp b/python/handwritten/navigation/ImuFactor.cpp index c947ae9ee..2d7e36f47 100644 --- a/python/handwritten/navigation/ImuFactor.cpp +++ b/python/handwritten/navigation/ImuFactor.cpp @@ -20,6 +20,7 @@ #include #include "gtsam/navigation/ImuFactor.h" +#include "gtsam/navigation/GPSFactor.h" using namespace boost::python; using namespace gtsam; @@ -32,6 +33,8 @@ BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, print, 0, 1) BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(attitude_overloads, attitude, 0, 1) BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(position_overloads, position, 0, 1) BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(velocity_overloads, velocity, 0, 1) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, PreintegratedImuMeasurements::equals, 1, 2) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(predict_overloads, PreintegrationBase::predict, 2, 4) void exportImuFactor() { class_("OptionalJacobian39", init<>()); @@ -39,6 +42,7 @@ void exportImuFactor() { class_("OptionalJacobian9", init<>()); class_("NavState", init<>()) + .def(init()) // TODO(frank): overload with jacobians .def("print", &NavState::print, print_overloads()) .def("attitude", &NavState::attitude, @@ -73,22 +77,45 @@ void exportImuFactor() { .def("MakeSharedU", &PreintegrationParams::MakeSharedU) .staticmethod("MakeSharedU"); - class_( + // NOTE(frank): https://mail.python.org/pipermail/cplusplus-sig/2016-January/017362.html + register_ptr_to_python< boost::shared_ptr >(); + + class_( + "PreintegrationBase", + init&, const imuBias::ConstantBias&>()) + .def("predict", &PreintegrationBase::predict, predict_overloads()) + .def("computeError", &PreintegrationBase::computeError) + .def("resetIntegration", &PreintegrationBase::resetIntegration) + .def("deltaTij", &PreintegrationBase::deltaTij); + + class_>( "PreintegratedImuMeasurements", - init&, - const imuBias::ConstantBias&>()) + init&, const imuBias::ConstantBias&>()) .def(repr(self)) - .def("predict", &PreintegratedImuMeasurements::predict) - .def("computeError", &PreintegratedImuMeasurements::computeError) - .def("resetIntegration", &PreintegratedImuMeasurements::resetIntegration) - .def("integrateMeasurement", - &PreintegratedImuMeasurements::integrateMeasurement) + .def("equals", &PreintegratedImuMeasurements::equals, equals_overloads(args("other", "tol"))) + .def("integrateMeasurement", &PreintegratedImuMeasurements::integrateMeasurement) + .def("integrateMeasurements", &PreintegratedImuMeasurements::integrateMeasurements) .def("preintMeasCov", &PreintegratedImuMeasurements::preintMeasCov); - // NOTE(frank): Abstract classes need boost::noncopyable - class_, boost::shared_ptr>( - "ImuFactor") + class_, boost::shared_ptr>("ImuFactor") .def("error", &ImuFactor::error) .def(init()) .def(repr(self)); + register_ptr_to_python>(); + + class_, boost::shared_ptr>("ImuFactor2") + .def("error", &ImuFactor2::error) + .def(init()) + .def(repr(self)); + register_ptr_to_python>(); + + class_, boost::shared_ptr>("GPSFactor") + .def("error", &GPSFactor::error) + .def(init()); + register_ptr_to_python>(); + + class_, boost::shared_ptr>("GPSFactor2") + .def("error", &GPSFactor2::error) + .def(init()); + register_ptr_to_python>(); } diff --git a/python/handwritten/nonlinear/LevenbergMarquardtOptimizer.cpp b/python/handwritten/nonlinear/LevenbergMarquardtOptimizer.cpp index 7c7cdf3cc..f1c6a0b73 100644 --- a/python/handwritten/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/python/handwritten/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -3,25 +3,40 @@ #define NO_IMPORT_ARRAY #include +#include #include +#include using namespace boost::python; using namespace gtsam; -void exportLevenbergMarquardtOptimizer(){ - class_("LevenbergMarquardtParams", init<>()) - .def("setDiagonalDamping", &LevenbergMarquardtParams::setDiagonalDamping) - .def("setlambdaFactor", &LevenbergMarquardtParams::setlambdaFactor) - .def("setlambdaInitial", &LevenbergMarquardtParams::setlambdaInitial) - .def("setlambdaLowerBound", &LevenbergMarquardtParams::setlambdaLowerBound) - .def("setlambdaUpperBound", &LevenbergMarquardtParams::setlambdaUpperBound) - .def("setLogFile", &LevenbergMarquardtParams::setLogFile) - .def("setUseFixedLambdaFactor", &LevenbergMarquardtParams::setUseFixedLambdaFactor) - .def("setVerbosityLM", &LevenbergMarquardtParams::setVerbosityLM) - ; +void exportLevenbergMarquardtOptimizer() { + class_("GaussNewtonParams", init<>()) + .def_readwrite("maxIterations", &GaussNewtonParams::maxIterations) + .def_readwrite("relativeErrorTol", &GaussNewtonParams::relativeErrorTol); - class_("LevenbergMarquardtOptimizer", - init()) - .def("optimize", &LevenbergMarquardtOptimizer::optimize, return_value_policy()) - ; + class_( + "GaussNewtonOptimizer", + init()) + .def("optimize", &GaussNewtonOptimizer::optimize, + return_value_policy()); + + class_("LevenbergMarquardtParams", init<>()) + .def("setDiagonalDamping", &LevenbergMarquardtParams::setDiagonalDamping) + .def("setlambdaFactor", &LevenbergMarquardtParams::setlambdaFactor) + .def("setlambdaInitial", &LevenbergMarquardtParams::setlambdaInitial) + .def("setlambdaLowerBound", &LevenbergMarquardtParams::setlambdaLowerBound) + .def("setlambdaUpperBound", &LevenbergMarquardtParams::setlambdaUpperBound) + .def("setLogFile", &LevenbergMarquardtParams::setLogFile) + .def("setUseFixedLambdaFactor", &LevenbergMarquardtParams::setUseFixedLambdaFactor) + .def("setVerbosityLM", &LevenbergMarquardtParams::setVerbosityLM); + + class_( + "LevenbergMarquardtOptimizer", + init()) + .def("optimize", &LevenbergMarquardtOptimizer::optimize, + return_value_policy()); + + class_("Marginals", init()) + .def("marginalCovariance", &Marginals::marginalCovariance); } diff --git a/python/handwritten/nonlinear/Values.cpp b/python/handwritten/nonlinear/Values.cpp index 84e82f376..b635589c3 100644 --- a/python/handwritten/nonlinear/Values.cpp +++ b/python/handwritten/nonlinear/Values.cpp @@ -27,6 +27,7 @@ #include "gtsam/geometry/Rot3.h" #include "gtsam/geometry/Pose3.h" #include "gtsam/navigation/ImuBias.h" +#include "gtsam/navigation/NavState.h" using namespace boost::python; using namespace gtsam; @@ -45,6 +46,7 @@ void exportValues(){ void (Values::*insert_rot3) (Key, const gtsam::Rot3&) = &Values::insert; void (Values::*insert_pose3) (Key, const gtsam::Pose3&) = &Values::insert; void (Values::*insert_bias) (Key, const Bias&) = &Values::insert; + void (Values::*insert_navstate) (Key, const NavState&) = &Values::insert; void (Values::*insert_vector3) (Key, const gtsam::Vector3&) = &Values::insert; class_("Values", init<>()) @@ -65,6 +67,7 @@ void exportValues(){ .def("insert", insert_rot3) .def("insert", insert_pose3) .def("insert", insert_bias) + .def("insert", insert_navstate) .def("insert", insert_vector3) .def("atPoint2", &Values::at, return_value_policy()) .def("atRot2", &Values::at, return_value_policy()) @@ -73,6 +76,7 @@ void exportValues(){ .def("atRot3", &Values::at, return_value_policy()) .def("atPose3", &Values::at, return_value_policy()) .def("atConstantBias", &Values::at, return_value_policy()) + .def("atNavState", &Values::at, return_value_policy()) .def("atVector3", &Values::at, return_value_policy()) .def("exists", exists1) .def("keys", &Values::keys) diff --git a/python/handwritten/slam/PriorFactor.cpp b/python/handwritten/slam/PriorFactor.cpp index 3ada91f43..ea917d2fa 100644 --- a/python/handwritten/slam/PriorFactor.cpp +++ b/python/handwritten/slam/PriorFactor.cpp @@ -27,6 +27,7 @@ #include "gtsam/geometry/Point3.h" #include "gtsam/geometry/Rot3.h" #include "gtsam/geometry/Pose3.h" +#include "gtsam/navigation/NavState.h" using namespace boost::python; using namespace gtsam; @@ -55,4 +56,5 @@ void exportPriorFactors() PRIORFACTOR(Rot3) PRIORFACTOR(Pose3) PRIORFACTOR(Vector3) + PRIORFACTOR(NavState) } diff --git a/python/handwritten/slam/export_slam.cpp b/python/handwritten/slam/export_slam.cpp new file mode 100644 index 000000000..9431af7aa --- /dev/null +++ b/python/handwritten/slam/export_slam.cpp @@ -0,0 +1,36 @@ +/* ---------------------------------------------------------------------------- + + * 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 export_slam + * @brief wraps slam classes + * @author Ellon Paiva Mendes (LAAS-CNRS) + * @author Frank Dellaert + **/ + +#include + +#define NO_IMPORT_ARRAY +#include + +#include + +using namespace boost::python; +using namespace gtsam; +using namespace std; + +void export_slam() { + class_>( + "RotateDirectionsFactor", + init()) + .def("Initialize", &RotateDirectionsFactor::Initialize) + .staticmethod("Initialize"); +} diff --git a/tests/testExpressionFactor.cpp b/tests/testExpressionFactor.cpp index bda23b3f6..282a6b816 100644 --- a/tests/testExpressionFactor.cpp +++ b/tests/testExpressionFactor.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) diff --git a/tests/testGaussianBayesTreeB.cpp b/tests/testGaussianBayesTreeB.cpp index f7f5467af..cf54ce96e 100644 --- a/tests/testGaussianBayesTreeB.cpp +++ b/tests/testGaussianBayesTreeB.cpp @@ -309,7 +309,8 @@ TEST(GaussianBayesTree, shortcut_overlapping_separator) // c(5|6) // c(1,2|5) // c(3,4|5) - GaussianBayesTree bt = *fg.eliminateMultifrontal(Ordering(fg.keys())); // eliminate in increasing key order, fg.keys() is sorted. + Ordering ordering(fg.keys()); + GaussianBayesTree bt = *fg.eliminateMultifrontal(ordering); // eliminate in increasing key order, fg.keys() is sorted. GaussianFactorGraph joint = *bt.joint(1,2, EliminateQR); diff --git a/tests/testNonlinearEquality.cpp b/tests/testNonlinearEquality.cpp index 130dc1bc8..a9ce8ff0a 100644 --- a/tests/testNonlinearEquality.cpp +++ b/tests/testNonlinearEquality.cpp @@ -18,10 +18,11 @@ #include #include -#include #include #include #include +#include +#include #include #include #include @@ -513,70 +514,63 @@ TEST (testNonlinearEqualityConstraint, map_warp ) { CHECK(assert_equal(expected, actual, tol)); } -// make a realistic calibration matrix -static double fov = 60; // degrees -static int w = 640, h = 480; -static Cal3_S2 K(fov, w, h); -static boost::shared_ptr shK(new Cal3_S2(K)); - -// typedefs for visual SLAM example -typedef NonlinearFactorGraph VGraph; - -// factors for visual slam -typedef NonlinearEquality2 Point3Equality; - //****************************************************************************** TEST (testNonlinearEqualityConstraint, stereo_constrained ) { + // make a realistic calibration matrix + static double fov = 60; // degrees + static int w = 640, h = 480; + static Cal3_S2 K(fov, w, h); + static boost::shared_ptr shK(new Cal3_S2(K)); + // create initial estimates - Rot3 faceDownY( - (Matrix) (Matrix(3, 3) << 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, -1.0, 0.0).finished()); - Pose3 pose1(faceDownY, Point3(0,0,0)); // origin, left camera - SimpleCamera camera1(pose1, K); - Pose3 pose2(faceDownY, Point3(2.0, 0.0, 0.0)); // 2 units to the left - SimpleCamera camera2(pose2, K); - Point3 landmark(1.0, 5.0, 0.0); //centered between the cameras, 5 units away + Rot3 faceTowardsY(Point3(1, 0, 0), Point3(0, 0, -1), Point3(0, 1, 0)); + Pose3 poseLeft(faceTowardsY, Point3()); // origin, left camera + SimpleCamera leftCamera(poseLeft, K); + Pose3 poseRight(faceTowardsY, Point3(2, 0, 0)); // 2 units to the right + SimpleCamera rightCamera(poseRight, K); + Point3 landmark(1, 5, 0); //centered between the cameras, 5 units away // keys - Symbol x1('x', 1), x2('x', 2); - Symbol l1('l', 1), l2('l', 2); + Symbol key_x1('x', 1), key_x2('x', 2); + Symbol key_l1('l', 1), key_l2('l', 2); // create graph - VGraph graph; + NonlinearFactorGraph graph; // create equality constraints for poses - graph += NonlinearEquality(x1, camera1.pose()); - graph += NonlinearEquality(x2, camera2.pose()); + graph += NonlinearEquality(key_x1, leftCamera.pose()); + graph += NonlinearEquality(key_x2, rightCamera.pose()); // create factors SharedDiagonal vmodel = noiseModel::Unit::Create(2); graph += GenericProjectionFactor( - camera1.project(landmark), vmodel, x1, l1, shK); + leftCamera.project(landmark), vmodel, key_x1, key_l1, shK); graph += GenericProjectionFactor( - camera2.project(landmark), vmodel, x2, l2, shK); + rightCamera.project(landmark), vmodel, key_x2, key_l2, shK); - // add equality constraint - graph += Point3Equality(l1, l2); + // add equality constraint saying there is only one point + graph += NonlinearEquality2(key_l1, key_l2); // create initial data - Point3 landmark1(0.5, 5.0, 0.0); - Point3 landmark2(1.5, 5.0, 0.0); + Point3 landmark1(0.5, 5, 0); + Point3 landmark2(1.5, 5, 0); Values initValues; - initValues.insert(x1, pose1); - initValues.insert(x2, pose2); - initValues.insert(l1, landmark1); - initValues.insert(l2, landmark2); + initValues.insert(key_x1, poseLeft); + initValues.insert(key_x2, poseRight); + initValues.insert(key_l1, landmark1); + initValues.insert(key_l2, landmark2); // optimize Values actual = LevenbergMarquardtOptimizer(graph, initValues).optimize(); // create config Values truthValues; - truthValues.insert(x1, camera1.pose()); - truthValues.insert(x2, camera2.pose()); - truthValues.insert(l1, landmark); - truthValues.insert(l2, landmark); + truthValues.insert(key_x1, leftCamera.pose()); + truthValues.insert(key_x2, rightCamera.pose()); + truthValues.insert(key_l1, landmark); + truthValues.insert(key_l2, landmark); // check if correct CHECK(assert_equal(truthValues, actual, 1e-5)); From 872f86698bc2ce2b98142c940687dec0e522651f Mon Sep 17 00:00:00 2001 From: Frank Date: Wed, 24 Feb 2016 12:28:30 -0800 Subject: [PATCH 47/90] Fixed a few issues with Point3 typedef path --- gtsam/geometry/tests/testPoint3.cpp | 16 ++++++++-------- gtsam/navigation/GPSFactor.cpp | 7 ++++--- gtsam/nonlinear/tests/testExpression.cpp | 6 +++--- 3 files changed, 15 insertions(+), 14 deletions(-) diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp index e457da887..da7696d0d 100644 --- a/gtsam/geometry/tests/testPoint3.cpp +++ b/gtsam/geometry/tests/testPoint3.cpp @@ -93,20 +93,20 @@ TEST( Point3, dot) { // Use numerical derivatives to calculate the expected Jacobians Matrix H1, H2; - boost::function f = boost::bind(&Point3::dot, _1, _2, // - boost::none, boost::none); + boost::function f = + [](const Point3& p, const Point3& q) { return gtsam::dot(p, q); }; { - p.dot(q, H1, H2); + gtsam::dot(p, q, H1, H2); EXPECT(assert_equal(numericalDerivative21(f, p, q), H1, 1e-9)); EXPECT(assert_equal(numericalDerivative22(f, p, q), H2, 1e-9)); } { - p.dot(r, H1, H2); + gtsam::dot(p, r, H1, H2); EXPECT(assert_equal(numericalDerivative21(f, p, r), H1, 1e-9)); EXPECT(assert_equal(numericalDerivative22(f, p, r), H2, 1e-9)); } { - p.dot(t, H1, H2); + gtsam::dot(p, t, H1, H2); EXPECT(assert_equal(numericalDerivative21(f, p, t), H1, 1e-9)); EXPECT(assert_equal(numericalDerivative22(f, p, t), H2, 1e-9)); } @@ -134,15 +134,15 @@ TEST( Point3, cross2) { // Use numerical derivatives to calculate the expected Jacobians Matrix H1, H2; - boost::function f = boost::bind(&Point3::cross, _1, _2, // + boost::function f = boost::bind(>sam::cross, _1, _2, // boost::none, boost::none); { - p.cross(q, H1, H2); + gtsam::cross(p, q, H1, H2); EXPECT(assert_equal(numericalDerivative21(f, p, q), H1, 1e-9)); EXPECT(assert_equal(numericalDerivative22(f, p, q), H2, 1e-9)); } { - p.cross(r, H1, H2); + gtsam::cross(p, r, H1, H2); EXPECT(assert_equal(numericalDerivative21(f, p, r), H1, 1e-9)); EXPECT(assert_equal(numericalDerivative22(f, p, r), H2, 1e-9)); } diff --git a/gtsam/navigation/GPSFactor.cpp b/gtsam/navigation/GPSFactor.cpp index 782c98148..05b7259bf 100644 --- a/gtsam/navigation/GPSFactor.cpp +++ b/gtsam/navigation/GPSFactor.cpp @@ -66,20 +66,21 @@ pair GPSFactor::EstimateState(double t1, const Point3& NED1, //*************************************************************************** void GPSFactor2::print(const string& s, const KeyFormatter& keyFormatter) const { cout << s << "GPSFactor2 on " << keyFormatter(key()) << "\n"; - nT_.print(" GPS measurement: "); + cout << " GPS measurement: " << nT_.transpose() << endl; noiseModel_->print(" noise model: "); } //*************************************************************************** bool GPSFactor2::equals(const NonlinearFactor& expected, double tol) const { const This* e = dynamic_cast(&expected); - return e != NULL && Base::equals(*e, tol) && nT_.equals(e->nT_, tol); + return e != NULL && Base::equals(*e, tol) && + traits::Equals(nT_, e->nT_, tol); } //*************************************************************************** Vector GPSFactor2::evaluateError(const NavState& p, boost::optional H) const { - return p.position(H).vector() -nT_.vector(); + return p.position(H) -nT_; } //*************************************************************************** diff --git a/gtsam/nonlinear/tests/testExpression.cpp b/gtsam/nonlinear/tests/testExpression.cpp index bbf19869d..43f1cd017 100644 --- a/gtsam/nonlinear/tests/testExpression.cpp +++ b/gtsam/nonlinear/tests/testExpression.cpp @@ -372,7 +372,7 @@ TEST(Expression, TripleSum) { /* ************************************************************************* */ TEST(Expression, SumOfUnaries) { const Key key(67); - const double_ norm_(Point3_(key), &Point3::norm); + const double_ norm_(>sam::norm, Point3_(key)); const double_ sum_ = norm_ + norm_; Values values; @@ -391,7 +391,7 @@ TEST(Expression, SumOfUnaries) { TEST(Expression, UnaryOfSum) { const Key key1(42), key2(67); const Point3_ sum_ = Point3_(key1) + Point3_(key2); - const double_ norm_(sum_, &Point3::norm); + const double_ norm_(>sam::norm, sum_); map actual_dims, expected_dims = map_list_of(key1, 3)(key2, 3); norm_.dims(actual_dims); @@ -455,7 +455,7 @@ TEST(Expression, Subtract) { /* ************************************************************************* */ TEST(Expression, LinearExpression) { const Key key(67); - const boost::function f = boost::bind(&Point3::vector, _1); + const boost::function f = [](const Point3& p) { return (Vector3)p; }; const Matrix3 kIdentity = I_3x3; const Expression linear_ = linearExpression(f, Point3_(key), kIdentity); From 3226ec1d6a54ca256e5440f3ae4890aeb04c41c3 Mon Sep 17 00:00:00 2001 From: Enrique Fernandez Date: Wed, 24 Feb 2016 16:28:04 -0500 Subject: [PATCH 48/90] :lipstick: --- cmake/GtsamBuildTypes.cmake | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/cmake/GtsamBuildTypes.cmake b/cmake/GtsamBuildTypes.cmake index 43ae36929..a17d3b465 100644 --- a/cmake/GtsamBuildTypes.cmake +++ b/cmake/GtsamBuildTypes.cmake @@ -48,7 +48,7 @@ if(NOT FIRST_PASS_DONE) set(CMAKE_C_FLAGS_PROFILING "-std=c11 -g -O3 -Wall -DNDEBUG" CACHE STRING "Flags used by the compiler during profiling builds." FORCE) set(CMAKE_CXX_FLAGS_PROFILING "-std=c++11 -g -O3 -Wall -DNDEBUG" CACHE STRING "Flags used by the compiler during profiling builds." FORCE) set(CMAKE_EXE_LINKER_FLAGS_PROFILING "${CMAKE_EXE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during profiling builds." FORCE) - set(CMAKE_SHARED_LINKER_FLAGS_PROFILING "${CMAKE__LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during profiling builds." FORCE) + set(CMAKE_SHARED_LINKER_FLAGS_PROFILING "${CMAKE__LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during profiling builds." FORCE) mark_as_advanced(CMAKE_C_FLAGS_PROFILING CMAKE_CXX_FLAGS_PROFILING CMAKE_EXE_LINKER_FLAGS_PROFILING CMAKE_SHARED_LINKER_FLAGS_PROFILING) endif() endif() @@ -105,7 +105,7 @@ endif() # Mark that first pass is done set(FIRST_PASS_DONE TRUE CACHE INTERNAL "Internally used to mark whether cmake has been run multiple times") -mark_as_advanced(FIRST_PASS_DONE) +mark_as_advanced(FIRST_PASS_DONE) # Enable Visual Studio solution folders set_property(GLOBAL PROPERTY USE_FOLDERS On) From 443bb0776bce839248b5854280452c5ca036340f Mon Sep 17 00:00:00 2001 From: Enrique Fernandez Date: Wed, 24 Feb 2016 16:55:22 -0500 Subject: [PATCH 49/90] Set FLAGS for CMAKE_BUILD_TYPE None MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Set the FLAGS to the ones from RelWithDebInfo CMAKE_BUILD_TYPE See https://cmake.org/Wiki/CMake_Useful_Variables#Compilers_and_Tools Without this, the C and CXX FLAGS are this (wrong): -- C compilation flags : -g -O2 -fstack-protector --param=ssp-buffer-size=4 -Wformat -Werror=format-security -D_FORTIFY_SOURCE=2 -- C++ compilation flags : -g -O2 -fstack-protector --param=ssp-buffer-size=4 -Wformat -Werror=format-security -D_FORTIFY_SOURCE=2 and the compilation fails with this and similar errors (because C++11 is needed): gtsam/navigation/ImuFactor.cpp:144:15: error: ‘nullptr’ was not declared in this scope return e != nullptr && base && pim; With this changes, the C and CXX FLAGS are this (good): -- C compilation flags : -std=c11 -g -O3 -Wall -DNDEBUG -- C++ compilation flags : -std=c++11 -g -O3 -Wall -DNDEBUG and everything compiles. --- cmake/GtsamBuildTypes.cmake | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/cmake/GtsamBuildTypes.cmake b/cmake/GtsamBuildTypes.cmake index a17d3b465..c8cd9df35 100644 --- a/cmake/GtsamBuildTypes.cmake +++ b/cmake/GtsamBuildTypes.cmake @@ -14,6 +14,8 @@ option(GTSAM_BUILD_TYPE_POSTFIXES "Enable/Disable appending the build typ # Add debugging flags but only on the first pass if(NOT FIRST_PASS_DONE) + # Set all CMAKE_BUILD_TYPE flags: + # (see https://cmake.org/Wiki/CMake_Useful_Variables#Compilers_and_Tools) if(MSVC) set(CMAKE_C_FLAGS_DEBUG "/D_DEBUG /MDd /Zi /Ob0 /Od /RTC1 /W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN /DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Flags used by the compiler during debug builds." FORCE) set(CMAKE_CXX_FLAGS_DEBUG "/D_DEBUG /MDd /Zi /Ob0 /Od /RTC1 /W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN /DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Flags used by the compiler during debug builds." FORCE) @@ -51,6 +53,9 @@ if(NOT FIRST_PASS_DONE) set(CMAKE_SHARED_LINKER_FLAGS_PROFILING "${CMAKE__LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during profiling builds." FORCE) mark_as_advanced(CMAKE_C_FLAGS_PROFILING CMAKE_CXX_FLAGS_PROFILING CMAKE_EXE_LINKER_FLAGS_PROFILING CMAKE_SHARED_LINKER_FLAGS_PROFILING) endif() + # Make CMAKE_BUILD_TYPE=None flags default to the CMAKE_BUILD_TYPE=RelWithDebInfo ones: + set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS_RELWITHDEBINFO}" CACHE STRING "Flags used by the compiler during none builds." FORCE) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS_RELWITHDEBINFO}" CACHE STRING "Flags used by the compiler during none builds." FORCE) endif() # Clang uses a template depth that is less than standard and is too small From 8e6baf9e7e4ccf3387f1b22b3644d95e9e6dab62 Mon Sep 17 00:00:00 2001 From: Frank Date: Wed, 24 Feb 2016 14:44:05 -0800 Subject: [PATCH 50/90] Fixed Point3 constructor --- tests/testNonlinearEquality.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/tests/testNonlinearEquality.cpp b/tests/testNonlinearEquality.cpp index a9ce8ff0a..f72e08083 100644 --- a/tests/testNonlinearEquality.cpp +++ b/tests/testNonlinearEquality.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -525,10 +525,13 @@ TEST (testNonlinearEqualityConstraint, stereo_constrained ) { // create initial estimates Rot3 faceTowardsY(Point3(1, 0, 0), Point3(0, 0, -1), Point3(0, 1, 0)); - Pose3 poseLeft(faceTowardsY, Point3()); // origin, left camera + + Pose3 poseLeft(faceTowardsY, Point3(0, 0, 0)); // origin, left camera SimpleCamera leftCamera(poseLeft, K); - Pose3 poseRight(faceTowardsY, Point3(2, 0, 0)); // 2 units to the right + + Pose3 poseRight(faceTowardsY, Point3(2, 0, 0)); // 2 units to the right SimpleCamera rightCamera(poseRight, K); + Point3 landmark(1, 5, 0); //centered between the cameras, 5 units away // keys From e76f838d2f606094a435da22b48b8b2f44c5c78f Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 25 Feb 2016 09:34:13 -0800 Subject: [PATCH 51/90] Prototype cacheing --- gtsam/slam/SmartFactorBase.h | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index 01a8fcf8d..51e53769e 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -80,6 +80,9 @@ protected: typedef Eigen::Matrix VectorD; typedef Eigen::Matrix Matrix2; + // Cache for Fblocks, to avoid a malloc ever time we re-linearize + mutable std::vector Fblocks; + public: // Definitions for blocks of F, externally visible @@ -98,8 +101,10 @@ public: /// Constructor SmartFactorBase(const SharedNoiseModel& sharedNoiseModel, - boost::optional body_P_sensor = boost::none) : - body_P_sensor_(body_P_sensor){ + boost::optional body_P_sensor = boost::none, size_t expectedNumberCameras=10) : + body_P_sensor_(body_P_sensor) { + + Fblocks.reserve(expectedNumberCameras); if (!sharedNoiseModel) throw std::runtime_error("SmartFactorBase: sharedNoiseModel is required"); @@ -283,7 +288,6 @@ public: const Cameras& cameras, const Point3& point, const double lambda = 0.0, bool diagonalDamping = false) const { - std::vector Fblocks; Matrix E; Vector b; computeJacobians(Fblocks, E, b, cameras, point); From a611cd078dcd9f771ddf45daade5db34392371b0 Mon Sep 17 00:00:00 2001 From: Frank Date: Thu, 25 Feb 2016 16:52:41 -0800 Subject: [PATCH 52/90] Modernized a bit --- gtsam/slam/dataset.cpp | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 8636138f2..3021fe112 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -658,10 +658,11 @@ bool readBundler(const string& filename, SfM_data &data) { Pose3 pose = openGL2gtsam(R, tx, ty, tz); - data.cameras.push_back(SfM_Camera(pose, K)); + data.cameras.emplace_back(pose, K); } // Get the information for the 3D points + data.tracks.reserve(nrPoints); for (size_t j = 0; j < nrPoints; j++) { SfM_Track track; @@ -681,12 +682,14 @@ bool readBundler(const string& filename, SfM_data &data) { size_t nvisible = 0; is >> nvisible; + track.measurements.reserve(nvisible); + track.siftIndices.reserve(nvisible); for (size_t k = 0; k < nvisible; k++) { size_t cam_idx = 0, point_idx = 0; float u, v; is >> cam_idx >> point_idx >> u >> v; - track.measurements.push_back(make_pair(cam_idx, Point2(u, -v))); - track.siftIndices.push_back(make_pair(cam_idx, point_idx)); + track.measurements.emplace_back(cam_idx, Point2(u, -v)); + track.siftIndices.emplace_back(cam_idx, point_idx); } data.tracks.push_back(track); @@ -716,7 +719,7 @@ bool readBAL(const string& filename, SfM_data &data) { size_t i = 0, j = 0; float u, v; is >> i >> j >> u >> v; - data.tracks[j].measurements.push_back(make_pair(i, Point2(u, -v))); + data.tracks[j].emplace_back(i, Point2(u, -v)); } // Get the information for the camera poses @@ -737,7 +740,7 @@ bool readBAL(const string& filename, SfM_data &data) { is >> f >> k1 >> k2; Cal3Bundler K(f, k1, k2); - data.cameras.push_back(SfM_Camera(pose, K)); + data.cameras.emplace_back(pose, K); } // Get the information for the 3D points From 55bebdc72daca9b9f47910affb52d4dd5c552990 Mon Sep 17 00:00:00 2001 From: Frank Date: Thu, 25 Feb 2016 16:53:17 -0800 Subject: [PATCH 53/90] Got rid of obsolete types and mate FBlocks a mutable cache --- gtsam/slam/SmartFactorBase.h | 33 +++++++++++---------------------- 1 file changed, 11 insertions(+), 22 deletions(-) diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index 51e53769e..eca7f531b 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -50,6 +50,12 @@ private: typedef SmartFactorBase This; typedef typename CAMERA::Measurement Z; +public: + + static const int Dim = traits::dimension; ///< Camera dimension + static const int ZDim = traits::dimension; ///< Measurement dimension + typedef Eigen::Matrix MatrixZD; // F blocks (derivatives wrpt camera) + protected: /** * As of Feb 22, 2015, the noise model is the same for all measurements and @@ -70,24 +76,11 @@ protected: const boost::optional body_P_sensor_; ///< Pose of the camera in the body frame /// @} - static const int Dim = traits::dimension; ///< Camera dimension - static const int ZDim = traits::dimension; ///< Measurement dimension - - // Definitions for block matrices used internally - typedef Eigen::Matrix MatrixD2; // F' - typedef Eigen::Matrix MatrixDD; // camera hessian block - typedef Eigen::Matrix Matrix23; - typedef Eigen::Matrix VectorD; - typedef Eigen::Matrix Matrix2; - // Cache for Fblocks, to avoid a malloc ever time we re-linearize mutable std::vector Fblocks; public: - // Definitions for blocks of F, externally visible - typedef Eigen::Matrix MatrixZD; // F - EIGEN_MAKE_ALIGNED_OPERATOR_NEW /// shorthand for a smart pointer to a factor @@ -101,10 +94,9 @@ public: /// Constructor SmartFactorBase(const SharedNoiseModel& sharedNoiseModel, - boost::optional body_P_sensor = boost::none, size_t expectedNumberCameras=10) : - body_P_sensor_(body_P_sensor) { - - Fblocks.reserve(expectedNumberCameras); + boost::optional body_P_sensor = boost::none, + size_t expectedNumberCameras = 10) + : body_P_sensor_(body_P_sensor), Fblocks(expectedNumberCameras) { if (!sharedNoiseModel) throw std::runtime_error("SmartFactorBase: sharedNoiseModel is required"); @@ -293,8 +285,7 @@ public: computeJacobians(Fblocks, E, b, cameras, point); // build augmented hessian - SymmetricBlockMatrix augmentedHessian = Cameras::SchurComplement(Fblocks, E, - b); + SymmetricBlockMatrix augmentedHessian = Cameras::SchurComplement(Fblocks, E, b); return boost::make_shared >(keys_, augmentedHessian); @@ -311,10 +302,8 @@ public: const FastVector allKeys) const { Matrix E; Vector b; - std::vector Fblocks; computeJacobians(Fblocks, E, b, cameras, point); - Cameras::UpdateSchurComplement(Fblocks, E, b, allKeys, keys_, - augmentedHessian); + Cameras::UpdateSchurComplement(Fblocks, E, b, allKeys, keys_, augmentedHessian); } /// Whiten the Jacobians computed by computeJacobians using noiseModel_ From e7ba5515230134d21e685fd83752fd65c5b4277f Mon Sep 17 00:00:00 2001 From: Frank Date: Thu, 25 Feb 2016 17:06:31 -0800 Subject: [PATCH 54/90] I believe that this is the right way: CMAKE_CXX_FLAGS are set to -std=c++11 -Wall, and the different build types *append* to it. E.g., for Debug, the CMAKE_CXX_FLAGS_DEBUG (set from GTSAM_CMAKE_CXX_FLAGS_DEBUG) will be appended automatically. --- cmake/GtsamBuildTypes.cmake | 52 ++++++++++++++++++++----------------- 1 file changed, 28 insertions(+), 24 deletions(-) diff --git a/cmake/GtsamBuildTypes.cmake b/cmake/GtsamBuildTypes.cmake index 5a944aee2..99fa5d598 100644 --- a/cmake/GtsamBuildTypes.cmake +++ b/cmake/GtsamBuildTypes.cmake @@ -18,27 +18,31 @@ option(GTSAM_BUILD_TYPE_POSTFIXES "Enable/Disable appending the build typ # Set all CMAKE_BUILD_TYPE flags: # (see https://cmake.org/Wiki/CMake_Useful_Variables#Compilers_and_Tools) if(MSVC) - set(GTSAM_CMAKE_C_FLAGS_DEBUG "/D_DEBUG /MDd /Zi /Ob0 /Od /RTC1 /W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN /DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Flags used by the compiler during debug builds.") - set(GTSAM_CMAKE_CXX_FLAGS_DEBUG "/D_DEBUG /MDd /Zi /Ob0 /Od /RTC1 /W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN /DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Flags used by the compiler during debug builds.") - set(GTSAM_CMAKE_C_FLAGS_RELWITHDEBINFO "/MD /O2 /DNDEBUG /W3 /GR /EHsc /MP /Zi /d2Zi+ /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during relwithdebinfo builds.") - set(GTSAM_CMAKE_CXX_FLAGS_RELWITHDEBINFO "/MD /O2 /DNDEBUG /W3 /GR /EHsc /MP /Zi /d2Zi+ /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during relwithdebinfo builds.") - set(GTSAM_CMAKE_C_FLAGS_RELEASE "/MD /O2 /DNDEBUG /W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during release builds.") - set(GTSAM_CMAKE_CXX_FLAGS_RELEASE "/MD /O2 /DNDEBUG /W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during release builds.") - set(GTSAM_CMAKE_C_FLAGS_PROFILING "${GTSAM_CMAKE_C_FLAGS_RELEASE} /Zi" CACHE STRING "Flags used by the compiler during profiling builds.") - set(GTSAM_CMAKE_CXX_FLAGS_PROFILING "${GTSAM_CMAKE_CXX_FLAGS_RELEASE} /Zi" CACHE STRING "Flags used by the compiler during profiling builds.") - set(GTSAM_CMAKE_C_FLAGS_TIMING "${GTSAM_CMAKE_C_FLAGS_RELEASE} /DENABLE_TIMING" CACHE STRING "Flags used by the compiler during timing builds.") - set(GTSAM_CMAKE_CXX_FLAGS_TIMING "${GTSAM_CMAKE_CXX_FLAGS_RELEASE} /DENABLE_TIMING" CACHE STRING "Flags used by the compiler during timing builds.") + set(GTSAM_CMAKE_C_FLAGS "/W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler for all builds.") + set(GTSAM_CMAKE_CXX_FLAGS "/W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler for all builds.") + set(GTSAM_CMAKE_C_FLAGS_DEBUG "/D_DEBUG /MDd /Zi /Ob0 /Od /RTC1 /DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Extra flags used by the compiler during debug builds.") + set(GTSAM_CMAKE_CXX_FLAGS_DEBUG "/D_DEBUG /MDd /Zi /Ob0 /Od /RTC1 /DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Extra flags used by the compiler during debug builds.") + set(GTSAM_CMAKE_C_FLAGS_RELWITHDEBINFO "/MD /O2 /DNDEBUG /Zi /d2Zi+" CACHE STRING "Extra flags used by the compiler during relwithdebinfo builds.") + set(GTSAM_CMAKE_CXX_FLAGS_RELWITHDEBINFO "/MD /O2 /DNDEBUG /Zi /d2Zi+" CACHE STRING "Extra flags used by the compiler during relwithdebinfo builds.") + set(GTSAM_CMAKE_C_FLAGS_RELEASE "/MD /O2 /DNDEBUG" CACHE STRING "Extra flags used by the compiler during release builds.") + set(GTSAM_CMAKE_CXX_FLAGS_RELEASE "/MD /O2 /DNDEBUG" CACHE STRING "Extra flags used by the compiler during release builds.") + set(GTSAM_CMAKE_C_FLAGS_PROFILING "${GTSAM_CMAKE_C_FLAGS_RELEASE} /Zi" CACHE STRING "Extra flags used by the compiler during profiling builds.") + set(GTSAM_CMAKE_CXX_FLAGS_PROFILING "${GTSAM_CMAKE_CXX_FLAGS_RELEASE} /Zi" CACHE STRING "Extra flags used by the compiler during profiling builds.") + set(GTSAM_CMAKE_C_FLAGS_TIMING "${GTSAM_CMAKE_C_FLAGS_RELEASE} /DENABLE_TIMING" CACHE STRING "Extra flags used by the compiler during timing builds.") + set(GTSAM_CMAKE_CXX_FLAGS_TIMING "${GTSAM_CMAKE_CXX_FLAGS_RELEASE} /DENABLE_TIMING" CACHE STRING "Extra flags used by the compiler during timing builds.") else() - set(GTSAM_CMAKE_C_FLAGS_DEBUG "-std=c11 -Wall -g -fno-inline -DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Flags used by the compiler during debug builds.") - set(GTSAM_CMAKE_CXX_FLAGS_DEBUG "-std=c++11 -Wall -g -fno-inline -DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Flags used by the compiler during debug builds.") - set(GTSAM_CMAKE_C_FLAGS_RELWITHDEBINFO "-std=c11 -Wall -g -O3 -DNDEBUG" CACHE STRING "Flags used by the compiler during relwithdebinfo builds.") - set(GTSAM_CMAKE_CXX_FLAGS_RELWITHDEBINFO "-std=c++11 -Wall -g -O3 -DNDEBUG" CACHE STRING "Flags used by the compiler during relwithdebinfo builds.") - set(GTSAM_CMAKE_C_FLAGS_RELEASE "-std=c11 -Wall -O3 -DNDEBUG" CACHE STRING "Flags used by the compiler during release builds.") - set(GTSAM_CMAKE_CXX_FLAGS_RELEASE "-std=c++11 -Wall -O3 -DNDEBUG" CACHE STRING "Flags used by the compiler during release builds.") - set(GTSAM_CMAKE_C_FLAGS_PROFILING "${GTSAM_CMAKE_C_FLAGS_RELEASE}" CACHE STRING "Flags used by the compiler during profiling builds.") - set(GTSAM_CMAKE_CXX_FLAGS_PROFILING "${GTSAM_CMAKE_CXX_FLAGS_RELEASE}" CACHE STRING "Flags used by the compiler during profiling builds.") - set(GTSAM_CMAKE_C_FLAGS_TIMING "${GTSAM_CMAKE_C_FLAGS_RELEASE} -DENABLE_TIMING" CACHE STRING "Flags used by the compiler during timing builds.") - set(GTSAM_CMAKE_CXX_FLAGS_TIMING "${GTSAM_CMAKE_CXX_FLAGS_RELEASE} -DENABLE_TIMING" CACHE STRING "Flags used by the compiler during timing builds.") + set(GTSAM_CMAKE_C_FLAGS "-std=c11 -Wall" CACHE STRING "Flags used by the compiler for all builds.") + set(GTSAM_CMAKE_CXX_FLAGS "-std=c++11 -Wall" CACHE STRING "Flags used by the compiler for all builds.") + set(GTSAM_CMAKE_C_FLAGS_DEBUG "-g -fno-inline -DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Extra flags used by the compiler during debug builds.") + set(GTSAM_CMAKE_CXX_FLAGS_DEBUG "-g -fno-inline -DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Extra flags used by the compiler during debug builds.") + set(GTSAM_CMAKE_C_FLAGS_RELWITHDEBINFO "-g -O3 -DNDEBUG" CACHE STRING "Extra flags used by the compiler during relwithdebinfo builds.") + set(GTSAM_CMAKE_CXX_FLAGS_RELWITHDEBINFO "-g -O3 -DNDEBUG" CACHE STRING "Extra flags used by the compiler during relwithdebinfo builds.") + set(GTSAM_CMAKE_C_FLAGS_RELEASE " -O3 -DNDEBUG" CACHE STRING "Extra flags used by the compiler during release builds.") + set(GTSAM_CMAKE_CXX_FLAGS_RELEASE " -O3 -DNDEBUG" CACHE STRING "Extra flags used by the compiler during release builds.") + set(GTSAM_CMAKE_C_FLAGS_PROFILING "${GTSAM_CMAKE_C_FLAGS_RELEASE}" CACHE STRING "Extra flags used by the compiler during profiling builds.") + set(GTSAM_CMAKE_CXX_FLAGS_PROFILING "${GTSAM_CMAKE_CXX_FLAGS_RELEASE}" CACHE STRING "Extra flags used by the compiler during profiling builds.") + set(GTSAM_CMAKE_C_FLAGS_TIMING "${GTSAM_CMAKE_C_FLAGS_RELEASE} -DENABLE_TIMING" CACHE STRING "Extra flags used by the compiler during timing builds.") + set(GTSAM_CMAKE_CXX_FLAGS_TIMING "${GTSAM_CMAKE_CXX_FLAGS_RELEASE} -DENABLE_TIMING" CACHE STRING "Extra flags used by the compiler during timing builds.") endif() set(GTSAM_CMAKE_SHARED_LINKER_FLAGS_TIMING "${CMAKE_SHARED_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during timing builds.") @@ -54,6 +58,10 @@ mark_as_advanced(GTSAM_CMAKE_C_FLAGS_TIMING GTSAM_CMAKE_CXX_FLAGS_TIMING GTSAM_C GTSAM_CMAKE_C_FLAGS_PROFILING GTSAM_CMAKE_CXX_FLAGS_PROFILING GTSAM_CMAKE_EXE_LINKER_FLAGS_PROFILING GTSAM_CMAKE_SHARED_LINKER_FLAGS_PROFILING GTSAM_CMAKE_MODULE_LINKER_FLAGS_PROFILING) +# Make CMAKE_BUILD_TYPE=None flags default to the CMAKE_BUILD_TYPE=RelWithDebInfo ones: +set(CMAKE_C_FLAGS ${GTSAM_CMAKE_C_FLAGS}) +set(CMAKE_CXX_FLAGS ${GTSAM_CMAKE_CXX_FLAGS}) + # Apply the gtsam specific build flags as normal variables. This makes it so that they only # apply to the gtsam part of the build if gtsam is built as a subproject set(CMAKE_C_FLAGS_DEBUG ${GTSAM_CMAKE_C_FLAGS_DEBUG}) @@ -67,10 +75,6 @@ set(CMAKE_CXX_FLAGS_PROFILING ${GTSAM_CMAKE_CXX_FLAGS_PROFILING}) set(CMAKE_C_FLAGS_TIMING ${GTSAM_CMAKE_C_FLAGS_TIMING}) set(CMAKE_CXX_FLAGS_TIMING ${GTSAM_CMAKE_CXX_FLAGS_TIMING}) -# Make CMAKE_BUILD_TYPE=None flags default to the CMAKE_BUILD_TYPE=RelWithDebInfo ones: -set(CMAKE_C_FLAGS ${GTSAM_CMAKE_C_FLAGS_RELWITHDEBINFO}) -set(CMAKE_CXX_FLAGS ${GTSAM_CMAKE_CXX_FLAGS_RELWITHDEBINFO}) - set(CMAKE_SHARED_LINKER_FLAGS_TIMING ${GTSAM_CMAKE_SHARED_LINKER_FLAGS_TIMING}) set(CMAKE_MODULE_LINKER_FLAGS_TIMING ${GTSAM_CMAKE_MODULE_LINKER_FLAGS_TIMING}) set(CMAKE_EXE_LINKER_FLAGS_TIMING ${GTSAM_CMAKE_EXE_LINKER_FLAGS_TIMING}) From 0ff7854f1504e43ef24ef6cd5d5d40d556095491 Mon Sep 17 00:00:00 2001 From: Frank Date: Thu, 25 Feb 2016 18:25:53 -0800 Subject: [PATCH 55/90] Fixed bug --- timing/timeSFMBAL.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/timing/timeSFMBAL.h b/timing/timeSFMBAL.h index cd859326c..24b67557a 100644 --- a/timing/timeSFMBAL.h +++ b/timing/timeSFMBAL.h @@ -54,7 +54,7 @@ SfM_data preamble(int argc, char* argv[]) { filename = argv[argc - 1]; else filename = findExampleDataFile("dubrovnik-16-22106-pre"); - bool success = readBAL(argv[argc - 1], db); + bool success = readBAL(filename, db); if (!success) throw runtime_error("Could not access file!"); return db; } From 5a9ba414f602e4eeb86f2a47426d484cce910b6f Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 26 Feb 2016 04:46:21 +0000 Subject: [PATCH 56/90] Removed obsolete comment --- cmake/GtsamBuildTypes.cmake | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/cmake/GtsamBuildTypes.cmake b/cmake/GtsamBuildTypes.cmake index 99fa5d598..96a0f11f3 100644 --- a/cmake/GtsamBuildTypes.cmake +++ b/cmake/GtsamBuildTypes.cmake @@ -58,12 +58,10 @@ mark_as_advanced(GTSAM_CMAKE_C_FLAGS_TIMING GTSAM_CMAKE_CXX_FLAGS_TIMING GTSAM_C GTSAM_CMAKE_C_FLAGS_PROFILING GTSAM_CMAKE_CXX_FLAGS_PROFILING GTSAM_CMAKE_EXE_LINKER_FLAGS_PROFILING GTSAM_CMAKE_SHARED_LINKER_FLAGS_PROFILING GTSAM_CMAKE_MODULE_LINKER_FLAGS_PROFILING) -# Make CMAKE_BUILD_TYPE=None flags default to the CMAKE_BUILD_TYPE=RelWithDebInfo ones: -set(CMAKE_C_FLAGS ${GTSAM_CMAKE_C_FLAGS}) -set(CMAKE_CXX_FLAGS ${GTSAM_CMAKE_CXX_FLAGS}) - # Apply the gtsam specific build flags as normal variables. This makes it so that they only # apply to the gtsam part of the build if gtsam is built as a subproject +set(CMAKE_C_FLAGS ${GTSAM_CMAKE_C_FLAGS}) +set(CMAKE_CXX_FLAGS ${GTSAM_CMAKE_CXX_FLAGS}) set(CMAKE_C_FLAGS_DEBUG ${GTSAM_CMAKE_C_FLAGS_DEBUG}) set(CMAKE_CXX_FLAGS_DEBUG ${GTSAM_CMAKE_CXX_FLAGS_DEBUG}) set(CMAKE_C_FLAGS_RELWITHDEBINFO ${GTSAM_CMAKE_C_FLAGS_RELWITHDEBINFO}) From 018fbf720f192e3d99837db637c4dec32118b52c Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 25 Feb 2016 21:37:20 -0800 Subject: [PATCH 57/90] Fixed compile issue --- gtsam/slam/dataset.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 3021fe112..14192fcaa 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -719,7 +719,7 @@ bool readBAL(const string& filename, SfM_data &data) { size_t i = 0, j = 0; float u, v; is >> i >> j >> u >> v; - data.tracks[j].emplace_back(i, Point2(u, -v)); + data.tracks[j].measurements.emplace_back(i, Point2(u, -v)); } // Get the information for the camera poses From 64aae16b3cb2fb2844271b50f91546b93a00a656 Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 25 Feb 2016 22:07:04 -0800 Subject: [PATCH 58/90] Fixed problem with wrapping iSAM by a hack: made vector into KeyVector. This is not a long-term solution: we should enable wrapping of vector. --- gtsam/nonlinear/ISAM2.cpp | 2 +- gtsam/nonlinear/ISAM2.h | 2 +- .../nonlinear/ConcurrentIncrementalFilter.cpp | 22 ++++++------- .../ConcurrentIncrementalSmoother.cpp | 10 +++--- .../nonlinear/ConcurrentIncrementalSmoother.h | 4 +-- .../nonlinear/IncrementalFixedLagSmoother.cpp | 32 +++++++++---------- 6 files changed, 36 insertions(+), 36 deletions(-) diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index af37ca954..754623e7e 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -510,7 +510,7 @@ boost::shared_ptr ISAM2::recalculate(const KeySet& markedKeys, const Ke /* ************************************************************************* */ ISAM2Result ISAM2::update( - const NonlinearFactorGraph& newFactors, const Values& newTheta, const vector& removeFactorIndices, + const NonlinearFactorGraph& newFactors, const Values& newTheta, const KeyVector& removeFactorIndices, const boost::optional >& constrainedKeys, const boost::optional >& noRelinKeys, const boost::optional >& extraReelimKeys, bool force_relinearize) { diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index e8165aad9..a921f6e76 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -530,7 +530,7 @@ public: */ virtual ISAM2Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(), - const std::vector& removeFactorIndices = std::vector(), + const KeyVector& removeFactorIndices = KeyVector(), const boost::optional >& constrainedKeys = boost::none, const boost::optional >& noRelinKeys = boost::none, const boost::optional >& extraReelimKeys = boost::none, diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp index 4c4442141..6435bd2df 100644 --- a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp @@ -56,43 +56,43 @@ ConcurrentIncrementalFilter::Result ConcurrentIncrementalFilter::update(const No Result result; // We do not need to remove any factors at this time - gtsam::FastVector removedFactors; + KeyVector removedFactors; if(removeFactorIndices){ removedFactors.insert(removedFactors.end(), removeFactorIndices->begin(), removeFactorIndices->end()); } // Generate ordering constraints that force the 'keys to move' to the end - boost::optional > orderingConstraints = boost::none; + boost::optional > orderingConstraints = boost::none; if(keysToMove && keysToMove->size() > 0) { - orderingConstraints = gtsam::FastMap(); + orderingConstraints = FastMap(); int group = 1; // Set all existing variables to Group1 if(isam2_.getLinearizationPoint().size() > 0) { - BOOST_FOREACH(const gtsam::Values::ConstKeyValuePair& key_value, isam2_.getLinearizationPoint()) { + BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, isam2_.getLinearizationPoint()) { orderingConstraints->operator[](key_value.key) = group; } ++group; } // Assign new variables to the root - BOOST_FOREACH(const gtsam::Values::ConstKeyValuePair& key_value, newTheta) { + BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, newTheta) { orderingConstraints->operator[](key_value.key) = group; } // Set marginalizable variables to Group0 - BOOST_FOREACH(gtsam::Key key, *keysToMove){ + BOOST_FOREACH(Key key, *keysToMove){ orderingConstraints->operator[](key) = 0; } } // Create the set of linear keys that iSAM2 should hold constant // iSAM2 takes care of this for us; no need to specify additional noRelin keys - boost::optional > noRelinKeys = boost::none; + boost::optional > noRelinKeys = boost::none; // Mark additional keys between the 'keys to move' and the leaves boost::optional > additionalKeys = boost::none; if(keysToMove && keysToMove->size() > 0) { std::set markedKeys; - BOOST_FOREACH(gtsam::Key key, *keysToMove) { + BOOST_FOREACH(Key key, *keysToMove) { if(isam2_.getLinearizationPoint().exists(key)) { ISAM2Clique::shared_ptr clique = isam2_[key]; GaussianConditional::const_iterator key_iter = clique->conditional()->begin(); @@ -100,7 +100,7 @@ ConcurrentIncrementalFilter::Result ConcurrentIncrementalFilter::update(const No markedKeys.insert(*key_iter); ++key_iter; } - BOOST_FOREACH(const gtsam::ISAM2Clique::shared_ptr& child, clique->children) { + BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children) { RecursiveMarkAffectedKeys(key, child, markedKeys); } } @@ -110,7 +110,7 @@ ConcurrentIncrementalFilter::Result ConcurrentIncrementalFilter::update(const No // Update the system using iSAM2 gttic(isam2); - gtsam::ISAM2Result isam2Result = isam2_.update(newFactors, newTheta, removedFactors, orderingConstraints, noRelinKeys, additionalKeys); + ISAM2Result isam2Result = isam2_.update(newFactors, newTheta, removedFactors, orderingConstraints, noRelinKeys, additionalKeys); gttoc(isam2); if(keysToMove && keysToMove->size() > 0) { @@ -210,7 +210,7 @@ void ConcurrentIncrementalFilter::synchronize(const NonlinearFactorGraph& smooth isam2_.params().getEliminationFunction()); // Remove the old factors on the separator and insert the new ones - FastVector removeFactors(currentSmootherSummarizationSlots_.begin(), currentSmootherSummarizationSlots_.end()); + KeyVector removeFactors(currentSmootherSummarizationSlots_.begin(), currentSmootherSummarizationSlots_.end()); ISAM2Result result = isam2_.update(currentSmootherSummarization, Values(), removeFactors, boost::none, noRelinKeys, boost::none, false); currentSmootherSummarizationSlots_ = result.newFactorsIndices; diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp b/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp index b87409aae..21b6b3cd0 100644 --- a/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp @@ -45,14 +45,14 @@ bool ConcurrentIncrementalSmoother::equals(const ConcurrentSmoother& rhs, double /* ************************************************************************* */ ConcurrentIncrementalSmoother::Result ConcurrentIncrementalSmoother::update(const NonlinearFactorGraph& newFactors, const Values& newTheta, - const boost::optional< std::vector >& removeFactorIndices) { + const boost::optional& removeFactorIndices) { gttic(update); // Create the return result meta-data Result result; - gtsam::FastVector removedFactors; + FastVector removedFactors; if(removeFactorIndices){ // Be very careful to this line @@ -72,7 +72,7 @@ ConcurrentIncrementalSmoother::Result ConcurrentIncrementalSmoother::update(cons } // Use iSAM2 to perform an update - gtsam::ISAM2Result isam2Result; + ISAM2Result isam2Result; if(isam2_.getFactorsUnsafe().size() + newFactors.size() + smootherFactors_.size() + filterSummarizationFactors_.size() > 0) { if(synchronizationUpdatesAvailable_) { // Augment any new factors/values with the cached data from the last synchronization @@ -106,7 +106,7 @@ ConcurrentIncrementalSmoother::Result ConcurrentIncrementalSmoother::update(cons synchronizationUpdatesAvailable_ = false; } else { // Update the system using iSAM2 - isam2Result = isam2_.update(newFactors, newTheta, FastVector(), constrainedKeys, noRelinKeys); + isam2Result = isam2_.update(newFactors, newTheta, KeyVector(), constrainedKeys, noRelinKeys); } } @@ -245,7 +245,7 @@ void ConcurrentIncrementalSmoother::updateSmootherSummarization() { } // Get the set of separator keys - gtsam::KeySet separatorKeys; + KeySet separatorKeys; BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { separatorKeys.insert(key_value.key); } diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.h b/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.h index 78bab5079..564ed3e52 100644 --- a/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.h +++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.h @@ -109,7 +109,7 @@ public: * and additionally, variables that were already in the system must not be included here. */ Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(), - const boost::optional< std::vector >& removeFactorIndices = boost::none); + const boost::optional& removeFactorIndices = boost::none); /** * Perform any required operations before the synchronization process starts. @@ -152,7 +152,7 @@ protected: Values smootherValues_; ///< New variables to be added to the smoother during the next update NonlinearFactorGraph filterSummarizationFactors_; ///< New filter summarization factors to replace the existing filter summarization during the next update Values separatorValues_; ///< The linearization points of the separator variables. These should not be changed during optimization. - FastVector filterSummarizationSlots_; ///< The slots in factor graph that correspond to the current filter summarization factors + KeyVector filterSummarizationSlots_; ///< The slots in factor graph that correspond to the current filter summarization factors bool synchronizationUpdatesAvailable_; ///< Flag indicating the currently stored synchronization updates have not been applied yet // Storage for information to be sent to the filter diff --git a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp index 23cd42a0a..2265cab5e 100644 --- a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp @@ -115,10 +115,10 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update( } // Mark additional keys between the marginalized keys and the leaves - std::set additionalKeys; - BOOST_FOREACH(gtsam::Key key, marginalizableKeys) { - gtsam::ISAM2Clique::shared_ptr clique = isam_[key]; - BOOST_FOREACH(const gtsam::ISAM2Clique::shared_ptr& child, clique->children) { + std::set additionalKeys; + BOOST_FOREACH(Key key, marginalizableKeys) { + ISAM2Clique::shared_ptr clique = isam_[key]; + BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children) { recursiveMarkAffectedKeys(key, child, additionalKeys); } } @@ -126,7 +126,7 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update( // Update iSAM2 ISAM2Result isamResult = isam_.update(newFactors, newTheta, - FastVector(), constrainedKeys, boost::none, additionalMarkedKeys); + KeyVector(), constrainedKeys, boost::none, additionalMarkedKeys); if (debug) { PrintSymbolicTree(isam_, @@ -194,8 +194,8 @@ void IncrementalFixedLagSmoother::createOrderingConstraints( void IncrementalFixedLagSmoother::PrintKeySet(const std::set& keys, const std::string& label) { std::cout << label; - BOOST_FOREACH(gtsam::Key key, keys) { - std::cout << " " << gtsam::DefaultKeyFormatter(key); + BOOST_FOREACH(Key key, keys) { + std::cout << " " << DefaultKeyFormatter(key); } std::cout << std::endl; } @@ -204,8 +204,8 @@ void IncrementalFixedLagSmoother::PrintKeySet(const std::set& keys, void IncrementalFixedLagSmoother::PrintSymbolicFactor( const GaussianFactor::shared_ptr& factor) { std::cout << "f("; - BOOST_FOREACH(gtsam::Key key, factor->keys()) { - std::cout << " " << gtsam::DefaultKeyFormatter(key); + BOOST_FOREACH(Key key, factor->keys()) { + std::cout << " " << DefaultKeyFormatter(key); } std::cout << " )" << std::endl; } @@ -220,7 +220,7 @@ void IncrementalFixedLagSmoother::PrintSymbolicGraph( } /* ************************************************************************* */ -void IncrementalFixedLagSmoother::PrintSymbolicTree(const gtsam::ISAM2& isam, +void IncrementalFixedLagSmoother::PrintSymbolicTree(const ISAM2& isam, const std::string& label) { std::cout << label << std::endl; if (!isam.roots().empty()) { @@ -233,22 +233,22 @@ void IncrementalFixedLagSmoother::PrintSymbolicTree(const gtsam::ISAM2& isam, /* ************************************************************************* */ void IncrementalFixedLagSmoother::PrintSymbolicTreeHelper( - const gtsam::ISAM2Clique::shared_ptr& clique, const std::string indent) { + const ISAM2Clique::shared_ptr& clique, const std::string indent) { // Print the current clique std::cout << indent << "P( "; - BOOST_FOREACH(gtsam::Key key, clique->conditional()->frontals()) { - std::cout << gtsam::DefaultKeyFormatter(key) << " "; + BOOST_FOREACH(Key key, clique->conditional()->frontals()) { + std::cout << DefaultKeyFormatter(key) << " "; } if (clique->conditional()->nrParents() > 0) std::cout << "| "; - BOOST_FOREACH(gtsam::Key key, clique->conditional()->parents()) { - std::cout << gtsam::DefaultKeyFormatter(key) << " "; + BOOST_FOREACH(Key key, clique->conditional()->parents()) { + std::cout << DefaultKeyFormatter(key) << " "; } std::cout << ")" << std::endl; // Recursively print all of the children - BOOST_FOREACH(const gtsam::ISAM2Clique::shared_ptr& child, clique->children) { + BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children) { PrintSymbolicTreeHelper(child, indent + " "); } } From fad9462661d1f1ad651e91068f532398b7cad434 Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 25 Feb 2016 23:51:01 -0800 Subject: [PATCH 59/90] A better solution through typedef of FactorIndices -> FastVector --- gtsam.h | 6 +++-- gtsam/nonlinear/ISAM2-impl.cpp | 2 +- gtsam/nonlinear/ISAM2-impl.h | 2 +- gtsam/nonlinear/ISAM2.cpp | 6 ++--- gtsam/nonlinear/ISAM2.h | 9 ++++--- .../nonlinear/ConcurrentIncrementalFilter.cpp | 4 +-- .../ConcurrentIncrementalSmoother.cpp | 4 +-- .../nonlinear/ConcurrentIncrementalSmoother.h | 4 +-- .../nonlinear/IncrementalFixedLagSmoother.cpp | 2 +- tests/testGaussianISAM2.cpp | 26 +++++++++---------- 10 files changed, 34 insertions(+), 31 deletions(-) diff --git a/gtsam.h b/gtsam.h index 3d225529e..c5424ef80 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2161,6 +2161,8 @@ class ISAM2Result { size_t getCliques() const; }; +class FactorIndices {}; + class ISAM2 { ISAM2(); ISAM2(const gtsam::ISAM2Params& params); @@ -2173,8 +2175,8 @@ class ISAM2 { gtsam::ISAM2Result update(); gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta); - gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& removeFactorIndices); - gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& removeFactorIndices, const gtsam::KeyGroupMap& constrainedKeys); + gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices); + gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices, const gtsam::KeyGroupMap& constrainedKeys); // TODO: wrap the full version of update //void update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& removeFactorIndices, FastMap& constrainedKeys); //void update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& removeFactorIndices, FastMap& constrainedKeys, bool force_relinearize); diff --git a/gtsam/nonlinear/ISAM2-impl.cpp b/gtsam/nonlinear/ISAM2-impl.cpp index 4e8c0e303..0211df735 100644 --- a/gtsam/nonlinear/ISAM2-impl.cpp +++ b/gtsam/nonlinear/ISAM2-impl.cpp @@ -46,7 +46,7 @@ void ISAM2::Impl::AddVariables( /* ************************************************************************* */ void ISAM2::Impl::AddFactorsStep1(const NonlinearFactorGraph& newFactors, bool useUnusedSlots, - NonlinearFactorGraph& nonlinearFactors, FastVector& newFactorIndices) + NonlinearFactorGraph& nonlinearFactors, FactorIndices& newFactorIndices) { newFactorIndices.resize(newFactors.size()); diff --git a/gtsam/nonlinear/ISAM2-impl.h b/gtsam/nonlinear/ISAM2-impl.h index d34480144..161932344 100644 --- a/gtsam/nonlinear/ISAM2-impl.h +++ b/gtsam/nonlinear/ISAM2-impl.h @@ -52,7 +52,7 @@ struct GTSAM_EXPORT ISAM2::Impl { /// complete list of nonlinear factors, and populates the list of new factor indices, both /// optionally finding and reusing empty factor slots. static void AddFactorsStep1(const NonlinearFactorGraph& newFactors, bool useUnusedSlots, - NonlinearFactorGraph& nonlinearFactors, FastVector& newFactorIndices); + NonlinearFactorGraph& nonlinearFactors, FactorIndices& newFactorIndices); /** * Remove variables from the ISAM2 system. diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 754623e7e..d6f1a636a 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -510,7 +510,7 @@ boost::shared_ptr ISAM2::recalculate(const KeySet& markedKeys, const Ke /* ************************************************************************* */ ISAM2Result ISAM2::update( - const NonlinearFactorGraph& newFactors, const Values& newTheta, const KeyVector& removeFactorIndices, + const NonlinearFactorGraph& newFactors, const Values& newTheta, const FactorIndices& removeFactorIndices, const boost::optional >& constrainedKeys, const boost::optional >& noRelinKeys, const boost::optional >& extraReelimKeys, bool force_relinearize) { @@ -753,8 +753,8 @@ ISAM2Result ISAM2::update( /* ************************************************************************* */ void ISAM2::marginalizeLeaves(const FastList& leafKeysList, - boost::optional&> marginalFactorsIndices, - boost::optional&> deletedFactorsIndices) + boost::optional marginalFactorsIndices, + boost::optional deletedFactorsIndices) { // Convert to ordered set KeySet leafKeys(leafKeysList.begin(), leafKeysList.end()); diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index a921f6e76..27f21be12 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -257,6 +257,7 @@ struct GTSAM_EXPORT ISAM2Params { /// @} }; +typedef FastVector FactorIndices; /** * @addtogroup ISAM2 @@ -318,7 +319,7 @@ struct GTSAM_EXPORT ISAM2Result { * factors passed as \c newFactors to ISAM2::update(). These indices may be * used later to refer to the factors in order to remove them. */ - FastVector newFactorsIndices; + FactorIndices newFactorsIndices; /** A struct holding detailed results, which must be enabled with * ISAM2Params::enableDetailedResults. @@ -530,7 +531,7 @@ public: */ virtual ISAM2Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(), - const KeyVector& removeFactorIndices = KeyVector(), + const FactorIndices& removeFactorIndices = FactorIndices(), const boost::optional >& constrainedKeys = boost::none, const boost::optional >& noRelinKeys = boost::none, const boost::optional >& extraReelimKeys = boost::none, @@ -551,8 +552,8 @@ public: * indices of any factor that was removed during the 'marginalizeLeaves' call */ void marginalizeLeaves(const FastList& leafKeys, - boost::optional&> marginalFactorsIndices = boost::none, - boost::optional&> deletedFactorsIndices = boost::none); + boost::optional marginalFactorsIndices = boost::none, + boost::optional deletedFactorsIndices = boost::none); /// Access the current linearization point const Values& getLinearizationPoint() const { diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp index 6435bd2df..f0c7212c8 100644 --- a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp @@ -56,7 +56,7 @@ ConcurrentIncrementalFilter::Result ConcurrentIncrementalFilter::update(const No Result result; // We do not need to remove any factors at this time - KeyVector removedFactors; + FactorIndices removedFactors; if(removeFactorIndices){ removedFactors.insert(removedFactors.end(), removeFactorIndices->begin(), removeFactorIndices->end()); @@ -210,7 +210,7 @@ void ConcurrentIncrementalFilter::synchronize(const NonlinearFactorGraph& smooth isam2_.params().getEliminationFunction()); // Remove the old factors on the separator and insert the new ones - KeyVector removeFactors(currentSmootherSummarizationSlots_.begin(), currentSmootherSummarizationSlots_.end()); + FactorIndices removeFactors(currentSmootherSummarizationSlots_.begin(), currentSmootherSummarizationSlots_.end()); ISAM2Result result = isam2_.update(currentSmootherSummarization, Values(), removeFactors, boost::none, noRelinKeys, boost::none, false); currentSmootherSummarizationSlots_ = result.newFactorsIndices; diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp b/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp index 21b6b3cd0..268160451 100644 --- a/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp @@ -45,7 +45,7 @@ bool ConcurrentIncrementalSmoother::equals(const ConcurrentSmoother& rhs, double /* ************************************************************************* */ ConcurrentIncrementalSmoother::Result ConcurrentIncrementalSmoother::update(const NonlinearFactorGraph& newFactors, const Values& newTheta, - const boost::optional& removeFactorIndices) { + const boost::optional& removeFactorIndices) { gttic(update); @@ -106,7 +106,7 @@ ConcurrentIncrementalSmoother::Result ConcurrentIncrementalSmoother::update(cons synchronizationUpdatesAvailable_ = false; } else { // Update the system using iSAM2 - isam2Result = isam2_.update(newFactors, newTheta, KeyVector(), constrainedKeys, noRelinKeys); + isam2Result = isam2_.update(newFactors, newTheta, FactorIndices(), constrainedKeys, noRelinKeys); } } diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.h b/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.h index 564ed3e52..2d519bf25 100644 --- a/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.h +++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.h @@ -109,7 +109,7 @@ public: * and additionally, variables that were already in the system must not be included here. */ Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(), - const boost::optional& removeFactorIndices = boost::none); + const boost::optional& removeFactorIndices = boost::none); /** * Perform any required operations before the synchronization process starts. @@ -152,7 +152,7 @@ protected: Values smootherValues_; ///< New variables to be added to the smoother during the next update NonlinearFactorGraph filterSummarizationFactors_; ///< New filter summarization factors to replace the existing filter summarization during the next update Values separatorValues_; ///< The linearization points of the separator variables. These should not be changed during optimization. - KeyVector filterSummarizationSlots_; ///< The slots in factor graph that correspond to the current filter summarization factors + FactorIndices filterSummarizationSlots_; ///< The slots in factor graph that correspond to the current filter summarization factors bool synchronizationUpdatesAvailable_; ///< Flag indicating the currently stored synchronization updates have not been applied yet // Storage for information to be sent to the filter diff --git a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp index 2265cab5e..8623136cd 100644 --- a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp @@ -126,7 +126,7 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update( // Update iSAM2 ISAM2Result isamResult = isam_.update(newFactors, newTheta, - KeyVector(), constrainedKeys, boost::none, additionalMarkedKeys); + FactorIndices(), constrainedKeys, boost::none, additionalMarkedKeys); if (debug) { PrintSymbolicTree(isam_, diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index 58c718726..d358caa35 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -301,9 +301,9 @@ TEST(ISAM2, AddFactorsStep1) expectedNonlinearFactors += PriorFactor(11, Zero, model); expectedNonlinearFactors += PriorFactor(2, Zero, model); - const FastVector expectedNewFactorIndices = list_of(1)(3); + const FactorIndices expectedNewFactorIndices = list_of(1)(3); - FastVector actualNewFactorIndices; + FactorIndices actualNewFactorIndices; ISAM2::Impl::AddFactorsStep1(newFactors, true, nonlinearFactors, actualNewFactorIndices); @@ -419,7 +419,7 @@ TEST(ISAM2, removeFactors) ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false)); // Remove the 2nd measurement on landmark 0 (Key 100) - FastVector toRemove; + FactorIndices toRemove; toRemove.push_back(12); isam.update(NonlinearFactorGraph(), Values(), toRemove); @@ -439,7 +439,7 @@ TEST(ISAM2, removeVariables) ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false)); // Remove the measurement on landmark 0 (Key 100) - FastVector toRemove; + FactorIndices toRemove; toRemove.push_back(7); toRemove.push_back(14); isam.update(NonlinearFactorGraph(), Values(), toRemove); @@ -466,7 +466,7 @@ TEST(ISAM2, swapFactors) // Remove the measurement on landmark 0 and replace with a different one { size_t swap_idx = isam.getFactorsUnsafe().size()-2; - FastVector toRemove; + FactorIndices toRemove; toRemove.push_back(swap_idx); fullgraph.remove(swap_idx); @@ -549,7 +549,7 @@ TEST(ISAM2, constrained_ordering) fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); if(i >= 3) - isam.update(newfactors, init, FastVector(), constrained); + isam.update(newfactors, init, FactorIndices(), constrained); else isam.update(newfactors, init); } @@ -570,7 +570,7 @@ TEST(ISAM2, constrained_ordering) fullinit.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); fullinit.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); - isam.update(newfactors, init, FastVector(), constrained); + isam.update(newfactors, init, FactorIndices(), constrained); ++ i; } @@ -584,7 +584,7 @@ TEST(ISAM2, constrained_ordering) init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - isam.update(newfactors, init, FastVector(), constrained); + isam.update(newfactors, init, FactorIndices(), constrained); } // Add odometry from time 10 to 11 and landmark measurement at time 10 @@ -599,7 +599,7 @@ TEST(ISAM2, constrained_ordering) init.insert((i+1), Pose2(6.9, 0.1, 0.01)); fullinit.insert((i+1), Pose2(6.9, 0.1, 0.01)); - isam.update(newfactors, init, FastVector(), constrained); + isam.update(newfactors, init, FactorIndices(), constrained); ++ i; } @@ -713,7 +713,7 @@ TEST(ISAM2, marginalizeLeaves1) constrainedKeys.insert(make_pair(1,1)); constrainedKeys.insert(make_pair(2,2)); - isam.update(factors, values, FastVector(), constrainedKeys); + isam.update(factors, values, FactorIndices(), constrainedKeys); FastList leafKeys = list_of(0); EXPECT(checkMarginalizeLeaves(isam, leafKeys)); @@ -744,7 +744,7 @@ TEST(ISAM2, marginalizeLeaves2) constrainedKeys.insert(make_pair(2,2)); constrainedKeys.insert(make_pair(3,3)); - isam.update(factors, values, FastVector(), constrainedKeys); + isam.update(factors, values, FactorIndices(), constrainedKeys); FastList leafKeys = list_of(0); EXPECT(checkMarginalizeLeaves(isam, leafKeys)); @@ -784,7 +784,7 @@ TEST(ISAM2, marginalizeLeaves3) constrainedKeys.insert(make_pair(4,4)); constrainedKeys.insert(make_pair(5,5)); - isam.update(factors, values, FastVector(), constrainedKeys); + isam.update(factors, values, FactorIndices(), constrainedKeys); FastList leafKeys = list_of(0); EXPECT(checkMarginalizeLeaves(isam, leafKeys)); @@ -810,7 +810,7 @@ TEST(ISAM2, marginalizeLeaves4) constrainedKeys.insert(make_pair(1,1)); constrainedKeys.insert(make_pair(2,2)); - isam.update(factors, values, FastVector(), constrainedKeys); + isam.update(factors, values, FactorIndices(), constrainedKeys); FastList leafKeys = list_of(1); EXPECT(checkMarginalizeLeaves(isam, leafKeys)); From 2ca649a11f3a0e03ab0c58c207f74f5bb61ce827 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 26 Feb 2016 00:01:59 -0800 Subject: [PATCH 60/90] Made some type changes to FactorIndices --- .../nonlinear/ConcurrentIncrementalFilter.cpp | 12 ++++++------ .../nonlinear/ConcurrentIncrementalFilter.h | 8 ++++---- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp index f0c7212c8..1b7f86d5c 100644 --- a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp @@ -43,7 +43,7 @@ bool ConcurrentIncrementalFilter::equals(const ConcurrentFilter& rhs, double tol /* ************************************************************************* */ ConcurrentIncrementalFilter::Result ConcurrentIncrementalFilter::update(const NonlinearFactorGraph& newFactors, const Values& newTheta, - const boost::optional >& keysToMove, const boost::optional< std::vector >& removeFactorIndices) { + const boost::optional >& keysToMove, const boost::optional< FactorIndices >& removeFactorIndices) { gttic(update); @@ -117,7 +117,7 @@ ConcurrentIncrementalFilter::Result ConcurrentIncrementalFilter::update(const No gttic(cache_smoother_factors); // Find the set of factors that will be removed - std::vector removedFactorSlots = FindAdjacentFactors(isam2_, *keysToMove, currentSmootherSummarizationSlots_); + FactorIndices removedFactorSlots = FindAdjacentFactors(isam2_, *keysToMove, currentSmootherSummarizationSlots_); // Cache these factors for later transmission to the smoother NonlinearFactorGraph removedFactors; BOOST_FOREACH(size_t slot, removedFactorSlots) { @@ -134,8 +134,8 @@ ConcurrentIncrementalFilter::Result ConcurrentIncrementalFilter::update(const No gttoc(cache_smoother_factors); gttic(marginalize); - std::vector marginalFactorsIndices; - std::vector deletedFactorsIndices; + FactorIndices marginalFactorsIndices; + FactorIndices deletedFactorsIndices; isam2_.marginalizeLeaves(*keysToMove, marginalFactorsIndices, deletedFactorsIndices); currentSmootherSummarizationSlots_.insert(currentSmootherSummarizationSlots_.end(), marginalFactorsIndices.begin(), marginalFactorsIndices.end()); BOOST_FOREACH(size_t index, deletedFactorsIndices) { @@ -285,10 +285,10 @@ void ConcurrentIncrementalFilter::RecursiveMarkAffectedKeys(const Key& key, cons } /* ************************************************************************* */ -std::vector ConcurrentIncrementalFilter::FindAdjacentFactors(const ISAM2& isam2, const FastList& keys, const std::vector& factorsToIgnore) { +FactorIndices ConcurrentIncrementalFilter::FindAdjacentFactors(const ISAM2& isam2, const FastList& keys, const FactorIndices& factorsToIgnore) { // Identify any new factors to be sent to the smoother (i.e. any factor involving keysToMove) - std::vector removedFactorSlots; + FactorIndices removedFactorSlots; const VariableIndex& variableIndex = isam2.getVariableIndex(); BOOST_FOREACH(Key key, keys) { const FastVector& slots = variableIndex[key]; diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.h b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.h index 11012674e..c28b3bcd1 100644 --- a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.h +++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.h @@ -46,7 +46,7 @@ public: * factors passed as \c newFactors update(). These indices may be * used later to refer to the factors in order to remove them. */ - std::vector newFactorsIndices; + FactorIndices newFactorsIndices; double error; ///< The final factor graph error @@ -124,7 +124,7 @@ public: */ Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(), const boost::optional >& keysToMove = boost::none, - const boost::optional< std::vector >& removeFactorIndices = boost::none); + const boost::optional< FactorIndices >& removeFactorIndices = boost::none); /** * Perform any required operations before the synchronization process starts. @@ -170,7 +170,7 @@ protected: // ??? NonlinearFactorGraph previousSmootherSummarization_; ///< The smoother summarization on the old separator sent by the smoother during the last synchronization - std::vector currentSmootherSummarizationSlots_; ///< The slots in factor graph that correspond to the current smoother summarization on the current separator + FactorIndices currentSmootherSummarizationSlots_; ///< The slots in factor graph that correspond to the current smoother summarization on the current separator NonlinearFactorGraph smootherShortcut_; ///< A set of conditional factors from the old separator to the current separator (recursively calculated during each filter update) // Storage for information to be sent to the smoother @@ -183,7 +183,7 @@ private: static void RecursiveMarkAffectedKeys(const Key& key, const ISAM2Clique::shared_ptr& clique, std::set& additionalKeys); /** Find the set of iSAM2 factors adjacent to 'keys' */ - static std::vector FindAdjacentFactors(const ISAM2& isam2, const FastList& keys, const std::vector& factorsToIgnore); + static FactorIndices FindAdjacentFactors(const ISAM2& isam2, const FastList& keys, const FactorIndices& factorsToIgnore); /** Update the shortcut marginal between the current separator keys and the previous separator keys */ // TODO: Make this a static function From 5c16caf38bf939e19da27fe50bc2846bb0dc4a57 Mon Sep 17 00:00:00 2001 From: Ellon Mendes Date: Tue, 26 Jan 2016 14:01:57 +0100 Subject: [PATCH 61/90] Wrap NonlinearFactorGraph's clone method to python --- python/handwritten/nonlinear/NonlinearFactorGraph.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/python/handwritten/nonlinear/NonlinearFactorGraph.cpp b/python/handwritten/nonlinear/NonlinearFactorGraph.cpp index 4ba4ba008..55929f3f1 100644 --- a/python/handwritten/nonlinear/NonlinearFactorGraph.cpp +++ b/python/handwritten/nonlinear/NonlinearFactorGraph.cpp @@ -49,6 +49,7 @@ void exportNonlinearFactorGraph(){ .def("resize", &NonlinearFactorGraph::resize) .def("empty", &NonlinearFactorGraph::empty) .def("print", &NonlinearFactorGraph::print, print_overloads(args("s"))) + .def("clone", &NonlinearFactorGraph::clone) ; def("getNonlinearFactor", getNonlinearFactor); From 853b5192a54d522b33200f378c72ca6fb5cd1c97 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 26 Feb 2016 07:32:32 -0800 Subject: [PATCH 62/90] Changed to more types to FactorIndices --- .../tests/testConcurrentIncrementalFilter.cpp | 30 +++++-------------- .../testConcurrentIncrementalSmootherGN.cpp | 9 +----- 2 files changed, 8 insertions(+), 31 deletions(-) diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp index 08dbc311c..a283ece29 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp @@ -559,7 +559,6 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_2 ) /* ************************************************************************* */ TEST( ConcurrentIncrementalFilter, synchronize_0 ) { - std::cout << "*********************** synchronize_0 ************************" << std::endl; // Create a set of optimizer parameters ISAM2Params parameters; @@ -593,7 +592,6 @@ TEST( ConcurrentIncrementalFilter, synchronize_0 ) ///* ************************************************************************* */ TEST( ConcurrentIncrementalFilter, synchronize_1 ) { - std::cout << "*********************** synchronize_1 ************************" << std::endl; // Create a set of optimizer parameters ISAM2Params parameters; parameters.relinearizeThreshold = 0; @@ -641,7 +639,6 @@ TEST( ConcurrentIncrementalFilter, synchronize_1 ) /* ************************************************************************* */ TEST( ConcurrentIncrementalFilter, synchronize_2 ) { - std::cout << "*********************** synchronize_2 ************************" << std::endl; // Create a set of optimizer parameters ISAM2Params parameters; parameters.relinearizeThreshold = 0; @@ -712,7 +709,6 @@ TEST( ConcurrentIncrementalFilter, synchronize_2 ) /* ************************************************************************* */ TEST( ConcurrentIncrementalFilter, synchronize_3 ) { - std::cout << "*********************** synchronize_3 ************************" << std::endl; // Create a set of optimizer parameters ISAM2Params parameters; parameters.relinearizeThreshold = 0; @@ -800,7 +796,6 @@ TEST( ConcurrentIncrementalFilter, synchronize_3 ) /* ************************************************************************* */ TEST( ConcurrentIncrementalFilter, synchronize_4 ) { - std::cout << "*********************** synchronize_4 ************************" << std::endl; // Create a set of optimizer parameters ISAM2Params parameters; parameters.relinearizeThreshold = 0; @@ -896,7 +891,6 @@ TEST( ConcurrentIncrementalFilter, synchronize_4 ) /* ************************************************************************* */ TEST( ConcurrentIncrementalFilter, synchronize_5 ) { - std::cout << "*********************** synchronize_5 ************************" << std::endl; // Create a set of optimizer parameters ISAM2Params parameters; parameters.relinearizeThreshold = 0; @@ -1092,7 +1086,6 @@ TEST( ConcurrentIncrementalFilter, synchronize_5 ) ///* ************************************************************************* */ TEST( ConcurrentIncrementalFilter, CalculateMarginals_1 ) { - std::cout << "*********************** CalculateMarginals_1 ************************" << std::endl; // We compare the manual computation of the linear marginals from a factor graph, with the function CalculateMarginals NonlinearFactor::shared_ptr factor1(new PriorFactor(1, poseInitial, noisePrior)); NonlinearFactor::shared_ptr factor2(new BetweenFactor(1, 2, poseOdometry, noiseOdometery)); @@ -1141,8 +1134,6 @@ TEST( ConcurrentIncrementalFilter, CalculateMarginals_1 ) ///* ************************************************************************* */ TEST( ConcurrentIncrementalFilter, CalculateMarginals_2 ) { - std::cout << "*********************** CalculateMarginals_2 ************************" << std::endl; - // We compare the manual computation of the linear marginals from a factor graph, with the function CalculateMarginals NonlinearFactor::shared_ptr factor1(new PriorFactor(1, poseInitial, noisePrior)); NonlinearFactor::shared_ptr factor2(new BetweenFactor(1, 2, poseOdometry, noiseOdometery)); @@ -1165,7 +1156,6 @@ TEST( ConcurrentIncrementalFilter, CalculateMarginals_2 ) newValues.insert(3, value3); - // Create the set of marginalizable variables std::vector linearIndices; linearIndices.push_back(1); @@ -1190,8 +1180,6 @@ TEST( ConcurrentIncrementalFilter, CalculateMarginals_2 ) // Check CHECK(assert_equal(expectedMarginals, actualMarginals, 1e-6)); -// actualMarginals.print("actualMarginals \n"); -// expectedMarginals.print("expectedMarginals \n"); } @@ -1199,8 +1187,6 @@ TEST( ConcurrentIncrementalFilter, CalculateMarginals_2 ) ///* ************************************************************************* */ TEST( ConcurrentIncrementalFilter, removeFactors_topology_1 ) { - std::cout << "*********************** removeFactors_topology_1 ************************" << std::endl; - // Create a set of optimizer parameters ISAM2Params parameters; parameters.relinearizeThreshold = 0; @@ -1233,8 +1219,9 @@ TEST( ConcurrentIncrementalFilter, removeFactors_topology_1 ) // factor we want to remove // NOTE: we can remove factors, paying attention that the remaining graph remains connected - // we remove a single factor, the number 1, which is a BetweenFactor(1, 2, poseOdometry, noiseOdometery); - std::vector removeFactorIndices(1,1); + // we remove a single factor, the number 1, which is a BetweenFactor(1, 2, poseOdometry, noiseOdometery) + FactorIndices removeFactorIndices; + removeFactorIndices.push_back(1); // Add no factors to the filter (we only want to test the removal) NonlinearFactorGraph noFactors; @@ -1245,7 +1232,7 @@ TEST( ConcurrentIncrementalFilter, removeFactors_topology_1 ) NonlinearFactorGraph expectedGraph; expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); - // we removed this one: expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + // we removed this one: expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)) // we should add an empty one, so that the ordering and labeling of the factors is preserved expectedGraph.push_back(NonlinearFactor::shared_ptr()); expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); @@ -1258,7 +1245,6 @@ TEST( ConcurrentIncrementalFilter, removeFactors_topology_1 ) /////* ************************************************************************* */ TEST( ConcurrentIncrementalFilter, removeFactors_topology_2 ) { - std::cout << "*********************** removeFactors_topology_2 ************************" << std::endl; // we try removing the last factor ISAM2Params parameters; @@ -1293,7 +1279,7 @@ TEST( ConcurrentIncrementalFilter, removeFactors_topology_2 ) // factor we want to remove // NOTE: we can remove factors, paying attention that the remaining graph remains connected // we remove a single factor, the number 1, which is a BetweenFactor(1, 2, poseOdometry, noiseOdometery); - std::vector removeFactorIndices(1,4); + FactorIndices removeFactorIndices(1,4); // Add no factors to the filter (we only want to test the removal) NonlinearFactorGraph noFactors; @@ -1318,7 +1304,6 @@ TEST( ConcurrentIncrementalFilter, removeFactors_topology_2 ) /////* ************************************************************************* */ TEST( ConcurrentIncrementalFilter, removeFactors_topology_3 ) { - std::cout << "*********************** removeFactors_topology_3 ************************" << std::endl; // we try removing the first factor ISAM2Params parameters; @@ -1353,7 +1338,7 @@ TEST( ConcurrentIncrementalFilter, removeFactors_topology_3 ) // factor we want to remove // NOTE: we can remove factors, paying attention that the remaining graph remains connected // we remove a single factor, the number 0, which is a BetweenFactor(1, 2, poseOdometry, noiseOdometery); - std::vector removeFactorIndices(1,0); + FactorIndices removeFactorIndices(1,0); // Add no factors to the filter (we only want to test the removal) NonlinearFactorGraph noFactors; @@ -1376,7 +1361,6 @@ TEST( ConcurrentIncrementalFilter, removeFactors_topology_3 ) /////* ************************************************************************* */ TEST( ConcurrentIncrementalFilter, removeFactors_values ) { - std::cout << "*********************** removeFactors_values ************************" << std::endl; // we try removing the last factor ISAM2Params parameters; @@ -1411,7 +1395,7 @@ TEST( ConcurrentIncrementalFilter, removeFactors_values ) // factor we want to remove // NOTE: we can remove factors, paying attention that the remaining graph remains connected // we remove a single factor, the number 4, which is a BetweenFactor(1, 2, poseOdometry, noiseOdometery); - std::vector removeFactorIndices(1,4); + FactorIndices removeFactorIndices(1,4); // Add no factors to the filter (we only want to test the removal) NonlinearFactorGraph noFactors; diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp index c372577ca..858fbb75c 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp @@ -600,8 +600,6 @@ TEST( ConcurrentIncrementalSmootherGN, synchronize_3 ) ///* ************************************************************************* */ TEST( ConcurrentIncrementalSmoother, removeFactors_topology_1 ) { - std::cout << "*********************** removeFactors_topology_1 ************************" << std::endl; - // Create a set of optimizer parameters ISAM2Params parameters; parameters.optimizationParams = ISAM2GaussNewtonParams(); @@ -629,9 +627,7 @@ TEST( ConcurrentIncrementalSmoother, removeFactors_topology_1 ) // factor we want to remove // NOTE: we can remove factors, paying attention that the remaining graph remains connected // we remove a single factor, the number 1, which is a BetweenFactor(1, 2, poseOdometry, noiseOdometery); - std::vector removeFactorIndices(2,1); - - + FactorIndices removeFactorIndices(2,1); // Add no factors to the smoother (we only want to test the removal) NonlinearFactorGraph noFactors; @@ -657,7 +653,6 @@ TEST( ConcurrentIncrementalSmoother, removeFactors_topology_1 ) /////* ************************************************************************* */ //TEST( ConcurrentIncrementalSmoother, removeFactors_topology_2 ) //{ -// std::cout << "*********************** removeFactors_topology_2 ************************" << std::endl; // // we try removing the last factor // // // Create a set of optimizer parameters @@ -711,7 +706,6 @@ TEST( ConcurrentIncrementalSmoother, removeFactors_topology_1 ) /////* ************************************************************************* */ //TEST( ConcurrentBatchSmoother, removeFactors_topology_3 ) //{ -// std::cout << "*********************** removeFactors_topology_3 ************************" << std::endl; // // we try removing the first factor // // // Create a set of optimizer parameters @@ -761,7 +755,6 @@ TEST( ConcurrentIncrementalSmoother, removeFactors_topology_1 ) /////* ************************************************************************* */ //TEST( ConcurrentBatchSmoother, removeFactors_values ) //{ -// std::cout << "*********************** removeFactors_values ************************" << std::endl; // // we try removing the last factor // // // Create a set of optimizer parameters From 4c8ba55d0e8e2fddcf02c27057c65e878473b01d Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 26 Feb 2016 08:28:16 -0800 Subject: [PATCH 63/90] Added smart timing --- timing/timeSFMBAL.h | 9 ++++-- timing/timeSFMBALnavTcam.cpp | 2 +- timing/timeSFMBALsmart.cpp | 55 ++++++++++++++++++++++++++++++++++++ 3 files changed, 62 insertions(+), 4 deletions(-) create mode 100644 timing/timeSFMBALsmart.cpp diff --git a/timing/timeSFMBAL.h b/timing/timeSFMBAL.h index 24b67557a..1642af7e8 100644 --- a/timing/timeSFMBAL.h +++ b/timing/timeSFMBAL.h @@ -67,7 +67,7 @@ int optimize(const SfM_data& db, const NonlinearFactorGraph& graph, // Set parameters to be similar to ceres LevenbergMarquardtParams params; LevenbergMarquardtParams::SetCeresDefaults(¶ms); - params.setVerbosityLM("SUMMARY"); +// params.setVerbosityLM("SUMMARY"); if (gUseSchur) { // Create Schur-complement ordering @@ -81,8 +81,11 @@ int optimize(const SfM_data& db, const NonlinearFactorGraph& graph, } // Optimize - LevenbergMarquardtOptimizer lm(graph, initial, params); - Values result = lm.optimize(); + { + gttic_(optimize); + LevenbergMarquardtOptimizer lm(graph, initial, params); + Values result = lm.optimize(); + } tictoc_finishedIteration_(); tictoc_print_(); diff --git a/timing/timeSFMBALnavTcam.cpp b/timing/timeSFMBALnavTcam.cpp index e370a5a67..0d0eb4f65 100644 --- a/timing/timeSFMBALnavTcam.cpp +++ b/timing/timeSFMBALnavTcam.cpp @@ -35,12 +35,12 @@ int main(int argc, char* argv[]) { // Build graph using conventional GeneralSFMFactor NonlinearFactorGraph graph; for (size_t j = 0; j < db.number_tracks(); j++) { + Point3_ nav_point_(P(j)); BOOST_FOREACH (const SfM_Measurement& m, db.tracks[j].measurements) { size_t i = m.first; Point2 z = m.second; Pose3_ navTcam_(C(i)); Cal3Bundler_ calibration_(K(i)); - Point3_ nav_point_(P(j)); graph.addExpressionFactor( gNoiseModel, z, uncalibrate(calibration_, diff --git a/timing/timeSFMBALsmart.cpp b/timing/timeSFMBALsmart.cpp new file mode 100644 index 000000000..708c25695 --- /dev/null +++ b/timing/timeSFMBALsmart.cpp @@ -0,0 +1,55 @@ +/* ---------------------------------------------------------------------------- + + * 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 timeSFMBALsmart.cpp + * @brief time SFM with BAL file, SmartProjectionFactor + * @author Frank Dellaert + * @date Feb 26, 2016 + */ + +#include "timeSFMBAL.h" + +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +typedef PinholeCamera Camera; +typedef SmartProjectionFactor SfmFactor; + +int main(int argc, char* argv[]) { + // parse options and read BAL file + SfM_data db = preamble(argc, argv); + + // Add smart factors to graph + NonlinearFactorGraph graph; + for (size_t j = 0; j < db.number_tracks(); j++) { + auto smartFactor = boost::make_shared(gNoiseModel); + for (const SfM_Measurement& m : db.tracks[j].measurements) { + size_t i = m.first; + Point2 z = m.second; + smartFactor->add(z, C(i)); + } + graph.push_back(smartFactor); + } + + Values initial; + size_t i = 0; + gUseSchur = false; + for (const SfM_Camera& camera : db.cameras) + initial.insert(C(i++), camera); + + return optimize(db, graph, initial); +} From 2762a40f258cb76100666cc1fc3531b546191bc8 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 26 Feb 2016 08:46:08 -0800 Subject: [PATCH 64/90] Very small improvement --- gtsam/geometry/CameraSet.h | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h index 899e6227c..0df85d3d2 100644 --- a/gtsam/geometry/CameraSet.h +++ b/gtsam/geometry/CameraSet.h @@ -220,17 +220,15 @@ public: static SymmetricBlockMatrix SchurComplement(const FBlocks& Fblocks, const Matrix& E, const Vector& b, const double lambda = 0.0, bool diagonalDamping = false) { - SymmetricBlockMatrix augmentedHessian; if (E.cols() == 2) { Matrix2 P; ComputePointCovariance(P, E, lambda, diagonalDamping); - augmentedHessian = SchurComplement(Fblocks, E, P, b); + return SchurComplement(Fblocks, E, P, b); } else { Matrix3 P; ComputePointCovariance(P, E, lambda, diagonalDamping); - augmentedHessian = SchurComplement(Fblocks, E, P, b); + return SchurComplement(Fblocks, E, P, b); } - return augmentedHessian; } /** From 22c3af906e23a53e182b2c553bb443264e543339 Mon Sep 17 00:00:00 2001 From: Alex Hagiopol Date: Sat, 5 Mar 2016 18:49:34 -0500 Subject: [PATCH 65/90] Deprecated emul() in Vector.h. --- gtsam/base/Matrix.cpp | 2 +- gtsam/base/Vector.h | 3 ++- gtsam/base/tests/testVector.cpp | 7 +++---- gtsam/linear/tests/testKalmanFilter.cpp | 2 +- gtsam_unstable/slam/AHRS.cpp | 5 +++-- 5 files changed, 10 insertions(+), 9 deletions(-) diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp index d92be12f5..3542d06d9 100644 --- a/gtsam/base/Matrix.cpp +++ b/gtsam/base/Matrix.cpp @@ -339,7 +339,7 @@ weighted_eliminate(Matrix& A, Vector& b, const Vector& sigmas) { list > results; Vector pseudo(m); // allocate storage for pseudo-inverse - Vector weights = reciprocal(emul(sigmas,sigmas)); // calculate weights once + Vector weights = reciprocal(sigmas.cwiseProduct(sigmas)); // calculate weights once // We loop over all columns, because the columns that can be eliminated // are not necessarily contiguous. For each one, estimate the corresponding diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index f87703e2b..527068a6d 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -20,7 +20,6 @@ #pragma once - #ifndef MKL_BLAS #define MKL_BLAS MKL_DOMAIN_BLAS #endif @@ -213,6 +212,7 @@ GTSAM_EXPORT ConstSubVector sub(const Vector &v, size_t i1, size_t i2); */ GTSAM_EXPORT void subInsert(Vector& fullVector, const Vector& subVector, size_t i); +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 /** * elementwise multiplication * @param a first vector @@ -220,6 +220,7 @@ GTSAM_EXPORT void subInsert(Vector& fullVector, const Vector& subVector, size_t * @return vector [a(i)*b(i)] */ GTSAM_EXPORT Vector emul(const Vector &a, const Vector &b); +#endif /** * elementwise division diff --git a/gtsam/base/tests/testVector.cpp b/gtsam/base/tests/testVector.cpp index 00e40039b..dd3527b46 100644 --- a/gtsam/base/tests/testVector.cpp +++ b/gtsam/base/tests/testVector.cpp @@ -198,7 +198,7 @@ TEST(Vector, weightedPseudoinverse ) // create sigmas Vector sigmas(2); sigmas(0) = 0.1; sigmas(1) = 0.2; - Vector weights = reciprocal(emul(sigmas,sigmas)); + Vector weights = reciprocal(sigmas.cwiseProduct(sigmas)); // perform solve Vector actual; double precision; @@ -224,8 +224,7 @@ TEST(Vector, weightedPseudoinverse_constraint ) // create sigmas Vector sigmas(2); sigmas(0) = 0.0; sigmas(1) = 0.2; - Vector weights = reciprocal(emul(sigmas,sigmas)); - + Vector weights = reciprocal(sigmas.cwiseProduct(sigmas)); // perform solve Vector actual; double precision; boost::tie(actual, precision) = weightedPseudoinverse(x, weights); @@ -244,7 +243,7 @@ TEST(Vector, weightedPseudoinverse_nan ) { Vector a = (Vector(4) << 1., 0., 0., 0.).finished(); Vector sigmas = (Vector(4) << 0.1, 0.1, 0., 0.).finished(); - Vector weights = reciprocal(emul(sigmas,sigmas)); + Vector weights = reciprocal(sigmas.cwiseProduct(sigmas)); Vector pseudo; double precision; boost::tie(pseudo, precision) = weightedPseudoinverse(a, weights); diff --git a/gtsam/linear/tests/testKalmanFilter.cpp b/gtsam/linear/tests/testKalmanFilter.cpp index 999541ff5..c14f88129 100644 --- a/gtsam/linear/tests/testKalmanFilter.cpp +++ b/gtsam/linear/tests/testKalmanFilter.cpp @@ -270,7 +270,7 @@ TEST( KalmanFilter, QRvsCholesky ) { EXPECT(assert_equal(expected2, pb2->covariance(), 1e-7)); // do the above update again, this time with a full Matrix Q - Matrix modelQ = diag(emul(sigmas,sigmas)); + Matrix modelQ = diag(sigmas.cwiseProduct(sigmas)); KalmanFilter::State pa3 = kfa.updateQ(pa, H, z, modelQ); KalmanFilter::State pb3 = kfb.updateQ(pb, H, z, modelQ); diff --git a/gtsam_unstable/slam/AHRS.cpp b/gtsam_unstable/slam/AHRS.cpp index 2a27730f4..9ea923ebc 100644 --- a/gtsam_unstable/slam/AHRS.cpp +++ b/gtsam_unstable/slam/AHRS.cpp @@ -171,7 +171,7 @@ std::pair AHRS::aid( // calculate residual gravity measurement z = n_g_ - trans(bRn) * measured_b_g; H = collect(3, &n_g_cross_, &Z_3x3, &bRn); - R = trans(bRn) * diag(emul(sigmas_v_a_, sigmas_v_a_)) * bRn; + R = trans(bRn) * diag(sigmas_v_a_.cwiseProduct(sigmas_v_a_)) * bRn; } else { // my measurement prediction (in body frame): // F(:,k) = bias - b_g @@ -186,7 +186,8 @@ std::pair AHRS::aid( Matrix b_g = bRn * n_g_cross_; H = collect(3, &b_g, &Z_3x3, &I_3x3); // And the measurement noise, TODO: should be created once where sigmas_v_a is given - R = diag(emul(sigmas_v_a_, sigmas_v_a_)); + R = diag(sigmas_v_a_.cwiseQuotient(sigmas_v_a_)); + } // update the Kalman filter From caa45ad67bd652697d7737e25143d43ba96b262b Mon Sep 17 00:00:00 2001 From: Alex Hagiopol Date: Sat, 5 Mar 2016 21:08:24 -0500 Subject: [PATCH 66/90] Replaced cwiseProduct() with array().square(). Killed deprecated comments. --- gtsam/base/Matrix.cpp | 2 +- gtsam/base/Vector.h | 6 ------ gtsam/base/tests/testVector.cpp | 6 +++--- gtsam/linear/tests/testKalmanFilter.cpp | 2 +- gtsam_unstable/slam/AHRS.cpp | 3 +-- 5 files changed, 6 insertions(+), 13 deletions(-) diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp index 3542d06d9..213b6bdf1 100644 --- a/gtsam/base/Matrix.cpp +++ b/gtsam/base/Matrix.cpp @@ -339,7 +339,7 @@ weighted_eliminate(Matrix& A, Vector& b, const Vector& sigmas) { list > results; Vector pseudo(m); // allocate storage for pseudo-inverse - Vector weights = reciprocal(sigmas.cwiseProduct(sigmas)); // calculate weights once + Vector weights = reciprocal(sigmas.array().square()); // calculate weights once // We loop over all columns, because the columns that can be eliminated // are not necessarily contiguous. For each one, estimate the corresponding diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index 527068a6d..d7994da83 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -213,12 +213,6 @@ GTSAM_EXPORT ConstSubVector sub(const Vector &v, size_t i1, size_t i2); GTSAM_EXPORT void subInsert(Vector& fullVector, const Vector& subVector, size_t i); #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 -/** - * elementwise multiplication - * @param a first vector - * @param b second vector - * @return vector [a(i)*b(i)] - */ GTSAM_EXPORT Vector emul(const Vector &a, const Vector &b); #endif diff --git a/gtsam/base/tests/testVector.cpp b/gtsam/base/tests/testVector.cpp index dd3527b46..6ea1627a5 100644 --- a/gtsam/base/tests/testVector.cpp +++ b/gtsam/base/tests/testVector.cpp @@ -198,7 +198,7 @@ TEST(Vector, weightedPseudoinverse ) // create sigmas Vector sigmas(2); sigmas(0) = 0.1; sigmas(1) = 0.2; - Vector weights = reciprocal(sigmas.cwiseProduct(sigmas)); + Vector weights = reciprocal(sigmas.array().square()); // perform solve Vector actual; double precision; @@ -224,7 +224,7 @@ TEST(Vector, weightedPseudoinverse_constraint ) // create sigmas Vector sigmas(2); sigmas(0) = 0.0; sigmas(1) = 0.2; - Vector weights = reciprocal(sigmas.cwiseProduct(sigmas)); + Vector weights = reciprocal(sigmas.array().square()); // perform solve Vector actual; double precision; boost::tie(actual, precision) = weightedPseudoinverse(x, weights); @@ -243,7 +243,7 @@ TEST(Vector, weightedPseudoinverse_nan ) { Vector a = (Vector(4) << 1., 0., 0., 0.).finished(); Vector sigmas = (Vector(4) << 0.1, 0.1, 0., 0.).finished(); - Vector weights = reciprocal(sigmas.cwiseProduct(sigmas)); + Vector weights = reciprocal(sigmas.array().square()); Vector pseudo; double precision; boost::tie(pseudo, precision) = weightedPseudoinverse(a, weights); diff --git a/gtsam/linear/tests/testKalmanFilter.cpp b/gtsam/linear/tests/testKalmanFilter.cpp index c14f88129..16be98306 100644 --- a/gtsam/linear/tests/testKalmanFilter.cpp +++ b/gtsam/linear/tests/testKalmanFilter.cpp @@ -270,7 +270,7 @@ TEST( KalmanFilter, QRvsCholesky ) { EXPECT(assert_equal(expected2, pb2->covariance(), 1e-7)); // do the above update again, this time with a full Matrix Q - Matrix modelQ = diag(sigmas.cwiseProduct(sigmas)); + Matrix modelQ = diag(sigmas.array().square()); KalmanFilter::State pa3 = kfa.updateQ(pa, H, z, modelQ); KalmanFilter::State pb3 = kfb.updateQ(pb, H, z, modelQ); diff --git a/gtsam_unstable/slam/AHRS.cpp b/gtsam_unstable/slam/AHRS.cpp index 9ea923ebc..511514347 100644 --- a/gtsam_unstable/slam/AHRS.cpp +++ b/gtsam_unstable/slam/AHRS.cpp @@ -186,8 +186,7 @@ std::pair AHRS::aid( Matrix b_g = bRn * n_g_cross_; H = collect(3, &b_g, &Z_3x3, &I_3x3); // And the measurement noise, TODO: should be created once where sigmas_v_a is given - R = diag(sigmas_v_a_.cwiseQuotient(sigmas_v_a_)); - + R = diag(sigmas_v_a_.array().square()); } // update the Kalman filter From 7d0b5629087e6c744f58c4e587cca1b72196255b Mon Sep 17 00:00:00 2001 From: Alex Hagiopol Date: Sat, 5 Mar 2016 21:20:32 -0500 Subject: [PATCH 67/90] Fixed two missed replacements of cwiseProduct() with array().square(). --- gtsam_unstable/slam/AHRS.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam_unstable/slam/AHRS.cpp b/gtsam_unstable/slam/AHRS.cpp index 511514347..258bc5d03 100644 --- a/gtsam_unstable/slam/AHRS.cpp +++ b/gtsam_unstable/slam/AHRS.cpp @@ -48,7 +48,7 @@ AHRS::AHRS(const Matrix& stationaryU, const Matrix& stationaryF, double g_e, Vector3 var_omega_w = 0 * ones(3); // TODO Vector3 var_omega_g = (0.0034 * 0.0034) * ones(3); Vector3 var_omega_a = (0.034 * 0.034) * ones(3); - Vector3 sigmas_v_g_sq = sigmas_v_g.cwiseProduct(sigmas_v_g); + Vector3 sigmas_v_g_sq = sigmas_v_g.array().square(); var_w_ << var_omega_w, var_omega_g, sigmas_v_g_sq, var_omega_a; // Quantities needed for aiding @@ -171,7 +171,7 @@ std::pair AHRS::aid( // calculate residual gravity measurement z = n_g_ - trans(bRn) * measured_b_g; H = collect(3, &n_g_cross_, &Z_3x3, &bRn); - R = trans(bRn) * diag(sigmas_v_a_.cwiseProduct(sigmas_v_a_)) * bRn; + R = trans(bRn) * diag(sigmas_v_a_.array().square()) * bRn; } else { // my measurement prediction (in body frame): // F(:,k) = bias - b_g From d7add67855631b96b57d4e176db3cea9c95774aa Mon Sep 17 00:00:00 2001 From: Alex Hagiopol Date: Sat, 5 Mar 2016 21:51:44 -0500 Subject: [PATCH 68/90] Moved deprecated implementation of emul() to Vector.h. --- gtsam/base/Vector.cpp | 6 ------ gtsam/base/Vector.h | 2 +- 2 files changed, 1 insertion(+), 7 deletions(-) diff --git a/gtsam/base/Vector.cpp b/gtsam/base/Vector.cpp index 61a42fead..205f48839 100644 --- a/gtsam/base/Vector.cpp +++ b/gtsam/base/Vector.cpp @@ -186,12 +186,6 @@ void subInsert(Vector& fullVector, const Vector& subVector, size_t i) { fullVector.segment(i, subVector.size()) = subVector; } -/* ************************************************************************* */ -Vector emul(const Vector &a, const Vector &b) { - assert (b.size()==a.size()); - return a.cwiseProduct(b); -} - /* ************************************************************************* */ Vector ediv(const Vector &a, const Vector &b) { assert (b.size()==a.size()); diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index d7994da83..3d8c09d1e 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -213,7 +213,7 @@ GTSAM_EXPORT ConstSubVector sub(const Vector &v, size_t i1, size_t i2); GTSAM_EXPORT void subInsert(Vector& fullVector, const Vector& subVector, size_t i); #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 -GTSAM_EXPORT Vector emul(const Vector &a, const Vector &b); +GTSAM_EXPORT inline Vector emul(const Vector &a, const Vector &b) {assert (b.size()==a.size()); return a.cwiseProduct(b);} #endif /** From 68c8bcc5e84335d77713a3889b88448ffcf755d9 Mon Sep 17 00:00:00 2001 From: Alex Hagiopol Date: Fri, 11 Mar 2016 11:32:20 -0500 Subject: [PATCH 69/90] Moved deprecated emul() to end of Vector.h --- gtsam/base/Vector.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index 3d8c09d1e..8b61e32f0 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -212,10 +212,6 @@ GTSAM_EXPORT ConstSubVector sub(const Vector &v, size_t i1, size_t i2); */ GTSAM_EXPORT void subInsert(Vector& fullVector, const Vector& subVector, size_t i); -#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 -GTSAM_EXPORT inline Vector emul(const Vector &a, const Vector &b) {assert (b.size()==a.size()); return a.cwiseProduct(b);} -#endif - /** * elementwise division * @param a first vector @@ -351,6 +347,10 @@ GTSAM_EXPORT Vector concatVectors(const std::list& vs); */ GTSAM_EXPORT Vector concatVectors(size_t nrVectors, ...); +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 +GTSAM_EXPORT inline Vector emul(const Vector &a, const Vector &b) {assert (b.size()==a.size()); return a.cwiseProduct(b);} +#endif + } // namespace gtsam #include From b3dfb6d97830fd60a9799ccf9c3ede5610ec514c Mon Sep 17 00:00:00 2001 From: Alex Hagiopol Date: Fri, 11 Mar 2016 13:31:57 -0500 Subject: [PATCH 70/90] Deprecated reciprocal() in Vector.h --- gtsam/base/Matrix.cpp | 2 +- gtsam/base/Vector.cpp | 5 ----- gtsam/base/Vector.h | 9 ++------- gtsam/base/tests/testVector.cpp | 8 ++++---- gtsam/linear/NoiseModel.h | 6 +++--- 5 files changed, 10 insertions(+), 20 deletions(-) diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp index 213b6bdf1..1e5da7d86 100644 --- a/gtsam/base/Matrix.cpp +++ b/gtsam/base/Matrix.cpp @@ -339,7 +339,7 @@ weighted_eliminate(Matrix& A, Vector& b, const Vector& sigmas) { list > results; Vector pseudo(m); // allocate storage for pseudo-inverse - Vector weights = reciprocal(sigmas.array().square()); // calculate weights once + Vector weights = sigmas.array().square().inverse(); // calculate weights once // We loop over all columns, because the columns that can be eliminated // are not necessarily contiguous. For each one, estimate the corresponding diff --git a/gtsam/base/Vector.cpp b/gtsam/base/Vector.cpp index 205f48839..ce3f0a7b0 100644 --- a/gtsam/base/Vector.cpp +++ b/gtsam/base/Vector.cpp @@ -214,11 +214,6 @@ double norm_2(const Vector& v) { return v.norm(); } -/* ************************************************************************* */ -Vector reciprocal(const Vector &a) { - return a.array().inverse(); -} - /* ************************************************************************* */ Vector esqrt(const Vector& v) { return v.cwiseSqrt(); diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index 8b61e32f0..5bb732f5c 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -243,13 +243,6 @@ GTSAM_EXPORT double sum(const Vector &a); */ GTSAM_EXPORT double norm_2(const Vector& v); -/** - * Elementwise reciprocal of vector elements - * @param a vector - * @return [1/a(i)] - */ -GTSAM_EXPORT Vector reciprocal(const Vector &a); - /** * Elementwise sqrt of vector elements * @param v is a vector @@ -349,8 +342,10 @@ GTSAM_EXPORT Vector concatVectors(size_t nrVectors, ...); #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 GTSAM_EXPORT inline Vector emul(const Vector &a, const Vector &b) {assert (b.size()==a.size()); return a.cwiseProduct(b);} +GTSAM_EXPORT inline Vector reciprocal(const Vector &a) {return a.array().inverse();} #endif + } // namespace gtsam #include diff --git a/gtsam/base/tests/testVector.cpp b/gtsam/base/tests/testVector.cpp index 6ea1627a5..a2620ef0c 100644 --- a/gtsam/base/tests/testVector.cpp +++ b/gtsam/base/tests/testVector.cpp @@ -198,7 +198,7 @@ TEST(Vector, weightedPseudoinverse ) // create sigmas Vector sigmas(2); sigmas(0) = 0.1; sigmas(1) = 0.2; - Vector weights = reciprocal(sigmas.array().square()); + Vector weights = sigmas.array().square().inverse(); // perform solve Vector actual; double precision; @@ -224,7 +224,7 @@ TEST(Vector, weightedPseudoinverse_constraint ) // create sigmas Vector sigmas(2); sigmas(0) = 0.0; sigmas(1) = 0.2; - Vector weights = reciprocal(sigmas.array().square()); + Vector weights = sigmas.array().square().inverse(); // perform solve Vector actual; double precision; boost::tie(actual, precision) = weightedPseudoinverse(x, weights); @@ -243,7 +243,7 @@ TEST(Vector, weightedPseudoinverse_nan ) { Vector a = (Vector(4) << 1., 0., 0., 0.).finished(); Vector sigmas = (Vector(4) << 0.1, 0.1, 0., 0.).finished(); - Vector weights = reciprocal(sigmas.array().square()); + Vector weights = sigmas.array().square().inverse(); Vector pseudo; double precision; boost::tie(pseudo, precision) = weightedPseudoinverse(a, weights); @@ -306,7 +306,7 @@ TEST(Vector, greater_than ) TEST(Vector, reciprocal ) { Vector v = Vector3(1.0, 2.0, 4.0); - EXPECT(assert_equal(Vector3(1.0, 0.5, 0.25),reciprocal(v))); + EXPECT(assert_equal(Vector3(1.0, 0.5, 0.25), (Vector) v.array().inverse())); } /* ************************************************************************* */ diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index c76db1b55..2f229cb22 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -309,7 +309,7 @@ namespace gtsam { * i.e. the diagonal of the information matrix, i.e., weights */ static shared_ptr Precisions(const Vector& precisions, bool smart = true) { - return Variances(reciprocal(precisions), smart); + return Variances(precisions.array().inverse(), smart); } virtual void print(const std::string& name) const; @@ -443,10 +443,10 @@ namespace gtsam { * precisions, some of which might be inf */ static shared_ptr MixedPrecisions(const Vector& mu, const Vector& precisions) { - return MixedVariances(mu, reciprocal(precisions)); + return MixedVariances(mu, precisions.array().inverse()); } static shared_ptr MixedPrecisions(const Vector& precisions) { - return MixedVariances(reciprocal(precisions)); + return MixedVariances(precisions.array().inverse()); } /** From 9baed00516c8ffec7e44f074ddf127cd61f9279b Mon Sep 17 00:00:00 2001 From: Alex Hagiopol Date: Fri, 11 Mar 2016 13:47:25 -0500 Subject: [PATCH 71/90] Deprecated sum() in Vector.h --- gtsam/base/Vector.cpp | 5 ----- gtsam/base/Vector.h | 9 +-------- 2 files changed, 1 insertion(+), 13 deletions(-) diff --git a/gtsam/base/Vector.cpp b/gtsam/base/Vector.cpp index ce3f0a7b0..a9a0efa36 100644 --- a/gtsam/base/Vector.cpp +++ b/gtsam/base/Vector.cpp @@ -204,11 +204,6 @@ Vector ediv_(const Vector &a, const Vector &b) { return c; } -/* ************************************************************************* */ -double sum(const Vector &a) { - return a.sum(); -} - /* ************************************************************************* */ double norm_2(const Vector& v) { return v.norm(); diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index 5bb732f5c..7e9214789 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -228,13 +228,6 @@ GTSAM_EXPORT Vector ediv(const Vector &a, const Vector &b); */ GTSAM_EXPORT Vector ediv_(const Vector &a, const Vector &b); -/** - * sum vector elements - * @param a vector - * @return sum_i a(i) - */ -GTSAM_EXPORT double sum(const Vector &a); - /** * Calculates L2 norm for a vector * modeled after boost.ublas for compatibility @@ -343,9 +336,9 @@ GTSAM_EXPORT Vector concatVectors(size_t nrVectors, ...); #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 GTSAM_EXPORT inline Vector emul(const Vector &a, const Vector &b) {assert (b.size()==a.size()); return a.cwiseProduct(b);} GTSAM_EXPORT inline Vector reciprocal(const Vector &a) {return a.array().inverse();} +GTSAM_EXPORT inline double sum(const Vector &a){return a.sum();} #endif - } // namespace gtsam #include From c0fb1abaf08c6b2ff82d25540f2f85a952fd7065 Mon Sep 17 00:00:00 2001 From: Alex Hagiopol Date: Fri, 11 Mar 2016 14:00:04 -0500 Subject: [PATCH 72/90] Deprecated max() in Vector.h. --- gtsam/base/Vector.cpp | 5 ----- gtsam/base/Vector.h | 8 +------- 2 files changed, 1 insertion(+), 12 deletions(-) diff --git a/gtsam/base/Vector.cpp b/gtsam/base/Vector.cpp index a9a0efa36..39b61a4cb 100644 --- a/gtsam/base/Vector.cpp +++ b/gtsam/base/Vector.cpp @@ -219,11 +219,6 @@ Vector abs(const Vector& v) { return v.cwiseAbs(); } -/* ************************************************************************* */ -double max(const Vector &a) { - return a.maxCoeff(); -} - /* ************************************************************************* */ // imperative version, pass in x double houseInPlace(Vector &v) { diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index 7e9214789..63ca92799 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -250,13 +250,6 @@ GTSAM_EXPORT Vector esqrt(const Vector& v); */ GTSAM_EXPORT Vector abs(const Vector& v); -/** - * Return the max element of a vector - * @param a is a vector - * @return max(a) - */ -GTSAM_EXPORT double max(const Vector &a); - /** * Dot product */ @@ -337,6 +330,7 @@ GTSAM_EXPORT Vector concatVectors(size_t nrVectors, ...); GTSAM_EXPORT inline Vector emul(const Vector &a, const Vector &b) {assert (b.size()==a.size()); return a.cwiseProduct(b);} GTSAM_EXPORT inline Vector reciprocal(const Vector &a) {return a.array().inverse();} GTSAM_EXPORT inline double sum(const Vector &a){return a.sum();} +GTSAM_EXPORT inline double max(const Vector &a){return a.maxCoeff();} #endif } // namespace gtsam From 649c5a21ff312a3ddc4119f8fb35128c644692ba Mon Sep 17 00:00:00 2001 From: Alex Hagiopol Date: Fri, 11 Mar 2016 14:14:15 -0500 Subject: [PATCH 73/90] Deprecated abs() in Vector.h. --- gtsam/base/Vector.cpp | 5 ----- gtsam/base/Vector.h | 10 ++-------- 2 files changed, 2 insertions(+), 13 deletions(-) diff --git a/gtsam/base/Vector.cpp b/gtsam/base/Vector.cpp index 39b61a4cb..390609917 100644 --- a/gtsam/base/Vector.cpp +++ b/gtsam/base/Vector.cpp @@ -214,11 +214,6 @@ Vector esqrt(const Vector& v) { return v.cwiseSqrt(); } -/* ************************************************************************* */ -Vector abs(const Vector& v) { - return v.cwiseAbs(); -} - /* ************************************************************************* */ // imperative version, pass in x double houseInPlace(Vector &v) { diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index 63ca92799..ea7ab08cf 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -243,13 +243,6 @@ GTSAM_EXPORT double norm_2(const Vector& v); */ GTSAM_EXPORT Vector esqrt(const Vector& v); -/** - * Absolute values of vector elements - * @param v is a vector - * @return [abs(a(i))] - */ -GTSAM_EXPORT Vector abs(const Vector& v); - /** * Dot product */ @@ -327,10 +320,11 @@ GTSAM_EXPORT Vector concatVectors(const std::list& vs); GTSAM_EXPORT Vector concatVectors(size_t nrVectors, ...); #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 +GTSAM_EXPORT inline Vector abs(const Vector& v){return v.cwiseAbs();} GTSAM_EXPORT inline Vector emul(const Vector &a, const Vector &b) {assert (b.size()==a.size()); return a.cwiseProduct(b);} +GTSAM_EXPORT inline double max(const Vector &a){return a.maxCoeff();} GTSAM_EXPORT inline Vector reciprocal(const Vector &a) {return a.array().inverse();} GTSAM_EXPORT inline double sum(const Vector &a){return a.sum();} -GTSAM_EXPORT inline double max(const Vector &a){return a.maxCoeff();} #endif } // namespace gtsam From 2f146e85efe03f476c3811d046bbc4139a916a80 Mon Sep 17 00:00:00 2001 From: Alex Hagiopol Date: Fri, 11 Mar 2016 14:34:19 -0500 Subject: [PATCH 74/90] Deprecated esqrt() from Vector.h. --- gtsam/base/Vector.cpp | 7 ------- gtsam/base/Vector.h | 8 +------- gtsam/linear/NoiseModel.cpp | 2 +- gtsam/linear/NoiseModel.h | 4 ++-- gtsam_unstable/slam/AHRS.cpp | 4 ++-- 5 files changed, 6 insertions(+), 19 deletions(-) diff --git a/gtsam/base/Vector.cpp b/gtsam/base/Vector.cpp index 390609917..d2c1fb738 100644 --- a/gtsam/base/Vector.cpp +++ b/gtsam/base/Vector.cpp @@ -209,11 +209,6 @@ double norm_2(const Vector& v) { return v.norm(); } -/* ************************************************************************* */ -Vector esqrt(const Vector& v) { - return v.cwiseSqrt(); -} - /* ************************************************************************* */ // imperative version, pass in x double houseInPlace(Vector &v) { @@ -337,6 +332,4 @@ Vector concatVectors(size_t nrVectors, ...) return concatVectors(vs); } -/* ************************************************************************* */ - } // namespace gtsam diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index ea7ab08cf..eeb2aa600 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -236,13 +236,6 @@ GTSAM_EXPORT Vector ediv_(const Vector &a, const Vector &b); */ GTSAM_EXPORT double norm_2(const Vector& v); -/** - * Elementwise sqrt of vector elements - * @param v is a vector - * @return [sqrt(a(i))] - */ -GTSAM_EXPORT Vector esqrt(const Vector& v); - /** * Dot product */ @@ -321,6 +314,7 @@ GTSAM_EXPORT Vector concatVectors(size_t nrVectors, ...); #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 GTSAM_EXPORT inline Vector abs(const Vector& v){return v.cwiseAbs();} +GTSAM_EXPORT inline Vector esqrt(const Vector& v) { return v.cwiseSqrt();} GTSAM_EXPORT inline Vector emul(const Vector &a, const Vector &b) {assert (b.size()==a.size()); return a.cwiseProduct(b);} GTSAM_EXPORT inline double max(const Vector &a){return a.maxCoeff();} GTSAM_EXPORT inline Vector reciprocal(const Vector &a) {return a.array().inverse();} diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index 15b2dcf81..5212374a7 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -258,7 +258,7 @@ Diagonal::shared_ptr Diagonal::Variances(const Vector& variances, bool smart) { if (variances(j) != variances(0)) goto full; return Isotropic::Variance(n, variances(0), true); } - full: return shared_ptr(new Diagonal(esqrt(variances))); + full: return shared_ptr(new Diagonal(variances.cwiseSqrt())); } /* ************************************************************************* */ diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 2f229cb22..cef270100 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -432,10 +432,10 @@ namespace gtsam { * standard devations, some of which might be zero */ static shared_ptr MixedVariances(const Vector& mu, const Vector& variances) { - return shared_ptr(new Constrained(mu, esqrt(variances))); + return shared_ptr(new Constrained(mu, variances.cwiseSqrt())); } static shared_ptr MixedVariances(const Vector& variances) { - return shared_ptr(new Constrained(esqrt(variances))); + return shared_ptr(new Constrained(variances.cwiseSqrt())); } /** diff --git a/gtsam_unstable/slam/AHRS.cpp b/gtsam_unstable/slam/AHRS.cpp index 258bc5d03..ce8fd29f3 100644 --- a/gtsam_unstable/slam/AHRS.cpp +++ b/gtsam_unstable/slam/AHRS.cpp @@ -32,7 +32,7 @@ AHRS::AHRS(const Matrix& stationaryU, const Matrix& stationaryF, double g_e, // estimate standard deviation on gyroscope readings Pg_ = Cov(stationaryU); - Vector3 sigmas_v_g = esqrt(Pg_.diagonal() * T); + Vector3 sigmas_v_g = (Pg_.diagonal() * T).cwiseSqrt(); // estimate standard deviation on accelerometer readings Pa_ = Cov(stationaryF); @@ -52,7 +52,7 @@ AHRS::AHRS(const Matrix& stationaryU, const Matrix& stationaryF, double g_e, var_w_ << var_omega_w, var_omega_g, sigmas_v_g_sq, var_omega_a; // Quantities needed for aiding - sigmas_v_a_ = esqrt(T * Pa_.diagonal()); + sigmas_v_a_ = (T * Pa_.diagonal()).cwiseSqrt(); // gravity in nav frame n_g_ = (Vector(3) << 0.0, 0.0, g_e).finished(); From 5502691022bf45fc71d7a2b592dc05ca5cc02bd1 Mon Sep 17 00:00:00 2001 From: Alex Hagiopol Date: Fri, 11 Mar 2016 19:34:49 -0500 Subject: [PATCH 75/90] Deprecated norm_2() in Vector.h --- gtsam/base/Vector.cpp | 5 ----- gtsam/base/Vector.h | 9 +-------- gtsam/geometry/tests/testPose3.cpp | 2 +- gtsam/geometry/tests/testRot3.cpp | 4 ++-- gtsam/navigation/tests/testAHRSFactor.cpp | 2 +- gtsam/slam/tests/testSmartProjectionCameraFactor.cpp | 2 +- gtsam_unstable/slam/Mechanization_bRn2.cpp | 2 +- 7 files changed, 7 insertions(+), 19 deletions(-) diff --git a/gtsam/base/Vector.cpp b/gtsam/base/Vector.cpp index d2c1fb738..4095cc693 100644 --- a/gtsam/base/Vector.cpp +++ b/gtsam/base/Vector.cpp @@ -204,11 +204,6 @@ Vector ediv_(const Vector &a, const Vector &b) { return c; } -/* ************************************************************************* */ -double norm_2(const Vector& v) { - return v.norm(); -} - /* ************************************************************************* */ // imperative version, pass in x double houseInPlace(Vector &v) { diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index eeb2aa600..0b825bcfb 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -228,14 +228,6 @@ GTSAM_EXPORT Vector ediv(const Vector &a, const Vector &b); */ GTSAM_EXPORT Vector ediv_(const Vector &a, const Vector &b); -/** - * Calculates L2 norm for a vector - * modeled after boost.ublas for compatibility - * @param v vector - * @return the L2 norm - */ -GTSAM_EXPORT double norm_2(const Vector& v); - /** * Dot product */ @@ -317,6 +309,7 @@ GTSAM_EXPORT inline Vector abs(const Vector& v){return v.cwiseAbs();} GTSAM_EXPORT inline Vector esqrt(const Vector& v) { return v.cwiseSqrt();} GTSAM_EXPORT inline Vector emul(const Vector &a, const Vector &b) {assert (b.size()==a.size()); return a.cwiseProduct(b);} GTSAM_EXPORT inline double max(const Vector &a){return a.maxCoeff();} +GTSAM_EXPORT inline double norm_2(const Vector& v) {return v.norm();} GTSAM_EXPORT inline Vector reciprocal(const Vector &a) {return a.array().inverse();} GTSAM_EXPORT inline double sum(const Vector &a){return a.sum();} #endif diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 1aba34bd6..0e7da537a 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -148,7 +148,7 @@ TEST(Pose3, Adjoint_full) Pose3 Agrawal06iros(const Vector& xi) { Vector w = xi.head(3); Vector v = xi.tail(3); - double t = norm_2(w); + double t = w.norm(); if (t < 1e-5) return Pose3(Rot3(), Point3(v)); else { diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 5364d72f6..9621898bf 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -96,7 +96,7 @@ TEST( Rot3, equals) /* ************************************************************************* */ // Notice this uses J^2 whereas fast uses w*w', and has cos(t)*I + .... Rot3 slow_but_correct_Rodrigues(const Vector& w) { - double t = norm_2(w); + double t = w.norm(); Matrix3 J = skewSymmetric(w / t); if (t < 1e-5) return Rot3(); Matrix3 R = I_3x3 + sin(t) * J + (1.0 - cos(t)) * (J * J); @@ -130,7 +130,7 @@ TEST( Rot3, Rodrigues2) TEST( Rot3, Rodrigues3) { Vector w = Vector3(0.1, 0.2, 0.3); - Rot3 R1 = Rot3::AxisAngle(w / norm_2(w), norm_2(w)); + Rot3 R1 = Rot3::AxisAngle(w / w.norm(), w.norm()); Rot3 R2 = slow_but_correct_Rodrigues(w); CHECK(assert_equal(R2,R1)); } diff --git a/gtsam/navigation/tests/testAHRSFactor.cpp b/gtsam/navigation/tests/testAHRSFactor.cpp index 02911acb1..208d0e709 100644 --- a/gtsam/navigation/tests/testAHRSFactor.cpp +++ b/gtsam/navigation/tests/testAHRSFactor.cpp @@ -268,7 +268,7 @@ TEST( AHRSFactor, PartialDerivativeLogmap ) { const Vector3 x = thetahat; // parametrization of so(3) const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^ - double normx = norm_2(x); + double normx = x.norm(); const Matrix3 actualDelFdeltheta = I_3x3 + 0.5 * X + (1 / (normx * normx) - (1 + cos(normx)) / (2 * normx * sin(normx))) * X * X; diff --git a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp index e441c37ff..daecb56bf 100644 --- a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp @@ -652,7 +652,7 @@ TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor ) { Vector e1 = sfm1.evaluateError(values.at(c1), values.at(l1)); Vector e2 = sfm2.evaluateError(values.at(c2), values.at(l1)); double actualError = 0.5 - * (norm_2(e1) * norm_2(e1) + norm_2(e2) * norm_2(e2)); + * (e1.norm() * e1.norm() + e2.norm() * e2.norm()); double actualErrorGraph = generalGraph.error(values); DOUBLES_EQUAL(expectedErrorGraph, actualErrorGraph, 1e-7); diff --git a/gtsam_unstable/slam/Mechanization_bRn2.cpp b/gtsam_unstable/slam/Mechanization_bRn2.cpp index 20ffbdd4f..2205f53bd 100644 --- a/gtsam_unstable/slam/Mechanization_bRn2.cpp +++ b/gtsam_unstable/slam/Mechanization_bRn2.cpp @@ -33,7 +33,7 @@ Mechanization_bRn2 Mechanization_bRn2::initialize(const Matrix& U, if(g_e == 0) { if (flat) // acceleration measured is along the z-axis. - b_g = (Vector3(3) << 0.0, 0.0, norm_2(meanF)).finished(); + b_g = (Vector3(3) << 0.0, 0.0, meanF.norm()).finished(); else // acceleration measured is the opposite of gravity (10.13) b_g = -meanF; From 6504e0e692db1d6fe41ffbb370eb28244bc8dce2 Mon Sep 17 00:00:00 2001 From: Alex Hagiopol Date: Fri, 11 Mar 2016 19:49:03 -0500 Subject: [PATCH 76/90] Deprecated ediv() in Vector.h. --- gtsam/base/Vector.cpp | 6 ------ gtsam/base/Vector.h | 9 +-------- gtsam/base/tests/testVector.cpp | 2 +- 3 files changed, 2 insertions(+), 15 deletions(-) diff --git a/gtsam/base/Vector.cpp b/gtsam/base/Vector.cpp index 4095cc693..d3c39eeda 100644 --- a/gtsam/base/Vector.cpp +++ b/gtsam/base/Vector.cpp @@ -186,12 +186,6 @@ void subInsert(Vector& fullVector, const Vector& subVector, size_t i) { fullVector.segment(i, subVector.size()) = subVector; } -/* ************************************************************************* */ -Vector ediv(const Vector &a, const Vector &b) { - assert (b.size()==a.size()); - return a.cwiseQuotient(b); -} - /* ************************************************************************* */ Vector ediv_(const Vector &a, const Vector &b) { size_t n = a.size(); diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index 0b825bcfb..4c2f5e3fa 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -212,14 +212,6 @@ GTSAM_EXPORT ConstSubVector sub(const Vector &v, size_t i1, size_t i2); */ GTSAM_EXPORT void subInsert(Vector& fullVector, const Vector& subVector, size_t i); -/** - * elementwise division - * @param a first vector - * @param b second vector - * @return vector [a(i)/b(i)] - */ -GTSAM_EXPORT Vector ediv(const Vector &a, const Vector &b); - /** * elementwise division, but 0/0 = 0, not inf * @param a first vector @@ -306,6 +298,7 @@ GTSAM_EXPORT Vector concatVectors(size_t nrVectors, ...); #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 GTSAM_EXPORT inline Vector abs(const Vector& v){return v.cwiseAbs();} +GTSAM_EXPORT Vector ediv(const Vector &a, const Vector &b) {assert (b.size()==a.size()); return a.cwiseQuotient(b);} GTSAM_EXPORT inline Vector esqrt(const Vector& v) { return v.cwiseSqrt();} GTSAM_EXPORT inline Vector emul(const Vector &a, const Vector &b) {assert (b.size()==a.size()); return a.cwiseProduct(b);} GTSAM_EXPORT inline double max(const Vector &a){return a.maxCoeff();} diff --git a/gtsam/base/tests/testVector.cpp b/gtsam/base/tests/testVector.cpp index a2620ef0c..718ce9c80 100644 --- a/gtsam/base/tests/testVector.cpp +++ b/gtsam/base/tests/testVector.cpp @@ -257,7 +257,7 @@ TEST(Vector, ediv ) { Vector a = Vector3(10., 20., 30.); Vector b = Vector3(2.0, 5.0, 6.0); - Vector actual(ediv(a,b)); + Vector actual(a.cwiseQuotient(b)); Vector c = Vector3(5.0, 4.0, 5.0); EXPECT(assert_equal(c,actual)); From 3989178aba4feb55f850aebd1c0f99e2d2f8fd0f Mon Sep 17 00:00:00 2001 From: Alex Hagiopol Date: Fri, 11 Mar 2016 20:13:51 -0500 Subject: [PATCH 77/90] Deprecated subInsert() in Vector.h --- gtsam/base/Vector.cpp | 5 ----- gtsam/base/Vector.h | 9 +-------- gtsam/base/tests/testVector.cpp | 2 +- gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp | 3 ++- 4 files changed, 4 insertions(+), 15 deletions(-) diff --git a/gtsam/base/Vector.cpp b/gtsam/base/Vector.cpp index d3c39eeda..3365525c4 100644 --- a/gtsam/base/Vector.cpp +++ b/gtsam/base/Vector.cpp @@ -181,11 +181,6 @@ ConstSubVector sub(const Vector &v, size_t i1, size_t i2) { return v.segment(i1,i2-i1); } -/* ************************************************************************* */ -void subInsert(Vector& fullVector, const Vector& subVector, size_t i) { - fullVector.segment(i, subVector.size()) = subVector; -} - /* ************************************************************************* */ Vector ediv_(const Vector &a, const Vector &b) { size_t n = a.size(); diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index 4c2f5e3fa..3b0634702 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -204,14 +204,6 @@ GTSAM_EXPORT bool linear_dependent(const Vector& vec1, const Vector& vec2, doubl */ GTSAM_EXPORT ConstSubVector sub(const Vector &v, size_t i1, size_t i2); -/** - * Inserts a subvector into a vector IN PLACE - * @param fullVector is the vector to be changed - * @param subVector is the vector to insert - * @param i is the index where the subvector should be inserted - */ -GTSAM_EXPORT void subInsert(Vector& fullVector, const Vector& subVector, size_t i); - /** * elementwise division, but 0/0 = 0, not inf * @param a first vector @@ -304,6 +296,7 @@ GTSAM_EXPORT inline Vector emul(const Vector &a, const Vector &b) {assert (b.siz GTSAM_EXPORT inline double max(const Vector &a){return a.maxCoeff();} GTSAM_EXPORT inline double norm_2(const Vector& v) {return v.norm();} GTSAM_EXPORT inline Vector reciprocal(const Vector &a) {return a.array().inverse();} +GTSAM_EXPORT void subInsert(Vector& fullVector, const Vector& subVector, size_t i) {fullVector.segment(i, subVector.size()) = subVector;} GTSAM_EXPORT inline double sum(const Vector &a){return a.sum();} #endif diff --git a/gtsam/base/tests/testVector.cpp b/gtsam/base/tests/testVector.cpp index 718ce9c80..7e421573b 100644 --- a/gtsam/base/tests/testVector.cpp +++ b/gtsam/base/tests/testVector.cpp @@ -142,7 +142,7 @@ TEST(Vector, subInsert ) small = ones(3); size_t i = 2; - subInsert(big, small, i); + big.segment(i,small.size()) = small; Vector expected = (Vector(6) << 0.0, 0.0, 1.0, 1.0, 1.0, 0.0).finished(); diff --git a/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp b/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp index 8b224f510..a3c622d9b 100644 --- a/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp +++ b/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp @@ -90,7 +90,8 @@ Vector newtonEuler(const Vector& Vb, const Vector& Fb, const Matrix& Inertia) { TEST( DiscreteEulerPoincareHelicopter, evaluateError) { Vector Fu = computeFu(gamma2, u2); Vector fGravity_g1 = zero(6); - subInsert(fGravity_g1, g1.rotation().unrotate(Point3(0.0, 0.0, -mass*9.81)), 3); // gravity force in g1 frame + Vector subVector = g1.rotation().unrotate(Point3(0.0, 0.0, -mass*9.81)); + fGravity_g1.segment(3,subVector.size()) = subVector; Vector Fb = Fu+fGravity_g1; Vector dV = newtonEuler(V1_g1, Fb, Inertia); From 97e26cc3656d254c4e3d81ddb107b3026c4ac6c8 Mon Sep 17 00:00:00 2001 From: Alex Hagiopol Date: Fri, 11 Mar 2016 20:30:19 -0500 Subject: [PATCH 78/90] Deprecated sub() in Vector.h. --- gtsam/base/Vector.cpp | 5 ----- gtsam/base/Vector.h | 10 +--------- gtsam/base/tests/testVector.cpp | 2 +- gtsam/linear/VectorValues.cpp | 2 +- gtsam_unstable/slam/Mechanization_bRn2.cpp | 6 +++--- 5 files changed, 6 insertions(+), 19 deletions(-) diff --git a/gtsam/base/Vector.cpp b/gtsam/base/Vector.cpp index 3365525c4..c69a0ad20 100644 --- a/gtsam/base/Vector.cpp +++ b/gtsam/base/Vector.cpp @@ -176,11 +176,6 @@ bool linear_dependent(const Vector& vec1, const Vector& vec2, double tol) { return flag; } -/* ************************************************************************* */ -ConstSubVector sub(const Vector &v, size_t i1, size_t i2) { - return v.segment(i1,i2-i1); -} - /* ************************************************************************* */ Vector ediv_(const Vector &a, const Vector &b) { size_t n = a.size(); diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index 3b0634702..d4502b6ad 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -195,15 +195,6 @@ GTSAM_EXPORT bool assert_equal(const ConstSubVector& vec1, const ConstSubVector& */ GTSAM_EXPORT bool linear_dependent(const Vector& vec1, const Vector& vec2, double tol=1e-9); -/** - * extract subvector, slice semantics, i.e. range = [i1,i2[ excluding i2 - * @param v Vector - * @param i1 first row index - * @param i2 last row index + 1 - * @return subvector v(i1:i2) - */ -GTSAM_EXPORT ConstSubVector sub(const Vector &v, size_t i1, size_t i2); - /** * elementwise division, but 0/0 = 0, not inf * @param a first vector @@ -296,6 +287,7 @@ GTSAM_EXPORT inline Vector emul(const Vector &a, const Vector &b) {assert (b.siz GTSAM_EXPORT inline double max(const Vector &a){return a.maxCoeff();} GTSAM_EXPORT inline double norm_2(const Vector& v) {return v.norm();} GTSAM_EXPORT inline Vector reciprocal(const Vector &a) {return a.array().inverse();} +GTSAM_EXPORT ConstSubVector sub(const Vector &v, size_t i1, size_t i2) {return v.segment(i1,i2-i1);} GTSAM_EXPORT void subInsert(Vector& fullVector, const Vector& subVector, size_t i) {fullVector.segment(i, subVector.size()) = subVector;} GTSAM_EXPORT inline double sum(const Vector &a){return a.sum();} #endif diff --git a/gtsam/base/tests/testVector.cpp b/gtsam/base/tests/testVector.cpp index 7e421573b..7e794ac04 100644 --- a/gtsam/base/tests/testVector.cpp +++ b/gtsam/base/tests/testVector.cpp @@ -126,7 +126,7 @@ TEST(Vector, sub ) a(0) = 10; a(1) = 20; a(2) = 3; a(3) = 34; a(4) = 11; a(5) = 2; - Vector result(sub(a,2,5)); + Vector result(a.segment(2,3)); Vector b(3); b(0) = 3; b(1) = 34; b(2) =11; diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp index 33c62cfb6..1439d4b61 100644 --- a/gtsam/linear/VectorValues.cpp +++ b/gtsam/linear/VectorValues.cpp @@ -52,7 +52,7 @@ namespace gtsam { Key key; size_t n; boost::tie(key, n) = v; - values_.insert(make_pair(key, sub(x, j, j + n))); + values_.insert(make_pair(key, x.segment(j,n))); j += n; } } diff --git a/gtsam_unstable/slam/Mechanization_bRn2.cpp b/gtsam_unstable/slam/Mechanization_bRn2.cpp index 2205f53bd..edd92b580 100644 --- a/gtsam_unstable/slam/Mechanization_bRn2.cpp +++ b/gtsam_unstable/slam/Mechanization_bRn2.cpp @@ -67,13 +67,13 @@ Mechanization_bRn2 Mechanization_bRn2::initialize(const Matrix& U, /* ************************************************************************* */ Mechanization_bRn2 Mechanization_bRn2::correct(const Vector3& dx) const { - Vector3 rho = sub(dx, 0, 3); + Vector3 rho = dx.segment(0,3); Rot3 delta_nRn = Rot3::Rodrigues(rho); Rot3 bRn = bRn_ * delta_nRn; - Vector3 x_g = x_g_ + sub(dx, 3, 6); - Vector3 x_a = x_a_ + sub(dx, 6, 9); + Vector3 x_g = x_g_ + dx.segment(3,3); + Vector3 x_a = x_a_ + dx.segment(6,3); return Mechanization_bRn2(bRn, x_g, x_a); } From 026a0f59fed39fe07c39c3e6f08b8c6508b38a5b Mon Sep 17 00:00:00 2001 From: Alex Hagiopol Date: Fri, 11 Mar 2016 23:10:18 -0500 Subject: [PATCH 79/90] Deprecated repeat() in Vector.h. --- gtsam/base/Vector.cpp | 5 ----- gtsam/base/Vector.h | 14 ++++---------- gtsam/geometry/tests/testPose3.cpp | 2 +- gtsam/geometry/tests/testRot3.cpp | 4 ++-- gtsam/linear/NoiseModel.cpp | 2 +- gtsam/linear/NoiseModel.h | 14 +++++++------- gtsam/linear/tests/testNoiseModel.cpp | 12 ++++++------ gtsam/nonlinear/NonlinearEquality.h | 2 +- .../tests/testRegularImplicitSchurFactor.cpp | 16 ++++++++-------- .../geometry/tests/testSimilarity3.cpp | 2 +- tests/testBoundingConstraint.cpp | 16 ++++++++-------- tests/testNonlinearEquality.cpp | 2 +- 12 files changed, 40 insertions(+), 51 deletions(-) diff --git a/gtsam/base/Vector.cpp b/gtsam/base/Vector.cpp index c69a0ad20..dbfe7ab87 100644 --- a/gtsam/base/Vector.cpp +++ b/gtsam/base/Vector.cpp @@ -42,11 +42,6 @@ bool zero(const Vector& v) { return result; } -/* ************************************************************************* */ -Vector repeat(size_t n, double value) { - return Vector::Constant(n, value); -} - /* ************************************************************************* */ Vector delta(size_t n, size_t i, double value) { return Vector::Unit(n, i) * value; diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index d4502b6ad..9438937ce 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -63,13 +63,6 @@ GTSAM_MAKE_VECTOR_DEFS(12); typedef Eigen::VectorBlock SubVector; typedef Eigen::VectorBlock ConstSubVector; -/** - * Create vector initialized to a constant value - * @param n is the size of the vector - * @param value is a constant value to insert into the vector - */ -GTSAM_EXPORT Vector repeat(size_t n, double value); - /** * Create basis vector of dimension n, * with a constant in spot i @@ -281,14 +274,15 @@ GTSAM_EXPORT Vector concatVectors(size_t nrVectors, ...); #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 GTSAM_EXPORT inline Vector abs(const Vector& v){return v.cwiseAbs();} -GTSAM_EXPORT Vector ediv(const Vector &a, const Vector &b) {assert (b.size()==a.size()); return a.cwiseQuotient(b);} +GTSAM_EXPORT inline Vector ediv(const Vector &a, const Vector &b) {assert (b.size()==a.size()); return a.cwiseQuotient(b);} GTSAM_EXPORT inline Vector esqrt(const Vector& v) { return v.cwiseSqrt();} GTSAM_EXPORT inline Vector emul(const Vector &a, const Vector &b) {assert (b.size()==a.size()); return a.cwiseProduct(b);} GTSAM_EXPORT inline double max(const Vector &a){return a.maxCoeff();} GTSAM_EXPORT inline double norm_2(const Vector& v) {return v.norm();} GTSAM_EXPORT inline Vector reciprocal(const Vector &a) {return a.array().inverse();} -GTSAM_EXPORT ConstSubVector sub(const Vector &v, size_t i1, size_t i2) {return v.segment(i1,i2-i1);} -GTSAM_EXPORT void subInsert(Vector& fullVector, const Vector& subVector, size_t i) {fullVector.segment(i, subVector.size()) = subVector;} +GTSAM_EXPORT inline Vector repeat(size_t n, double value) {return Vector::Constant(n, value);} +GTSAM_EXPORT inline const SubVector sub(const Vector &v, size_t i1, size_t i2) {return v.segment(i1,i2-i1);} +GTSAM_EXPORT inline void subInsert(Vector& fullVector, const Vector& subVector, size_t i) {fullVector.segment(i, subVector.size()) = subVector;} GTSAM_EXPORT inline double sum(const Vector &a){return a.sum();} #endif diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 0e7da537a..e096f23ce 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -538,7 +538,7 @@ TEST(Pose3, retract_localCoordinates) /* ************************************************************************* */ TEST(Pose3, expmap_logmap) { - Vector d12 = repeat(6,0.1); + Vector d12 = Vector::Constant(6,0.1); Pose3 t1 = T, t2 = t1.expmap(d12); EXPECT(assert_equal(d12, t1.logmap(t2))); } diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 9621898bf..f5e0a6913 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -224,14 +224,14 @@ TEST(Rot3, log) /* ************************************************************************* */ TEST(Rot3, retract_localCoordinates) { - Vector3 d12 = repeat(3,0.1); + Vector3 d12 = Vector::Constant(3,0.1); Rot3 R2 = R.retract(d12); EXPECT(assert_equal(d12, R.localCoordinates(R2))); } /* ************************************************************************* */ TEST(Rot3, expmap_logmap) { - Vector3 d12 = repeat(3,0.1); + Vector3 d12 = Vector::Constant(3,0.1); Rot3 R2 = R.expmap(d12); EXPECT(assert_equal(d12, R.logmap(R2))); } diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index 5212374a7..31b059989 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -326,7 +326,7 @@ static void fix(const Vector& sigmas, Vector& precisions, Vector& invsigmas) { /* ************************************************************************* */ Constrained::Constrained(const Vector& sigmas) - : Diagonal(sigmas), mu_(repeat(sigmas.size(), 1000.0)) { + : Diagonal(sigmas), mu_(Vector::Constant(sigmas.size(), 1000.0)) { internal::fix(sigmas, precisions_, invsigmas_); } diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index cef270100..c1718c0b3 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -416,7 +416,7 @@ namespace gtsam { * standard devations, some of which might be zero */ static shared_ptr MixedSigmas(const Vector& sigmas) { - return MixedSigmas(repeat(sigmas.size(), 1000.0), sigmas); + return MixedSigmas(Vector::Constant(sigmas.size(), 1000.0), sigmas); } /** @@ -424,7 +424,7 @@ namespace gtsam { * standard devations, some of which might be zero */ static shared_ptr MixedSigmas(double m, const Vector& sigmas) { - return MixedSigmas(repeat(sigmas.size(), m), sigmas); + return MixedSigmas(Vector::Constant(sigmas.size(), m), sigmas); } /** @@ -458,17 +458,17 @@ namespace gtsam { /** Fully constrained variations */ static shared_ptr All(size_t dim) { - return shared_ptr(new Constrained(repeat(dim, 1000.0), repeat(dim,0))); + return shared_ptr(new Constrained(Vector::Constant(dim, 1000.0), Vector::Constant(dim,0))); } /** Fully constrained variations */ static shared_ptr All(size_t dim, const Vector& mu) { - return shared_ptr(new Constrained(mu, repeat(dim,0))); + return shared_ptr(new Constrained(mu, Vector::Constant(dim,0))); } /** Fully constrained variations with a mu parameter */ static shared_ptr All(size_t dim, double mu) { - return shared_ptr(new Constrained(repeat(dim, mu), repeat(dim,0))); + return shared_ptr(new Constrained(Vector::Constant(dim, mu), Vector::Constant(dim,0))); } virtual void print(const std::string& name) const; @@ -522,10 +522,10 @@ namespace gtsam { /** protected constructor takes sigma */ Isotropic(size_t dim, double sigma) : - Diagonal(repeat(dim, sigma)),sigma_(sigma),invsigma_(1.0/sigma) {} + Diagonal(Vector::Constant(dim, sigma)),sigma_(sigma),invsigma_(1.0/sigma) {} /* dummy constructor to allow for serialization */ - Isotropic() : Diagonal(repeat(1, 1.0)),sigma_(1.0),invsigma_(1.0) {} + Isotropic() : Diagonal(Vector::Constant(1, 1.0)),sigma_(1.0),invsigma_(1.0) {} public: diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index 7ac4846f9..434d94d7f 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -155,13 +155,13 @@ TEST(NoiseModel, ConstrainedConstructors ) Vector3 mu(200.0, 300.0, 400.0); actual = Constrained::All(d); // TODO: why should this be a thousand ??? Dummy variable? - EXPECT(assert_equal(gtsam::repeat(d, 1000.0), actual->mu())); - EXPECT(assert_equal(gtsam::repeat(d, 0), actual->sigmas())); - EXPECT(assert_equal(gtsam::repeat(d, 0), actual->invsigmas())); // Actually zero as dummy value - EXPECT(assert_equal(gtsam::repeat(d, 0), actual->precisions())); // Actually zero as dummy value + EXPECT(assert_equal(Vector::Constant(d, 1000.0), actual->mu())); + EXPECT(assert_equal(Vector::Constant(d, 0), actual->sigmas())); + EXPECT(assert_equal(Vector::Constant(d, 0), actual->invsigmas())); // Actually zero as dummy value + EXPECT(assert_equal(Vector::Constant(d, 0), actual->precisions())); // Actually zero as dummy value actual = Constrained::All(d, m); - EXPECT(assert_equal(gtsam::repeat(d, m), actual->mu())); + EXPECT(assert_equal(Vector::Constant(d, m), actual->mu())); actual = Constrained::All(d, mu); EXPECT(assert_equal(mu, actual->mu())); @@ -170,7 +170,7 @@ TEST(NoiseModel, ConstrainedConstructors ) EXPECT(assert_equal(mu, actual->mu())); actual = Constrained::MixedSigmas(m, sigmas); - EXPECT(assert_equal( gtsam::repeat(d, m), actual->mu())); + EXPECT(assert_equal(Vector::Constant(d, m), actual->mu())); } /* ************************************************************************* */ diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index 86fb34fe0..e24c0d610 100644 --- a/gtsam/nonlinear/NonlinearEquality.h +++ b/gtsam/nonlinear/NonlinearEquality.h @@ -153,7 +153,7 @@ public: throw std::invalid_argument( "Linearization point not feasible for " + DefaultKeyFormatter(this->key()) + "!"); - return repeat(nj, std::numeric_limits::infinity()); // set error to infinity if not equal + return Vector::Constant(nj, std::numeric_limits::infinity()); // set error to infinity if not equal } } diff --git a/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp b/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp index 77944da83..3664de9c5 100644 --- a/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp +++ b/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp @@ -69,16 +69,16 @@ TEST( regularImplicitSchurFactor, addHessianMultiply ) { double alpha = 0.5; VectorValues xvalues = map_list_of // - (0, gtsam::repeat(6, 2))// - (1, gtsam::repeat(6, 4))// - (2, gtsam::repeat(6, 0))// distractor - (3, gtsam::repeat(6, 8)); + (0, Vector::Constant(6, 2))// + (1, Vector::Constant(6, 4))// + (2, Vector::Constant(6, 0))// distractor + (3, Vector::Constant(6, 8)); VectorValues yExpected = map_list_of// - (0, gtsam::repeat(6, 27))// - (1, gtsam::repeat(6, -40))// - (2, gtsam::repeat(6, 0))// distractor - (3, gtsam::repeat(6, 279)); + (0, Vector::Constant(6, 27))// + (1, Vector::Constant(6, -40))// + (2, Vector::Constant(6, 0))// distractor + (3, Vector::Constant(6, 279)); // Create full F size_t M=4, m = 3, d = 6; diff --git a/gtsam_unstable/geometry/tests/testSimilarity3.cpp b/gtsam_unstable/geometry/tests/testSimilarity3.cpp index e2f2a2a86..b1630c776 100644 --- a/gtsam_unstable/geometry/tests/testSimilarity3.cpp +++ b/gtsam_unstable/geometry/tests/testSimilarity3.cpp @@ -167,7 +167,7 @@ TEST( Similarity3, retract_first_order) { //****************************************************************************** TEST(Similarity3, localCoordinates_first_order) { - Vector d12 = repeat(7, 0.1); + Vector d12 = Vector::Constant(7, 0.1); d12(6) = 1.0; Similarity3 t1 = T1, t2 = t1.retract(d12); EXPECT(assert_equal(d12, t1.localCoordinates(t2))); diff --git a/tests/testBoundingConstraint.cpp b/tests/testBoundingConstraint.cpp index 7c1e20a1a..d4cefd329 100644 --- a/tests/testBoundingConstraint.cpp +++ b/tests/testBoundingConstraint.cpp @@ -73,8 +73,8 @@ TEST( testBoundingConstraint, unary_basics_inactive2 ) { EXPECT_DOUBLES_EQUAL(2.0, constraint4.threshold(), tol); EXPECT(!constraint3.isGreaterThan()); EXPECT(!constraint4.isGreaterThan()); - EXPECT(assert_equal(repeat(1, 3.0), constraint3.evaluateError(pt2), tol)); - EXPECT(assert_equal(repeat(1, 5.0), constraint4.evaluateError(pt2), tol)); + EXPECT(assert_equal(Vector::Constant(1, 3.0), constraint3.evaluateError(pt2), tol)); + EXPECT(assert_equal(Vector::Constant(1, 5.0), constraint4.evaluateError(pt2), tol)); EXPECT(assert_equal(zero(1), constraint3.unwhitenedError(config), tol)); EXPECT(assert_equal(zero(1), constraint4.unwhitenedError(config), tol)); EXPECT_DOUBLES_EQUAL(0.0, constraint3.error(config), tol); @@ -88,10 +88,10 @@ TEST( testBoundingConstraint, unary_basics_active1 ) { config.insert(key, pt2); EXPECT(constraint1.active(config)); EXPECT(constraint2.active(config)); - EXPECT(assert_equal(repeat(1,-3.0), constraint1.evaluateError(pt2), tol)); - EXPECT(assert_equal(repeat(1,-5.0), constraint2.evaluateError(pt2), tol)); - EXPECT(assert_equal(repeat(1,-3.0), constraint1.unwhitenedError(config), tol)); - EXPECT(assert_equal(repeat(1,-5.0), constraint2.unwhitenedError(config), tol)); + EXPECT(assert_equal(Vector::Constant(1,-3.0), constraint1.evaluateError(pt2), tol)); + EXPECT(assert_equal(Vector::Constant(1,-5.0), constraint2.evaluateError(pt2), tol)); + EXPECT(assert_equal(Vector::Constant(1,-3.0), constraint1.unwhitenedError(config), tol)); + EXPECT(assert_equal(Vector::Constant(1,-5.0), constraint2.unwhitenedError(config), tol)); EXPECT_DOUBLES_EQUAL(45.0, constraint1.error(config), tol); EXPECT_DOUBLES_EQUAL(125.0, constraint2.error(config), tol); } @@ -129,8 +129,8 @@ TEST( testBoundingConstraint, unary_linearization_active) { config2.insert(key, pt2); GaussianFactor::shared_ptr actual1 = constraint1.linearize(config2); GaussianFactor::shared_ptr actual2 = constraint2.linearize(config2); - JacobianFactor expected1(key, (Matrix(1, 2) << 1.0, 0.0).finished(), repeat(1, 3.0), hard_model1); - JacobianFactor expected2(key, (Matrix(1, 2) << 0.0, 1.0).finished(), repeat(1, 5.0), hard_model1); + JacobianFactor expected1(key, (Matrix(1, 2) << 1.0, 0.0).finished(), Vector::Constant(1, 3.0), hard_model1); + JacobianFactor expected2(key, (Matrix(1, 2) << 0.0, 1.0).finished(), Vector::Constant(1, 5.0), hard_model1); EXPECT(assert_equal((const GaussianFactor&)expected1, *actual1, tol)); EXPECT(assert_equal((const GaussianFactor&)expected2, *actual2, tol)); } diff --git a/tests/testNonlinearEquality.cpp b/tests/testNonlinearEquality.cpp index f72e08083..7142f72d5 100644 --- a/tests/testNonlinearEquality.cpp +++ b/tests/testNonlinearEquality.cpp @@ -137,7 +137,7 @@ TEST ( NonlinearEquality, error ) { actual = nle->unwhitenedError(bad_linearize); EXPECT( - assert_equal(actual, repeat(3, std::numeric_limits::infinity()))); + assert_equal(actual, Vector::Constant(3, std::numeric_limits::infinity()))); } //****************************************************************************** From f2d8bc7d0ee1995d6a141834616662e21a0eaded Mon Sep 17 00:00:00 2001 From: Alex Hagiopol Date: Sat, 12 Mar 2016 19:25:45 -0500 Subject: [PATCH 80/90] Return type of sub() changed to Vector to eliminate errors when GTSAM_ALLOW_DEPRECATED_SINCE_V4 is off. --- gtsam/base/Vector.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index 9438937ce..51d2f07c0 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -272,6 +272,7 @@ GTSAM_EXPORT Vector concatVectors(const std::list& vs); */ GTSAM_EXPORT Vector concatVectors(size_t nrVectors, ...); + #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 GTSAM_EXPORT inline Vector abs(const Vector& v){return v.cwiseAbs();} GTSAM_EXPORT inline Vector ediv(const Vector &a, const Vector &b) {assert (b.size()==a.size()); return a.cwiseQuotient(b);} @@ -281,7 +282,7 @@ GTSAM_EXPORT inline double max(const Vector &a){return a.maxCoeff();} GTSAM_EXPORT inline double norm_2(const Vector& v) {return v.norm();} GTSAM_EXPORT inline Vector reciprocal(const Vector &a) {return a.array().inverse();} GTSAM_EXPORT inline Vector repeat(size_t n, double value) {return Vector::Constant(n, value);} -GTSAM_EXPORT inline const SubVector sub(const Vector &v, size_t i1, size_t i2) {return v.segment(i1,i2-i1);} +GTSAM_EXPORT inline const Vector sub(const Vector &v, size_t i1, size_t i2) {return v.segment(i1,i2-i1);} GTSAM_EXPORT inline void subInsert(Vector& fullVector, const Vector& subVector, size_t i) {fullVector.segment(i, subVector.size()) = subVector;} GTSAM_EXPORT inline double sum(const Vector &a){return a.sum();} #endif From cdecdfbf1eb77c0b2d7ce207e9c755667d08553c Mon Sep 17 00:00:00 2001 From: Alex Hagiopol Date: Sat, 12 Mar 2016 20:00:42 -0500 Subject: [PATCH 81/90] Replaced repeat() with Vector::Constant to please Jenkins. --- timing/timeSchurFactors.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/timing/timeSchurFactors.cpp b/timing/timeSchurFactors.cpp index e08924400..ae816917e 100644 --- a/timing/timeSchurFactors.cpp +++ b/timing/timeSchurFactors.cpp @@ -55,14 +55,14 @@ void timeAll(size_t m, size_t N) { Matrix P = (E.transpose() * E).inverse(); // RHS and sigmas - const Vector b = gtsam::repeat(2 * m, 1); + const Vector b = Vector::Constant(2*m,1); const SharedDiagonal model; // parameters for multiplyHessianAdd double alpha = 0.5; VectorValues xvalues, yvalues; for (size_t i = 0; i < m; i++) - xvalues.insert(i, gtsam::repeat(D, 2)); + xvalues.insert(i, Vector::Constant(D,2); // Implicit RegularImplicitSchurFactor implicitFactor(keys, Fblocks, E, P, b); From d45a1ecda3595aa3bd1a69de1f9defc693d17bb3 Mon Sep 17 00:00:00 2001 From: Alex Hagiopol Date: Sat, 12 Mar 2016 20:08:14 -0500 Subject: [PATCH 82/90] Fixed stupid mistake. --- timing/timeSchurFactors.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/timing/timeSchurFactors.cpp b/timing/timeSchurFactors.cpp index ae816917e..29e8e9916 100644 --- a/timing/timeSchurFactors.cpp +++ b/timing/timeSchurFactors.cpp @@ -62,7 +62,7 @@ void timeAll(size_t m, size_t N) { double alpha = 0.5; VectorValues xvalues, yvalues; for (size_t i = 0; i < m; i++) - xvalues.insert(i, Vector::Constant(D,2); + xvalues.insert(i, Vector::Constant(D,2)); // Implicit RegularImplicitSchurFactor implicitFactor(keys, Fblocks, E, P, b); From ae19e078c7849a86b1a05638813742e7c7ec822a Mon Sep 17 00:00:00 2001 From: Alex Hagiopol Date: Sat, 12 Mar 2016 20:44:05 -0500 Subject: [PATCH 83/90] Changed argument and implementation of correct() in Mechanization_bRn2 --- gtsam_unstable/slam/Mechanization_bRn2.cpp | 8 ++++---- gtsam_unstable/slam/Mechanization_bRn2.h | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/gtsam_unstable/slam/Mechanization_bRn2.cpp b/gtsam_unstable/slam/Mechanization_bRn2.cpp index edd92b580..94126a68c 100644 --- a/gtsam_unstable/slam/Mechanization_bRn2.cpp +++ b/gtsam_unstable/slam/Mechanization_bRn2.cpp @@ -66,14 +66,14 @@ Mechanization_bRn2 Mechanization_bRn2::initialize(const Matrix& U, } /* ************************************************************************* */ -Mechanization_bRn2 Mechanization_bRn2::correct(const Vector3& dx) const { - Vector3 rho = dx.segment(0,3); +Mechanization_bRn2 Mechanization_bRn2::correct(const Vector9& dx) const { + Vector3 rho = dx.segment<3>(0); Rot3 delta_nRn = Rot3::Rodrigues(rho); Rot3 bRn = bRn_ * delta_nRn; - Vector3 x_g = x_g_ + dx.segment(3,3); - Vector3 x_a = x_a_ + dx.segment(6,3); + Vector3 x_g = x_g_ + dx.segment<3>(3); + Vector3 x_a = x_a_ + dx.segment<3>(6); return Mechanization_bRn2(bRn, x_g, x_a); } diff --git a/gtsam_unstable/slam/Mechanization_bRn2.h b/gtsam_unstable/slam/Mechanization_bRn2.h index 4c85614d4..4268aa0e5 100644 --- a/gtsam_unstable/slam/Mechanization_bRn2.h +++ b/gtsam_unstable/slam/Mechanization_bRn2.h @@ -68,7 +68,7 @@ public: * @param obj The current state * @param dx The error used to correct and return a new state */ - Mechanization_bRn2 correct(const Vector3& dx) const; + Mechanization_bRn2 correct(const Vector9& dx) const; /** * Integrate to get new state From 93bf31f8527d98ba92707ba15556cb69e217d470 Mon Sep 17 00:00:00 2001 From: Alex Hagiopol Date: Sat, 12 Mar 2016 20:47:16 -0500 Subject: [PATCH 84/90] Replaced Vector::Constant(N,value) with VectorN::Constant(value). --- gtsam/geometry/tests/testPose3.cpp | 2 +- gtsam/geometry/tests/testRot3.cpp | 6 +++--- gtsam/linear/NoiseModel.h | 2 +- gtsam_unstable/geometry/tests/testSimilarity3.cpp | 2 +- 4 files changed, 6 insertions(+), 6 deletions(-) diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index e096f23ce..648197ced 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -538,7 +538,7 @@ TEST(Pose3, retract_localCoordinates) /* ************************************************************************* */ TEST(Pose3, expmap_logmap) { - Vector d12 = Vector::Constant(6,0.1); + Vector d12 = Vector6::Constant(0.1); Pose3 t1 = T, t2 = t1.expmap(d12); EXPECT(assert_equal(d12, t1.logmap(t2))); } diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index f5e0a6913..876897ab3 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -224,16 +224,16 @@ TEST(Rot3, log) /* ************************************************************************* */ TEST(Rot3, retract_localCoordinates) { - Vector3 d12 = Vector::Constant(3,0.1); + Vector3 d12 = Vector3::Constant(0.1); Rot3 R2 = R.retract(d12); EXPECT(assert_equal(d12, R.localCoordinates(R2))); } /* ************************************************************************* */ TEST(Rot3, expmap_logmap) { - Vector3 d12 = Vector::Constant(3,0.1); + Vector3 d12 = Vector3::Constant(0.1); Rot3 R2 = R.expmap(d12); - EXPECT(assert_equal(d12, R.logmap(R2))); + EXPECT(assert_equal(d12, (Vector) R.logmap(R2))); } /* ************************************************************************* */ diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index c1718c0b3..15fad5a78 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -525,7 +525,7 @@ namespace gtsam { Diagonal(Vector::Constant(dim, sigma)),sigma_(sigma),invsigma_(1.0/sigma) {} /* dummy constructor to allow for serialization */ - Isotropic() : Diagonal(Vector::Constant(1, 1.0)),sigma_(1.0),invsigma_(1.0) {} + Isotropic() : Diagonal(Vector1::Constant(1.0)),sigma_(1.0),invsigma_(1.0) {} public: diff --git a/gtsam_unstable/geometry/tests/testSimilarity3.cpp b/gtsam_unstable/geometry/tests/testSimilarity3.cpp index b1630c776..1866fafc6 100644 --- a/gtsam_unstable/geometry/tests/testSimilarity3.cpp +++ b/gtsam_unstable/geometry/tests/testSimilarity3.cpp @@ -167,7 +167,7 @@ TEST( Similarity3, retract_first_order) { //****************************************************************************** TEST(Similarity3, localCoordinates_first_order) { - Vector d12 = Vector::Constant(7, 0.1); + Vector7 d12 = Vector7::Constant(0.1); d12(6) = 1.0; Similarity3 t1 = T1, t2 = t1.retract(d12); EXPECT(assert_equal(d12, t1.localCoordinates(t2))); From c769ed845205080f11ac318defde5282376f8632 Mon Sep 17 00:00:00 2001 From: Alex Hagiopol Date: Sat, 12 Mar 2016 20:49:00 -0500 Subject: [PATCH 85/90] One line definition of gravity force. --- gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp b/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp index a3c622d9b..6f75c264c 100644 --- a/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp +++ b/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp @@ -90,8 +90,7 @@ Vector newtonEuler(const Vector& Vb, const Vector& Fb, const Matrix& Inertia) { TEST( DiscreteEulerPoincareHelicopter, evaluateError) { Vector Fu = computeFu(gamma2, u2); Vector fGravity_g1 = zero(6); - Vector subVector = g1.rotation().unrotate(Point3(0.0, 0.0, -mass*9.81)); - fGravity_g1.segment(3,subVector.size()) = subVector; + fGravity_g1.segment<3>(3) = g1.rotation().unrotate(Vector3(0, 0, -mass*9.81)); // gravity force in g1 frame Vector Fb = Fu+fGravity_g1; Vector dV = newtonEuler(V1_g1, Fb, Inertia); From 3a222cf8c456db037aa2a6e49f448af95cd3e506 Mon Sep 17 00:00:00 2001 From: Alex Hagiopol Date: Sat, 12 Mar 2016 21:53:13 -0500 Subject: [PATCH 86/90] Removed tests for deprecated functions. --- gtsam/base/tests/testVector.cpp | 48 --------------------------------- 1 file changed, 48 deletions(-) diff --git a/gtsam/base/tests/testVector.cpp b/gtsam/base/tests/testVector.cpp index 7e794ac04..790e0922c 100644 --- a/gtsam/base/tests/testVector.cpp +++ b/gtsam/base/tests/testVector.cpp @@ -119,36 +119,6 @@ TEST(Vector, negate ) EXPECT(assert_equal(b, -a)); } -/* ************************************************************************* */ -TEST(Vector, sub ) -{ - Vector a(6); - a(0) = 10; a(1) = 20; a(2) = 3; - a(3) = 34; a(4) = 11; a(5) = 2; - - Vector result(a.segment(2,3)); - - Vector b(3); - b(0) = 3; b(1) = 34; b(2) =11; - - EXPECT(b==result); - EXPECT(assert_equal(b, result)); -} - -/* ************************************************************************* */ -TEST(Vector, subInsert ) -{ - Vector big = zero(6), - small = ones(3); - - size_t i = 2; - big.segment(i,small.size()) = small; - - Vector expected = (Vector(6) << 0.0, 0.0, 1.0, 1.0, 1.0, 0.0).finished(); - - EXPECT(assert_equal(expected, big)); -} - /* ************************************************************************* */ TEST(Vector, householder ) { @@ -252,17 +222,6 @@ TEST(Vector, weightedPseudoinverse_nan ) DOUBLES_EQUAL(100, precision, 1e-5); } -/* ************************************************************************* */ -TEST(Vector, ediv ) -{ - Vector a = Vector3(10., 20., 30.); - Vector b = Vector3(2.0, 5.0, 6.0); - Vector actual(a.cwiseQuotient(b)); - - Vector c = Vector3(5.0, 4.0, 5.0); - EXPECT(assert_equal(c,actual)); -} - /* ************************************************************************* */ TEST(Vector, dot ) { @@ -302,13 +261,6 @@ TEST(Vector, greater_than ) EXPECT(greaterThanOrEqual(v1, v2)); // test equals } -/* ************************************************************************* */ -TEST(Vector, reciprocal ) -{ - Vector v = Vector3(1.0, 2.0, 4.0); - EXPECT(assert_equal(Vector3(1.0, 0.5, 0.25), (Vector) v.array().inverse())); -} - /* ************************************************************************* */ TEST(Vector, linear_dependent ) { From 76a605c5ff337d916056082aee23a07b61ac8431 Mon Sep 17 00:00:00 2001 From: Jing Dong Date: Wed, 30 Mar 2016 12:33:13 -0400 Subject: [PATCH 87/90] more fix of smart factor serialization --- gtsam/geometry/triangulation.h | 21 +++++++++++++++++++ gtsam/slam/SmartFactorBase.h | 5 ++++- gtsam/slam/SmartProjectionFactor.h | 19 +++++++++++++++-- .../tests/testSmartProjectionPoseFactor.cpp | 20 ++++++++++++++++++ 4 files changed, 62 insertions(+), 3 deletions(-) diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index c6e613b14..d08aef042 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -363,6 +363,18 @@ struct TriangulationParameters { << p.dynamicOutlierRejectionThreshold << std::endl; return os; } + +private: + + /// Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(rankTolerance); + ar & BOOST_SERIALIZATION_NVP(enableEPI); + ar & BOOST_SERIALIZATION_NVP(landmarkDistanceThreshold); + ar & BOOST_SERIALIZATION_NVP(dynamicOutlierRejectionThreshold); + } }; /** @@ -411,6 +423,15 @@ public: os << "no point, status = " << result.status_ << std::endl; return os; } + +private: + + /// Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(status_); + } }; /// triangulateSafe: extensive checking of the outcome diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index 01a8fcf8d..010ceffca 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -29,6 +29,7 @@ #include #include +#include #include #include @@ -67,7 +68,7 @@ protected: std::vector measured_; /// @name Pose of the camera in the body frame - const boost::optional body_P_sensor_; ///< Pose of the camera in the body frame + boost::optional body_P_sensor_; ///< Pose of the camera in the body frame /// @} static const int Dim = traits::dimension; ///< Camera dimension @@ -392,7 +393,9 @@ private: template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar & BOOST_SERIALIZATION_NVP(noiseModel_); ar & BOOST_SERIALIZATION_NVP(measured_); + ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); } }; // end class SmartFactorBase diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index 09fe84caa..631527d86 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -112,6 +112,20 @@ struct GTSAM_EXPORT SmartProjectionParams { void setDynamicOutlierRejectionThreshold(double dynOutRejectionThreshold) { triangulation.dynamicOutlierRejectionThreshold = dynOutRejectionThreshold; } + +private: + + /// Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(linearizationMode); + ar & BOOST_SERIALIZATION_NVP(degeneracyMode); + ar & BOOST_SERIALIZATION_NVP(triangulation); + ar & BOOST_SERIALIZATION_NVP(retriangulationThreshold); + ar & BOOST_SERIALIZATION_NVP(throwCheirality); + ar & BOOST_SERIALIZATION_NVP(verboseCheirality); + } }; /** @@ -535,8 +549,9 @@ private: template void serialize(ARCHIVE & ar, const unsigned int version) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - ar & BOOST_SERIALIZATION_NVP(params_.throwCheirality); - ar & BOOST_SERIALIZATION_NVP(params_.verboseCheirality); + ar & BOOST_SERIALIZATION_NVP(params_); + ar & BOOST_SERIALIZATION_NVP(result_); + ar & BOOST_SERIALIZATION_NVP(cameraPosesTriangulation_); } } ; diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index 1c1bc3c03..b6ecd1db6 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -1409,6 +1409,26 @@ TEST(SmartProjectionPoseFactor, serialize) { EXPECT(equalsBinary(factor)); } +TEST(SmartProjectionPoseFactor, serialize2) { + using namespace vanillaPose; + using namespace gtsam::serializationTestHelpers; + SmartProjectionParams params; + params.setRankTolerance(rankTol); + Pose3 bts; + SmartFactor factor(model, sharedK, bts, params); + + // insert some measurments + vector key_view; + vector meas_view; + key_view.push_back(Symbol('x', 1)); + meas_view.push_back(Point2(10, 10)); + factor.add(meas_view, key_view); + + EXPECT(equalsObj(factor)); + EXPECT(equalsXML(factor)); + EXPECT(equalsBinary(factor)); +} + /* ************************************************************************* */ int main() { TestResult tr; From 71b6c1da8244426a8222fb2fb1a2fe09117d489a Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 10 Apr 2016 10:56:09 -0700 Subject: [PATCH 88/90] Fixed issues with factor and IMU parameters --- .../FlightCameraTransformIMU.m | 23 +++++++++++-------- 1 file changed, 14 insertions(+), 9 deletions(-) diff --git a/matlab/unstable_examples/FlightCameraTransformIMU.m b/matlab/unstable_examples/FlightCameraTransformIMU.m index b0b37ee33..d030a590b 100644 --- a/matlab/unstable_examples/FlightCameraTransformIMU.m +++ b/matlab/unstable_examples/FlightCameraTransformIMU.m @@ -31,6 +31,12 @@ IMU_metadata.AccelerometerBiasSigma = 1e-6; IMU_metadata.GyroscopeBiasSigma = 1e-6; IMU_metadata.IntegrationSigma = 1e-1; +n_gravity = [0;0;-9.8]; +IMU_params = PreintegrationParams(n_gravity); +IMU_params.setAccelerometerCovariance(IMU_metadata.AccelerometerSigma.^2 * eye(3)); +IMU_params.setGyroscopeCovariance(IMU_metadata.GyroscopeSigma.^2 * eye(3)); +IMU_params.setIntegrationCovariance(IMU_metadata.IntegrationSigma.^2 * eye(3)); + transformKey = 1000; calibrationKey = 2000; @@ -52,7 +58,7 @@ K = Cal3_S2(20,1280,960); % initialize K incorrectly K_corrupt = Cal3_S2(K.fx()+10,K.fy()+10,0,K.px(),K.py()); -isamParams = gtsam.ISAM2Params; +isamParams = ISAM2Params; isamParams.setFactorization('QR'); isam = ISAM2(isamParams); @@ -63,7 +69,6 @@ currentBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1)); sigma_init_v = noiseModel.Isotropic.Sigma(3, 1.0); sigma_init_b = noiseModel.Isotropic.Sigmas([ 0.100; 0.100; 0.100; 5.00e-05; 5.00e-05; 5.00e-05 ]); sigma_between_b = [ IMU_metadata.AccelerometerBiasSigma * ones(3,1); IMU_metadata.GyroscopeBiasSigma * ones(3,1) ]; -g = [0;0;-9.8]; w_coriolis = [0;0;0]; %% generate trajectory and landmarks @@ -181,12 +186,14 @@ for i=1:size(trajectory)-1 zKey = measurementKeys.at(zz); lKey = symbol('l',symbolIndex(zKey)); - fg.add(TransformCalProjectionFactorCal3_S2(measurements.atPoint2(zKey), ... + fg.add(ProjectionFactorPPPCCal3_S2(measurements.atPoint2(zKey), ... z_cov, xKey, transformKey, lKey, calibrationKey, false, true)); % only add landmark to values if doesn't exist yet if ~result.exists(lKey) - noisy_landmark = landmarks.atPoint3(lKey).compose(Point3(normrnd(0,landmark_noise,3,1))); + p = landmarks.atPoint3(lKey); + n = normrnd(0,landmark_noise,3,1); + noisy_landmark = Point3(p.x()+n(1),p.y()+n(2),p.z()+n(3)); initial.insert(lKey, noisy_landmark); % and add a prior since its position is known @@ -211,11 +218,9 @@ for i=1:size(trajectory)-1 % [ currentIMUPoseGlobal, currentVelocityGlobal ] = imuSimulator.integrateTrajectory( ... % currentIMUPoseGlobal, omega, velocity, velocity, deltaT); - currentSummarizedMeasurement = gtsam.ImuFactorPreintegratedMeasurements( ... - currentBias, IMU_metadata.AccelerometerSigma.^2 * eye(3), ... - IMU_metadata.GyroscopeSigma.^2 * eye(3), IMU_metadata.IntegrationSigma.^2 * eye(3)); + currentSummarizedMeasurement = PreintegratedImuMeasurements(IMU_params,currentBias); - accMeas = acc_omega(1:3)-g; + accMeas = acc_omega(1:3)-n_gravity; omegaMeas = acc_omega(4:6); currentSummarizedMeasurement.integrateMeasurement(accMeas, omegaMeas, deltaT); @@ -223,7 +228,7 @@ for i=1:size(trajectory)-1 fg.add(ImuFactor( ... xKey_prev, currentVelKey-1, ... xKey, currentVelKey, ... - currentBiasKey, currentSummarizedMeasurement, g, w_coriolis)); + currentBiasKey, currentSummarizedMeasurement)); % Bias evolution as given in the IMU metadata fg.add(BetweenFactorConstantBias(currentBiasKey-1, currentBiasKey, imuBias.ConstantBias(zeros(3,1), zeros(3,1)), ... From 2c0c3d195558375632fa86ed42df772fce7af42b Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 10 Apr 2016 10:56:36 -0700 Subject: [PATCH 89/90] Added setters and getters for MATLAB wrapper --- gtsam.h | 24 +++++++++- gtsam/navigation/PreintegratedRotation.h | 56 ++++++++++++++---------- gtsam/navigation/PreintegrationParams.h | 12 ++++- 3 files changed, 65 insertions(+), 27 deletions(-) diff --git a/gtsam.h b/gtsam.h index c5424ef80..03963c773 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2489,10 +2489,30 @@ class NavState { gtsam::Pose3 pose() const; }; +#include +virtual class PreintegratedRotationParams { + PreintegratedRotationParams(); + void setGyroscopeCovariance(Matrix cov); + void setOmegaCoriolis(Vector omega); + void setBodyPSensor(const gtsam::Pose3& pose); + + Matrix getGyroscopeCovariance() const; + + // TODO(frank): allow optional + // boost::optional getOmegaCoriolis() const; + // boost::optional getBodyPSensor() const; +}; + #include -class PreintegrationParams { +virtual class PreintegrationParams : gtsam::PreintegratedRotationParams { PreintegrationParams(Vector n_gravity); - // TODO(frank): add setters/getters or make this MATLAB wrapper feature + void setAccelerometerCovariance(Matrix cov); + void setIntegrationCovariance(Matrix cov); + void setUse2ndOrderCoriolis(bool flag); + + Matrix getAccelerometerCovariance() const; + Matrix getIntegrationCovariance() const; + bool getUse2ndOrderCoriolis() const; }; #include diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h index b170ea863..88d9c6437 100644 --- a/gtsam/navigation/PreintegratedRotation.h +++ b/gtsam/navigation/PreintegratedRotation.h @@ -26,6 +26,38 @@ namespace gtsam { +/// Parameters for pre-integration: +/// Usage: Create just a single Params and pass a shared pointer to the constructor +struct PreintegratedRotationParams { + Matrix3 gyroscopeCovariance; ///< continuous-time "Covariance" of gyroscope measurements + boost::optional omegaCoriolis; ///< Coriolis constant + boost::optional body_P_sensor; ///< The pose of the sensor in the body frame + + PreintegratedRotationParams() : gyroscopeCovariance(I_3x3) {} + + virtual void print(const std::string& s) const; + virtual bool equals(const PreintegratedRotationParams& other, double tol=1e-9) const; + + void setGyroscopeCovariance(const Matrix3& cov) { gyroscopeCovariance = cov; } + void setOmegaCoriolis(const Vector3& omega) { omegaCoriolis.reset(omega); } + void setBodyPSensor(const Pose3& pose) { body_P_sensor.reset(pose); } + + const Matrix3& getGyroscopeCovariance() const { return gyroscopeCovariance; } + boost::optional getOmegaCoriolis() const { return omegaCoriolis; } + boost::optional getBodyPSensor() const { return body_P_sensor; } + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + namespace bs = ::boost::serialization; + ar & bs::make_nvp("gyroscopeCovariance", bs::make_array(gyroscopeCovariance.data(), gyroscopeCovariance.size())); + ar & BOOST_SERIALIZATION_NVP(omegaCoriolis); + ar & BOOST_SERIALIZATION_NVP(body_P_sensor); + } +}; + /** * PreintegratedRotation is the base class for all PreintegratedMeasurements * classes (in AHRSFactor, ImuFactor, and CombinedImuFactor). @@ -33,29 +65,7 @@ namespace gtsam { */ class PreintegratedRotation { public: - /// Parameters for pre-integration: - /// Usage: Create just a single Params and pass a shared pointer to the constructor - struct Params { - Matrix3 gyroscopeCovariance; ///< continuous-time "Covariance" of gyroscope measurements - boost::optional omegaCoriolis; ///< Coriolis constant - boost::optional body_P_sensor; ///< The pose of the sensor in the body frame - - Params() : gyroscopeCovariance(I_3x3) {} - - virtual void print(const std::string& s) const; - virtual bool equals(const Params& other, double tol=1e-9) const; - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - namespace bs = ::boost::serialization; - ar & bs::make_nvp("gyroscopeCovariance", bs::make_array(gyroscopeCovariance.data(), gyroscopeCovariance.size())); - ar & BOOST_SERIALIZATION_NVP(omegaCoriolis); - ar & BOOST_SERIALIZATION_NVP(body_P_sensor); - } - }; + typedef PreintegratedRotationParams Params; protected: /// Parameters diff --git a/gtsam/navigation/PreintegrationParams.h b/gtsam/navigation/PreintegrationParams.h index f68f711f1..f2b2c0fef 100644 --- a/gtsam/navigation/PreintegrationParams.h +++ b/gtsam/navigation/PreintegrationParams.h @@ -23,7 +23,7 @@ namespace gtsam { /// Parameters for pre-integration: /// Usage: Create just a single Params and pass a shared pointer to the constructor -struct PreintegrationParams: PreintegratedRotation::Params { +struct PreintegrationParams: PreintegratedRotationParams { Matrix3 accelerometerCovariance; ///< continuous-time "Covariance" of accelerometer Matrix3 integrationCovariance; ///< continuous-time "Covariance" describing integration uncertainty bool use2ndOrderCoriolis; ///< Whether to use second order Coriolis integration @@ -50,6 +50,14 @@ struct PreintegrationParams: PreintegratedRotation::Params { void print(const std::string& s) const; bool equals(const PreintegratedRotation::Params& other, double tol) const; + void setAccelerometerCovariance(const Matrix3& cov) { accelerometerCovariance = cov; } + void setIntegrationCovariance(const Matrix3& cov) { integrationCovariance = cov; } + void setUse2ndOrderCoriolis(bool flag) { use2ndOrderCoriolis = flag; } + + const Matrix3& getAccelerometerCovariance() const { return accelerometerCovariance; } + const Matrix3& getIntegrationCovariance() const { return integrationCovariance; } + bool getUse2ndOrderCoriolis() const { return use2ndOrderCoriolis; } + protected: /// Default constructor for serialization only: uninitialized! PreintegrationParams() {} @@ -60,7 +68,7 @@ protected: void serialize(ARCHIVE & ar, const unsigned int /*version*/) { namespace bs = ::boost::serialization; ar & boost::serialization::make_nvp("PreintegratedRotation_Params", - boost::serialization::base_object(*this)); + boost::serialization::base_object(*this)); ar & bs::make_nvp("accelerometerCovariance", bs::make_array(accelerometerCovariance.data(), accelerometerCovariance.size())); ar & bs::make_nvp("integrationCovariance", bs::make_array(integrationCovariance.data(), integrationCovariance.size())); ar & BOOST_SERIALIZATION_NVP(use2ndOrderCoriolis); From 69c182d5a4a9a60bd301a5785bb3e4c1504b90d3 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 10 Apr 2016 11:12:46 -0700 Subject: [PATCH 90/90] Some comments, formatting --- .../FlightCameraTransformIMU.m | 23 ++++++++++--------- 1 file changed, 12 insertions(+), 11 deletions(-) diff --git a/matlab/unstable_examples/FlightCameraTransformIMU.m b/matlab/unstable_examples/FlightCameraTransformIMU.m index d030a590b..9a8a27344 100644 --- a/matlab/unstable_examples/FlightCameraTransformIMU.m +++ b/matlab/unstable_examples/FlightCameraTransformIMU.m @@ -1,3 +1,7 @@ +% Simulation for concurrent IMU, camera, IMU-camera transform estimation during flight with known landmarks +% author: Chris Beall +% date: July 2014 + clear all; clf; @@ -24,7 +28,7 @@ if(write_video) open(videoObj); end -% IMU parameters +%% IMU parameters IMU_metadata.AccelerometerSigma = 1e-2; IMU_metadata.GyroscopeSigma = 1e-2; IMU_metadata.AccelerometerBiasSigma = 1e-6; @@ -104,7 +108,9 @@ zlabel('z'); title('Estimated vs. actual IMU-cam transform'); axis equal; +%% Main loop for i=1:size(trajectory)-1 + %% Preliminaries xKey = symbol('x',i); pose = trajectory.atPose3(xKey); % GT pose pose_t = pose.translation(); % GT pose-translation @@ -139,6 +145,7 @@ for i=1:size(trajectory)-1 gps_pose = pose.retract([0; 0; 0; normrnd(0,gps_noise,3,1)]); fg.add(PoseTranslationPrior3D(xKey, gps_pose, GPS_trans_cov)); + %% First-time initialization if i==1 % camera transform if use_camera_transform_noise @@ -158,12 +165,10 @@ for i=1:size(trajectory)-1 result = initial; end - % priors on first two poses + %% priors on first two poses if i < 3 -% fg.add(PriorFactorLieVector(currentVelKey, currentVelocityGlobal, sigma_init_v)); + % fg.add(PriorFactorLieVector(currentVelKey, currentVelocityGlobal, sigma_init_v)); fg.add(PriorFactorConstantBias(currentBiasKey, currentBias, sigma_init_b)); - - end %% the 'normal' case @@ -202,14 +207,12 @@ for i=1:size(trajectory)-1 end end % end landmark observations - %% IMU deltaT = 1; logmap = Pose3.Logmap(step); omega = logmap(1:3); velocity = logmap(4:6); - % Simulate IMU measurements, considering Coriolis effect % (in this simple example we neglect gravity and there are no other forces acting on the body) acc_omega = imuSimulator.calculateIMUMeas_coriolis( ... @@ -234,7 +237,7 @@ for i=1:size(trajectory)-1 fg.add(BetweenFactorConstantBias(currentBiasKey-1, currentBiasKey, imuBias.ConstantBias(zeros(3,1), zeros(3,1)), ... noiseModel.Diagonal.Sigmas(sqrt(10) * sigma_between_b))); - % ISAM update + %% ISAM update isam.update(fg, initial); result = isam.calculateEstimate(); @@ -300,10 +303,8 @@ for i=1:size(trajectory)-1 end -% print out final camera transform +%% print out final camera transform and write video result.at(transformKey); - - if(write_video) close(videoObj); end \ No newline at end of file