From c7298da4fc621fc6c85fd95bddedd4c1702e45d8 Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 19 Jan 2015 20:38:24 +0100 Subject: [PATCH 01/91] Removed all fixed hacks - replaced it with possibly something worse (no reference returned from at) --- gtsam.h | 9 --- gtsam/nonlinear/Values-inl.h | 102 +++++++++++++++++++++++---- gtsam/nonlinear/Values.cpp | 55 +++++---------- gtsam/nonlinear/Values.h | 34 ++++++--- gtsam/nonlinear/tests/testValues.cpp | 15 +++- 5 files changed, 141 insertions(+), 74 deletions(-) diff --git a/gtsam.h b/gtsam.h index 1fbc0f907..8c266b6a9 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1722,9 +1722,6 @@ class Values { void insert(size_t j, Vector t); void insert(size_t j, Matrix t); - // Fixed size version - void insertFixed(size_t j, Vector t, size_t n); - void update(size_t j, const gtsam::Point2& t); void update(size_t j, const gtsam::Point3& t); void update(size_t j, const gtsam::Rot2& t); @@ -1742,12 +1739,6 @@ class Values { template T at(size_t j); - /// Fixed size versions, for n in 1..9 - void insertFixed(size_t j, Vector t, size_t n); - - /// 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; diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h index fe2c3f3ca..96c352833 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -252,23 +252,97 @@ namespace gtsam { return filter(key_value.key); } - /* ************************************************************************* */ - template - const ValueType& Values::at(Key j) const { - // Find the item - KeyValueMap::const_iterator item = values_.find(j); + /* ************************************************************************* */ - // Throw exception if it does not exist - if(item == values_.end()) - throw ValuesKeyDoesNotExist("retrieve", j); + namespace internal { // Check the type and throw exception if incorrect - try { - return dynamic_cast&>(*item->second).value(); - } catch (std::bad_cast &) { - throw ValuesIncorrectType(j, typeid(*item->second), typeid(ValueType)); - } - } + template + struct handle { + ValueType operator()(Key j, const gtsam::Value * const pointer) { + try { + return dynamic_cast&>(*pointer).value(); + } catch (std::bad_cast &) { + throw ValuesIncorrectType(j, typeid(*pointer), typeid(ValueType)); + } + } + }; + + // Handle dynamic vectors + template<> + struct handle > { + Eigen::Matrix operator()(Key j, + const gtsam::Value * const pointer) { + try { + return dynamic_cast >&>(*pointer).value(); + } catch (std::bad_cast &) { + throw ValuesIncorrectType(j, typeid(*pointer), + typeid(Eigen::Matrix)); + } + } + }; + + // Handle dynamic matrices + template + struct handle > { + Eigen::Matrix operator()(Key j, + const gtsam::Value * const pointer) { + try { + return dynamic_cast >&>(*pointer).value(); + } catch (std::bad_cast &) { + throw ValuesIncorrectType(j, typeid(*pointer), + typeid(Eigen::Matrix)); + } + } + }; + + // Request for a fixed vector + template + struct handle > { + Eigen::Matrix operator()(Key j, + const gtsam::Value * const pointer) { + try { + return dynamic_cast >&>(*pointer).value(); + } catch (std::bad_cast &) { + Matrix A = handle()(j, pointer); + if (A.rows() != M || A.cols() != 1) + throw NoMatchFoundForFixed(M, 1, A.rows(), A.cols()); + else + return A; + } + } + }; + + // Request for a fixed matrix + template + struct handle > { + Eigen::Matrix operator()(Key j, + const gtsam::Value * const pointer) { + try { + return dynamic_cast >&>(*pointer).value(); + } catch (std::bad_cast &) { + Matrix A = handle()(j, pointer); + if (A.rows() != M || A.cols() != N) + throw NoMatchFoundForFixed(M, N, A.rows(), A.cols()); + else + return A; + } + } + }; + } // internal + + /* ************************************************************************* */ + template + ValueType Values::at(Key j) const { + // Find the item + KeyValueMap::const_iterator item = values_.find(j); + + // Throw exception if it does not exist + if(item == values_.end()) + throw ValuesKeyDoesNotExist("at", j); + + return internal::handle()(j,item->second); + } /* ************************************************************************* */ template diff --git a/gtsam/nonlinear/Values.cpp b/gtsam/nonlinear/Values.cpp index 372bced8e..ccbb51578 100644 --- a/gtsam/nonlinear/Values.cpp +++ b/gtsam/nonlinear/Values.cpp @@ -25,8 +25,6 @@ #include #include -#include - #include #ifdef __GNUC__ #pragma GCC diagnostic push @@ -38,6 +36,9 @@ #endif #include +#include +#include + using namespace std; namespace gtsam { @@ -112,24 +113,6 @@ namespace gtsam { return result; } - /* ************************************************************************* */ - Vector Values::atFixed(Key j, size_t n) { - switch (n) { - case 1: return at(j); - case 2: return at(j); - case 3: return at(j); - case 4: return at(j); - case 5: return at(j); - case 6: return at(j); - case 7: return at(j); - case 8: return at(j); - case 9: return at(j); - default: - throw runtime_error( - "Values::at fixed size can only handle n in 1..9"); - } - } - /* ************************************************************************* */ const Value& Values::at(Key j) const { // Find the item @@ -148,24 +131,6 @@ namespace gtsam { throw ValuesKeyAlreadyExists(j); } - /* ************************************************************************* */ - void Values::insertFixed(Key j, const Vector& v, size_t n) { - switch (n) { - case 1: insert(j,v); break; - case 2: insert(j,v); break; - case 3: insert(j,v); break; - case 4: insert(j,v); break; - case 5: insert(j,v); break; - case 6: insert(j,v); break; - case 7: insert(j,v); break; - case 8: insert(j,v); break; - case 9: insert(j,v); break; - default: - throw runtime_error( - "Values::insert fixed size can only handle n in 1..9"); - } - } - /* ************************************************************************* */ void Values::insert(const Values& values) { for(const_iterator key_value = values.begin(); key_value != values.end(); ++key_value) { @@ -267,4 +232,18 @@ namespace gtsam { return message_.c_str(); } + /* ************************************************************************* */ + const char* NoMatchFoundForFixed::what() const throw() { + if(message_.empty()) { + ostringstream oss; + oss + << "Attempting to retrieve fixed-size matrix with dimensions " // + << M1_ << "x" << N1_ + << ", but found dynamic Matrix with mismatched dimensions " // + << M2_ << "x" << N2_; + message_ = oss.str(); + } + return message_.c_str(); + } + } diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index 73d0711be..15d3ac9e2 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -175,14 +175,10 @@ namespace gtsam { * @param j Retrieve the value associated with this key * @tparam Value The type of the value stored with this key, this method * throws DynamicValuesIncorrectType if this requested type is not correct. - * @return A const reference to the stored value + * @return The stored value */ template - const ValueType& at(Key j) const; - - /// Special version for small fixed size vectors, for matlab/python - /// throws truntime error if n<1 || n>9 - Vector atFixed(Key j, size_t n); + ValueType at(Key j) const; /// version for double double atDouble(size_t key) const { return at(key);} @@ -264,10 +260,6 @@ namespace gtsam { template void insert(Key j, const ValueType& val); - /// Special version for small fixed size vectors, for matlab/python - /// throws truntime error if n<1 || n>9 - void insertFixed(Key j, const Vector& v, size_t n); - /// version for double void insertDouble(Key j, double c) { insert(j,c); } @@ -505,6 +497,28 @@ namespace gtsam { } }; + /* ************************************************************************* */ + class GTSAM_EXPORT NoMatchFoundForFixed: public std::exception { + + protected: + const size_t M1_, N1_; + const size_t M2_, N2_; + + private: + mutable std::string message_; + + public: + NoMatchFoundForFixed(size_t M1, size_t N1, size_t M2, size_t N2) throw () : + M1_(M1), N1_(N1), M2_(M2), N2_(N2) { + } + + virtual ~NoMatchFoundForFixed() throw () { + } + + virtual const char* what() const throw (); + }; + + /* ************************************************************************* */ /// traits template<> struct traits : public Testable { diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index faa285cd5..ea5c67d9f 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -476,13 +476,22 @@ TEST(Values, Destructors) { } /* ************************************************************************* */ -TEST(Values, FixedSize) { +TEST(Values, FixedVector) { Values values; Vector v(3); v << 5.0, 6.0, 7.0; - values.insertFixed(key1, v, 3); + values.insert(key1, v); Vector3 expected(5.0, 6.0, 7.0); CHECK(assert_equal((Vector)expected, values.at(key1))); - CHECK_EXCEPTION(values.insertFixed(key1, v, 12),runtime_error); + CHECK_EXCEPTION(values.at(key1), exception); +} +/* ************************************************************************* */ +TEST(Values, FixedMatrix) { + Values values; + Matrix v(1,3); v << 5.0, 6.0, 7.0; + values.insert(key1, v); + Vector3 expected(5.0, 6.0, 7.0); + CHECK(assert_equal((Vector)expected, values.at(key1))); + CHECK_EXCEPTION(values.at(key1), exception); } /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } From af32cdb2849e33d2a2b4874b1cc760dfa9c8d618 Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 20 Jan 2015 16:10:29 +0100 Subject: [PATCH 02/91] Added clone --- gtsam/nonlinear/ExpressionFactor.h | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/gtsam/nonlinear/ExpressionFactor.h b/gtsam/nonlinear/ExpressionFactor.h index 4769e5048..9f843129a 100644 --- a/gtsam/nonlinear/ExpressionFactor.h +++ b/gtsam/nonlinear/ExpressionFactor.h @@ -106,6 +106,12 @@ public: return factor; } + + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new ExpressionFactor(*this))); } + }; // ExpressionFactor From 9922c483e349dca863ef6eb6b42529150ec82d34 Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 21 Jan 2015 11:30:57 +0100 Subject: [PATCH 03/91] Use Eigen::format, now compatible with matlab, and stream precision affects printing. --- gtsam/base/Matrix.cpp | 26 +++++++++++--------------- gtsam/linear/JacobianFactor.cpp | 16 ++++++++++++---- 2 files changed, 23 insertions(+), 19 deletions(-) diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp index 7bcd32b9f..e6eef04d5 100644 --- a/gtsam/base/Matrix.cpp +++ b/gtsam/base/Matrix.cpp @@ -181,21 +181,17 @@ void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, SubVec /* ************************************************************************* */ void print(const Matrix& A, const string &s, ostream& stream) { - size_t m = A.rows(), n = A.cols(); - - // print out all elements - stream << s << "[\n"; - for( size_t i = 0 ; i < m ; i++) { - for( size_t j = 0 ; j < n ; j++) { - double aij = A(i,j); - if(aij != 0.0) - stream << setw(12) << setprecision(9) << aij << ",\t"; - else - stream << " 0.0,\t"; - } - stream << endl; - } - stream << "];" << endl; + static const Eigen::IOFormat matlab( + Eigen::StreamPrecision, // precision + 0, // flags + " ", // coeffSeparator + ";\n", // rowSeparator + " ", // rowPrefix + "", // rowSuffix + "[\n", // matPrefix + "\n ]" // matSuffix + ); + cout << s << A.format(matlab) << endl; } /* ************************************************************************* */ diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index eba06f99a..935ed40ae 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -347,13 +347,21 @@ JacobianFactor::JacobianFactor(const GaussianFactorGraph& graph, /* ************************************************************************* */ void JacobianFactor::print(const string& s, const KeyFormatter& formatter) const { + static const Eigen::IOFormat matlab( + Eigen::StreamPrecision, // precision + 0, // flags + " ", // coeffSeparator + ";\n", // rowSeparator + " ", // rowPrefix + "", // rowSuffix + "[\n", // matPrefix + "\n ]" // matSuffix + ); if (!s.empty()) cout << s << "\n"; for (const_iterator key = begin(); key != end(); ++key) { - cout - << formatMatrixIndented( - (boost::format(" A[%1%] = ") % formatter(*key)).str(), getA(key)) - << endl; + cout << boost::format(" A[%1%] = ") % formatter(*key); + cout << getA(key).format(matlab) << endl; } cout << formatMatrixIndented(" b = ", getb(), true) << "\n"; if (model_) From 1816cc3a2ca5a13671ecbf042c5ecc3d69ca790a Mon Sep 17 00:00:00 2001 From: Jing Dong Date: Wed, 21 Jan 2015 14:05:27 -0500 Subject: [PATCH 04/91] comment unused expression to avoid conflicts --- gtsam/nonlinear/expressions.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/nonlinear/expressions.h b/gtsam/nonlinear/expressions.h index a549517e5..3e4664158 100644 --- a/gtsam/nonlinear/expressions.h +++ b/gtsam/nonlinear/expressions.h @@ -18,7 +18,7 @@ Expression between(const Expression& t1, const Expression& t2) { return Expression(traits::Between, t1, t2); } -typedef Expression double_; +//typedef Expression double_; typedef Expression Vector3_; } // \namespace gtsam From 743d145c9b92a68c729b9978af280dbb1d43e0c6 Mon Sep 17 00:00:00 2001 From: Jing Dong Date: Wed, 21 Jan 2015 14:21:19 -0500 Subject: [PATCH 05/91] change double expression name to Double_ --- gtsam/nonlinear/expressions.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/nonlinear/expressions.h b/gtsam/nonlinear/expressions.h index 3e4664158..c496834d4 100644 --- a/gtsam/nonlinear/expressions.h +++ b/gtsam/nonlinear/expressions.h @@ -18,7 +18,7 @@ Expression between(const Expression& t1, const Expression& t2) { return Expression(traits::Between, t1, t2); } -//typedef Expression double_; +typedef Expression Double_; typedef Expression Vector3_; } // \namespace gtsam From 14cc64673a9fe8b5ff67e2ea8e179bc202d4ab51 Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 21 Jan 2015 21:17:45 +0100 Subject: [PATCH 06/91] Mixing BAL and expressions --- .cproject | 106 ++++++++++++---------- examples/SFMExampleExpressions_bal.cpp | 117 +++++++++++++++++++++++++ gtsam/slam/expressions.h | 11 +++ 3 files changed, 190 insertions(+), 44 deletions(-) create mode 100644 examples/SFMExampleExpressions_bal.cpp diff --git a/.cproject b/.cproject index 8a2eaaf1c..e6af94066 100644 --- a/.cproject +++ b/.cproject @@ -584,6 +584,7 @@ make + tests/testBayesTree.run true false @@ -591,6 +592,7 @@ make + testBinaryBayesNet.run true false @@ -638,6 +640,7 @@ make + testSymbolicBayesNet.run true false @@ -645,6 +648,7 @@ make + tests/testSymbolicFactor.run true false @@ -652,6 +656,7 @@ make + testSymbolicFactorGraph.run true false @@ -667,6 +672,7 @@ make + tests/testBayesTree true false @@ -1098,6 +1104,7 @@ make + testErrors.run true false @@ -1327,6 +1334,46 @@ true true + + make + -j5 + testBTree.run + true + true + true + + + make + -j5 + testDSF.run + true + true + true + + + make + -j5 + testDSFMap.run + true + true + true + + + make + -j5 + testDSFVector.run + true + true + true + + + make + -j5 + testFixedVector.run + true + true + true + make -j2 @@ -1409,7 +1456,6 @@ make - testSimulated2DOriented.run true false @@ -1449,7 +1495,6 @@ make - testSimulated2D.run true false @@ -1457,7 +1502,6 @@ make - testSimulated3D.run true false @@ -1471,46 +1515,6 @@ true true - - make - -j5 - testBTree.run - true - true - true - - - make - -j5 - testDSF.run - true - true - true - - - make - -j5 - testDSFMap.run - true - true - true - - - make - -j5 - testDSFVector.run - true - true - true - - - make - -j5 - testFixedVector.run - true - true - true - make -j5 @@ -1776,6 +1780,7 @@ cpack + -G DEB true false @@ -1783,6 +1788,7 @@ cpack + -G RPM true false @@ -1790,6 +1796,7 @@ cpack + -G TGZ true false @@ -1797,6 +1804,7 @@ cpack + --config CPackSourceConfig.cmake true false @@ -2675,6 +2683,7 @@ make + testGraph.run true false @@ -2682,6 +2691,7 @@ make + testJunctionTree.run true false @@ -2689,6 +2699,7 @@ make + testSymbolicBayesNetB.run true false @@ -3118,6 +3129,14 @@ true true + + make + -j4 + SFMExampleExpressions_bal.run + true + true + true + make -j5 @@ -3288,7 +3307,6 @@ make - tests/testGaussianISAM2 true false diff --git a/examples/SFMExampleExpressions_bal.cpp b/examples/SFMExampleExpressions_bal.cpp new file mode 100644 index 000000000..7c79deb2a --- /dev/null +++ b/examples/SFMExampleExpressions_bal.cpp @@ -0,0 +1,117 @@ +/* ---------------------------------------------------------------------------- + + * 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 SFMExampleExpressions_bal.cpp + * @brief A structure-from-motion example done with Expressions + * @author Frank Dellaert + * @date January 2015 + */ + +/** + * This is the Expression version of SFMExample + * See detailed description of headers there, this focuses on explaining the AD part + */ + +// The two new headers that allow using our Automatic Differentiation Expression framework +#include +#include + +// Header order is close to far +#include +#include +#include +#include +#include +#include // for loading BAL datasets ! + +#include + +using namespace std; +using namespace gtsam; +using namespace noiseModel; +using symbol_shorthand::C; +using symbol_shorthand::P; + +// An SfM_Camera is defined in datase.h as a camera with unknown Cal3Bundler calibration +// and has a total of 9 free parameters + +/* ************************************************************************* */ +int main (int argc, char* argv[]) { + + // Find default file, but if an argument is given, try loading a file + string filename = findExampleDataFile("dubrovnik-3-7-pre"); + if (argc>1) filename = string(argv[1]); + + // Load the SfM data from file + SfM_data mydata; + assert(readBAL(filename, mydata)); + cout << boost::format("read %1% tracks on %2% cameras\n") % mydata.number_tracks() % mydata.number_cameras(); + + // Create a factor graph + ExpressionFactorGraph graph; + + // Here we don't use a PriorFactor but directly the ExpressionFactor class + // First, we create an expression to the pose from the first camera + Expression camera0_(C(0)); + // Then, to get its pose: + Pose3_ pose0_(&SfM_Camera::pose,camera0_); + // Finally, we say it should be equal to first guess + graph.addExpressionFactor(pose0_, mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1)); + + // similarly, we create a prior on the first point + Point3_ point0_(P(0)); + graph.addExpressionFactor(point0_, mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1)); + + // We share *one* noiseModel between all projection factors + noiseModel::Isotropic::shared_ptr noise = + noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v + + // Simulated measurements from each camera pose, adding them to the factor graph + size_t j = 0; + BOOST_FOREACH(const SfM_Track& track, mydata.tracks) { + // Leaf expression for j^th point + Point3_ point_('p', j); + BOOST_FOREACH(const SfM_Measurement& m, track.measurements) { + size_t i = m.first; + Point2 uv = m.second; + // Leaf expression for i^th camera + Expression camera_(C(i)); + // Below an expression for the prediction of the measurement: + Point2_ predict_ = project2(camera_,point_); + // Again, here we use an ExpressionFactor + graph.addExpressionFactor(predict_, uv, noise); + } + j += 1; + } + + // Create initial estimate + Values initial; + size_t i = 0; j = 0; + BOOST_FOREACH(const SfM_Camera& camera, mydata.cameras) initial.insert(C(i++), camera); + BOOST_FOREACH(const SfM_Track& track, mydata.tracks) initial.insert(P(j++), track.p); + + /* Optimize the graph and print results */ + Values result; + try { + LevenbergMarquardtParams params; + params.setVerbosity("ERROR"); + LevenbergMarquardtOptimizer lm(graph, initial, params); + result = lm.optimize(); + } catch (exception& e) { + cout << e.what(); + } + cout << "final error: " << graph.error(result) << endl; + + return 0; +} +/* ************************************************************************* */ + diff --git a/gtsam/slam/expressions.h b/gtsam/slam/expressions.h index 1d9ddd6d4..694782c16 100644 --- a/gtsam/slam/expressions.h +++ b/gtsam/slam/expressions.h @@ -41,6 +41,17 @@ Point2_ project(const Point3_& p_cam) { return Point2_(PinholeCamera::project_to_camera, p_cam); } +template +Point2 project4(const CAMERA& camera, const Point3& p, + OptionalJacobian<2, CAMERA::dimension> Dcam, OptionalJacobian<2, 3> Dpoint) { + return camera.project2(p, Dcam, Dpoint); +} + +template +Point2_ project2(const Expression& camera_, const Point3_& p_) { + return Point2_(project4, camera_, p_); +} + Point2 project6(const Pose3& x, const Point3& p, const Cal3_S2& K, OptionalJacobian<2, 6> Dpose, OptionalJacobian<2, 3> Dpoint, OptionalJacobian<2, 5> Dcal) { return PinholeCamera(x, K).project(p, Dpose, Dpoint, Dcal); From 21e6154a243b22cff71fa0ef5c529f7cd22d1656 Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 22 Jan 2015 00:20:23 +0100 Subject: [PATCH 07/91] Fixed test --- gtsam/geometry/tests/testPinholeCamera.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/geometry/tests/testPinholeCamera.cpp b/gtsam/geometry/tests/testPinholeCamera.cpp index fb186259a..20f7a3231 100644 --- a/gtsam/geometry/tests/testPinholeCamera.cpp +++ b/gtsam/geometry/tests/testPinholeCamera.cpp @@ -59,12 +59,12 @@ TEST( PinholeCamera, constructor) TEST(PinholeCamera, Pose) { Matrix actualH; - EXPECT(assert_equal(pose, camera.pose(actualH))); + EXPECT(assert_equal(pose, camera.getPose(actualH))); // Check derivative boost::function f = // - boost::bind(&Camera::pose,_1,boost::none); - Matrix numericalH = numericalDerivative11(&Camera::getPose,camera); + boost::bind(&Camera::getPose,_1,boost::none); + Matrix numericalH = numericalDerivative11(f,camera); EXPECT(assert_equal(numericalH, actualH, 1e-9)); } From 71ae1e5c0a01c2ff3d87d453d931f904435ef6ce Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 22 Jan 2015 00:40:52 +0100 Subject: [PATCH 08/91] Cal3Bundler_ --- gtsam/slam/expressions.h | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsam/slam/expressions.h b/gtsam/slam/expressions.h index 694782c16..7a731d29d 100644 --- a/gtsam/slam/expressions.h +++ b/gtsam/slam/expressions.h @@ -36,6 +36,7 @@ Point3_ transform_to(const Pose3_& x, const Point3_& p) { // Projection typedef Expression Cal3_S2_; +typedef Expression Cal3Bundler_; Point2_ project(const Point3_& p_cam) { return Point2_(PinholeCamera::project_to_camera, p_cam); From 2bcc86e2ac2015af45ae2e61da95a3b2848024ac Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 22 Jan 2015 00:41:02 +0100 Subject: [PATCH 09/91] Create, with derivatives --- gtsam/geometry/PinholeCamera.h | 14 ++++++++++++++ gtsam/geometry/tests/testPinholeCamera.cpp | 15 +++++++++++++++ 2 files changed, 29 insertions(+) diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index 46df47ecb..3b4858a4a 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -108,6 +108,20 @@ public: return PinholeCamera(pose3, K); } + // Create PinholeCamera, with derivatives + static PinholeCamera Create(const Pose3& pose, const Calibration &K, + OptionalJacobian H1 = boost::none, // + OptionalJacobian H2 = boost::none) { + typedef Eigen::Matrix MatrixK6; + if (H1) + *H1 << I_6x6, MatrixK6::Zero(); + typedef Eigen::Matrix Matrix6K; + typedef Eigen::Matrix MatrixK; + if (H2) + *H2 << Matrix6K::Zero(), MatrixK::Identity(); + return PinholeCamera(pose,K); + } + /// @} /// @name Advanced Constructors /// @{ diff --git a/gtsam/geometry/tests/testPinholeCamera.cpp b/gtsam/geometry/tests/testPinholeCamera.cpp index 20f7a3231..fd041ef38 100644 --- a/gtsam/geometry/tests/testPinholeCamera.cpp +++ b/gtsam/geometry/tests/testPinholeCamera.cpp @@ -55,6 +55,21 @@ TEST( PinholeCamera, constructor) EXPECT(assert_equal( pose, camera.pose())); } +//****************************************************************************** +TEST(PinholeCamera, Create) { + + Matrix actualH1, actualH2; + EXPECT(assert_equal(camera, Camera::Create(pose,K, actualH1, actualH2))); + + // Check derivative + boost::function f = // + boost::bind(Camera::Create,_1,_2,boost::none,boost::none); + Matrix numericalH1 = numericalDerivative21(f,pose,K); + EXPECT(assert_equal(numericalH1, actualH1, 1e-9)); + Matrix numericalH2 = numericalDerivative22(f,pose,K); + EXPECT(assert_equal(numericalH2, actualH2, 1e-8)); +} + //****************************************************************************** TEST(PinholeCamera, Pose) { From 10f086014b5bbc62e17f46db3ff9464037f2ce1b Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 22 Jan 2015 00:45:07 +0100 Subject: [PATCH 10/91] Include --- gtsam/slam/expressions.h | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsam/slam/expressions.h b/gtsam/slam/expressions.h index 7a731d29d..6b7ffdf12 100644 --- a/gtsam/slam/expressions.h +++ b/gtsam/slam/expressions.h @@ -9,6 +9,7 @@ #include #include +#include #include namespace gtsam { From 7430d0484b04d42f5884a50235566a4c68d0ea7a Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 22 Jan 2015 02:10:17 +0100 Subject: [PATCH 11/91] Faster version if no marginals --- matlab/+gtsam/plot3DPoints.m | 28 +++++++++++++++++----------- 1 file changed, 17 insertions(+), 11 deletions(-) diff --git a/matlab/+gtsam/plot3DPoints.m b/matlab/+gtsam/plot3DPoints.m index 46e161807..20931499b 100644 --- a/matlab/+gtsam/plot3DPoints.m +++ b/matlab/+gtsam/plot3DPoints.m @@ -7,7 +7,7 @@ function plot3DPoints(values, linespec, marginals) import gtsam.* if ~exist('linespec', 'var') || isempty(linespec) - linespec = 'g'; + linespec = 'g*'; end haveMarginals = exist('marginals', 'var'); keys = KeyVector(values.keys); @@ -15,19 +15,25 @@ keys = KeyVector(values.keys); holdstate = ishold; hold on -% Plot points and covariance matrices -for i = 0:keys.size-1 - key = keys.at(i); - try - p = values.atPoint3(key); - if haveMarginals +if haveMarginals + % Plot points and covariance matrices (slow) + for i = 0:keys.size-1 + key = keys.at(i); + try + p = values.atPoint3(key) P = marginals.marginalCovariance(key); gtsam.plotPoint3(p, linespec, P); - else - gtsam.plotPoint3(p, linespec); + catch + % I guess it's not a Point3 end - catch - % I guess it's not a Point3 + end +else + % Extract all in C++ and plot all at once (fast) + P = utilities.extractPoint3(values); + if size(linespec,2)==1 + plot3(P(:,1),P(:,2),P(:,3),[linespec '*']); + else + plot3(P(:,1),P(:,2),P(:,3),linespec); end end From 6239881746e756810b383323502defba1c8428cb Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 22 Jan 2015 03:02:53 +0100 Subject: [PATCH 12/91] Parse successful --- wrap/Template.h | 44 +++++++++++++++++++++++++++++++++++-- wrap/tests/testTemplate.cpp | 9 ++++++++ 2 files changed, 51 insertions(+), 2 deletions(-) diff --git a/wrap/Template.h b/wrap/Template.h index 5a64412ed..e04ea1792 100644 --- a/wrap/Template.h +++ b/wrap/Template.h @@ -26,6 +26,7 @@ namespace wrap { class Template { std::string argName_; std::vector argValues_; + std::vector intList_; friend struct TemplateGrammar; public: /// The only way to get values into a Template is via our friendly Grammar @@ -38,6 +39,9 @@ public: const std::string& argName() const { return argName_; } + const std::vector& intList() const { + return intList_; + } const std::vector& argValues() const { return argValues_; } @@ -53,16 +57,52 @@ public: }; +/* ************************************************************************* */ +// http://boost-spirit.com/distrib/spirit_1_8_2/libs/spirit/doc/grammar.html +struct IntListGrammar: public classic::grammar { + + typedef std::vector IntList; + IntList& result_; ///< successful parse will be placed in here + + /// Construct type grammar and specify where result is placed + IntListGrammar(IntList& result) : + result_(result) { + } + + /// Definition of type grammar + template + struct definition { + + classic::rule integer_p, intList_p; + + definition(IntListGrammar const& self) { + using namespace classic; + + integer_p = int_p[push_back_a(self.result_)]; + + intList_p = '{' >> !integer_p >> *(',' >> integer_p) >> '}'; + } + + classic::rule const& start() const { + return intList_p; + } + + }; +}; +// IntListGrammar + /* ************************************************************************* */ // http://boost-spirit.com/distrib/spirit_1_8_2/libs/spirit/doc/grammar.html struct TemplateGrammar: public classic::grammar { Template& result_; ///< successful parse will be placed in here TypeListGrammar<'{', '}'> argValues_g; ///< TypeList parser + IntListGrammar intList_g; ///< TypeList parser /// Construct type grammar and specify where result is placed TemplateGrammar(Template& result) : - result_(result), argValues_g(result.argValues_) { + result_(result), argValues_g(result.argValues_), // + intList_g(result.intList_) { } /// Definition of type grammar @@ -76,7 +116,7 @@ struct TemplateGrammar: public classic::grammar { using classic::assign_a; templateArgValues_p = (str_p("template") >> '<' >> (BasicRules::name_p)[assign_a(self.result_.argName_)] - >> '=' >> self.argValues_g >> '>'); + >> '=' >> (self.argValues_g | self.intList_g) >> '>'); } classic::rule const& start() const { diff --git a/wrap/tests/testTemplate.cpp b/wrap/tests/testTemplate.cpp index 37cb95205..eed144677 100644 --- a/wrap/tests/testTemplate.cpp +++ b/wrap/tests/testTemplate.cpp @@ -47,6 +47,15 @@ TEST( Template, grammar ) { EXPECT(actual[2]==Qualified("Vector",Qualified::EIGEN)); EXPECT(actual[3]==Qualified("Matrix",Qualified::EIGEN)); actual.clear(); + + EXPECT(parse("template", g, space_p).full); + EXPECT_LONGS_EQUAL(4, actual.intList().size()); + EXPECT(actual.argName()=="N"); + EXPECT_LONGS_EQUAL(1,actual.intList()[0]); + EXPECT_LONGS_EQUAL(2,actual.intList()[1]); + EXPECT_LONGS_EQUAL(3,actual.intList()[2]); + EXPECT_LONGS_EQUAL(4,actual.intList()[3]); + actual.clear(); } //****************************************************************************** From 11ab035380c5c8bf8a4ed3decee73900993dc50e Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 22 Jan 2015 03:34:39 +0100 Subject: [PATCH 13/91] Expand with intList --- wrap/Class.cpp | 20 ++++++++++++++++++++ wrap/Class.h | 4 ++++ wrap/tests/testClass.cpp | 23 +++++++++++++++++++++++ 3 files changed, 47 insertions(+) diff --git a/wrap/Class.cpp b/wrap/Class.cpp index 6e415065d..e62e31bc1 100644 --- a/wrap/Class.cpp +++ b/wrap/Class.cpp @@ -28,6 +28,7 @@ #include #include #include +#include #include // std::ostream_iterator //#include // on Linux GCC: fails with error regarding needing C++0x std flags //#include // same failure as above @@ -314,6 +315,25 @@ vector Class::expandTemplate(Str templateArg, return result; } +/* ************************************************************************* */ +vector Class::expandTemplate(Str templateArg, + const vector& integers) const { + vector result; + BOOST_FOREACH(int i, integers) { + Qualified expandedClass = (Qualified) (*this); + stringstream ss; ss << i; + string instName = ss.str(); + expandedClass.expand(instName); + const TemplateSubstitution ts(templateArg, instName, expandedClass); + Class inst = expandTemplate(ts); + inst.name_ = expandedClass.name(); + inst.templateArgs.clear(); + inst.typedefName = qualifiedName("::") + "<" + instName + ">"; + result.push_back(inst); + } + return result; +} + /* ************************************************************************* */ void Class::addMethod(bool verbose, bool is_const, Str methodName, const ArgumentList& argumentList, const ReturnValue& returnValue, diff --git a/wrap/Class.h b/wrap/Class.h index f4c687eca..2f7457f06 100644 --- a/wrap/Class.h +++ b/wrap/Class.h @@ -106,6 +106,10 @@ public: std::vector expandTemplate(Str templateArg, const std::vector& instantiations) const; + // Create new classes with integer template arguments + std::vector expandTemplate(Str templateArg, + const std::vector& integers) const; + /// Add potentially overloaded, potentially templated method void addMethod(bool verbose, bool is_const, Str methodName, const ArgumentList& argumentList, const ReturnValue& returnValue, diff --git a/wrap/tests/testClass.cpp b/wrap/tests/testClass.cpp index a133e15ac..3d8d79b33 100644 --- a/wrap/tests/testClass.cpp +++ b/wrap/tests/testClass.cpp @@ -221,6 +221,29 @@ TEST( Class, TemplateSubstition ) { } +//****************************************************************************** +TEST( Class, TemplateSubstitionIntList ) { + + using classic::space_p; + + // Create type grammar that will place result in cls + Class cls; + Template t; + ClassGrammar g(cls, t); + + string markup(string("template" + "class Point2 {" + " void myMethod(Matrix A) const;" + "};")); + + EXPECT(parse(markup.c_str(), g, space_p).full); + + vector classes = cls.expandTemplate(t.argName(), t.intList()); + + // check the number of new classes is 2 + EXPECT_LONGS_EQUAL(2, classes.size()); +} + //****************************************************************************** TEST(Class, Template) { From 1292d324ff7362ab507ae8f2539cca5de0d0ce52 Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 22 Jan 2015 03:35:08 +0100 Subject: [PATCH 14/91] Now also expand intList --- wrap/Module.cpp | 12 +++++++----- wrap/Template.h | 3 +++ 2 files changed, 10 insertions(+), 5 deletions(-) diff --git a/wrap/Module.cpp b/wrap/Module.cpp index 092c732f7..9d505a525 100644 --- a/wrap/Module.cpp +++ b/wrap/Module.cpp @@ -47,15 +47,17 @@ namespace fs = boost::filesystem; // If a number of template arguments were given, generate a number of expanded // class names, e.g., PriorFactor -> PriorFactorPose2, and add those classes static void handle_possible_template(vector& classes, const Class& cls, - const vector& instantiations) { - if (cls.templateArgs.empty() || instantiations.empty()) { + const Template& t) { + if (cls.templateArgs.empty() || t.empty()) { classes.push_back(cls); } else { if (cls.templateArgs.size() != 1) throw std::runtime_error( "In-line template instantiations only handle a single template argument"); - vector classInstantiations = // - cls.expandTemplate(cls.templateArgs.front(), instantiations); + string arg = cls.templateArgs.front(); + vector classInstantiations = + (t.nrValues() > 0) ? cls.expandTemplate(arg, t.argValues()) : + cls.expandTemplate(arg, t.intList()); BOOST_FOREACH(const Class& c, classInstantiations) classes.push_back(c); } @@ -107,7 +109,7 @@ void Module::parseMarkup(const std::string& data) { Rule class_p = class_g // [assign_a(cls.namespaces_, namespaces)] [bl::bind(&handle_possible_template, bl::var(classes), bl::var(cls), - bl::var(classTemplate.argValues()))] + bl::var(classTemplate))] [clear_a(classTemplate)] // [assign_a(cls,cls0)]; diff --git a/wrap/Template.h b/wrap/Template.h index e04ea1792..c6f6833c8 100644 --- a/wrap/Template.h +++ b/wrap/Template.h @@ -45,6 +45,9 @@ public: const std::vector& argValues() const { return argValues_; } + bool empty() const { + return argValues_.empty() && intList_.empty(); + } size_t nrValues() const { return argValues_.size(); } From cd9d03668f59a7809b8b5503b99dc5aab9112820 Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 22 Jan 2015 03:35:27 +0100 Subject: [PATCH 15/91] Add test --- wrap/tests/geometry.h | 6 ++++++ wrap/tests/testWrap.cpp | 4 ++-- 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/wrap/tests/geometry.h b/wrap/tests/geometry.h index 69bc7e3be..376e39b62 100644 --- a/wrap/tests/geometry.h +++ b/wrap/tests/geometry.h @@ -127,6 +127,12 @@ class MyFactor { // and a typedef specializing it typedef MyFactor MyFactorPosePoint2; +// A class with integer template arguments +template +class MyVector { + MyVector(); +}; + // comments at the end! // even more comments at the end! diff --git a/wrap/tests/testWrap.cpp b/wrap/tests/testWrap.cpp index 1a9c82143..64469c7fd 100644 --- a/wrap/tests/testWrap.cpp +++ b/wrap/tests/testWrap.cpp @@ -148,7 +148,7 @@ TEST( wrap, Small ) { TEST( wrap, Geometry ) { string markup_header_path = topdir + "/wrap/tests"; Module module(markup_header_path.c_str(), "geometry",enable_verbose); - EXPECT_LONGS_EQUAL(7, module.classes.size()); + EXPECT_LONGS_EQUAL(9, module.classes.size()); // forward declarations LONGS_EQUAL(2, module.forward_declarations.size()); @@ -159,7 +159,7 @@ TEST( wrap, Geometry ) { strvec exp_includes; exp_includes += "folder/path/to/Test.h"; EXPECT(assert_equal(exp_includes, module.includes)); - LONGS_EQUAL(7, module.classes.size()); + LONGS_EQUAL(9, module.classes.size()); // Key for ReturnType::return_category // CLASS = 1, From 4c8d2675cfe25bf97003bf74aa0c83ee8cdaf65b Mon Sep 17 00:00:00 2001 From: Jing Dong Date: Thu, 22 Jan 2015 02:20:57 -0500 Subject: [PATCH 16/91] add expressions --- gtsam/nonlinear/expressions.h | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/gtsam/nonlinear/expressions.h b/gtsam/nonlinear/expressions.h index c496834d4..efbf207d4 100644 --- a/gtsam/nonlinear/expressions.h +++ b/gtsam/nonlinear/expressions.h @@ -18,6 +18,11 @@ Expression between(const Expression& t1, const Expression& t2) { return Expression(traits::Between, t1, t2); } +template +Expression compose(const Expression& t1, const Expression& t2) { + return Expression(traits::Compose, t1, t2); +} + typedef Expression Double_; typedef Expression Vector3_; From a35152399f13dfebfd0386acd3d259256b4c7eac Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 22 Jan 2015 12:44:43 +0100 Subject: [PATCH 17/91] Needs to be cleared, fixes problem with two or more int-templated classes --- wrap/Template.h | 1 + 1 file changed, 1 insertion(+) diff --git a/wrap/Template.h b/wrap/Template.h index c6f6833c8..991c6c883 100644 --- a/wrap/Template.h +++ b/wrap/Template.h @@ -35,6 +35,7 @@ public: void clear() { argName_.clear(); argValues_.clear(); + intList_.clear(); } const std::string& argName() const { return argName_; From 920bdcb0884850b28d92ed71a8072ae153e9044a Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 22 Jan 2015 16:28:05 +0100 Subject: [PATCH 18/91] Avoid linking duplicates --- gtsam_unstable/geometry/Event.cpp | 27 +++++++++++++++++++++++++++ gtsam_unstable/geometry/Event.h | 3 --- 2 files changed, 27 insertions(+), 3 deletions(-) create mode 100644 gtsam_unstable/geometry/Event.cpp diff --git a/gtsam_unstable/geometry/Event.cpp b/gtsam_unstable/geometry/Event.cpp new file mode 100644 index 000000000..09cedb512 --- /dev/null +++ b/gtsam_unstable/geometry/Event.cpp @@ -0,0 +1,27 @@ +/* ---------------------------------------------------------------------------- + + * 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 Event + * @brief Space-time event + * @author Frank Dellaert + * @author Jay Chakravarty + * @date December 2014 + */ + +#include + +namespace gtsam { + +const double Event::Speed = 330; +const Matrix14 Event::JacobianZ = (Matrix14() << 0,0,0,1).finished(); + +} //\ namespace gtsam diff --git a/gtsam_unstable/geometry/Event.h b/gtsam_unstable/geometry/Event.h index 9d35331bb..3c622924a 100644 --- a/gtsam_unstable/geometry/Event.h +++ b/gtsam_unstable/geometry/Event.h @@ -99,9 +99,6 @@ public: } }; -const double Event::Speed = 330; -const Matrix14 Event::JacobianZ = (Matrix14() << 0,0,0,1).finished(); - // Define GTSAM traits template<> struct GTSAM_EXPORT traits : internal::Manifold {}; From b871489a6a5aa36fe39994f1bd229cd8ac0ea7fd Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 22 Jan 2015 16:46:46 +0100 Subject: [PATCH 19/91] Side-stepped linking mess --- gtsam_unstable/geometry/Event.cpp | 3 --- gtsam_unstable/geometry/Event.h | 6 ++---- gtsam_unstable/geometry/tests/testEvent.cpp | 6 +++--- 3 files changed, 5 insertions(+), 10 deletions(-) diff --git a/gtsam_unstable/geometry/Event.cpp b/gtsam_unstable/geometry/Event.cpp index 09cedb512..5248241e1 100644 --- a/gtsam_unstable/geometry/Event.cpp +++ b/gtsam_unstable/geometry/Event.cpp @@ -21,7 +21,4 @@ namespace gtsam { -const double Event::Speed = 330; -const Matrix14 Event::JacobianZ = (Matrix14() << 0,0,0,1).finished(); - } //\ namespace gtsam diff --git a/gtsam_unstable/geometry/Event.h b/gtsam_unstable/geometry/Event.h index 3c622924a..dd362c7f4 100644 --- a/gtsam_unstable/geometry/Event.h +++ b/gtsam_unstable/geometry/Event.h @@ -33,10 +33,6 @@ class Event { public: enum { dimension = 4 }; - /// Speed of sound - static const double Speed; - static const Matrix14 JacobianZ; - /// Default Constructor Event() : time_(0) { @@ -57,6 +53,7 @@ public: // TODO we really have to think of a better way to do linear arguments double height(OptionalJacobian<1,4> H = boost::none) const { + static const Matrix14 JacobianZ = (Matrix14() << 0,0,0,1).finished(); if (H) *H = JacobianZ; return location_.z(); } @@ -87,6 +84,7 @@ public: double toa(const Point3& microphone, // OptionalJacobian<1, 4> H1 = boost::none, // OptionalJacobian<1, 3> H2 = boost::none) const { + static const double Speed = 330; Matrix13 D1, D2; double distance = location_.distance(microphone, D1, D2); if (H1) diff --git a/gtsam_unstable/geometry/tests/testEvent.cpp b/gtsam_unstable/geometry/tests/testEvent.cpp index 433ca7e7f..0842e2146 100644 --- a/gtsam_unstable/geometry/tests/testEvent.cpp +++ b/gtsam_unstable/geometry/tests/testEvent.cpp @@ -45,13 +45,13 @@ TEST( Event, Constructor ) { //***************************************************************************** TEST( Event, Toa1 ) { Event event(0, 1, 0, 0); - double expected = 1 / Event::Speed; + double expected = 1. / 330; EXPECT_DOUBLES_EQUAL(expected, event.toa(microphoneAt0), 1e-9); } //***************************************************************************** TEST( Event, Toa2 ) { - double expectedTOA = timeOfEvent + 1 / Event::Speed; + double expectedTOA = timeOfEvent + 1. / 330; EXPECT_DOUBLES_EQUAL(expectedTOA, exampleEvent.toa(microphoneAt0), 1e-9); } @@ -79,7 +79,7 @@ TEST( Event, Expression ) { Values values; values.insert(key, exampleEvent); - double expectedTOA = timeOfEvent + 1 / Event::Speed; + double expectedTOA = timeOfEvent + 1. / 330; EXPECT_DOUBLES_EQUAL(expectedTOA, expression.value(values), 1e-9); } From e84ed155fab947113f6e9220663ffac4c9148e6b Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 23 Jan 2015 20:54:47 +0100 Subject: [PATCH 20/91] Avoid link errors --- gtsam/slam/expressions.h | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/gtsam/slam/expressions.h b/gtsam/slam/expressions.h index 6b7ffdf12..f7314c73f 100644 --- a/gtsam/slam/expressions.h +++ b/gtsam/slam/expressions.h @@ -20,7 +20,7 @@ typedef Expression Point2_; typedef Expression Rot2_; typedef Expression Pose2_; -Point2_ transform_to(const Pose2_& x, const Point2_& p) { +inline Point2_ transform_to(const Pose2_& x, const Point2_& p) { return Point2_(x, &Pose2::transform_to, p); } @@ -30,7 +30,7 @@ typedef Expression Point3_; typedef Expression Rot3_; typedef Expression Pose3_; -Point3_ transform_to(const Pose3_& x, const Point3_& p) { +inline Point3_ transform_to(const Pose3_& x, const Point3_& p) { return Point3_(x, &Pose3::transform_to, p); } @@ -39,7 +39,7 @@ Point3_ transform_to(const Pose3_& x, const Point3_& p) { typedef Expression Cal3_S2_; typedef Expression Cal3Bundler_; -Point2_ project(const Point3_& p_cam) { +inline Point2_ project(const Point3_& p_cam) { return Point2_(PinholeCamera::project_to_camera, p_cam); } @@ -54,12 +54,12 @@ Point2_ project2(const Expression& camera_, const Point3_& p_) { return Point2_(project4, camera_, p_); } -Point2 project6(const Pose3& x, const Point3& p, const Cal3_S2& K, +inline Point2 project6(const Pose3& x, const Point3& p, const Cal3_S2& K, OptionalJacobian<2, 6> Dpose, OptionalJacobian<2, 3> Dpoint, OptionalJacobian<2, 5> Dcal) { return PinholeCamera(x, K).project(p, Dpose, Dpoint, Dcal); } -Point2_ project3(const Pose3_& x, const Point3_& p, const Cal3_S2_& K) { +inline Point2_ project3(const Pose3_& x, const Point3_& p, const Cal3_S2_& K) { return Point2_(project6, x, p, K); } From fad11c0f4b570aad4fd8db38b16023dd5c197a52 Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 18 Feb 2015 08:20:09 +0100 Subject: [PATCH 21/91] Cleaned up (obsolete?) CG-based solver by Yong-Dian --- .../NonlinearConjugateGradientOptimizer.cpp | 48 ++++-- .../NonlinearConjugateGradientOptimizer.h | 163 +++++++++++------- tests/testGradientDescentOptimizer.cpp | 49 ++---- 3 files changed, 153 insertions(+), 107 deletions(-) diff --git a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp index 107ec7d3f..6d7196ff5 100644 --- a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp @@ -1,7 +1,18 @@ +/* ---------------------------------------------------------------------------- + + * 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 GradientDescentOptimizer.cpp - * @brief - * @author ydjian + * @file NonlinearConjugateGradientOptimizer.cpp + * @brief Simple non-linear optimizer that solves using *non-preconditioned* CG + * @author Yong-Dian Jian * @date Jun 11, 2012 */ @@ -16,36 +27,49 @@ using namespace std; namespace gtsam { -/* Return the gradient vector of a nonlinear factor given a linearization point and a variable ordering - * Can be moved to NonlinearFactorGraph.h if desired */ -VectorValues gradientInPlace(const NonlinearFactorGraph &nfg, const Values &values) { +/** + * @brief Return the gradient vector of a nonlinear factor graph + * @param nfg the graph + * @param values a linearization point + * Can be moved to NonlinearFactorGraph.h if desired + */ +static VectorValues gradientInPlace(const NonlinearFactorGraph &nfg, + const Values &values) { // Linearize graph GaussianFactorGraph::shared_ptr linear = nfg.linearize(values); return linear->gradientAtZero(); } -double NonlinearConjugateGradientOptimizer::System::error(const State &state) const { +double NonlinearConjugateGradientOptimizer::System::error( + const State &state) const { return graph_.error(state); } -NonlinearConjugateGradientOptimizer::System::Gradient NonlinearConjugateGradientOptimizer::System::gradient(const State &state) const { +NonlinearConjugateGradientOptimizer::System::Gradient NonlinearConjugateGradientOptimizer::System::gradient( + const State &state) const { return gradientInPlace(graph_, state); } -NonlinearConjugateGradientOptimizer::System::State NonlinearConjugateGradientOptimizer::System::advance(const State ¤t, const double alpha, const Gradient &g) const { + +NonlinearConjugateGradientOptimizer::System::State NonlinearConjugateGradientOptimizer::System::advance( + const State ¤t, const double alpha, const Gradient &g) const { Gradient step = g; step *= alpha; return current.retract(step); } void NonlinearConjugateGradientOptimizer::iterate() { - int dummy ; - boost::tie(state_.values, dummy) = nonlinearConjugateGradient(System(graph_), state_.values, params_, true /* single iterations */); + int dummy; + boost::tie(state_.values, dummy) = nonlinearConjugateGradient( + System(graph_), state_.values, params_, true /* single iterations */); ++state_.iterations; state_.error = graph_.error(state_.values); } const Values& NonlinearConjugateGradientOptimizer::optimize() { - boost::tie(state_.values, state_.iterations) = nonlinearConjugateGradient(System(graph_), state_.values, params_, false /* up to convergent */); + // Optimize until convergence + System system(graph_); + boost::tie(state_.values, state_.iterations) = // + nonlinearConjugateGradient(system, state_.values, params_, false); state_.error = graph_.error(state_.values); return state_.values; } diff --git a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h index 1ad8fa2ab..9c4db5fd9 100644 --- a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h +++ b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h @@ -1,8 +1,19 @@ +/* ---------------------------------------------------------------------------- + + * 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 GradientDescentOptimizer.cpp - * @brief + * @file NonlinearConjugateGradientOptimizer.h + * @brief Simple non-linear optimizer that solves using *non-preconditioned* CG * @author Yong-Dian Jian - * @date Jun 11, 2012 + * @date June 11, 2012 */ #pragma once @@ -13,15 +24,18 @@ namespace gtsam { -/** An implementation of the nonlinear cg method using the template below */ -class GTSAM_EXPORT NonlinearConjugateGradientState : public NonlinearOptimizerState { +/** An implementation of the nonlinear CG method using the template below */ +class GTSAM_EXPORT NonlinearConjugateGradientState: public NonlinearOptimizerState { public: typedef NonlinearOptimizerState Base; - NonlinearConjugateGradientState(const NonlinearFactorGraph& graph, const Values& values) - : Base(graph, values) {} + NonlinearConjugateGradientState(const NonlinearFactorGraph& graph, + const Values& values) : + Base(graph, values) { + } }; -class GTSAM_EXPORT NonlinearConjugateGradientOptimizer : public NonlinearOptimizer { +class GTSAM_EXPORT NonlinearConjugateGradientOptimizer: public NonlinearOptimizer { + /* a class for the nonlinearConjugateGradient template */ class System { public: @@ -33,37 +47,49 @@ class GTSAM_EXPORT NonlinearConjugateGradientOptimizer : public NonlinearOptimiz const NonlinearFactorGraph &graph_; public: - System(const NonlinearFactorGraph &graph): graph_(graph) {} - double error(const State &state) const ; - Gradient gradient(const State &state) const ; - State advance(const State ¤t, const double alpha, const Gradient &g) const ; + System(const NonlinearFactorGraph &graph) : + graph_(graph) { + } + double error(const State &state) const; + Gradient gradient(const State &state) const; + State advance(const State ¤t, const double alpha, + const Gradient &g) const; }; public: + typedef NonlinearOptimizer Base; - typedef NonlinearConjugateGradientState States; typedef NonlinearOptimizerParams Parameters; typedef boost::shared_ptr shared_ptr; protected: - States state_; + NonlinearConjugateGradientState state_; Parameters params_; public: - NonlinearConjugateGradientOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues, - const Parameters& params = Parameters()) - : Base(graph), state_(graph, initialValues), params_(params) {} + /// Constructor + NonlinearConjugateGradientOptimizer(const NonlinearFactorGraph& graph, + const Values& initialValues, const Parameters& params = Parameters()) : + Base(graph), state_(graph, initialValues), params_(params) { + } + + /// Destructor + virtual ~NonlinearConjugateGradientOptimizer() { + } - virtual ~NonlinearConjugateGradientOptimizer() {} virtual void iterate(); - virtual const Values& optimize (); - virtual const NonlinearOptimizerState& _state() const { return state_; } - virtual const NonlinearOptimizerParams& _params() const { return params_; } + virtual const Values& optimize(); + virtual const NonlinearOptimizerState& _state() const { + return state_; + } + virtual const NonlinearOptimizerParams& _params() const { + return params_; + } }; /** Implement the golden-section line search algorithm */ -template +template double lineSearch(const S &system, const V currentValues, const W &gradient) { /* normalize it such that it becomes a unit vector */ @@ -71,37 +97,40 @@ double lineSearch(const S &system, const V currentValues, const W &gradient) { // perform the golden section search algorithm to decide the the optimal step size // detail refer to http://en.wikipedia.org/wiki/Golden_section_search - const double phi = 0.5*(1.0+std::sqrt(5.0)), resphi = 2.0 - phi, tau = 1e-5; - double minStep = -1.0/g, maxStep = 0, - newStep = minStep + (maxStep-minStep) / (phi+1.0) ; + const double phi = 0.5 * (1.0 + std::sqrt(5.0)), resphi = 2.0 - phi, tau = + 1e-5; + double minStep = -1.0 / g, maxStep = 0, newStep = minStep + + (maxStep - minStep) / (phi + 1.0); V newValues = system.advance(currentValues, newStep, gradient); double newError = system.error(newValues); while (true) { - const bool flag = (maxStep - newStep > newStep - minStep) ? true : false ; - const double testStep = flag ? - newStep + resphi * (maxStep - newStep) : newStep - resphi * (newStep - minStep); + const bool flag = (maxStep - newStep > newStep - minStep) ? true : false; + const double testStep = + flag ? newStep + resphi * (maxStep - newStep) : + newStep - resphi * (newStep - minStep); - if ( (maxStep- minStep) < tau * (std::fabs(testStep) + std::fabs(newStep)) ) { - return 0.5*(minStep+maxStep); + if ((maxStep - minStep) + < tau * (std::fabs(testStep) + std::fabs(newStep))) { + return 0.5 * (minStep + maxStep); } const V testValues = system.advance(currentValues, testStep, gradient); const double testError = system.error(testValues); // update the working range - if ( testError >= newError ) { - if ( flag ) maxStep = testStep; - else minStep = testStep; - } - else { - if ( flag ) { + if (testError >= newError) { + if (flag) + maxStep = testStep; + else + minStep = testStep; + } else { + if (flag) { minStep = newStep; newStep = testStep; newError = testError; - } - else { + } else { maxStep = newStep; newStep = testStep; newError = testError; @@ -112,7 +141,7 @@ double lineSearch(const S &system, const V currentValues, const W &gradient) { } /** - * Implement the nonlinear conjugate gradient method using the Polak-Ribieve formula suggested in + * Implement the nonlinear conjugate gradient method using the Polak-Ribiere formula suggested in * http://en.wikipedia.org/wiki/Nonlinear_conjugate_gradient_method. * * The S (system) class requires three member functions: error(state), gradient(state) and @@ -120,8 +149,10 @@ double lineSearch(const S &system, const V currentValues, const W &gradient) { * * The last parameter is a switch between gradient-descent and conjugate gradient */ -template -boost::tuple nonlinearConjugateGradient(const S &system, const V &initial, const NonlinearOptimizerParams ¶ms, const bool singleIteration, const bool gradientDescent = false) { +template +boost::tuple nonlinearConjugateGradient(const S &system, + const V &initial, const NonlinearOptimizerParams ¶ms, + const bool singleIteration, const bool gradientDescent = false) { // GTSAM_CONCEPT_MANIFOLD_TYPE(V); @@ -129,57 +160,67 @@ boost::tuple nonlinearConjugateGradient(const S &system, const V &initia // check if we're already close enough double currentError = system.error(initial); - if(currentError <= params.errorTol) { - if (params.verbosity >= NonlinearOptimizerParams::ERROR){ - std::cout << "Exiting, as error = " << currentError << " < " << params.errorTol << std::endl; + if (currentError <= params.errorTol) { + if (params.verbosity >= NonlinearOptimizerParams::ERROR) { + std::cout << "Exiting, as error = " << currentError << " < " + << params.errorTol << std::endl; } return boost::tie(initial, iteration); } V currentValues = initial; - typename S::Gradient currentGradient = system.gradient(currentValues), prevGradient, - direction = currentGradient; + typename S::Gradient currentGradient = system.gradient(currentValues), + prevGradient, direction = currentGradient; /* do one step of gradient descent */ - V prevValues = currentValues; double prevError = currentError; + V prevValues = currentValues; + double prevError = currentError; double alpha = lineSearch(system, currentValues, direction); currentValues = system.advance(prevValues, alpha, direction); currentError = system.error(currentValues); // Maybe show output - if (params.verbosity >= NonlinearOptimizerParams::ERROR) std::cout << "Initial error: " << currentError << std::endl; + if (params.verbosity >= NonlinearOptimizerParams::ERROR) + std::cout << "Initial error: " << currentError << std::endl; // Iterative loop do { - if ( gradientDescent == true) { + if (gradientDescent == true) { direction = system.gradient(currentValues); - } - else { + } else { prevGradient = currentGradient; currentGradient = system.gradient(currentValues); - const double beta = std::max(0.0, currentGradient.dot(currentGradient-prevGradient)/currentGradient.dot(currentGradient)); - direction = currentGradient + (beta*direction); + // Polak-Ribiere: beta = g'*(g_n-g_n-1)/g_n-1'*g_n-1 + const double beta = std::max(0.0, + currentGradient.dot(currentGradient - prevGradient) + / currentGradient.dot(currentGradient)); + direction = currentGradient + (beta * direction); } alpha = lineSearch(system, currentValues, direction); - prevValues = currentValues; prevError = currentError; + prevValues = currentValues; + prevError = currentError; currentValues = system.advance(prevValues, alpha, direction); currentError = system.error(currentValues); // Maybe show output - if(params.verbosity >= NonlinearOptimizerParams::ERROR) std::cout << "currentError: " << currentError << std::endl; - } while( ++iteration < params.maxIterations && - !singleIteration && - !checkConvergence(params.relativeErrorTol, params.absoluteErrorTol, params.errorTol, prevError, currentError, params.verbosity)); + if (params.verbosity >= NonlinearOptimizerParams::ERROR) + std::cout << "iteration: " << iteration << ", currentError: " << currentError << std::endl; + } while (++iteration < params.maxIterations && !singleIteration + && !checkConvergence(params.relativeErrorTol, params.absoluteErrorTol, + params.errorTol, prevError, currentError, params.verbosity)); // Printing if verbose - if (params.verbosity >= NonlinearOptimizerParams::ERROR && iteration >= params.maxIterations) - std::cout << "nonlinearConjugateGradient: Terminating because reached maximum iterations" << std::endl; + if (params.verbosity >= NonlinearOptimizerParams::ERROR + && iteration >= params.maxIterations) + std::cout + << "nonlinearConjugateGradient: Terminating because reached maximum iterations" + << std::endl; return boost::tie(currentValues, iteration); } -} +} // \ namespace gtsam diff --git a/tests/testGradientDescentOptimizer.cpp b/tests/testGradientDescentOptimizer.cpp index 81bcac344..b2d41c9fe 100644 --- a/tests/testGradientDescentOptimizer.cpp +++ b/tests/testGradientDescentOptimizer.cpp @@ -1,7 +1,14 @@ +/** + * @file NonlinearConjugateGradientOptimizer.cpp + * @brief Test simple CG optimizer + * @author Yong-Dian Jian + * @date June 11, 2012 + */ + /** * @file testGradientDescentOptimizer.cpp - * @brief - * @author ydjian + * @brief Small test of NonlinearConjugateGradientOptimizer + * @author Yong-Dian Jian * @date Jun 11, 2012 */ @@ -22,7 +29,7 @@ using namespace std; using namespace gtsam; - +// Generate a small PoseSLAM problem boost::tuple generateProblem() { // 1. Create graph container and add factors to it @@ -56,49 +63,23 @@ boost::tuple generateProblem() { } /* ************************************************************************* */ -TEST(optimize, GradientDescentOptimizer) { +TEST(NonlinearConjugateGradientOptimizer, Optimize) { NonlinearFactorGraph graph; Values initialEstimate; boost::tie(graph, initialEstimate) = generateProblem(); - // cout << "initial error = " << graph.error(initialEstimate) << endl ; + cout << "initial error = " << graph.error(initialEstimate) << endl ; - // Single Step Optimization using Levenberg-Marquardt NonlinearOptimizerParams param; param.maxIterations = 500; /* requires a larger number of iterations to converge */ - param.verbosity = NonlinearOptimizerParams::SILENT; + param.verbosity = NonlinearOptimizerParams::ERROR; NonlinearConjugateGradientOptimizer optimizer(graph, initialEstimate, param); Values result = optimizer.optimize(); -// cout << "gd1 solver final error = " << graph.error(result) << endl; + cout << "cg final error = " << graph.error(result) << endl; - /* the optimality of the solution is not comparable to the */ - DOUBLES_EQUAL(0.0, graph.error(result), 1e-2); - - CHECK(1); -} - -/* ************************************************************************* */ -TEST(optimize, ConjugateGradientOptimizer) { - - NonlinearFactorGraph graph; - Values initialEstimate; - - boost::tie(graph, initialEstimate) = generateProblem(); -// cout << "initial error = " << graph.error(initialEstimate) << endl ; - - // Single Step Optimization using Levenberg-Marquardt - NonlinearOptimizerParams param; - param.maxIterations = 500; /* requires a larger number of iterations to converge */ - param.verbosity = NonlinearOptimizerParams::SILENT; - - NonlinearConjugateGradientOptimizer optimizer(graph, initialEstimate, param); - Values result = optimizer.optimize(); -// cout << "cg final error = " << graph.error(result) << endl; - - /* the optimality of the solution is not comparable to the */ - DOUBLES_EQUAL(0.0, graph.error(result), 1e-2); + EXPECT_DOUBLES_EQUAL(0.0, graph.error(result), 1e-4); } /* ************************************************************************* */ From 5ed0c6cc49a154cfa6ab89a8522e9097758293b0 Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 18 Feb 2015 08:22:08 +0100 Subject: [PATCH 22/91] fixed *BUG* in Yong-Dian's (obsolete?) CG solver --- gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h index 9c4db5fd9..04d4734a4 100644 --- a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h +++ b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h @@ -193,7 +193,7 @@ boost::tuple nonlinearConjugateGradient(const S &system, // Polak-Ribiere: beta = g'*(g_n-g_n-1)/g_n-1'*g_n-1 const double beta = std::max(0.0, currentGradient.dot(currentGradient - prevGradient) - / currentGradient.dot(currentGradient)); + / prevGradient.dot(prevGradient)); direction = currentGradient + (beta * direction); } From 06643d461694e0b6f73b25d5c84b2c43a4b4e5b7 Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 18 Feb 2015 08:22:22 +0100 Subject: [PATCH 23/91] Turned off verbosity --- tests/testGradientDescentOptimizer.cpp | 45 ++++++++++++++++---------- 1 file changed, 28 insertions(+), 17 deletions(-) diff --git a/tests/testGradientDescentOptimizer.cpp b/tests/testGradientDescentOptimizer.cpp index b2d41c9fe..e45f234aa 100644 --- a/tests/testGradientDescentOptimizer.cpp +++ b/tests/testGradientDescentOptimizer.cpp @@ -25,7 +25,6 @@ #include #include - using namespace std; using namespace gtsam; @@ -33,31 +32,40 @@ using namespace gtsam; boost::tuple generateProblem() { // 1. Create graph container and add factors to it - NonlinearFactorGraph graph ; + NonlinearFactorGraph graph; // 2a. Add Gaussian prior Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin - SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); + SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas( + Vector3(0.3, 0.3, 0.1)); graph += PriorFactor(1, priorMean, priorNoise); // 2b. Add odometry factors - SharedDiagonal odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); - graph += BetweenFactor(1, 2, Pose2(2.0, 0.0, 0.0 ), odometryNoise); + SharedDiagonal odometryNoise = noiseModel::Diagonal::Sigmas( + Vector3(0.2, 0.2, 0.1)); + graph += BetweenFactor(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise); graph += BetweenFactor(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise); graph += BetweenFactor(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise); graph += BetweenFactor(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise); // 2c. Add pose constraint - SharedDiagonal constraintUncertainty = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); - graph += BetweenFactor(5, 2, Pose2(2.0, 0.0, M_PI_2), constraintUncertainty); + SharedDiagonal constraintUncertainty = noiseModel::Diagonal::Sigmas( + Vector3(0.2, 0.2, 0.1)); + graph += BetweenFactor(5, 2, Pose2(2.0, 0.0, M_PI_2), + constraintUncertainty); // 3. Create the data structure to hold the initialEstimate estimate to the solution Values initialEstimate; - Pose2 x1(0.5, 0.0, 0.2 ); initialEstimate.insert(1, x1); - Pose2 x2(2.3, 0.1,-0.2 ); initialEstimate.insert(2, x2); - Pose2 x3(4.1, 0.1, M_PI_2); initialEstimate.insert(3, x3); - Pose2 x4(4.0, 2.0, M_PI ); initialEstimate.insert(4, x4); - Pose2 x5(2.1, 2.1,-M_PI_2); initialEstimate.insert(5, x5); + Pose2 x1(0.5, 0.0, 0.2); + initialEstimate.insert(1, x1); + Pose2 x2(2.3, 0.1, -0.2); + initialEstimate.insert(2, x2); + Pose2 x3(4.1, 0.1, M_PI_2); + initialEstimate.insert(3, x3); + Pose2 x4(4.0, 2.0, M_PI); + initialEstimate.insert(4, x4); + Pose2 x5(2.1, 2.1, -M_PI_2); + initialEstimate.insert(5, x5); return boost::tie(graph, initialEstimate); } @@ -69,19 +77,22 @@ TEST(NonlinearConjugateGradientOptimizer, Optimize) { Values initialEstimate; boost::tie(graph, initialEstimate) = generateProblem(); - cout << "initial error = " << graph.error(initialEstimate) << endl ; +// cout << "initial error = " << graph.error(initialEstimate) << endl; NonlinearOptimizerParams param; - param.maxIterations = 500; /* requires a larger number of iterations to converge */ - param.verbosity = NonlinearOptimizerParams::ERROR; + param.maxIterations = 500; /* requires a larger number of iterations to converge */ + param.verbosity = NonlinearOptimizerParams::SILENT; NonlinearConjugateGradientOptimizer optimizer(graph, initialEstimate, param); Values result = optimizer.optimize(); - cout << "cg final error = " << graph.error(result) << endl; +// cout << "cg final error = " << graph.error(result) << endl; EXPECT_DOUBLES_EQUAL(0.0, graph.error(result), 1e-4); } /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ From f1b0a693cf1e10d6c13295043b072dd0fc9e25af Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 18 Feb 2015 08:32:20 +0100 Subject: [PATCH 24/91] Added testPCGSolver --- .cproject | 3322 ++++++++++++++++++++++++++--------------------------- 1 file changed, 1659 insertions(+), 1663 deletions(-) diff --git a/.cproject b/.cproject index 7f81be78d..b10acf8da 100644 --- a/.cproject +++ b/.cproject @@ -1,7 +1,5 @@ - - - + @@ -486,26 +484,265 @@ - + make -j5 - SmartProjectionFactorExample_kitti_nonbatch.run + testCombinedImuFactor.run true true true - + make -j5 - SmartProjectionFactorExample_kitti.run + testImuFactor.run true true true - + make -j5 - SmartProjectionFactorTesting.run + testAHRSFactor.run + true + true + true + + + make + -j8 + testAttitudeFactor.run + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + testNonlinearConstraint.run + true + true + true + + + make + -j2 + testLieConfig.run + true + true + true + + + make + -j2 + testConstraintOptimizer.run + true + true + true + + + make + -j2 + testGaussianFactor.run + true + true + true + + + make + -j4 + testImuFactor.run + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + testBTree.run + true + true + true + + + make + -j2 + testDSF.run + true + true + true + + + make + -j2 + testDSFVector.run + true + true + true + + + make + -j2 + testMatrix.run + true + true + true + + + make + -j2 + testSPQRUtil.run + true + true + true + + + make + -j2 + testVector.run + true + true + true + + + make + -j2 + timeMatrix.run + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j5 + wrap + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + install + true + true + true + + + make + -j2 + all + true + true + true + + + cmake + .. + true + false + true + + + make + -j2 + check + true + true + true + + + make + -j2 + tests/testLieConfig.run true true true @@ -534,6 +771,70 @@ true true + + make + -j2 + all + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j5 + all + true + false + true + + + make + -j5 + check + true + false + true + + + make + -j5 + SmartProjectionFactorExample_kitti_nonbatch.run + true + true + true + + + make + -j5 + SmartProjectionFactorExample_kitti.run + true + true + true + + + make + -j5 + SmartProjectionFactorTesting.run + true + true + true + make -j2 @@ -558,7 +859,159 @@ true true - + + make + -j5 + testCal3Bundler.run + true + true + true + + + make + -j5 + testCal3DS2.run + true + true + true + + + make + -j5 + testCalibratedCamera.run + true + true + true + + + make + -j5 + testEssentialMatrix.run + true + true + true + + + make + -j1 VERBOSE=1 + testHomography2.run + true + false + true + + + make + -j5 + testPinholeCamera.run + true + true + true + + + make + -j5 + testPoint2.run + true + true + true + + + make + -j5 + testPoint3.run + true + true + true + + + make + -j5 + testPose2.run + true + true + true + + + make + -j5 + testPose3.run + true + true + true + + + make + -j5 + testRot3M.run + true + true + true + + + make + -j5 + testSphere2.run + true + true + true + + + make + -j5 + testStereoCamera.run + true + true + true + + + make + -j5 + testCal3Unified.run + true + true + true + + + make + -j5 + testRot2.run + true + true + true + + + make + -j5 + testRot3Q.run + true + true + true + + + make + -j5 + testRot3.run + true + true + true + + + make + -j4 + testSO3.run + true + true + true + + + make + -j4 + testQuaternion.run + true + true + true + + make -j2 all @@ -566,7 +1019,7 @@ true true - + make -j2 clean @@ -574,143 +1027,23 @@ true true - - make - -k - check - true - false - true - - - make - - tests/testBayesTree.run - true - false - true - - - make - - testBinaryBayesNet.run - true - false - true - - + make -j2 - testFactorGraph.run + clean all true true true - + make -j2 - testISAM.run + install true true true - - make - -j2 - testJunctionTree.run - true - true - true - - - make - -j2 - testKey.run - true - true - true - - - make - -j2 - testOrdering.run - true - true - true - - - make - - testSymbolicBayesNet.run - true - false - true - - - make - - tests/testSymbolicFactor.run - true - false - true - - - make - - testSymbolicFactorGraph.run - true - false - true - - - make - -j2 - timeSymbolMaps.run - true - true - true - - - make - - tests/testBayesTree - true - false - true - - - make - -j2 - tests/testPose2.run - true - true - true - - - make - -j2 - tests/testPose3.run - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - check - true - true - true - - + make -j2 clean @@ -862,154 +1195,146 @@ true true - + make - -j5 - testGaussianFactorGraphUnordered.run + -j2 + all true true true - + make - -j5 - testGaussianBayesNetUnordered.run + -j2 + check true true true - + make - -j5 - testGaussianConditional.run - true - true - true - - - make - -j5 - testGaussianDensity.run - true - true - true - - - make - -j5 - testGaussianJunctionTree.run - true - true - true - - - make - -j5 - testHessianFactor.run - true - true - true - - - make - -j5 - testJacobianFactor.run - true - true - true - - - make - -j5 - testKalmanFilter.run - true - true - true - - - make - -j5 - testNoiseModel.run - true - true - true - - - make - -j5 - testSampler.run - true - true - true - - - make - -j5 - testSerializationLinear.run - true - true - true - - - make - -j5 - testVectorValues.run - true - true - true - - - make - -j5 - testGaussianBayesTree.run - true - true - true - - - make - -j5 - testCombinedImuFactor.run - true - true - true - - - make - -j5 - testImuFactor.run - true - true - true - - - make - -j5 - testAHRSFactor.run - true - true - true - - - make - -j8 - testAttitudeFactor.run - true - true - true - - - make - -j5 + -j2 clean true true true - + make - -j5 - all + -j2 + testPlanarSLAM.run + true + true + true + + + make + -j2 + testPose2Config.run + true + true + true + + + make + -j2 + testPose2Factor.run + true + true + true + + + make + -j2 + testPose2Prior.run + true + true + true + + + make + -j2 + testPose2SLAM.run + true + true + true + + + make + -j2 + testPose3Config.run + true + true + true + + + make + -j2 + testPose3SLAM.run + true + true + true + + + make + + testSimulated2DOriented.run + true + false + true + + + make + -j2 + testVSLAMConfig.run + true + true + true + + + make + -j2 + testVSLAMFactor.run + true + true + true + + + make + -j2 + testVSLAMGraph.run + true + true + true + + + make + -j2 + testPose3Factor.run + true + true + true + + + make + + testSimulated2D.run + true + false + true + + + make + + testSimulated3D.run + true + false + true + + + make + -j2 + tests/testGaussianISAM2 true true true @@ -1104,477 +1429,95 @@ make - testErrors.run true false true - - make - -j2 - check - true - true - true - - - make - -j2 - tests/testGaussianJunctionTree.run - true - true - true - - - make - -j2 - tests/testGaussianFactor.run - true - true - true - - - make - -j2 - tests/testGaussianConditional.run - true - true - true - - - make - -j2 - tests/timeSLAMlike.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - testBTree.run - true - true - true - - - make - -j2 - testDSF.run - true - true - true - - - make - -j2 - testDSFVector.run - true - true - true - - - make - -j2 - testMatrix.run - true - true - true - - - make - -j2 - testSPQRUtil.run - true - true - true - - - make - -j2 - testVector.run - true - true - true - - - make - -j2 - timeMatrix.run - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - testClusterTree.run - true - true - true - - - make - -j2 - testJunctionTree.run - true - true - true - - - make - -j2 - tests/testEliminationTree.run - true - true - true - - - make - -j2 - tests/testSymbolicFactor.run - true - true - true - - - make - -j2 - tests/testVariableSlots.run - true - true - true - - - make - -j2 - tests/testConditional.run - true - true - true - - - make - -j2 - tests/testSymbolicFactorGraph.run - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - testNonlinearConstraint.run - true - true - true - - - make - -j2 - testLieConfig.run - true - true - true - - - make - -j2 - testConstraintOptimizer.run - true - true - true - - - make - -j2 - check - true - true - true - - + make -j5 - testBTree.run + testAntiFactor.run true true true - + make -j5 - testDSF.run + testPriorFactor.run true true true - + make -j5 - testDSFMap.run + testDataset.run true true true - + make -j5 - testDSFVector.run + testEssentialMatrixFactor.run true true true - + make -j5 - testFixedVector.run + testGeneralSFMFactor_Cal3Bundler.run true true true - - make - -j2 - all - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - testPlanarSLAM.run - true - true - true - - - make - -j2 - testPose2Config.run - true - true - true - - - make - -j2 - testPose2Factor.run - true - true - true - - - make - -j2 - testPose2Prior.run - true - true - true - - - make - -j2 - testPose2SLAM.run - true - true - true - - - make - -j2 - testPose3Config.run - true - true - true - - - make - -j2 - testPose3SLAM.run - true - true - true - - - make - testSimulated2DOriented.run - true - false - true - - - make - -j2 - testVSLAMConfig.run - true - true - true - - - make - -j2 - testVSLAMFactor.run - true - true - true - - - make - -j2 - testVSLAMGraph.run - true - true - true - - - make - -j2 - testPose3Factor.run - true - true - true - - - make - testSimulated2D.run - true - false - true - - - make - testSimulated3D.run - true - false - true - - - make - -j2 - tests/testGaussianISAM2 - true - true - true - - + make -j5 - testEliminationTree.run + testGeneralSFMFactor.run true true true - + make -j5 - testInference.run + testProjectionFactor.run true true true - + make -j5 - testKey.run + testRotateFactor.run true true true - + make - -j1 - testSymbolicBayesTree.run + -j5 + testPoseRotationPrior.run true - false + true true - + make - -j1 - testSymbolicSequentialSolver.run + -j5 + testImplicitSchurFactor.run true - false + true true - + make -j4 - testLabeledSymbol.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - tests/testLieConfig.run + testRangeFactor.run true true true @@ -1780,7 +1723,6 @@ cpack - -G DEB true false @@ -1788,7 +1730,6 @@ cpack - -G RPM true false @@ -1796,7 +1737,6 @@ cpack - -G TGZ true false @@ -1804,7 +1744,6 @@ cpack - --config CPackSourceConfig.cmake true false @@ -1986,7 +1925,7 @@ false true - + make -j2 check @@ -1994,38 +1933,55 @@ true true - + make - -j2 - clean - true - true - true - - - make - -j2 - install - true - true - true - - - make - -j2 - all - true - true - true - - - cmake - .. + + tests/testGaussianISAM2 true false true - + + make + -j2 + check + true + true + true + + + make + -j2 + tests/testGaussianJunctionTree.run + true + true + true + + + make + -j2 + tests/testGaussianFactor.run + true + true + true + + + make + -j2 + tests/testGaussianConditional.run + true + true + true + + + make + -j2 + tests/timeSLAMlike.run + true + true + true + + make -j2 testGaussianFactor.run @@ -2033,418 +1989,26 @@ true true - + make -j5 - testCal3Bundler.run + testParticleFactor.run true true true - + make -j5 - testCal3DS2.run + testExpressionMeta.run true true true - - make - -j5 - testCalibratedCamera.run - true - true - true - - - make - -j5 - testEssentialMatrix.run - true - true - true - - - make - -j1 VERBOSE=1 - testHomography2.run - true - false - true - - - make - -j5 - testPinholeCamera.run - true - true - true - - - make - -j5 - testPoint2.run - true - true - true - - - make - -j5 - testPoint3.run - true - true - true - - - make - -j5 - testPose2.run - true - true - true - - - make - -j5 - testPose3.run - true - true - true - - - make - -j5 - testRot3M.run - true - true - true - - - make - -j5 - testSphere2.run - true - true - true - - - make - -j5 - testStereoCamera.run - true - true - true - - - make - -j5 - testCal3Unified.run - true - true - true - - - make - -j5 - testRot2.run - true - true - true - - - make - -j5 - testRot3Q.run - true - true - true - - - make - -j5 - testRot3.run - true - true - true - - + make -j4 - testSO3.run - true - true - true - - - make - -j4 - testQuaternion.run - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j5 - all - true - false - true - - - make - -j5 - check - true - false - true - - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - clean all - true - true - true - - - make - -j5 - testIMUSystem.run - true - true - true - - - make - -j5 - testPoseRTV.run - true - true - true - - - make - -j5 - testVelocityConstraint.run - true - true - true - - - make - -j5 - testVelocityConstraint3.run - true - true - true - - - make - -j1 - testDiscreteBayesTree.run - true - false - true - - - make - -j5 - testDiscreteConditional.run - true - true - true - - - make - -j5 - testDiscreteFactor.run - true - true - true - - - make - -j5 - testDiscreteFactorGraph.run - true - true - true - - - make - -j5 - testDiscreteMarginals.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - timeCalibratedCamera.run - true - true - true - - - make - -j2 - timeRot3.run - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j5 - testWrap.run - true - true - true - - - make - -j5 - testSpirit.run - true - true - true - - - make - -j5 - check.wrap - true - true - true - - - make - -j5 - testMethod.run - true - true - true - - - make - -j5 - testClass.run - true - true - true - - - make - -j4 - testType.run - true - true - true - - - make - -j4 - testArgument.run - true - true - true - - - make - -j4 - testReturnValue.run - true - true - true - - - make - -j4 - testTemplate.run - true - true - true - - - make - -j4 - testGlobalFunction.run + testCustomChartExpression.run true true true @@ -2497,327 +2061,145 @@ true true - + make -j2 - vSFMexample.run + all true true true - + make -j2 - testVSLAMGraph - true - true - true - - - make - -j5 - testMatrix.run - true - true - true - - - make - -j5 - testVector.run - true - true - true - - - make - -j5 - testNumericalDerivative.run - true - true - true - - - make - -j5 - testVerticalBlockMatrix.run - true - true - true - - - make - -j4 - testOptionalJacobian.run - true - true - true - - - make - -j5 - check.tests - true - true - true - - - make - -j2 - timeGaussianFactorGraph.run - true - true - true - - - make - -j5 - testMarginals.run - true - true - true - - - make - -j5 - testGaussianISAM2.run - true - true - true - - - make - -j5 - testSymbolicFactorGraphB.run - true - true - true - - - make - -j2 - timeSequentialOnDataset.run - true - true - true - - - make - -j5 - testGradientDescentOptimizer.run - true - true - true - - - make - -j2 - testGaussianFactor.run - true - true - true - - - make - -j2 - testNonlinearOptimizer.run - true - true - true - - - make - -j2 - testGaussianBayesNet.run - true - true - true - - - make - -j2 - testNonlinearISAM.run - true - true - true - - - make - -j2 - testNonlinearEquality.run - true - true - true - - - make - -j2 - testExtendedKalmanFilter.run - true - true - true - - - make - -j5 - timing.tests - true - true - true - - - make - -j5 - testNonlinearFactor.run - true - true - true - - - make - -j5 clean true true true - + make - -j5 - testGaussianJunctionTreeB.run - true - true - true - - - make - - testGraph.run + -k + check true false true - + make - + tests/testBayesTree.run + true + false + true + + + make + testBinaryBayesNet.run + true + false + true + + + make + -j2 + testFactorGraph.run + true + true + true + + + make + -j2 + testISAM.run + true + true + true + + + make + -j2 testJunctionTree.run true - false + true true - + make - - testSymbolicBayesNetB.run + -j2 + testKey.run + true + true + true + + + make + -j2 + testOrdering.run + true + true + true + + + make + testSymbolicBayesNet.run true false true - + make - -j5 - testGaussianISAM.run + tests/testSymbolicFactor.run true - true + false true - + make - -j5 - testDoglegOptimizer.run + testSymbolicFactorGraph.run true - true + false true - - make - -j5 - testNonlinearFactorGraph.run - true - true - true - - - make - -j5 - testIterative.run - true - true - true - - - make - -j5 - testSubgraphSolver.run - true - true - true - - - make - -j5 - testGaussianFactorGraphB.run - true - true - true - - - make - -j5 - testSummarization.run - true - true - true - - - make - -j5 - testManifold.run - true - true - true - - - make - -j5 - testParticleFactor.run - true - true - true - - - make - -j5 - testExpressionMeta.run - true - true - true - - - make - -j4 - testCustomChartExpression.run - true - true - true - - + make -j2 - testGaussianFactor.run + timeSymbolMaps.run true true true - + + make + tests/testBayesTree + true + false + true + + make -j2 - install + check true true true - + + make + -j2 + timeCalibratedCamera.run + true + true + true + + + make + -j2 + timeRot3.run + true + true + true + + make -j2 clean @@ -2825,122 +2207,18 @@ true true - + make -j2 - all + tests/testPose2.run true true true - + make -j2 - clean - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j5 - testAntiFactor.run - true - true - true - - - make - -j5 - testPriorFactor.run - true - true - true - - - make - -j5 - testDataset.run - true - true - true - - - make - -j5 - testEssentialMatrixFactor.run - true - true - true - - - make - -j5 - testGeneralSFMFactor_Cal3Bundler.run - true - true - true - - - make - -j5 - testGeneralSFMFactor.run - true - true - true - - - make - -j5 - testProjectionFactor.run - true - true - true - - - make - -j5 - testRotateFactor.run - true - true - true - - - make - -j5 - testPoseRotationPrior.run - true - true - true - - - make - -j5 - testImplicitSchurFactor.run - true - true - true - - - make - -j4 - testRangeFactor.run + tests/testPose3.run true true true @@ -3145,181 +2423,6 @@ true true - - make - -j5 - testLago.run - true - true - true - - - make - -j5 - testLinearContainerFactor.run - true - true - true - - - make - -j5 - testOrdering.run - true - true - true - - - make - -j5 - testValues.run - true - true - true - - - make - -j5 - testWhiteNoiseFactor.run - true - true - true - - - make - -j4 - testExpression.run - true - true - true - - - make - -j4 - testAdaptAutoDiff.run - true - true - true - - - make - -j4 - testCallRecord.run - true - true - true - - - make - -j4 - testExpressionFactor.run - true - true - true - - - make - -j4 - testImuFactor.run - true - true - true - - - make - -j5 - timeCalibratedCamera.run - true - true - true - - - make - -j5 - timePinholeCamera.run - true - true - true - - - make - -j5 - timeStereoCamera.run - true - true - true - - - make - -j5 - timeLago.run - true - true - true - - - make - -j5 - timePose3.run - true - true - true - - - make - -j4 - timeAdaptAutoDiff.run - true - true - true - - - make - -j4 - timeCameraExpression.run - true - true - true - - - make - -j4 - timeOneCameraExpression.run - true - true - true - - - make - -j4 - timeSFMExpressions.run - true - true - true - - - make - -j4 - timeIncremental.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - tests/testGaussianISAM2 - true - false - true - make -j2 @@ -3416,10 +2519,903 @@ true true - + + make + -j1 + testDiscreteBayesTree.run + true + false + true + + make -j5 - wrap + testDiscreteConditional.run + true + true + true + + + make + -j5 + testDiscreteFactor.run + true + true + true + + + make + -j5 + testDiscreteFactorGraph.run + true + true + true + + + make + -j5 + testDiscreteMarginals.run + true + true + true + + + make + -j5 + testEliminationTree.run + true + true + true + + + make + -j5 + testInference.run + true + true + true + + + make + -j5 + testKey.run + true + true + true + + + make + -j1 + testSymbolicBayesTree.run + true + false + true + + + make + -j1 + testSymbolicSequentialSolver.run + true + false + true + + + make + -j4 + testLabeledSymbol.run + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + testClusterTree.run + true + true + true + + + make + -j2 + testJunctionTree.run + true + true + true + + + make + -j2 + tests/testEliminationTree.run + true + true + true + + + make + -j2 + tests/testSymbolicFactor.run + true + true + true + + + make + -j2 + tests/testVariableSlots.run + true + true + true + + + make + -j2 + tests/testConditional.run + true + true + true + + + make + -j2 + tests/testSymbolicFactorGraph.run + true + true + true + + + make + -j5 + testLago.run + true + true + true + + + make + -j5 + testLinearContainerFactor.run + true + true + true + + + make + -j5 + testOrdering.run + true + true + true + + + make + -j5 + testValues.run + true + true + true + + + make + -j5 + testWhiteNoiseFactor.run + true + true + true + + + make + -j4 + testExpression.run + true + true + true + + + make + -j4 + testAdaptAutoDiff.run + true + true + true + + + make + -j4 + testCallRecord.run + true + true + true + + + make + -j4 + testExpressionFactor.run + true + true + true + + + make + -j5 + testIMUSystem.run + true + true + true + + + make + -j5 + testPoseRTV.run + true + true + true + + + make + -j5 + testVelocityConstraint.run + true + true + true + + + make + -j5 + testVelocityConstraint3.run + true + true + true + + + make + -j5 + timeCalibratedCamera.run + true + true + true + + + make + -j5 + timePinholeCamera.run + true + true + true + + + make + -j5 + timeStereoCamera.run + true + true + true + + + make + -j5 + timeLago.run + true + true + true + + + make + -j5 + timePose3.run + true + true + true + + + make + -j4 + timeAdaptAutoDiff.run + true + true + true + + + make + -j4 + timeCameraExpression.run + true + true + true + + + make + -j4 + timeOneCameraExpression.run + true + true + true + + + make + -j4 + timeSFMExpressions.run + true + true + true + + + make + -j4 + timeIncremental.run + true + true + true + + + make + -j5 + testBTree.run + true + true + true + + + make + -j5 + testDSF.run + true + true + true + + + make + -j5 + testDSFMap.run + true + true + true + + + make + -j5 + testDSFVector.run + true + true + true + + + make + -j5 + testFixedVector.run + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j5 + testGaussianFactorGraphUnordered.run + true + true + true + + + make + -j5 + testGaussianBayesNetUnordered.run + true + true + true + + + make + -j5 + testGaussianConditional.run + true + true + true + + + make + -j5 + testGaussianDensity.run + true + true + true + + + make + -j5 + testGaussianJunctionTree.run + true + true + true + + + make + -j5 + testHessianFactor.run + true + true + true + + + make + -j5 + testJacobianFactor.run + true + true + true + + + make + -j5 + testKalmanFilter.run + true + true + true + + + make + -j5 + testNoiseModel.run + true + true + true + + + make + -j5 + testSampler.run + true + true + true + + + make + -j5 + testSerializationLinear.run + true + true + true + + + make + -j5 + testVectorValues.run + true + true + true + + + make + -j5 + testGaussianBayesTree.run + true + true + true + + + make + -j5 + testWrap.run + true + true + true + + + make + -j5 + testSpirit.run + true + true + true + + + make + -j5 + check.wrap + true + true + true + + + make + -j5 + testMethod.run + true + true + true + + + make + -j5 + testClass.run + true + true + true + + + make + -j4 + testType.run + true + true + true + + + make + -j4 + testArgument.run + true + true + true + + + make + -j4 + testReturnValue.run + true + true + true + + + make + -j4 + testTemplate.run + true + true + true + + + make + -j4 + testGlobalFunction.run + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j5 + testMatrix.run + true + true + true + + + make + -j5 + testVector.run + true + true + true + + + make + -j5 + testNumericalDerivative.run + true + true + true + + + make + -j5 + testVerticalBlockMatrix.run + true + true + true + + + make + -j4 + testOptionalJacobian.run + true + true + true + + + make + -j5 + check.tests + true + true + true + + + make + -j2 + timeGaussianFactorGraph.run + true + true + true + + + make + -j5 + testMarginals.run + true + true + true + + + make + -j5 + testGaussianISAM2.run + true + true + true + + + make + -j5 + testSymbolicFactorGraphB.run + true + true + true + + + make + -j2 + timeSequentialOnDataset.run + true + true + true + + + make + -j5 + testGradientDescentOptimizer.run + true + true + true + + + make + -j2 + testGaussianFactor.run + true + true + true + + + make + -j2 + testNonlinearOptimizer.run + true + true + true + + + make + -j2 + testGaussianBayesNet.run + true + true + true + + + make + -j2 + testNonlinearISAM.run + true + true + true + + + make + -j2 + testNonlinearEquality.run + true + true + true + + + make + -j2 + testExtendedKalmanFilter.run + true + true + true + + + make + -j5 + timing.tests + true + true + true + + + make + -j5 + testNonlinearFactor.run + true + true + true + + + make + -j5 + clean + true + true + true + + + make + -j5 + testGaussianJunctionTreeB.run + true + true + true + + + make + testGraph.run + true + false + true + + + make + testJunctionTree.run + true + false + true + + + make + testSymbolicBayesNetB.run + true + false + true + + + make + -j5 + testGaussianISAM.run + true + true + true + + + make + -j5 + testDoglegOptimizer.run + true + true + true + + + make + -j5 + testNonlinearFactorGraph.run + true + true + true + + + make + -j5 + testIterative.run + true + true + true + + + make + -j5 + testSubgraphSolver.run + true + true + true + + + make + -j5 + testGaussianFactorGraphB.run + true + true + true + + + make + -j5 + testSummarization.run + true + true + true + + + make + -j5 + testManifold.run + true + true + true + + + make + -j4 + testPCGSolver.run + true + true + true + + + make + -j2 + vSFMexample.run + true + true + true + + + make + -j5 + clean + true + true + true + + + make + -j5 + all + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + testVSLAMGraph true true true From 1ebb3cf2a92e2ebf09d2283ce4bc147286f67de1 Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 18 Feb 2015 08:32:35 +0100 Subject: [PATCH 25/91] Fixed header bloat --- tests/testPCGSolver.cpp | 10 ---------- 1 file changed, 10 deletions(-) diff --git a/tests/testPCGSolver.cpp b/tests/testPCGSolver.cpp index c90b09db1..66b022c82 100644 --- a/tests/testPCGSolver.cpp +++ b/tests/testPCGSolver.cpp @@ -17,21 +17,11 @@ */ #include -#include -#include -#include -#include -#include -#include -#include #include #include #include -#include #include -#include #include -#include #include #include From f8f2c2db920306848ea5595c5d5fa8c00eef9ddb Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 18 Feb 2015 09:30:39 +0100 Subject: [PATCH 26/91] Doxygen, header bloat, standard formatting --- .cproject | 24 ++ gtsam/linear/ConjugateGradientSolver.cpp | 24 +- gtsam/linear/ConjugateGradientSolver.h | 9 +- gtsam/linear/IterativeSolver.cpp | 101 ++++---- gtsam/linear/IterativeSolver.h | 231 +++++++++++------- gtsam/linear/PCGSolver.cpp | 79 +++--- gtsam/linear/PCGSolver.h | 57 +++-- gtsam/linear/SubgraphSolver.cpp | 128 +++++----- gtsam/linear/SubgraphSolver.h | 104 +++++--- gtsam/slam/JacobianFactorQ.h | 11 + gtsam/slam/JacobianFactorQR.h | 7 +- gtsam/slam/JacobianSchurFactor.h | 15 +- gtsam/slam/RegularImplicitSchurFactor.h | 4 +- gtsam/slam/RegularJacobianFactor.h | 39 +-- .../tests/testRegularImplicitSchurFactor.cpp | 20 +- .../slam/tests/testRegularJacobianFactor.cpp | 17 +- .../tests/testSmartProjectionPoseFactor.cpp | 2 +- 17 files changed, 549 insertions(+), 323 deletions(-) diff --git a/.cproject b/.cproject index b10acf8da..a974f16d4 100644 --- a/.cproject +++ b/.cproject @@ -1522,6 +1522,30 @@ true true + + make + -j4 + testSmartProjectionPoseFactor.run + true + true + true + + + make + -j4 + testRegularImplicitSchurFactor.run + true + true + true + + + make + -j4 + testRegularJacobianFactor.run + true + true + true + make -j3 diff --git a/gtsam/linear/ConjugateGradientSolver.cpp b/gtsam/linear/ConjugateGradientSolver.cpp index 5e7e08f61..a1b9e2d83 100644 --- a/gtsam/linear/ConjugateGradientSolver.cpp +++ b/gtsam/linear/ConjugateGradientSolver.cpp @@ -1,8 +1,20 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + /* - * ConjugateGradientSolver.cpp - * - * Created on: Jun 4, 2014 - * Author: Yong-Dian Jian + * @file ConjugateGradientSolver.cpp + * @brief Implementation of Conjugate Gradient solver for a linear system + * @author Yong-Dian Jian + * @author Sungtae An + * @date Nov 6, 2014 */ #include @@ -35,7 +47,8 @@ std::string ConjugateGradientParameters::blasTranslator(const BLASKernel value) } /*****************************************************************************/ -ConjugateGradientParameters::BLASKernel ConjugateGradientParameters::blasTranslator(const std::string &src) { +ConjugateGradientParameters::BLASKernel ConjugateGradientParameters::blasTranslator( + const std::string &src) { std::string s = src; boost::algorithm::to_upper(s); if (s == "GTSAM") return ConjugateGradientParameters::GTSAM; @@ -43,6 +56,7 @@ ConjugateGradientParameters::BLASKernel ConjugateGradientParameters::blasTransla return ConjugateGradientParameters::GTSAM; } +/*****************************************************************************/ } diff --git a/gtsam/linear/ConjugateGradientSolver.h b/gtsam/linear/ConjugateGradientSolver.h index 20e0c5262..2596a7403 100644 --- a/gtsam/linear/ConjugateGradientSolver.h +++ b/gtsam/linear/ConjugateGradientSolver.h @@ -20,7 +20,6 @@ #pragma once #include -#include namespace gtsam { @@ -87,9 +86,8 @@ public: static BLASKernel blasTranslator(const std::string &s) ; }; -/*********************************************************************************************/ /* - * A template of linear preconditioned conjugate gradient method. + * A template for the linear preconditioned conjugate gradient method. * System class should support residual(v, g), multiply(v,Av), scal(alpha,v), dot(v,v), axpy(alpha,x,y) * leftPrecondition(v, L^{-1}v, rightPrecondition(v, L^{-T}v) where preconditioner M = L*L^T * Note that the residual is in the preconditioned domain. Refer to Section 9.2 of Saad's book. @@ -98,8 +96,9 @@ public: * [1] Y. Saad, "Preconditioned Iterations," in Iterative Methods for Sparse Linear Systems, * 2nd ed. SIAM, 2003, ch. 9, sec. 2, pp.276-281. */ -template -V preconditionedConjugateGradient(const S &system, const V &initial, const ConjugateGradientParameters ¶meters) { +template +V preconditionedConjugateGradient(const S &system, const V &initial, + const ConjugateGradientParameters ¶meters) { V estimate, residual, direction, q1, q2; estimate = residual = direction = q1 = q2 = initial; diff --git a/gtsam/linear/IterativeSolver.cpp b/gtsam/linear/IterativeSolver.cpp index ab3ccde13..79ade1c8a 100644 --- a/gtsam/linear/IterativeSolver.cpp +++ b/gtsam/linear/IterativeSolver.cpp @@ -1,6 +1,17 @@ +/* ---------------------------------------------------------------------------- + + * 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 IterativeSolver.cpp - * @brief + * @brief Some support classes for iterative solvers * @date Sep 3, 2012 * @author Yong-Dian Jian */ @@ -9,18 +20,22 @@ #include #include #include +#include #include -#include using namespace std; namespace gtsam { /*****************************************************************************/ -string IterativeOptimizationParameters::getVerbosity() const { return verbosityTranslator(verbosity_); } +string IterativeOptimizationParameters::getVerbosity() const { + return verbosityTranslator(verbosity_); +} /*****************************************************************************/ -void IterativeOptimizationParameters::setVerbosity(const string &src) { verbosity_ = verbosityTranslator(src); } +void IterativeOptimizationParameters::setVerbosity(const string &src) { + verbosity_ = verbosityTranslator(src); +} /*****************************************************************************/ void IterativeOptimizationParameters::print() const { @@ -29,78 +44,82 @@ void IterativeOptimizationParameters::print() const { /*****************************************************************************/ void IterativeOptimizationParameters::print(ostream &os) const { - os << "IterativeOptimizationParameters:" << endl - << "verbosity: " << verbosityTranslator(verbosity_) << endl; + os << "IterativeOptimizationParameters:" << endl << "verbosity: " + << verbosityTranslator(verbosity_) << endl; } /*****************************************************************************/ - ostream& operator<<(ostream &os, const IterativeOptimizationParameters &p) { +ostream& operator<<(ostream &os, const IterativeOptimizationParameters &p) { p.print(os); return os; } /*****************************************************************************/ -IterativeOptimizationParameters::Verbosity IterativeOptimizationParameters::verbosityTranslator(const string &src) { - string s = src; boost::algorithm::to_upper(s); - if (s == "SILENT") return IterativeOptimizationParameters::SILENT; - else if (s == "COMPLEXITY") return IterativeOptimizationParameters::COMPLEXITY; - else if (s == "ERROR") return IterativeOptimizationParameters::ERROR; +IterativeOptimizationParameters::Verbosity IterativeOptimizationParameters::verbosityTranslator( + const string &src) { + string s = src; + boost::algorithm::to_upper(s); + if (s == "SILENT") + return IterativeOptimizationParameters::SILENT; + else if (s == "COMPLEXITY") + return IterativeOptimizationParameters::COMPLEXITY; + else if (s == "ERROR") + return IterativeOptimizationParameters::ERROR; /* default is default */ - else return IterativeOptimizationParameters::SILENT; + else + return IterativeOptimizationParameters::SILENT; } /*****************************************************************************/ - string IterativeOptimizationParameters::verbosityTranslator(IterativeOptimizationParameters::Verbosity verbosity) { - if (verbosity == SILENT) return "SILENT"; - else if (verbosity == COMPLEXITY) return "COMPLEXITY"; - else if (verbosity == ERROR) return "ERROR"; - else return "UNKNOWN"; +string IterativeOptimizationParameters::verbosityTranslator( + IterativeOptimizationParameters::Verbosity verbosity) { + if (verbosity == SILENT) + return "SILENT"; + else if (verbosity == COMPLEXITY) + return "COMPLEXITY"; + else if (verbosity == ERROR) + return "ERROR"; + else + return "UNKNOWN"; } /*****************************************************************************/ -VectorValues IterativeSolver::optimize( - const GaussianFactorGraph &gfg, +VectorValues IterativeSolver::optimize(const GaussianFactorGraph &gfg, boost::optional keyInfo, - boost::optional&> lambda) -{ - return optimize( - gfg, - keyInfo ? *keyInfo : KeyInfo(gfg), - lambda ? *lambda : std::map()); + boost::optional&> lambda) { + return optimize(gfg, keyInfo ? *keyInfo : KeyInfo(gfg), + lambda ? *lambda : std::map()); } /*****************************************************************************/ -VectorValues IterativeSolver::optimize ( - const GaussianFactorGraph &gfg, - const KeyInfo &keyInfo, - const std::map &lambda) -{ +VectorValues IterativeSolver::optimize(const GaussianFactorGraph &gfg, + const KeyInfo &keyInfo, const std::map &lambda) { return optimize(gfg, keyInfo, lambda, keyInfo.x0()); } /****************************************************************************/ -KeyInfo::KeyInfo(const GaussianFactorGraph &fg, const Ordering &ordering) - : ordering_(ordering) { +KeyInfo::KeyInfo(const GaussianFactorGraph &fg, const Ordering &ordering) : + ordering_(ordering) { initialize(fg); } /****************************************************************************/ -KeyInfo::KeyInfo(const GaussianFactorGraph &fg) - : ordering_(Ordering::Natural(fg)) { +KeyInfo::KeyInfo(const GaussianFactorGraph &fg) : + ordering_(Ordering::Natural(fg)) { initialize(fg); } /****************************************************************************/ -void KeyInfo::initialize(const GaussianFactorGraph &fg){ +void KeyInfo::initialize(const GaussianFactorGraph &fg) { const map colspec = fg.getKeyDimMap(); const size_t n = ordering_.size(); size_t start = 0; - for ( size_t i = 0 ; i < n ; ++i ) { + for (size_t i = 0; i < n; ++i) { const size_t key = ordering_[i]; const size_t dim = colspec.find(key)->second; insert(make_pair(key, KeyInfoEntry(i, dim, start))); - start += dim ; + start += dim; } numCols_ = start; } @@ -108,7 +127,7 @@ void KeyInfo::initialize(const GaussianFactorGraph &fg){ /****************************************************************************/ vector KeyInfo::colSpec() const { std::vector result(size(), 0); - BOOST_FOREACH ( const gtsam::KeyInfo::value_type &item, *this ) { + BOOST_FOREACH ( const KeyInfo::value_type &item, *this ) { result[item.second.index()] = item.second.dim(); } return result; @@ -117,7 +136,7 @@ vector KeyInfo::colSpec() const { /****************************************************************************/ VectorValues KeyInfo::x0() const { VectorValues result; - BOOST_FOREACH ( const gtsam::KeyInfo::value_type &item, *this ) { + BOOST_FOREACH ( const KeyInfo::value_type &item, *this ) { result.insert(item.first, Vector::Zero(item.second.dim())); } return result; @@ -128,7 +147,5 @@ Vector KeyInfo::x0vector() const { return Vector::Zero(numCols_); } - } - diff --git a/gtsam/linear/IterativeSolver.h b/gtsam/linear/IterativeSolver.h index d6e1b6e98..442a5fc6b 100644 --- a/gtsam/linear/IterativeSolver.h +++ b/gtsam/linear/IterativeSolver.h @@ -9,131 +9,178 @@ * -------------------------------------------------------------------------- */ +/** + * @file IterativeSolver.h + * @brief Some support classes for iterative solvers + * @date 2010 + * @author Yong-Dian Jian + */ + #pragma once -#include -#include #include +#include + #include -#include -#include +#include +#include + #include -#include #include -#include +#include namespace gtsam { - // Forward declarations - class KeyInfo; - class KeyInfoEntry; - class GaussianFactorGraph; - class Values; - class VectorValues; +// Forward declarations +class KeyInfo; +class KeyInfoEntry; +class GaussianFactorGraph; +class Values; +class VectorValues; - /************************************************************************************/ - /** - * parameters for iterative linear solvers - */ - class GTSAM_EXPORT IterativeOptimizationParameters { +/** + * parameters for iterative linear solvers + */ +class GTSAM_EXPORT IterativeOptimizationParameters { - public: +public: - typedef boost::shared_ptr shared_ptr; - enum Verbosity { SILENT = 0, COMPLEXITY, ERROR } verbosity_; + typedef boost::shared_ptr shared_ptr; + enum Verbosity { + SILENT = 0, COMPLEXITY, ERROR + } verbosity_; - public: +public: - IterativeOptimizationParameters(Verbosity v = SILENT) - : verbosity_(v) {} + IterativeOptimizationParameters(Verbosity v = SILENT) : + verbosity_(v) { + } - virtual ~IterativeOptimizationParameters() {} + virtual ~IterativeOptimizationParameters() { + } - /* utility */ - inline Verbosity verbosity() const { return verbosity_; } - std::string getVerbosity() const; - void setVerbosity(const std::string &s) ; + /* utility */ + inline Verbosity verbosity() const { + return verbosity_; + } + std::string getVerbosity() const; + void setVerbosity(const std::string &s); - /* matlab interface */ - void print() const ; + /* matlab interface */ + void print() const; - /* virtual print function */ - virtual void print(std::ostream &os) const ; + /* virtual print function */ + virtual void print(std::ostream &os) const; - /* for serialization */ - friend std::ostream& operator<<(std::ostream &os, const IterativeOptimizationParameters &p); + /* for serialization */ + friend std::ostream& operator<<(std::ostream &os, + const IterativeOptimizationParameters &p); - static Verbosity verbosityTranslator(const std::string &s); - static std::string verbosityTranslator(Verbosity v); - }; + static Verbosity verbosityTranslator(const std::string &s); + static std::string verbosityTranslator(Verbosity v); +}; - /************************************************************************************/ - class GTSAM_EXPORT IterativeSolver { - public: - typedef boost::shared_ptr shared_ptr; - IterativeSolver() {} - virtual ~IterativeSolver() {} +/** + * Base class for Iterative Solvers like SubgraphSolver + */ +class GTSAM_EXPORT IterativeSolver { +public: + typedef boost::shared_ptr shared_ptr; + IterativeSolver() { + } + virtual ~IterativeSolver() { + } - /* interface to the nonlinear optimizer, without metadata, damping and initial estimate */ - VectorValues optimize ( - const GaussianFactorGraph &gfg, + /* interface to the nonlinear optimizer, without metadata, damping and initial estimate */ + VectorValues optimize(const GaussianFactorGraph &gfg, boost::optional = boost::none, - boost::optional&> lambda = boost::none - ); + boost::optional&> lambda = boost::none); - /* interface to the nonlinear optimizer, without initial estimate */ - VectorValues optimize ( - const GaussianFactorGraph &gfg, - const KeyInfo &keyInfo, - const std::map &lambda - ); + /* interface to the nonlinear optimizer, without initial estimate */ + VectorValues optimize(const GaussianFactorGraph &gfg, const KeyInfo &keyInfo, + const std::map &lambda); - /* interface to the nonlinear optimizer that the subclasses have to implement */ - virtual VectorValues optimize ( - const GaussianFactorGraph &gfg, - const KeyInfo &keyInfo, - const std::map &lambda, - const VectorValues &initial - ) = 0; + /* interface to the nonlinear optimizer that the subclasses have to implement */ + virtual VectorValues optimize(const GaussianFactorGraph &gfg, + const KeyInfo &keyInfo, const std::map &lambda, + const VectorValues &initial) = 0; - }; +}; - /************************************************************************************/ - /* Handy data structure for iterative solvers - * key to (index, dimension, colstart) */ - class GTSAM_EXPORT KeyInfoEntry : public boost::tuple { - public: - typedef boost::tuple Base; - KeyInfoEntry(){} - KeyInfoEntry(size_t idx, size_t d, Key start) : Base(idx, d, start) {} - size_t index() const { return this->get<0>(); } - size_t dim() const { return this->get<1>(); } - size_t colstart() const { return this->get<2>(); } - }; +/** + * Handy data structure for iterative solvers + * key to (index, dimension, colstart) + */ +class GTSAM_EXPORT KeyInfoEntry: public boost::tuple { - /************************************************************************************/ - class GTSAM_EXPORT KeyInfo : public std::map { - public: - typedef std::map Base; - KeyInfo() : numCols_(0) {} - KeyInfo(const GaussianFactorGraph &fg); - KeyInfo(const GaussianFactorGraph &fg, const Ordering &ordering); +public: - std::vector colSpec() const ; - VectorValues x0() const; - Vector x0vector() const; + typedef boost::tuple Base; - inline size_t numCols() const { return numCols_; } - inline const Ordering & ordering() const { return ordering_; } + KeyInfoEntry() { + } + KeyInfoEntry(size_t idx, size_t d, Key start) : + Base(idx, d, start) { + } + size_t index() const { + return this->get<0>(); + } + size_t dim() const { + return this->get<1>(); + } + size_t colstart() const { + return this->get<2>(); + } +}; - protected: +/** + * Handy data structure for iterative solvers + */ +class GTSAM_EXPORT KeyInfo: public std::map { - void initialize(const GaussianFactorGraph &fg); +public: - Ordering ordering_; - size_t numCols_; + typedef std::map Base; - }; +protected: + Ordering ordering_; + size_t numCols_; -} + void initialize(const GaussianFactorGraph &fg); + +public: + + /// Default Constructor + KeyInfo() : + numCols_(0) { + } + + /// Construct from Gaussian factor graph, use "Natural" ordering + KeyInfo(const GaussianFactorGraph &fg); + + /// Construct from Gaussian factor graph and a given ordering + KeyInfo(const GaussianFactorGraph &fg, const Ordering &ordering); + + /// Return the total number of columns (scalar variables = sum of dimensions) + inline size_t numCols() const { + return numCols_; + } + + /// Return the ordering + inline const Ordering & ordering() const { + return ordering_; + } + + /// Return a vector of dimensions ordered by ordering() + std::vector colSpec() const; + + /// Return VectorValues with zeros, of correct dimension + VectorValues x0() const; + + /// Return zero Vector of correct dimension + Vector x0vector() const; + +}; + +} // \ namespace gtsam diff --git a/gtsam/linear/PCGSolver.cpp b/gtsam/linear/PCGSolver.cpp index 3698edc2f..caf7d0095 100644 --- a/gtsam/linear/PCGSolver.cpp +++ b/gtsam/linear/PCGSolver.cpp @@ -1,16 +1,31 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + /* - * PCGSolver.cpp - * - * Created on: Feb 14, 2012 - * Author: Yong-Dian Jian - * Author: Sungtae An + * @file PCGSolver.cpp + * @brief Preconditioned Conjugate Gradient Solver for linear systems + * @date Feb 14, 2012 + * @author Yong-Dian Jian + * @author Sungtae An */ -#include #include +#include #include +#include +#include #include + +#include #include #include @@ -21,7 +36,7 @@ namespace gtsam { /*****************************************************************************/ void PCGSolverParameters::print(ostream &os) const { Base::print(os); - os << "PCGSolverParameters:" << endl; + os << "PCGSolverParameters:" << endl; preconditioner_->print(os); } @@ -32,30 +47,27 @@ PCGSolver::PCGSolver(const PCGSolverParameters &p) { } /*****************************************************************************/ -VectorValues PCGSolver::optimize ( - const GaussianFactorGraph &gfg, - const KeyInfo &keyInfo, - const std::map &lambda, - const VectorValues &initial) -{ +VectorValues PCGSolver::optimize(const GaussianFactorGraph &gfg, + const KeyInfo &keyInfo, const std::map &lambda, + const VectorValues &initial) { /* build preconditioner */ preconditioner_->build(gfg, keyInfo, lambda); /* apply pcg */ - const Vector sol = preconditionedConjugateGradient( - GaussianFactorGraphSystem(gfg, *preconditioner_, keyInfo, lambda), - initial.vector(keyInfo.ordering()), parameters_); + GaussianFactorGraphSystem system(gfg, *preconditioner_, keyInfo, lambda); + Vector x0 = initial.vector(keyInfo.ordering()); + const Vector sol = preconditionedConjugateGradient(system, x0, parameters_); return buildVectorValues(sol, keyInfo); } /*****************************************************************************/ GaussianFactorGraphSystem::GaussianFactorGraphSystem( - const GaussianFactorGraph &gfg, - const Preconditioner &preconditioner, - const KeyInfo &keyInfo, - const std::map &lambda) - : gfg_(gfg), preconditioner_(preconditioner), keyInfo_(keyInfo), lambda_(lambda) {} + const GaussianFactorGraph &gfg, const Preconditioner &preconditioner, + const KeyInfo &keyInfo, const std::map &lambda) : + gfg_(gfg), preconditioner_(preconditioner), keyInfo_(keyInfo), lambda_( + lambda) { +} /*****************************************************************************/ void GaussianFactorGraphSystem::residual(const Vector &x, Vector &r) const { @@ -67,7 +79,7 @@ void GaussianFactorGraphSystem::residual(const Vector &x, Vector &r) const { /* substract A*x */ Vector Ax = Vector::Zero(r.rows(), 1); multiply(x, Ax); - r -= Ax ; + r -= Ax; } /*****************************************************************************/ @@ -75,7 +87,7 @@ void GaussianFactorGraphSystem::multiply(const Vector &x, Vector& AtAx) const { /* implement A^T*(A*x), assume x and AtAx are pre-allocated */ // Build a VectorValues for Vector x - VectorValues vvX = buildVectorValues(x,keyInfo_); + VectorValues vvX = buildVectorValues(x, keyInfo_); // VectorValues form of A'Ax for multiplyHessianAdd VectorValues vvAtAx; @@ -99,31 +111,33 @@ void GaussianFactorGraphSystem::getb(Vector &b) const { } /**********************************************************************************/ -void GaussianFactorGraphSystem::leftPrecondition(const Vector &x, Vector &y) const { +void GaussianFactorGraphSystem::leftPrecondition(const Vector &x, + Vector &y) const { // For a preconditioner M = L*L^T // Calculate y = L^{-1} x preconditioner_.solve(x, y); } /**********************************************************************************/ -void GaussianFactorGraphSystem::rightPrecondition(const Vector &x, Vector &y) const { +void GaussianFactorGraphSystem::rightPrecondition(const Vector &x, + Vector &y) const { // For a preconditioner M = L*L^T // Calculate y = L^{-T} x preconditioner_.transposeSolve(x, y); } /**********************************************************************************/ -VectorValues buildVectorValues(const Vector &v, - const Ordering &ordering, - const map & dimensions) { +VectorValues buildVectorValues(const Vector &v, const Ordering &ordering, + const map & dimensions) { VectorValues result; DenseIndex offset = 0; - for ( size_t i = 0 ; i < ordering.size() ; ++i ) { + for (size_t i = 0; i < ordering.size(); ++i) { const Key &key = ordering[i]; map::const_iterator it = dimensions.find(key); - if ( it == dimensions.end() ) { - throw invalid_argument("buildVectorValues: inconsistent ordering and dimensions"); + if (it == dimensions.end()) { + throw invalid_argument( + "buildVectorValues: inconsistent ordering and dimensions"); } const size_t dim = it->second; result.insert(key, v.segment(offset, dim)); @@ -137,7 +151,8 @@ VectorValues buildVectorValues(const Vector &v, VectorValues buildVectorValues(const Vector &v, const KeyInfo &keyInfo) { VectorValues result; BOOST_FOREACH ( const KeyInfo::value_type &item, keyInfo ) { - result.insert(item.first, v.segment(item.second.colstart(), item.second.dim())); + result.insert(item.first, + v.segment(item.second.colstart(), item.second.dim())); } return result; } diff --git a/gtsam/linear/PCGSolver.h b/gtsam/linear/PCGSolver.h index 0201000ad..f5b278ae5 100644 --- a/gtsam/linear/PCGSolver.h +++ b/gtsam/linear/PCGSolver.h @@ -1,20 +1,25 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + /* - * PCGSolver.h - * - * Created on: Jan 31, 2012 - * Author: Yong-Dian Jian + * @file PCGSolver.h + * @brief Preconditioned Conjugate Gradient Solver for linear systems + * @date Jan 31, 2012 + * @author Yong-Dian Jian + * @author Sungtae An */ #pragma once -#include #include -#include -#include - -#include -#include -#include #include namespace gtsam { @@ -22,15 +27,19 @@ namespace gtsam { class GaussianFactorGraph; class KeyInfo; class Preconditioner; +class VectorValues; struct PreconditionerParameters; -/*****************************************************************************/ +/** + * Parameters for PCG + */ struct GTSAM_EXPORT PCGSolverParameters: public ConjugateGradientParameters { public: typedef ConjugateGradientParameters Base; typedef boost::shared_ptr shared_ptr; - PCGSolverParameters() {} + PCGSolverParameters() { + } virtual void print(std::ostream &os) const; @@ -42,8 +51,9 @@ public: boost::shared_ptr preconditioner_; }; -/*****************************************************************************/ -/* A virtual base class for the preconditioned conjugate gradient solver */ +/** + * A virtual base class for the preconditioned conjugate gradient solver + */ class GTSAM_EXPORT PCGSolver: public IterativeSolver { public: typedef IterativeSolver Base; @@ -57,7 +67,8 @@ protected: public: /* Interface to initialize a solver without a problem */ PCGSolver(const PCGSolverParameters &p); - virtual ~PCGSolver() {} + virtual ~PCGSolver() { + } using IterativeSolver::optimize; @@ -67,7 +78,9 @@ public: }; -/*****************************************************************************/ +/** + * System class needed for calling preconditionedConjugateGradient + */ class GTSAM_EXPORT GaussianFactorGraphSystem { public: @@ -97,13 +110,17 @@ public: void getb(Vector &b) const; }; -/* utility functions */ -/**********************************************************************************/ +/// @name utility functions +/// @{ + +/// Create VectorValues from a Vector VectorValues buildVectorValues(const Vector &v, const Ordering &ordering, const std::map & dimensions); -/**********************************************************************************/ +/// Create VectorValues from a Vector and a KeyInfo class VectorValues buildVectorValues(const Vector &v, const KeyInfo &keyInfo); +/// @} + } diff --git a/gtsam/linear/SubgraphSolver.cpp b/gtsam/linear/SubgraphSolver.cpp index 6f10d04ad..3c7256c29 100644 --- a/gtsam/linear/SubgraphSolver.cpp +++ b/gtsam/linear/SubgraphSolver.cpp @@ -9,73 +9,81 @@ * -------------------------------------------------------------------------- */ -#include -#include +/** + * @file SubgraphSolver.cpp + * @brief Subgraph Solver from IROS 2010 + * @date 2010 + * @author Frank Dellaert + * @author Yong Dian Jian + */ + +#include #include #include #include #include -#include -#include -#include #include -#include -#include -#include using namespace std; namespace gtsam { /**************************************************************************************************/ -SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &gfg, const Parameters ¶meters, const Ordering& ordering) - : parameters_(parameters), ordering_(ordering) -{ +SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &gfg, + const Parameters ¶meters, const Ordering& ordering) : + parameters_(parameters), ordering_(ordering) { initialize(gfg); } /**************************************************************************************************/ -SubgraphSolver::SubgraphSolver(const GaussianFactorGraph::shared_ptr &jfg, const Parameters ¶meters, const Ordering& ordering) - : parameters_(parameters), ordering_(ordering) -{ +SubgraphSolver::SubgraphSolver(const GaussianFactorGraph::shared_ptr &jfg, + const Parameters ¶meters, const Ordering& ordering) : + parameters_(parameters), ordering_(ordering) { initialize(*jfg); } /**************************************************************************************************/ -SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &Ab1, const GaussianFactorGraph &Ab2, const Parameters ¶meters, const Ordering& ordering) - : parameters_(parameters), ordering_(ordering) { +SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &Ab1, + const GaussianFactorGraph &Ab2, const Parameters ¶meters, + const Ordering& ordering) : + parameters_(parameters), ordering_(ordering) { - GaussianBayesNet::shared_ptr Rc1 = Ab1.eliminateSequential(ordering_, EliminateQR); + GaussianBayesNet::shared_ptr Rc1 = Ab1.eliminateSequential(ordering_, + EliminateQR); initialize(Rc1, boost::make_shared(Ab2)); } /**************************************************************************************************/ SubgraphSolver::SubgraphSolver(const GaussianFactorGraph::shared_ptr &Ab1, - const GaussianFactorGraph::shared_ptr &Ab2, const Parameters ¶meters, const Ordering& ordering) - : parameters_(parameters), ordering_(ordering) { + const GaussianFactorGraph::shared_ptr &Ab2, const Parameters ¶meters, + const Ordering& ordering) : + parameters_(parameters), ordering_(ordering) { - GaussianBayesNet::shared_ptr Rc1 = Ab1->eliminateSequential(ordering_, EliminateQR); + GaussianBayesNet::shared_ptr Rc1 = Ab1->eliminateSequential(ordering_, + EliminateQR); initialize(Rc1, Ab2); } /**************************************************************************************************/ -SubgraphSolver::SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, const GaussianFactorGraph &Ab2, - const Parameters ¶meters, const Ordering& ordering) : parameters_(parameters), ordering_(ordering) -{ +SubgraphSolver::SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, + const GaussianFactorGraph &Ab2, const Parameters ¶meters, + const Ordering& ordering) : + parameters_(parameters), ordering_(ordering) { initialize(Rc1, boost::make_shared(Ab2)); } /**************************************************************************************************/ SubgraphSolver::SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, - const GaussianFactorGraph::shared_ptr &Ab2, const Parameters ¶meters, const Ordering& ordering) : -parameters_(parameters), ordering_(ordering) -{ + const GaussianFactorGraph::shared_ptr &Ab2, const Parameters ¶meters, + const Ordering& ordering) : + parameters_(parameters), ordering_(ordering) { initialize(Rc1, Ab2); } /**************************************************************************************************/ VectorValues SubgraphSolver::optimize() { - VectorValues ybar = conjugateGradients(*pc_, pc_->zero(), parameters_); + VectorValues ybar = conjugateGradients(*pc_, pc_->zero(), parameters_); return pc_->x(ybar); } @@ -86,29 +94,33 @@ VectorValues SubgraphSolver::optimize(const VectorValues &initial) { } /**************************************************************************************************/ -void SubgraphSolver::initialize(const GaussianFactorGraph &jfg) -{ - GaussianFactorGraph::shared_ptr Ab1 = boost::make_shared(), - Ab2 = boost::make_shared(); +void SubgraphSolver::initialize(const GaussianFactorGraph &jfg) { + GaussianFactorGraph::shared_ptr Ab1 = + boost::make_shared(), Ab2 = boost::make_shared< + GaussianFactorGraph>(); - boost::tie(Ab1, Ab2) = splitGraph(jfg) ; + boost::tie(Ab1, Ab2) = splitGraph(jfg); if (parameters_.verbosity()) - cout << "Split A into (A1) " << Ab1->size() << " and (A2) " << Ab2->size() << " factors" << endl; + cout << "Split A into (A1) " << Ab1->size() << " and (A2) " << Ab2->size() + << " factors" << endl; - GaussianBayesNet::shared_ptr Rc1 = Ab1->eliminateSequential(ordering_, EliminateQR); - VectorValues::shared_ptr xbar = boost::make_shared(Rc1->optimize()); + GaussianBayesNet::shared_ptr Rc1 = Ab1->eliminateSequential(ordering_, + EliminateQR); + VectorValues::shared_ptr xbar = boost::make_shared( + Rc1->optimize()); pc_ = boost::make_shared(Ab2, Rc1, xbar); } /**************************************************************************************************/ -void SubgraphSolver::initialize(const GaussianBayesNet::shared_ptr &Rc1, const GaussianFactorGraph::shared_ptr &Ab2) -{ - VectorValues::shared_ptr xbar = boost::make_shared(Rc1->optimize()); +void SubgraphSolver::initialize(const GaussianBayesNet::shared_ptr &Rc1, + const GaussianFactorGraph::shared_ptr &Ab2) { + VectorValues::shared_ptr xbar = boost::make_shared( + Rc1->optimize()); pc_ = boost::make_shared(Ab2, Rc1, xbar); } /**************************************************************************************************/ -boost::tuple +boost::tuple // SubgraphSolver::splitGraph(const GaussianFactorGraph &jfg) { const VariableIndex index(jfg); @@ -116,39 +128,43 @@ SubgraphSolver::splitGraph(const GaussianFactorGraph &jfg) { DSFVector D(n); GaussianFactorGraph::shared_ptr At(new GaussianFactorGraph()); - GaussianFactorGraph::shared_ptr Ac( new GaussianFactorGraph()); + GaussianFactorGraph::shared_ptr Ac(new GaussianFactorGraph()); size_t t = 0; BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, jfg ) { - if ( gf->keys().size() > 2 ) { - throw runtime_error("SubgraphSolver::splitGraph the graph is not simple, sanity check failed "); + if (gf->keys().size() > 2) { + throw runtime_error( + "SubgraphSolver::splitGraph the graph is not simple, sanity check failed "); } - bool augment = false ; + bool augment = false; /* check whether this factor should be augmented to the "tree" graph */ - if ( gf->keys().size() == 1 ) augment = true; + if (gf->keys().size() == 1) + augment = true; else { - const Key u = gf->keys()[0], v = gf->keys()[1], - u_root = D.findSet(u), v_root = D.findSet(v); - if ( u_root != v_root ) { - t++; augment = true ; + const Key u = gf->keys()[0], v = gf->keys()[1], u_root = D.findSet(u), + v_root = D.findSet(v); + if (u_root != v_root) { + t++; + augment = true; D.makeUnionInPlace(u_root, v_root); } } - if ( augment ) At->push_back(gf); - else Ac->push_back(gf); + if (augment) + At->push_back(gf); + else + Ac->push_back(gf); } return boost::tie(At, Ac); } /****************************************************************************/ -VectorValues SubgraphSolver::optimize ( - const GaussianFactorGraph &gfg, - const KeyInfo &keyInfo, - const std::map &lambda, - const VectorValues &initial -) { return VectorValues(); } +VectorValues SubgraphSolver::optimize(const GaussianFactorGraph &gfg, + const KeyInfo &keyInfo, const std::map &lambda, + const VectorValues &initial) { + return VectorValues(); +} } // \namespace gtsam diff --git a/gtsam/linear/SubgraphSolver.h b/gtsam/linear/SubgraphSolver.h index ac8a9da87..318c029f3 100644 --- a/gtsam/linear/SubgraphSolver.h +++ b/gtsam/linear/SubgraphSolver.h @@ -9,27 +9,37 @@ * -------------------------------------------------------------------------- */ +/** + * @file SubgraphSolver.h + * @brief Subgraph Solver from IROS 2010 + * @date 2010 + * @author Frank Dellaert + * @author Yong Dian Jian + */ + #pragma once - #include -#include -#include -#include namespace gtsam { - // Forward declarations - class GaussianFactorGraph; - class GaussianBayesNet; - class SubgraphPreconditioner; +// Forward declarations +class GaussianFactorGraph; +class GaussianBayesNet; +class SubgraphPreconditioner; -class GTSAM_EXPORT SubgraphSolverParameters : public ConjugateGradientParameters { +class GTSAM_EXPORT SubgraphSolverParameters: public ConjugateGradientParameters { public: typedef ConjugateGradientParameters Base; - SubgraphSolverParameters() : Base() {} - void print() const { Base::print(); } - virtual void print(std::ostream &os) const { Base::print(os); } + SubgraphSolverParameters() : + Base() { + } + void print() const { + Base::print(); + } + virtual void print(std::ostream &os) const { + Base::print(os); + } }; /** @@ -53,8 +63,7 @@ public: * * \nosubgrouping */ - -class GTSAM_EXPORT SubgraphSolver : public IterativeSolver { +class GTSAM_EXPORT SubgraphSolver: public IterativeSolver { public: typedef SubgraphSolverParameters Parameters; @@ -62,41 +71,64 @@ public: protected: Parameters parameters_; Ordering ordering_; - boost::shared_ptr pc_; ///< preconditioner object + boost::shared_ptr pc_; ///< preconditioner object public: - /* Given a gaussian factor graph, split it into a spanning tree (A1) + others (A2) for SPCG */ - SubgraphSolver(const GaussianFactorGraph &A, const Parameters ¶meters, const Ordering& ordering); - SubgraphSolver(const boost::shared_ptr &A, const Parameters ¶meters, const Ordering& ordering); - /* The user specify the subgraph part and the constraint part, may throw exception if A1 is underdetermined */ - SubgraphSolver(const GaussianFactorGraph &Ab1, const GaussianFactorGraph &Ab2, const Parameters ¶meters, const Ordering& ordering); - SubgraphSolver(const boost::shared_ptr &Ab1, const boost::shared_ptr &Ab2, const Parameters ¶meters, const Ordering& ordering); + /// Given a gaussian factor graph, split it into a spanning tree (A1) + others (A2) for SPCG + SubgraphSolver(const GaussianFactorGraph &A, const Parameters ¶meters, + const Ordering& ordering); + + /// Shared pointer version + SubgraphSolver(const boost::shared_ptr &A, + const Parameters ¶meters, const Ordering& ordering); + + /** + * The user specify the subgraph part and the constraint part + * may throw exception if A1 is underdetermined + */ + SubgraphSolver(const GaussianFactorGraph &Ab1, const GaussianFactorGraph &Ab2, + const Parameters ¶meters, const Ordering& ordering); + + /// Shared pointer version + SubgraphSolver(const boost::shared_ptr &Ab1, + const boost::shared_ptr &Ab2, + const Parameters ¶meters, const Ordering& ordering); /* The same as above, but the A1 is solved before */ - SubgraphSolver(const boost::shared_ptr &Rc1, const GaussianFactorGraph &Ab2, const Parameters ¶meters, const Ordering& ordering); - SubgraphSolver(const boost::shared_ptr &Rc1, const boost::shared_ptr &Ab2, const Parameters ¶meters, const Ordering& ordering); + SubgraphSolver(const boost::shared_ptr &Rc1, + const GaussianFactorGraph &Ab2, const Parameters ¶meters, + const Ordering& ordering); - virtual ~SubgraphSolver() {} + /// Shared pointer version + SubgraphSolver(const boost::shared_ptr &Rc1, + const boost::shared_ptr &Ab2, + const Parameters ¶meters, const Ordering& ordering); - VectorValues optimize () ; - VectorValues optimize (const VectorValues &initial) ; + /// Destructor + virtual ~SubgraphSolver() { + } - /* interface to the nonlinear optimizer that the subclasses have to implement */ - virtual VectorValues optimize ( - const GaussianFactorGraph &gfg, - const KeyInfo &keyInfo, - const std::map &lambda, - const VectorValues &initial - ) ; + /// Optimize from zero + VectorValues optimize(); + + /// Optimize from given initial values + VectorValues optimize(const VectorValues &initial); + + /** Interface that IterativeSolver subclasses have to implement */ + virtual VectorValues optimize(const GaussianFactorGraph &gfg, + const KeyInfo &keyInfo, const std::map &lambda, + const VectorValues &initial); protected: void initialize(const GaussianFactorGraph &jfg); - void initialize(const boost::shared_ptr &Rc1, const boost::shared_ptr &Ab2); + void initialize(const boost::shared_ptr &Rc1, + const boost::shared_ptr &Ab2); - boost::tuple, boost::shared_ptr > - splitGraph(const GaussianFactorGraph &gfg) ; + boost::tuple, + boost::shared_ptr > + splitGraph(const GaussianFactorGraph &gfg); }; } // namespace gtsam diff --git a/gtsam/slam/JacobianFactorQ.h b/gtsam/slam/JacobianFactorQ.h index 923edddb7..49f5513b6 100644 --- a/gtsam/slam/JacobianFactorQ.h +++ b/gtsam/slam/JacobianFactorQ.h @@ -1,3 +1,14 @@ +/* ---------------------------------------------------------------------------- + + * 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 JacobianFactorQ.h * @date Oct 27, 2013 diff --git a/gtsam/slam/JacobianFactorQR.h b/gtsam/slam/JacobianFactorQR.h index ccd6e8756..112278766 100644 --- a/gtsam/slam/JacobianFactorQR.h +++ b/gtsam/slam/JacobianFactorQR.h @@ -7,8 +7,13 @@ #pragma once #include +#include +#include namespace gtsam { + +class GaussianBayesNet; + /** * JacobianFactor for Schur complement that uses Q noise model */ @@ -38,7 +43,7 @@ public: //gfg.print("gfg"); // eliminate the point - GaussianBayesNet::shared_ptr bn; + boost::shared_ptr bn; GaussianFactorGraph::shared_ptr fg; std::vector < Key > variables; variables.push_back(pointKey); diff --git a/gtsam/slam/JacobianSchurFactor.h b/gtsam/slam/JacobianSchurFactor.h index 5d28bbada..d31eea05b 100644 --- a/gtsam/slam/JacobianSchurFactor.h +++ b/gtsam/slam/JacobianSchurFactor.h @@ -1,3 +1,14 @@ +/* ---------------------------------------------------------------------------- + + * 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 JacobianSchurFactor.h * @brief Jacobianfactor that combines and eliminates points @@ -7,11 +18,7 @@ #pragma once -#include -#include #include -#include -#include #include namespace gtsam { diff --git a/gtsam/slam/RegularImplicitSchurFactor.h b/gtsam/slam/RegularImplicitSchurFactor.h index 75b2d44ba..b1d9cd9f6 100644 --- a/gtsam/slam/RegularImplicitSchurFactor.h +++ b/gtsam/slam/RegularImplicitSchurFactor.h @@ -7,12 +7,10 @@ #pragma once -#include #include #include #include -#include -#include +#include namespace gtsam { diff --git a/gtsam/slam/RegularJacobianFactor.h b/gtsam/slam/RegularJacobianFactor.h index bb275ef3f..97b2ca460 100644 --- a/gtsam/slam/RegularJacobianFactor.h +++ b/gtsam/slam/RegularJacobianFactor.h @@ -52,8 +52,8 @@ public: * factor. */ template RegularJacobianFactor(const KEYS& keys, - const VerticalBlockMatrix& augmentedMatrix, - const SharedDiagonal& sigmas = SharedDiagonal()) : + const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& sigmas = + SharedDiagonal()) : JacobianFactor(keys, augmentedMatrix, sigmas) { } @@ -78,10 +78,10 @@ public: /// Just iterate over all A matrices and multiply in correct config part (looping over keys) /// E.g.: Jacobian A = [A0 A1 A2] multiplies x = [x0 x1 x2]' /// Hence: Ax = A0 x0 + A1 x1 + A2 x2 (hence we loop over the keys and accumulate) - for (size_t pos = 0; pos < size(); ++pos) - { + for (size_t pos = 0; pos < size(); ++pos) { Ax += Ab_(pos) - * ConstMapD(x + accumulatedDims[keys_[pos]], accumulatedDims[keys_[pos] + 1] - accumulatedDims[keys_[pos]]); + * ConstMapD(x + accumulatedDims[keys_[pos]], + accumulatedDims[keys_[pos] + 1] - accumulatedDims[keys_[pos]]); } /// Deal with noise properly, need to Double* whiten as we are dividing by variance if (model_) { @@ -93,8 +93,9 @@ public: Ax *= alpha; /// Again iterate over all A matrices and insert Ai^T into y - for (size_t pos = 0; pos < size(); ++pos){ - MapD(y + accumulatedDims[keys_[pos]], accumulatedDims[keys_[pos] + 1] - accumulatedDims[keys_[pos]]) += Ab_( + for (size_t pos = 0; pos < size(); ++pos) { + MapD(y + accumulatedDims[keys_[pos]], + accumulatedDims[keys_[pos] + 1] - accumulatedDims[keys_[pos]]) += Ab_( pos).transpose() * Ax; } } @@ -102,21 +103,25 @@ public: /** Raw memory access version of multiplyHessianAdd */ void multiplyHessianAdd(double alpha, const double* x, double* y) const { - if (empty()) return; + if (empty()) + return; Vector Ax = zero(Ab_.rows()); // Just iterate over all A matrices and multiply in correct config part - for(size_t pos=0; poswhitenInPlace(Ax); model_->whitenInPlace(Ax); } + if (model_) { + model_->whitenInPlace(Ax); + model_->whitenInPlace(Ax); + } // multiply with alpha Ax *= alpha; // Again iterate over all A matrices and insert Ai^e into y - for(size_t pos=0; poswhiten(column_k); dj(k) = dot(column_k, column_k); - }else{ + } else { dj(k) = Ab_(j).col(k).squaredNorm(); } } @@ -156,13 +161,13 @@ public: } // Just iterate over all A matrices - for (DenseIndex j = 0; j < (DenseIndex)size(); ++j) { + for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) { DVector dj; // gradient -= A'*b/sigma^2 // Computing with each column - for (size_t k = 0; k < D; ++k){ + for (size_t k = 0; k < D; ++k) { Vector column_k = Ab_(j).col(k); - dj(k) = -1.0*dot(b, column_k); + dj(k) = -1.0 * dot(b, column_k); } DMap(d + D * j) += dj; } diff --git a/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp b/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp index 29eaf2ac1..8571a345d 100644 --- a/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp +++ b/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp @@ -1,16 +1,24 @@ +/* ---------------------------------------------------------------------------- + + * 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 testImplicitSchurFactor.cpp + * @file testRegularImplicitSchurFactor.cpp * @brief unit test implicit jacobian factors * @author Frank Dellaert * @date Oct 20, 2013 */ -//#include -#include -//#include #include -//#include "gtsam_unstable/slam/JacobianFactorQR.h" -#include "gtsam/slam/JacobianFactorQR.h" +#include +#include #include #include diff --git a/gtsam/slam/tests/testRegularJacobianFactor.cpp b/gtsam/slam/tests/testRegularJacobianFactor.cpp index b56b65d7b..5803516a1 100644 --- a/gtsam/slam/tests/testRegularJacobianFactor.cpp +++ b/gtsam/slam/tests/testRegularJacobianFactor.cpp @@ -1,3 +1,14 @@ +/* ---------------------------------------------------------------------------- + + * 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 testRegularJacobianFactor.cpp * @brief unit test regular jacobian factors @@ -5,13 +16,13 @@ * @date Nov 12, 2014 */ -#include -#include - #include #include #include #include +#include + +#include #include #include diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index c2855f09b..6a79a331f 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file TestSmartProjectionPoseFactor.cpp + * @file testSmartProjectionPoseFactor.cpp * @brief Unit tests for ProjectionFactor Class * @author Chris Beall * @author Luca Carlone From 5e568bc29db748b080ded69a892635808e874a7e Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 18 Feb 2015 10:09:20 +0100 Subject: [PATCH 27/91] Restored VectorValues versions, they work fine --- .cproject | 4 +-- gtsam/slam/RegularHessianFactor.h | 27 +++++++++---------- gtsam/slam/RegularJacobianFactor.h | 6 +++++ gtsam/slam/tests/testRegularHessianFactor.cpp | 22 +++++++++------ 4 files changed, 34 insertions(+), 25 deletions(-) diff --git a/.cproject b/.cproject index a974f16d4..d2323e19c 100644 --- a/.cproject +++ b/.cproject @@ -1538,10 +1538,10 @@ true true - + make -j4 - testRegularJacobianFactor.run + testRegularHessianFactor.run true true true diff --git a/gtsam/slam/RegularHessianFactor.h b/gtsam/slam/RegularHessianFactor.h index 8c3df87cf..dbe374958 100644 --- a/gtsam/slam/RegularHessianFactor.h +++ b/gtsam/slam/RegularHessianFactor.h @@ -56,13 +56,14 @@ public: mutable std::vector y; /** y += alpha * A'*A*x */ - void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const{ - throw std::runtime_error( - "RegularHessianFactor::forbidden use of multiplyHessianAdd without raw memory access, use HessianFactor instead"); + virtual void multiplyHessianAdd(double alpha, const VectorValues& x, + VectorValues& y) const { + HessianFactor::multiplyHessianAdd(alpha, x, y); } /** y += alpha * A'*A*x */ - void multiplyHessianAdd(double alpha, const double* x, double* yvalues) const { + void multiplyHessianAdd(double alpha, const double* x, + double* yvalues) const { // Create a vector of temporary y values, corresponding to rows i y.resize(size()); BOOST_FOREACH(DVector & yi, y) @@ -95,6 +96,7 @@ public: } } + /// Raw memory version, with offsets TODO document reasoning void multiplyHessianAdd(double alpha, const double* x, double* yvalues, std::vector offsets) const { @@ -131,43 +133,38 @@ public: // copy to yvalues for (DenseIndex i = 0; i < (DenseIndex) size(); ++i) - DMap(yvalues + offsets[keys_[i]], offsets[keys_[i] + 1] - offsets[keys_[i]]) += - alpha * y[i]; + DMap(yvalues + offsets[keys_[i]], + offsets[keys_[i] + 1] - offsets[keys_[i]]) += alpha * y[i]; } /** Return the diagonal of the Hessian for this factor (raw memory version) */ virtual void hessianDiagonal(double* d) const { // Use eigen magic to access raw memory - //typedef Eigen::Matrix DVector; typedef Eigen::Matrix DVector; typedef Eigen::Map DMap; // Loop over all variables in the factor - for (DenseIndex pos = 0; pos < (DenseIndex)size(); ++pos) { + for (DenseIndex pos = 0; pos < (DenseIndex) size(); ++pos) { Key j = keys_[pos]; // Get the diagonal block, and insert its diagonal const Matrix& B = info_(pos, pos).selfadjointView(); - //DMap(d + 9 * j) += B.diagonal(); DMap(d + D * j) += B.diagonal(); } } - /* ************************************************************************* */ - // TODO: currently assumes all variables of the same size 9 and keys arranged from 0 to n + /// Add gradient at zero to d TODO: is it really the goal to add ?? virtual void gradientAtZero(double* d) const { // Use eigen magic to access raw memory - //typedef Eigen::Matrix DVector; typedef Eigen::Matrix DVector; typedef Eigen::Map DMap; // Loop over all variables in the factor - for (DenseIndex pos = 0; pos < (DenseIndex)size(); ++pos) { + for (DenseIndex pos = 0; pos < (DenseIndex) size(); ++pos) { Key j = keys_[pos]; // Get the diagonal block, and insert its diagonal - VectorD dj = -info_(pos,size()).knownOffDiagonal(); - //DMap(d + 9 * j) += dj; + VectorD dj = -info_(pos, size()).knownOffDiagonal(); DMap(d + D * j) += dj; } } diff --git a/gtsam/slam/RegularJacobianFactor.h b/gtsam/slam/RegularJacobianFactor.h index 97b2ca460..dcf709854 100644 --- a/gtsam/slam/RegularJacobianFactor.h +++ b/gtsam/slam/RegularJacobianFactor.h @@ -57,6 +57,12 @@ public: JacobianFactor(keys, augmentedMatrix, sigmas) { } + /** y += alpha * A'*A*x */ + virtual void multiplyHessianAdd(double alpha, const VectorValues& x, + VectorValues& y) const { + JacobianFactor::multiplyHessianAdd(alpha, x, y); + } + /** Raw memory access version of multiplyHessianAdd y += alpha * A'*A*x * Note: this is not assuming a fixed dimension for the variables, * but requires the vector accumulatedDims to tell the dimension of diff --git a/gtsam/slam/tests/testRegularHessianFactor.cpp b/gtsam/slam/tests/testRegularHessianFactor.cpp index 8dfb753f4..e2b8ac3cf 100644 --- a/gtsam/slam/tests/testRegularHessianFactor.cpp +++ b/gtsam/slam/tests/testRegularHessianFactor.cpp @@ -15,11 +15,10 @@ * @date March 4, 2014 */ -#include -#include - -//#include #include +#include + +#include #include #include @@ -29,8 +28,6 @@ using namespace std; using namespace gtsam; using namespace boost::assign; -const double tol = 1e-5; - /* ************************************************************************* */ TEST(RegularHessianFactor, ConstructorNWay) { @@ -77,15 +74,24 @@ TEST(RegularHessianFactor, ConstructorNWay) expected.insert(1, Y.segment<2>(2)); expected.insert(3, Y.segment<2>(4)); + // VectorValues version + double alpha = 1.0; + VectorValues actualVV; + actualVV.insert(0, zero(2)); + actualVV.insert(1, zero(2)); + actualVV.insert(3, zero(2)); + factor.multiplyHessianAdd(alpha, x, actualVV); + EXPECT(assert_equal(expected, actualVV)); + // RAW ACCESS Vector expected_y(8); expected_y << 2633, 2674, 4465, 4501, 0, 0, 5669, 5696; Vector fast_y = gtsam::zero(8); double xvalues[8] = {1,2,3,4,0,0,5,6}; - factor.multiplyHessianAdd(1, xvalues, fast_y.data()); + factor.multiplyHessianAdd(alpha, xvalues, fast_y.data()); EXPECT(assert_equal(expected_y, fast_y)); // now, do it with non-zero y - factor.multiplyHessianAdd(1, xvalues, fast_y.data()); + factor.multiplyHessianAdd(alpha, xvalues, fast_y.data()); EXPECT(assert_equal(2*expected_y, fast_y)); // check some expressions From 9f51aad0fcca568d1df66fe8af3e7589ec01ded8 Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 18 Feb 2015 12:02:33 +0100 Subject: [PATCH 28/91] Cleaned up comments and headers in examples --- examples/Pose2SLAMExampleExpressions.cpp | 10 ++-- examples/Pose2SLAMExample_graph.cpp | 9 ++-- examples/Pose2SLAMExample_graphviz.cpp | 4 +- examples/Pose2SLAMwSPCG.cpp | 69 ++++++------------------ examples/SFMExample.cpp | 8 +-- examples/SFMExample_SmartFactor.cpp | 41 +++----------- 6 files changed, 35 insertions(+), 106 deletions(-) diff --git a/examples/Pose2SLAMExampleExpressions.cpp b/examples/Pose2SLAMExampleExpressions.cpp index 92f94c3f3..1f6de6cb1 100644 --- a/examples/Pose2SLAMExampleExpressions.cpp +++ b/examples/Pose2SLAMExampleExpressions.cpp @@ -14,20 +14,18 @@ * @brief Expressions version of Pose2SLAMExample.cpp * @date Oct 2, 2014 * @author Frank Dellaert - * @author Yong Dian Jian */ // The two new headers that allow using our Automatic Differentiation Expression framework #include #include -// Header order is close to far -#include +// For an explanation of headers below, please see Pose2SLAMExample.cpp +#include +#include +#include #include #include -#include -#include -#include using namespace std; using namespace gtsam; diff --git a/examples/Pose2SLAMExample_graph.cpp b/examples/Pose2SLAMExample_graph.cpp index 46ca02ee4..0c64634c5 100644 --- a/examples/Pose2SLAMExample_graph.cpp +++ b/examples/Pose2SLAMExample_graph.cpp @@ -16,11 +16,14 @@ * @author Frank Dellaert */ -#include +// For an explanation of headers below, please see Pose2SLAMExample.cpp #include -#include +#include #include -#include +#include + +// This new header allows us to read examples easily from .graph files +#include using namespace std; using namespace gtsam; diff --git a/examples/Pose2SLAMExample_graphviz.cpp b/examples/Pose2SLAMExample_graphviz.cpp index 818a9e577..314ccbdb4 100644 --- a/examples/Pose2SLAMExample_graphviz.cpp +++ b/examples/Pose2SLAMExample_graphviz.cpp @@ -16,11 +16,11 @@ * @author Frank Dellaert */ +// For an explanation of headers below, please see Pose2SLAMExample.cpp #include #include -#include -#include #include +#include #include using namespace std; diff --git a/examples/Pose2SLAMwSPCG.cpp b/examples/Pose2SLAMwSPCG.cpp index 4422586b0..2c25d2f8e 100644 --- a/examples/Pose2SLAMwSPCG.cpp +++ b/examples/Pose2SLAMwSPCG.cpp @@ -16,47 +16,15 @@ * @date June 2, 2012 */ -/** - * A simple 2D pose slam example solved using a Conjugate-Gradient method - * - The robot moves in a 2 meter square - * - The robot moves 2 meters each step, turning 90 degrees after each step - * - The robot initially faces along the X axis (horizontal, to the right in 2D) - * - We have full odometry between pose - * - We have a loop closure constraint when the robot returns to the first position - */ - -// As this is a planar SLAM example, we will use Pose2 variables (x, y, theta) to represent -// the robot positions -#include -#include - -// Each variable in the system (poses) must be identified with a unique key. -// We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1). -// Here we will use simple integer keys -#include - -// In GTSAM, measurement functions are represented as 'factors'. Several common factors -// have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems. -// Here we will use Between factors for the relative motion described by odometry measurements. -// We will also use a Between Factor to encode the loop closure constraint -// Also, we will initialize the robot at the origin using a Prior factor. +// For an explanation of headers below, please see Pose2SLAMExample.cpp #include #include - -// When the factors are created, we will add them to a Factor Graph. As the factors we are using -// are nonlinear factors, we will need a Nonlinear Factor Graph. -#include - -// The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the -// nonlinear functions around an initial linearization point, then solve the linear system -// to update the linearization point. This happens repeatedly until the solver converges -// to a consistent set of variable values. This requires us to specify an initial guess -// for each variable, held in a Values container. -#include - -#include +#include #include +// In contrast to that example, however, we will use a PCG solver here +#include + using namespace std; using namespace gtsam; @@ -66,32 +34,24 @@ int main(int argc, char** argv) { NonlinearFactorGraph graph; // 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) Pose2 prior(0.0, 0.0, 0.0); // prior at origin noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); graph.push_back(PriorFactor(1, prior, priorNoise)); // 2b. Add odometry factors - // For simplicity, we will use the same noise model for each odometry factor noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); - // Create odometry (Between) factors between consecutive poses graph.push_back(BetweenFactor(1, 2, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); graph.push_back(BetweenFactor(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); graph.push_back(BetweenFactor(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); graph.push_back(BetweenFactor(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); // 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, with the distance set to zero, noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); graph.push_back(BetweenFactor(5, 1, Pose2(0.0, 0.0, 0.0), 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 Values initialEstimate; initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2)); initialEstimate.insert(2, Pose2(2.3, 0.1, 1.1)); @@ -104,15 +64,18 @@ int main(int argc, char** argv) { LevenbergMarquardtParams parameters; parameters.verbosity = NonlinearOptimizerParams::ERROR; parameters.verbosityLM = LevenbergMarquardtParams::LAMBDA; - parameters.linearSolverType = NonlinearOptimizerParams::Iterative; - { - parameters.iterativeParams = boost::make_shared(); - LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, parameters); - Values result = optimizer.optimize(); - result.print("Final Result:\n"); - cout << "subgraph solver final error = " << graph.error(result) << endl; - } + // LM is still the outer optimization loop, but by specifying "Iterative" below + // We indicate that an iterative linear solver should be used. + // In addition, the *type* of the iterativeParams decides on the type of + // iterative solver, in this case the SPCG (subgraph PCG) + parameters.linearSolverType = NonlinearOptimizerParams::Iterative; + parameters.iterativeParams = boost::make_shared(); + + LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, parameters); + Values result = optimizer.optimize(); + result.print("Final Result:\n"); + cout << "subgraph solver final error = " << graph.error(result) << endl; return 0; } diff --git a/examples/SFMExample.cpp b/examples/SFMExample.cpp index 5be266d11..38dd1ca81 100644 --- a/examples/SFMExample.cpp +++ b/examples/SFMExample.cpp @@ -15,13 +15,7 @@ * @author Duy-Nguyen Ta */ -/** - * A structure-from-motion example with landmarks - * - The landmarks form a 10 meter cube - * - The robot rotates around the landmarks, always facing towards the cube - */ - -// For loading the data +// For loading the data, see the comments therein for scenario (camera rotates around cube) #include "SFMdata.h" // Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y). diff --git a/examples/SFMExample_SmartFactor.cpp b/examples/SFMExample_SmartFactor.cpp index b999e6600..fbd2581ef 100644 --- a/examples/SFMExample_SmartFactor.cpp +++ b/examples/SFMExample_SmartFactor.cpp @@ -17,46 +17,17 @@ * @author Frank Dellaert */ -/** - * A structure-from-motion example with landmarks - * - The landmarks form a 10 meter cube - * - The robot rotates around the landmarks, always facing towards the cube - */ - -// For loading the data -#include "SFMdata.h" - -// Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y). -#include - // In GTSAM, measurement functions are represented as 'factors'. -// The factor we used here is SmartProjectionPoseFactor. Every smart factor represent a single landmark, -// The SmartProjectionPoseFactor only optimize the pose of camera, not the calibration, -// The calibration should be known. +// The factor we used here is SmartProjectionPoseFactor. +// Every smart factor represent a single landmark, seen from multiple cameras. +// The SmartProjectionPoseFactor only optimizes for the poses of a camera, +// not the calibration, which is assumed known. #include -// Also, we will initialize the robot at some location using a Prior factor. -#include - -// When the factors are created, we will add them to a Factor Graph. As the factors we are using -// are nonlinear factors, we will need a Nonlinear Factor Graph. -#include - -// Finally, once all of the factors have been added to our factor graph, we will want to -// solve/optimize to graph to find the best (Maximum A Posteriori) set of variable values. -// GTSAM includes several nonlinear optimizers to perform this step. Here we will use a -// trust-region method known as Powell's Degleg +// For an explanation of these headers, see SFMExample.cpp +#include "SFMdata.h" #include -// The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the -// nonlinear functions around an initial linearization point, then solve the linear system -// to update the linearization point. This happens repeatedly until the solver converges -// to a consistent set of variable values. This requires us to specify an initial guess -// for each variable, held in a Values container. -#include - -#include - using namespace std; using namespace gtsam; From 0914e2c8d5d9aa7c6f70022f14c2df1062ad6bb7 Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 18 Feb 2015 12:15:55 +0100 Subject: [PATCH 29/91] New example with smart PCG --- .cproject | 24 +++-- examples/SFMExample_SmartFactorPCG.cpp | 118 +++++++++++++++++++++++++ 2 files changed, 134 insertions(+), 8 deletions(-) create mode 100644 examples/SFMExample_SmartFactorPCG.cpp diff --git a/.cproject b/.cproject index d2323e19c..f3f62e42d 100644 --- a/.cproject +++ b/.cproject @@ -2303,14 +2303,6 @@ true true - - make - -j2 - Pose2SLAMwSPCG_easy.run - true - true - true - make -j5 @@ -2447,6 +2439,22 @@ true true + + make + -j4 + Pose2SLAMwSPCG.run + true + true + true + + + make + -j4 + SFMExample_SmartFactorPCG.run + true + true + true + make -j2 diff --git a/examples/SFMExample_SmartFactorPCG.cpp b/examples/SFMExample_SmartFactorPCG.cpp new file mode 100644 index 000000000..82d7f62c3 --- /dev/null +++ b/examples/SFMExample_SmartFactorPCG.cpp @@ -0,0 +1,118 @@ +/* ---------------------------------------------------------------------------- + + * 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 SFMExample_SmartFactorPCG.cpp + * @brief Version of SFMExample_SmartFactor that uses Preconditioned Conjugate Gradient + * @author Frank Dellaert + */ + +// For an explanation of these headers, see SFMExample_SmartFactor.cpp +#include "SFMdata.h" +#include + +// These extra headers allow us a LM outer loop with PCG linear solver (inner loop) +#include +#include +#include + +using namespace std; +using namespace gtsam; + +// Make the typename short so it looks much cleaner +typedef gtsam::SmartProjectionPoseFactor SmartFactor; + +/* ************************************************************************* */ +int main(int argc, char* argv[]) { + + // Define the camera calibration parameters + Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)); + + // Define the camera observation noise model + noiseModel::Isotropic::shared_ptr measurementNoise = + noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v + + // Create the set of ground-truth landmarks and poses + vector points = createPoints(); + vector poses = createPoses(); + + // Create a factor graph + NonlinearFactorGraph graph; + + // Simulated measurements from each camera pose, adding them to the factor graph + for (size_t j = 0; j < points.size(); ++j) { + + // every landmark represent a single landmark, we use shared pointer to init the factor, and then insert measurements. + SmartFactor::shared_ptr smartfactor(new SmartFactor()); + + for (size_t i = 0; i < poses.size(); ++i) { + + // generate the 2D measurement + SimpleCamera camera(poses[i], *K); + Point2 measurement = camera.project(points[j]); + + // call add() function to add measurement into a single factor, here we need to add: + smartfactor->add(measurement, i, measurementNoise, K); + } + + // insert the smart factor in the graph + graph.push_back(smartfactor); + } + + // Add a prior on pose x0. This indirectly specifies where the origin is. + // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw + noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas( + (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished()); + graph.push_back(PriorFactor(0, poses[0], poseNoise)); + + // Fix the scale ambiguity by adding a prior + graph.push_back(PriorFactor(1, poses[1], poseNoise)); + + // Create the initial estimate to the solution + Values initialEstimate; + Pose3 delta(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)); + for (size_t i = 0; i < poses.size(); ++i) + initialEstimate.insert(i, poses[i].compose(delta)); + + // We will use LM in the outer optimization loop, but by specifying "Iterative" below + // We indicate that an iterative linear solver should be used. + // In addition, the *type* of the iterativeParams decides on the type of + // iterative solver, in this case the SPCG (subgraph PCG) + LevenbergMarquardtParams parameters; + parameters.linearSolverType = NonlinearOptimizerParams::Iterative; + PCGSolverParameters::shared_ptr pcg = + boost::make_shared(); + pcg->preconditioner_ = + boost::make_shared(); + parameters.iterativeParams = pcg; + + LevenbergMarquardtOptimizer Optimizer(graph, initialEstimate, parameters); + Values result = Optimizer.optimize(); + + // Display result as in SFMExample_SmartFactor.run + result.print("Final results:\n"); + Values landmark_result; + for (size_t j = 0; j < points.size(); ++j) { + SmartFactor::shared_ptr smart = // + boost::dynamic_pointer_cast(graph[j]); + if (smart) { + boost::optional point = smart->point(result); + if (point) // ignore if boost::optional return NULL + landmark_result.insert(j, *point); + } + } + + landmark_result.print("Landmark results:\n"); + + return 0; +} +/* ************************************************************************* */ + From 37cc0acbf798f5d909f03e640200bea9c81924d6 Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 18 Feb 2015 14:24:35 +0100 Subject: [PATCH 30/91] Ignore doxygen docs --- doc/.gitignore | 1 + 1 file changed, 1 insertion(+) create mode 100644 doc/.gitignore diff --git a/doc/.gitignore b/doc/.gitignore new file mode 100644 index 000000000..ac7af2e80 --- /dev/null +++ b/doc/.gitignore @@ -0,0 +1 @@ +/html/ From fd7411c68c892817966c6d0339ae323ce756b106 Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 18 Feb 2015 15:47:06 +0100 Subject: [PATCH 31/91] Making Direct and Iterative solvers comparable --- examples/SFMExample_SmartFactor.cpp | 15 ++++++++------- examples/SFMExample_SmartFactorPCG.cpp | 14 +++++++++++--- 2 files changed, 19 insertions(+), 10 deletions(-) diff --git a/examples/SFMExample_SmartFactor.cpp b/examples/SFMExample_SmartFactor.cpp index fbd2581ef..45a4662d6 100644 --- a/examples/SFMExample_SmartFactor.cpp +++ b/examples/SFMExample_SmartFactor.cpp @@ -26,7 +26,7 @@ // For an explanation of these headers, see SFMExample.cpp #include "SFMdata.h" -#include +#include using namespace std; using namespace gtsam; @@ -98,7 +98,8 @@ int main(int argc, char* argv[]) { initialEstimate.print("Initial Estimates:\n"); // Optimize the graph and print results - Values result = DoglegOptimizer(graph, initialEstimate).optimize(); + LevenbergMarquardtOptimizer optimizer(graph, initialEstimate); + Values result = optimizer.optimize(); result.print("Final results:\n"); // A smart factor represent the 3D point inside the factor, not as a variable. @@ -107,20 +108,20 @@ int main(int argc, char* argv[]) { Values landmark_result; for (size_t j = 0; j < points.size(); ++j) { - // The output of point() is in boost::optional, as sometimes - // the triangulation operation inside smart factor will encounter degeneracy. - boost::optional point; - // The graph stores Factor shared_ptrs, so we cast back to a SmartFactor first SmartFactor::shared_ptr smart = boost::dynamic_pointer_cast(graph[j]); if (smart) { - point = smart->point(result); + // The output of point() is in boost::optional, as sometimes + // the triangulation operation inside smart factor will encounter degeneracy. + boost::optional point = smart->point(result); if (point) // ignore if boost::optional return NULL landmark_result.insert(j, *point); } } landmark_result.print("Landmark results:\n"); + cout << "final error: " << graph.error(result) << endl; + cout << "number of iterations: " << optimizer.iterations() << endl; return 0; } diff --git a/examples/SFMExample_SmartFactorPCG.cpp b/examples/SFMExample_SmartFactorPCG.cpp index 82d7f62c3..472cd9447 100644 --- a/examples/SFMExample_SmartFactorPCG.cpp +++ b/examples/SFMExample_SmartFactorPCG.cpp @@ -21,8 +21,8 @@ // These extra headers allow us a LM outer loop with PCG linear solver (inner loop) #include -#include #include +#include using namespace std; using namespace gtsam; @@ -88,14 +88,20 @@ int main(int argc, char* argv[]) { // iterative solver, in this case the SPCG (subgraph PCG) LevenbergMarquardtParams parameters; parameters.linearSolverType = NonlinearOptimizerParams::Iterative; + parameters.absoluteErrorTol = 1e-10; + parameters.relativeErrorTol = 1e-10; + parameters.maxIterations = 500; PCGSolverParameters::shared_ptr pcg = boost::make_shared(); pcg->preconditioner_ = boost::make_shared(); + // Following is crucial: + pcg->setEpsilon_abs(1e-10); + pcg->setEpsilon_rel(1e-10); parameters.iterativeParams = pcg; - LevenbergMarquardtOptimizer Optimizer(graph, initialEstimate, parameters); - Values result = Optimizer.optimize(); + LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, parameters); + Values result = optimizer.optimize(); // Display result as in SFMExample_SmartFactor.run result.print("Final results:\n"); @@ -111,6 +117,8 @@ int main(int argc, char* argv[]) { } landmark_result.print("Landmark results:\n"); + cout << "final error: " << graph.error(result) << endl; + cout << "number of iterations: " << optimizer.iterations() << endl; return 0; } From 3f437c448b716f18312931ee98179899da910abc Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 19 Feb 2015 01:19:14 +0100 Subject: [PATCH 32/91] Removed dividers, added empty comments --- gtsam/slam/SmartFactorBase.h | 115 ++++++++++++++++++++--------------- 1 file changed, 66 insertions(+), 49 deletions(-) diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index 44d6902fa..88afe2c21 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -35,20 +35,26 @@ #include namespace gtsam { -/// Base class with no internal point, completely functional + +/** + * @brief Base class for smart factors + * This base class has no internal point, but it has a measurement, noise model + * and an optional sensor pose. + */ template class SmartFactorBase: public NonlinearFactor { protected: - // Keep a copy of measurement and calibration for I/O + // Keep a copy of measurement for I/O std::vector measured_; ///< 2D measurement for each of the m views - std::vector noise_; ///< noise model used ///< (important that the order is the same as the keys that we use to create the factor) + std::vector noise_; ///< noise model used + boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame (one for all cameras) - static const int ZDim = traits::dimension; ///< Measurement dimension + static const int ZDim = traits::dimension; ///< Measurement dimension /// Definitions for blocks of F typedef Eigen::Matrix Matrix2D; // F @@ -65,11 +71,8 @@ protected: /// shorthand for this class typedef SmartFactorBase This; - public: - - EIGEN_MAKE_ALIGNED_OPERATOR_NEW /// shorthand for a smart pointer to a factor @@ -107,7 +110,6 @@ public: /** * variant of the previous add: adds a bunch of measurements, together with the camera keys and noises */ - // **************************************************************************************************** void add(std::vector& measurements, std::vector& poseKeys, std::vector& noises) { for (size_t i = 0; i < measurements.size(); i++) { @@ -120,7 +122,6 @@ public: /** * variant of the previous add: adds a bunch of measurements and uses the same noise model for all of them */ - // **************************************************************************************************** void add(std::vector& measurements, std::vector& poseKeys, const SharedNoiseModel& noise) { for (size_t i = 0; i < measurements.size(); i++) { @@ -134,7 +135,6 @@ public: * Adds an entire SfM_track (collection of cameras observing a single point). * The noise is assumed to be the same for all measurements */ - // **************************************************************************************************** template void add(const SFM_TRACK& trackToAdd, const SharedNoiseModel& noise) { for (size_t k = 0; k < trackToAdd.number_measurements(); k++) { @@ -187,8 +187,7 @@ public: && body_P_sensor_->equals(*e->body_P_sensor_))); } - // **************************************************************************************************** -// /// Calculate vector of re-projection errors, before applying noise model + /// Calculate vector of re-projection errors, before applying noise model Vector reprojectionError(const Cameras& cameras, const Point3& point) const { Vector b = zero(ZDim * cameras.size()); @@ -210,7 +209,6 @@ public: return b; } - // **************************************************************************************************** /** * Calculate the error of the factor. * This is the log-likelihood, e.g. \f$ 0.5(h(x)-z)^2/\sigma^2 \f$ in case of Gaussian. @@ -229,7 +227,7 @@ public: try { Z reprojectionError(camera.project(point) - zi); overallError += 0.5 - * this->noise_.at(i)->distance(reprojectionError.vector()); + * this->noise_.at(i)->distance(reprojectionError.vector()); } catch (CheiralityException&) { std::cout << "Cheirality exception " << std::endl; exit(EXIT_FAILURE); @@ -239,7 +237,6 @@ public: return overallError; } - // **************************************************************************************************** /// Assumes non-degenerate ! void computeEP(Matrix& E, Matrix& PointCov, const Cameras& cameras, const Point3& point) const { @@ -264,9 +261,10 @@ public: PointCov.noalias() = (E.transpose() * E).inverse(); } - // **************************************************************************************************** - /// Compute F, E only (called below in both vanilla and SVD versions) - /// Given a Point3, assumes dimensionality is 3 + /** + * Compute F, E only (called below in both vanilla and SVD versions) + * Given a Point3, assumes dimensionality is 3 + */ double computeJacobians(std::vector& Fblocks, Matrix& E, Vector& b, const Cameras& cameras, const Point3& point) const { @@ -280,9 +278,11 @@ public: Vector bi; try { - bi = -(cameras[i].project(point, Fi, Ei, Hcali) - this->measured_.at(i)).vector(); - if(body_P_sensor_){ - Pose3 w_Pose_body = (cameras[i].pose()).compose(body_P_sensor_->inverse()); + bi = + -(cameras[i].project(point, Fi, Ei, Hcali) - this->measured_.at(i)).vector(); + if (body_P_sensor_) { + Pose3 w_Pose_body = (cameras[i].pose()).compose( + body_P_sensor_->inverse()); Matrix J(6, 6); Pose3 world_P_body = w_Pose_body.compose(*body_P_sensor_, J); Fi = Fi * J; @@ -307,7 +307,6 @@ public: return f; } - // **************************************************************************************************** /// Version that computes PointCov, with optional lambda parameter double computeJacobians(std::vector& Fblocks, Matrix& E, Matrix3& PointCov, Vector& b, const Cameras& cameras, const Point3& point, @@ -322,7 +321,7 @@ public: EtE(0, 0) += lambda * EtE(0, 0); EtE(1, 1) += lambda * EtE(1, 1); EtE(2, 2) += lambda * EtE(2, 2); - }else{ + } else { EtE(0, 0) += lambda; EtE(1, 1) += lambda; EtE(2, 2) += lambda; @@ -333,7 +332,6 @@ public: return f; } - // **************************************************************************************************** // TODO, there should not be a Matrix version, really double computeJacobians(Matrix& F, Matrix& E, Matrix3& PointCov, Vector& b, const Cameras& cameras, const Point3& point, @@ -351,7 +349,6 @@ public: return f; } - // **************************************************************************************************** /// SVD version double computeJacobiansSVD(std::vector& Fblocks, Matrix& Enull, Vector& b, const Cameras& cameras, const Point3& point, double lambda = @@ -372,7 +369,6 @@ public: return f; } - // **************************************************************************************************** /// Matrix version of SVD // TODO, there should not be a Matrix version, really double computeJacobiansSVD(Matrix& F, Matrix& Enull, Vector& b, @@ -390,7 +386,6 @@ public: return f; } - // **************************************************************************************************** /// linearize returns a Hessianfactor that is an approximation of error(p) boost::shared_ptr > createHessianFactor( const Cameras& cameras, const Point3& point, const double lambda = 0.0, @@ -433,8 +428,9 @@ public: #endif } - // **************************************************************************************************** - // slow version - works on full (sparse) matrices + /** + * slow version - works on full (sparse) matrices + */ void schurComplement(const std::vector& Fblocks, const Matrix& E, const Matrix& PointCov, const Vector& b, /*output ->*/std::vector& Gs, std::vector& gs) const { @@ -490,7 +486,8 @@ public: // D = (Dx2) * (2) // (augmentedHessian.matrix()).block (i1,numKeys+1) = Fi1.transpose() * b.segment < 2 > (2 * i1); // F' * b - augmentedHessian(i1, numKeys) = Fi1.transpose() * b.segment(ZDim * i1) // F' * b + augmentedHessian(i1, numKeys) = Fi1.transpose() + * b.segment(ZDim * i1) // F' * b - Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (DxZDim) * (ZDimx3) * (3*ZDimm) * (ZDimm x 1) // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) ) @@ -508,7 +505,9 @@ public: } // end of for over cameras } - // **************************************************************************************************** + /** + * + */ void sparseSchurComplement(const std::vector& Fblocks, const Matrix& E, const Matrix& P /*Point Covariance*/, const Vector& b, /*output ->*/std::vector& Gs, std::vector& gs) const { @@ -535,7 +534,7 @@ public: { // for i1 = i2 // D = (Dx2) * (2) gs.at(i1) = Fi1.transpose() * b.segment(ZDim * i1) // F' * b - -Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (DxZDim) * (ZDimx3) * (3*ZDimm) * (ZDimm x 1) + - Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (DxZDim) * (ZDimx3) * (3*ZDimm) * (ZDimm x 1) // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) ) Gs.at(GsIndex) = Fi1.transpose() @@ -554,7 +553,9 @@ public: } // end of for over cameras } - // **************************************************************************************************** + /** + * + */ void updateAugmentedHessian(const Cameras& cameras, const Point3& point, const double lambda, bool diagonalDamping, SymmetricBlockMatrix& augmentedHessian, @@ -569,10 +570,13 @@ public: double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda, diagonalDamping); - updateSparseSchurComplement(Fblocks, E, PointCov, b, f, allKeys, augmentedHessian); // augmentedHessian.matrix().block (i1,i2) = ... + updateSparseSchurComplement(Fblocks, E, PointCov, b, f, allKeys, + augmentedHessian); // augmentedHessian.matrix().block (i1,i2) = ... } - // **************************************************************************************************** + /** + * + */ void updateSparseSchurComplement(const std::vector& Fblocks, const Matrix& E, const Matrix& P /*Point Covariance*/, const Vector& b, const double f, const FastVector allKeys, @@ -584,9 +588,9 @@ public: MatrixDD matrixBlock; typedef SymmetricBlockMatrix::Block Block; ///< A block from the Hessian matrix - FastMap KeySlotMap; - for (size_t slot=0; slot < allKeys.size(); slot++) - KeySlotMap.insert(std::make_pair(allKeys[slot],slot)); + FastMap KeySlotMap; + for (size_t slot = 0; slot < allKeys.size(); slot++) + KeySlotMap.insert(std::make_pair(allKeys[slot], slot)); // a single point is observed in numKeys cameras size_t numKeys = this->keys_.size(); // cameras observing current point @@ -607,7 +611,8 @@ public: // information vector - store previous vector // vectorBlock = augmentedHessian(aug_i1, aug_numKeys).knownOffDiagonal(); // add contribution of current factor - augmentedHessian(aug_i1, aug_numKeys) = augmentedHessian(aug_i1, aug_numKeys).knownOffDiagonal() + augmentedHessian(aug_i1, aug_numKeys) = augmentedHessian(aug_i1, + aug_numKeys).knownOffDiagonal() + Fi1.transpose() * b.segment(ZDim * i1) // F' * b - Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (DxZDim) * (ZDimx3) * (3*ZDimm) * (ZDimm x 1) @@ -615,8 +620,11 @@ public: // main block diagonal - store previous block matrixBlock = augmentedHessian(aug_i1, aug_i1); // add contribution of current factor - augmentedHessian(aug_i1, aug_i1) = matrixBlock + - ( Fi1.transpose() * (Fi1 - Ei1_P * E.block(ZDim * i1, 0).transpose() * Fi1) ); + augmentedHessian(aug_i1, aug_i1) = + matrixBlock + + (Fi1.transpose() + * (Fi1 + - Ei1_P * E.block(ZDim * i1, 0).transpose() * Fi1)); // upper triangular part of the hessian for (size_t i2 = i1 + 1; i2 < numKeys; i2++) { // for each camera @@ -629,15 +637,19 @@ public: // off diagonal block - store previous block // matrixBlock = augmentedHessian(aug_i1, aug_i2).knownOffDiagonal(); // add contribution of current factor - augmentedHessian(aug_i1, aug_i2) = augmentedHessian(aug_i1, aug_i2).knownOffDiagonal() - - Fi1.transpose() * (Ei1_P * E.block(ZDim * i2, 0).transpose() * Fi2); + augmentedHessian(aug_i1, aug_i2) = + augmentedHessian(aug_i1, aug_i2).knownOffDiagonal() + - Fi1.transpose() + * (Ei1_P * E.block(ZDim * i2, 0).transpose() * Fi2); } } // end of for over cameras augmentedHessian(aug_numKeys, aug_numKeys)(0, 0) += f; } - // **************************************************************************************************** + /** + * + */ boost::shared_ptr > createRegularImplicitSchurFactor( const Cameras& cameras, const Point3& point, double lambda = 0.0, bool diagonalDamping = false) const { @@ -649,7 +661,9 @@ public: return f; } - // **************************************************************************************************** + /** + * + */ boost::shared_ptr > createJacobianQFactor( const Cameras& cameras, const Point3& point, double lambda = 0.0, bool diagonalDamping = false) const { @@ -659,18 +673,21 @@ public: Vector b; computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda, diagonalDamping); - return boost::make_shared >(Fblocks, E, PointCov, b); + return boost::make_shared >(Fblocks, E, PointCov, + b); } - // **************************************************************************************************** + /** + * + */ boost::shared_ptr createJacobianSVDFactor( const Cameras& cameras, const Point3& point, double lambda = 0.0) const { size_t numKeys = this->keys_.size(); std::vector Fblocks; Vector b; - Matrix Enull(ZDim*numKeys, ZDim*numKeys-3); + Matrix Enull(ZDim * numKeys, ZDim * numKeys - 3); computeJacobiansSVD(Fblocks, Enull, b, cameras, point, lambda); - return boost::make_shared< JacobianFactorSVD<6, ZDim> >(Fblocks, Enull, b); + return boost::make_shared >(Fblocks, Enull, b); } private: From 319d26312e779b3dbcb6b83b489e251f91b76d34 Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 19 Feb 2015 02:01:36 +0100 Subject: [PATCH 33/91] Better documentation --- gtsam/slam/SmartFactorBase.h | 116 +++++++++++++++++++++-------------- 1 file changed, 71 insertions(+), 45 deletions(-) diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index 88afe2c21..f9f1c6e2b 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -40,6 +40,9 @@ namespace gtsam { * @brief Base class for smart factors * This base class has no internal point, but it has a measurement, noise model * and an optional sensor pose. + * This class mainly computes the derivatives and returns them as a variety of factors. + * The methods take a Cameras argument, which should behave like PinholeCamera, and + * the value of a point, which is kept in the base class. */ template class SmartFactorBase: public NonlinearFactor { @@ -47,9 +50,12 @@ class SmartFactorBase: public NonlinearFactor { protected: // Keep a copy of measurement for I/O - std::vector measured_; ///< 2D measurement for each of the m views - ///< (important that the order is the same as the keys that we use to create the factor) + /** + * 2D measurement and noise model for each of the m views + * The order is kept the same as the keys that we use to create the factor. + */ + std::vector measured_; std::vector noise_; ///< noise model used boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame (one for all cameras) @@ -95,7 +101,7 @@ public: } /** - * add a new measurement and pose key + * Add a new measurement and pose key * @param measured_i is the 2m dimensional projection of a single landmark * @param poseKey is the index corresponding to the camera observing the landmark * @param noise_i is the measurement noise @@ -108,7 +114,7 @@ public: } /** - * variant of the previous add: adds a bunch of measurements, together with the camera keys and noises + * Add a bunch of measurements, together with the camera keys and noises */ void add(std::vector& measurements, std::vector& poseKeys, std::vector& noises) { @@ -120,7 +126,7 @@ public: } /** - * variant of the previous add: adds a bunch of measurements and uses the same noise model for all of them + * Add a bunch of measurements and uses the same noise model for all of them */ void add(std::vector& measurements, std::vector& poseKeys, const SharedNoiseModel& noise) { @@ -149,7 +155,7 @@ public: return measured_; } - /** return the noise model */ + /** return the noise models */ const std::vector& noise() const { return noise_; } @@ -237,7 +243,11 @@ public: return overallError; } - /// Assumes non-degenerate ! + /** + * Compute the matrices E and PointCov = inv(E'*E), where E stacks the ZDim*3 derivatives + * of project with respect to the point, given each of the m cameras. + * Assumes non-degenerate ! + */ void computeEP(Matrix& E, Matrix& PointCov, const Cameras& cameras, const Point3& point) const { @@ -262,8 +272,12 @@ public: } /** - * Compute F, E only (called below in both vanilla and SVD versions) - * Given a Point3, assumes dimensionality is 3 + * Compute F, E, and b (called below in both vanilla and SVD versions), where + * F is a vector of derivatives wrpt the cameras, and E the stacked derivatives + * with respect to the point. The value of cameras/point are passed as parameters. + * Given a Point3, assumes dimensionality is 3. + * TODO We assume below the dimensionality of POSE is 6. Frank thinks the templating + * of this factor is only for show, and should just assume a PinholeCamera. */ double computeJacobians(std::vector& Fblocks, Matrix& E, Vector& b, const Cameras& cameras, const Point3& point) const { @@ -273,13 +287,15 @@ public: b = zero(ZDim * numKeys); double f = 0; - Matrix Fi(ZDim, 6), Ei(ZDim, 3), Hcali(ZDim, D - 6), Hcam(ZDim, D); + Matrix Fi(ZDim, 6), Ei(ZDim, 3), Hcali(ZDim, D - 6); for (size_t i = 0; i < this->measured_.size(); i++) { Vector bi; try { - bi = - -(cameras[i].project(point, Fi, Ei, Hcali) - this->measured_.at(i)).vector(); + const Z& zi = this->measured_.at(i); + bi = -(cameras[i].project(point, Fi, Ei, Hcali) - zi).vector(); + + // if we have a sensor offset, correct derivatives if (body_P_sensor_) { Pose3 w_Pose_body = (cameras[i].pose()).compose( body_P_sensor_->inverse()); @@ -289,14 +305,18 @@ public: } } catch (CheiralityException&) { std::cout << "Cheirality exception " << std::endl; - exit(EXIT_FAILURE); + exit(EXIT_FAILURE); // TODO: throw exception } + + // Whiten derivatives according to noise parameters this->noise_.at(i)->WhitenSystem(Fi, Ei, Hcali, bi); f += bi.squaredNorm(); - if (D == 6) { // optimize only camera pose + // TODO: if D==6 we optimize only camera pose. What's up with that ??? + if (D == 6) { Fblocks.push_back(KeyMatrix2D(this->keys_[i], Fi)); } else { + Matrix Hcam(ZDim, D); Hcam.block(0, 0) = Fi; // ZDim x 6 block for the cameras Hcam.block(0, 6) = Hcali; // ZDim x nrCal block for the cameras Fblocks.push_back(KeyMatrix2D(this->keys_[i], Hcam)); @@ -386,7 +406,9 @@ public: return f; } - /// linearize returns a Hessianfactor that is an approximation of error(p) + /** + * Linearize returns a Hessianfactor that is an approximation of error(p) + */ boost::shared_ptr > createHessianFactor( const Cameras& cameras, const Point3& point, const double lambda = 0.0, bool diagonalDamping = false) const { @@ -412,8 +434,7 @@ public: //std::vector < Matrix > Gs2(Gs.begin(), Gs.end()); //std::vector < Vector > gs2(gs.begin(), gs.end()); - return boost::make_shared < RegularHessianFactor - > (this->keys_, Gs, gs, f); + return boost::make_shared < RegularHessianFactor > (this->keys_, Gs, gs, f); #else // we create directly a SymmetricBlockMatrix size_t n1 = D * numKeys + 1; std::vector dims(numKeys + 1); // this also includes the b term @@ -429,7 +450,8 @@ public: } /** - * slow version - works on full (sparse) matrices + * Do Schur complement, given Jacobian as F,E,PointCov. + * Slow version - works on full matrices */ void schurComplement(const std::vector& Fblocks, const Matrix& E, const Matrix& PointCov, const Vector& b, @@ -467,7 +489,10 @@ public: } } - // **************************************************************************************************** + /** + * Do Schur complement, given Jacobian as F,E,PointCov, return SymmetricBlockMatrix + * Fast version - works on with sparsity + */ void sparseSchurComplement(const std::vector& Fblocks, const Matrix& E, const Matrix& P /*Point Covariance*/, const Vector& b, /*output ->*/SymmetricBlockMatrix& augmentedHessian) const { @@ -506,7 +531,8 @@ public: } /** - * + * Do Schur complement, given Jacobian as F,E,PointCov, return Gs/gs + * Fast version - works on with sparsity */ void sparseSchurComplement(const std::vector& Fblocks, const Matrix& E, const Matrix& P /*Point Covariance*/, const Vector& b, @@ -554,28 +580,7 @@ public: } /** - * - */ - void updateAugmentedHessian(const Cameras& cameras, const Point3& point, - const double lambda, bool diagonalDamping, - SymmetricBlockMatrix& augmentedHessian, - const FastVector allKeys) const { - - // int numKeys = this->keys_.size(); - - std::vector Fblocks; - Matrix E; - Matrix3 PointCov; - Vector b; - double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda, - diagonalDamping); - - updateSparseSchurComplement(Fblocks, E, PointCov, b, f, allKeys, - augmentedHessian); // augmentedHessian.matrix().block (i1,i2) = ... - } - - /** - * + * No idea. TODO Luca should document */ void updateSparseSchurComplement(const std::vector& Fblocks, const Matrix& E, const Matrix& P /*Point Covariance*/, const Vector& b, @@ -648,7 +653,28 @@ public: } /** - * + * No idea. TODO Luca should document + */ + void updateAugmentedHessian(const Cameras& cameras, const Point3& point, + const double lambda, bool diagonalDamping, + SymmetricBlockMatrix& augmentedHessian, + const FastVector allKeys) const { + + // int numKeys = this->keys_.size(); + + std::vector Fblocks; + Matrix E; + Matrix3 PointCov; + Vector b; + double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda, + diagonalDamping); + + updateSparseSchurComplement(Fblocks, E, PointCov, b, f, allKeys, + augmentedHessian); // augmentedHessian.matrix().block (i1,i2) = ... + } + + /** + * Return Jacobians as RegularImplicitSchurFactor with raw access */ boost::shared_ptr > createRegularImplicitSchurFactor( const Cameras& cameras, const Point3& point, double lambda = 0.0, @@ -662,7 +688,7 @@ public: } /** - * + * Return Jacobians as JacobianFactorQ */ boost::shared_ptr > createJacobianQFactor( const Cameras& cameras, const Point3& point, double lambda = 0.0, @@ -678,7 +704,7 @@ public: } /** - * + * Return Jacobians as JacobianFactor */ boost::shared_ptr createJacobianSVDFactor( const Cameras& cameras, const Point3& point, double lambda = 0.0) const { From 7c1a795cb461a15a89a3e704273effcfaf87f336 Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 19 Feb 2015 10:54:25 +0100 Subject: [PATCH 34/91] Fixed bug in dim (it should be #rows not #columns!) and moved to base class --- gtsam/slam/SmartFactorBase.h | 11 +++++++---- gtsam/slam/SmartProjectionPoseFactor.h | 5 ----- gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h | 5 ----- 3 files changed, 7 insertions(+), 14 deletions(-) diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index f9f1c6e2b..ec5bed1b0 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -25,10 +25,8 @@ #include #include -#include // for Cheirality exception -#include -#include -#include +#include +#include // for Cheirality exception #include #include @@ -150,6 +148,11 @@ public: } } + /// get the dimension (number of rows!) of the factor + virtual size_t dim() const { + return ZDim * this->measured_.size(); + } + /** return the measurements */ const std::vector& measured() const { return measured_; diff --git a/gtsam/slam/SmartProjectionPoseFactor.h b/gtsam/slam/SmartProjectionPoseFactor.h index 0801d141f..ac0951e87 100644 --- a/gtsam/slam/SmartProjectionPoseFactor.h +++ b/gtsam/slam/SmartProjectionPoseFactor.h @@ -141,11 +141,6 @@ public: return e && Base::equals(p, tol); } - /// get the dimension of the factor - virtual size_t dim() const { - return 6 * this->keys_.size(); - } - /** * Collect all cameras involved in this factor * @param values Values structure which must contain camera poses corresponding diff --git a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h index 2e3bc866b..6a6d55988 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h @@ -143,11 +143,6 @@ public: return e && Base::equals(p, tol); } - /// get the dimension of the factor - virtual size_t dim() const { - return 6 * this->keys_.size(); - } - /** * Collect all cameras involved in this factor * @param values Values structure which must contain camera poses corresponding From a7370818fad722ea6768a135c4d8c18f8552fe35 Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 19 Feb 2015 10:54:55 +0100 Subject: [PATCH 35/91] Added unit test for SmartFactorBase (which immediatly exposed dim bug!) --- .cproject | 8 +++ gtsam/slam/tests/testSmartFactorBase.cpp | 72 ++++++++++++++++++++++++ 2 files changed, 80 insertions(+) create mode 100644 gtsam/slam/tests/testSmartFactorBase.cpp diff --git a/.cproject b/.cproject index f3f62e42d..0bef605c5 100644 --- a/.cproject +++ b/.cproject @@ -1546,6 +1546,14 @@ true true + + make + -j4 + testSmartFactorBase.run + true + true + true + make -j3 diff --git a/gtsam/slam/tests/testSmartFactorBase.cpp b/gtsam/slam/tests/testSmartFactorBase.cpp new file mode 100644 index 000000000..618644981 --- /dev/null +++ b/gtsam/slam/tests/testSmartFactorBase.cpp @@ -0,0 +1,72 @@ +/* ---------------------------------------------------------------------------- + + * 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 testSmartFactorBase.cpp + * @brief Unit tests for testSmartFactorBase Class + * @author Frank Dellaert + * @date Feb 2015 + */ + +#include "../SmartFactorBase.h" +#include +#include + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +#include +#include +class PinholeFactor: public SmartFactorBase, 9> { + virtual double error(const Values& values) const { + return 0.0; + } + virtual boost::shared_ptr linearize( + const Values& values) const { + return boost::shared_ptr(new JacobianFactor()); + } +}; + +TEST(SmartFactorBase, Pinhole) { + PinholeFactor f; + f.add(Point2(),1,SharedNoiseModel()); + f.add(Point2(),2,SharedNoiseModel()); + EXPECT_LONGS_EQUAL(2*2,f.dim()); +} + +/* ************************************************************************* */ +#include +class StereoFactor: public SmartFactorBase { + virtual double error(const Values& values) const { + return 0.0; + } + virtual boost::shared_ptr linearize( + const Values& values) const { + return boost::shared_ptr(new JacobianFactor()); + } +}; + +TEST(SmartFactorBase, Stereo) { + StereoFactor f; + f.add(StereoPoint2(),1,SharedNoiseModel()); + f.add(StereoPoint2(),2,SharedNoiseModel()); + EXPECT_LONGS_EQUAL(2*3,f.dim()); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + From 454aec4554850ddce798ebd5e813a40f80c1a78d Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 19 Feb 2015 12:23:49 +0100 Subject: [PATCH 36/91] Deal with forward declaration. --- tests/testPreconditioner.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/tests/testPreconditioner.cpp b/tests/testPreconditioner.cpp index 89c3d5cf8..f00916475 100644 --- a/tests/testPreconditioner.cpp +++ b/tests/testPreconditioner.cpp @@ -18,11 +18,12 @@ #include -#include -#include #include -#include +#include +#include +#include #include +#include using namespace std; using namespace gtsam; From 370b447e7549e5a8500da8b1972072670ae7d40b Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 19 Feb 2015 12:24:02 +0100 Subject: [PATCH 37/91] Add Measurement typedef --- gtsam/geometry/PinholeCamera.h | 8 ++++++++ gtsam/geometry/StereoCamera.h | 8 ++++++++ 2 files changed, 16 insertions(+) diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index 3b4858a4a..2af7d622b 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -32,6 +32,14 @@ namespace gtsam { template class PinholeCamera { +public: + + /** + * Some classes template on either PinholeCamera or StereoCamera, + * and this typedef informs those classes what "project" returns. + */ + typedef Point2 Measurement; + private: Pose3 pose_; Calibration K_; diff --git a/gtsam/geometry/StereoCamera.h b/gtsam/geometry/StereoCamera.h index 913b1eab3..8d16de4dd 100644 --- a/gtsam/geometry/StereoCamera.h +++ b/gtsam/geometry/StereoCamera.h @@ -38,6 +38,14 @@ public: */ class GTSAM_EXPORT StereoCamera { +public: + + /** + * Some classes template on either PinholeCamera or StereoCamera, + * and this typedef informs those classes what "project" returns. + */ + typedef StereoPoint2 Measurement; + private: Pose3 leftCamPose_; Cal3_S2Stereo::shared_ptr K_; From 8a64d5bffe5560e1169cffe18bdcbd5e66736b06 Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 19 Feb 2015 12:27:40 +0100 Subject: [PATCH 38/91] Got rid of template parameters POSE (which was a fiction) and Z (which is now derived as CAMERA::Measurement). --- gtsam.h | 10 +++---- gtsam/slam/SmartFactorBase.h | 15 +++++----- gtsam/slam/SmartProjectionFactor.h | 30 ++++++++----------- gtsam/slam/SmartProjectionPoseFactor.h | 16 +++++----- gtsam/slam/tests/testSmartFactorBase.cpp | 17 +++++------ .../tests/testSmartProjectionPoseFactor.cpp | 6 ++-- .../slam/SmartStereoProjectionFactor.h | 19 ++++++------ .../slam/SmartStereoProjectionPoseFactor.h | 16 +++++----- .../testSmartStereoProjectionPoseFactor.cpp | 4 +-- 9 files changed, 64 insertions(+), 69 deletions(-) diff --git a/gtsam.h b/gtsam.h index 2aa20c278..43bcd86be 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2301,23 +2301,23 @@ virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor { }; #include -template +template virtual class SmartProjectionPoseFactor : gtsam::NonlinearFactor { SmartProjectionPoseFactor(double rankTol, double linThreshold, - bool manageDegeneracy, bool enableEPI, const POSE& body_P_sensor); + bool manageDegeneracy, bool enableEPI, const gtsam::Pose3& body_P_sensor); SmartProjectionPoseFactor(double rankTol); SmartProjectionPoseFactor(); - void add(const gtsam::Point2& measured_i, size_t poseKey_i, const gtsam::noiseModel::Base* noise_i, - const CALIBRATION* K_i); + void add(const gtsam::Point2& measured_i, size_t poseKey_i, + const gtsam::noiseModel::Base* noise_i, const CALIBRATION* K_i); // enabling serialization functionality //void serialize() const; }; -typedef gtsam::SmartProjectionPoseFactor SmartProjectionPose3Factor; +typedef gtsam::SmartProjectionPoseFactor SmartProjectionPose3Factor; #include diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index ec5bed1b0..3bb66e57f 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -42,7 +42,7 @@ namespace gtsam { * The methods take a Cameras argument, which should behave like PinholeCamera, and * the value of a point, which is kept in the base class. */ -template +template class SmartFactorBase: public NonlinearFactor { protected: @@ -53,10 +53,11 @@ protected: * 2D measurement and noise model for each of the m views * The order is kept the same as the keys that we use to create the factor. */ + typedef typename CAMERA::Measurement Z; std::vector measured_; std::vector noise_; ///< noise model used - boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame (one for all cameras) + boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame (one for all cameras) static const int ZDim = traits::dimension; ///< Measurement dimension @@ -73,7 +74,7 @@ protected: typedef NonlinearFactor Base; /// shorthand for this class - typedef SmartFactorBase This; + typedef SmartFactorBase This; public: @@ -90,7 +91,7 @@ public: * Constructor * @param body_P_sensor is the transform from sensor to body frame (default identity) */ - SmartFactorBase(boost::optional body_P_sensor = boost::none) : + SmartFactorBase(boost::optional body_P_sensor = boost::none) : body_P_sensor_(body_P_sensor) { } @@ -279,7 +280,7 @@ public: * F is a vector of derivatives wrpt the cameras, and E the stacked derivatives * with respect to the point. The value of cameras/point are passed as parameters. * Given a Point3, assumes dimensionality is 3. - * TODO We assume below the dimensionality of POSE is 6. Frank thinks the templating + * TODO We assume below the dimensionality of Pose3 is 6. Frank thinks the templating * of this factor is only for show, and should just assume a PinholeCamera. */ double computeJacobians(std::vector& Fblocks, Matrix& E, @@ -731,7 +732,7 @@ private: } }; -template -const int SmartFactorBase::ZDim; +template +const int SmartFactorBase::ZDim; } // \ namespace gtsam diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index 2bfcbfaa0..6b86b0b1e 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -58,11 +58,10 @@ enum LinearizationMode { }; /** - * SmartProjectionFactor: triangulates point - * TODO: why LANDMARK parameter? + * SmartProjectionFactor: triangulates point and keeps an estimate of it around. */ -template -class SmartProjectionFactor: public SmartFactorBase, D> { +template +class SmartProjectionFactor: public SmartFactorBase, D> { protected: // Some triangulation parameters @@ -92,7 +91,7 @@ protected: typedef boost::shared_ptr SmartFactorStatePtr; /// shorthand for base class type - typedef SmartFactorBase, D> Base; + typedef SmartFactorBase, D> Base; double landmarkDistanceThreshold_; // if the landmark is triangulated at a // distance larger than that the factor is considered degenerate @@ -102,9 +101,7 @@ protected: // and the factor is disregarded if the error is large /// shorthand for this class - typedef SmartProjectionFactor This; - - static const int ZDim = 2; ///< Measurement dimension + typedef SmartProjectionFactor This; public: @@ -126,7 +123,7 @@ public: */ SmartProjectionFactor(const double rankTol, const double linThreshold, const bool manageDegeneracy, const bool enableEPI, - boost::optional body_P_sensor = boost::none, + boost::optional body_P_sensor = boost::none, double landmarkDistanceThreshold = 1e10, double dynamicOutlierRejectionThreshold = -1, SmartFactorStatePtr state = SmartFactorStatePtr(new SmartProjectionFactorState())) : @@ -338,7 +335,7 @@ public: || (!this->manageDegeneracy_ && (this->cheiralityException_ || this->degenerate_))) { // std::cout << "In linearize: exception" << std::endl; - BOOST_FOREACH(gtsam::Matrix& m, Gs) + BOOST_FOREACH(Matrix& m, Gs) m = zeros(D, D); BOOST_FOREACH(Vector& v, gs) v = zero(D); @@ -420,16 +417,16 @@ public: } /// create factor - boost::shared_ptr > createJacobianQFactor( + boost::shared_ptr > createJacobianQFactor( const Cameras& cameras, double lambda) const { if (triangulateForLinearize(cameras)) return Base::createJacobianQFactor(cameras, point_, lambda); else - return boost::make_shared< JacobianFactorQ >(this->keys_); + return boost::make_shared< JacobianFactorQ >(this->keys_); } /// Create a factor, takes values - boost::shared_ptr > createJacobianQFactor( + boost::shared_ptr > createJacobianQFactor( const Values& values, double lambda) const { Cameras myCameras; // TODO triangulate twice ?? @@ -437,7 +434,7 @@ public: if (nonDegenerate) return createJacobianQFactor(myCameras, lambda); else - return boost::make_shared< JacobianFactorQ >(this->keys_); + return boost::make_shared< JacobianFactorQ >(this->keys_); } /// different (faster) way to compute Jacobian factor @@ -446,7 +443,7 @@ public: if (triangulateForLinearize(cameras)) return Base::createJacobianSVDFactor(cameras, point_, lambda); else - return boost::make_shared< JacobianFactorSVD >(this->keys_); + return boost::make_shared< JacobianFactorSVD >(this->keys_); } /// Returns true if nonDegenerate @@ -709,7 +706,4 @@ private: } }; -template -const int SmartProjectionFactor::ZDim; - } // \ namespace gtsam diff --git a/gtsam/slam/SmartProjectionPoseFactor.h b/gtsam/slam/SmartProjectionPoseFactor.h index ac0951e87..70476ba3e 100644 --- a/gtsam/slam/SmartProjectionPoseFactor.h +++ b/gtsam/slam/SmartProjectionPoseFactor.h @@ -37,8 +37,8 @@ namespace gtsam { * The calibration is known here. The factor only constraints poses (variable dimension is 6) * @addtogroup SLAM */ -template -class SmartProjectionPoseFactor: public SmartProjectionFactor { +template +class SmartProjectionPoseFactor: public SmartProjectionFactor { protected: LinearizationMode linearizeTo_; ///< How to linearize the factor (HESSIAN, JACOBIAN_SVD, JACOBIAN_Q) @@ -48,10 +48,10 @@ protected: public: /// shorthand for base class type - typedef SmartProjectionFactor Base; + typedef SmartProjectionFactor Base; /// shorthand for this class - typedef SmartProjectionPoseFactor This; + typedef SmartProjectionPoseFactor This; /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; @@ -67,7 +67,7 @@ public: */ SmartProjectionPoseFactor(const double rankTol = 1, const double linThreshold = -1, const bool manageDegeneracy = false, - const bool enableEPI = false, boost::optional body_P_sensor = boost::none, + const bool enableEPI = false, boost::optional body_P_sensor = boost::none, LinearizationMode linearizeTo = HESSIAN, double landmarkDistanceThreshold = 1e10, double dynamicOutlierRejectionThreshold = -1) : Base(rankTol, linThreshold, manageDegeneracy, enableEPI, body_P_sensor, @@ -211,7 +211,9 @@ private: }; // end of class declaration /// traits -template -struct traits > : public Testable > {}; +template +struct traits > : public Testable< + SmartProjectionPoseFactor > { +}; } // \ namespace gtsam diff --git a/gtsam/slam/tests/testSmartFactorBase.cpp b/gtsam/slam/tests/testSmartFactorBase.cpp index 618644981..b5ef18842 100644 --- a/gtsam/slam/tests/testSmartFactorBase.cpp +++ b/gtsam/slam/tests/testSmartFactorBase.cpp @@ -26,8 +26,7 @@ using namespace gtsam; /* ************************************************************************* */ #include #include -class PinholeFactor: public SmartFactorBase, 9> { +class PinholeFactor: public SmartFactorBase, 9> { virtual double error(const Values& values) const { return 0.0; } @@ -39,14 +38,14 @@ class PinholeFactor: public SmartFactorBase -class StereoFactor: public SmartFactorBase { +class StereoFactor: public SmartFactorBase { virtual double error(const Values& values) const { return 0.0; } @@ -58,9 +57,9 @@ class StereoFactor: public SmartFactorBase TEST(SmartFactorBase, Stereo) { StereoFactor f; - f.add(StereoPoint2(),1,SharedNoiseModel()); - f.add(StereoPoint2(),2,SharedNoiseModel()); - EXPECT_LONGS_EQUAL(2*3,f.dim()); + f.add(StereoPoint2(), 1, SharedNoiseModel()); + f.add(StereoPoint2(), 2, SharedNoiseModel()); + EXPECT_LONGS_EQUAL(2 * 3, f.dim()); } /* ************************************************************************* */ diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index 6a79a331f..99df9f273 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -60,8 +60,8 @@ static Key poseKey1(x1); static Point2 measurement1(323.0, 240.0); static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); -typedef SmartProjectionPoseFactor SmartFactor; -typedef SmartProjectionPoseFactor SmartFactorBundler; +typedef SmartProjectionPoseFactor SmartFactor; +typedef SmartProjectionPoseFactor SmartFactorBundler; void projectToMultipleCameras( SimpleCamera cam1, SimpleCamera cam2, SimpleCamera cam3, Point3 landmark, @@ -1202,7 +1202,7 @@ TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ){ /* ************************************************************************* */ TEST( SmartProjectionPoseFactor, ConstructorWithCal3Bundler) { - SmartProjectionPoseFactor factor1(rankTol, linThreshold); + SmartFactorBundler factor1(rankTol, linThreshold); boost::shared_ptr Kbundler(new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000)); factor1.add(measurement1, poseKey1, model, Kbundler); } diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index 5d2ba3323..aeefe6c58 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -62,10 +62,9 @@ enum LinearizationMode { /** * SmartStereoProjectionFactor: triangulates point - * TODO: why LANDMARK parameter? */ -template -class SmartStereoProjectionFactor: public SmartFactorBase { +template +class SmartStereoProjectionFactor: public SmartFactorBase { protected: // Some triangulation parameters @@ -95,7 +94,7 @@ protected: typedef boost::shared_ptr SmartFactorStatePtr; /// shorthand for base class type - typedef SmartFactorBase Base; + typedef SmartFactorBase Base; double landmarkDistanceThreshold_; // if the landmark is triangulated at a // distance larger than that the factor is considered degenerate @@ -105,7 +104,7 @@ protected: // and the factor is disregarded if the error is large /// shorthand for this class - typedef SmartStereoProjectionFactor This; + typedef SmartStereoProjectionFactor This; enum {ZDim = 3}; ///< Dimension trait of measurement type @@ -131,7 +130,7 @@ public: */ SmartStereoProjectionFactor(const double rankTol, const double linThreshold, const bool manageDegeneracy, const bool enableEPI, - boost::optional body_P_sensor = boost::none, + boost::optional body_P_sensor = boost::none, double landmarkDistanceThreshold = 1e10, double dynamicOutlierRejectionThreshold = -1, SmartFactorStatePtr state = SmartFactorStatePtr(new SmartStereoProjectionFactorState())) : @@ -370,7 +369,7 @@ public: || (!this->manageDegeneracy_ && (this->cheiralityException_ || this->degenerate_))) { if (isDebug) std::cout << "In linearize: exception" << std::endl; - BOOST_FOREACH(gtsam::Matrix& m, Gs) + BOOST_FOREACH(Matrix& m, Gs) m = zeros(D, D); BOOST_FOREACH(Vector& v, gs) v = zero(D); @@ -746,9 +745,9 @@ private: }; /// traits -template -struct traits > : - public Testable > { +template +struct traits > : + public Testable > { }; } // \ namespace gtsam diff --git a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h index 6a6d55988..3e0c6476f 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h @@ -37,8 +37,8 @@ namespace gtsam { * The calibration is known here. The factor only constraints poses (variable dimension is 6) * @addtogroup SLAM */ -template -class SmartStereoProjectionPoseFactor: public SmartStereoProjectionFactor { +template +class SmartStereoProjectionPoseFactor: public SmartStereoProjectionFactor { protected: LinearizationMode linearizeTo_; ///< How to linearize the factor (HESSIAN, JACOBIAN_SVD, JACOBIAN_Q) @@ -50,10 +50,10 @@ public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW /// shorthand for base class type - typedef SmartStereoProjectionFactor Base; + typedef SmartStereoProjectionFactor Base; /// shorthand for this class - typedef SmartStereoProjectionPoseFactor This; + typedef SmartStereoProjectionPoseFactor This; /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; @@ -69,7 +69,7 @@ public: */ SmartStereoProjectionPoseFactor(const double rankTol = 1, const double linThreshold = -1, const bool manageDegeneracy = false, - const bool enableEPI = false, boost::optional body_P_sensor = boost::none, + const bool enableEPI = false, boost::optional body_P_sensor = boost::none, LinearizationMode linearizeTo = HESSIAN, double landmarkDistanceThreshold = 1e10, double dynamicOutlierRejectionThreshold = -1) : Base(rankTol, linThreshold, manageDegeneracy, enableEPI, body_P_sensor, @@ -211,9 +211,9 @@ private: }; // end of class declaration /// traits -template -struct traits > : - public Testable > { +template +struct traits > : public Testable< + SmartStereoProjectionPoseFactor > { }; } // \ namespace gtsam diff --git a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp index 4691da384..bab9712c2 100644 --- a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp @@ -62,8 +62,8 @@ static Key poseKey1(x1); static StereoPoint2 measurement1(323.0, 300.0, 240.0); //potentially use more reasonable measurement value? static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); -typedef SmartStereoProjectionPoseFactor SmartFactor; -typedef SmartStereoProjectionPoseFactor SmartFactorBundler; +typedef SmartStereoProjectionPoseFactor SmartFactor; +typedef SmartStereoProjectionPoseFactor SmartFactorBundler; vector stereo_projectToMultipleCameras( const StereoCamera& cam1, const StereoCamera& cam2, From 2d014b45223da517c71fe6fabdeee9b905726042 Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 19 Feb 2015 13:30:41 +0100 Subject: [PATCH 39/91] First draft of CameraSet --- .cproject | 8 ++ gtsam/geometry/CameraSet.h | 126 +++++++++++++++++++++++++ gtsam/geometry/tests/testCameraSet.cpp | 51 ++++++++++ 3 files changed, 185 insertions(+) create mode 100644 gtsam/geometry/CameraSet.h create mode 100644 gtsam/geometry/tests/testCameraSet.cpp diff --git a/.cproject b/.cproject index 0bef605c5..0a3e306f4 100644 --- a/.cproject +++ b/.cproject @@ -1011,6 +1011,14 @@ true true + + make + -j4 + testCameraSet.run + true + true + true + make -j2 diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h new file mode 100644 index 000000000..6d64ca045 --- /dev/null +++ b/gtsam/geometry/CameraSet.h @@ -0,0 +1,126 @@ +/* ---------------------------------------------------------------------------- + + * 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 CameraSet.h + * @brief Base class to create smart factors on poses or cameras + * @author Frank Dellaert + * @date Feb 19, 2015 + */ + +#pragma once + +#include +#include // for Cheirality exception +#include + +#include + +namespace gtsam { + +/** + * @brief A set of cameras, all with their own calibration + */ +template +class CameraSet { + +private: + + std::vector cameras_; + + /** + * 2D measurement and noise model for each of the m views + * The order is kept the same as the keys that we use to create the factor. + */ + typedef typename CAMERA::Measurement Z; + + static const int ZDim = traits::dimension; ///< Measurement dimension + static const int Dim = traits::dimension; ///< Camera dimension + + /// shorthand for this class + typedef CameraSet This; + +public: + + /// Default Constructor + CameraSet() { + } + + /** Virtual destructor */ + virtual ~CameraSet() { + } + + /** + * Add a new camera + */ + void add(const CAMERA& camera) { + cameras_.push_back(camera); + } + + /** + * print + * @param s optional string naming the factor + * @param keyFormatter optional formatter useful for printing Symbols + */ + void print(const std::string& s = "") const { + std::cout << s << "CameraSet, cameras = \n"; + for (size_t k = 0; k < cameras_.size(); ++k) + cameras_[k]->print(); + } + + /// equals + virtual bool equals(const CameraSet& p, double tol = 1e-9) const { + bool camerasAreEqual = true; + for (size_t i = 0; i < cameras_.size(); i++) { + if (cameras_.at(i).equals(p.cameras_.at(i), tol) == false) + camerasAreEqual = false; + break; + } + return camerasAreEqual; + } + + /** + * project, with derivatives in this, point, and calibration + * throws CheiralityException + */ + std::vector project(const Point3& point, boost::optional F, + boost::optional E, boost::optional H) const { + + size_t nrCameras = cameras_.size(); + if (F) F->resize(ZDim * nrCameras, 6); + if (E) E->resize(ZDim * nrCameras, 3); + if (H) H->resize(ZDim * nrCameras, Dim - 6); + std::vector z(nrCameras); + + for (size_t i = 0; i < cameras_.size(); i++) { + Matrix Fi(ZDim, 6), Ei(ZDim, 3), Hi(ZDim, Dim - 6); + z[i] = cameras_[i].project(point, F ? &Fi : 0, E ? &Ei : 0, H ? &Hi : 0); + if (F) F->block(ZDim * i, 0) = Fi; + if (E) E->block(ZDim * i, 0) = Ei; + if (H) H->block(ZDim * i, 0) = Hi; + } + return z; + } + +private: + + /// Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & cameras_; + } +}; + +template +const int CameraSet::ZDim; + +} // \ namespace gtsam diff --git a/gtsam/geometry/tests/testCameraSet.cpp b/gtsam/geometry/tests/testCameraSet.cpp new file mode 100644 index 000000000..730d7fa36 --- /dev/null +++ b/gtsam/geometry/tests/testCameraSet.cpp @@ -0,0 +1,51 @@ +/* ---------------------------------------------------------------------------- + + * 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 testCameraSet.cpp + * @brief Unit tests for testCameraSet Class + * @author Frank Dellaert + * @date Feb 19, 2015 + */ + +#include +#include +#include + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +#include +#include +class PinholeSet: public CameraSet > { +}; + +TEST(CameraSet, Pinhole) { + PinholeSet f; +} + +/* ************************************************************************* */ +#include +class StereoSet: public CameraSet { +}; + +TEST(CameraSet, Stereo) { + StereoSet f; +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + From 6f0ea81c5e4ef7e5ab029380f374a06709cd9e5c Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 19 Feb 2015 13:38:20 +0100 Subject: [PATCH 40/91] Fixed compile issues --- gtsam/geometry/CameraSet.h | 4 +++- gtsam/geometry/tests/testCameraSet.cpp | 20 ++++++++++++-------- 2 files changed, 15 insertions(+), 9 deletions(-) diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h index 6d64ca045..c87705088 100644 --- a/gtsam/geometry/CameraSet.h +++ b/gtsam/geometry/CameraSet.h @@ -101,7 +101,9 @@ public: std::vector z(nrCameras); for (size_t i = 0; i < cameras_.size(); i++) { - Matrix Fi(ZDim, 6), Ei(ZDim, 3), Hi(ZDim, Dim - 6); + Eigen::Matrix Fi; + Eigen::Matrix Ei; + Eigen::Matrix Hi; z[i] = cameras_[i].project(point, F ? &Fi : 0, E ? &Ei : 0, H ? &Hi : 0); if (F) F->block(ZDim * i, 0) = Fi; if (E) E->block(ZDim * i, 0) = Ei; diff --git a/gtsam/geometry/tests/testCameraSet.cpp b/gtsam/geometry/tests/testCameraSet.cpp index 730d7fa36..5bf79e0b4 100644 --- a/gtsam/geometry/tests/testCameraSet.cpp +++ b/gtsam/geometry/tests/testCameraSet.cpp @@ -18,6 +18,7 @@ #include #include +#include #include using namespace std; @@ -26,20 +27,23 @@ using namespace gtsam; /* ************************************************************************* */ #include #include -class PinholeSet: public CameraSet > { -}; - TEST(CameraSet, Pinhole) { - PinholeSet f; + typedef PinholeCamera Camera; + CameraSet set; + set.add(Camera()); + set.add(Camera()); + Point3 p(0,0,1); + Matrix F,E,H; + vector z = set.project(p,F,E,H); + Point2 expected; + CHECK(assert_equal(expected,z[0])); + CHECK(assert_equal(expected,z[1])); } /* ************************************************************************* */ #include -class StereoSet: public CameraSet { -}; - TEST(CameraSet, Stereo) { - StereoSet f; + CameraSet f; } /* ************************************************************************* */ From 9a5f39b55bc3e4f3933460ac3873227545550f18 Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 19 Feb 2015 15:00:19 +0100 Subject: [PATCH 41/91] Check derivatives --- gtsam/geometry/tests/testCameraSet.cpp | 33 ++++++++++++++++++++------ 1 file changed, 26 insertions(+), 7 deletions(-) diff --git a/gtsam/geometry/tests/testCameraSet.cpp b/gtsam/geometry/tests/testCameraSet.cpp index 5bf79e0b4..8c9d11532 100644 --- a/gtsam/geometry/tests/testCameraSet.cpp +++ b/gtsam/geometry/tests/testCameraSet.cpp @@ -20,6 +20,7 @@ #include #include #include +#include using namespace std; using namespace gtsam; @@ -29,15 +30,33 @@ using namespace gtsam; #include TEST(CameraSet, Pinhole) { typedef PinholeCamera Camera; + typedef vector ZZ; CameraSet set; - set.add(Camera()); - set.add(Camera()); - Point3 p(0,0,1); - Matrix F,E,H; - vector z = set.project(p,F,E,H); + Camera camera; + set.add(camera); + set.add(camera); + Point3 p(0, 0, 1); + // Calculate actual + Matrix46 actualF; + Matrix43 actualE; + Matrix43 actualH; + { + Matrix26 F1; + Matrix23 E1; + Matrix23 H1; + camera.project(p, F1, E1, H1); + actualE << E1, E1; + actualF << F1, F1; + actualH << H1, H1; + } Point2 expected; - CHECK(assert_equal(expected,z[0])); - CHECK(assert_equal(expected,z[1])); + Matrix F, E, H; + ZZ z = set.project(p, F, E, H); + CHECK(assert_equal(expected, z[0])); + CHECK(assert_equal(expected, z[1])); + CHECK(assert_equal(actualF, F)); + CHECK(assert_equal(actualE, E)); + CHECK(assert_equal(actualH, H)); } /* ************************************************************************* */ From bc1da8577fc7128c3c482c983c0bdc9f65f38fce Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 19 Feb 2015 17:19:59 +0100 Subject: [PATCH 42/91] Make it work with StereoCamera --- gtsam/geometry/CameraSet.h | 9 +++-- gtsam/geometry/tests/testCameraSet.cpp | 48 ++++++++++++++++++++++---- 2 files changed, 46 insertions(+), 11 deletions(-) diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h index c87705088..defdbe494 100644 --- a/gtsam/geometry/CameraSet.h +++ b/gtsam/geometry/CameraSet.h @@ -20,14 +20,13 @@ #include #include // for Cheirality exception -#include - #include namespace gtsam { /** * @brief A set of cameras, all with their own calibration + * Assumes that a camera is laid out as 6 Pose3 parameters then calibration */ template class CameraSet { @@ -91,13 +90,13 @@ public: * project, with derivatives in this, point, and calibration * throws CheiralityException */ - std::vector project(const Point3& point, boost::optional F, - boost::optional E, boost::optional H) const { + std::vector project(const Point3& point, boost::optional F=boost::none, + boost::optional E=boost::none, boost::optional H=boost::none) const { size_t nrCameras = cameras_.size(); if (F) F->resize(ZDim * nrCameras, 6); if (E) E->resize(ZDim * nrCameras, 3); - if (H) H->resize(ZDim * nrCameras, Dim - 6); + if (H && Dim>6) H->resize(ZDim * nrCameras, Dim - 6); std::vector z(nrCameras); for (size_t i = 0; i < cameras_.size(); i++) { diff --git a/gtsam/geometry/tests/testCameraSet.cpp b/gtsam/geometry/tests/testCameraSet.cpp index 8c9d11532..c1ff5476f 100644 --- a/gtsam/geometry/tests/testCameraSet.cpp +++ b/gtsam/geometry/tests/testCameraSet.cpp @@ -26,6 +26,7 @@ using namespace std; using namespace gtsam; /* ************************************************************************* */ +// Cal3Bundler test #include #include TEST(CameraSet, Pinhole) { @@ -36,7 +37,14 @@ TEST(CameraSet, Pinhole) { set.add(camera); set.add(camera); Point3 p(0, 0, 1); - // Calculate actual + + // Check measurements + Point2 expected; + ZZ z = set.project(p); + CHECK(assert_equal(expected, z[0])); + CHECK(assert_equal(expected, z[1])); + + // Calculate expected derivatives using Pinhole Matrix46 actualF; Matrix43 actualE; Matrix43 actualH; @@ -49,11 +57,10 @@ TEST(CameraSet, Pinhole) { actualF << F1, F1; actualH << H1, H1; } - Point2 expected; + + // Check computed derivatives Matrix F, E, H; - ZZ z = set.project(p, F, E, H); - CHECK(assert_equal(expected, z[0])); - CHECK(assert_equal(expected, z[1])); + set.project(p, F, E, H); CHECK(assert_equal(actualF, F)); CHECK(assert_equal(actualE, E)); CHECK(assert_equal(actualH, H)); @@ -62,7 +69,36 @@ TEST(CameraSet, Pinhole) { /* ************************************************************************* */ #include TEST(CameraSet, Stereo) { - CameraSet f; + typedef vector ZZ; + CameraSet set; + StereoCamera camera; + set.add(camera); + set.add(camera); + Point3 p(0, 0, 1); + EXPECT_LONGS_EQUAL(6,traits::dimension); + + // Check measurements + StereoPoint2 expected; + ZZ z = set.project(p); + CHECK(assert_equal(expected, z[0])); + CHECK(assert_equal(expected, z[1])); + + // Calculate expected derivatives using Pinhole + Matrix46 actualF; + Matrix43 actualE; + { + Matrix36 F1; + Matrix33 E1; + camera.project(p, F1, E1); + actualE << E1, E1; + actualF << F1, F1; + } + + // Check computed derivatives + Matrix F, E; + set.project(p, F, E); + CHECK(assert_equal(actualF, F)); + CHECK(assert_equal(actualE, E)); } /* ************************************************************************* */ From 0a3272dab6c98b5225789b631b4703406d8ebd20 Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 19 Feb 2015 18:05:30 +0100 Subject: [PATCH 43/91] equals/print compile --- gtsam/geometry/CameraSet.h | 47 +++++++++++++++++--------- gtsam/geometry/tests/testCameraSet.cpp | 4 +++ 2 files changed, 35 insertions(+), 16 deletions(-) diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h index defdbe494..b1bd20c4a 100644 --- a/gtsam/geometry/CameraSet.h +++ b/gtsam/geometry/CameraSet.h @@ -20,6 +20,7 @@ #include #include // for Cheirality exception +#include #include namespace gtsam { @@ -53,17 +54,16 @@ public: CameraSet() { } - /** Virtual destructor */ - virtual ~CameraSet() { - } - - /** - * Add a new camera - */ + /// Add a new camera void add(const CAMERA& camera) { cameras_.push_back(camera); } + /// Retrieve ith camera + const CAMERA& operator[](size_t i) const { + return cameras_[i]; + } + /** * print * @param s optional string naming the factor @@ -72,7 +72,7 @@ public: void print(const std::string& s = "") const { std::cout << s << "CameraSet, cameras = \n"; for (size_t k = 0; k < cameras_.size(); ++k) - cameras_[k]->print(); + cameras_[k].print(); } /// equals @@ -90,13 +90,17 @@ public: * project, with derivatives in this, point, and calibration * throws CheiralityException */ - std::vector project(const Point3& point, boost::optional F=boost::none, - boost::optional E=boost::none, boost::optional H=boost::none) const { + std::vector project(const Point3& point, boost::optional F = + boost::none, boost::optional E = boost::none, + boost::optional H = boost::none) const { size_t nrCameras = cameras_.size(); - if (F) F->resize(ZDim * nrCameras, 6); - if (E) E->resize(ZDim * nrCameras, 3); - if (H && Dim>6) H->resize(ZDim * nrCameras, Dim - 6); + if (F) + F->resize(ZDim * nrCameras, 6); + if (E) + E->resize(ZDim * nrCameras, 3); + if (H && Dim > 6) + H->resize(ZDim * nrCameras, Dim - 6); std::vector z(nrCameras); for (size_t i = 0; i < cameras_.size(); i++) { @@ -104,9 +108,12 @@ public: Eigen::Matrix Ei; Eigen::Matrix Hi; z[i] = cameras_[i].project(point, F ? &Fi : 0, E ? &Ei : 0, H ? &Hi : 0); - if (F) F->block(ZDim * i, 0) = Fi; - if (E) E->block(ZDim * i, 0) = Ei; - if (H) H->block(ZDim * i, 0) = Hi; + if (F) + F->block(ZDim * i, 0) = Fi; + if (E) + E->block(ZDim * i, 0) = Ei; + if (H) + H->block(ZDim * i, 0) = Hi; } return z; } @@ -124,4 +131,12 @@ private: template const int CameraSet::ZDim; +template +struct traits > : public Testable > { +}; + +template +struct traits > : public Testable > { +}; + } // \ namespace gtsam diff --git a/gtsam/geometry/tests/testCameraSet.cpp b/gtsam/geometry/tests/testCameraSet.cpp index c1ff5476f..c24ba39b1 100644 --- a/gtsam/geometry/tests/testCameraSet.cpp +++ b/gtsam/geometry/tests/testCameraSet.cpp @@ -37,6 +37,10 @@ TEST(CameraSet, Pinhole) { set.add(camera); set.add(camera); Point3 p(0, 0, 1); + CHECK(assert_equal(set,set)); + CameraSet set2 = set; + set2.add(camera); + CHECK(!assert_equal(set,set2)); // Check measurements Point2 expected; From b5d9ed45291368b37d678d4e6883755d5803b354 Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 19 Feb 2015 18:05:50 +0100 Subject: [PATCH 44/91] Make derivative zero, throw exception. --- gtsam/geometry/StereoCamera.cpp | 5 ++--- gtsam/geometry/StereoCamera.h | 2 +- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/gtsam/geometry/StereoCamera.cpp b/gtsam/geometry/StereoCamera.cpp index 9170f8282..eb83fd10f 100644 --- a/gtsam/geometry/StereoCamera.cpp +++ b/gtsam/geometry/StereoCamera.cpp @@ -31,7 +31,7 @@ namespace gtsam { /* ************************************************************************* */ StereoPoint2 StereoCamera::project(const Point3& point, OptionalJacobian<3,6> H1, OptionalJacobian<3,3> H2, - OptionalJacobian<3,6> H3) const { + OptionalJacobian<3,0> H3) const { #ifdef STEREOCAMERA_CHAIN_RULE const Point3 q = leftCamPose_.transform_to(point, H1, H2); @@ -78,8 +78,7 @@ namespace gtsam { } #endif if (H3) - // TODO, this is set to zero, as Cal3_S2Stereo cannot be optimized yet - H3->setZero(); + throw std::runtime_error("StereoCamera::project does not support third derivative yet"); } // finally translate diff --git a/gtsam/geometry/StereoCamera.h b/gtsam/geometry/StereoCamera.h index 8d16de4dd..006095420 100644 --- a/gtsam/geometry/StereoCamera.h +++ b/gtsam/geometry/StereoCamera.h @@ -125,7 +125,7 @@ public: StereoPoint2 project(const Point3& point, OptionalJacobian<3, 6> H1 = boost::none, OptionalJacobian<3, 3> H2 = boost::none, - OptionalJacobian<3, 6> H3 = boost::none) const; + OptionalJacobian<3, 0> H3 = boost::none) const; /** * From 03689e418e702dcaf36bb15475f363cf01a1e4c8 Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 19 Feb 2015 23:44:56 +0100 Subject: [PATCH 45/91] Cleaned up, made default constructor a working StereoCamera --- gtsam/geometry/StereoCamera.h | 51 +++++++++++++++++++++-------------- 1 file changed, 31 insertions(+), 20 deletions(-) diff --git a/gtsam/geometry/StereoCamera.h b/gtsam/geometry/StereoCamera.h index 006095420..88ffbcdbd 100644 --- a/gtsam/geometry/StereoCamera.h +++ b/gtsam/geometry/StereoCamera.h @@ -28,10 +28,11 @@ namespace gtsam { class GTSAM_EXPORT StereoCheiralityException: public std::runtime_error { public: - StereoCheiralityException() : std::runtime_error("Stereo Cheirality Exception") {} + StereoCheiralityException() : + std::runtime_error("Stereo Cheirality Exception") { + } }; - /** * A stereo camera class, parameterize by left camera pose and stereo calibration * @addtogroup geometry @@ -52,16 +53,22 @@ private: public: - enum { dimension = 6 }; + enum { + dimension = 6 + }; /// @name Standard Constructors /// @{ - StereoCamera() { + /// Default constructor allocates a calibration! + StereoCamera() : + K_(new Cal3_S2Stereo()) { } + /// Construct from pose and shared calibration StereoCamera(const Pose3& leftCamPose, const Cal3_S2Stereo::shared_ptr K); + /// Return shared pointer to calibration const Cal3_S2Stereo::shared_ptr calibration() const { return K_; } @@ -70,26 +77,28 @@ public: /// @name Testable /// @{ + /// print void print(const std::string& s = "") const { leftCamPose_.print(s + ".camera."); K_->print(s + ".calibration."); } + /// equals bool equals(const StereoCamera &camera, double tol = 1e-9) const { - return leftCamPose_.equals(camera.leftCamPose_, tol) && K_->equals( - *camera.K_, tol); + return leftCamPose_.equals(camera.leftCamPose_, tol) + && K_->equals(*camera.K_, tol); } /// @} /// @name Manifold /// @{ - /** Dimensionality of the tangent space */ + /// Dimensionality of the tangent space inline size_t dim() const { return 6; } - /** Dimensionality of the tangent space */ + /// Dimensionality of the tangent space static inline size_t Dim() { return 6; } @@ -108,10 +117,12 @@ public: /// @name Standard Interface /// @{ + /// pose const Pose3& pose() const { return leftCamPose_; } + /// baseline double baseline() const { return K_->baseline(); } @@ -122,19 +133,16 @@ public: * @param H2 derivative with respect to point * @param H3 IGNORED (for calibration) */ - StereoPoint2 project(const Point3& point, - OptionalJacobian<3, 6> H1 = boost::none, - OptionalJacobian<3, 3> H2 = boost::none, + StereoPoint2 project(const Point3& point, OptionalJacobian<3, 6> H1 = + boost::none, OptionalJacobian<3, 3> H2 = boost::none, OptionalJacobian<3, 0> H3 = boost::none) const; - /** - * - */ + /// back-project a measurement Point3 backproject(const StereoPoint2& z) const { Vector measured = z.vector(); - double Z = K_->baseline()*K_->fx()/(measured[0]-measured[1]); - double X = Z *(measured[0]- K_->px()) / K_->fx(); - double Y = Z *(measured[2]- K_->py()) / K_->fy(); + double Z = K_->baseline() * K_->fx() / (measured[0] - measured[1]); + double X = Z * (measured[0] - K_->px()) / K_->fx(); + double Y = Z * (measured[2] - K_->py()) / K_->fy(); Point3 world_point = leftCamPose_.transform_from(Point3(X, Y, Z)); return world_point; } @@ -142,7 +150,8 @@ public: /// @} private: - /** utility functions */ + + /// utility function Matrix3 Dproject_to_stereo_camera1(const Point3& P) const; friend class boost::serialization::access; @@ -155,8 +164,10 @@ private: }; template<> -struct traits : public internal::Manifold {}; +struct traits : public internal::Manifold { +}; template<> -struct traits : public internal::Manifold {}; +struct traits : public internal::Manifold { +}; } From 8daefafb91c024f732f436e1e857e0d7ef75bcb8 Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 19 Feb 2015 23:45:12 +0100 Subject: [PATCH 46/91] Only now it really works with StereoCamera --- gtsam/geometry/CameraSet.h | 57 ++++++++------------------ gtsam/geometry/tests/testCameraSet.cpp | 22 +++++----- 2 files changed, 28 insertions(+), 51 deletions(-) diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h index b1bd20c4a..27c0a78f3 100644 --- a/gtsam/geometry/CameraSet.h +++ b/gtsam/geometry/CameraSet.h @@ -30,12 +30,10 @@ namespace gtsam { * Assumes that a camera is laid out as 6 Pose3 parameters then calibration */ template -class CameraSet { +class CameraSet: public std::vector { private: - std::vector cameras_; - /** * 2D measurement and noise model for each of the m views * The order is kept the same as the keys that we use to create the factor. @@ -45,25 +43,8 @@ private: static const int ZDim = traits::dimension; ///< Measurement dimension static const int Dim = traits::dimension; ///< Camera dimension - /// shorthand for this class - typedef CameraSet This; - public: - /// Default Constructor - CameraSet() { - } - - /// Add a new camera - void add(const CAMERA& camera) { - cameras_.push_back(camera); - } - - /// Retrieve ith camera - const CAMERA& operator[](size_t i) const { - return cameras_[i]; - } - /** * print * @param s optional string naming the factor @@ -71,15 +52,17 @@ public: */ void print(const std::string& s = "") const { std::cout << s << "CameraSet, cameras = \n"; - for (size_t k = 0; k < cameras_.size(); ++k) - cameras_[k].print(); + for (size_t k = 0; k < this->size(); ++k) + this->at(k).print(); } /// equals virtual bool equals(const CameraSet& p, double tol = 1e-9) const { + if (this->size() != p.size()) + return false; bool camerasAreEqual = true; - for (size_t i = 0; i < cameras_.size(); i++) { - if (cameras_.at(i).equals(p.cameras_.at(i), tol) == false) + for (size_t i = 0; i < this->size(); i++) { + if (this->at(i).equals(p.at(i), tol) == false) camerasAreEqual = false; break; } @@ -94,26 +77,20 @@ public: boost::none, boost::optional E = boost::none, boost::optional H = boost::none) const { - size_t nrCameras = cameras_.size(); - if (F) - F->resize(ZDim * nrCameras, 6); - if (E) - E->resize(ZDim * nrCameras, 3); - if (H && Dim > 6) - H->resize(ZDim * nrCameras, Dim - 6); + size_t nrCameras = this->size(); + if (F) F->resize(ZDim * nrCameras, 6); + if (E) E->resize(ZDim * nrCameras, 3); + if (H && Dim > 6) H->resize(ZDim * nrCameras, Dim - 6); std::vector z(nrCameras); - for (size_t i = 0; i < cameras_.size(); i++) { + for (size_t i = 0; i < nrCameras; i++) { Eigen::Matrix Fi; Eigen::Matrix Ei; Eigen::Matrix Hi; - z[i] = cameras_[i].project(point, F ? &Fi : 0, E ? &Ei : 0, H ? &Hi : 0); - if (F) - F->block(ZDim * i, 0) = Fi; - if (E) - E->block(ZDim * i, 0) = Ei; - if (H) - H->block(ZDim * i, 0) = Hi; + z[i] = this->at(i).project(point, F ? &Fi : 0, E ? &Ei : 0, H ? &Hi : 0); + if (F) F->block(ZDim * i, 0) = Fi; + if (E) E->block(ZDim * i, 0) = Ei; + if (H) H->block(ZDim * i, 0) = Hi; } return z; } @@ -124,7 +101,7 @@ private: friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int version) { - ar & cameras_; + ar & (*this); } }; diff --git a/gtsam/geometry/tests/testCameraSet.cpp b/gtsam/geometry/tests/testCameraSet.cpp index c24ba39b1..42cf0f299 100644 --- a/gtsam/geometry/tests/testCameraSet.cpp +++ b/gtsam/geometry/tests/testCameraSet.cpp @@ -34,13 +34,13 @@ TEST(CameraSet, Pinhole) { typedef vector ZZ; CameraSet set; Camera camera; - set.add(camera); - set.add(camera); + set.push_back(camera); + set.push_back(camera); Point3 p(0, 0, 1); - CHECK(assert_equal(set,set)); + CHECK(assert_equal(set, set)); CameraSet set2 = set; - set2.add(camera); - CHECK(!assert_equal(set,set2)); + set2.push_back(camera); + CHECK(!set.equals(set2)); // Check measurements Point2 expected; @@ -76,20 +76,20 @@ TEST(CameraSet, Stereo) { typedef vector ZZ; CameraSet set; StereoCamera camera; - set.add(camera); - set.add(camera); + set.push_back(camera); + set.push_back(camera); Point3 p(0, 0, 1); - EXPECT_LONGS_EQUAL(6,traits::dimension); + EXPECT_LONGS_EQUAL(6, traits::dimension); // Check measurements - StereoPoint2 expected; + StereoPoint2 expected(0, -1, 0); ZZ z = set.project(p); CHECK(assert_equal(expected, z[0])); CHECK(assert_equal(expected, z[1])); // Calculate expected derivatives using Pinhole - Matrix46 actualF; - Matrix43 actualE; + Matrix66 actualF; + Matrix63 actualE; { Matrix36 F1; Matrix33 E1; From 36e858bc960e4d8794ac49e6797eb770ada2deb5 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 20 Feb 2015 00:51:32 +0100 Subject: [PATCH 47/91] removed obsolete target --- .cproject | 8 -------- 1 file changed, 8 deletions(-) diff --git a/.cproject b/.cproject index 0a3e306f4..35bb1ecf3 100644 --- a/.cproject +++ b/.cproject @@ -1107,14 +1107,6 @@ true true - - make - -j5 - testSmartProjectionFactor.run - true - true - true - make -j5 From ed267ed69d6a064abf71787b0492e5141b9681ad Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 20 Feb 2015 00:57:00 +0100 Subject: [PATCH 48/91] BIG CHANGE: Smart factors now call project in CameraSet. It makes it a bit more clear by having a separately tested unit with projection/derivatives. But in the end it did not help much. SmartFactorBase is still long and (too) complex. --- gtsam/slam/SmartFactorBase.h | 244 +++++++++++------- gtsam/slam/SmartProjectionFactor.h | 2 +- .../slam/SmartStereoProjectionFactor.h | 2 +- 3 files changed, 149 insertions(+), 99 deletions(-) diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index 3bb66e57f..b5ed39c55 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -25,8 +25,7 @@ #include #include -#include -#include // for Cheirality exception +#include #include #include @@ -47,14 +46,15 @@ class SmartFactorBase: public NonlinearFactor { protected: - // Keep a copy of measurement for I/O + typedef typename CAMERA::Measurement Z; /** * 2D measurement and noise model for each of the m views + * We keep a copy of measurements for I/O and computing the error. * The order is kept the same as the keys that we use to create the factor. */ - typedef typename CAMERA::Measurement Z; std::vector measured_; + std::vector noise_; ///< noise model used boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame (one for all cameras) @@ -83,9 +83,7 @@ public: /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; - /// shorthand for a pinhole camera - typedef CAMERA Camera; - typedef std::vector Cameras; + typedef CameraSet Cameras; /** * Constructor @@ -200,22 +198,32 @@ public: /// Calculate vector of re-projection errors, before applying noise model Vector reprojectionError(const Cameras& cameras, const Point3& point) const { - Vector b = zero(ZDim * cameras.size()); - - size_t i = 0; - BOOST_FOREACH(const CAMERA& camera, cameras) { - const Z& zi = this->measured_.at(i); - try { - Z e(camera.project(point) - zi); - b[ZDim * i] = e.x(); - b[ZDim * i + 1] = e.y(); - } catch (CheiralityException& e) { - std::cout << "Cheirality exception " << std::endl; - exit(EXIT_FAILURE); - } - i += 1; + // Project into CameraSet + std::vector predicted; + try { + predicted = cameras.project(point); + } catch (CheiralityException&) { + std::cout << "reprojectionError: Cheirality exception " << std::endl; + exit(EXIT_FAILURE); // TODO: throw exception } + // Calculate vector of errors + size_t nrCameras = cameras.size(); + Vector b(ZDim * nrCameras); + for (size_t i = 0, row = 0; i < nrCameras; i++, row += ZDim) { + Z e = predicted[i] - measured_.at(i); + b.segment(row) = e.vector(); + } + + return b; + } + + /// Calculate vector of re-projection errors, noise model applied + Vector whitenedError(const Cameras& cameras, const Point3& point) const { + Vector b = reprojectionError(cameras, point); + size_t nrCameras = cameras.size(); + for (size_t i = 0, row = 0; i < nrCameras; i++, row += ZDim) + b.segment(row) = noise_.at(i)->whiten(b.segment(row)); return b; } @@ -228,23 +236,12 @@ public: */ double totalReprojectionError(const Cameras& cameras, const Point3& point) const { - + Vector b = reprojectionError(cameras, point); double overallError = 0; - - size_t i = 0; - BOOST_FOREACH(const CAMERA& camera, cameras) { - const Z& zi = this->measured_.at(i); - try { - Z reprojectionError(camera.project(point) - zi); - overallError += 0.5 - * this->noise_.at(i)->distance(reprojectionError.vector()); - } catch (CheiralityException&) { - std::cout << "Cheirality exception " << std::endl; - exit(EXIT_FAILURE); - } - i += 1; - } - return overallError; + size_t nrCameras = cameras.size(); + for (size_t i = 0; i < nrCameras; i++) + overallError += noise_.at(i)->distance(b.segment(i * ZDim)); + return 0.5 * overallError; } /** @@ -255,78 +252,124 @@ public: void computeEP(Matrix& E, Matrix& PointCov, const Cameras& cameras, const Point3& point) const { - int numKeys = this->keys_.size(); - E = zeros(ZDim * numKeys, 3); - Vector b = zero(2 * numKeys); - - Matrix Ei(ZDim, 3); - for (size_t i = 0; i < this->measured_.size(); i++) { - try { - cameras[i].project(point, boost::none, Ei, boost::none); - } catch (CheiralityException& e) { - std::cout << "Cheirality exception " << std::endl; - exit(EXIT_FAILURE); - } - this->noise_.at(i)->WhitenSystem(Ei, b); - E.block(ZDim * i, 0) = Ei; + // Project into CameraSet, calculating derivatives + std::vector predicted; + try { + using boost::none; + predicted = cameras.project(point, none, E, none); + } catch (CheiralityException&) { + std::cout << "computeEP: Cheirality exception " << std::endl; + exit(EXIT_FAILURE); // TODO: throw exception } + // if needed, whiten + for (size_t i = 0, row = 0; i < noise_.size(); i++, row += ZDim) { + SharedNoiseModel model = noise_.at(i); + if (model) { + // TODO: re-factor noiseModel to take any block/fixed vector/matrix + Vector dummy; + Matrix Ei = E.block(row, 0); + model->WhitenSystem(Ei, dummy); + E.block(row, 0) = Ei; + } + } // Matrix PointCov; PointCov.noalias() = (E.transpose() * E).inverse(); } + /** + * Compute F, E, and optionally H, where F and E are the stacked derivatives + * with respect to the cameras, point, and calibration, respectively. + * The value of cameras/point are passed as parameters. + * Returns error vector b + * TODO: the treatment of body_P_sensor_ is weird: the transformation + * is applied in the caller, but the derivatives are computed here. + */ + Vector whitenedError(const Cameras& cameras, const Point3& point, Matrix& F, + Matrix& E, boost::optional H = boost::none) const { + + // Project into CameraSet, calculating derivatives + std::vector predicted; + try { + predicted = cameras.project(point, F, E, H); + } catch (CheiralityException&) { + std::cout << "computeJacobians: Cheirality exception " << std::endl; + exit(EXIT_FAILURE); // TODO: throw exception + } + + // Calculate error and whiten derivatives if needed + size_t numKeys = keys_.size(); + Vector b(ZDim * numKeys); + for (size_t i = 0, row = 0; i < numKeys; i++, row += ZDim) { + + // Calculate error + const Z& zi = measured_.at(i); + b.segment(row) = (zi - predicted[i]).vector(); + + // if we have a sensor offset, correct camera derivatives + if (body_P_sensor_) { + // TODO: no simpler way ?? + const Pose3& pose_i = cameras[i].pose(); + Pose3 w_Pose_body = pose_i.compose(body_P_sensor_->inverse()); + Matrix66 J; + Pose3 world_P_body = w_Pose_body.compose(*body_P_sensor_, J); + F.block(row, 0) *= J; + } + + // if needed, whiten + SharedNoiseModel model = noise_.at(i); + if (model) { + // TODO, refactor noiseModel so we can take blocks + Matrix Fi = F.block(row, 0); + Matrix Ei = E.block(row, 0); + Vector bi = b.segment(row); + if (!H) + model->WhitenSystem(Fi, Ei, bi); + else { + Matrix Hi = H->block(row, 0); + model->WhitenSystem(Fi, Ei, Hi, bi); + H->block(row, 0) = Hi; + } + b.segment(row) = bi; + F.block(row, 0) = Fi; + E.block(row, 0) = Ei; + } + } + return b; + } + /** * Compute F, E, and b (called below in both vanilla and SVD versions), where * F is a vector of derivatives wrpt the cameras, and E the stacked derivatives * with respect to the point. The value of cameras/point are passed as parameters. - * Given a Point3, assumes dimensionality is 3. - * TODO We assume below the dimensionality of Pose3 is 6. Frank thinks the templating - * of this factor is only for show, and should just assume a PinholeCamera. */ double computeJacobians(std::vector& Fblocks, Matrix& E, Vector& b, const Cameras& cameras, const Point3& point) const { - size_t numKeys = this->keys_.size(); - E = zeros(ZDim * numKeys, 3); - b = zero(ZDim * numKeys); - double f = 0; + // Project into Camera set and calculate derivatives + // TODO: if D==6 we optimize only camera pose. That is fairly hacky! + Matrix F, H; + using boost::none; + boost::optional optionalH(H); + b = whitenedError(cameras, point, F, E, D == 6 ? none : optionalH); - Matrix Fi(ZDim, 6), Ei(ZDim, 3), Hcali(ZDim, D - 6); - for (size_t i = 0; i < this->measured_.size(); i++) { + // Now calculate f and divide up the F derivatives into Fblocks + double f = 0.0; + size_t numKeys = keys_.size(); + for (size_t i = 0, row = 0; i < numKeys; i++, row += ZDim) { - Vector bi; - try { - const Z& zi = this->measured_.at(i); - bi = -(cameras[i].project(point, Fi, Ei, Hcali) - zi).vector(); + // Accumulate normalized error + f += b.segment(row).squaredNorm(); - // if we have a sensor offset, correct derivatives - if (body_P_sensor_) { - Pose3 w_Pose_body = (cameras[i].pose()).compose( - body_P_sensor_->inverse()); - Matrix J(6, 6); - Pose3 world_P_body = w_Pose_body.compose(*body_P_sensor_, J); - Fi = Fi * J; - } - } catch (CheiralityException&) { - std::cout << "Cheirality exception " << std::endl; - exit(EXIT_FAILURE); // TODO: throw exception - } + // Get piece of F and possibly H + Matrix2D Fi; + if (D == 6) + Fi << F.block(row, 0); + else + Fi << F.block(row, 0), H.block(row, 0); - // Whiten derivatives according to noise parameters - this->noise_.at(i)->WhitenSystem(Fi, Ei, Hcali, bi); - - f += bi.squaredNorm(); - // TODO: if D==6 we optimize only camera pose. What's up with that ??? - if (D == 6) { - Fblocks.push_back(KeyMatrix2D(this->keys_[i], Fi)); - } else { - Matrix Hcam(ZDim, D); - Hcam.block(0, 0) = Fi; // ZDim x 6 block for the cameras - Hcam.block(0, 6) = Hcali; // ZDim x nrCal block for the cameras - Fblocks.push_back(KeyMatrix2D(this->keys_[i], Hcam)); - } - E.block(ZDim * i, 0) = Ei; - subInsert(b, bi, ZDim * i); + // Push it onto Fblocks + Fblocks.push_back(KeyMatrix2D(keys_[i], Fi)); } return f; } @@ -356,18 +399,21 @@ public: return f; } - // TODO, there should not be a Matrix version, really + /** + * Compute F, E, and b, where F and E are the stacked derivatives + * with respect to the point. The value of cameras/point are passed as parameters. + */ double computeJacobians(Matrix& F, Matrix& E, Matrix3& PointCov, Vector& b, const Cameras& cameras, const Point3& point, const double lambda = 0.0) const { - size_t numKeys = this->keys_.size(); + size_t numKeys = keys_.size(); std::vector Fblocks; double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda); F = zeros(This::ZDim * numKeys, D * numKeys); - for (size_t i = 0; i < this->keys_.size(); ++i) { + for (size_t i = 0; i < keys_.size(); ++i) { F.block(This::ZDim * i, D * i) = Fblocks.at(i).second; // ZDim x 6 block for the cameras } return f; @@ -584,7 +630,8 @@ public: } /** - * No idea. TODO Luca should document + * Applies Schur complement (exploiting block structure) to get a smart factor on cameras, + * and adds the contribution of the smart factor to a pre-allocated augmented Hessian. */ void updateSparseSchurComplement(const std::vector& Fblocks, const Matrix& E, const Matrix& P /*Point Covariance*/, const Vector& b, @@ -657,7 +704,9 @@ public: } /** - * No idea. TODO Luca should document + * Add the contribution of the smart factor to a pre-allocated Hessian, + * using sparse linear algebra. More efficient than the creation of the + * Hessian without preallocation of the SymmetricBlockMatrix */ void updateAugmentedHessian(const Cameras& cameras, const Point3& point, const double lambda, bool diagonalDamping, @@ -730,7 +779,8 @@ private: ar & BOOST_SERIALIZATION_NVP(measured_); ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); } -}; +} +; template const int SmartFactorBase::ZDim; diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index 6b86b0b1e..e763b3b02 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -110,7 +110,7 @@ public: /// shorthand for a pinhole camera typedef PinholeCamera Camera; - typedef std::vector Cameras; + typedef CameraSet Cameras; /** * Constructor diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index aeefe6c58..2fa23d09f 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -117,7 +117,7 @@ public: typedef StereoCamera Camera; /// Vector of cameras - typedef std::vector Cameras; + typedef CameraSet Cameras; /** * Constructor From 4badb4a10f23e15dccd962647c2e5cfb4aebdff7 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 20 Feb 2015 00:57:27 +0100 Subject: [PATCH 49/91] Derivative tests, and re-formatting (and getting rid of gtsam::/std::) --- .../tests/testSmartProjectionPoseFactor.cpp | 775 +++++++++++------- .../testSmartStereoProjectionPoseFactor.cpp | 423 ++++++---- 2 files changed, 726 insertions(+), 472 deletions(-) diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index 99df9f273..02e4fd3b6 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -20,11 +20,12 @@ #include "../SmartProjectionPoseFactor.h" -#include #include #include -#include +#include +#include #include +#include #include using namespace std; @@ -35,37 +36,38 @@ static bool isDebugTest = false; // make a realistic calibration matrix static double fov = 60; // degrees -static size_t w=640,h=480; +static size_t w = 640, h = 480; -static Cal3_S2::shared_ptr K(new Cal3_S2(fov,w,h)); +static Cal3_S2::shared_ptr K(new Cal3_S2(fov, w, h)); static Cal3_S2::shared_ptr K2(new Cal3_S2(1500, 1200, 0, 640, 480)); -static boost::shared_ptr Kbundler(new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000)); +static boost::shared_ptr Kbundler( + new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000)); static double rankTol = 1.0; static double linThreshold = -1.0; static bool manageDegeneracy = true; // Create a noise model for the pixel error -static SharedNoiseModel model(noiseModel::Unit::Create(2)); +static SharedNoiseModel model(noiseModel::Isotropic::Sigma(2, 0.1)); // Convenience for named keys using symbol_shorthand::X; using symbol_shorthand::L; // tests data -static Symbol x1('X', 1); -static Symbol x2('X', 2); -static Symbol x3('X', 3); +static Symbol x1('X', 1); +static Symbol x2('X', 2); +static Symbol x3('X', 3); static Key poseKey1(x1); static Point2 measurement1(323.0, 240.0); -static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); +static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), + Point3(0.25, -0.10, 1.0)); typedef SmartProjectionPoseFactor SmartFactor; typedef SmartProjectionPoseFactor SmartFactorBundler; -void projectToMultipleCameras( - SimpleCamera cam1, SimpleCamera cam2, SimpleCamera cam3, Point3 landmark, - vector& measurements_cam){ +void projectToMultipleCameras(SimpleCamera cam1, SimpleCamera cam2, + SimpleCamera cam3, Point3 landmark, vector& measurements_cam) { Point2 cam1_uv1 = cam1.project(landmark); Point2 cam2_uv1 = cam2.project(landmark); @@ -101,7 +103,8 @@ TEST( SmartProjectionPoseFactor, Constructor4) { TEST( SmartProjectionPoseFactor, ConstructorWithTransform) { bool manageDegeneracy = true; bool enableEPI = false; - SmartFactor factor1(rankTol, linThreshold, manageDegeneracy, enableEPI, body_P_sensor1); + SmartFactor factor1(rankTol, linThreshold, manageDegeneracy, enableEPI, + body_P_sensor1); factor1.add(measurement1, poseKey1, model, K); } @@ -117,21 +120,22 @@ TEST( SmartProjectionPoseFactor, Equals ) { } /* *************************************************************************/ -TEST_UNSAFE( SmartProjectionPoseFactor, noiseless ){ +TEST_UNSAFE( SmartProjectionPoseFactor, noiseless ) { // cout << " ************************ SmartProjectionPoseFactor: noisy ****************************" << endl; // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + Pose3 level_pose = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), + Point3(0, 0, 1)); SimpleCamera level_camera(level_pose, *K2); // create second camera 1 meter to the right of first camera - Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1,0,0)); + Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1, 0, 0)); SimpleCamera level_camera_right(level_pose_right, *K2); // landmark ~5 meters infront of camera Point3 landmark(5, 0.5, 1.2); - // 1. Project two landmarks into two cameras and triangulate + // Project two landmarks into two cameras Point2 level_uv = level_camera.project(landmark); Point2 level_uv_right = level_camera_right.project(landmark); @@ -139,93 +143,114 @@ TEST_UNSAFE( SmartProjectionPoseFactor, noiseless ){ values.insert(x1, level_pose); values.insert(x2, level_pose_right); - SmartFactor factor1; - factor1.add(level_uv, x1, model, K); - factor1.add(level_uv_right, x2, model, K); + SmartFactor factor; + factor.add(level_uv, x1, model, K); + factor.add(level_uv_right, x2, model, K); - double actualError = factor1.error(values); + double actualError = factor.error(values); double expectedError = 0.0; EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); - SmartFactor::Cameras cameras = factor1.cameras(values); - double actualError2 = factor1.totalReprojectionError(cameras); + SmartFactor::Cameras cameras = factor.cameras(values); + double actualError2 = factor.totalReprojectionError(cameras); EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1e-7); // test vector of errors - //Vector actual = factor1.unwhitenedError(values); + //Vector actual = factor.unwhitenedError(values); //EXPECT(assert_equal(zero(4),actual,1e-8)); + + // Check derivatives + + // Calculate expected derivative for point (easiest to check) + boost::function f = // + boost::bind(&SmartFactor::whitenedError, factor, cameras, _1); + boost::optional point = factor.point(); + CHECK(point); + Matrix expectedE = numericalDerivative11(f, *point); + + // Calculate using computeEP + Matrix actualE, PointCov; + factor.computeEP(actualE, PointCov, cameras); + EXPECT(assert_equal(expectedE, actualE, 1e-7)); + + // Calculate using whitenedError + Matrix F, actualE2; + Vector actualErrors = factor.whitenedError(cameras, *point, F, actualE2); + EXPECT(assert_equal(expectedE, actualE2, 1e-7)); + EXPECT(assert_equal(f(*point), actualErrors, 1e-7)); } /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, noisy ){ +TEST( SmartProjectionPoseFactor, noisy ) { // cout << " ************************ SmartProjectionPoseFactor: noisy ****************************" << endl; // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + Pose3 level_pose = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), + Point3(0, 0, 1)); SimpleCamera level_camera(level_pose, *K2); // create second camera 1 meter to the right of first camera - Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1,0,0)); + Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1, 0, 0)); SimpleCamera level_camera_right(level_pose_right, *K2); // landmark ~5 meters infront of camera Point3 landmark(5, 0.5, 1.2); // 1. Project two landmarks into two cameras and triangulate - Point2 pixelError(0.2,0.2); + Point2 pixelError(0.2, 0.2); Point2 level_uv = level_camera.project(landmark) + pixelError; Point2 level_uv_right = level_camera_right.project(landmark); Values values; values.insert(x1, level_pose); - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10), + Point3(0.5, 0.1, 0.3)); values.insert(x2, level_pose_right.compose(noise_pose)); - SmartFactor::shared_ptr factor1(new SmartFactor()); - factor1->add(level_uv, x1, model, K); - factor1->add(level_uv_right, x2, model, K); + SmartFactor::shared_ptr factor(new SmartFactor()); + factor->add(level_uv, x1, model, K); + factor->add(level_uv_right, x2, model, K); - double actualError1= factor1->error(values); + double actualError1 = factor->error(values); SmartFactor::shared_ptr factor2(new SmartFactor()); vector measurements; measurements.push_back(level_uv); measurements.push_back(level_uv_right); - std::vector< SharedNoiseModel > noises; + vector noises; noises.push_back(model); noises.push_back(model); - std::vector< boost::shared_ptr > Ks; ///< shared pointer to calibration object (one for each camera) + vector > Ks; ///< shared pointer to calibration object (one for each camera) Ks.push_back(K); Ks.push_back(K); - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); factor2->add(measurements, views, noises, Ks); - double actualError2= factor2->error(values); + double actualError2 = factor2->error(values); DOUBLES_EQUAL(actualError1, actualError2, 1e-7); } - /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ){ +TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) { // cout << " ************************ SmartProjectionPoseFactor: 3 cams + 3 landmarks **********************" << endl; // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); SimpleCamera cam1(pose1, *K2); // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); SimpleCamera cam2(pose2, *K2); // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); SimpleCamera cam3(pose3, *K2); // three landmarks ~5 meters infront of camera @@ -240,7 +265,7 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ){ projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); @@ -263,21 +288,25 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ){ graph.push_back(PriorFactor(x1, pose1, noisePrior)); graph.push_back(PriorFactor(x2, pose2, noisePrior)); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, pose1); values.insert(x2, pose2); // initialize third pose with some noise, we expect it to move back to original pose3 - values.insert(x3, pose3*noise_pose); - if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); + values.insert(x3, pose3 * noise_pose); + if (isDebugTest) + values.at(x3).print("Smart: Pose3 before optimization: "); LevenbergMarquardtParams params; - if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; - if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; + if (isDebugTest) + params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; + if (isDebugTest) + params.verbosity = NonlinearOptimizerParams::ERROR; Values result; - gttic_(SmartProjectionPoseFactor); + gttic_ (SmartProjectionPoseFactor); LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); gttoc_(SmartProjectionPoseFactor); @@ -287,25 +316,29 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ){ // VectorValues delta = GFG->optimize(); // result.print("results of 3 camera, 3 landmark optimization \n"); - if(isDebugTest) result.at(x3).print("Smart: Pose3 after optimization: "); - EXPECT(assert_equal(pose3,result.at(x3),1e-8)); - if(isDebugTest) tictoc_print_(); + if (isDebugTest) + result.at(x3).print("Smart: Pose3 after optimization: "); + EXPECT(assert_equal(pose3, result.at(x3), 1e-8)); + if (isDebugTest) + tictoc_print_(); } /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){ +TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ) { // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 cameraPose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); // body poses - Pose3 cameraPose2 = cameraPose1 * Pose3(Rot3(), Point3(1,0,0)); - Pose3 cameraPose3 = cameraPose1 * Pose3(Rot3(), Point3(0,-1,0)); + Pose3 cameraPose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), + Point3(0, 0, 1)); // body poses + Pose3 cameraPose2 = cameraPose1 * Pose3(Rot3(), Point3(1, 0, 0)); + Pose3 cameraPose3 = cameraPose1 * Pose3(Rot3(), Point3(0, -1, 0)); SimpleCamera cam1(cameraPose1, *K); // with camera poses SimpleCamera cam2(cameraPose2, *K); SimpleCamera cam3(cameraPose3, *K); // create arbitrary body_Pose_sensor (transforms from sensor to body) - Pose3 sensor_to_body = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(1, 1, 1)); // Pose3(); // + Pose3 sensor_to_body = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), + Point3(1, 1, 1)); // Pose3(); // // These are the poses we want to estimate, from camera measurements Pose3 bodyPose1 = cameraPose1.compose(sensor_to_body.inverse()); @@ -325,7 +358,7 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){ projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); // Create smart factors - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); @@ -335,13 +368,19 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){ bool manageDegeneracy = false; bool enableEPI = false; - SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol,linThreshold,manageDegeneracy,enableEPI,sensor_to_body)); + SmartFactor::shared_ptr smartFactor1( + new SmartFactor(rankTol, linThreshold, manageDegeneracy, enableEPI, + sensor_to_body)); smartFactor1->add(measurements_cam1, views, model, K); - SmartFactor::shared_ptr smartFactor2(new SmartFactor(rankTol,linThreshold,manageDegeneracy,enableEPI,sensor_to_body)); + SmartFactor::shared_ptr smartFactor2( + new SmartFactor(rankTol, linThreshold, manageDegeneracy, enableEPI, + sensor_to_body)); smartFactor2->add(measurements_cam2, views, model, K); - SmartFactor::shared_ptr smartFactor3(new SmartFactor(rankTol,linThreshold,manageDegeneracy,enableEPI,sensor_to_body)); + SmartFactor::shared_ptr smartFactor3( + new SmartFactor(rankTol, linThreshold, manageDegeneracy, enableEPI, + sensor_to_body)); smartFactor3->add(measurements_cam3, views, model, K); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -363,12 +402,13 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){ double expectedError = 0.0; DOUBLES_EQUAL(expectedError, actualError, 1e-7) - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); Values values; values.insert(x1, bodyPose1); values.insert(x2, bodyPose2); // initialize third pose with some noise, we expect it to move back to original pose3 - values.insert(x3, bodyPose3*noise_pose); + values.insert(x3, bodyPose3 * noise_pose); LevenbergMarquardtParams params; Values result; @@ -376,30 +416,55 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){ result = optimizer.optimize(); // result.print("results of 3 camera, 3 landmark optimization \n"); - if(isDebugTest) result.at(x3).print("Smart: Pose3 after optimization: "); - EXPECT(assert_equal(bodyPose3,result.at(x3))); + if (isDebugTest) + result.at(x3).print("Smart: Pose3 after optimization: "); + EXPECT(assert_equal(bodyPose3, result.at(x3))); + + // Check derivatives + + // Calculate expected derivative for point (easiest to check) + SmartFactor::Cameras cameras = smartFactor1->cameras(values); + boost::function f = // + boost::bind(&SmartFactor::whitenedError, *smartFactor1, cameras, _1); + boost::optional point = smartFactor1->point(); + CHECK(point); + Matrix expectedE = numericalDerivative11(f, *point); + + // Calculate using computeEP + Matrix actualE, PointCov; + smartFactor1->computeEP(actualE, PointCov, cameras); + EXPECT(assert_equal(expectedE, actualE, 1e-7)); + + // Calculate using whitenedError + Matrix F, actualE2; + Vector actualErrors = smartFactor1->whitenedError(cameras, *point, F, + actualE2); + EXPECT(assert_equal(expectedE, actualE2, 1e-7)); + + // TODO the derivatives agree with f, but returned errors are -f :-( + // TODO We should merge the two whitenedErrors functions and make derivatives optional + EXPECT(assert_equal(-f(*point), actualErrors, 1e-7)); } - /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ){ +TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) { // cout << " ************************ SmartProjectionPoseFactor: 3 cams + 3 landmarks **********************" << endl; - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); SimpleCamera cam1(pose1, *K); // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); SimpleCamera cam2(pose2, *K); // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); SimpleCamera cam3(pose3, *K); // three landmarks ~5 meters infront of camera @@ -432,48 +497,54 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ){ graph.push_back(PriorFactor(x1, pose1, noisePrior)); graph.push_back(PriorFactor(x2, pose2, noisePrior)); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, pose1); values.insert(x2, pose2); // initialize third pose with some noise, we expect it to move back to original pose3 - values.insert(x3, pose3*noise_pose); - if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); + values.insert(x3, pose3 * noise_pose); + if (isDebugTest) + values.at(x3).print("Smart: Pose3 before optimization: "); LevenbergMarquardtParams params; - if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; - if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; + if (isDebugTest) + params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; + if (isDebugTest) + params.verbosity = NonlinearOptimizerParams::ERROR; Values result; - gttic_(SmartProjectionPoseFactor); + gttic_ (SmartProjectionPoseFactor); LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); gttoc_(SmartProjectionPoseFactor); tictoc_finishedIteration_(); // result.print("results of 3 camera, 3 landmark optimization \n"); - if(isDebugTest) result.at(x3).print("Smart: Pose3 after optimization: "); - EXPECT(assert_equal(pose3,result.at(x3), 1e-7)); - if(isDebugTest) tictoc_print_(); + if (isDebugTest) + result.at(x3).print("Smart: Pose3 after optimization: "); + EXPECT(assert_equal(pose3, result.at(x3), 1e-7)); + if (isDebugTest) + tictoc_print_(); } /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, jacobianSVD ){ +TEST( SmartProjectionPoseFactor, jacobianSVD ) { - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); SimpleCamera cam1(pose1, *K); // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); SimpleCamera cam2(pose2, *K); // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); SimpleCamera cam3(pose3, *K); // three landmarks ~5 meters infront of camera @@ -488,13 +559,16 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ){ projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); + SmartFactor::shared_ptr smartFactor1( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); smartFactor1->add(measurements_cam1, views, model, K); - SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); + SmartFactor::shared_ptr smartFactor2( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); smartFactor2->add(measurements_cam2, views, model, K); - SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); + SmartFactor::shared_ptr smartFactor3( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); smartFactor3->add(measurements_cam3, views, model, K); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -506,38 +580,39 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ){ graph.push_back(PriorFactor(x1, pose1, noisePrior)); graph.push_back(PriorFactor(x2, pose2, noisePrior)); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, pose1); values.insert(x2, pose2); - values.insert(x3, pose3*noise_pose); + values.insert(x3, pose3 * noise_pose); LevenbergMarquardtParams params; Values result; LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); - EXPECT(assert_equal(pose3,result.at(x3), 1e-8)); + EXPECT(assert_equal(pose3, result.at(x3), 1e-8)); } /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, landmarkDistance ){ +TEST( SmartProjectionPoseFactor, landmarkDistance ) { double excludeLandmarksFutherThanDist = 2; - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); SimpleCamera cam1(pose1, *K); // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); SimpleCamera cam2(pose2, *K); // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); SimpleCamera cam3(pose3, *K); // three landmarks ~5 meters infront of camera @@ -552,13 +627,19 @@ TEST( SmartProjectionPoseFactor, landmarkDistance ){ projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); + SmartFactor::shared_ptr smartFactor1( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + excludeLandmarksFutherThanDist)); smartFactor1->add(measurements_cam1, views, model, K); - SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); + SmartFactor::shared_ptr smartFactor2( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + excludeLandmarksFutherThanDist)); smartFactor2->add(measurements_cam2, views, model, K); - SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); + SmartFactor::shared_ptr smartFactor3( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + excludeLandmarksFutherThanDist)); smartFactor3->add(measurements_cam3, views, model, K); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -570,40 +651,41 @@ TEST( SmartProjectionPoseFactor, landmarkDistance ){ graph.push_back(PriorFactor(x1, pose1, noisePrior)); graph.push_back(PriorFactor(x2, pose2, noisePrior)); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, pose1); values.insert(x2, pose2); - values.insert(x3, pose3*noise_pose); + values.insert(x3, pose3 * noise_pose); // All factors are disabled and pose should remain where it is LevenbergMarquardtParams params; Values result; LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); - EXPECT(assert_equal(values.at(x3),result.at(x3))); + EXPECT(assert_equal(values.at(x3), result.at(x3))); } /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ){ +TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) { double excludeLandmarksFutherThanDist = 1e10; double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); SimpleCamera cam1(pose1, *K); // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); SimpleCamera cam2(pose2, *K); // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); SimpleCamera cam3(pose3, *K); // three landmarks ~5 meters infront of camera @@ -612,29 +694,34 @@ TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ){ Point3 landmark3(3, 0, 3.0); Point3 landmark4(5, -0.5, 1); - vector measurements_cam1, measurements_cam2, measurements_cam3, measurements_cam4; + vector measurements_cam1, measurements_cam2, measurements_cam3, + measurements_cam4; // 1. Project three landmarks into three cameras and triangulate projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4); - measurements_cam4.at(0) = measurements_cam4.at(0) + Point2(10,10); // add outlier + measurements_cam4.at(0) = measurements_cam4.at(0) + Point2(10, 10); // add outlier - SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, - JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); + SmartFactor::shared_ptr smartFactor1( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); smartFactor1->add(measurements_cam1, views, model, K); - SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, - excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); + SmartFactor::shared_ptr smartFactor2( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); smartFactor2->add(measurements_cam2, views, model, K); - SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, - excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); + SmartFactor::shared_ptr smartFactor3( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); smartFactor3->add(measurements_cam3, views, model, K); - SmartFactor::shared_ptr smartFactor4(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, - excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); + SmartFactor::shared_ptr smartFactor4( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); smartFactor4->add(measurements_cam4, views, model, K); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -647,7 +734,8 @@ TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ){ graph.push_back(PriorFactor(x1, pose1, noisePrior)); graph.push_back(PriorFactor(x2, pose2, noisePrior)); - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, pose1); values.insert(x2, pose2); @@ -658,25 +746,25 @@ TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ){ Values result; LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); - EXPECT(assert_equal(pose3,result.at(x3))); + EXPECT(assert_equal(pose3, result.at(x3))); } /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, jacobianQ ){ +TEST( SmartProjectionPoseFactor, jacobianQ ) { - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); SimpleCamera cam1(pose1, *K); // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); SimpleCamera cam2(pose2, *K); // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); SimpleCamera cam3(pose3, *K); // three landmarks ~5 meters infront of camera @@ -691,13 +779,16 @@ TEST( SmartProjectionPoseFactor, jacobianQ ){ projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q)); + SmartFactor::shared_ptr smartFactor1( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q)); smartFactor1->add(measurements_cam1, views, model, K); - SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q)); + SmartFactor::shared_ptr smartFactor2( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q)); smartFactor2->add(measurements_cam2, views, model, K); - SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q)); + SmartFactor::shared_ptr smartFactor3( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q)); smartFactor3->add(measurements_cam3, views, model, K); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -709,39 +800,40 @@ TEST( SmartProjectionPoseFactor, jacobianQ ){ graph.push_back(PriorFactor(x1, pose1, noisePrior)); graph.push_back(PriorFactor(x2, pose2, noisePrior)); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, pose1); values.insert(x2, pose2); - values.insert(x3, pose3*noise_pose); + values.insert(x3, pose3 * noise_pose); LevenbergMarquardtParams params; Values result; LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); - EXPECT(assert_equal(pose3,result.at(x3), 1e-8)); + EXPECT(assert_equal(pose3, result.at(x3), 1e-8)); } /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, 3poses_projection_factor ){ +TEST( SmartProjectionPoseFactor, 3poses_projection_factor ) { // cout << " ************************ Normal ProjectionFactor: 3 cams + 3 landmarks **********************" << endl; - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); SimpleCamera cam1(pose1, *K2); // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); SimpleCamera cam2(pose2, *K2); // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); SimpleCamera cam3(pose3, *K2); // three landmarks ~5 meters infront of camera @@ -753,60 +845,74 @@ TEST( SmartProjectionPoseFactor, 3poses_projection_factor ){ NonlinearFactorGraph graph; // 1. Project three landmarks into three cameras and triangulate - graph.push_back(ProjectionFactor(cam1.project(landmark1), model, x1, L(1), K2)); - graph.push_back(ProjectionFactor(cam2.project(landmark1), model, x2, L(1), K2)); - graph.push_back(ProjectionFactor(cam3.project(landmark1), model, x3, L(1), K2)); + graph.push_back( + ProjectionFactor(cam1.project(landmark1), model, x1, L(1), K2)); + graph.push_back( + ProjectionFactor(cam2.project(landmark1), model, x2, L(1), K2)); + graph.push_back( + ProjectionFactor(cam3.project(landmark1), model, x3, L(1), K2)); - graph.push_back(ProjectionFactor(cam1.project(landmark2), model, x1, L(2), K2)); - graph.push_back(ProjectionFactor(cam2.project(landmark2), model, x2, L(2), K2)); - graph.push_back(ProjectionFactor(cam3.project(landmark2), model, x3, L(2), K2)); + graph.push_back( + ProjectionFactor(cam1.project(landmark2), model, x1, L(2), K2)); + graph.push_back( + ProjectionFactor(cam2.project(landmark2), model, x2, L(2), K2)); + graph.push_back( + ProjectionFactor(cam3.project(landmark2), model, x3, L(2), K2)); - graph.push_back(ProjectionFactor(cam1.project(landmark3), model, x1, L(3), K2)); - graph.push_back(ProjectionFactor(cam2.project(landmark3), model, x2, L(3), K2)); - graph.push_back(ProjectionFactor(cam3.project(landmark3), model, x3, L(3), K2)); + graph.push_back( + ProjectionFactor(cam1.project(landmark3), model, x1, L(3), K2)); + graph.push_back( + ProjectionFactor(cam2.project(landmark3), model, x2, L(3), K2)); + graph.push_back( + ProjectionFactor(cam3.project(landmark3), model, x3, L(3), K2)); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); graph.push_back(PriorFactor(x1, pose1, noisePrior)); graph.push_back(PriorFactor(x2, pose2, noisePrior)); - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10), + Point3(0.5, 0.1, 0.3)); Values values; values.insert(x1, pose1); values.insert(x2, pose2); - values.insert(x3, pose3* noise_pose); + values.insert(x3, pose3 * noise_pose); values.insert(L(1), landmark1); values.insert(L(2), landmark2); values.insert(L(3), landmark3); - if(isDebugTest) values.at(x3).print("Pose3 before optimization: "); + if (isDebugTest) + values.at(x3).print("Pose3 before optimization: "); LevenbergMarquardtParams params; - if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; - if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; + if (isDebugTest) + params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; + if (isDebugTest) + params.verbosity = NonlinearOptimizerParams::ERROR; LevenbergMarquardtOptimizer optimizer(graph, values, params); Values result = optimizer.optimize(); - if(isDebugTest) result.at(x3).print("Pose3 after optimization: "); - EXPECT(assert_equal(pose3,result.at(x3), 1e-7)); + if (isDebugTest) + result.at(x3).print("Pose3 after optimization: "); + EXPECT(assert_equal(pose3, result.at(x3), 1e-7)); } /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, CheckHessian){ +TEST( SmartProjectionPoseFactor, CheckHessian) { - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); SimpleCamera cam1(pose1, *K); // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); + Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); SimpleCamera cam2(pose2, *K); // create third camera 1 meter above the first camera - Pose3 pose3 = pose2 * 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)); SimpleCamera cam3(pose3, *K); // three landmarks ~5 meters infront of camera @@ -837,57 +943,65 @@ TEST( SmartProjectionPoseFactor, CheckHessian){ graph.push_back(smartFactor2); graph.push_back(smartFactor3); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, pose1); values.insert(x2, pose2); // initialize third pose with some noise, we expect it to move back to original pose3 - values.insert(x3, pose3*noise_pose); - if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); + values.insert(x3, pose3 * noise_pose); + if (isDebugTest) + values.at(x3).print("Smart: Pose3 before optimization: "); - boost::shared_ptr hessianFactor1 = smartFactor1->linearize(values); - boost::shared_ptr hessianFactor2 = smartFactor2->linearize(values); - boost::shared_ptr hessianFactor3 = smartFactor3->linearize(values); + boost::shared_ptr hessianFactor1 = smartFactor1->linearize( + values); + boost::shared_ptr hessianFactor2 = smartFactor2->linearize( + values); + boost::shared_ptr hessianFactor3 = smartFactor3->linearize( + values); - Matrix CumulativeInformation = hessianFactor1->information() + hessianFactor2->information() + hessianFactor3->information(); + Matrix CumulativeInformation = hessianFactor1->information() + + hessianFactor2->information() + hessianFactor3->information(); - boost::shared_ptr GaussianGraph = graph.linearize(values); + boost::shared_ptr GaussianGraph = graph.linearize( + values); Matrix GraphInformation = GaussianGraph->hessian().first; // Check Hessian EXPECT(assert_equal(GraphInformation, CumulativeInformation, 1e-8)); - Matrix AugInformationMatrix = hessianFactor1->augmentedInformation() + - hessianFactor2->augmentedInformation() + hessianFactor3->augmentedInformation(); + Matrix AugInformationMatrix = hessianFactor1->augmentedInformation() + + hessianFactor2->augmentedInformation() + + hessianFactor3->augmentedInformation(); // Check Information vector // cout << AugInformationMatrix.size() << endl; - Vector InfoVector = AugInformationMatrix.block(0,18,18,1); // 18x18 Hessian + information vector + Vector InfoVector = AugInformationMatrix.block(0, 18, 18, 1); // 18x18 Hessian + information vector // Check Hessian EXPECT(assert_equal(InfoVector, GaussianGraph->hessian().second, 1e-8)); } /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_factor ){ +TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_factor ) { // cout << " ************************ SmartProjectionPoseFactor: 3 cams + 2 landmarks: Rotation Only**********************" << endl; - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); SimpleCamera cam1(pose1, *K2); // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); + Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); SimpleCamera cam2(pose2, *K2); // create third camera 1 meter above the first camera - Pose3 pose3 = pose2 * 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)); SimpleCamera cam3(pose3, *K2); // three landmarks ~5 meters infront of camera @@ -901,68 +1015,81 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); double rankTol = 50; - SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol, linThreshold, manageDegeneracy)); + SmartFactor::shared_ptr smartFactor1( + new SmartFactor(rankTol, linThreshold, manageDegeneracy)); smartFactor1->add(measurements_cam1, views, model, K2); - SmartFactor::shared_ptr smartFactor2(new SmartFactor(rankTol, linThreshold, manageDegeneracy)); + SmartFactor::shared_ptr smartFactor2( + new SmartFactor(rankTol, linThreshold, manageDegeneracy)); smartFactor2->add(measurements_cam2, views, model, K2); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, 0.10); - Point3 positionPrior = gtsam::Point3(0,0,1); + const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, + 0.10); + Point3 positionPrior = Point3(0, 0, 1); NonlinearFactorGraph graph; graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(PriorFactor(x1, pose1, noisePrior)); - graph.push_back(PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); - graph.push_back(PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); + graph.push_back( + PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); + graph.push_back( + PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10), + Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, pose1); - values.insert(x2, pose2*noise_pose); + values.insert(x2, pose2 * noise_pose); // initialize third pose with some noise, we expect it to move back to original pose3 - values.insert(x3, pose3*noise_pose*noise_pose); - if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); + values.insert(x3, pose3 * noise_pose * noise_pose); + if (isDebugTest) + values.at(x3).print("Smart: Pose3 before optimization: "); LevenbergMarquardtParams params; - if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYDELTA; - if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; + if (isDebugTest) + params.verbosityLM = LevenbergMarquardtParams::TRYDELTA; + if (isDebugTest) + params.verbosity = NonlinearOptimizerParams::ERROR; Values result; - gttic_(SmartProjectionPoseFactor); + gttic_ (SmartProjectionPoseFactor); LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); gttoc_(SmartProjectionPoseFactor); tictoc_finishedIteration_(); // result.print("results of 3 camera, 3 landmark optimization \n"); - if(isDebugTest) result.at(x3).print("Smart: Pose3 after optimization: "); - std::cout << "TEST COMMENTED: rotation only version of smart factors has been deprecated " << std::endl; + if (isDebugTest) + result.at(x3).print("Smart: Pose3 after optimization: "); + cout + << "TEST COMMENTED: rotation only version of smart factors has been deprecated " + << endl; // EXPECT(assert_equal(pose3,result.at(x3))); - if(isDebugTest) tictoc_print_(); + if (isDebugTest) + tictoc_print_(); } /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ){ +TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ) { // cout << " ************************ SmartProjectionPoseFactor: 3 cams + 3 landmarks: Rotation Only**********************" << endl; - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); SimpleCamera cam1(pose1, *K); // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); + Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); SimpleCamera cam2(pose2, *K); // create third camera 1 meter above the first camera - Pose3 pose3 = pose2 * 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)); SimpleCamera cam3(pose3, *K); // three landmarks ~5 meters infront of camera @@ -979,68 +1106,82 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ){ double rankTol = 10; - SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol, linThreshold, manageDegeneracy)); + SmartFactor::shared_ptr smartFactor1( + new SmartFactor(rankTol, linThreshold, manageDegeneracy)); smartFactor1->add(measurements_cam1, views, model, K); - SmartFactor::shared_ptr smartFactor2(new SmartFactor(rankTol, linThreshold, manageDegeneracy)); + SmartFactor::shared_ptr smartFactor2( + new SmartFactor(rankTol, linThreshold, manageDegeneracy)); smartFactor2->add(measurements_cam2, views, model, K); - SmartFactor::shared_ptr smartFactor3(new SmartFactor(rankTol, linThreshold, manageDegeneracy)); + SmartFactor::shared_ptr smartFactor3( + new SmartFactor(rankTol, linThreshold, manageDegeneracy)); smartFactor3->add(measurements_cam3, views, model, K); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, 0.10); - Point3 positionPrior = gtsam::Point3(0,0,1); + const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, + 0.10); + Point3 positionPrior = Point3(0, 0, 1); NonlinearFactorGraph graph; graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); graph.push_back(PriorFactor(x1, pose1, noisePrior)); - graph.push_back(PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); - graph.push_back(PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); + graph.push_back( + PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); + graph.push_back( + PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, pose1); values.insert(x2, pose2); // initialize third pose with some noise, we expect it to move back to original pose3 - values.insert(x3, pose3*noise_pose); - if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); + values.insert(x3, pose3 * noise_pose); + if (isDebugTest) + values.at(x3).print("Smart: Pose3 before optimization: "); LevenbergMarquardtParams params; - if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYDELTA; - if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; + if (isDebugTest) + params.verbosityLM = LevenbergMarquardtParams::TRYDELTA; + if (isDebugTest) + params.verbosity = NonlinearOptimizerParams::ERROR; Values result; - gttic_(SmartProjectionPoseFactor); + gttic_ (SmartProjectionPoseFactor); LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); gttoc_(SmartProjectionPoseFactor); tictoc_finishedIteration_(); // result.print("results of 3 camera, 3 landmark optimization \n"); - if(isDebugTest) result.at(x3).print("Smart: Pose3 after optimization: "); - std::cout << "TEST COMMENTED: rotation only version of smart factors has been deprecated " << std::endl; + if (isDebugTest) + result.at(x3).print("Smart: Pose3 after optimization: "); + cout + << "TEST COMMENTED: rotation only version of smart factors has been deprecated " + << endl; // EXPECT(assert_equal(pose3,result.at(x3))); - if(isDebugTest) tictoc_print_(); + if (isDebugTest) + tictoc_print_(); } /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, Hessian ){ +TEST( SmartProjectionPoseFactor, Hessian ) { // cout << " ************************ SmartProjectionPoseFactor: Hessian **********************" << endl; - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); SimpleCamera cam1(pose1, *K2); // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); SimpleCamera cam2(pose2, *K2); // three landmarks ~5 meters infront of camera @@ -1054,15 +1195,18 @@ TEST( SmartProjectionPoseFactor, Hessian ){ measurements_cam1.push_back(cam2_uv1); SmartFactor::shared_ptr smartFactor1(new SmartFactor()); - smartFactor1->add(measurements_cam1,views, model, K2); + smartFactor1->add(measurements_cam1, views, model, K2); - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10), + Point3(0.5, 0.1, 0.3)); Values values; values.insert(x1, pose1); values.insert(x2, pose2); - boost::shared_ptr hessianFactor = smartFactor1->linearize(values); - if(isDebugTest) hessianFactor->print("Hessian factor \n"); + boost::shared_ptr hessianFactor = smartFactor1->linearize( + values); + if (isDebugTest) + hessianFactor->print("Hessian factor \n"); // compute triangulation from linearization point // compute reprojection errors (sum squared) @@ -1070,26 +1214,25 @@ TEST( SmartProjectionPoseFactor, Hessian ){ // check that it is correctly scaled when using noiseProjection = [1/4 0; 0 1/4] } - /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, HessianWithRotation ){ +TEST( SmartProjectionPoseFactor, HessianWithRotation ) { // cout << " ************************ SmartProjectionPoseFactor: rotated Hessian **********************" << endl; - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); SimpleCamera cam1(pose1, *K); // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); SimpleCamera cam2(pose2, *K); // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); SimpleCamera cam3(pose3, *K); Point3 landmark1(5, 0.5, 1.2); @@ -1106,54 +1249,62 @@ TEST( SmartProjectionPoseFactor, HessianWithRotation ){ values.insert(x2, pose2); values.insert(x3, pose3); - boost::shared_ptr hessianFactor = smartFactorInstance->linearize(values); + boost::shared_ptr hessianFactor = + smartFactorInstance->linearize(values); // hessianFactor->print("Hessian factor \n"); - Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,0)); + Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0)); Values rotValues; rotValues.insert(x1, poseDrift.compose(pose1)); rotValues.insert(x2, poseDrift.compose(pose2)); rotValues.insert(x3, poseDrift.compose(pose3)); - boost::shared_ptr hessianFactorRot = smartFactorInstance->linearize(rotValues); + boost::shared_ptr hessianFactorRot = + smartFactorInstance->linearize(rotValues); // hessianFactorRot->print("Hessian factor \n"); // Hessian is invariant to rotations in the nondegenerate case - EXPECT(assert_equal(hessianFactor->information(), hessianFactorRot->information(), 1e-8) ); + EXPECT( + assert_equal(hessianFactor->information(), + hessianFactorRot->information(), 1e-7)); - Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI/2, -M_PI/3, -M_PI/2), gtsam::Point3(10,-4,5)); + Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2), + Point3(10, -4, 5)); Values tranValues; tranValues.insert(x1, poseDrift2.compose(pose1)); tranValues.insert(x2, poseDrift2.compose(pose2)); tranValues.insert(x3, poseDrift2.compose(pose3)); - boost::shared_ptr hessianFactorRotTran = smartFactorInstance->linearize(tranValues); + boost::shared_ptr hessianFactorRotTran = + smartFactorInstance->linearize(tranValues); // Hessian is invariant to rotations and translations in the nondegenerate case - EXPECT(assert_equal(hessianFactor->information(), hessianFactorRotTran->information(), 1e-8) ); + EXPECT( + assert_equal(hessianFactor->information(), + hessianFactorRotTran->information(), 1e-7)); } /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ){ +TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ) { // cout << " ************************ SmartProjectionPoseFactor: rotated Hessian (degenerate) **********************" << endl; - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); SimpleCamera cam1(pose1, *K2); // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(0,0,0)); + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(0, 0, 0)); SimpleCamera cam2(pose2, *K2); // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,0,0)); + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, 0, 0)); SimpleCamera cam3(pose3, *K2); Point3 landmark1(5, 0.5, 1.2); @@ -1165,62 +1316,72 @@ TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ){ SmartFactor::shared_ptr smartFactor(new SmartFactor()); smartFactor->add(measurements_cam1, views, model, K2); - Values values; values.insert(x1, pose1); values.insert(x2, pose2); values.insert(x3, pose3); - boost::shared_ptr hessianFactor = smartFactor->linearize(values); - if(isDebugTest) hessianFactor->print("Hessian factor \n"); + boost::shared_ptr hessianFactor = smartFactor->linearize( + values); + if (isDebugTest) + hessianFactor->print("Hessian factor \n"); - Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,0)); + Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0)); Values rotValues; rotValues.insert(x1, poseDrift.compose(pose1)); rotValues.insert(x2, poseDrift.compose(pose2)); rotValues.insert(x3, poseDrift.compose(pose3)); - boost::shared_ptr hessianFactorRot = smartFactor->linearize(rotValues); - if(isDebugTest) hessianFactorRot->print("Hessian factor \n"); + boost::shared_ptr hessianFactorRot = smartFactor->linearize( + rotValues); + if (isDebugTest) + hessianFactorRot->print("Hessian factor \n"); // Hessian is invariant to rotations in the nondegenerate case - EXPECT(assert_equal(hessianFactor->information(), hessianFactorRot->information(), 1e-8) ); + EXPECT( + assert_equal(hessianFactor->information(), + hessianFactorRot->information(), 1e-8)); - Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI/2, -M_PI/3, -M_PI/2), gtsam::Point3(10,-4,5)); + Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2), + Point3(10, -4, 5)); Values tranValues; tranValues.insert(x1, poseDrift2.compose(pose1)); tranValues.insert(x2, poseDrift2.compose(pose2)); tranValues.insert(x3, poseDrift2.compose(pose3)); - boost::shared_ptr hessianFactorRotTran = smartFactor->linearize(tranValues); + boost::shared_ptr hessianFactorRotTran = + smartFactor->linearize(tranValues); // Hessian is invariant to rotations and translations in the nondegenerate case - EXPECT(assert_equal(hessianFactor->information(), hessianFactorRotTran->information(), 1e-8) ); + EXPECT( + assert_equal(hessianFactor->information(), + hessianFactorRotTran->information(), 1e-8)); } /* ************************************************************************* */ TEST( SmartProjectionPoseFactor, ConstructorWithCal3Bundler) { - SmartFactorBundler factor1(rankTol, linThreshold); - boost::shared_ptr Kbundler(new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000)); - factor1.add(measurement1, poseKey1, model, Kbundler); + SmartFactorBundler factor(rankTol, linThreshold); + boost::shared_ptr Kbundler( + new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000)); + factor.add(measurement1, poseKey1, model, Kbundler); } /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, Cal3Bundler ){ +TEST( SmartProjectionPoseFactor, Cal3Bundler ) { // cout << " ************************ SmartProjectionPoseFactor: Cal3Bundler **********************" << endl; // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); PinholeCamera cam1(pose1, *Kbundler); // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); PinholeCamera cam2(pose2, *Kbundler); // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); PinholeCamera cam3(pose3, *Kbundler); // three landmarks ~5 meters infront of camera @@ -1252,7 +1413,7 @@ TEST( SmartProjectionPoseFactor, Cal3Bundler ){ measurements_cam3.push_back(cam2_uv3); measurements_cam3.push_back(cam3_uv3); - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); @@ -1275,50 +1436,56 @@ TEST( SmartProjectionPoseFactor, Cal3Bundler ){ graph.push_back(PriorFactor(x1, pose1, noisePrior)); graph.push_back(PriorFactor(x2, pose2, noisePrior)); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, pose1); values.insert(x2, pose2); // initialize third pose with some noise, we expect it to move back to original pose3 - values.insert(x3, pose3*noise_pose); - if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); + values.insert(x3, pose3 * noise_pose); + if (isDebugTest) + values.at(x3).print("Smart: Pose3 before optimization: "); LevenbergMarquardtParams params; - if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; - if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; + if (isDebugTest) + params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; + if (isDebugTest) + params.verbosity = NonlinearOptimizerParams::ERROR; Values result; - gttic_(SmartProjectionPoseFactor); + gttic_ (SmartProjectionPoseFactor); LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); gttoc_(SmartProjectionPoseFactor); tictoc_finishedIteration_(); // result.print("results of 3 camera, 3 landmark optimization \n"); - if(isDebugTest) result.at(x3).print("Smart: Pose3 after optimization: "); - EXPECT(assert_equal(pose3,result.at(x3), 1e-6)); - if(isDebugTest) tictoc_print_(); - } + if (isDebugTest) + result.at(x3).print("Smart: Pose3 after optimization: "); + EXPECT(assert_equal(pose3, result.at(x3), 1e-6)); + if (isDebugTest) + tictoc_print_(); +} /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ){ +TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) { - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); PinholeCamera cam1(pose1, *Kbundler); // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); + Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); PinholeCamera cam2(pose2, *Kbundler); // create third camera 1 meter above the first camera - Pose3 pose3 = pose2 * 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)); PinholeCamera cam3(pose3, *Kbundler); // three landmarks ~5 meters infront of camera @@ -1352,56 +1519,72 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ){ double rankTol = 10; - SmartFactorBundler::shared_ptr smartFactor1(new SmartFactorBundler(rankTol, linThreshold, manageDegeneracy)); + SmartFactorBundler::shared_ptr smartFactor1( + new SmartFactorBundler(rankTol, linThreshold, manageDegeneracy)); smartFactor1->add(measurements_cam1, views, model, Kbundler); - SmartFactorBundler::shared_ptr smartFactor2(new SmartFactorBundler(rankTol, linThreshold, manageDegeneracy)); + SmartFactorBundler::shared_ptr smartFactor2( + new SmartFactorBundler(rankTol, linThreshold, manageDegeneracy)); smartFactor2->add(measurements_cam2, views, model, Kbundler); - SmartFactorBundler::shared_ptr smartFactor3(new SmartFactorBundler(rankTol, linThreshold, manageDegeneracy)); + SmartFactorBundler::shared_ptr smartFactor3( + new SmartFactorBundler(rankTol, linThreshold, manageDegeneracy)); smartFactor3->add(measurements_cam3, views, model, Kbundler); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, 0.10); - Point3 positionPrior = gtsam::Point3(0,0,1); + const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, + 0.10); + Point3 positionPrior = Point3(0, 0, 1); NonlinearFactorGraph graph; graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); graph.push_back(PriorFactor(x1, pose1, noisePrior)); - graph.push_back(PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); - graph.push_back(PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); + graph.push_back( + PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); + graph.push_back( + PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, pose1); values.insert(x2, pose2); // initialize third pose with some noise, we expect it to move back to original pose3 - values.insert(x3, pose3*noise_pose); - if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); + values.insert(x3, pose3 * noise_pose); + if (isDebugTest) + values.at(x3).print("Smart: Pose3 before optimization: "); LevenbergMarquardtParams params; - if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYDELTA; - if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; + if (isDebugTest) + params.verbosityLM = LevenbergMarquardtParams::TRYDELTA; + if (isDebugTest) + params.verbosity = NonlinearOptimizerParams::ERROR; Values result; - gttic_(SmartProjectionPoseFactor); + gttic_ (SmartProjectionPoseFactor); LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); gttoc_(SmartProjectionPoseFactor); tictoc_finishedIteration_(); // result.print("results of 3 camera, 3 landmark optimization \n"); - if(isDebugTest) result.at(x3).print("Smart: Pose3 after optimization: "); - std::cout << "TEST COMMENTED: rotation only version of smart factors has been deprecated " << std::endl; + if (isDebugTest) + result.at(x3).print("Smart: Pose3 after optimization: "); + cout + << "TEST COMMENTED: rotation only version of smart factors has been deprecated " + << endl; // EXPECT(assert_equal(pose3,result.at(x3))); - if(isDebugTest) tictoc_print_(); + if (isDebugTest) + tictoc_print_(); } /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ - diff --git a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp index bab9712c2..4cc8e66ff 100644 --- a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp @@ -32,42 +32,44 @@ using namespace std; using namespace boost::assign; using namespace gtsam; -static bool isDebugTest = true; +static bool isDebugTest = false; // make a realistic calibration matrix static double fov = 60; // degrees -static size_t w=640,h=480; +static size_t w = 640, h = 480; static double b = 1; -static Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fov,w,h,b)); -static Cal3_S2Stereo::shared_ptr K2(new Cal3_S2Stereo(1500, 1200, 0, 640, 480,b)); -static boost::shared_ptr Kbundler(new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000)); +static Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fov, w, h, b)); +static Cal3_S2Stereo::shared_ptr K2( + new Cal3_S2Stereo(1500, 1200, 0, 640, 480, b)); +static boost::shared_ptr Kbundler( + new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000)); static double rankTol = 1.0; static double linThreshold = -1.0; // static bool manageDegeneracy = true; // Create a noise model for the pixel error -static SharedNoiseModel model(noiseModel::Unit::Create(3)); +static SharedNoiseModel model(noiseModel::Isotropic::Sigma(3, 0.1)); // Convenience for named keys using symbol_shorthand::X; using symbol_shorthand::L; // tests data -static Symbol x1('X', 1); -static Symbol x2('X', 2); -static Symbol x3('X', 3); +static Symbol x1('X', 1); +static Symbol x2('X', 2); +static Symbol x3('X', 3); static Key poseKey1(x1); static StereoPoint2 measurement1(323.0, 300.0, 240.0); //potentially use more reasonable measurement value? -static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); +static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), + Point3(0.25, -0.10, 1.0)); typedef SmartStereoProjectionPoseFactor SmartFactor; typedef SmartStereoProjectionPoseFactor SmartFactorBundler; -vector stereo_projectToMultipleCameras( - const StereoCamera& cam1, const StereoCamera& cam2, - const StereoCamera& cam3, Point3 landmark){ +vector stereo_projectToMultipleCameras(const StereoCamera& cam1, + const StereoCamera& cam2, const StereoCamera& cam3, Point3 landmark) { vector measurements_cam; @@ -83,7 +85,7 @@ vector stereo_projectToMultipleCameras( /* ************************************************************************* */ TEST( SmartStereoProjectionPoseFactor, Constructor) { - fprintf(stderr,"Test 1 Complete"); + fprintf(stderr, "Test 1 Complete"); SmartFactor::shared_ptr factor1(new SmartFactor()); } @@ -108,7 +110,8 @@ TEST( SmartStereoProjectionPoseFactor, Constructor4) { TEST( SmartStereoProjectionPoseFactor, ConstructorWithTransform) { bool manageDegeneracy = true; bool enableEPI = false; - SmartFactor factor1(rankTol, linThreshold, manageDegeneracy, enableEPI, body_P_sensor1); + SmartFactor factor1(rankTol, linThreshold, manageDegeneracy, enableEPI, + body_P_sensor1); factor1.add(measurement1, poseKey1, model, K); } @@ -124,15 +127,16 @@ TEST( SmartStereoProjectionPoseFactor, Equals ) { } /* *************************************************************************/ -TEST_UNSAFE( SmartStereoProjectionPoseFactor, noiseless ){ +TEST_UNSAFE( SmartStereoProjectionPoseFactor, noiseless ) { // cout << " ************************ SmartStereoProjectionPoseFactor: noisy ****************************" << endl; // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + Pose3 level_pose = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), + Point3(0, 0, 1)); StereoCamera level_camera(level_pose, K2); // create second camera 1 meter to the right of first camera - Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1,0,0)); + Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1, 0, 0)); StereoCamera level_camera_right(level_pose_right, K2); // landmark ~5 meters infront of camera @@ -164,75 +168,78 @@ TEST_UNSAFE( SmartStereoProjectionPoseFactor, noiseless ){ } /* *************************************************************************/ -TEST( SmartStereoProjectionPoseFactor, noisy ){ +TEST( SmartStereoProjectionPoseFactor, noisy ) { // cout << " ************************ SmartStereoProjectionPoseFactor: noisy ****************************" << endl; // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + Pose3 level_pose = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), + Point3(0, 0, 1)); StereoCamera level_camera(level_pose, K2); // create second camera 1 meter to the right of first camera - Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1,0,0)); + Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1, 0, 0)); StereoCamera level_camera_right(level_pose_right, K2); // landmark ~5 meters infront of camera Point3 landmark(5, 0.5, 1.2); // 1. Project two landmarks into two cameras and triangulate - StereoPoint2 pixelError(0.2,0.2,0); + StereoPoint2 pixelError(0.2, 0.2, 0); StereoPoint2 level_uv = level_camera.project(landmark) + pixelError; StereoPoint2 level_uv_right = level_camera_right.project(landmark); Values values; values.insert(x1, level_pose); - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10), + Point3(0.5, 0.1, 0.3)); values.insert(x2, level_pose_right.compose(noise_pose)); SmartFactor::shared_ptr factor1(new SmartFactor()); factor1->add(level_uv, x1, model, K); factor1->add(level_uv_right, x2, model, K); - double actualError1= factor1->error(values); + double actualError1 = factor1->error(values); SmartFactor::shared_ptr factor2(new SmartFactor()); vector measurements; measurements.push_back(level_uv); measurements.push_back(level_uv_right); - std::vector< SharedNoiseModel > noises; + vector noises; noises.push_back(model); noises.push_back(model); - std::vector< boost::shared_ptr > Ks; ///< shared pointer to calibration object (one for each camera) + vector > Ks; ///< shared pointer to calibration object (one for each camera) Ks.push_back(K); Ks.push_back(K); - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); factor2->add(measurements, views, noises, Ks); - double actualError2= factor2->error(values); + double actualError2 = factor2->error(values); DOUBLES_EQUAL(actualError1, actualError2, 1e-7); } - /* *************************************************************************/ -TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ){ - cout << " ************************ SmartStereoProjectionPoseFactor: 3 cams + 3 landmarks **********************" << endl; +TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) { + cout + << " ************************ SmartStereoProjectionPoseFactor: 3 cams + 3 landmarks **********************" + << endl; // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); StereoCamera cam1(pose1, K2); // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); StereoCamera cam2(pose2, K2); // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); StereoCamera cam3(pose3, K2); // three landmarks ~5 meters infront of camera @@ -241,11 +248,14 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ){ Point3 landmark3(3, 0, 3.0); // 1. Project three landmarks into three cameras and triangulate - vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1); - vector measurements_cam2 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2); - vector measurements_cam3 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3); + vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark1); + vector measurements_cam2 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark2); + vector measurements_cam3 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark3); - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); @@ -268,21 +278,25 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ){ graph.push_back(PriorFactor(x1, pose1, noisePrior)); graph.push_back(PriorFactor(x2, pose2, noisePrior)); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, pose1); values.insert(x2, pose2); // initialize third pose with some noise, we expect it to move back to original pose3 - values.insert(x3, pose3*noise_pose); - if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); + values.insert(x3, pose3 * noise_pose); + if (isDebugTest) + values.at(x3).print("Smart: Pose3 before optimization: "); LevenbergMarquardtParams params; - if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; - if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; + if (isDebugTest) + params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; + if (isDebugTest) + params.verbosity = NonlinearOptimizerParams::ERROR; Values result; - gttic_(SmartStereoProjectionPoseFactor); + gttic_ (SmartStereoProjectionPoseFactor); LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); gttoc_(SmartStereoProjectionPoseFactor); @@ -292,28 +306,29 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ){ // VectorValues delta = GFG->optimize(); // result.print("results of 3 camera, 3 landmark optimization \n"); - if(isDebugTest) result.at(x3).print("Smart: Pose3 after optimization: "); - EXPECT(assert_equal(pose3,result.at(x3))); - if(isDebugTest) tictoc_print_(); + if (isDebugTest) + result.at(x3).print("Smart: Pose3 after optimization: "); + EXPECT(assert_equal(pose3, result.at(x3))); + if (isDebugTest) + tictoc_print_(); } - /* *************************************************************************/ -TEST( SmartStereoProjectionPoseFactor, jacobianSVD ){ +TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) { - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); StereoCamera cam1(pose1, K); // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); StereoCamera cam2(pose2, K); // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); StereoCamera cam3(pose3, K); // three landmarks ~5 meters infront of camera @@ -322,17 +337,23 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ){ Point3 landmark3(3, 0, 3.0); // 1. Project three landmarks into three cameras and triangulate - vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1); - vector measurements_cam2 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2); - vector measurements_cam3 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3); + vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark1); + vector measurements_cam2 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark2); + vector measurements_cam3 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark3); - SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); + SmartFactor::shared_ptr smartFactor1( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); smartFactor1->add(measurements_cam1, views, model, K); - SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); + SmartFactor::shared_ptr smartFactor2( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); smartFactor2->add(measurements_cam2, views, model, K); - SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); + SmartFactor::shared_ptr smartFactor3( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); smartFactor3->add(measurements_cam3, views, model, K); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -344,38 +365,39 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ){ graph.push_back(PriorFactor(x1, pose1, noisePrior)); graph.push_back(PriorFactor(x2, pose2, noisePrior)); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, pose1); values.insert(x2, pose2); - values.insert(x3, pose3*noise_pose); + values.insert(x3, pose3 * noise_pose); LevenbergMarquardtParams params; Values result; LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); - EXPECT(assert_equal(pose3,result.at(x3))); + EXPECT(assert_equal(pose3, result.at(x3))); } /* *************************************************************************/ -TEST( SmartStereoProjectionPoseFactor, landmarkDistance ){ +TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) { double excludeLandmarksFutherThanDist = 2; - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); StereoCamera cam1(pose1, K); // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); StereoCamera cam2(pose2, K); // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); StereoCamera cam3(pose3, K); // three landmarks ~5 meters infront of camera @@ -384,18 +406,26 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ){ Point3 landmark3(3, 0, 3.0); // 1. Project three landmarks into three cameras and triangulate - vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1); - vector measurements_cam2 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2); - vector measurements_cam3 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3); + vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark1); + vector measurements_cam2 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark2); + vector measurements_cam3 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark3); - - SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); + SmartFactor::shared_ptr smartFactor1( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + excludeLandmarksFutherThanDist)); smartFactor1->add(measurements_cam1, views, model, K); - SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); + SmartFactor::shared_ptr smartFactor2( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + excludeLandmarksFutherThanDist)); smartFactor2->add(measurements_cam2, views, model, K); - SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); + SmartFactor::shared_ptr smartFactor3( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + excludeLandmarksFutherThanDist)); smartFactor3->add(measurements_cam3, views, model, K); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -407,40 +437,41 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ){ graph.push_back(PriorFactor(x1, pose1, noisePrior)); graph.push_back(PriorFactor(x2, pose2, noisePrior)); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, pose1); values.insert(x2, pose2); - values.insert(x3, pose3*noise_pose); + values.insert(x3, pose3 * noise_pose); // All factors are disabled and pose should remain where it is LevenbergMarquardtParams params; Values result; LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); - EXPECT(assert_equal(values.at(x3),result.at(x3))); + EXPECT(assert_equal(values.at(x3), result.at(x3))); } /* *************************************************************************/ -TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ){ +TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) { double excludeLandmarksFutherThanDist = 1e10; double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); StereoCamera cam1(pose1, K); // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); StereoCamera cam2(pose2, K); // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); StereoCamera cam3(pose3, K); // three landmarks ~5 meters infront of camera @@ -450,28 +481,35 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ){ Point3 landmark4(5, -0.5, 1); // 1. Project four landmarks into three cameras and triangulate - vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1); - vector measurements_cam2 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2); - vector measurements_cam3 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3); - vector measurements_cam4 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark4); + vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark1); + vector measurements_cam2 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark2); + vector measurements_cam3 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark3); + vector measurements_cam4 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark4); + measurements_cam4.at(0) = measurements_cam4.at(0) + StereoPoint2(10, 10, 1); // add outlier - measurements_cam4.at(0) = measurements_cam4.at(0) + StereoPoint2(10,10,1); // add outlier - - SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, - JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); + SmartFactor::shared_ptr smartFactor1( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); smartFactor1->add(measurements_cam1, views, model, K); - SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, - excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); + SmartFactor::shared_ptr smartFactor2( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); smartFactor2->add(measurements_cam2, views, model, K); - SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, - excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); + SmartFactor::shared_ptr smartFactor3( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); smartFactor3->add(measurements_cam3, views, model, K); - SmartFactor::shared_ptr smartFactor4(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, - excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); + SmartFactor::shared_ptr smartFactor4( + new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); smartFactor4->add(measurements_cam4, views, model, K); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -484,7 +522,8 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ){ graph.push_back(PriorFactor(x1, pose1, noisePrior)); graph.push_back(PriorFactor(x2, pose2, noisePrior)); - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, pose1); values.insert(x2, pose2); @@ -495,19 +534,19 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ){ Values result; LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); - EXPECT(assert_equal(pose3,result.at(x3))); + EXPECT(assert_equal(pose3, result.at(x3))); } // ///* *************************************************************************/ //TEST( SmartStereoProjectionPoseFactor, jacobianQ ){ // -// std::vector views; +// vector views; // views.push_back(x1); // views.push_back(x2); // views.push_back(x3); // // // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) -// Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); +// Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1)); // StereoCamera cam1(pose1, K); // // create second camera 1 meter to the right of first camera // Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); @@ -546,8 +585,8 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ){ // graph.push_back(PriorFactor(x1, pose1, noisePrior)); // graph.push_back(PriorFactor(x2, pose2, noisePrior)); // -// // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below -// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise +// // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below +// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), Point3(0.1,0.1,0.1)); // smaller noise // Values values; // values.insert(x1, pose1); // values.insert(x2, pose2); @@ -564,13 +603,13 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ){ //TEST( SmartStereoProjectionPoseFactor, 3poses_projection_factor ){ // // cout << " ************************ Normal ProjectionFactor: 3 cams + 3 landmarks **********************" << endl; // -// std::vector views; +// vector views; // views.push_back(x1); // views.push_back(x2); // views.push_back(x3); // // // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) -// Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); +// Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1)); // StereoCamera cam1(pose1, K2); // // // create second camera 1 meter to the right of first camera @@ -606,7 +645,7 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ){ // graph.push_back(PriorFactor(x1, pose1, noisePrior)); // graph.push_back(PriorFactor(x2, pose2, noisePrior)); // -// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); +// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // Values values; // values.insert(x1, pose1); // values.insert(x2, pose2); @@ -627,23 +666,23 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ){ //} // /* *************************************************************************/ -TEST( SmartStereoProjectionPoseFactor, CheckHessian){ +TEST( SmartStereoProjectionPoseFactor, CheckHessian) { - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); StereoCamera cam1(pose1, K); - // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); + // create second camera + Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); StereoCamera cam2(pose2, K); - // create third camera 1 meter above the first camera - Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); + // create third camera + Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); StereoCamera cam3(pose3, K); // three landmarks ~5 meters infront of camera @@ -651,12 +690,15 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){ Point3 landmark2(5, -0.5, 1.2); Point3 landmark3(3, 0, 3.0); - // 1. Project three landmarks into three cameras and triangulate - vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1); - vector measurements_cam2 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2); - vector measurements_cam3 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3); - + // Project three landmarks into three cameras and triangulate + vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark1); + vector measurements_cam2 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark2); + vector measurements_cam3 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark3); + // Create smartfactors double rankTol = 10; SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol)); @@ -668,38 +710,47 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){ SmartFactor::shared_ptr smartFactor3(new SmartFactor(rankTol)); smartFactor3->add(measurements_cam3, views, model, K); + // Create graph to optimize NonlinearFactorGraph graph; graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise Values values; values.insert(x1, pose1); values.insert(x2, pose2); // initialize third pose with some noise, we expect it to move back to original pose3 - values.insert(x3, pose3*noise_pose); - if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise + values.insert(x3, pose3 * noise_pose); + if (isDebugTest) + values.at(x3).print("Smart: Pose3 before optimization: "); - boost::shared_ptr hessianFactor1 = smartFactor1->linearize(values); - boost::shared_ptr hessianFactor2 = smartFactor2->linearize(values); - boost::shared_ptr hessianFactor3 = smartFactor3->linearize(values); + // TODO: next line throws Cheirality exception on Mac + boost::shared_ptr hessianFactor1 = smartFactor1->linearize( + values); + boost::shared_ptr hessianFactor2 = smartFactor2->linearize( + values); + boost::shared_ptr hessianFactor3 = smartFactor3->linearize( + values); - Matrix CumulativeInformation = hessianFactor1->information() + hessianFactor2->information() + hessianFactor3->information(); + Matrix CumulativeInformation = hessianFactor1->information() + + hessianFactor2->information() + hessianFactor3->information(); - boost::shared_ptr GaussianGraph = graph.linearize(values); + boost::shared_ptr GaussianGraph = graph.linearize( + values); Matrix GraphInformation = GaussianGraph->hessian().first; // Check Hessian EXPECT(assert_equal(GraphInformation, CumulativeInformation, 1e-8)); - Matrix AugInformationMatrix = hessianFactor1->augmentedInformation() + - hessianFactor2->augmentedInformation() + hessianFactor3->augmentedInformation(); + Matrix AugInformationMatrix = hessianFactor1->augmentedInformation() + + hessianFactor2->augmentedInformation() + + hessianFactor3->augmentedInformation(); // Check Information vector // cout << AugInformationMatrix.size() << endl; - Vector InfoVector = AugInformationMatrix.block(0,18,18,1); // 18x18 Hessian + information vector + Vector InfoVector = AugInformationMatrix.block(0, 18, 18, 1); // 18x18 Hessian + information vector // Check Hessian EXPECT(assert_equal(InfoVector, GaussianGraph->hessian().second, 1e-8)); @@ -709,13 +760,13 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){ //TEST( SmartStereoProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_factor ){ // // cout << " ************************ SmartStereoProjectionPoseFactor: 3 cams + 2 landmarks: Rotation Only**********************" << endl; // -// std::vector views; +// vector views; // views.push_back(x1); // views.push_back(x2); // views.push_back(x3); // // // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) -// Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); +// Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1)); // StereoCamera cam1(pose1, K2); // // // create second camera 1 meter to the right of first camera @@ -745,7 +796,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){ // // const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); // const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, 0.10); -// Point3 positionPrior = gtsam::Point3(0,0,1); +// Point3 positionPrior = Point3(0,0,1); // // NonlinearFactorGraph graph; // graph.push_back(smartFactor1); @@ -754,7 +805,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){ // graph.push_back(PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); // graph.push_back(PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); // -// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.1,0.1,0.1)); // smaller noise +// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.1,0.1,0.1)); // smaller noise // Values values; // values.insert(x1, pose1); // values.insert(x2, pose2*noise_pose); @@ -775,7 +826,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){ // // // result.print("results of 3 camera, 3 landmark optimization \n"); // if(isDebugTest) result.at(x3).print("Smart: Pose3 after optimization: "); -// std::cout << "TEST COMMENTED: rotation only version of smart factors has been deprecated " << std::endl; +// cout << "TEST COMMENTED: rotation only version of smart factors has been deprecated " << endl; // // EXPECT(assert_equal(pose3,result.at(x3))); // if(isDebugTest) tictoc_print_(); //} @@ -784,13 +835,13 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){ //TEST( SmartStereoProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ){ // // cout << " ************************ SmartStereoProjectionPoseFactor: 3 cams + 3 landmarks: Rotation Only**********************" << endl; // -// std::vector views; +// vector views; // views.push_back(x1); // views.push_back(x2); // views.push_back(x3); // // // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) -// Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); +// Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1)); // StereoCamera cam1(pose1, K); // // // create second camera 1 meter to the right of first camera @@ -826,7 +877,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){ // // const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); // const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, 0.10); -// Point3 positionPrior = gtsam::Point3(0,0,1); +// Point3 positionPrior = Point3(0,0,1); // // NonlinearFactorGraph graph; // graph.push_back(smartFactor1); @@ -836,8 +887,8 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){ // graph.push_back(PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); // graph.push_back(PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); // -// // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below -// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise +// // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below +// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), Point3(0.1,0.1,0.1)); // smaller noise // Values values; // values.insert(x1, pose1); // values.insert(x2, pose2); @@ -858,7 +909,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){ // // // result.print("results of 3 camera, 3 landmark optimization \n"); // if(isDebugTest) result.at(x3).print("Smart: Pose3 after optimization: "); -// std::cout << "TEST COMMENTED: rotation only version of smart factors has been deprecated " << std::endl; +// cout << "TEST COMMENTED: rotation only version of smart factors has been deprecated " << endl; // // EXPECT(assert_equal(pose3,result.at(x3))); // if(isDebugTest) tictoc_print_(); //} @@ -867,12 +918,12 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){ //TEST( SmartStereoProjectionPoseFactor, Hessian ){ // // cout << " ************************ SmartStereoProjectionPoseFactor: Hessian **********************" << endl; // -// std::vector views; +// vector views; // views.push_back(x1); // views.push_back(x2); // // // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) -// Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); +// Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1)); // StereoCamera cam1(pose1, K2); // // // create second camera 1 meter to the right of first camera @@ -892,7 +943,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){ // SmartFactor::shared_ptr smartFactor1(new SmartFactor()); // smartFactor1->add(measurements_cam1,views, model, K2); // -// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); +// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // Values values; // values.insert(x1, pose1); // values.insert(x2, pose2); @@ -908,29 +959,30 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){ // /* *************************************************************************/ -TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ){ +TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ) { // cout << " ************************ SmartStereoProjectionPoseFactor: rotated Hessian **********************" << endl; - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); StereoCamera cam1(pose1, K); // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); StereoCamera cam2(pose2, K); // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); StereoCamera cam3(pose3, K); Point3 landmark1(5, 0.5, 1.2); - vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1); + vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark1); SmartFactor::shared_ptr smartFactorInstance(new SmartFactor()); smartFactorInstance->add(measurements_cam1, views, model, K); @@ -940,46 +992,54 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ){ values.insert(x2, pose2); values.insert(x3, pose3); - boost::shared_ptr hessianFactor = smartFactorInstance->linearize(values); + boost::shared_ptr hessianFactor = + smartFactorInstance->linearize(values); // hessianFactor->print("Hessian factor \n"); - Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,0)); + Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0)); Values rotValues; rotValues.insert(x1, poseDrift.compose(pose1)); rotValues.insert(x2, poseDrift.compose(pose2)); rotValues.insert(x3, poseDrift.compose(pose3)); - boost::shared_ptr hessianFactorRot = smartFactorInstance->linearize(rotValues); + boost::shared_ptr hessianFactorRot = + smartFactorInstance->linearize(rotValues); // hessianFactorRot->print("Hessian factor \n"); // Hessian is invariant to rotations in the nondegenerate case - EXPECT(assert_equal(hessianFactor->information(), hessianFactorRot->information(), 1e-8) ); + EXPECT( + assert_equal(hessianFactor->information(), + hessianFactorRot->information(), 1e-7)); - Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI/2, -M_PI/3, -M_PI/2), gtsam::Point3(10,-4,5)); + Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2), + Point3(10, -4, 5)); Values tranValues; tranValues.insert(x1, poseDrift2.compose(pose1)); tranValues.insert(x2, poseDrift2.compose(pose2)); tranValues.insert(x3, poseDrift2.compose(pose3)); - boost::shared_ptr hessianFactorRotTran = smartFactorInstance->linearize(tranValues); + boost::shared_ptr hessianFactorRotTran = + smartFactorInstance->linearize(tranValues); // Hessian is invariant to rotations and translations in the nondegenerate case - EXPECT(assert_equal(hessianFactor->information(), hessianFactorRotTran->information(), 1e-8) ); + EXPECT( + assert_equal(hessianFactor->information(), + hessianFactorRotTran->information(), 1e-7)); } /* *************************************************************************/ -TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ){ +TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) { // cout << " ************************ SmartStereoProjectionPoseFactor: rotated Hessian (degenerate) **********************" << endl; - std::vector views; + vector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); StereoCamera cam1(pose1, K2); // Second and third cameras in same place, which is a degenerate configuration @@ -990,49 +1050,60 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ){ Point3 landmark1(5, 0.5, 1.2); - vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1); + vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark1); SmartFactor::shared_ptr smartFactor(new SmartFactor()); smartFactor->add(measurements_cam1, views, model, K2); - Values values; values.insert(x1, pose1); values.insert(x2, pose2); values.insert(x3, pose3); - boost::shared_ptr hessianFactor = smartFactor->linearize(values); - if(isDebugTest) hessianFactor->print("Hessian factor \n"); + boost::shared_ptr hessianFactor = smartFactor->linearize( + values); + if (isDebugTest) + hessianFactor->print("Hessian factor \n"); - Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,0)); + Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0)); Values rotValues; rotValues.insert(x1, poseDrift.compose(pose1)); rotValues.insert(x2, poseDrift.compose(pose2)); rotValues.insert(x3, poseDrift.compose(pose3)); - boost::shared_ptr hessianFactorRot = smartFactor->linearize(rotValues); - if(isDebugTest) hessianFactorRot->print("Hessian factor \n"); + boost::shared_ptr hessianFactorRot = smartFactor->linearize( + rotValues); + if (isDebugTest) + hessianFactorRot->print("Hessian factor \n"); // Hessian is invariant to rotations in the nondegenerate case - EXPECT(assert_equal(hessianFactor->information(), hessianFactorRot->information(), 1e-8) ); + EXPECT( + assert_equal(hessianFactor->information(), + hessianFactorRot->information(), 1e-8)); - Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI/2, -M_PI/3, -M_PI/2), gtsam::Point3(10,-4,5)); + Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2), + Point3(10, -4, 5)); Values tranValues; tranValues.insert(x1, poseDrift2.compose(pose1)); tranValues.insert(x2, poseDrift2.compose(pose2)); tranValues.insert(x3, poseDrift2.compose(pose3)); - boost::shared_ptr hessianFactorRotTran = smartFactor->linearize(tranValues); + boost::shared_ptr hessianFactorRotTran = + smartFactor->linearize(tranValues); // Hessian is invariant to rotations and translations in the nondegenerate case - EXPECT(assert_equal(hessianFactor->information(), hessianFactorRotTran->information(), 1e-8) ); + EXPECT( + assert_equal(hessianFactor->information(), + hessianFactorRotTran->information(), 1e-8)); } - /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ - From 843bc7cb826e3bde8926726ffc49510d04ac2cfe Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 20 Feb 2015 02:06:47 +0100 Subject: [PATCH 50/91] Isolated calculation of Point Covariance in static method(s) PointCov --- gtsam/slam/SmartFactorBase.h | 247 +++++++++--------- gtsam/slam/SmartProjectionFactor.h | 96 +++---- .../slam/SmartStereoProjectionFactor.h | 29 +- 3 files changed, 178 insertions(+), 194 deletions(-) diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index b5ed39c55..a2bdfc059 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -245,12 +245,12 @@ public: } /** - * Compute the matrices E and PointCov = inv(E'*E), where E stacks the ZDim*3 derivatives - * of project with respect to the point, given each of the m cameras. - * Assumes non-degenerate ! + * Compute whitenedError, returning only the derivative E, i.e., + * the stacked ZDim*3 derivatives of project with respect to the point. + * Assumes non-degenerate ! TODO explain this */ - void computeEP(Matrix& E, Matrix& PointCov, const Cameras& cameras, - const Point3& point) const { + Vector whitenedError(const Cameras& cameras, const Point3& point, + Matrix& E) const { // Project into CameraSet, calculating derivatives std::vector predicted; @@ -258,12 +258,20 @@ public: using boost::none; predicted = cameras.project(point, none, E, none); } catch (CheiralityException&) { - std::cout << "computeEP: Cheirality exception " << std::endl; + std::cout << "whitenedError(E): Cheirality exception " << std::endl; exit(EXIT_FAILURE); // TODO: throw exception } // if needed, whiten - for (size_t i = 0, row = 0; i < noise_.size(); i++, row += ZDim) { + size_t m = keys_.size(); + Vector b(ZDim * m); + for (size_t i = 0, row = 0; i < m; i++, row += ZDim) { + + // Calculate error + const Z& zi = measured_.at(i); + Vector bi = (zi - predicted[i]).vector(); + + // if needed, whiten SharedNoiseModel model = noise_.at(i); if (model) { // TODO: re-factor noiseModel to take any block/fixed vector/matrix @@ -272,9 +280,9 @@ public: model->WhitenSystem(Ei, dummy); E.block(row, 0) = Ei; } + b.segment(row) = bi; } - // Matrix PointCov; - PointCov.noalias() = (E.transpose() * E).inverse(); + return b; } /** @@ -286,25 +294,25 @@ public: * is applied in the caller, but the derivatives are computed here. */ Vector whitenedError(const Cameras& cameras, const Point3& point, Matrix& F, - Matrix& E, boost::optional H = boost::none) const { + Matrix& E, boost::optional G = boost::none) const { // Project into CameraSet, calculating derivatives std::vector predicted; try { - predicted = cameras.project(point, F, E, H); + predicted = cameras.project(point, F, E, G); } catch (CheiralityException&) { - std::cout << "computeJacobians: Cheirality exception " << std::endl; + std::cout << "whitenedError(E,F): Cheirality exception " << std::endl; exit(EXIT_FAILURE); // TODO: throw exception } // Calculate error and whiten derivatives if needed - size_t numKeys = keys_.size(); - Vector b(ZDim * numKeys); - for (size_t i = 0, row = 0; i < numKeys; i++, row += ZDim) { + size_t m = keys_.size(); + Vector b(ZDim * m); + for (size_t i = 0, row = 0; i < m; i++, row += ZDim) { // Calculate error const Z& zi = measured_.at(i); - b.segment(row) = (zi - predicted[i]).vector(); + Vector bi = (zi - predicted[i]).vector(); // if we have a sensor offset, correct camera derivatives if (body_P_sensor_) { @@ -322,66 +330,30 @@ public: // TODO, refactor noiseModel so we can take blocks Matrix Fi = F.block(row, 0); Matrix Ei = E.block(row, 0); - Vector bi = b.segment(row); - if (!H) + if (!G) model->WhitenSystem(Fi, Ei, bi); else { - Matrix Hi = H->block(row, 0); - model->WhitenSystem(Fi, Ei, Hi, bi); - H->block(row, 0) = Hi; + Matrix Gi = G->block(row, 0); + model->WhitenSystem(Fi, Ei, Gi, bi); + G->block(row, 0) = Gi; } - b.segment(row) = bi; F.block(row, 0) = Fi; E.block(row, 0) = Ei; } + b.segment(row) = bi; } return b; } - /** - * Compute F, E, and b (called below in both vanilla and SVD versions), where - * F is a vector of derivatives wrpt the cameras, and E the stacked derivatives - * with respect to the point. The value of cameras/point are passed as parameters. - */ - double computeJacobians(std::vector& Fblocks, Matrix& E, - Vector& b, const Cameras& cameras, const Point3& point) const { - - // Project into Camera set and calculate derivatives - // TODO: if D==6 we optimize only camera pose. That is fairly hacky! - Matrix F, H; - using boost::none; - boost::optional optionalH(H); - b = whitenedError(cameras, point, F, E, D == 6 ? none : optionalH); - - // Now calculate f and divide up the F derivatives into Fblocks - double f = 0.0; - size_t numKeys = keys_.size(); - for (size_t i = 0, row = 0; i < numKeys; i++, row += ZDim) { - - // Accumulate normalized error - f += b.segment(row).squaredNorm(); - - // Get piece of F and possibly H - Matrix2D Fi; - if (D == 6) - Fi << F.block(row, 0); - else - Fi << F.block(row, 0), H.block(row, 0); - - // Push it onto Fblocks - Fblocks.push_back(KeyMatrix2D(keys_[i], Fi)); - } - return f; + /// Computes Point Covariance P from E + static Matrix3 PointCov(Matrix& E) { + return (E.transpose() * E).inverse(); } - /// Version that computes PointCov, with optional lambda parameter - double computeJacobians(std::vector& Fblocks, Matrix& E, - Matrix3& PointCov, Vector& b, const Cameras& cameras, const Point3& point, - double lambda = 0.0, bool diagonalDamping = false) const { + /// Computes Point Covariance P, with lambda parameter + static Matrix3 PointCov(Matrix& E, double lambda, + bool diagonalDamping = false) { - double f = computeJacobians(Fblocks, E, b, cameras, point); - - // Point covariance inv(E'*E) Matrix3 EtE = E.transpose() * E; if (diagonalDamping) { // diagonal of the hessian @@ -394,47 +366,86 @@ public: EtE(2, 2) += lambda; } - PointCov.noalias() = (EtE).inverse(); + return (EtE).inverse(); + } + /// Assumes non-degenerate ! + void computeEP(Matrix& E, Matrix& P, const Cameras& cameras, + const Point3& point) const { + whitenedError(cameras, point, E); + P = PointCov(E); + } + + /** + * Compute F, E, and b (called below in both vanilla and SVD versions), where + * F is a vector of derivatives wrpt the cameras, and E the stacked derivatives + * with respect to the point. The value of cameras/point are passed as parameters. + */ + double computeJacobians(std::vector& Fblocks, Matrix& E, + Vector& b, const Cameras& cameras, const Point3& point) const { + + // Project into Camera set and calculate derivatives + // TODO: if D==6 we optimize only camera pose. That is fairly hacky! + Matrix F, G; + using boost::none; + boost::optional optionalG(G); + b = whitenedError(cameras, point, F, E, D == 6 ? none : optionalG); + + // Now calculate f and divide up the F derivatives into Fblocks + double f = 0.0; + size_t m = keys_.size(); + for (size_t i = 0, row = 0; i < m; i++, row += ZDim) { + + // Accumulate normalized error + f += b.segment(row).squaredNorm(); + + // Get piece of F and possibly G + Matrix2D Fi; + if (D == 6) + Fi << F.block(row, 0); + else + Fi << F.block(row, 0), G.block(row, 0); + + // Push it onto Fblocks + Fblocks.push_back(KeyMatrix2D(keys_[i], Fi)); + } return f; } + /// Create BIG block-diagonal matrix F from Fblocks + static void FillDiagonalF(const std::vector& Fblocks, Matrix& F) { + size_t m = Fblocks.size(); + F.resize(ZDim * m, D * m); + F.setZero(); + for (size_t i = 0; i < m; ++i) + F.block(This::ZDim * i, D * i) = Fblocks.at(i).second; + } + /** * Compute F, E, and b, where F and E are the stacked derivatives * with respect to the point. The value of cameras/point are passed as parameters. */ - double computeJacobians(Matrix& F, Matrix& E, Matrix3& PointCov, Vector& b, - const Cameras& cameras, const Point3& point, - const double lambda = 0.0) const { - - size_t numKeys = keys_.size(); + double computeJacobians(Matrix& F, Matrix& E, Vector& b, + const Cameras& cameras, const Point3& point) const { std::vector Fblocks; - double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, - lambda); - F = zeros(This::ZDim * numKeys, D * numKeys); - - for (size_t i = 0; i < keys_.size(); ++i) { - F.block(This::ZDim * i, D * i) = Fblocks.at(i).second; // ZDim x 6 block for the cameras - } + double f = computeJacobians(Fblocks, E, b, cameras, point); + FillDiagonalF(Fblocks, F); return f; } /// SVD version double computeJacobiansSVD(std::vector& Fblocks, Matrix& Enull, - Vector& b, const Cameras& cameras, const Point3& point, double lambda = - 0.0, bool diagonalDamping = false) const { + Vector& b, const Cameras& cameras, const Point3& point) const { Matrix E; - Matrix3 PointCov; // useless - double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda, - diagonalDamping); // diagonalDamping should have no effect (only on PointCov) + double f = computeJacobians(Fblocks, E, b, cameras, point); // Do SVD on A Eigen::JacobiSVD svd(E, Eigen::ComputeFullU); Vector s = svd.singularValues(); - // Enull = zeros(ZDim * numKeys, ZDim * numKeys - 3); - size_t numKeys = this->keys_.size(); - Enull = svd.matrixU().block(0, 3, ZDim * numKeys, ZDim * numKeys - 3); // last ZDimm-3 columns + size_t m = this->keys_.size(); + // Enull = zeros(ZDim * m, ZDim * m - 3); + Enull = svd.matrixU().block(0, 3, ZDim * m, ZDim * m - 3); // last ZDim*m-3 columns return f; } @@ -443,16 +454,9 @@ public: // TODO, there should not be a Matrix version, really double computeJacobiansSVD(Matrix& F, Matrix& Enull, Vector& b, const Cameras& cameras, const Point3& point) const { - - int numKeys = this->keys_.size(); std::vector Fblocks; double f = computeJacobiansSVD(Fblocks, Enull, b, cameras, point); - F.resize(ZDim * numKeys, D * numKeys); - F.setZero(); - - for (size_t i = 0; i < this->keys_.size(); ++i) - F.block(ZDim * i, D * i) = Fblocks.at(i).second; // ZDim x 6 block for the cameras - + FillDiagonalF(Fblocks, F); return f; } @@ -467,10 +471,9 @@ public: std::vector Fblocks; Matrix E; - Matrix3 PointCov; Vector b; - double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda, - diagonalDamping); + double f = computeJacobians(Fblocks, E, b, cameras, point); + Matrix3 P = PointCov(E, lambda, diagonalDamping); //#define HESSIAN_BLOCKS // slower, as internally the Hessian factor will transform the blocks into SymmetricBlockMatrix #ifdef HESSIAN_BLOCKS @@ -478,8 +481,8 @@ public: std::vector < Matrix > Gs(numKeys * (numKeys + 1) / 2); std::vector < Vector > gs(numKeys); - sparseSchurComplement(Fblocks, E, PointCov, b, Gs, gs); - // schurComplement(Fblocks, E, PointCov, b, Gs, gs); + sparseSchurComplement(Fblocks, E, P, b, Gs, gs); + // schurComplement(Fblocks, E, P, b, Gs, gs); //std::vector < Matrix > Gs2(Gs.begin(), Gs.end()); //std::vector < Vector > gs2(gs.begin(), gs.end()); @@ -492,7 +495,7 @@ public: dims.back() = 1; SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(n1, n1)); // for 10 cameras, size should be (10*D+1 x 10*D+1) - sparseSchurComplement(Fblocks, E, PointCov, b, augmentedHessian); // augmentedHessian.matrix().block (i1,i2) = ... + sparseSchurComplement(Fblocks, E, P, b, augmentedHessian); // augmentedHessian.matrix().block (i1,i2) = ... augmentedHessian(numKeys, numKeys)(0, 0) = f; return boost::make_shared >(this->keys_, augmentedHessian); @@ -500,11 +503,11 @@ public: } /** - * Do Schur complement, given Jacobian as F,E,PointCov. + * Do Schur complement, given Jacobian as F,E,P. * Slow version - works on full matrices */ void schurComplement(const std::vector& Fblocks, const Matrix& E, - const Matrix& PointCov, const Vector& b, + const Matrix3& P, const Vector& b, /*output ->*/std::vector& Gs, std::vector& gs) const { // Schur complement trick // Gs = F' * F - F' * E * inv(E'*E) * E' * F @@ -514,16 +517,14 @@ public: int numKeys = this->keys_.size(); /// Compute Full F ???? - Matrix F = zeros(ZDim * numKeys, D * numKeys); - for (size_t i = 0; i < this->keys_.size(); ++i) - F.block(ZDim * i, D * i) = Fblocks.at(i).second; // ZDim x 6 block for the cameras + Matrix F; + FillDiagonalF(Fblocks, F); Matrix H(D * numKeys, D * numKeys); Vector gs_vector; - H.noalias() = F.transpose() * (F - (E * (PointCov * (E.transpose() * F)))); - gs_vector.noalias() = F.transpose() - * (b - (E * (PointCov * (E.transpose() * b)))); + H.noalias() = F.transpose() * (F - (E * (P * (E.transpose() * F)))); + gs_vector.noalias() = F.transpose() * (b - (E * (P * (E.transpose() * b)))); // Populate Gs and gs int GsCount2 = 0; @@ -540,11 +541,11 @@ public: } /** - * Do Schur complement, given Jacobian as F,E,PointCov, return SymmetricBlockMatrix + * Do Schur complement, given Jacobian as F,E,P, return SymmetricBlockMatrix * Fast version - works on with sparsity */ void sparseSchurComplement(const std::vector& Fblocks, - const Matrix& E, const Matrix& P /*Point Covariance*/, const Vector& b, + const Matrix& E, const Matrix3& P /*Point Covariance*/, const Vector& b, /*output ->*/SymmetricBlockMatrix& augmentedHessian) const { // Schur complement trick // Gs = F' * F - F' * E * P * E' * F @@ -581,11 +582,11 @@ public: } /** - * Do Schur complement, given Jacobian as F,E,PointCov, return Gs/gs + * Do Schur complement, given Jacobian as F,E,P, return Gs/gs * Fast version - works on with sparsity */ void sparseSchurComplement(const std::vector& Fblocks, - const Matrix& E, const Matrix& P /*Point Covariance*/, const Vector& b, + const Matrix& E, const Matrix3& P /*Point Covariance*/, const Vector& b, /*output ->*/std::vector& Gs, std::vector& gs) const { // Schur complement trick // Gs = F' * F - F' * E * P * E' * F @@ -634,7 +635,7 @@ public: * and adds the contribution of the smart factor to a pre-allocated augmented Hessian. */ void updateSparseSchurComplement(const std::vector& Fblocks, - const Matrix& E, const Matrix& P /*Point Covariance*/, const Vector& b, + const Matrix& E, const Matrix3& P /*Point Covariance*/, const Vector& b, const double f, const FastVector allKeys, /*output ->*/SymmetricBlockMatrix& augmentedHessian) const { // Schur complement trick @@ -717,13 +718,10 @@ public: std::vector Fblocks; Matrix E; - Matrix3 PointCov; Vector b; - double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda, - diagonalDamping); - - updateSparseSchurComplement(Fblocks, E, PointCov, b, f, allKeys, - augmentedHessian); // augmentedHessian.matrix().block (i1,i2) = ... + double f = computeJacobians(Fblocks, E, b, cameras, point); + Matrix3 P = PointCov(E, lambda, diagonalDamping); + updateSparseSchurComplement(Fblocks, E, P, b, f, allKeys, augmentedHessian); // augmentedHessian.matrix().block (i1,i2) = ... } /** @@ -734,8 +732,8 @@ public: bool diagonalDamping = false) const { typename boost::shared_ptr > f( new RegularImplicitSchurFactor()); - computeJacobians(f->Fblocks(), f->E(), f->PointCovariance(), f->b(), - cameras, point, lambda, diagonalDamping); + computeJacobians(f->Fblocks(), f->E(), f->b(), cameras, point); + f->PointCovariance() = PointCov(f->E(), lambda, diagonalDamping); f->initKeys(); return f; } @@ -748,16 +746,15 @@ public: bool diagonalDamping = false) const { std::vector Fblocks; Matrix E; - Matrix3 PointCov; Vector b; - computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda, - diagonalDamping); - return boost::make_shared >(Fblocks, E, PointCov, - b); + computeJacobians(Fblocks, E, b, cameras, point); + Matrix3 P = PointCov(E, lambda, diagonalDamping); + return boost::make_shared >(Fblocks, E, P, b); } /** * Return Jacobians as JacobianFactor + * TODO lambda is currently ignored */ boost::shared_ptr createJacobianSVDFactor( const Cameras& cameras, const Point3& point, double lambda = 0.0) const { @@ -765,7 +762,7 @@ public: std::vector Fblocks; Vector b; Matrix Enull(ZDim * numKeys, ZDim * numKeys - 3); - computeJacobiansSVD(Fblocks, Enull, b, cameras, point, lambda); + computeJacobiansSVD(Fblocks, Enull, b, cameras, point); return boost::make_shared >(Fblocks, Enull, b); } diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index e763b3b02..7fb43152a 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -61,7 +61,8 @@ enum LinearizationMode { * SmartProjectionFactor: triangulates point and keeps an estimate of it around. */ template -class SmartProjectionFactor: public SmartFactorBase, D> { +class SmartProjectionFactor: public SmartFactorBase, + D> { protected: // Some triangulation parameters @@ -125,14 +126,14 @@ public: const bool manageDegeneracy, const bool enableEPI, boost::optional body_P_sensor = boost::none, double landmarkDistanceThreshold = 1e10, - double dynamicOutlierRejectionThreshold = -1, - SmartFactorStatePtr state = SmartFactorStatePtr(new SmartProjectionFactorState())) : + double dynamicOutlierRejectionThreshold = -1, SmartFactorStatePtr state = + SmartFactorStatePtr(new SmartProjectionFactorState())) : Base(body_P_sensor), rankTolerance_(rankTol), retriangulationThreshold_( 1e-5), manageDegeneracy_(manageDegeneracy), enableEPI_(enableEPI), linearizationThreshold_( linThreshold), degenerate_(false), cheiralityException_(false), throwCheirality_( - false), verboseCheirality_(false), state_(state), - landmarkDistanceThreshold_(landmarkDistanceThreshold), - dynamicOutlierRejectionThreshold_(dynamicOutlierRejectionThreshold) { + false), verboseCheirality_(false), state_(state), landmarkDistanceThreshold_( + landmarkDistanceThreshold), dynamicOutlierRejectionThreshold_( + dynamicOutlierRejectionThreshold) { } /** Virtual destructor */ @@ -249,11 +250,11 @@ public: // Check landmark distance and reprojection errors to avoid outliers double totalReprojError = 0.0; - size_t i=0; + size_t i = 0; BOOST_FOREACH(const Camera& camera, cameras) { Point3 cameraTranslation = camera.pose().translation(); // we discard smart factors corresponding to points that are far away - if(cameraTranslation.distance(point_) > landmarkDistanceThreshold_){ + if (cameraTranslation.distance(point_) > landmarkDistanceThreshold_) { degenerate_ = true; break; } @@ -267,8 +268,8 @@ public: i += 1; } // we discard smart factors that have large reprojection error - if(dynamicOutlierRejectionThreshold_ > 0 && - totalReprojError/m > dynamicOutlierRejectionThreshold_) + if (dynamicOutlierRejectionThreshold_ > 0 + && totalReprojError / m > dynamicOutlierRejectionThreshold_) degenerate_ = true; } catch (TriangulationUnderconstrainedException&) { @@ -297,7 +298,8 @@ public: || (!this->manageDegeneracy_ && (this->cheiralityException_ || this->degenerate_))) { if (isDebug) { - std::cout << "createRegularImplicitSchurFactor: degenerate configuration" + std::cout + << "createRegularImplicitSchurFactor: degenerate configuration" << std::endl; } return false; @@ -318,9 +320,9 @@ public: bool isDebug = false; size_t numKeys = this->keys_.size(); // Create structures for Hessian Factors - std::vector < Key > js; - std::vector < Matrix > Gs(numKeys * (numKeys + 1) / 2); - std::vector < Vector > gs(numKeys); + std::vector js; + std::vector Gs(numKeys * (numKeys + 1) / 2); + std::vector gs(numKeys); if (this->measured_.size() != cameras.size()) { std::cout @@ -370,9 +372,8 @@ public: // ================================================================== Matrix F, E; - Matrix3 PointCov; Vector b; - double f = computeJacobians(F, E, PointCov, b, cameras, lambda); + double f = computeJacobians(F, E, b, cameras); // Schur complement trick // Frank says: should be possible to do this more efficiently? @@ -380,20 +381,20 @@ public: Matrix H(D * numKeys, D * numKeys); Vector gs_vector; - H.noalias() = F.transpose() * (F - (E * (PointCov * (E.transpose() * F)))); - gs_vector.noalias() = F.transpose() - * (b - (E * (PointCov * (E.transpose() * b)))); + Matrix3 P = Base::PointCov(E, lambda); + H.noalias() = F.transpose() * (F - (E * (P * (E.transpose() * F)))); + gs_vector.noalias() = F.transpose() * (b - (E * (P * (E.transpose() * b)))); if (isDebug) std::cout << "gs_vector size " << gs_vector.size() << std::endl; // Populate Gs and gs int GsCount2 = 0; - for (DenseIndex i1 = 0; i1 < (DenseIndex)numKeys; i1++) { // for each camera + for (DenseIndex i1 = 0; i1 < (DenseIndex) numKeys; i1++) { // for each camera DenseIndex i1D = i1 * D; - gs.at(i1) = gs_vector.segment < D > (i1D); - for (DenseIndex i2 = 0; i2 < (DenseIndex)numKeys; i2++) { + gs.at(i1) = gs_vector.segment(i1D); + for (DenseIndex i2 = 0; i2 < (DenseIndex) numKeys; i2++) { if (i2 >= i1) { - Gs.at(GsCount2) = H.block < D, D > (i1D, i2 * D); + Gs.at(GsCount2) = H.block(i1D, i2 * D); GsCount2++; } } @@ -422,7 +423,7 @@ public: if (triangulateForLinearize(cameras)) return Base::createJacobianQFactor(cameras, point_, lambda); else - return boost::make_shared< JacobianFactorQ >(this->keys_); + return boost::make_shared >(this->keys_); } /// Create a factor, takes values @@ -434,16 +435,16 @@ public: if (nonDegenerate) return createJacobianQFactor(myCameras, lambda); else - return boost::make_shared< JacobianFactorQ >(this->keys_); + return boost::make_shared >(this->keys_); } /// different (faster) way to compute Jacobian factor - boost::shared_ptr< JacobianFactor > createJacobianSVDFactor(const Cameras& cameras, - double lambda) const { + boost::shared_ptr createJacobianSVDFactor( + const Cameras& cameras, double lambda) const { if (triangulateForLinearize(cameras)) return Base::createJacobianSVDFactor(cameras, point_, lambda); else - return boost::make_shared< JacobianFactorSVD >(this->keys_); + return boost::make_shared >(this->keys_); } /// Returns true if nonDegenerate @@ -476,27 +477,27 @@ public: return true; } + /// Assumes non-degenerate ! + void computeEP(Matrix& E, Matrix& P, const Cameras& cameras) const { + return Base::computeEP(E, P, cameras, point_); + } + /// Takes values - bool computeEP(Matrix& E, Matrix& PointCov, const Values& values) const { + bool computeEP(Matrix& E, Matrix& P, const Values& values) const { Cameras myCameras; bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras); if (nonDegenerate) - computeEP(E, PointCov, myCameras); + computeEP(E, P, myCameras); return nonDegenerate; } - /// Assumes non-degenerate ! - void computeEP(Matrix& E, Matrix& PointCov, const Cameras& cameras) const { - return Base::computeEP(E, PointCov, cameras, point_); - } - /// Version that takes values, and creates the point bool computeJacobians(std::vector& Fblocks, - Matrix& E, Matrix& PointCov, Vector& b, const Values& values) const { + Matrix& E, Vector& b, const Values& values) const { Cameras myCameras; bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras); if (nonDegenerate) - computeJacobians(Fblocks, E, PointCov, b, myCameras, 0.0); + computeJacobians(Fblocks, E, b, myCameras); return nonDegenerate; } @@ -535,7 +536,7 @@ public: this->noise_.at(i)->WhitenSystem(Fi, Ei, bi); f += bi.squaredNorm(); Fblocks.push_back(typename Base::KeyMatrix2D(this->keys_[i], Fi)); - E.block < 2, 2 > (2 * i, 0) = Ei; + E.block<2, 2>(2 * i, 0) = Ei; subInsert(b, bi, 2 * i); } return f; @@ -545,19 +546,6 @@ public: } // end else } - /// Version that computes PointCov, with optional lambda parameter - double computeJacobians(std::vector& Fblocks, - Matrix& E, Matrix& PointCov, Vector& b, const Cameras& cameras, - const double lambda = 0.0) const { - - double f = computeJacobians(Fblocks, E, b, cameras); - - // Point covariance inv(E'*E) - PointCov.noalias() = (E.transpose() * E + lambda * eye(E.cols())).inverse(); - - return f; - } - /// takes values bool computeJacobiansSVD(std::vector& Fblocks, Matrix& Enull, Vector& b, const Values& values) const { @@ -582,9 +570,9 @@ public: } /// Returns Matrix, TODO: maybe should not exist -> not sparse ! - double computeJacobians(Matrix& F, Matrix& E, Matrix3& PointCov, Vector& b, - const Cameras& cameras, const double lambda) const { - return Base::computeJacobians(F, E, PointCov, b, cameras, point_, lambda); + double computeJacobians(Matrix& F, Matrix& E, Vector& b, + const Cameras& cameras) const { + return Base::computeJacobians(F, E, b, cameras, point_); } /// Calculate vector of re-projection errors, before applying noise model diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index 2fa23d09f..11513d19f 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -407,9 +407,8 @@ public: // ================================================================== Matrix F, E; - Matrix3 PointCov; Vector b; - double f = computeJacobians(F, E, PointCov, b, cameras, lambda); + double f = computeJacobians(F, E, b, cameras); // Schur complement trick // Frank says: should be possible to do this more efficiently? @@ -417,9 +416,9 @@ public: Matrix H(D * numKeys, D * numKeys); Vector gs_vector; - H.noalias() = F.transpose() * (F - (E * (PointCov * (E.transpose() * F)))); - gs_vector.noalias() = F.transpose() - * (b - (E * (PointCov * (E.transpose() * b)))); + Matrix3 P = Base::PointCov(E,lambda); + H.noalias() = F.transpose() * (F - (E * (P * (E.transpose() * F)))); + gs_vector.noalias() = F.transpose() * (b - (E * (P * (E.transpose() * b)))); if (isDebug) std::cout << "gs_vector size " << gs_vector.size() << std::endl; if (isDebug) std::cout << "H:\n" << H << std::endl; @@ -514,6 +513,11 @@ public: return true; } + /// Assumes non-degenerate ! + void computeEP(Matrix& E, Matrix& PointCov, const Cameras& cameras) const { + Base::computeEP(E, PointCov, cameras, point_); + } + /// Takes values bool computeEP(Matrix& E, Matrix& PointCov, const Values& values) const { Cameras myCameras; @@ -523,18 +527,13 @@ public: return nonDegenerate; } - /// Assumes non-degenerate ! - void computeEP(Matrix& E, Matrix& PointCov, const Cameras& cameras) const { - return Base::computeEP(E, PointCov, cameras, point_); - } - /// Version that takes values, and creates the point bool computeJacobians(std::vector& Fblocks, - Matrix& E, Matrix& PointCov, Vector& b, const Values& values) const { + Matrix& E, Vector& b, const Values& values) const { Cameras myCameras; bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras); if (nonDegenerate) - computeJacobians(Fblocks, E, PointCov, b, myCameras, 0.0); + computeJacobians(Fblocks, E, b, myCameras, 0.0); return nonDegenerate; } @@ -620,9 +619,9 @@ public: // } /// Returns Matrix, TODO: maybe should not exist -> not sparse ! - double computeJacobians(Matrix& F, Matrix& E, Matrix3& PointCov, Vector& b, - const Cameras& cameras, const double lambda) const { - return Base::computeJacobians(F, E, PointCov, b, cameras, point_, lambda); + double computeJacobians(Matrix& F, Matrix& E, Vector& b, + const Cameras& cameras) const { + return Base::computeJacobians(F, E, b, cameras, point_); } /// Calculate vector of re-projection errors, before applying noise model From 7ecf8ec1ac40d2bbec8c32f8013b875d65e98722 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 20 Feb 2015 02:42:51 +0100 Subject: [PATCH 51/91] Fixed compile issues with examples --- examples/SFMExample_SmartFactor.cpp | 4 ++-- examples/SFMExample_SmartFactorPCG.cpp | 2 +- gtsam_unstable/examples/SmartProjectionFactorExample.cpp | 2 +- .../examples/SmartStereoProjectionFactorExample.cpp | 2 +- 4 files changed, 5 insertions(+), 5 deletions(-) diff --git a/examples/SFMExample_SmartFactor.cpp b/examples/SFMExample_SmartFactor.cpp index 45a4662d6..fce046a59 100644 --- a/examples/SFMExample_SmartFactor.cpp +++ b/examples/SFMExample_SmartFactor.cpp @@ -32,7 +32,7 @@ using namespace std; using namespace gtsam; // Make the typename short so it looks much cleaner -typedef gtsam::SmartProjectionPoseFactor SmartFactor; +typedef SmartProjectionPoseFactor SmartFactor; /* ************************************************************************* */ int main(int argc, char* argv[]) { @@ -111,7 +111,7 @@ int main(int argc, char* argv[]) { // The graph stores Factor shared_ptrs, so we cast back to a SmartFactor first SmartFactor::shared_ptr smart = boost::dynamic_pointer_cast(graph[j]); if (smart) { - // The output of point() is in boost::optional, as sometimes + // The output of point() is in boost::optional, as sometimes // the triangulation operation inside smart factor will encounter degeneracy. boost::optional point = smart->point(result); if (point) // ignore if boost::optional return NULL diff --git a/examples/SFMExample_SmartFactorPCG.cpp b/examples/SFMExample_SmartFactorPCG.cpp index 472cd9447..49482292f 100644 --- a/examples/SFMExample_SmartFactorPCG.cpp +++ b/examples/SFMExample_SmartFactorPCG.cpp @@ -28,7 +28,7 @@ using namespace std; using namespace gtsam; // Make the typename short so it looks much cleaner -typedef gtsam::SmartProjectionPoseFactor SmartFactor; +typedef SmartProjectionPoseFactor SmartFactor; /* ************************************************************************* */ int main(int argc, char* argv[]) { diff --git a/gtsam_unstable/examples/SmartProjectionFactorExample.cpp b/gtsam_unstable/examples/SmartProjectionFactorExample.cpp index c8a4e7123..dc921a7da 100644 --- a/gtsam_unstable/examples/SmartProjectionFactorExample.cpp +++ b/gtsam_unstable/examples/SmartProjectionFactorExample.cpp @@ -46,7 +46,7 @@ using namespace gtsam; int main(int argc, char** argv){ - typedef SmartProjectionPoseFactor SmartFactor; + typedef SmartProjectionPoseFactor SmartFactor; Values initial_estimate; NonlinearFactorGraph graph; diff --git a/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp b/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp index 770c5f18c..cfed9ae0c 100644 --- a/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp +++ b/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp @@ -46,7 +46,7 @@ using namespace gtsam; int main(int argc, char** argv){ - typedef SmartStereoProjectionPoseFactor SmartFactor; + typedef SmartStereoProjectionPoseFactor SmartFactor; bool output_poses = true; string poseOutput("../../../examples/data/optimized_poses.txt"); From a27722e03a7680395d1077cb134c04d281d15987 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 20 Feb 2015 11:03:47 +0100 Subject: [PATCH 52/91] Add some definitions. --- gtsam/geometry/CameraSet.h | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h index 27c0a78f3..3a34ca1fd 100644 --- a/gtsam/geometry/CameraSet.h +++ b/gtsam/geometry/CameraSet.h @@ -32,7 +32,7 @@ namespace gtsam { template class CameraSet: public std::vector { -private: +protected: /** * 2D measurement and noise model for each of the m views @@ -45,6 +45,10 @@ private: public: + /// Definitions for blocks of F + typedef Eigen::Matrix MatrixZD; // F + typedef std::pair FBlock; // Fblocks + /** * print * @param s optional string naming the factor @@ -70,7 +74,7 @@ public: } /** - * project, with derivatives in this, point, and calibration + * Project a point, with derivatives in this, point, and calibration * throws CheiralityException */ std::vector project(const Point3& point, boost::optional F = From 1ccce8cbea10e27da4252e059c96720999c84cb6 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 21 Feb 2015 18:06:51 +0100 Subject: [PATCH 53/91] Merged in feature/extraSmartTests (pull request #109) Extra test for smart factors, fixed for new noise values --- gtsam/slam/RegularHessianFactor.h | 16 ++++ gtsam/slam/RegularImplicitSchurFactor.h | 28 +++++-- .../tests/testSmartProjectionPoseFactor.cpp | 79 +++++++++++++++++++ 3 files changed, 116 insertions(+), 7 deletions(-) diff --git a/gtsam/slam/RegularHessianFactor.h b/gtsam/slam/RegularHessianFactor.h index dbe374958..c272eac8e 100644 --- a/gtsam/slam/RegularHessianFactor.h +++ b/gtsam/slam/RegularHessianFactor.h @@ -43,6 +43,15 @@ public: HessianFactor(js, Gs, gs, f) { } + /** Construct a binary factor. Gxx are the upper-triangle blocks of the + * quadratic term (the Hessian matrix), gx the pieces of the linear vector + * term, and f the constant term. + */ + RegularHessianFactor(Key j1, Key j2, const Matrix& G11, const Matrix& G12, + const Vector& g1, const Matrix& G22, const Vector& g2, double f) : + HessianFactor(j1, j2, G11, G12, g1, G22, g2, f) { + } + /** Constructor with an arbitrary number of keys and with the augmented information matrix * specified as a block matrix. */ template @@ -169,6 +178,13 @@ public: } } + /* ************************************************************************* */ + +}; // end class RegularHessianFactor + +// traits +template struct traits > : public Testable< + RegularHessianFactor > { }; } diff --git a/gtsam/slam/RegularImplicitSchurFactor.h b/gtsam/slam/RegularImplicitSchurFactor.h index b1d9cd9f6..b1490d465 100644 --- a/gtsam/slam/RegularImplicitSchurFactor.h +++ b/gtsam/slam/RegularImplicitSchurFactor.h @@ -87,18 +87,27 @@ public: const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { std::cout << " RegularImplicitSchurFactor " << std::endl; Factor::print(s); - std::cout << " PointCovariance_ \n" << PointCovariance_ << std::endl; - std::cout << " E_ \n" << E_ << std::endl; - std::cout << " b_ \n" << b_.transpose() << std::endl; + for (size_t pos = 0; pos < size(); ++pos) { + std::cout << "Fblock:\n" << Fblocks_[pos].second << std::endl; + } + std::cout << "PointCovariance:\n" << PointCovariance_ << std::endl; + std::cout << "E:\n" << E_ << std::endl; + std::cout << "b:\n" << b_.transpose() << std::endl; } /// equals bool equals(const GaussianFactor& lf, double tol) const { - if (!dynamic_cast(&lf)) - return false; - else { + const This* f = dynamic_cast(&lf); + if (!f) return false; + for (size_t pos = 0; pos < size(); ++pos) { + if (keys_[pos] != f->keys_[pos]) return false; + if (Fblocks_[pos].first != f->Fblocks_[pos].first) return false; + if (!equal_with_abs_tol(Fblocks_[pos].second,f->Fblocks_[pos].second,tol)) return false; } + return equal_with_abs_tol(PointCovariance_, f->PointCovariance_, tol) + && equal_with_abs_tol(E_, f->E_, tol) + && equal_with_abs_tol(b_, f->b_, tol); } /// Degrees of freedom of camera @@ -458,7 +467,12 @@ public: }; -// RegularImplicitSchurFactor +// end class RegularImplicitSchurFactor + +// traits +template struct traits > : public Testable< + RegularImplicitSchurFactor > { +}; } diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index 02e4fd3b6..4f982f456 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -446,6 +446,85 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ) { EXPECT(assert_equal(-f(*point), actualErrors, 1e-7)); } +/* *************************************************************************/ +TEST( SmartProjectionPoseFactor, Factors ){ + + // Default cameras for simple derivatives + Rot3 R; + static Cal3_S2::shared_ptr K(new Cal3_S2(100, 100, 0, 0, 0)); + SimpleCamera cam1(Pose3(R,Point3(0,0,0)),*K), cam2(Pose3(R,Point3(1,0,0)),*K); + + // one landmarks 1m in front of camera + Point3 landmark1(0,0,10); + + vector measurements_cam1; + + // Project 2 landmarks into 2 cameras + cout << cam1.project(landmark1) << endl; + cout << cam2.project(landmark1) << endl; + measurements_cam1.push_back(cam1.project(landmark1)); + measurements_cam1.push_back(cam2.project(landmark1)); + + // Create smart factors + std::vector views; + views.push_back(x1); + views.push_back(x2); + + SmartFactor::shared_ptr smartFactor1 = boost::make_shared(); + smartFactor1->add(measurements_cam1, views, model, K); + + SmartFactor::Cameras cameras; + cameras.push_back(cam1); + cameras.push_back(cam2); + + // Make sure triangulation works + LONGS_EQUAL(2,smartFactor1->triangulateSafe(cameras)); + CHECK(!smartFactor1->isDegenerate()); + CHECK(!smartFactor1->isPointBehindCamera()); + boost::optional p = smartFactor1->point(); + CHECK(p); + EXPECT(assert_equal(landmark1,*p)); + + { + // RegularHessianFactor<6> + Matrix66 G11; G11.setZero(); G11(0,0) = 100; G11(0,4) = -10; G11(4,0) = -10; G11(4,4) = 1; + Matrix66 G12; G12 = -G11; G12(0,2) = -10; G12(4,2) = 1; + Matrix66 G22; G22 = G11; G22(0,2) = 10; G22(2,0) = 10; G22(2,2) = 1; G22(2,4) = -1; G22(4,2) = -1; + + Vector6 g1; g1.setZero(); + Vector6 g2; g2.setZero(); + + double f = 0; + + RegularHessianFactor<6> expected(x1, x2, 5000 * G11, 5000 * G12, g1, 5000 * G22, g2, f); + + boost::shared_ptr > actual = + smartFactor1->createHessianFactor(cameras, 0.0); + CHECK(assert_equal(expected.information(),actual->information(),1e-8)); + CHECK(assert_equal(expected,*actual,1e-8)); + } + + { + // RegularImplicitSchurFactor<6> + Matrix26 F1; F1.setZero(); F1(0,1)=-100; F1(0,3)=-10; F1(1,0)=100; F1(1,4)=-10; + Matrix26 F2; F2.setZero(); F2(0,1)=-101; F2(0,3)=-10; F2(0,5)=-1; F2(1,0)=100; F2(1,2)=10; F2(1,4)=-10; + Matrix43 E; E.setZero(); E(0,0)=100; E(1,1)=100; E(2,0)=100; E(2,2)=10;E(3,1)=100; + const vector > Fblocks = list_of > // + (make_pair(x1, 10*F1))(make_pair(x2, 10*F2)); + Matrix3 P = (E.transpose() * E).inverse(); + Vector4 b; b.setZero(); + + RegularImplicitSchurFactor<6> expected(Fblocks, E, P, b); + + boost::shared_ptr > actual = + smartFactor1->createRegularImplicitSchurFactor(cameras, 0.0); + CHECK(actual); + CHECK(assert_equal(expected,*actual)); + } + +} + + /* *************************************************************************/ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) { // cout << " ************************ SmartProjectionPoseFactor: 3 cams + 3 landmarks **********************" << endl; From 9755b95e8999af96fe248eb16126655ef8c425de Mon Sep 17 00:00:00 2001 From: Jing Dong Date: Sat, 4 Apr 2015 17:13:58 -0400 Subject: [PATCH 54/91] looking at fix Values of FixedInsertDynamicRead --- gtsam/nonlinear/Values-inl.h | 6 +++++- gtsam/nonlinear/tests/testValues.cpp | 30 +++++++++++++++++++++++++--- 2 files changed, 32 insertions(+), 4 deletions(-) diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h index 96c352833..8a905a0ac 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -281,7 +281,7 @@ namespace gtsam { } } }; - +/* // Handle dynamic matrices template struct handle > { @@ -296,6 +296,8 @@ namespace gtsam { } }; + */ + // Request for a fixed vector template struct handle > { @@ -329,6 +331,8 @@ namespace gtsam { } } }; + + } // internal /* ************************************************************************* */ diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index ea5c67d9f..ecc01c95b 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -476,16 +476,40 @@ TEST(Values, Destructors) { } /* ************************************************************************* */ -TEST(Values, FixedVector) { +TEST(Values, VectorDynamicInsertFixedRead) { Values values; Vector v(3); v << 5.0, 6.0, 7.0; values.insert(key1, v); Vector3 expected(5.0, 6.0, 7.0); - CHECK(assert_equal((Vector)expected, values.at(key1))); + Vector3 actual = values.at(key1); + CHECK(assert_equal(expected, actual)); CHECK_EXCEPTION(values.at(key1), exception); } + /* ************************************************************************* */ -TEST(Values, FixedMatrix) { +TEST(Values, VectorFixedInsertDynamicRead) { + Values values; + Vector3 v; v << 5.0, 6.0, 7.0; + values.insert(key1, v); + Vector expected(3); expected << 5.0, 6.0, 7.0; + Vector actual(3); actual = values.at(key1); + //CHECK(assert_equal(expected, actual)); + CHECK_EXCEPTION(values.at(key1), exception); +} + +/* ************************************************************************* */ +TEST(Values, VectorFixedInsertFixedRead) { + Values values; + Vector3 v; v << 5.0, 6.0, 7.0; + values.insert(key1, v); + Vector3 expected; expected << 5.0, 6.0, 7.0; + Vector3 actual = values.at(key1); + //CHECK(assert_equal(expected, actual)); + CHECK_EXCEPTION(values.at(key1), exception); +} + +/* ************************************************************************* */ +TEST(Values, MatrixDynamicInsertFixedRead) { Values values; Matrix v(1,3); v << 5.0, 6.0, 7.0; values.insert(key1, v); From e1faf8874e4569ad02234534e6866ac0e587c5e4 Mon Sep 17 00:00:00 2001 From: Jing Dong Date: Sun, 5 Apr 2015 18:24:17 -0400 Subject: [PATCH 55/91] try to fix FixedValues branch by wrap all Eigen type in Dynamic size in Values, since fixed size Eigen type cannot read out by GenericValue if size is not explictly given --- gtsam/nonlinear/Values-inl.h | 42 ++++++++++++++++++++++++---- gtsam/nonlinear/tests/testValues.cpp | 25 +++++++++++++---- 2 files changed, 56 insertions(+), 11 deletions(-) diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h index 8a905a0ac..c2e7ac8fe 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -274,7 +274,7 @@ namespace gtsam { Eigen::Matrix operator()(Key j, const gtsam::Value * const pointer) { try { - return dynamic_cast >&>(*pointer).value(); + return reinterpret_cast >&>(*pointer).value(); } catch (std::bad_cast &) { throw ValuesIncorrectType(j, typeid(*pointer), typeid(Eigen::Matrix)); @@ -367,16 +367,48 @@ namespace gtsam { } /* ************************************************************************* */ + + // wrap all fixed in dynamics when insert and update + namespace internal { + + // general type + template + struct handle_wrap { + inline gtsam::GenericValue operator()(Key j, const ValueType& val) { + return gtsam::GenericValue(val); + } + }; + + // when insert/update a fixed size vector: convert to dynamic size + template + struct handle_wrap > { + inline gtsam::GenericValue > operator()( + Key j, const Eigen::Matrix& val) { + return gtsam::GenericValue >(val); + } + }; + + // when insert/update a fixed size matrix: convert to dynamic size + template + struct handle_wrap > { + inline gtsam::GenericValue > operator()( + Key j, const Eigen::Matrix& val) { + return gtsam::GenericValue >(val); + } + }; + + } + // insert a templated value template - void Values::insert(Key j, const ValueType& val) { - insert(j, static_cast(GenericValue(val))); - } + void Values::insert(Key j, const ValueType& val) { + insert(j, static_cast(internal::handle_wrap()(j, val))); + } // update with templated value template void Values::update(Key j, const ValueType& val) { - update(j, static_cast(GenericValue(val))); + update(j, static_cast(internal::handle_wrap()(j, val))); } } diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index ecc01c95b..b283f5355 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -487,14 +487,15 @@ TEST(Values, VectorDynamicInsertFixedRead) { } /* ************************************************************************* */ -TEST(Values, VectorFixedInsertDynamicRead) { +TEST(Values, VectorDynamicInsertDynamicRead) { Values values; - Vector3 v; v << 5.0, 6.0, 7.0; + Vector v(3); v << 5.0, 6.0, 7.0; values.insert(key1, v); Vector expected(3); expected << 5.0, 6.0, 7.0; - Vector actual(3); actual = values.at(key1); - //CHECK(assert_equal(expected, actual)); - CHECK_EXCEPTION(values.at(key1), exception); + Vector actual = values.at(key1); + LONGS_EQUAL(3, actual.rows()); + LONGS_EQUAL(1, actual.cols()); + CHECK(assert_equal(expected, actual)); } /* ************************************************************************* */ @@ -504,10 +505,22 @@ TEST(Values, VectorFixedInsertFixedRead) { values.insert(key1, v); Vector3 expected; expected << 5.0, 6.0, 7.0; Vector3 actual = values.at(key1); - //CHECK(assert_equal(expected, actual)); + CHECK(assert_equal(expected, actual)); CHECK_EXCEPTION(values.at(key1), exception); } +/* ************************************************************************* */ +TEST(Values, VectorFixedInsertDynamicRead) { + Values values; + Vector3 v; v << 5.0, 6.0, 7.0; + values.insert(key1, v); + Vector expected(3); expected << 5.0, 6.0, 7.0; + Vector actual = values.at(key1); + LONGS_EQUAL(3, actual.rows()); + LONGS_EQUAL(1, actual.cols()); + CHECK(assert_equal(expected, actual)); +} + /* ************************************************************************* */ TEST(Values, MatrixDynamicInsertFixedRead) { Values values; From 69ff1c4e7dcf61eface0bd843f79e47818333493 Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 6 Apr 2015 08:58:38 -0700 Subject: [PATCH 56/91] Commented with Jing --- gtsam/nonlinear/Values-inl.h | 22 +++++++++++++++++----- gtsam/nonlinear/Values.h | 5 +++-- 2 files changed, 20 insertions(+), 7 deletions(-) diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h index 8a905a0ac..90ede7a2d 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -261,6 +261,7 @@ namespace gtsam { struct handle { ValueType operator()(Key j, const gtsam::Value * const pointer) { try { + // value returns a const ValueType&, and the return makes a copy !!!!! return dynamic_cast&>(*pointer).value(); } catch (std::bad_cast &) { throw ValuesIncorrectType(j, typeid(*pointer), typeid(ValueType)); @@ -274,42 +275,49 @@ namespace gtsam { Eigen::Matrix operator()(Key j, const gtsam::Value * const pointer) { try { + // value returns a const Vector&, and the return makes a copy !!!!! return dynamic_cast >&>(*pointer).value(); } catch (std::bad_cast &) { + // If a fixed vector was stored, we end up here as well. throw ValuesIncorrectType(j, typeid(*pointer), typeid(Eigen::Matrix)); } } }; -/* + // Handle dynamic matrices template struct handle > { Eigen::Matrix operator()(Key j, const gtsam::Value * const pointer) { try { + // value returns a const Matrix&, and the return makes a copy !!!!! return dynamic_cast >&>(*pointer).value(); } catch (std::bad_cast &) { + // If a fixed matrix was stored, we end up here as well. throw ValuesIncorrectType(j, typeid(*pointer), typeid(Eigen::Matrix)); } } }; - */ - // Request for a fixed vector + // TODO(jing): is this piece of code really needed ??? template struct handle > { Eigen::Matrix operator()(Key j, const gtsam::Value * const pointer) { try { + // value returns a const VectorM&, and the return makes a copy !!!!! return dynamic_cast >&>(*pointer).value(); } catch (std::bad_cast &) { - Matrix A = handle()(j, pointer); + // Check if a dynamic vector was stored + Matrix A = handle()(j, pointer); // will throw if not.... + // Yes: check size, and throw if not a match if (A.rows() != M || A.cols() != 1) throw NoMatchFoundForFixed(M, 1, A.rows(), A.cols()); else + // This is not a copy because of RVO return A; } } @@ -321,12 +329,16 @@ namespace gtsam { Eigen::Matrix operator()(Key j, const gtsam::Value * const pointer) { try { + // value returns a const MatrixMN&, and the return makes a copy !!!!! return dynamic_cast >&>(*pointer).value(); } catch (std::bad_cast &) { - Matrix A = handle()(j, pointer); + // Check if a dynamic matrix was stored + Matrix A = handle()(j, pointer); // will throw if not.... + // Yes: check size, and throw if not a match if (A.rows() != M || A.cols() != N) throw NoMatchFoundForFixed(M, N, A.rows(), A.cols()); else + // This is not a copy because of RVO return A; } } diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index 15d3ac9e2..780a75931 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -173,8 +173,9 @@ namespace gtsam { /** Retrieve a variable by key \c j. The type of the value associated with * this key is supplied as a template argument to this function. * @param j Retrieve the value associated with this key - * @tparam Value The type of the value stored with this key, this method - * throws DynamicValuesIncorrectType if this requested type is not correct. + * @tparam ValueType The type of the value stored with this key, this method + * Throws DynamicValuesIncorrectType if this requested type is not correct. + * Dynamic matrices/vectors can be retrieved as fixed-size, but not vice-versa. * @return The stored value */ template From d1b582ccf70f14301ec2c87b070c090122180e00 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 7 Feb 2016 19:41:11 -0800 Subject: [PATCH 57/91] Restored to develop --- .cproject | 3358 +++++++++++++++++++++++++++-------------------------- 1 file changed, 1728 insertions(+), 1630 deletions(-) diff --git a/.cproject b/.cproject index 35bb1ecf3..10b16f91c 100644 --- a/.cproject +++ b/.cproject @@ -5,13 +5,13 @@ - - + + @@ -60,13 +60,13 @@ - - + + @@ -116,13 +116,13 @@ - - + + @@ -484,333 +484,6 @@ - - make - -j5 - testCombinedImuFactor.run - true - true - true - - - make - -j5 - testImuFactor.run - true - true - true - - - make - -j5 - testAHRSFactor.run - true - true - true - - - make - -j8 - testAttitudeFactor.run - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - testNonlinearConstraint.run - true - true - true - - - make - -j2 - testLieConfig.run - true - true - true - - - make - -j2 - testConstraintOptimizer.run - true - true - true - - - make - -j2 - testGaussianFactor.run - true - true - true - - - make - -j4 - testImuFactor.run - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - testBTree.run - true - true - true - - - make - -j2 - testDSF.run - true - true - true - - - make - -j2 - testDSFVector.run - true - true - true - - - make - -j2 - testMatrix.run - true - true - true - - - make - -j2 - testSPQRUtil.run - true - true - true - - - make - -j2 - testVector.run - true - true - true - - - make - -j2 - timeMatrix.run - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j5 - wrap - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - install - true - true - true - - - make - -j2 - all - true - true - true - - - cmake - .. - true - false - true - - - make - -j2 - check - true - true - true - - - make - -j2 - tests/testLieConfig.run - true - true - true - - - make - -j5 - testInvDepthCamera3.run - true - true - true - - - make - -j5 - testTriangulation.run - true - true - true - - - make - -j4 - testEvent.run - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j5 - all - true - false - true - - - make - -j5 - check - true - false - true - make -j5 @@ -859,167 +532,39 @@ true true - - make - -j5 - testCal3Bundler.run - true - true - true - - - make - -j5 - testCal3DS2.run - true - true - true - - - make - -j5 - testCalibratedCamera.run - true - true - true - - - make - -j5 - testEssentialMatrix.run - true - true - true - - - make - -j1 VERBOSE=1 - testHomography2.run - true - false - true - - - make - -j5 - testPinholeCamera.run - true - true - true - - - make - -j5 - testPoint2.run - true - true - true - - - make - -j5 - testPoint3.run - true - true - true - - - make - -j5 - testPose2.run - true - true - true - - - make - -j5 - testPose3.run - true - true - true - - - make - -j5 - testRot3M.run - true - true - true - - - make - -j5 - testSphere2.run - true - true - true - - - make - -j5 - testStereoCamera.run - true - true - true - - - make - -j5 - testCal3Unified.run - true - true - true - - - make - -j5 - testRot2.run - true - true - true - - - make - -j5 - testRot3Q.run - true - true - true - - - make - -j5 - testRot3.run - true - true - true - - + make -j4 - testSO3.run + testSimilarity3.run true true true - + + make + -j5 + testInvDepthCamera3.run + true + true + true + + + make + -j5 + testTriangulation.run + true + true + true + + make -j4 - testQuaternion.run + testEvent.run true true true - - make - -j4 - testCameraSet.run - true - true - true - - + make -j2 all @@ -1027,7 +572,7 @@ true true - + make -j2 clean @@ -1035,23 +580,143 @@ true true - + + make + -k + check + true + false + true + + + make + + tests/testBayesTree.run + true + false + true + + + make + + testBinaryBayesNet.run + true + false + true + + make -j2 - clean all + testFactorGraph.run true true true - + make -j2 - install + testISAM.run true true true - + + make + -j2 + testJunctionTree.run + true + true + true + + + make + -j2 + testKey.run + true + true + true + + + make + -j2 + testOrdering.run + true + true + true + + + make + + testSymbolicBayesNet.run + true + false + true + + + make + + tests/testSymbolicFactor.run + true + false + true + + + make + + testSymbolicFactorGraph.run + true + false + true + + + make + -j2 + timeSymbolMaps.run + true + true + true + + + make + + tests/testBayesTree + true + false + true + + + make + -j2 + tests/testPose2.run + true + true + true + + + make + -j2 + tests/testPose3.run + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + check + true + true + true + + make -j2 clean @@ -1107,6 +772,14 @@ true true + + make + -j5 + testSmartProjectionFactor.run + true + true + true + make -j5 @@ -1195,146 +868,154 @@ true true - + make - -j2 - all + -j5 + testGaussianFactorGraphUnordered.run true true true - + make - -j2 - check + -j5 + testGaussianBayesNetUnordered.run true true true - + make - -j2 + -j5 + testGaussianConditional.run + true + true + true + + + make + -j5 + testGaussianDensity.run + true + true + true + + + make + -j5 + testGaussianJunctionTree.run + true + true + true + + + make + -j5 + testHessianFactor.run + true + true + true + + + make + -j5 + testJacobianFactor.run + true + true + true + + + make + -j5 + testKalmanFilter.run + true + true + true + + + make + -j5 + testNoiseModel.run + true + true + true + + + make + -j5 + testSampler.run + true + true + true + + + make + -j5 + testSerializationLinear.run + true + true + true + + + make + -j5 + testVectorValues.run + true + true + true + + + make + -j5 + testGaussianBayesTree.run + true + true + true + + + make + -j5 + testCombinedImuFactor.run + true + true + true + + + make + -j5 + testImuFactor.run + true + true + true + + + make + -j5 + testAHRSFactor.run + true + true + true + + + make + -j8 + testAttitudeFactor.run + true + true + true + + + make + -j5 clean true true true - + make - -j2 - testPlanarSLAM.run - true - true - true - - - make - -j2 - testPose2Config.run - true - true - true - - - make - -j2 - testPose2Factor.run - true - true - true - - - make - -j2 - testPose2Prior.run - true - true - true - - - make - -j2 - testPose2SLAM.run - true - true - true - - - make - -j2 - testPose3Config.run - true - true - true - - - make - -j2 - testPose3SLAM.run - true - true - true - - - make - - testSimulated2DOriented.run - true - false - true - - - make - -j2 - testVSLAMConfig.run - true - true - true - - - make - -j2 - testVSLAMFactor.run - true - true - true - - - make - -j2 - testVSLAMGraph.run - true - true - true - - - make - -j2 - testPose3Factor.run - true - true - true - - - make - - testSimulated2D.run - true - false - true - - - make - - testSimulated3D.run - true - false - true - - - make - -j2 - tests/testGaussianISAM2 + -j5 + all true true true @@ -1429,127 +1110,477 @@ make + testErrors.run true false true - + make - -j5 - testAntiFactor.run + -j2 + check true true true - + make - -j5 - testPriorFactor.run + -j2 + tests/testGaussianJunctionTree.run true true true - + make - -j5 - testDataset.run + -j2 + tests/testGaussianFactor.run true true true - + make - -j5 - testEssentialMatrixFactor.run + -j2 + tests/testGaussianConditional.run true true true - + make - -j5 - testGeneralSFMFactor_Cal3Bundler.run + -j2 + tests/timeSLAMlike.run true true true - + make - -j5 - testGeneralSFMFactor.run + -j2 + check true true true - + make - -j5 - testProjectionFactor.run + -j2 + clean true true true - + make - -j5 - testRotateFactor.run + -j2 + testBTree.run true true true - + make - -j5 - testPoseRotationPrior.run + -j2 + testDSF.run true true true - + make - -j5 - testImplicitSchurFactor.run + -j2 + testDSFVector.run true true true - + + make + -j2 + testMatrix.run + true + true + true + + + make + -j2 + testSPQRUtil.run + true + true + true + + + make + -j2 + testVector.run + true + true + true + + + make + -j2 + timeMatrix.run + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + testClusterTree.run + true + true + true + + + make + -j2 + testJunctionTree.run + true + true + true + + + make + -j2 + tests/testEliminationTree.run + true + true + true + + + make + -j2 + tests/testSymbolicFactor.run + true + true + true + + + make + -j2 + tests/testVariableSlots.run + true + true + true + + + make + -j2 + tests/testConditional.run + true + true + true + + + make + -j2 + tests/testSymbolicFactorGraph.run + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + testNonlinearConstraint.run + true + true + true + + + make + -j2 + testLieConfig.run + true + true + true + + + make + -j2 + testConstraintOptimizer.run + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + testPlanarSLAM.run + true + true + true + + + make + -j2 + testPose2Config.run + true + true + true + + + make + -j2 + testPose2Factor.run + true + true + true + + + make + -j2 + testPose2Prior.run + true + true + true + + + make + -j2 + testPose2SLAM.run + true + true + true + + + make + -j2 + testPose3Config.run + true + true + true + + + make + -j2 + testPose3SLAM.run + true + true + true + + + make + testSimulated2DOriented.run + true + false + true + + + make + -j2 + testVSLAMConfig.run + true + true + true + + + make + -j2 + testVSLAMFactor.run + true + true + true + + + make + -j2 + testVSLAMGraph.run + true + true + true + + + make + -j2 + testPose3Factor.run + true + true + true + + + make + testSimulated2D.run + true + false + true + + + make + testSimulated3D.run + true + false + true + + + make + -j2 + tests/testGaussianISAM2 + true + true + true + + + make + -j5 + testBTree.run + true + true + true + + + make + -j5 + testDSF.run + true + true + true + + + make + -j5 + testDSFMap.run + true + true + true + + + make + -j5 + testDSFVector.run + true + true + true + + + make + -j5 + testFixedVector.run + true + true + true + + + make + -j5 + testEliminationTree.run + true + true + true + + + make + -j5 + testInference.run + true + true + true + + + make + -j5 + testKey.run + true + true + true + + + make + -j1 + testSymbolicBayesTree.run + true + false + true + + + make + -j1 + testSymbolicSequentialSolver.run + true + false + true + + make -j4 - testRangeFactor.run + testLabeledSymbol.run true true true - + make - -j4 - testSmartProjectionPoseFactor.run + -j2 + check true true true - + make - -j4 - testRegularImplicitSchurFactor.run - true - true - true - - - make - -j4 - testRegularHessianFactor.run - true - true - true - - - make - -j4 - testSmartFactorBase.run + -j2 + tests/testLieConfig.run true true true @@ -1755,6 +1786,7 @@ cpack + -G DEB true false @@ -1762,6 +1794,7 @@ cpack + -G RPM true false @@ -1769,6 +1802,7 @@ cpack + -G TGZ true false @@ -1776,6 +1810,7 @@ cpack + --config CPackSourceConfig.cmake true false @@ -1957,7 +1992,15 @@ false true - + + make + -j4 + check.sam + true + true + true + + make -j2 check @@ -1965,55 +2008,38 @@ true true - + make - - tests/testGaussianISAM2 + -j2 + clean + true + true + true + + + make + -j2 + install + true + true + true + + + make + -j2 + all + true + true + true + + + cmake + .. true false true - - make - -j2 - check - true - true - true - - - make - -j2 - tests/testGaussianJunctionTree.run - true - true - true - - - make - -j2 - tests/testGaussianFactor.run - true - true - true - - - make - -j2 - tests/testGaussianConditional.run - true - true - true - - - make - -j2 - tests/timeSLAMlike.run - true - true - true - - + make -j2 testGaussianFactor.run @@ -2021,521 +2047,207 @@ true true - + make -j5 - testParticleFactor.run + testCal3Bundler.run true true true - + make -j5 - testExpressionMeta.run + testCal3DS2.run true true true - - make - -j4 - testCustomChartExpression.run - true - true - true - - + make -j5 - schedulingExample.run - true - true - true - - - make - -j5 - schedulingQuals12.run - true - true - true - - - make - -j5 - schedulingQuals13.run - true - true - true - - - make - -j5 - testCSP.run - true - true - true - - - make - -j5 - testScheduler.run - true - true - true - - - make - -j5 - testSudoku.run - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -k - check - true - false - true - - - make - tests/testBayesTree.run - true - false - true - - - make - testBinaryBayesNet.run - true - false - true - - - make - -j2 - testFactorGraph.run - true - true - true - - - make - -j2 - testISAM.run - true - true - true - - - make - -j2 - testJunctionTree.run - true - true - true - - - make - -j2 - testKey.run - true - true - true - - - make - -j2 - testOrdering.run - true - true - true - - - make - testSymbolicBayesNet.run - true - false - true - - - make - tests/testSymbolicFactor.run - true - false - true - - - make - testSymbolicFactorGraph.run - true - false - true - - - make - -j2 - timeSymbolMaps.run - true - true - true - - - make - tests/testBayesTree - true - false - true - - - make - -j2 - check - true - true - true - - - make - -j2 - timeCalibratedCamera.run - true - true - true - - - make - -j2 - timeRot3.run - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - tests/testPose2.run - true - true - true - - - make - -j2 - tests/testPose3.run - true - true - true - - - make - -j2 - SimpleRotation.run - true - true - true - - - make - -j5 - CameraResectioning.run - true - true - true - - - make - -j5 - PlanarSLAMExample.run - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - easyPoint2KalmanFilter.run - true - true - true - - - make - -j2 - elaboratePoint2KalmanFilter.run - true - true - true - - - make - -j5 - Pose2SLAMExample.run - true - true - true - - - make - -j5 - UGM_small.run - true - true - true - - - make - -j5 - LocalizationExample.run - true - true - true - - - make - -j5 - OdometryExample.run - true - true - true - - - make - -j5 - RangeISAMExample_plaza2.run - true - true - true - - - make - -j5 - SelfCalibrationExample.run - true - true - true - - - make - -j5 - SFMExample.run - true - true - true - - - make - -j5 - VisualISAMExample.run - true - true - true - - - make - -j5 - VisualISAM2Example.run - true - true - true - - - make - -j5 - Pose2SLAMExample_graphviz.run - true - true - true - - - make - -j5 - Pose2SLAMExample_graph.run - true - true - true - - - make - -j5 - SFMExample_bal.run - true - true - true - - - make - -j5 - Pose2SLAMExample_lago.run - true - true - true - - - make - -j5 - Pose2SLAMExample_g2o.run - true - true - true - - - make - -j5 - SFMExample_SmartFactor.run - true - true - true - - - make - -j4 - Pose2SLAMExampleExpressions.run - true - true - true - - - make - -j4 - SFMExampleExpressions.run - true - true - true - - - make - -j4 - SFMExampleExpressions_bal.run - true - true - true - - - make - -j4 - Pose2SLAMwSPCG.run - true - true - true - - - make - -j4 - SFMExample_SmartFactorPCG.run - true - true - true - - - make - -j2 - testRot3.run - true - true - true - - - make - -j2 - testRot2.run - true - true - true - - - make - -j2 - testPose3.run - true - true - true - - - make - -j2 - timeRot3.run - true - true - true - - - make - -j2 - testPose2.run - true - true - true - - - make - -j2 - testCal3_S2.run - true - true - true - - - make - -j2 - testSimpleCamera.run - true - true - true - - - make - -j2 - testHomography2.run - true - true - true - - - make - -j2 testCalibratedCamera.run true true true - + + make + -j5 + testEssentialMatrix.run + true + true + true + + + make + -j1 VERBOSE=1 + testHomography2.run + true + false + true + + + make + -j5 + testPinholeCamera.run + true + true + true + + + make + -j5 + testPoint2.run + true + true + true + + + make + -j5 + testPoint3.run + true + true + true + + + make + -j5 + testPose2.run + true + true + true + + + make + -j5 + testPose3.run + true + true + true + + + make + -j5 + testRot3M.run + true + true + true + + + make + -j5 + testSphere2.run + true + true + true + + + make + -j5 + testStereoCamera.run + true + true + true + + + make + -j5 + testCal3Unified.run + true + true + true + + + make + -j5 + testRot2.run + true + true + true + + + make + -j5 + testRot3Q.run + true + true + true + + + make + -j5 + testRot3.run + true + true + true + + + make + -j4 + testSO3.run + true + true + true + + + make + -j4 + testQuaternion.run + true + true + true + + + make + -j4 + testOrientedPlane3.run + true + true + true + + + make + -j4 + testPinholePose.run + true + true + true + + + make + -j4 + testCyclic.run + true + true + true + + + make + -j4 + testUnit3.run + true + true + true + + + make + -j4 + testBearingRange.run + true + true + true + + + make + -j2 + all + true + true + true + + make -j2 check @@ -2543,7 +2255,7 @@ true true - + make -j2 clean @@ -2551,10 +2263,58 @@ true true - + + make + -j5 + all + true + false + true + + + make + -j5 + check + true + false + true + + make -j2 - testPoint2.run + all + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + clean all true true true @@ -2599,190 +2359,6 @@ true true - - make - -j5 - testEliminationTree.run - true - true - true - - - make - -j5 - testInference.run - true - true - true - - - make - -j5 - testKey.run - true - true - true - - - make - -j1 - testSymbolicBayesTree.run - true - false - true - - - make - -j1 - testSymbolicSequentialSolver.run - true - false - true - - - make - -j4 - testLabeledSymbol.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - testClusterTree.run - true - true - true - - - make - -j2 - testJunctionTree.run - true - true - true - - - make - -j2 - tests/testEliminationTree.run - true - true - true - - - make - -j2 - tests/testSymbolicFactor.run - true - true - true - - - make - -j2 - tests/testVariableSlots.run - true - true - true - - - make - -j2 - tests/testConditional.run - true - true - true - - - make - -j2 - tests/testSymbolicFactorGraph.run - true - true - true - - - make - -j5 - testLago.run - true - true - true - - - make - -j5 - testLinearContainerFactor.run - true - true - true - - - make - -j5 - testOrdering.run - true - true - true - - - make - -j5 - testValues.run - true - true - true - - - make - -j5 - testWhiteNoiseFactor.run - true - true - true - - - make - -j4 - testExpression.run - true - true - true - - - make - -j4 - testAdaptAutoDiff.run - true - true - true - - - make - -j4 - testCallRecord.run - true - true - true - - - make - -j4 - testExpressionFactor.run - true - true - true - make -j5 @@ -2815,135 +2391,31 @@ true true - + make - -j5 + -j2 + check + true + true + true + + + make + -j2 timeCalibratedCamera.run true true true - - make - -j5 - timePinholeCamera.run - true - true - true - - - make - -j5 - timeStereoCamera.run - true - true - true - - - make - -j5 - timeLago.run - true - true - true - - - make - -j5 - timePose3.run - true - true - true - - - make - -j4 - timeAdaptAutoDiff.run - true - true - true - - - make - -j4 - timeCameraExpression.run - true - true - true - - - make - -j4 - timeOneCameraExpression.run - true - true - true - - - make - -j4 - timeSFMExpressions.run - true - true - true - - - make - -j4 - timeIncremental.run - true - true - true - - - make - -j5 - testBTree.run - true - true - true - - - make - -j5 - testDSF.run - true - true - true - - - make - -j5 - testDSFMap.run - true - true - true - - - make - -j5 - testDSFVector.run - true - true - true - - - make - -j5 - testFixedVector.run - true - true - true - - + make -j2 - all + timeRot3.run true true true - + make -j2 clean @@ -2951,110 +2423,6 @@ true true - - make - -j5 - testGaussianFactorGraphUnordered.run - true - true - true - - - make - -j5 - testGaussianBayesNetUnordered.run - true - true - true - - - make - -j5 - testGaussianConditional.run - true - true - true - - - make - -j5 - testGaussianDensity.run - true - true - true - - - make - -j5 - testGaussianJunctionTree.run - true - true - true - - - make - -j5 - testHessianFactor.run - true - true - true - - - make - -j5 - testJacobianFactor.run - true - true - true - - - make - -j5 - testKalmanFilter.run - true - true - true - - - make - -j5 - testNoiseModel.run - true - true - true - - - make - -j5 - testSampler.run - true - true - true - - - make - -j5 - testSerializationLinear.run - true - true - true - - - make - -j5 - testVectorValues.run - true - true - true - - - make - -j5 - testGaussianBayesTree.run - true - true - true - make -j5 @@ -3135,18 +2503,66 @@ true true - + make - -j2 - all + -j5 + schedulingExample.run true true true - + + make + -j5 + schedulingQuals12.run + true + true + true + + + make + -j5 + schedulingQuals13.run + true + true + true + + + make + -j5 + testCSP.run + true + true + true + + + make + -j5 + testScheduler.run + true + true + true + + + make + -j5 + testSudoku.run + true + true + true + + make -j2 - clean + vSFMexample.run + true + true + true + + + make + -j2 + testVSLAMGraph true true true @@ -3191,6 +2607,14 @@ true true + + make + -j4 + testGroup.run + true + true + true + make -j5 @@ -3329,6 +2753,7 @@ make + testGraph.run true false @@ -3336,6 +2761,7 @@ make + testJunctionTree.run true false @@ -3343,6 +2769,7 @@ make + testSymbolicBayesNetB.run true false @@ -3412,39 +2839,487 @@ true true - + make -j4 - testPCGSolver.run + testLie.run true true true - + make - -j2 - vSFMexample.run + -j4 + testSerializationSLAM.run true true true - + make -j5 + testParticleFactor.run + true + true + true + + + make + -j2 + testGaussianFactor.run + true + true + true + + + make + -j2 + install + true + true + true + + + make + -j2 clean true true true - + make - -j5 + -j2 all true true true - + + make + -j2 + clean + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j5 + testAntiFactor.run + true + true + true + + + make + -j5 + testPriorFactor.run + true + true + true + + + make + -j5 + testDataset.run + true + true + true + + + make + -j5 + testEssentialMatrixFactor.run + true + true + true + + + make + -j5 + testGeneralSFMFactor_Cal3Bundler.run + true + true + true + + + make + -j5 + testGeneralSFMFactor.run + true + true + true + + + make + -j5 + testProjectionFactor.run + true + true + true + + + make + -j5 + testRotateFactor.run + true + true + true + + + make + -j5 + testPoseRotationPrior.run + true + true + true + + + make + -j5 + testImplicitSchurFactor.run + true + true + true + + + make + -j4 + testOrientedPlane3Factor.run + true + true + true + + + make + -j4 + testSmartProjectionPoseFactor.run + true + true + true + + + make + -j4 + testInitializePose3.run + true + true + true + + + make + -j2 + SimpleRotation.run + true + true + true + + + make + -j5 + CameraResectioning.run + true + true + true + + + make + -j5 + PlanarSLAMExample.run + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + easyPoint2KalmanFilter.run + true + true + true + + + make + -j2 + elaboratePoint2KalmanFilter.run + true + true + true + + + make + -j5 + Pose2SLAMExample.run + true + true + true + + + make + -j2 + Pose2SLAMwSPCG_easy.run + true + true + true + + + make + -j5 + UGM_small.run + true + true + true + + + make + -j5 + LocalizationExample.run + true + true + true + + + make + -j5 + OdometryExample.run + true + true + true + + + make + -j5 + RangeISAMExample_plaza2.run + true + true + true + + + make + -j5 + SelfCalibrationExample.run + true + true + true + + + make + -j5 + SFMExample.run + true + true + true + + + make + -j5 + VisualISAMExample.run + true + true + true + + + make + -j5 + VisualISAM2Example.run + true + true + true + + + make + -j5 + Pose2SLAMExample_graphviz.run + true + true + true + + + make + -j5 + Pose2SLAMExample_graph.run + true + true + true + + + make + -j5 + SFMExample_bal.run + true + true + true + + + make + -j5 + Pose2SLAMExample_lago.run + true + true + true + + + make + -j5 + Pose2SLAMExample_g2o.run + true + true + true + + + make + -j5 + SFMExample_SmartFactor.run + true + true + true + + + make + -j4 + Pose2SLAMExampleExpressions.run + true + true + true + + + make + -j4 + SFMExampleExpressions.run + true + true + true + + + make + -j4 + SFMExampleExpressions_bal.run + true + true + true + + + make + -j5 + testLago.run + true + true + true + + + make + -j5 + testLinearContainerFactor.run + true + true + true + + + make + -j5 + testOrdering.run + true + true + true + + + make + -j5 + testValues.run + true + true + true + + + make + -j5 + testWhiteNoiseFactor.run + true + true + true + + + make + -j4 + testExpression.run + true + true + true + + + make + -j4 + testAdaptAutoDiff.run + true + true + true + + + make + -j4 + testCallRecord.run + true + true + true + + + make + -j4 + testExpressionFactor.run + true + true + true + + + make + -j4 + testExecutionTrace.run + true + true + true + + + make + -j4 + testSerializationNonlinear.run + true + true + true + + + make + -j4 + testImuFactor.run + true + true + true + + make -j2 check @@ -3452,10 +3327,233 @@ true true - + + make + tests/testGaussianISAM2 + true + false + true + + + make + -j5 + timeCalibratedCamera.run + true + true + true + + + make + -j5 + timePinholeCamera.run + true + true + true + + + make + -j5 + timeStereoCamera.run + true + true + true + + + make + -j5 + timeLago.run + true + true + true + + + make + -j5 + timePose3.run + true + true + true + + + make + -j4 + timeAdaptAutoDiff.run + true + true + true + + + make + -j4 + timeCameraExpression.run + true + true + true + + + make + -j4 + timeOneCameraExpression.run + true + true + true + + + make + -j4 + timeSFMExpressions.run + true + true + true + + + make + -j4 + timeIncremental.run + true + true + true + + + make + -j4 + timeSchurFactors.run + true + true + true + + + make + -j4 + timeRot2.run + true + true + true + + make -j2 - testVSLAMGraph + testRot3.run + true + true + true + + + make + -j2 + testRot2.run + true + true + true + + + make + -j2 + testPose3.run + true + true + true + + + make + -j2 + timeRot3.run + true + true + true + + + make + -j2 + testPose2.run + true + true + true + + + make + -j2 + testCal3_S2.run + true + true + true + + + make + -j2 + testSimpleCamera.run + true + true + true + + + make + -j2 + testHomography2.run + true + true + true + + + make + -j2 + testCalibratedCamera.run + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + testPoint2.run + true + true + true + + + make + -j4 + testBearingFactor.run + true + true + true + + + make + -j4 + testRangeFactor.run + true + true + true + + + make + -j4 + testBearingRangeFactor.run + true + true + true + + + make + -j5 + wrap true true true From 71b6c1da8244426a8222fb2fb1a2fe09117d489a Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 10 Apr 2016 10:56:09 -0700 Subject: [PATCH 58/91] 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 59/91] 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 60/91] 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 From 6ccc2a84b35b46e1607272254ec4a20da107619f Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 7 Feb 2016 20:55:23 -0800 Subject: [PATCH 61/91] Deal with changes in Values --- python/handwritten/nonlinear/Values.cpp | 17 ++++++++--------- 1 file changed, 8 insertions(+), 9 deletions(-) diff --git a/python/handwritten/nonlinear/Values.cpp b/python/handwritten/nonlinear/Values.cpp index 84e82f376..9d99594e5 100644 --- a/python/handwritten/nonlinear/Values.cpp +++ b/python/handwritten/nonlinear/Values.cpp @@ -54,7 +54,6 @@ void exportValues(){ .def("empty", &Values::empty) .def("equals", &Values::equals) .def("erase", &Values::erase) - .def("insert_fixed", &Values::insertFixed) .def("print", &Values::print, print_overloads(args("s"))) .def("size", &Values::size) .def("swap", &Values::swap) @@ -66,14 +65,14 @@ void exportValues(){ .def("insert", insert_pose3) .def("insert", insert_bias) .def("insert", insert_vector3) - .def("atPoint2", &Values::at, return_value_policy()) - .def("atRot2", &Values::at, return_value_policy()) - .def("atPose2", &Values::at, return_value_policy()) - .def("atPoint3", &Values::at, return_value_policy()) - .def("atRot3", &Values::at, return_value_policy()) - .def("atPose3", &Values::at, return_value_policy()) - .def("atConstantBias", &Values::at, return_value_policy()) - .def("atVector3", &Values::at, return_value_policy()) + .def("atPoint2", &Values::at) + .def("atRot2", &Values::at) + .def("atPose2", &Values::at) + .def("atPoint3", &Values::at) + .def("atRot3", &Values::at) + .def("atPose3", &Values::at) + .def("atConstantBias", &Values::at) + .def("atVector3", &Values::at) .def("exists", exists1) .def("keys", &Values::keys) ; From f440ac92ff0b62091e50c48f36498c89208736ba Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 10 Apr 2016 18:30:24 -0700 Subject: [PATCH 62/91] Removed conflict stuff: Jing, why reinterpret_cast ? --- gtsam/nonlinear/Values-inl.h | 4 ---- 1 file changed, 4 deletions(-) diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h index 590767e90..cd8473a09 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -292,12 +292,8 @@ namespace gtsam { Eigen::Matrix operator()(Key j, const gtsam::Value * const pointer) { try { -//<<<<<<< HEAD // value returns a const Vector&, and the return makes a copy !!!!! return dynamic_cast >&>(*pointer).value(); -//======= -// return reinterpret_cast >&>(*pointer).value(); -//>>>>>>> feature/FixFixedValues } catch (std::bad_cast &) { // If a fixed vector was stored, we end up here as well. throw ValuesIncorrectType(j, typeid(*pointer), From ad54d7805c5ecb83101f16e98516c8378cc2fc64 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 10 Apr 2016 18:30:41 -0700 Subject: [PATCH 63/91] Return value, not const reference --- python/handwritten/nonlinear/Values.cpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/python/handwritten/nonlinear/Values.cpp b/python/handwritten/nonlinear/Values.cpp index 205f7fe23..0d1349972 100644 --- a/python/handwritten/nonlinear/Values.cpp +++ b/python/handwritten/nonlinear/Values.cpp @@ -68,15 +68,15 @@ void exportValues(){ .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()) - .def("atPose2", &Values::at, return_value_policy()) - .def("atPoint3", &Values::at, return_value_policy()) - .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("atPoint2", &Values::at) + .def("atRot2", &Values::at) + .def("atPose2", &Values::at) + .def("atPoint3", &Values::at) + .def("atRot3", &Values::at) + .def("atPose3", &Values::at) + .def("atConstantBias", &Values::at) + .def("atNavState", &Values::at) + .def("atVector3", &Values::at) .def("exists", exists1) .def("keys", &Values::keys) ; From a3b66a94eae53e5705ec0fc064d49412d1d491c7 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 10 Apr 2016 19:01:14 -0700 Subject: [PATCH 64/91] Cleaned up Double_ problems --- gtsam/nonlinear/tests/testExpression.cpp | 14 ++++++-------- gtsam_unstable/slam/TOAFactor.h | 6 +++--- gtsam_unstable/slam/tests/testTOAFactor.cpp | 5 ++--- 3 files changed, 11 insertions(+), 14 deletions(-) diff --git a/gtsam/nonlinear/tests/testExpression.cpp b/gtsam/nonlinear/tests/testExpression.cpp index 43f1cd017..8f059f84c 100644 --- a/gtsam/nonlinear/tests/testExpression.cpp +++ b/gtsam/nonlinear/tests/testExpression.cpp @@ -17,7 +17,7 @@ * @brief unit tests for Block Automatic Differentiation */ -#include +#include #include #include #include @@ -32,9 +32,7 @@ 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_; @@ -101,7 +99,7 @@ TEST(Expression, Unary1) { } TEST(Expression, Unary2) { using namespace unary; - double_ e(f2, p); + Double_ e(f2, p); EXPECT(expected == e.keys()); } @@ -156,7 +154,7 @@ Point3_ p_cam(x, &Pose3::transform_to, p); // Check that creating an expression to double compiles TEST(Expression, BinaryToDouble) { using namespace binary; - double_ p_cam(doubleF, x, p); + Double_ p_cam(doubleF, x, p); } /* ************************************************************************* */ @@ -372,8 +370,8 @@ TEST(Expression, TripleSum) { /* ************************************************************************* */ TEST(Expression, SumOfUnaries) { const Key key(67); - const double_ norm_(>sam::norm, Point3_(key)); - const double_ sum_ = norm_ + norm_; + const Double_ norm_(>sam::norm, Point3_(key)); + const Double_ sum_ = norm_ + norm_; Values values; values.insert(key, Point3(6, 0, 0)); @@ -391,7 +389,7 @@ TEST(Expression, SumOfUnaries) { TEST(Expression, UnaryOfSum) { const Key key1(42), key2(67); const Point3_ sum_ = Point3_(key1) + Point3_(key2); - const double_ norm_(>sam::norm, sum_); + const Double_ norm_(>sam::norm, sum_); map actual_dims, expected_dims = map_list_of(key1, 3)(key2, 3); norm_.dims(actual_dims); diff --git a/gtsam_unstable/slam/TOAFactor.h b/gtsam_unstable/slam/TOAFactor.h index b500b36e3..e37e8ff20 100644 --- a/gtsam_unstable/slam/TOAFactor.h +++ b/gtsam_unstable/slam/TOAFactor.h @@ -17,7 +17,7 @@ * @date December 2014 */ -#include +#include #include namespace gtsam { @@ -25,7 +25,7 @@ namespace gtsam { /// A "Time of Arrival" factor - so little code seems hardly worth it :-) class TOAFactor: public ExpressionFactor { - typedef Expression double_; + typedef Expression Double_; public: @@ -40,7 +40,7 @@ public: const Expression& microphone_, double toaMeasurement, const SharedNoiseModel& model) : ExpressionFactor(model, toaMeasurement, - double_(&Event::toa, eventExpression, microphone_)) { + Double_(&Event::toa, eventExpression, microphone_)) { } }; diff --git a/gtsam_unstable/slam/tests/testTOAFactor.cpp b/gtsam_unstable/slam/tests/testTOAFactor.cpp index 52786702d..233dbdceb 100644 --- a/gtsam_unstable/slam/tests/testTOAFactor.cpp +++ b/gtsam_unstable/slam/tests/testTOAFactor.cpp @@ -31,7 +31,6 @@ using namespace std; using namespace gtsam; // typedefs -typedef Eigen::Matrix Vector1; typedef Expression Point3_; typedef Expression Event_; @@ -52,7 +51,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 +91,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); } From b7381716520fd2d51ea6a1a78b62acc4ea8df52f Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 10 Apr 2016 19:01:42 -0700 Subject: [PATCH 65/91] restored use of internal::handle that was accidentally removed --- gtsam/nonlinear/Values-inl.h | 10 ++-------- 1 file changed, 2 insertions(+), 8 deletions(-) diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h index cd8473a09..446a67ec7 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -274,6 +274,7 @@ namespace gtsam { namespace internal { // Check the type and throw exception if incorrect + // Generic version, partially specialized below for various Eigen Matrix types template struct handle { ValueType operator()(Key j, const gtsam::Value * const pointer) { @@ -375,14 +376,7 @@ namespace gtsam { throw ValuesKeyDoesNotExist("at", j); // Check the type and throw exception if incorrect - const Value& value = *item->second; - try { - return dynamic_cast&>(value).value(); - } catch (std::bad_cast &) { - // NOTE(abe): clang warns about potential side effects if done in typeid - const Value* value = item->second; - throw ValuesIncorrectType(j, typeid(*value), typeid(ValueType)); - } + return internal::handle()(j,item->second); } /* ************************************************************************* */ From 1feed7c20ed292be6ccf8dfe4b2467aef81abea1 Mon Sep 17 00:00:00 2001 From: alexhagiopol Date: Mon, 11 Apr 2016 11:34:57 -0400 Subject: [PATCH 66/91] Work in progress: deprecating inline functions. --- gtsam/base/Matrix.cpp | 78 +++---------------------------- gtsam/base/Matrix.h | 63 +++++++++---------------- gtsam/geometry/OrientedPlane3.cpp | 2 +- gtsam/geometry/Pose2.cpp | 2 +- gtsam/geometry/triangulation.cpp | 2 +- gtsam/linear/KalmanFilter.h | 2 +- gtsam/linear/NoiseModel.h | 2 +- gtsam/slam/InitializePose3.cpp | 2 +- gtsam/slam/PriorFactor.h | 2 +- gtsam/slam/dataset.cpp | 8 ++-- gtsam/slam/lago.cpp | 4 +- 11 files changed, 41 insertions(+), 126 deletions(-) diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp index 1e5da7d86..ff509b41e 100644 --- a/gtsam/base/Matrix.cpp +++ b/gtsam/base/Matrix.cpp @@ -38,26 +38,6 @@ using namespace std; namespace gtsam { -/* ************************************************************************* */ -Matrix zeros( size_t m, size_t n ) { - return Matrix::Zero(m,n); -} - -/* ************************************************************************* */ -Matrix ones( size_t m, size_t n ) { - return Matrix::Ones(m,n); -} - -/* ************************************************************************* */ -Matrix eye( size_t m, size_t n) { - return Matrix::Identity(m, n); -} - -/* ************************************************************************* */ -Matrix diag(const Vector& v) { - return v.asDiagonal(); -} - /* ************************************************************************* */ bool assert_equal(const Matrix& expected, const Matrix& actual, double tol) { @@ -146,16 +126,6 @@ bool linear_dependent(const Matrix& A, const Matrix& B, double tol) { } } -/* ************************************************************************* */ -void multiplyAdd(double alpha, const Matrix& A, const Vector& x, Vector& e) { - e += alpha * A * x; -} - -/* ************************************************************************* */ -void multiplyAdd(const Matrix& A, const Vector& x, Vector& e) { - e += A * x; -} - /* ************************************************************************* */ Vector operator^(const Matrix& A, const Vector & v) { if (A.rows()!=v.size()) throw std::invalid_argument( @@ -166,21 +136,6 @@ Vector operator^(const Matrix& A, const Vector & v) { return A.transpose() * v; } -/* ************************************************************************* */ -void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, Vector& x) { - x += alpha * A.transpose() * e; -} - -/* ************************************************************************* */ -void transposeMultiplyAdd(const Matrix& A, const Vector& e, Vector& x) { - x += A.transpose() * e; -} - -/* ************************************************************************* */ -void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, SubVector x) { - x += alpha * A.transpose() * e; -} - /* ************************************************************************* */ //3 argument call void print(const Matrix& A, const string &s, ostream& stream) { @@ -250,7 +205,7 @@ Matrix diag(const std::vector& Hs) { rows+= Hs[i].rows(); cols+= Hs[i].cols(); } - Matrix results = zeros(rows,cols); + Matrix results = Matrix::Zero(rows,cols); size_t r = 0, c = 0; for (size_t i = 0; i& Hs) { return results; } -/* ************************************************************************* */ -void insertColumn(Matrix& A, const Vector& col, size_t j) { - A.col(j) = col; -} - -/* ************************************************************************* */ -void insertColumn(Matrix& A, const Vector& col, size_t i, size_t j) { - A.col(j).segment(i, col.size()) = col; -} - /* ************************************************************************* */ Vector columnNormSquare(const Matrix &A) { Vector v (A.cols()) ; @@ -279,24 +224,13 @@ Vector columnNormSquare(const Matrix &A) { return v ; } -/* ************************************************************************* */ -void solve(Matrix& A, Matrix& B) { - // Eigen version - untested - B = A.fullPivLu().solve(B); -} - -/* ************************************************************************* */ -Matrix inverse(const Matrix& A) { - return A.inverse(); -} - /* ************************************************************************* */ /** Householder QR factorization, Golub & Van Loan p 224, explicit version */ /* ************************************************************************* */ pair qr(const Matrix& A) { const size_t m = A.rows(), n = A.cols(), kprime = min(m,n); - Matrix Q=eye(m,m),R(A); + Matrix Q=Matrix::Identity(m,m),R(A); Vector v(m); // loop over the kprime first columns @@ -319,7 +253,7 @@ pair qr(const Matrix& A) { v(k) = k llt(A); - Matrix inv = eye(A.rows()); + Matrix inv = Matrix::Identity(A.rows(),A.rows()); llt.matrixU().solveInPlace(inv); return inv*inv.transpose(); } @@ -612,7 +546,7 @@ Matrix cholesky_inverse(const Matrix &A) // inv(B' * B) == A Matrix inverse_square_root(const Matrix& A) { Eigen::LLT llt(A); - Matrix inv = eye(A.rows()); + Matrix inv = Matrix::Identity(A.rows(),A.rows()); llt.matrixU().solveInPlace(inv); return inv.transpose(); } @@ -648,7 +582,7 @@ boost::tuple DLT(const Matrix& A, double rank_tol) { /* ************************************************************************* */ Matrix expm(const Matrix& A, size_t K) { - Matrix E = eye(A.rows()), A_k = eye(A.rows()); + Matrix E = Matrix::Identity(A.rows(),A.rows()), A_k = Matrix::Identity(A.rows(),A.rows()); for(size_t k=1;k<=K;k++) { A_k = A_k*A/double(k); E = E + A_k; diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index b0b292c56..84f201c51 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -33,7 +33,6 @@ #include #include - /** * Matrix is a typedef in the gtsam namespace * TODO: make a version to work with matlab wrapping @@ -74,38 +73,6 @@ GTSAM_MAKE_MATRIX_DEFS(9); typedef Eigen::Block SubMatrix; typedef Eigen::Block ConstSubMatrix; -// Matlab-like syntax - -/** - * Creates an zeros matrix, with matlab-like syntax - * - * Note: if assigning a block (created from an Eigen block() function) of a matrix to zeros, - * don't use this function, instead use ".setZero(m,n)" to avoid an Eigen error. - */ -GTSAM_EXPORT Matrix zeros(size_t m, size_t n); - -/** - * Creates an ones matrix, with matlab-like syntax - */ -GTSAM_EXPORT Matrix ones(size_t m, size_t n); - -/** - * Creates an identity matrix, with matlab-like syntax - * - * Note: if assigning a block (created from an Eigen block() function) of a matrix to identity, - * don't use this function, instead use ".setIdentity(m,n)" to avoid an Eigen error. - */ -GTSAM_EXPORT Matrix eye(size_t m, size_t n); - -/** - * Creates a square identity matrix, with matlab-like syntax - * - * Note: if assigning a block (created from an Eigen block() function) of a matrix to identity, - * don't use this function, instead use ".setIdentity(m)" to avoid an Eigen error. - */ -inline Matrix eye( size_t m ) { return eye(m,m); } -GTSAM_EXPORT Matrix diag(const Vector& v); - /** * equals with an tolerance */ @@ -292,8 +259,6 @@ const typename MATRIX::ConstRowXpr row(const MATRIX& A, size_t j) { GTSAM_EXPORT void insertColumn(Matrix& A, const Vector& col, size_t j); GTSAM_EXPORT void insertColumn(Matrix& A, const Vector& col, size_t i, size_t j); -GTSAM_EXPORT Vector columnNormSquare(const Matrix &A); - /** * Zeros all of the elements below the diagonal of a matrix, in place * @param A is a matrix, to be modified in place @@ -492,12 +457,6 @@ inline Matrix3 skewSymmetric(const Eigen::MatrixBase& w) { /** Use Cholesky to calculate inverse square root of a matrix */ GTSAM_EXPORT Matrix inverse_square_root(const Matrix& A); -/** Calculate the LL^t decomposition of a S.P.D matrix */ -GTSAM_EXPORT Matrix LLt(const Matrix& A); - -/** Calculate the R^tR decomposition of a S.P.D matrix */ -GTSAM_EXPORT Matrix RtR(const Matrix& A); - /** Return the inverse of a S.P.D. matrix. Inversion is done via Cholesky decomposition. */ GTSAM_EXPORT Matrix cholesky_inverse(const Matrix &A); @@ -603,6 +562,28 @@ struct MultiplyWithInverseFunction { const Operator phi_; }; +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 +inline Matrix zeros( size_t m, size_t n ) { return Matrix::Zero(m,n); } +inline Matrix ones( size_t m, size_t n ) { return Matrix::Ones(m,n); } +inline Matrix eye( size_t m, size_t n) { return Matrix::Identity(m, n); } +inline Matrix eye( size_t m ) { return eye(m,m); } +inline Matrix diag(const Vector& v) { return v.asDiagonal(); } +inline void multiplyAdd(double alpha, const Matrix& A, const Vector& x, Vector& e) { e += alpha * A * x; } +inline void multiplyAdd(const Matrix& A, const Vector& x, Vector& e) { e += A * x; } +inline void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, Vector& x) { x += alpha * A.transpose() * e; } +inline void transposeMultiplyAdd(const Matrix& A, const Vector& e, Vector& x) { x += A.transpose() * e; } +inline void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, SubVector x) { x += alpha * A.transpose() * e; } +inline void insertColumn(Matrix& A, const Vector& col, size_t j) { A.col(j) = col; } +inline void insertColumn(Matrix& A, const Vector& col, size_t i, size_t j) { A.col(j).segment(i, col.size()) = col; } +inline void solve(Matrix& A, Matrix& B) { B = A.fullPivLu().solve(B); } +inline Matrix inverse(const Matrix& A) { return A.inverse(); } +#endif + +GTSAM_EXPORT Matrix LLt(const Matrix& A); + +GTSAM_EXPORT Matrix RtR(const Matrix& A); + +GTSAM_EXPORT Vector columnNormSquare(const Matrix &A); } // namespace gtsam #include diff --git a/gtsam/geometry/OrientedPlane3.cpp b/gtsam/geometry/OrientedPlane3.cpp index aa023a09a..514328483 100644 --- a/gtsam/geometry/OrientedPlane3.cpp +++ b/gtsam/geometry/OrientedPlane3.cpp @@ -42,7 +42,7 @@ OrientedPlane3 OrientedPlane3::transform(const Pose3& xr, OptionalJacobian<3, 3> double pred_d = n_.unitVector().dot(xr.translation()) + d_; if (Hr) { - *Hr = zeros(3, 6); + *Hr = Matrix::Zero(3, 6); Hr->block<2, 3>(0, 0) = D_rotated_plane; Hr->block<1, 3>(2, 3) = unit_vec; } diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index 1aa8f060a..383c94dd1 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -132,7 +132,7 @@ Matrix3 Pose2::AdjointMap() const { /* ************************************************************************* */ Matrix3 Pose2::adjointMap(const Vector3& v) { // See Chirikjian12book2, vol.2, pg. 36 - Matrix3 ad = zeros(3,3); + Matrix3 ad = Matrix::Zero(3,3); ad(0,1) = -v[2]; ad(1,0) = v[2]; ad(0,2) = v[1]; diff --git a/gtsam/geometry/triangulation.cpp b/gtsam/geometry/triangulation.cpp index 7ec9edbb3..a1dc3269a 100644 --- a/gtsam/geometry/triangulation.cpp +++ b/gtsam/geometry/triangulation.cpp @@ -31,7 +31,7 @@ Vector4 triangulateHomogeneousDLT( size_t m = projection_matrices.size(); // Allocate DLT matrix - Matrix A = zeros(m * 2, 4); + Matrix A = Matrix::Zero(m * 2, 4); for (size_t i = 0; i < m; i++) { size_t row = i * 2; diff --git a/gtsam/linear/KalmanFilter.h b/gtsam/linear/KalmanFilter.h index 63271401c..7db29d861 100644 --- a/gtsam/linear/KalmanFilter.h +++ b/gtsam/linear/KalmanFilter.h @@ -69,7 +69,7 @@ public: // Constructor KalmanFilter(size_t n, Factorization method = KALMANFILTER_DEFAULT_FACTORIZATION) : - n_(n), I_(eye(n_, n_)), function_( + n_(n), I_(Matrix::Identity(n_, n_)), function_( method == QR ? GaussianFactorGraph::Eliminate(EliminateQR) : GaussianFactorGraph::Eliminate(EliminateCholesky)) { } diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 15fad5a78..d0c7a58db 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -341,7 +341,7 @@ namespace gtsam { * Return R itself, but note that Whiten(H) is cheaper than R*H */ virtual Matrix R() const { - return diag(invsigmas()); + return invsigmas().asDiagonal(); } private: diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp index b93fe1fc1..7d7a86405 100644 --- a/gtsam/slam/InitializePose3.cpp +++ b/gtsam/slam/InitializePose3.cpp @@ -32,7 +32,7 @@ namespace gtsam { namespace InitializePose3 { //static const Matrix I = eye(1); -static const Matrix I9 = eye(9); +static const Matrix I9 = Matrix::Identity(9,9); static const Vector zero9 = Vector::Zero(9); static const Matrix zero33= Matrix::Zero(3,3); diff --git a/gtsam/slam/PriorFactor.h b/gtsam/slam/PriorFactor.h index 8305fce12..9a3a4a47a 100644 --- a/gtsam/slam/PriorFactor.h +++ b/gtsam/slam/PriorFactor.h @@ -86,7 +86,7 @@ namespace gtsam { /** vector of errors */ Vector evaluateError(const T& x, boost::optional H = boost::none) const { - if (H) (*H) = eye(traits::GetDimension(x)); + if (H) (*H) = Matrix::Identity(traits::GetDimension(x),traits::GetDimension(x)); // manifold equivalent of z-x -> Local(x,z) // TODO(ASL) Add Jacobians. return -traits::Local(x, prior_); diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 14192fcaa..c1c919a58 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -472,7 +472,7 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate, << p.x() << " " << p.y() << " " << p.z() << " " << R.toQuaternion().x() << " " << R.toQuaternion().y() << " " << R.toQuaternion().z() << " " << R.toQuaternion().w(); - Matrix InfoG2o = eye(6); + Matrix InfoG2o = Matrix::Identity(6,6); InfoG2o.block(0,0,3,3) = Info.block(3,3,3,3); // cov translation InfoG2o.block(3,3,3,3) = Info.block(0,0,3,3); // cov rotation InfoG2o.block(0,3,3,3) = Info.block(0,3,3,3); // off diagonal @@ -539,7 +539,7 @@ GraphAndValues load3D(const string& filename) { ls >> id1 >> id2 >> x >> y >> z >> roll >> pitch >> yaw; Rot3 R = Rot3::Ypr(yaw,pitch,roll); Point3 t = Point3(x, y, z); - Matrix m = eye(6); + Matrix m = Matrix::Identity(6,6); for (int i = 0; i < 6; i++) for (int j = i; j < 6; j++) ls >> m(i, j); @@ -549,7 +549,7 @@ GraphAndValues load3D(const string& filename) { graph->push_back(factor); } if (tag == "EDGE_SE3:QUAT") { - Matrix m = eye(6); + Matrix m = Matrix::Identity(6,6); Key id1, id2; double x, y, z, qx, qy, qz, qw; ls >> id1 >> id2 >> x >> y >> z >> qx >> qy >> qz >> qw; @@ -563,7 +563,7 @@ GraphAndValues load3D(const string& filename) { m(j, i) = mij; } } - Matrix mgtsam = eye(6); + Matrix mgtsam = Matrix::Identity(6,6); mgtsam.block(0,0,3,3) = m.block(3,3,3,3); // cov rotation mgtsam.block(3,3,3,3) = m.block(0,0,3,3); // cov translation mgtsam.block(0,3,3,3) = m.block(0,3,3,3); // off diagonal diff --git a/gtsam/slam/lago.cpp b/gtsam/slam/lago.cpp index e46b8ee71..a29d3e8b1 100644 --- a/gtsam/slam/lago.cpp +++ b/gtsam/slam/lago.cpp @@ -30,8 +30,8 @@ using namespace std; namespace gtsam { namespace lago { -static const Matrix I = eye(1); -static const Matrix I3 = eye(3); +static const Matrix I = Matrix::Identity(1,1); +static const Matrix I3 = Matrix::Identity(3,3); static const Key keyAnchor = symbol('Z', 9999999); static const noiseModel::Diagonal::shared_ptr priorOrientationNoise = From 70b2aab35209bbd4d8957a5be91292474ef27c09 Mon Sep 17 00:00:00 2001 From: Alex Hagiopol Date: Mon, 11 Apr 2016 15:11:29 -0400 Subject: [PATCH 67/91] Deprecated all inline functions in Matrix.h. --- CMakeLists.txt | 2 +- gtsam/base/Matrix.h | 49 +---------------- gtsam/base/numericalDerivative.h | 2 +- gtsam/base/tests/testMatrix.cpp | 52 +++++++++---------- gtsam/base/tests/testSymmetricBlockMatrix.cpp | 2 +- gtsam/geometry/OrientedPlane3.cpp | 2 +- gtsam/geometry/Pose2.cpp | 2 +- gtsam/geometry/tests/testCal3_S2.cpp | 4 +- gtsam/geometry/tests/testPinholeCamera.cpp | 2 +- gtsam/geometry/tests/testPinholePose.cpp | 2 +- gtsam/geometry/tests/testPoint2.cpp | 8 +-- gtsam/geometry/tests/testPoint3.cpp | 8 +-- gtsam/geometry/tests/testPose2.cpp | 4 +- gtsam/geometry/tests/testPose3.cpp | 8 +-- gtsam/geometry/tests/testRot2.cpp | 8 +-- gtsam/geometry/tests/testRot3.cpp | 4 +- gtsam/geometry/tests/testSimpleCamera.cpp | 2 +- gtsam/linear/GaussianConditional.cpp | 2 +- gtsam/linear/JacobianFactor.cpp | 3 +- gtsam/linear/KalmanFilter.cpp | 4 +- gtsam/linear/iterative.h | 2 +- gtsam/linear/tests/testGaussianBayesTree.cpp | 4 +- .../linear/tests/testGaussianFactorGraph.cpp | 20 +++---- gtsam/linear/tests/testHessianFactor.cpp | 6 +-- gtsam/linear/tests/testJacobianFactor.cpp | 34 ++++++------ gtsam/linear/tests/testKalmanFilter.cpp | 30 +++++------ gtsam/linear/tests/testNoiseModel.cpp | 16 +++--- .../linear/tests/testSerializationLinear.cpp | 8 +-- gtsam/navigation/AttitudeFactor.h | 2 +- gtsam/navigation/MagFactor.h | 4 +- gtsam/navigation/PreintegrationBase.cpp | 2 +- gtsam/nonlinear/NonlinearEquality.h | 10 ++-- gtsam/nonlinear/tests/testExpression.cpp | 6 +-- .../tests/testLinearContainerFactor.cpp | 2 +- gtsam/slam/GeneralSFMFactor.h | 6 +-- gtsam/slam/InitializePose3.cpp | 11 ++-- gtsam/slam/JacobianFactorQ.h | 2 +- gtsam/slam/OrientedPlane3Factor.cpp | 2 +- gtsam/slam/PoseRotationPrior.h | 2 +- gtsam/slam/PoseTranslationPrior.h | 2 +- gtsam/slam/ProjectionFactor.h | 4 +- gtsam/slam/ReferenceFrameFactor.h | 2 +- gtsam/slam/SmartProjectionFactor.h | 2 +- gtsam/slam/StereoFactor.h | 4 +- gtsam/slam/TriangulationFactor.h | 2 +- gtsam/slam/dataset.cpp | 8 +-- gtsam/slam/lago.cpp | 4 +- .../tests/testRegularImplicitSchurFactor.cpp | 18 +++---- gtsam_unstable/dynamics/DynamicsPriors.h | 6 +-- gtsam_unstable/dynamics/Pendulum.h | 24 ++++----- gtsam_unstable/dynamics/SimpleHelicopter.h | 2 +- gtsam_unstable/dynamics/VelocityConstraint3.h | 6 +-- gtsam_unstable/dynamics/tests/testPoseRTV.cpp | 8 +-- .../dynamics/tests/testSimpleHelicopter.cpp | 4 +- gtsam_unstable/geometry/InvDepthCamera3.h | 2 +- gtsam_unstable/geometry/Pose3Upright.cpp | 10 ++-- .../linear/tests/testLinearEquality.cpp | 2 +- gtsam_unstable/linear/tests/testQPSolver.cpp | 34 ++++++------ .../nonlinear/tests/testParticleFactor.cpp | 22 ++++---- gtsam_unstable/slam/AHRS.cpp | 12 ++--- gtsam_unstable/slam/DummyFactor.cpp | 2 +- .../slam/EquivInertialNavFactor_GlobalVel.h | 18 +++---- .../EquivInertialNavFactor_GlobalVel_NoBias.h | 4 +- .../slam/GaussMarkov1stOrderFactor.h | 4 +- .../slam/InertialNavFactor_GlobalVelocity.h | 6 +-- gtsam_unstable/slam/InvDepthFactor3.h | 6 +-- gtsam_unstable/slam/MultiProjectionFactor.h | 4 +- gtsam_unstable/slam/PartialPriorFactor.h | 4 +- gtsam_unstable/slam/ProjectionFactorPPP.h | 6 +-- gtsam_unstable/slam/ProjectionFactorPPPC.h | 8 +-- .../slam/RelativeElevationFactor.cpp | 4 +- gtsam_unstable/slam/SmartRangeFactor.h | 2 +- .../slam/SmartStereoProjectionFactor.h | 2 +- gtsam_unstable/slam/tests/testDummyFactor.cpp | 2 +- .../testEquivInertialNavFactor_GlobalVel.cpp | 4 +- .../slam/tests/testSmartRangeFactor.cpp | 2 +- .../testing_tools/base/cholScalingTest.m | 4 +- matlab/gtsam_examples/IMUKittiExampleGPS.m | 2 +- matlab/gtsam_examples/SBAExample.m | 2 +- .../FlightCameraTransformIMU.m | 2 +- .../IMUKittiExampleAdvanced.m | 2 +- matlab/unstable_examples/IMUKittiExampleVO.m | 2 +- ...ansformCalProjectionFactorIMUExampleISAM.m | 2 +- tests/simulated2D.h | 10 ++-- tests/simulated2DConstraints.h | 2 +- tests/simulated2DOriented.h | 2 +- tests/simulated3D.h | 10 ++-- tests/smallExample.h | 20 +++---- tests/testExpressionFactor.cpp | 30 +++++------ tests/testExtendedKalmanFilter.cpp | 2 +- tests/testGaussianBayesTreeB.cpp | 8 +-- tests/testGaussianFactorGraphB.cpp | 24 ++++----- tests/testGraph.cpp | 2 +- tests/testNonlinearEquality.cpp | 12 ++--- timing/timeMatrix.cpp | 6 +-- timing/timePose2.cpp | 2 +- timing/timeRot2.cpp | 8 +-- 97 files changed, 348 insertions(+), 401 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 8c4229ed5..16fa5d4c5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -242,7 +242,7 @@ else() endif() # 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/") + include_directories(BEFORE SYSTEM "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 diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index 84f201c51..0c7a3f9bd 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -74,7 +74,7 @@ typedef Eigen::Block SubMatrix; typedef Eigen::Block ConstSubMatrix; /** - * equals with an tolerance + * equals with a tolerance */ template bool equal_with_abs_tol(const Eigen::DenseBase& A, const Eigen::DenseBase& B, double tol = 1e-9) { @@ -133,37 +133,12 @@ GTSAM_EXPORT bool linear_independent(const Matrix& A, const Matrix& B, double to */ GTSAM_EXPORT bool linear_dependent(const Matrix& A, const Matrix& B, double tol = 1e-9); -/** - * BLAS Level-2 style e <- e + alpha*A*x - */ -GTSAM_EXPORT void multiplyAdd(double alpha, const Matrix& A, const Vector& x, Vector& e); - -/** - * BLAS Level-2 style e <- e + A*x - */ -GTSAM_EXPORT void multiplyAdd(const Matrix& A, const Vector& x, Vector& e); - /** * overload ^ for trans(A)*v * We transpose the vectors for speed. */ GTSAM_EXPORT Vector operator^(const Matrix& A, const Vector & v); -/** - * BLAS Level-2 style x <- x + alpha*A'*e - */ -GTSAM_EXPORT void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, Vector& x); - -/** - * BLAS Level-2 style x <- x + A'*e - */ -GTSAM_EXPORT void transposeMultiplyAdd(const Matrix& A, const Vector& e, Vector& x); - -/** - * BLAS Level-2 style x <- x + alpha*A'*e - */ -GTSAM_EXPORT void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, SubVector x); - /** products using old-style format to improve compatibility */ template inline MATRIX prod(const MATRIX& A, const MATRIX&B) { @@ -248,17 +223,6 @@ const typename MATRIX::ConstRowXpr row(const MATRIX& A, size_t j) { return A.row(j); } -/** - * inserts a column into a matrix IN PLACE - * NOTE: there is no size checking - * Alternate form allows for vectors smaller than the whole column to be inserted - * @param A matrix to be modified in place - * @param col is the vector to be inserted - * @param j is the index to insert the column - */ -GTSAM_EXPORT void insertColumn(Matrix& A, const Vector& col, size_t j); -GTSAM_EXPORT void insertColumn(Matrix& A, const Vector& col, size_t i, size_t j); - /** * Zeros all of the elements below the diagonal of a matrix, in place * @param A is a matrix, to be modified in place @@ -320,17 +284,6 @@ inline typename Reshape::ReshapedTy return Reshape::reshape(m); } -/** - * solve AX=B via in-place Lu factorization and backsubstitution - * After calling, A contains LU, B the solved RHS vectors - */ -GTSAM_EXPORT void solve(Matrix& A, Matrix& B); - -/** - * invert A - */ -GTSAM_EXPORT Matrix inverse(const Matrix& A); - /** * QR factorization, inefficient, best use imperative householder below * m*n matrix -> m*m Q, m*n R diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h index 9a42db3ce..1bd62c257 100644 --- a/gtsam/base/numericalDerivative.h +++ b/gtsam/base/numericalDerivative.h @@ -142,7 +142,7 @@ typename internal::FixedSizeMatrix::type numericalDerivative11(boost::funct dx.setZero(); // Fill in Jacobian H - Matrix H = zeros(m, N); + Matrix H = Matrix::Zero(m, N); const double factor = 1.0 / (2.0 * delta); for (int j = 0; j < N; j++) { dx(j) = delta; diff --git a/gtsam/base/tests/testMatrix.cpp b/gtsam/base/tests/testMatrix.cpp index 13d32794e..b2d512e4a 100644 --- a/gtsam/base/tests/testMatrix.cpp +++ b/gtsam/base/tests/testMatrix.cpp @@ -156,8 +156,8 @@ TEST(Matrix, collect2 ) TEST(Matrix, collect3 ) { Matrix A, B; - A = eye(2, 3); - B = eye(2, 3); + A = Matrix::Identity(2,3); + B = Matrix::Identity(2,3); vector matrices; matrices.push_back(&A); matrices.push_back(&B); @@ -214,11 +214,11 @@ TEST(Matrix, column ) /* ************************************************************************* */ TEST(Matrix, insert_column ) { - Matrix big = zeros(5, 6); + Matrix big = Matrix::Zero(5,6); Vector col = ones(5); size_t j = 3; - insertColumn(big, col, j); + big.col(j) = col; Matrix expected = (Matrix(5, 6) << 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, @@ -233,15 +233,15 @@ TEST(Matrix, insert_column ) /* ************************************************************************* */ TEST(Matrix, insert_subcolumn ) { - Matrix big = zeros(5, 6); + Matrix big = Matrix::Zero(5,6); Vector col1 = ones(2); size_t i = 1; size_t j = 3; - insertColumn(big, col1, i, j); // check 1 + big.col(j).segment(i,col1.size()) = col1; Vector col2 = ones(1); - insertColumn(big, col2, 4, 5); // check 2 + big.col(5).segment(4,col2.size()) = col2; Matrix expected = (Matrix(5, 6) << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, @@ -283,7 +283,7 @@ TEST(Matrix, zeros ) A(1, 1) = 0; A(1, 2) = 0; - Matrix zero = zeros(2, 3); + Matrix zero = Matrix::Zero(2,3); EQUALITY(A , zero); } @@ -291,7 +291,7 @@ TEST(Matrix, zeros ) /* ************************************************************************* */ TEST(Matrix, insert_sub ) { - Matrix big = zeros(5, 6), small = (Matrix(2, 3) << 1.0, 1.0, 1.0, 1.0, 1.0, + Matrix big = Matrix::Zero(5,6), small = (Matrix(2, 3) << 1.0, 1.0, 1.0, 1.0, 1.0, 1.0).finished(); insertSub(big, small, 1, 2); @@ -307,9 +307,9 @@ TEST(Matrix, insert_sub ) TEST(Matrix, diagMatrices ) { std::vector Hs; - Hs.push_back(ones(3,3)); - Hs.push_back(ones(4,4)*2); - Hs.push_back(ones(2,2)*3); + Hs.push_back(Matrix::Ones(3,3)); + Hs.push_back(Matrix::Ones(4,4)*2); + Hs.push_back(Matrix::Ones(2,2)*3); Matrix actual = diag(Hs); @@ -723,9 +723,9 @@ TEST(Matrix, inverse ) A(2, 1) = 0; A(2, 2) = 6; - Matrix Ainv = inverse(A); - EXPECT(assert_equal(eye(3), A*Ainv)); - EXPECT(assert_equal(eye(3), Ainv*A)); + Matrix Ainv = A.inverse(); + EXPECT(assert_equal((Matrix) I_3x3, A*Ainv)); + EXPECT(assert_equal((Matrix) I_3x3, Ainv*A)); Matrix expected(3, 3); expected(0, 0) = 1.0909; @@ -746,13 +746,13 @@ TEST(Matrix, inverse ) 0.0, -1.0, 1.0, 1.0, 0.0, 2.0, 0.0, 0.0, 1.0).finished(), - inverse(lMg))); + lMg.inverse())); Matrix gMl((Matrix(3, 3) << 0.0, -1.0, 1.0, 1.0, 0.0, 2.0, 0.0, 0.0, 1.0).finished()); EXPECT(assert_equal((Matrix(3, 3) << 0.0, 1.0,-2.0, -1.0, 0.0, 1.0, 0.0, 0.0, 1.0).finished(), - inverse(gMl))); + gMl.inverse())); } /* ************************************************************************* */ @@ -769,7 +769,7 @@ TEST(Matrix, inverse2 ) A(2, 1) = 0; A(2, 2) = 1; - Matrix Ainv = inverse(A); + Matrix Ainv = A.inverse(); Matrix expected(3, 3); expected(0, 0) = 0; @@ -996,7 +996,7 @@ TEST(Matrix, inverse_square_root ) 10.0).finished(); EQUALITY(expected,actual); - EQUALITY(measurement_covariance,inverse(actual*actual)); + EQUALITY(measurement_covariance,(actual*actual).inverse()); // Randomly generated test. This test really requires inverse to // be working well; if it's not, there's the possibility of a @@ -1058,8 +1058,7 @@ TEST(Matrix, multiplyAdd ) Matrix A = (Matrix(3, 4) << 4., 0., 0., 1., 0., 4., 0., 2., 0., 0., 1., 3.).finished(); Vector x = (Vector(4) << 1., 2., 3., 4.).finished(), e = Vector3(5., 6., 7.), expected = e + A * x; - - multiplyAdd(1, A, x, e); + e += A * x; EXPECT(assert_equal(expected, e)); } @@ -1069,8 +1068,7 @@ TEST(Matrix, transposeMultiplyAdd ) Matrix A = (Matrix(3, 4) << 4., 0., 0., 1., 0., 4., 0., 2., 0., 0., 1., 3.).finished(); Vector x = (Vector(4) << 1., 2., 3., 4.).finished(), e = Vector3(5., 6., 7.), expected = x + trans(A) * e; - - transposeMultiplyAdd(1, A, e, x); + x += A.transpose() * e; EXPECT(assert_equal(expected, x)); } @@ -1102,12 +1100,12 @@ TEST(Matrix, linear_dependent3 ) TEST(Matrix, svd1 ) { Vector v = Vector3(2., 1., 0.); - Matrix U1 = eye(4, 3), S1 = diag(v), V1 = eye(3, 3), A = (U1 * S1) + Matrix U1 = Matrix::Identity(4, 3), S1 = v.asDiagonal(), V1 = I_3x3, A = (U1 * S1) * Matrix(trans(V1)); Matrix U, V; Vector s; svd(A, U, s, V); - Matrix S = diag(s); + Matrix S = s.asDiagonal(); EXPECT(assert_equal(U*S*Matrix(trans(V)),A)); EXPECT(assert_equal(S,S1)); } @@ -1158,7 +1156,7 @@ TEST(Matrix, svd3 ) V = -V; } - Matrix S = diag(s); + Matrix S = s.asDiagonal(); Matrix t = U * S; Matrix Vt = trans(V); @@ -1202,7 +1200,7 @@ TEST(Matrix, svd4 ) V.col(1) = -V.col(1); } - Matrix reconstructed = U * diag(s) * trans(V); + Matrix reconstructed = U * s.asDiagonal() * trans(V); EXPECT(assert_equal(A, reconstructed, 1e-4)); EXPECT(assert_equal(expectedU,U, 1e-3)); diff --git a/gtsam/base/tests/testSymmetricBlockMatrix.cpp b/gtsam/base/tests/testSymmetricBlockMatrix.cpp index df3403cf4..da193aec5 100644 --- a/gtsam/base/tests/testSymmetricBlockMatrix.cpp +++ b/gtsam/base/tests/testSymmetricBlockMatrix.cpp @@ -42,7 +42,7 @@ TEST(SymmetricBlockMatrix, ReadBlocks) 23, 29).finished(); Matrix actual1 = testBlockMatrix(1, 1); // Test only writing the upper triangle for efficiency - Matrix actual1t = Matrix::Zero(2, 2); + Matrix actual1t = Z_2x2; actual1t.triangularView() = testBlockMatrix(1, 1).triangularView(); EXPECT(assert_equal(expected1, actual1)); EXPECT(assert_equal(Matrix(expected1.triangularView()), actual1t)); diff --git a/gtsam/geometry/OrientedPlane3.cpp b/gtsam/geometry/OrientedPlane3.cpp index 514328483..4af0257ec 100644 --- a/gtsam/geometry/OrientedPlane3.cpp +++ b/gtsam/geometry/OrientedPlane3.cpp @@ -42,7 +42,7 @@ OrientedPlane3 OrientedPlane3::transform(const Pose3& xr, OptionalJacobian<3, 3> double pred_d = n_.unitVector().dot(xr.translation()) + d_; if (Hr) { - *Hr = Matrix::Zero(3, 6); + *Hr = Matrix::Zero(3,6); Hr->block<2, 3>(0, 0) = D_rotated_plane; Hr->block<1, 3>(2, 3) = unit_vec; } diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index 383c94dd1..8a0e8fbf5 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -132,7 +132,7 @@ Matrix3 Pose2::AdjointMap() const { /* ************************************************************************* */ Matrix3 Pose2::adjointMap(const Vector3& v) { // See Chirikjian12book2, vol.2, pg. 36 - Matrix3 ad = Matrix::Zero(3,3); + Matrix3 ad = Z_3x3; ad(0,1) = -v[2]; ad(1,0) = v[2]; ad(0,2) = v[1]; diff --git a/gtsam/geometry/tests/testCal3_S2.cpp b/gtsam/geometry/tests/testCal3_S2.cpp index 3e93dedc1..4ccb6075b 100644 --- a/gtsam/geometry/tests/testCal3_S2.cpp +++ b/gtsam/geometry/tests/testCal3_S2.cpp @@ -122,8 +122,8 @@ TEST(Cal3_S2, between) { Matrix H1, H2; EXPECT(assert_equal(Cal3_S2(0,1,2,3,4), k1.between(k2, H1, H2))); - EXPECT(assert_equal(-eye(5), H1)); - EXPECT(assert_equal(eye(5), H2)); + EXPECT(assert_equal(-I_5x5, H1)); + EXPECT(assert_equal(I_5x5, H2)); } diff --git a/gtsam/geometry/tests/testPinholeCamera.cpp b/gtsam/geometry/tests/testPinholeCamera.cpp index 4fbdbcbe1..99dcb95bf 100644 --- a/gtsam/geometry/tests/testPinholeCamera.cpp +++ b/gtsam/geometry/tests/testPinholeCamera.cpp @@ -116,7 +116,7 @@ TEST( PinholeCamera, lookat) Matrix R = camera2.pose().rotation().matrix(); Matrix I = trans(R)*R; - EXPECT(assert_equal(I, eye(3))); + EXPECT(assert_equal(I, I_3x3)); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testPinholePose.cpp b/gtsam/geometry/tests/testPinholePose.cpp index c5137e3b3..0d840de7e 100644 --- a/gtsam/geometry/tests/testPinholePose.cpp +++ b/gtsam/geometry/tests/testPinholePose.cpp @@ -87,7 +87,7 @@ TEST( PinholePose, lookat) Matrix R = camera2.pose().rotation().matrix(); Matrix I = trans(R)*R; - EXPECT(assert_equal(I, eye(3))); + EXPECT(assert_equal(I, I_3x3)); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testPoint2.cpp b/gtsam/geometry/tests/testPoint2.cpp index 6f8d45b3e..3a636b9bf 100644 --- a/gtsam/geometry/tests/testPoint2.cpp +++ b/gtsam/geometry/tests/testPoint2.cpp @@ -73,12 +73,12 @@ TEST(Point2, Lie) { Matrix H1, H2; EXPECT(assert_equal(Point2(5,7), traits::Compose(p1, p2, H1, H2))); - EXPECT(assert_equal(eye(2), H1)); - EXPECT(assert_equal(eye(2), H2)); + EXPECT(assert_equal(I_2x2, H1)); + EXPECT(assert_equal(I_2x2, H2)); EXPECT(assert_equal(Point2(3,3), traits::Between(p1, p2, H1, H2))); - EXPECT(assert_equal(-eye(2), H1)); - EXPECT(assert_equal(eye(2), H2)); + EXPECT(assert_equal(-I_2x2, H1)); + EXPECT(assert_equal(I_2x2, H2)); EXPECT(assert_equal(Point2(5,7), traits::Retract(p1, Vector2(4., 5.)))); EXPECT(assert_equal(Vector2(3.,3.), traits::Local(p1,p2))); diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp index da7696d0d..9b6e53323 100644 --- a/gtsam/geometry/tests/testPoint3.cpp +++ b/gtsam/geometry/tests/testPoint3.cpp @@ -47,12 +47,12 @@ TEST(Point3, Lie) { Matrix H1, H2; EXPECT(assert_equal(Point3(5, 7, 9), traits::Compose(p1, p2, H1, H2))); - EXPECT(assert_equal(eye(3), H1)); - EXPECT(assert_equal(eye(3), H2)); + EXPECT(assert_equal(I_3x3, H1)); + EXPECT(assert_equal(I_3x3, H2)); EXPECT(assert_equal(Point3(3, 3, 3), traits::Between(p1, p2, H1, H2))); - EXPECT(assert_equal(-eye(3), H1)); - EXPECT(assert_equal(eye(3), H2)); + EXPECT(assert_equal(-I_3x3, H1)); + EXPECT(assert_equal(I_3x3, H2)); EXPECT(assert_equal(Point3(5, 7, 9), traits::Retract(p1, Vector3(4,5,6)))); EXPECT(assert_equal(Vector3(3, 3, 3), traits::Local(p1,p2))); diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index 5b835200a..d1f68fe28 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -102,7 +102,7 @@ TEST(Pose2, expmap3) { 0.99, 0.0, -0.015, 0.0, 0.0, 0.0).finished(); Matrix A2 = A*A/2.0, A3 = A2*A/3.0, A4=A3*A/4.0; - Matrix expected = eye(3) + A + A2 + A3 + A4; + Matrix expected = I_3x3 + A + A2 + A3 + A4; Vector v = Vector3(0.01, -0.015, 0.99); Pose2 pose = Pose2::Expmap(v); @@ -311,7 +311,7 @@ TEST(Pose2, compose_a) -1.0, 0.0, 2.0, 0.0, 0.0, 1.0 ).finished(); - Matrix expectedH2 = eye(3); + Matrix expectedH2 = I_3x3; Matrix numericalH1 = numericalDerivative21(testing::compose, pose1, pose2); Matrix numericalH2 = numericalDerivative22(testing::compose, pose1, pose2); EXPECT(assert_equal(expectedH1,actualDcompose1)); diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 648197ced..957c7621a 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -153,7 +153,7 @@ Pose3 Agrawal06iros(const Vector& xi) { return Pose3(Rot3(), Point3(v)); else { Matrix W = skewSymmetric(w/t); - Matrix A = eye(3) + ((1 - cos(t)) / t) * W + ((t - sin(t)) / t) * (W * W); + Matrix A = I_3x3 + ((1 - cos(t)) / t) * W + ((t - sin(t)) / t) * (W * W); return Pose3(Rot3::Expmap (w), Point3(A * v)); } } @@ -267,7 +267,7 @@ TEST( Pose3, inverse) { Matrix actualDinverse; Matrix actual = T.inverse(actualDinverse).matrix(); - Matrix expected = inverse(T.matrix()); + Matrix expected = T.matrix().inverse(); EXPECT(assert_equal(actual,expected,1e-8)); Matrix numericalH = numericalDerivative11(testing::inverse, T); @@ -293,7 +293,7 @@ TEST( Pose3, inverseDerivatives2) TEST( Pose3, compose_inverse) { Matrix actual = (T*T.inverse()).matrix(); - Matrix expected = eye(4,4); + Matrix expected = I_4x4; EXPECT(assert_equal(actual,expected,1e-8)); } @@ -723,7 +723,7 @@ TEST( Pose3, adjointMap) { Matrix res = Pose3::adjointMap(screwPose3::xi); Matrix wh = skewSymmetric(screwPose3::xi(0), screwPose3::xi(1), screwPose3::xi(2)); Matrix vh = skewSymmetric(screwPose3::xi(3), screwPose3::xi(4), screwPose3::xi(5)); - Matrix Z3 = zeros(3,3); + Matrix Z3 = Z_3x3; Matrix6 expected; expected << wh, Z3, vh, wh; EXPECT(assert_equal(expected,res,1e-5)); diff --git a/gtsam/geometry/tests/testRot2.cpp b/gtsam/geometry/tests/testRot2.cpp index 6ead22860..4394103c9 100644 --- a/gtsam/geometry/tests/testRot2.cpp +++ b/gtsam/geometry/tests/testRot2.cpp @@ -62,8 +62,8 @@ TEST( Rot2, compose) Matrix H1, H2; (void) Rot2::fromAngle(1.0).compose(Rot2::fromAngle(2.0), H1, H2); - EXPECT(assert_equal(eye(1), H1)); - EXPECT(assert_equal(eye(1), H2)); + EXPECT(assert_equal(I_1x1, H1)); + EXPECT(assert_equal(I_1x1, H2)); } /* ************************************************************************* */ @@ -74,8 +74,8 @@ TEST( Rot2, between) Matrix H1, H2; (void) Rot2::fromAngle(1.0).between(Rot2::fromAngle(2.0), H1, H2); - EXPECT(assert_equal(-eye(1), H1)); - EXPECT(assert_equal(eye(1), H2)); + EXPECT(assert_equal(-I_1x1, H1)); + EXPECT(assert_equal(I_1x1, H2)); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 876897ab3..0c78cc426 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -555,8 +555,8 @@ TEST(Rot3, quaternion) { /* ************************************************************************* */ Matrix Cayley(const Matrix& A) { Matrix::Index n = A.cols(); - const Matrix I = eye(n); - return (I-A)*inverse(I+A); + const Matrix I = Matrix::Identity(n,n); + return (I-A)*(I+A).inverse(); } TEST( Rot3, Cayley ) { diff --git a/gtsam/geometry/tests/testSimpleCamera.cpp b/gtsam/geometry/tests/testSimpleCamera.cpp index 71dcabd4e..70b3069f2 100644 --- a/gtsam/geometry/tests/testSimpleCamera.cpp +++ b/gtsam/geometry/tests/testSimpleCamera.cpp @@ -76,7 +76,7 @@ TEST( SimpleCamera, lookat) Matrix R = camera2.pose().rotation().matrix(); Matrix I = trans(R)*R; - CHECK(assert_equal(I, eye(3))); + CHECK(assert_equal(I, I_3x3)); } /* ************************************************************************* */ diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index feb477a4e..eefb6302f 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -183,7 +183,7 @@ namespace gtsam { if (frontalVec.hasNaN()) throw IndeterminantLinearSystemException(this->keys().front()); for (const_iterator it = beginParents(); it!= endParents(); it++) - gtsam::transposeMultiplyAdd(-1.0, Matrix(getA(it)), frontalVec, gy[*it]); + gy[*it] += -1.0 * Matrix(getA(it)).transpose() * frontalVec; // Scale by sigmas if(model_) diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index fc1a7c841..4bc141798 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -565,8 +565,7 @@ void JacobianFactor::transposeMultiplyAdd(double alpha, const Vector& e, pair xi = x.tryInsert(j, Vector()); if (xi.second) xi.first->second = Vector::Zero(getDim(begin() + pos)); - gtsam::transposeMultiplyAdd(Ab_(pos), E, xi.first->second); - + xi.first->second += Ab_(pos).transpose()*E; } } diff --git a/gtsam/linear/KalmanFilter.cpp b/gtsam/linear/KalmanFilter.cpp index 68e6a00f1..c0d294adf 100644 --- a/gtsam/linear/KalmanFilter.cpp +++ b/gtsam/linear/KalmanFilter.cpp @@ -115,7 +115,7 @@ KalmanFilter::State KalmanFilter::predictQ(const State& p, const Matrix& F, // f2(x_{t},x_{t+1}) = (F*x_{t} + B*u - x_{t+1}) * Q^-1 * (F*x_{t} + B*u - x_{t+1})^T // See documentation in HessianFactor, we have A1 = -F, A2 = I_, b = B*u: // TODO: starts to seem more elaborate than straight-up KF equations? - Matrix M = inverse(Q), Ft = trans(F); + Matrix M = Q.inverse(), Ft = trans(F); Matrix G12 = -Ft * M, G11 = -G12 * F, G22 = M; Vector b = B * u, g2 = M * b, g1 = -Ft * g2; double f = dot(b, g2); @@ -147,7 +147,7 @@ KalmanFilter::State KalmanFilter::update(const State& p, const Matrix& H, KalmanFilter::State KalmanFilter::updateQ(const State& p, const Matrix& H, const Vector& z, const Matrix& Q) const { Key k = step(p); - Matrix M = inverse(Q), Ht = trans(H); + Matrix M = Q.inverse(), Ht = trans(H); Matrix G = Ht * M * H; Vector g = Ht * M * z; double f = dot(z, M * z); diff --git a/gtsam/linear/iterative.h b/gtsam/linear/iterative.h index 1ba0a7423..22f65b8de 100644 --- a/gtsam/linear/iterative.h +++ b/gtsam/linear/iterative.h @@ -86,7 +86,7 @@ namespace gtsam { /** x += alpha* A'*e */ void transposeMultiplyAdd(double alpha, const Vector& e, Vector& x) const { - gtsam::transposeMultiplyAdd(alpha, A(), e, x); + x += alpha * A().transpose() * e; } }; diff --git a/gtsam/linear/tests/testGaussianBayesTree.cpp b/gtsam/linear/tests/testGaussianBayesTree.cpp index 4f7c5c335..738a6d59f 100644 --- a/gtsam/linear/tests/testGaussianBayesTree.cpp +++ b/gtsam/linear/tests/testGaussianBayesTree.cpp @@ -170,7 +170,7 @@ TEST(GaussianBayesTree, complicatedMarginal) { LONGS_EQUAL(1, (long)actualJacobianQR.size()); LONGS_EQUAL(5, (long)actualJacobianQR.keys()[0]); Matrix actualA = actualJacobianQR.getA(actualJacobianQR.begin()); - Matrix actualCov = inverse(actualA.transpose() * actualA); + Matrix actualCov = (actualA.transpose() * actualA).inverse(); EXPECT(assert_equal(expectedCov, actualCov, 1e-1)); // Marginal on 6 @@ -187,7 +187,7 @@ TEST(GaussianBayesTree, complicatedMarginal) { LONGS_EQUAL(1, (long)actualJacobianQR.size()); LONGS_EQUAL(6, (long)actualJacobianQR.keys()[0]); actualA = actualJacobianQR.getA(actualJacobianQR.begin()); - actualCov = inverse(actualA.transpose() * actualA); + actualCov = (actualA.transpose() * actualA).inverse(); EXPECT(assert_equal(expectedCov, actualCov, 1e1)); } diff --git a/gtsam/linear/tests/testGaussianFactorGraph.cpp b/gtsam/linear/tests/testGaussianFactorGraph.cpp index 7404caa14..26e6b1925 100644 --- a/gtsam/linear/tests/testGaussianFactorGraph.cpp +++ b/gtsam/linear/tests/testGaussianFactorGraph.cpp @@ -47,10 +47,10 @@ TEST(GaussianFactorGraph, initialization) { SharedDiagonal unit2 = noiseModel::Unit::Create(2); fg += - JacobianFactor(0, 10*eye(2), -1.0*ones(2), unit2), - JacobianFactor(0, -10*eye(2),1, 10*eye(2), Vector2(2.0, -1.0), unit2), - JacobianFactor(0, -5*eye(2), 2, 5*eye(2), Vector2(0.0, 1.0), unit2), - JacobianFactor(1, -5*eye(2), 2, 5*eye(2), Vector2(-1.0, 1.5), unit2); + JacobianFactor(0, 10*I_2x2, -1.0*Matrix::Ones(2,2), unit2), + JacobianFactor(0, -10*I_2x2,1, 10*I_2x2, Vector2(2.0, -1.0), unit2), + JacobianFactor(0, -5*I_2x2, 2, 5*I_2x2, Vector2(0.0, 1.0), unit2), + JacobianFactor(1, -5*I_2x2, 2, 5*I_2x2, Vector2(-1.0, 1.5), unit2); EXPECT_LONGS_EQUAL(4, (long)fg.size()); @@ -166,13 +166,13 @@ static GaussianFactorGraph createSimpleGaussianFactorGraph() { GaussianFactorGraph fg; SharedDiagonal unit2 = noiseModel::Unit::Create(2); // linearized prior on x1: c[_x1_]+x1=0 i.e. x1=-c[_x1_] - fg += JacobianFactor(2, 10*eye(2), -1.0*ones(2), unit2); + fg += JacobianFactor(2, 10*I_2x2, -1.0*Matrix::Ones(2,2), unit2); // odometry between x1 and x2: x2-x1=[0.2;-0.1] - fg += JacobianFactor(0, 10*eye(2), 2, -10*eye(2), Vector2(2.0, -1.0), unit2); + fg += JacobianFactor(0, 10*I_2x2, 2, -10*I_2x2, Vector2(2.0, -1.0), unit2); // measurement between x1 and l1: l1-x1=[0.0;0.2] - fg += JacobianFactor(1, 5*eye(2), 2, -5*eye(2), Vector2(0.0, 1.0), unit2); + fg += JacobianFactor(1, 5*I_2x2, 2, -5*I_2x2, Vector2(0.0, 1.0), unit2); // measurement between x2 and l1: l1-x2=[-0.2;0.3] - fg += JacobianFactor(0, -5*eye(2), 1, 5*eye(2), Vector2(-1.0, 1.5), unit2); + fg += JacobianFactor(0, -5*I_2x2, 1, 5*I_2x2, Vector2(-1.0, 1.5), unit2); return fg; } @@ -280,8 +280,8 @@ TEST( GaussianFactorGraph, multiplyHessianAdd ) /* ************************************************************************* */ static GaussianFactorGraph createGaussianFactorGraphWithHessianFactor() { GaussianFactorGraph gfg = createSimpleGaussianFactorGraph(); - gfg += HessianFactor(1, 2, 100*eye(2,2), zeros(2,2), Vector2(0.0, 1.0), - 400*eye(2,2), Vector2(1.0, 1.0), 3.0); + gfg += HessianFactor(1, 2, 100*I_2x2, Z_2x2, Vector2(0.0, 1.0), + 400*I_2x2, Vector2(1.0, 1.0), 3.0); return gfg; } diff --git a/gtsam/linear/tests/testHessianFactor.cpp b/gtsam/linear/tests/testHessianFactor.cpp index 61083a926..4b824d818 100644 --- a/gtsam/linear/tests/testHessianFactor.cpp +++ b/gtsam/linear/tests/testHessianFactor.cpp @@ -54,7 +54,7 @@ TEST(HessianFactor, emptyConstructor) HessianFactor f; DOUBLES_EQUAL(0.0, f.constantTerm(), 1e-9); // Constant term should be zero EXPECT(assert_equal(Vector(), f.linearTerm())); // Linear term should be empty - EXPECT(assert_equal(zeros(1,1), f.info())); // Full matrix should be 1-by-1 zero matrix + EXPECT(assert_equal((Matrix) Z_1x1, f.info())); // Full matrix should be 1-by-1 zero matrix DOUBLES_EQUAL(0.0, f.error(VectorValues()), 1e-9); // Should have zero error } @@ -123,11 +123,11 @@ TEST(HessianFactor, Constructor1) TEST(HessianFactor, Constructor1b) { Vector mu = Vector2(1.0,2.0); - Matrix Sigma = eye(2,2); + Matrix Sigma = I_2x2; HessianFactor factor(0, mu, Sigma); - Matrix G = eye(2,2); + Matrix G = I_2x2; Vector g = G*mu; double f = dot(g,mu); diff --git a/gtsam/linear/tests/testJacobianFactor.cpp b/gtsam/linear/tests/testJacobianFactor.cpp index d57f896ef..7ae28264c 100644 --- a/gtsam/linear/tests/testJacobianFactor.cpp +++ b/gtsam/linear/tests/testJacobianFactor.cpp @@ -168,19 +168,19 @@ namespace simple_graph { Key keyX(10), keyY(8), keyZ(12); double sigma1 = 0.1; -Matrix A11 = Matrix::Identity(2, 2); +Matrix A11 = I_2x2; 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); +Matrix A21 = -2 * I_2x2; +Matrix A22 = 3 * I_2x2; 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); +Matrix A32 = -4 * I_2x2; +Matrix A33 = 5 * I_2x2; Vector2 b3(3, -6); JacobianFactor factor3(keyY, A32, keyZ, A33, b3, noiseModel::Isotropic::Sigma(2, sigma3)); @@ -193,8 +193,8 @@ TEST( JacobianFactor, construct_from_graph) { 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 A1(6,2); A1 << A11, A21, Z_2x2; + Matrix A2(6,2); A2 << Z_2x2, 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; @@ -268,9 +268,9 @@ TEST(JacobianFactor, matrices_NULL) // hessianBlockDiagonal map actualBD = factor.hessianBlockDiagonal(); LONGS_EQUAL(3,actualBD.size()); - EXPECT(assert_equal(1*eye(3),actualBD[5])); - EXPECT(assert_equal(4*eye(3),actualBD[10])); - EXPECT(assert_equal(9*eye(3),actualBD[15])); + EXPECT(assert_equal(1*I_3x3,actualBD[5])); + EXPECT(assert_equal(4*I_3x3,actualBD[10])); + EXPECT(assert_equal(9*I_3x3,actualBD[15])); } /* ************************************************************************* */ @@ -314,9 +314,9 @@ TEST(JacobianFactor, matrices) // hessianBlockDiagonal map actualBD = factor.hessianBlockDiagonal(); LONGS_EQUAL(3,actualBD.size()); - EXPECT(assert_equal(4*eye(3),actualBD[5])); - EXPECT(assert_equal(16*eye(3),actualBD[10])); - EXPECT(assert_equal(36*eye(3),actualBD[15])); + EXPECT(assert_equal(4*I_3x3,actualBD[5])); + EXPECT(assert_equal(16*I_3x3,actualBD[10])); + EXPECT(assert_equal(36*I_3x3,actualBD[15])); } /* ************************************************************************* */ @@ -324,7 +324,7 @@ TEST(JacobianFactor, operators ) { SharedDiagonal sigma0_1 = noiseModel::Isotropic::Sigma(2,0.1); - Matrix I = eye(2); + Matrix I = I_2x2; Vector b = Vector2(0.2,-0.1); JacobianFactor lf(1, -I, 2, I, b, sigma0_1); @@ -405,7 +405,7 @@ TEST(JacobianFactor, eliminate) gfg.add(0, A10, 1, A11, b1, noiseModel::Diagonal::Sigmas(s1, true)); gfg.add(1, A21, b2, noiseModel::Diagonal::Sigmas(s2, true)); - Matrix zero3x3 = zeros(3,3); + Matrix zero3x3 = Matrix::Zero(3,3); Matrix A0 = gtsam::stack(3, &A10, &zero3x3, &zero3x3); Matrix A1 = gtsam::stack(3, &A11, &A01, &A21); Vector9 b; b << b1, b0, b2; @@ -561,7 +561,7 @@ TEST ( JacobianFactor, constraint_eliminate1 ) { // construct a linear constraint Vector v(2); v(0)=1.2; v(1)=3.4; - JacobianFactor lc(1, eye(2), v, noiseModel::Constrained::All(2)); + JacobianFactor lc(1, I_2x2, v, noiseModel::Constrained::All(2)); // eliminate it pair @@ -572,7 +572,7 @@ TEST ( JacobianFactor, constraint_eliminate1 ) // verify conditional Gaussian Vector sigmas = Vector2(0.0, 0.0); - GaussianConditional expCG(1, v, eye(2), noiseModel::Diagonal::Sigmas(sigmas)); + GaussianConditional expCG(1, v, I_2x2, noiseModel::Diagonal::Sigmas(sigmas)); EXPECT(assert_equal(expCG, *actual.first)); } diff --git a/gtsam/linear/tests/testKalmanFilter.cpp b/gtsam/linear/tests/testKalmanFilter.cpp index 16be98306..b095c5a31 100644 --- a/gtsam/linear/tests/testKalmanFilter.cpp +++ b/gtsam/linear/tests/testKalmanFilter.cpp @@ -51,7 +51,7 @@ TEST( KalmanFilter, constructor ) { EXPECT(assert_equal(x_initial, p1->mean())); Matrix Sigma = (Matrix(2, 2) << 0.01, 0.0, 0.0, 0.01).finished(); EXPECT(assert_equal(Sigma, p1->covariance())); - EXPECT(assert_equal(inverse(Sigma), p1->information())); + EXPECT(assert_equal(Sigma.inverse(), p1->information())); // Create one with a sharedGaussian KalmanFilter::State p2 = kf1.init(x_initial, Sigma); @@ -65,33 +65,33 @@ TEST( KalmanFilter, constructor ) { TEST( KalmanFilter, linear1 ) { // Create the controls and measurement properties for our example - Matrix F = eye(2, 2); - Matrix B = eye(2, 2); + Matrix F = I_2x2; + Matrix B = I_2x2; Vector u = Vector2(1.0, 0.0); SharedDiagonal modelQ = noiseModel::Isotropic::Sigma(2, 0.1); - Matrix Q = 0.01*eye(2, 2); - Matrix H = eye(2, 2); + Matrix Q = 0.01*I_2x2; + Matrix H = I_2x2; State z1(1.0, 0.0); State z2(2.0, 0.0); State z3(3.0, 0.0); SharedDiagonal modelR = noiseModel::Isotropic::Sigma(2, 0.1); - Matrix R = 0.01*eye(2, 2); + Matrix R = 0.01*I_2x2; // Create the set of expected output TestValues State expected0(0.0, 0.0); - Matrix P00 = 0.01*eye(2, 2); + Matrix P00 = 0.01*I_2x2; State expected1(1.0, 0.0); Matrix P01 = P00 + Q; - Matrix I11 = inverse(P01) + inverse(R); + Matrix I11 = P01.inverse() + R.inverse(); State expected2(2.0, 0.0); - Matrix P12 = inverse(I11) + Q; - Matrix I22 = inverse(P12) + inverse(R); + Matrix P12 = I11.inverse() + Q; + Matrix I22 = P12.inverse() + R.inverse(); State expected3(3.0, 0.0); - Matrix P23 = inverse(I22) + Q; - Matrix I33 = inverse(P23) + inverse(R); + Matrix P23 = I22.inverse() + Q; + Matrix I33 = P23.inverse() + R.inverse(); // Create a Kalman filter of dimension 2 KalmanFilter kf(2); @@ -140,7 +140,7 @@ TEST( KalmanFilter, predict ) { Vector u = Vector3(1.0, 0.0, 2.0); Matrix R = (Matrix(2, 2) << 1.0, 0.5, 0.0, 3.0).finished(); Matrix M = trans(R)*R; - Matrix Q = inverse(M); + Matrix Q = M.inverse(); // Create a Kalman filter of dimension 2 KalmanFilter kf(2); @@ -197,7 +197,7 @@ TEST( KalmanFilter, QRvsCholesky ) { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0).finished(); - Matrix B = zeros(9, 1); + Matrix B = Matrix::Zero(9, 1); Vector u = zero(1); Matrix dt_Q_k = 1e-6 * (Matrix(9, 9) << 33.7, 3.1, -0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, @@ -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.array().square()); + Matrix modelQ = ((Matrix) sigmas.array().square()).asDiagonal(); KalmanFilter::State pa3 = kfa.updateQ(pa, H, z, modelQ); KalmanFilter::State pb3 = kfb.updateQ(pb, H, z, modelQ); diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index 434d94d7f..43ce271f3 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -112,7 +112,7 @@ TEST(NoiseModel, Unit) TEST(NoiseModel, equals) { Gaussian::shared_ptr g1 = Gaussian::SqrtInformation(R), - g2 = Gaussian::SqrtInformation(eye(3,3)); + g2 = Gaussian::SqrtInformation(I_3x3); Diagonal::shared_ptr d1 = Diagonal::Sigmas(Vector3(kSigma, kSigma, kSigma)), d2 = Diagonal::Sigmas(Vector3(0.1, 0.2, 0.3)); Isotropic::shared_ptr i1 = Isotropic::Sigma(3, kSigma), @@ -395,7 +395,7 @@ TEST(NoiseModel, SmartSqrtInformation ) { bool smart = true; gtsam::SharedGaussian expected = Unit::Create(3); - gtsam::SharedGaussian actual = Gaussian::SqrtInformation(eye(3), smart); + gtsam::SharedGaussian actual = Gaussian::SqrtInformation(I_3x3, smart); EXPECT(assert_equal(*expected,*actual)); } @@ -404,7 +404,7 @@ TEST(NoiseModel, SmartSqrtInformation2 ) { bool smart = true; gtsam::SharedGaussian expected = Unit::Isotropic::Sigma(3,2); - gtsam::SharedGaussian actual = Gaussian::SqrtInformation(0.5*eye(3), smart); + gtsam::SharedGaussian actual = Gaussian::SqrtInformation(0.5*I_3x3, smart); EXPECT(assert_equal(*expected,*actual)); } @@ -413,7 +413,7 @@ TEST(NoiseModel, SmartInformation ) { bool smart = true; gtsam::SharedGaussian expected = Unit::Isotropic::Variance(3,2); - Matrix M = 0.5*eye(3); + Matrix M = 0.5*I_3x3; EXPECT(checkIfDiagonal(M)); gtsam::SharedGaussian actual = Gaussian::Information(M, smart); EXPECT(assert_equal(*expected,*actual)); @@ -424,7 +424,7 @@ TEST(NoiseModel, SmartCovariance ) { bool smart = true; gtsam::SharedGaussian expected = Unit::Create(3); - gtsam::SharedGaussian actual = Gaussian::Covariance(eye(3), smart); + gtsam::SharedGaussian actual = Gaussian::Covariance(I_3x3, smart); EXPECT(assert_equal(*expected,*actual)); } @@ -433,7 +433,7 @@ TEST(NoiseModel, ScalarOrVector ) { bool smart = true; SharedGaussian expected = Unit::Create(3); - SharedGaussian actual = Gaussian::Covariance(eye(3), smart); + SharedGaussian actual = Gaussian::Covariance(I_3x3, smart); EXPECT(assert_equal(*expected,*actual)); } @@ -442,9 +442,9 @@ TEST(NoiseModel, WhitenInPlace) { Vector sigmas = Vector3(0.1, 0.1, 0.1); SharedDiagonal model = Diagonal::Sigmas(sigmas); - Matrix A = eye(3); + Matrix A = I_3x3; model->WhitenInPlace(A); - Matrix expected = eye(3) * 10; + Matrix expected = I_3x3 * 10; EXPECT(assert_equal(expected, A)); } diff --git a/gtsam/linear/tests/testSerializationLinear.cpp b/gtsam/linear/tests/testSerializationLinear.cpp index 71fdbe6a6..669aa30e8 100644 --- a/gtsam/linear/tests/testSerializationLinear.cpp +++ b/gtsam/linear/tests/testSerializationLinear.cpp @@ -51,7 +51,7 @@ BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); /* ************************************************************************* */ // example noise models static noiseModel::Diagonal::shared_ptr diag3 = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.2, 0.3)); -static noiseModel::Gaussian::shared_ptr gaussian3 = noiseModel::Gaussian::SqrtInformation(2.0 * eye(3,3)); +static noiseModel::Gaussian::shared_ptr gaussian3 = noiseModel::Gaussian::SqrtInformation(2.0 * I_3x3); static noiseModel::Isotropic::shared_ptr iso3 = noiseModel::Isotropic::Sigma(3, 0.2); static noiseModel::Constrained::shared_ptr constrained3 = noiseModel::Constrained::MixedSigmas(Vector3(0.0, 0.0, 0.1)); static noiseModel::Unit::shared_ptr unit3 = noiseModel::Unit::Create(3); @@ -144,7 +144,7 @@ TEST (Serialization, linear_factors) { EXPECT(equalsBinary(values)); Key i1 = 4, i2 = 7; - Matrix A1 = eye(3), A2 = -1.0 * eye(3); + Matrix A1 = I_3x3, A2 = -1.0 * I_3x3; Vector b = ones(3); SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector3(1.0, 2.0, 3.0)); JacobianFactor jacobianfactor(i1, A1, i2, A2, b, model); @@ -185,8 +185,8 @@ TEST (Serialization, gaussian_factor_graph) { { Key i1 = 4, i2 = 7; - Matrix A1 = eye(3), A2 = -1.0 * eye(3); - Vector b = ones(3); + Matrix A1 = I_3x3, A2 = -1.0 * I_3x3; + Vector b = Matrix::Ones(3,3); SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector3(1.0, 2.0, 3.0)); JacobianFactor jacobianfactor(i1, A1, i2, A2, b, model); HessianFactor hessianfactor(jacobianfactor); diff --git a/gtsam/navigation/AttitudeFactor.h b/gtsam/navigation/AttitudeFactor.h index b61a861d6..21f74ac06 100644 --- a/gtsam/navigation/AttitudeFactor.h +++ b/gtsam/navigation/AttitudeFactor.h @@ -189,7 +189,7 @@ public: Vector e = attitudeError(nTb.rotation(), H); if (H) { Matrix H23 = *H; - *H = Matrix::Zero(2, 6); + *H = Matrix::Zero(2,6); H->block<2,3>(0,0) = H23; } return e; diff --git a/gtsam/navigation/MagFactor.h b/gtsam/navigation/MagFactor.h index bee3e8944..3875391d0 100644 --- a/gtsam/navigation/MagFactor.h +++ b/gtsam/navigation/MagFactor.h @@ -154,7 +154,7 @@ public: // measured bM = nRb� * nM + b, where b is unknown bias Point3 hx = bRn_.rotate(nM, boost::none, H1) + bias; if (H2) - *H2 = eye(3); + *H2 = I_3x3; return (hx - measured_); } }; @@ -205,7 +205,7 @@ public: *H2 = scale * H * (*H2); } if (H3) - *H3 = eye(3); + *H3 = I_3x3; return (hx - measured_); } }; diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index fefd4b23c..c243ca860 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -105,7 +105,7 @@ pair PreintegrationBase::correctMeasurementsBySensorPose( // Update derivative: centrifugal causes the correlation between acc and omega!!! if (correctedAcc_H_unbiasedOmega) { double wdp = correctedOmega.dot(b_arm); - *correctedAcc_H_unbiasedOmega = -(diag(Vector3::Constant(wdp)) + *correctedAcc_H_unbiasedOmega = -( (Matrix) Vector3::Constant(wdp).asDiagonal() + correctedOmega * b_arm.transpose()) * bRs.matrix() + 2 * b_arm * unbiasedOmega.transpose(); } diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index e24c0d610..ccdcb86be 100644 --- a/gtsam/nonlinear/NonlinearEquality.h +++ b/gtsam/nonlinear/NonlinearEquality.h @@ -142,11 +142,11 @@ public: const size_t nj = traits::GetDimension(feasible_); if (allow_error_) { if (H) - *H = eye(nj); // FIXME: this is not the right linearization for nonlinear compare + *H = Matrix::Identity(nj,nj); // FIXME: this is not the right linearization for nonlinear compare return traits::Local(xj,feasible_); } else if (compare_(feasible_, xj)) { if (H) - *H = eye(nj); + *H = Matrix::Identity(nj,nj); return zero(nj); // set error to zero if equal } else { if (H) @@ -249,7 +249,7 @@ public: Vector evaluateError(const X& x1, boost::optional H = boost::none) const { if (H) - (*H) = eye(traits::GetDimension(x1)); + (*H) = Matrix::Identity(traits::GetDimension(x1),traits::GetDimension(x1)); // manifold equivalent of h(x)-z -> log(z,h(x)) return traits::Local(value_,x1); } @@ -322,8 +322,8 @@ public: Vector evaluateError(const X& x1, const X& x2, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { static const size_t p = traits::dimension; - if (H1) *H1 = -eye(p); - if (H2) *H2 = eye(p); + if (H1) *H1 = -Matrix::Identity(p,p); + if (H2) *H2 = Matrix::Identity(p,p); return traits::Local(x1,x2); } diff --git a/gtsam/nonlinear/tests/testExpression.cpp b/gtsam/nonlinear/tests/testExpression.cpp index 43f1cd017..0b0511e40 100644 --- a/gtsam/nonlinear/tests/testExpression.cpp +++ b/gtsam/nonlinear/tests/testExpression.cpp @@ -269,11 +269,11 @@ Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3, OptionalJacobi 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); + *H1 = I_3x3; if (H2) - *H2 = eye(3); + *H2 = I_3x3; if (H3) - *H3 = eye(3); + *H3 = I_3x3; return R1 * (R2 * R3); } diff --git a/gtsam/nonlinear/tests/testLinearContainerFactor.cpp b/gtsam/nonlinear/tests/testLinearContainerFactor.cpp index bd862ef94..7dd4a1f8b 100644 --- a/gtsam/nonlinear/tests/testLinearContainerFactor.cpp +++ b/gtsam/nonlinear/tests/testLinearContainerFactor.cpp @@ -234,7 +234,7 @@ TEST( testLinearContainerFactor, creation ) { // create a linear factor SharedDiagonal model = noiseModel::Unit::Create(2); JacobianFactor::shared_ptr linear_factor(new JacobianFactor( - l3, eye(2,2), l5, 2.0 * eye(2,2), zero(2), model)); + l3, I_2x2, l5, 2.0 * I_2x2, zero(2), model)); // create a set of values - build with full set of values gtsam::Values full_values, exp_values; diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index d2b042fed..db7dc4e74 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -266,9 +266,9 @@ public: return reprojError.vector(); } catch( CheiralityException& e) { - if (H1) *H1 = zeros(2, 6); - if (H2) *H2 = zeros(2, 3); - if (H3) *H3 = zeros(2, DimK); + if (H1) *H1 = Matrix::Zero(2, 6); + if (H2) *H2 = Matrix::Zero(2, 3); + if (H3) *H3 = Matrix::Zero(2, DimK); std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) << " behind Camera " << DefaultKeyFormatter(this->key1()) << std::endl; } diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp index 7d7a86405..180e5cd24 100644 --- a/gtsam/slam/InitializePose3.cpp +++ b/gtsam/slam/InitializePose3.cpp @@ -31,10 +31,9 @@ using namespace std; namespace gtsam { namespace InitializePose3 { -//static const Matrix I = eye(1); -static const Matrix I9 = Matrix::Identity(9,9); +static const Matrix I9 = I_9x9; static const Vector zero9 = Vector::Zero(9); -static const Matrix zero33= Matrix::Zero(3,3); +static const Matrix zero33 = Z_3x3; static const Key keyAnchor = symbol('Z', 9999999); @@ -54,11 +53,9 @@ GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g) { else std::cout << "Error in buildLinearOrientationGraph" << std::endl; - // std::cout << "Rij \n" << Rij << std::endl; - const FastVector& keys = factor->keys(); Key key1 = keys[0], key2 = keys[1]; - Matrix M9 = Matrix::Zero(9,9); + Matrix M9 = Z_9x9; M9.block(0,0,3,3) = Rij; M9.block(3,3,3,3) = Rij; M9.block(6,6,3,3) = Rij; @@ -74,7 +71,7 @@ GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g) { Values normalizeRelaxedRotations(const VectorValues& relaxedRot3) { gttic(InitializePose3_computeOrientationsChordal); - Matrix ppm = Matrix::Zero(3,3); // plus plus minus + Matrix ppm = Z_3x3; // plus plus minus ppm(0,0) = 1; ppm(1,1) = 1; ppm(2,2) = -1; Values validRot3; diff --git a/gtsam/slam/JacobianFactorQ.h b/gtsam/slam/JacobianFactorQ.h index 16560a43e..f9f258055 100644 --- a/gtsam/slam/JacobianFactorQ.h +++ b/gtsam/slam/JacobianFactorQ.h @@ -56,7 +56,7 @@ public: Base() { size_t j = 0, m2 = E.rows(), m = m2 / ZDim; // Calculate projector Q - Matrix Q = eye(m2) - E * P * E.transpose(); + Matrix Q = Matrix::Identity(m2,m2) - E * P * E.transpose(); // Calculate pre-computed Jacobian matrices // TODO: can we do better ? std::vector QF; diff --git a/gtsam/slam/OrientedPlane3Factor.cpp b/gtsam/slam/OrientedPlane3Factor.cpp index 3728e53b1..5b0c6a6b9 100644 --- a/gtsam/slam/OrientedPlane3Factor.cpp +++ b/gtsam/slam/OrientedPlane3Factor.cpp @@ -47,7 +47,7 @@ Vector OrientedPlane3DirectionPrior::evaluateError(const OrientedPlane3& plane, Vector e = n_hat_p.error(n_hat_q, H_p); H->resize(2, 3); H->block<2, 2>(0, 0) << H_p; - H->block<2, 1>(0, 2) << Matrix::Zero(2, 1); + H->block<2, 1>(0, 2) << Z_2x1; return e; } else { Unit3 n_hat_p = measured_p_.normal(); diff --git a/gtsam/slam/PoseRotationPrior.h b/gtsam/slam/PoseRotationPrior.h index 94c19a9d0..44f317a49 100644 --- a/gtsam/slam/PoseRotationPrior.h +++ b/gtsam/slam/PoseRotationPrior.h @@ -75,7 +75,7 @@ public: Vector evaluateError(const Pose& pose, boost::optional H = boost::none) const { const Rotation& newR = pose.rotation(); if (H) { - *H = gtsam::zeros(rDim, xDim); + *H = Matrix::Zero(rDim, xDim); std::pair rotInterval = POSE::rotationInterval(); (*H).middleCols(rotInterval.first, rDim).setIdentity(rDim, rDim); } diff --git a/gtsam/slam/PoseTranslationPrior.h b/gtsam/slam/PoseTranslationPrior.h index a24421b34..0b1c0cf63 100644 --- a/gtsam/slam/PoseTranslationPrior.h +++ b/gtsam/slam/PoseTranslationPrior.h @@ -65,7 +65,7 @@ public: const int tDim = traits::GetDimension(newTrans); const int xDim = traits::GetDimension(pose); if (H) { - *H = gtsam::zeros(tDim, xDim); + *H = Matrix::Zero(tDim, xDim); std::pair transInterval = POSE::translationInterval(); (*H).middleCols(transInterval.first, tDim) = R.matrix(); } diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h index 1056527d1..211e299e3 100644 --- a/gtsam/slam/ProjectionFactor.h +++ b/gtsam/slam/ProjectionFactor.h @@ -146,8 +146,8 @@ namespace gtsam { return reprojectionError.vector(); } } catch( CheiralityException& e) { - if (H1) *H1 = zeros(2,6); - if (H2) *H2 = zeros(2,3); + if (H1) *H1 = Matrix::Zero(2,6); + if (H2) *H2 = Matrix::Zero(2,3); if (verboseCheirality_) std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) << " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl; diff --git a/gtsam/slam/ReferenceFrameFactor.h b/gtsam/slam/ReferenceFrameFactor.h index 90ceeafc8..966ade343 100644 --- a/gtsam/slam/ReferenceFrameFactor.h +++ b/gtsam/slam/ReferenceFrameFactor.h @@ -100,7 +100,7 @@ public: boost::optional Dlocal = boost::none) const { Point newlocal = transform_point(trans, global, Dtrans, Dforeign); if (Dlocal) - *Dlocal = -1* gtsam::eye(Point::dimension); + *Dlocal = -1* Matrix::Identity(Point::dimension,Point::dimension); return traits::Local(local,newlocal); } diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index a5d5d1883..9d0f9c554 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -291,7 +291,7 @@ public: if (params_.degeneracyMode == ZERO_ON_DEGENERACY && !result_) { // failed: return"empty" Hessian BOOST_FOREACH(Matrix& m, Gs) - m = zeros(Base::Dim, Base::Dim); + m = Matrix::Zero(Base::Dim, Base::Dim); BOOST_FOREACH(Vector& v, gs) v = zero(Base::Dim); return boost::make_shared >(this->keys_, diff --git a/gtsam/slam/StereoFactor.h b/gtsam/slam/StereoFactor.h index 578422eaf..528479f80 100644 --- a/gtsam/slam/StereoFactor.h +++ b/gtsam/slam/StereoFactor.h @@ -138,8 +138,8 @@ public: return (stereoCam.project(point, H1, H2) - measured_).vector(); } } catch(StereoCheiralityException& e) { - if (H1) *H1 = zeros(3,6); - if (H2) *H2 = zeros(3,3); + if (H1) *H1 = Matrix::Zero(3,6); + if (H2) *H2 = Z_3x3; if (verboseCheirality_) std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) << " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl; diff --git a/gtsam/slam/TriangulationFactor.h b/gtsam/slam/TriangulationFactor.h index aa50929a5..2748599c4 100644 --- a/gtsam/slam/TriangulationFactor.h +++ b/gtsam/slam/TriangulationFactor.h @@ -124,7 +124,7 @@ public: return error.vector(); } catch (CheiralityException& e) { if (H2) - *H2 = zeros(Measurement::dimension, 3); + *H2 = Matrix::Zero(Measurement::dimension, 3); if (verboseCheirality_) std::cout << e.what() << ": Landmark " << DefaultKeyFormatter(this->key()) << " moved behind camera" diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index c1c919a58..cbfa1ad31 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -472,7 +472,7 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate, << p.x() << " " << p.y() << " " << p.z() << " " << R.toQuaternion().x() << " " << R.toQuaternion().y() << " " << R.toQuaternion().z() << " " << R.toQuaternion().w(); - Matrix InfoG2o = Matrix::Identity(6,6); + Matrix InfoG2o = I_6x6; InfoG2o.block(0,0,3,3) = Info.block(3,3,3,3); // cov translation InfoG2o.block(3,3,3,3) = Info.block(0,0,3,3); // cov rotation InfoG2o.block(0,3,3,3) = Info.block(0,3,3,3); // off diagonal @@ -539,7 +539,7 @@ GraphAndValues load3D(const string& filename) { ls >> id1 >> id2 >> x >> y >> z >> roll >> pitch >> yaw; Rot3 R = Rot3::Ypr(yaw,pitch,roll); Point3 t = Point3(x, y, z); - Matrix m = Matrix::Identity(6,6); + Matrix m = I_6x6; for (int i = 0; i < 6; i++) for (int j = i; j < 6; j++) ls >> m(i, j); @@ -549,7 +549,7 @@ GraphAndValues load3D(const string& filename) { graph->push_back(factor); } if (tag == "EDGE_SE3:QUAT") { - Matrix m = Matrix::Identity(6,6); + Matrix m = I_6x6; Key id1, id2; double x, y, z, qx, qy, qz, qw; ls >> id1 >> id2 >> x >> y >> z >> qx >> qy >> qz >> qw; @@ -563,7 +563,7 @@ GraphAndValues load3D(const string& filename) { m(j, i) = mij; } } - Matrix mgtsam = Matrix::Identity(6,6); + Matrix mgtsam = I_6x6; mgtsam.block(0,0,3,3) = m.block(3,3,3,3); // cov rotation mgtsam.block(3,3,3,3) = m.block(0,0,3,3); // cov translation mgtsam.block(0,3,3,3) = m.block(0,3,3,3); // off diagonal diff --git a/gtsam/slam/lago.cpp b/gtsam/slam/lago.cpp index a29d3e8b1..fa6fce37a 100644 --- a/gtsam/slam/lago.cpp +++ b/gtsam/slam/lago.cpp @@ -30,8 +30,8 @@ using namespace std; namespace gtsam { namespace lago { -static const Matrix I = Matrix::Identity(1,1); -static const Matrix I3 = Matrix::Identity(3,3); +static const Matrix I = I_1x1; +static const Matrix I3 = I_3x3; static const Key keyAnchor = symbol('Z', 9999999); static const noiseModel::Diagonal::shared_ptr priorOrientationNoise = diff --git a/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp b/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp index 3664de9c5..261cc1716 100644 --- a/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp +++ b/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp @@ -49,9 +49,9 @@ const Vector b = (Vector(6) << 1., 2., 3., 4., 5., 6.).finished(); //************************************************************************************* TEST( regularImplicitSchurFactor, creation ) { // Matrix E = Matrix::Ones(6,3); - Matrix E = zeros(6, 3); - E.block<2,2>(0, 0) = eye(2); - E.block<2,3>(2, 0) = 2 * ones(2, 3); + Matrix E = Matrix::Zero(6, 3); + E.block<2,2>(0, 0) = I_2x2; + E.block<2,3>(2, 0) = 2 * Matrix::Ones(2, 3); Matrix3 P = (E.transpose() * E).inverse(); RegularImplicitSchurFactor expected(keys, FBlocks, E, P, b); Matrix expectedP = expected.getPointCovariance(); @@ -61,10 +61,10 @@ TEST( regularImplicitSchurFactor, creation ) { /* ************************************************************************* */ TEST( regularImplicitSchurFactor, addHessianMultiply ) { - Matrix E = zeros(6, 3); - E.block<2,2>(0, 0) = eye(2); - E.block<2,3>(2, 0) = 2 * ones(2, 3); - E.block<2,2>(4, 1) = eye(2); + Matrix E = Matrix::Zero(6, 3); + E.block<2,2>(0, 0) = I_2x2; + E.block<2,3>(2, 0) = 2 * Matrix::Ones(2, 3); + E.block<2,2>(4, 1) = I_2x2; Matrix3 P = (E.transpose() * E).inverse(); double alpha = 0.5; @@ -83,7 +83,7 @@ TEST( regularImplicitSchurFactor, addHessianMultiply ) { // Create full F size_t M=4, m = 3, d = 6; Matrix F(2 * m, d * M); - F << F0, zeros(2, d * 3), zeros(2, d), F1, zeros(2, d*2), zeros(2, d * 3), F3; + F << F0, Matrix::Zero(2, d * 3), Matrix::Zero(2, d), F1, Matrix::Zero(2, d*2), Matrix::Zero(2, d * 3), F3; // Calculate expected result F'*alpha*(I - E*P*E')*F*x FastVector keys2; @@ -246,7 +246,7 @@ TEST(regularImplicitSchurFactor, hessianDiagonal) EXPECT(assert_equal(F3.transpose()*F3-FtE3*P*FtE3.transpose(),actualBD[3])); // variant two - Matrix I2 = eye(2); + Matrix I2 = I_2x2; Matrix E0 = E.block<2,3>(0, 0); Matrix F0t = F0.transpose(); EXPECT(assert_equal(F0t*F0-F0t*E0*P*E0.transpose()*F0,actualBD[0])); diff --git a/gtsam_unstable/dynamics/DynamicsPriors.h b/gtsam_unstable/dynamics/DynamicsPriors.h index 5b986415e..b4dfa7dc8 100644 --- a/gtsam_unstable/dynamics/DynamicsPriors.h +++ b/gtsam_unstable/dynamics/DynamicsPriors.h @@ -57,7 +57,7 @@ struct VelocityPrior : public gtsam::PartialPriorFactor { this->mask_[0] = 6; this->mask_[1] = 7; this->mask_[2] = 8; - this->H_ = zeros(3, 9); + this->H_ = Matrix::Zero(3, 9); this->fillH(); } }; @@ -81,7 +81,7 @@ struct DGroundConstraint : public gtsam::PartialPriorFactor { this->mask_[1] = 8; // vz this->mask_[2] = 0; // roll this->mask_[3] = 1; // pitch - this->H_ = zeros(3, 9); + this->H_ = Matrix::Zero(3, 9); this->fillH(); } @@ -97,7 +97,7 @@ struct DGroundConstraint : public gtsam::PartialPriorFactor { this->mask_[1] = 8; // vz this->mask_[2] = 0; // roll this->mask_[3] = 1; // pitch - this->H_ = zeros(3, 9); + this->H_ = Matrix::Zero(3, 9); this->fillH(); } }; diff --git a/gtsam_unstable/dynamics/Pendulum.h b/gtsam_unstable/dynamics/Pendulum.h index ecc43ad79..28a26edce 100644 --- a/gtsam_unstable/dynamics/Pendulum.h +++ b/gtsam_unstable/dynamics/Pendulum.h @@ -50,9 +50,9 @@ public: boost::optional H2 = boost::none, boost::optional H3 = boost::none) const { const size_t p = 1; - if (H1) *H1 = -eye(p); - if (H2) *H2 = eye(p); - if (H3) *H3 = eye(p)*h_; + if (H1) *H1 = -Matrix::Zero(p,p); + if (H2) *H2 = Matrix::Zero(p,p); + if (H3) *H3 = Matrix::Zero(p,p)*h_; return (Vector(1) << qk+v*h_-qk1).finished(); } @@ -98,9 +98,9 @@ public: boost::optional H2 = boost::none, boost::optional H3 = boost::none) const { const size_t p = 1; - if (H1) *H1 = -eye(p); - if (H2) *H2 = eye(p); - if (H3) *H3 = -eye(p)*h_*g_/r_*cos(q); + if (H1) *H1 = -Matrix::Zero(p,p); + if (H2) *H2 = Matrix::Zero(p,p); + if (H3) *H3 = -Matrix::Zero(p,p)*h_*g_/r_*cos(q); return (Vector(1) << vk - h_ * g_ / r_ * sin(q) - vk1).finished(); } @@ -154,9 +154,9 @@ public: double mr2_h = 1/h_*m_*r_*r_; double mgrh = m_*g_*r_*h_; - if (H1) *H1 = -eye(p); - if (H2) *H2 = eye(p)*(-mr2_h + mgrh*(1-alpha_)*(1-alpha_)*cos(qmid)); - if (H3) *H3 = eye(p)*( mr2_h + mgrh*(1-alpha_)*(alpha_)*cos(qmid)); + if (H1) *H1 = -Matrix::Zero(p,p); + if (H2) *H2 = Matrix::Zero(p,p)*(-mr2_h + mgrh*(1-alpha_)*(1-alpha_)*cos(qmid)); + if (H3) *H3 = Matrix::Zero(p,p)*( mr2_h + mgrh*(1-alpha_)*(alpha_)*cos(qmid)); return (Vector(1) << mr2_h * (qk1 - qk) + mgrh * (1 - alpha_) * sin(qmid) - pk).finished(); } @@ -210,9 +210,9 @@ public: double mr2_h = 1/h_*m_*r_*r_; double mgrh = m_*g_*r_*h_; - if (H1) *H1 = -eye(p); - if (H2) *H2 = eye(p)*(-mr2_h - mgrh*(1-alpha_)*alpha_*cos(qmid)); - if (H3) *H3 = eye(p)*( mr2_h - mgrh*alpha_*alpha_*cos(qmid)); + if (H1) *H1 = -Matrix::Zero(p,p); + if (H2) *H2 = Matrix::Zero(p,p)*(-mr2_h - mgrh*(1-alpha_)*alpha_*cos(qmid)); + if (H3) *H3 = Matrix::Zero(p,p)*( mr2_h - mgrh*alpha_*alpha_*cos(qmid)); return (Vector(1) << mr2_h * (qk1 - qk) - mgrh * alpha_ * sin(qmid) - pk1).finished(); } diff --git a/gtsam_unstable/dynamics/SimpleHelicopter.h b/gtsam_unstable/dynamics/SimpleHelicopter.h index a3dd6327b..f38c256b1 100644 --- a/gtsam_unstable/dynamics/SimpleHelicopter.h +++ b/gtsam_unstable/dynamics/SimpleHelicopter.h @@ -140,7 +140,7 @@ public: } if (H3) { - *H3 = zeros(6,6); + *H3 = Z_6x6; insertSub(*H3, -h_*D_gravityBody_gk, 3, 0); } diff --git a/gtsam_unstable/dynamics/VelocityConstraint3.h b/gtsam_unstable/dynamics/VelocityConstraint3.h index 9ecc7619e..63a2c43bf 100644 --- a/gtsam_unstable/dynamics/VelocityConstraint3.h +++ b/gtsam_unstable/dynamics/VelocityConstraint3.h @@ -41,9 +41,9 @@ public: boost::optional H2 = boost::none, boost::optional H3 = boost::none) const { const size_t p = 1; - if (H1) *H1 = eye(p); - if (H2) *H2 = -eye(p); - if (H3) *H3 = eye(p)*dt_; + if (H1) *H1 = Matrix::Zero(p,p); + if (H2) *H2 = -Matrix::Zero(p,p); + if (H3) *H3 = Matrix::Zero(p,p)*dt_; return (Vector(1) << x1+v*dt_-x2).finished(); } diff --git a/gtsam_unstable/dynamics/tests/testPoseRTV.cpp b/gtsam_unstable/dynamics/tests/testPoseRTV.cpp index 855131042..1e52988c0 100644 --- a/gtsam_unstable/dynamics/tests/testPoseRTV.cpp +++ b/gtsam_unstable/dynamics/tests/testPoseRTV.cpp @@ -227,15 +227,15 @@ TEST( testPoseRTV, transformed_from_2 ) { /* ************************************************************************* */ TEST(testPoseRTV, RRTMbn) { - EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMbn(kZero3))); - EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMbn(Rot3()))); + EXPECT(assert_equal(I_3x3, PoseRTV::RRTMbn(kZero3))); + EXPECT(assert_equal(I_3x3, PoseRTV::RRTMbn(Rot3()))); EXPECT(assert_equal(PoseRTV::RRTMbn(Vector3(0.3, 0.2, 0.1)), PoseRTV::RRTMbn(Rot3::Ypr(0.1, 0.2, 0.3)))); } /* ************************************************************************* */ TEST(testPoseRTV, RRTMnb) { - EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMnb(kZero3))); - EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMnb(Rot3()))); + EXPECT(assert_equal(I_3x3, PoseRTV::RRTMnb(kZero3))); + EXPECT(assert_equal(I_3x3, PoseRTV::RRTMnb(Rot3()))); EXPECT(assert_equal(PoseRTV::RRTMnb(Vector3(0.3, 0.2, 0.1)), PoseRTV::RRTMnb(Rot3::Ypr(0.1, 0.2, 0.3)))); } diff --git a/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp b/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp index 6f75c264c..db5c2bc6e 100644 --- a/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp +++ b/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp @@ -29,8 +29,8 @@ Vector gamma2 = Vector2(0.0, 0.0); // no shape Vector u2 = Vector2(0.0, 0.0); // no control at time 2 double distT = 1.0; // distance from the body-centered x axis to the big top motor double distR = 5.0; // distance from the body-centered z axis to the small motor -Matrix Mass = diag((Vector(3) << mass, mass, mass).finished()); -Matrix Inertia = diag((Vector(6) << 2.0/5.0*mass*distR*distR, 2.0/5.0*mass*distR*distR, 2.0/5.0*mass*distR*distR, mass, mass, mass).finished()); +Matrix Mass = ((Vector(3) << mass, mass, mass).finished()).asDiagonal(); +Matrix Inertia = (Vector(6) << 2.0/5.0*mass*distR*distR, 2.0/5.0*mass*distR*distR, 2.0/5.0*mass*distR*distR, mass, mass, mass).finished().asDiagonal(); Vector computeFu(const Vector& gamma, const Vector& control) { double gamma_r = gamma(0), gamma_p = gamma(1); diff --git a/gtsam_unstable/geometry/InvDepthCamera3.h b/gtsam_unstable/geometry/InvDepthCamera3.h index 4ce0eaedb..4f1b42610 100644 --- a/gtsam_unstable/geometry/InvDepthCamera3.h +++ b/gtsam_unstable/geometry/InvDepthCamera3.h @@ -102,7 +102,7 @@ public: gtsam::Matrix J2; gtsam::Point2 uv= camera.project(landmark,H1, J2, boost::none); if (H1) { - *H1 = (*H1) * gtsam::eye(6); + *H1 = (*H1) * I_6x6; } double cos_theta = cos(theta); diff --git a/gtsam_unstable/geometry/Pose3Upright.cpp b/gtsam_unstable/geometry/Pose3Upright.cpp index f83cb19fb..4237170f0 100644 --- a/gtsam_unstable/geometry/Pose3Upright.cpp +++ b/gtsam_unstable/geometry/Pose3Upright.cpp @@ -81,7 +81,7 @@ Pose3 Pose3Upright::pose() const { Pose3Upright Pose3Upright::inverse(boost::optional H1) const { Pose3Upright result(T_.inverse(H1), -z_); if (H1) { - Matrix H1_ = -eye(4,4); + Matrix H1_ = -I_4x4; H1_.topLeftCorner(2,2) = H1->topLeftCorner(2,2); H1_.topRightCorner(2, 1) = H1->topRightCorner(2, 1); *H1 = H1_; @@ -96,12 +96,12 @@ Pose3Upright Pose3Upright::compose(const Pose3Upright& p2, return Pose3Upright(T_.compose(p2.T_), z_ + p2.z_); Pose3Upright result(T_.compose(p2.T_, H1), z_ + p2.z_); if (H1) { - Matrix H1_ = eye(4,4); + Matrix H1_ = I_4x4; H1_.topLeftCorner(2,2) = H1->topLeftCorner(2,2); H1_.topRightCorner(2, 1) = H1->topRightCorner(2, 1); *H1 = H1_; } - if (H2) *H2 = eye(4,4); + if (H2) *H2 = I_4x4; return result; } @@ -112,12 +112,12 @@ Pose3Upright Pose3Upright::between(const Pose3Upright& p2, return Pose3Upright(T_.between(p2.T_), p2.z_ - z_); Pose3Upright result(T_.between(p2.T_, H1, H2), p2.z_ - z_); if (H1) { - Matrix H1_ = -eye(4,4); + Matrix H1_ = -I_4x4; H1_.topLeftCorner(2,2) = H1->topLeftCorner(2,2); H1_.topRightCorner(2, 1) = H1->topRightCorner(2, 1); *H1 = H1_; } - if (H2) *H2 = eye(4,4); + if (H2) *H2 = I_4x4; return result; } diff --git a/gtsam_unstable/linear/tests/testLinearEquality.cpp b/gtsam_unstable/linear/tests/testLinearEquality.cpp index bf41743a2..90f3de586 100644 --- a/gtsam_unstable/linear/tests/testLinearEquality.cpp +++ b/gtsam_unstable/linear/tests/testLinearEquality.cpp @@ -186,7 +186,7 @@ TEST(LinearEquality, matrices) /* ************************************************************************* */ TEST(LinearEquality, operators ) { - Matrix I = eye(2); + Matrix I = I_2x2; Vector b = (Vector(2) << 0.2,-0.1).finished(); LinearEquality lf(1, -I, 2, I, b, 0); diff --git a/gtsam_unstable/linear/tests/testQPSolver.cpp b/gtsam_unstable/linear/tests/testQPSolver.cpp index 8fca61ca4..d643223f8 100644 --- a/gtsam_unstable/linear/tests/testQPSolver.cpp +++ b/gtsam_unstable/linear/tests/testQPSolver.cpp @@ -26,7 +26,7 @@ using namespace std; using namespace gtsam; using namespace gtsam::symbol_shorthand; -const Matrix One = ones(1,1); +const Matrix One = Matrix::Ones(1,1); /* ************************************************************************* */ // Create test graph according to Forst10book_pg171Ex5 @@ -38,14 +38,14 @@ QP createTestCase() { // 0.5*x1'*G11*x1 + x1'*G12*x2 + 0.5*x2'*G22*x2 - x1'*g1 - x2'*g2 + 0.5*f // Hence, we have G11=2, G12 = -1, g1 = +3, G22 = 2, g2 = 0, f = 10 qp.cost.push_back( - HessianFactor(X(1), X(2), 2.0 * ones(1, 1), -ones(1, 1), 3.0 * ones(1), - 2.0 * ones(1, 1), zero(1), 10.0)); + HessianFactor(X(1), X(2), 2.0 * Matrix::Ones(1, 1), -Matrix::Ones(1, 1), 3.0 * ones(1), + 2.0 * Matrix::Ones(1, 1), zero(1), 10.0)); // Inequality constraints - qp.inequalities.push_back(LinearInequality(X(1), ones(1,1), X(2), ones(1,1), 2, 0)); // x1 + x2 <= 2 --> x1 + x2 -2 <= 0, --> b=2 - qp.inequalities.push_back(LinearInequality(X(1), -ones(1,1), 0, 1)); // -x1 <= 0 - qp.inequalities.push_back(LinearInequality(X(2), -ones(1,1), 0, 2)); // -x2 <= 0 - qp.inequalities.push_back(LinearInequality(X(1), ones(1,1), 1.5, 3)); // x1 <= 3/2 + qp.inequalities.push_back(LinearInequality(X(1), Matrix::Ones(1,1), X(2), Matrix::Ones(1,1), 2, 0)); // x1 + x2 <= 2 --> x1 + x2 -2 <= 0, --> b=2 + qp.inequalities.push_back(LinearInequality(X(1), -Matrix::Ones(1,1), 0, 1)); // -x1 <= 0 + qp.inequalities.push_back(LinearInequality(X(2), -Matrix::Ones(1,1), 0, 2)); // -x2 <= 0 + qp.inequalities.push_back(LinearInequality(X(1), Matrix::Ones(1,1), 1.5, 3)); // x1 <= 3/2 return qp; } @@ -53,8 +53,8 @@ QP createTestCase() { TEST(QPSolver, TestCase) { VectorValues values; double x1 = 5, x2 = 7; - values.insert(X(1), x1 * ones(1, 1)); - values.insert(X(2), x2 * ones(1, 1)); + values.insert(X(1), x1 * Matrix::Ones(1, 1)); + values.insert(X(2), x2 * Matrix::Ones(1, 1)); QP qp = createTestCase(); DOUBLES_EQUAL(29, x1 * x1 - x1 * x2 + x2 * x2 - 3 * x1 + 5, 1e-9); DOUBLES_EQUAL(29, qp.cost[0]->error(values), 1e-9); @@ -92,8 +92,8 @@ QP createEqualityConstrainedTest() { // 0.5*x1'*G11*x1 + x1'*G12*x2 + 0.5*x2'*G22*x2 - x1'*g1 - x2'*g2 + 0.5*f // Hence, we have G11=2, G12 = 0, g1 = 0, G22 = 2, g2 = 0, f = 0 qp.cost.push_back( - HessianFactor(X(1), X(2), 2.0 * ones(1, 1), zeros(1, 1), zero(1), - 2.0 * ones(1, 1), zero(1), 0.0)); + HessianFactor(X(1), X(2), 2.0 * Matrix::Ones(1, 1), Z_1x1, zero(1), + 2.0 * Matrix::Ones(1, 1), zero(1), 0.0)); // Equality constraints // x1 + x2 = 1 --> x1 + x2 -1 = 0, hence we negate the b vector @@ -219,8 +219,8 @@ QP createTestMatlabQPEx() { // 0.5*x1'*G11*x1 + x1'*G12*x2 + 0.5*x2'*G22*x2 - x1'*g1 - x2'*g2 + 0.5*f // Hence, we have G11=1, G12 = -1, g1 = +2, G22 = 2, g2 = +6, f = 0 qp.cost.push_back( - HessianFactor(X(1), X(2), 1.0 * ones(1, 1), -ones(1, 1), 2.0 * ones(1), - 2.0 * ones(1, 1), 6 * ones(1), 1000.0)); + HessianFactor(X(1), X(2), 1.0 * Matrix::Ones(1,1), -Matrix::Ones(1, 1), 2.0 * ones(1), + 2.0 * Matrix::Ones(1, 1), 6 * ones(1), 1000.0)); // Inequality constraints qp.inequalities.push_back(LinearInequality(X(1), One, X(2), One, 2, 0)); // x1 + x2 <= 2 @@ -251,8 +251,8 @@ TEST(QPSolver, optimizeMatlabEx) { QP createTestNocedal06bookEx16_4() { QP qp; - qp.cost.push_back(JacobianFactor(X(1), ones(1, 1), ones(1))); - qp.cost.push_back(JacobianFactor(X(2), ones(1, 1), 2.5 * ones(1))); + qp.cost.push_back(JacobianFactor(X(1), Matrix::Ones(1, 1), ones(1))); + qp.cost.push_back(JacobianFactor(X(2), Matrix::Ones(1, 1), 2.5 * ones(1))); // Inequality constraints qp.inequalities.push_back(LinearInequality(X(1), -One, X(2), 2 * One, 2, 0)); @@ -283,8 +283,8 @@ TEST(QPSolver, optimizeNocedal06bookEx16_4) { TEST(QPSolver, failedSubproblem) { QP qp; - qp.cost.push_back(JacobianFactor(X(1), eye(2), zero(2))); - qp.cost.push_back(HessianFactor(X(1), zeros(2, 2), zero(2), 100.0)); + qp.cost.push_back(JacobianFactor(X(1), I_2x2, zero(2))); + qp.cost.push_back(HessianFactor(X(1), Z_2x2, zero(2), 100.0)); qp.inequalities.push_back( LinearInequality(X(1), (Matrix(1,2) << -1.0, 0.0).finished(), -1.0, 0)); diff --git a/gtsam_unstable/nonlinear/tests/testParticleFactor.cpp b/gtsam_unstable/nonlinear/tests/testParticleFactor.cpp index bfd3c9168..1cf94b161 100644 --- a/gtsam_unstable/nonlinear/tests/testParticleFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testParticleFactor.cpp @@ -126,33 +126,33 @@ TEST( ParticleFilter, constructor) { TEST( ParticleFilter, linear1 ) { // Create the controls and measurement properties for our example - Matrix F = eye(2, 2); - Matrix B = eye(2, 2); + Matrix F = I_2x2; + Matrix B = I_2x2; Vector u = Vector2(1.0, 0.0); SharedDiagonal modelQ = noiseModel::Isotropic::Sigma(2, 0.1); - Matrix Q = 0.01 * eye(2, 2); - Matrix H = eye(2, 2); + Matrix Q = 0.01 * I_2x2; + Matrix H = I_2x2; State z1(1.0, 0.0); State z2(2.0, 0.0); State z3(3.0, 0.0); SharedDiagonal modelR = noiseModel::Isotropic::Sigma(2, 0.1); - Matrix R = 0.01 * eye(2, 2); + Matrix R = 0.01 * I_2x2; // Create the set of expected output TestValues State expected0(0.0, 0.0); - Matrix P00 = 0.01 * eye(2, 2); + Matrix P00 = 0.01 * I_2x2; State expected1(1.0, 0.0); Matrix P01 = P00 + Q; - Matrix I11 = inverse(P01) + inverse(R); + Matrix I11 = P01.inverse() + R.inverse(); State expected2(2.0, 0.0); - Matrix P12 = inverse(I11) + Q; - Matrix I22 = inverse(P12) + inverse(R); + Matrix P12 = I11.inverse() + Q; + Matrix I22 = P12.inverse() + R.inverse(); State expected3(3.0, 0.0); - Matrix P23 = inverse(I22) + Q; - Matrix I33 = inverse(P23) + inverse(R); + Matrix P23 = I22.inverse() + Q; + Matrix I33 = P23.inverse() + R.inverse(); // Create a Kalman filter of dimension 2 KalmanFilter kf(2); diff --git a/gtsam_unstable/slam/AHRS.cpp b/gtsam_unstable/slam/AHRS.cpp index ce8fd29f3..a16a4f9c0 100644 --- a/gtsam_unstable/slam/AHRS.cpp +++ b/gtsam_unstable/slam/AHRS.cpp @@ -83,7 +83,7 @@ std::pair AHRS::initialize(double g_e) 0.0, 0.0, 0.0).finished(); // we don't know anything on yaw // Calculate the initial covariance matrix for the error state dx, Farrell08book eq. 10.66 - Matrix Pa = 0.025 * 0.025 * eye(3); + Matrix Pa = 0.025 * 0.025 * I_3x3; Matrix P11 = Omega_T * (H_g * (Pa + Pa_) * trans(H_g)) * trans(Omega_T); P11(2, 2) = 0.0001; Matrix P12 = -Omega_T * H_g * Pa; @@ -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_.array().square()) * bRn; + R = trans(bRn) * ((Vector3) sigmas_v_a_.array().square()).asDiagonal() * bRn; } else { // my measurement prediction (in body frame): // F(:,k) = bias - b_g @@ -186,7 +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_.array().square()); + R = ((Vector3) sigmas_v_a_.array().square()).asDiagonal(); } // update the Kalman filter @@ -196,7 +196,7 @@ std::pair AHRS::aid( Mechanization_bRn2 newMech = mech.correct(updatedState->mean()); // reset KF state - Vector dx = zeros(9, 1); + Vector dx = Z_9x1; updatedState = KF_.init(dx, updatedState->covariance()); return make_pair(newMech, updatedState); @@ -217,7 +217,7 @@ std::pair AHRS::aidGeneral( Matrix H = collect(3, &b_g, &I_3x3, &Z_3x3); // Matrix R = diag(emul(sigmas_v_a_, sigmas_v_a_)); // Matrix R = diag(Vector3(1.0, 0.2, 1.0)); // good for L_twice - Matrix R = diag(Vector3(0.01, 0.0001, 0.01)); + Matrix R = Vector3(0.01, 0.0001, 0.01).asDiagonal(); // update the Kalman filter KalmanFilter::State updatedState = KF_.updateQ(state, H, z, R); @@ -226,7 +226,7 @@ std::pair AHRS::aidGeneral( Mechanization_bRn2 newMech = mech.correct(updatedState->mean()); // reset KF state - Vector dx = zeros(9, 1); + Vector dx = Z_9x1; updatedState = KF_.init(dx, updatedState->covariance()); return make_pair(newMech, updatedState); diff --git a/gtsam_unstable/slam/DummyFactor.cpp b/gtsam_unstable/slam/DummyFactor.cpp index 1ce3a878d..70aff0596 100644 --- a/gtsam_unstable/slam/DummyFactor.cpp +++ b/gtsam_unstable/slam/DummyFactor.cpp @@ -50,7 +50,7 @@ DummyFactor::linearize(const Values& c) const { std::vector > terms(this->size()); for(size_t j=0; jsize(); ++j) { terms[j].first = keys()[j]; - terms[j].second = zeros(rowDim_, dims_[j]); + terms[j].second = Matrix::Zero(rowDim_, dims_[j]); } noiseModel::Diagonal::shared_ptr model = noiseModel::Unit::Create(rowDim_); diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h index 79a37c2ff..58284c3a6 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h @@ -434,8 +434,8 @@ public: delta_t += msr_dt; // Update EquivCov_Overall - Matrix Z_3x3 = zeros(3,3); - Matrix I_3x3 = eye(3,3); + Matrix Z_3x3 = Z_3x3; + Matrix I_3x3 = I_3x3; Matrix H_pos_pos = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, _1, delta_vel_in_t0), delta_pos_in_t0); Matrix H_pos_vel = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, delta_pos_in_t0, _1), delta_vel_in_t0); @@ -461,7 +461,7 @@ public: noiseModel::Gaussian::shared_ptr model_discrete_curr = calc_descrete_noise_model(model_continuous_overall, msr_dt ); - Matrix Q_d = inverse(model_discrete_curr->R().transpose() * model_discrete_curr->R() ); + Matrix Q_d = (model_discrete_curr->R().transpose() * model_discrete_curr->R()).inverse(); EquivCov_Overall = F * EquivCov_Overall * F.transpose() + Q_d; // Luca: force identity covariance matrix (for testing purposes) @@ -536,9 +536,9 @@ public: static inline noiseModel::Gaussian::shared_ptr CalcEquivalentNoiseCov(const noiseModel::Gaussian::shared_ptr& gaussian_acc, const noiseModel::Gaussian::shared_ptr& gaussian_gyro, const noiseModel::Gaussian::shared_ptr& gaussian_process){ - Matrix cov_acc = inverse( gaussian_acc->R().transpose() * gaussian_acc->R() ); - Matrix cov_gyro = inverse( gaussian_gyro->R().transpose() * gaussian_gyro->R() ); - Matrix cov_process = inverse( gaussian_process->R().transpose() * gaussian_process->R() ); + Matrix cov_acc = ( gaussian_acc->R().transpose() * gaussian_acc->R() ).inverse(); + Matrix cov_gyro = ( gaussian_gyro->R().transpose() * gaussian_gyro->R() ).inverse(); + Matrix cov_process = ( gaussian_process->R().transpose() * gaussian_process->R() ).inverse(); cov_process.block(0,0, 3,3) += cov_gyro; cov_process.block(6,6, 3,3) += cov_acc; @@ -550,9 +550,9 @@ public: const noiseModel::Gaussian::shared_ptr& gaussian_process, Matrix& cov_acc, Matrix& cov_gyro, Matrix& cov_process_without_acc_gyro){ - cov_acc = inverse( gaussian_acc->R().transpose() * gaussian_acc->R() ); - cov_gyro = inverse( gaussian_gyro->R().transpose() * gaussian_gyro->R() ); - cov_process_without_acc_gyro = inverse( gaussian_process->R().transpose() * gaussian_process->R() ); + cov_acc = ( gaussian_acc->R().transpose() * gaussian_acc->R() ).inverse(); + cov_gyro = ( gaussian_gyro->R().transpose() * gaussian_gyro->R() ).inverse(); + cov_process_without_acc_gyro = ( gaussian_process->R().transpose() * gaussian_process->R() ).inverse(); } static inline void Calc_g_rho_omega_earth_NED(const Vector& Pos_NED, const Vector& Vel_NED, const Vector& LatLonHeight_IC, const Vector& Pos_NED_Initial, diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h index 8216942cd..acca233d9 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h @@ -367,8 +367,8 @@ public: delta_t += msr_dt; // Update EquivCov_Overall - Matrix Z_3x3 = zeros(3,3); - Matrix I_3x3 = eye(3,3); + Matrix Z_3x3 = Z_3x3; + Matrix I_3x3 = I_3x3; Matrix H_pos_pos = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, _1, delta_vel_in_t0), delta_pos_in_t0); Matrix H_pos_vel = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, delta_pos_in_t0, _1), delta_vel_in_t0); diff --git a/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h b/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h index 7f9564ee3..68c972a86 100644 --- a/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h +++ b/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h @@ -103,8 +103,8 @@ public: Vector hx(v2 - alpha_v1); - if(H1) *H1 = - diag(alpha); - if(H2) *H2 = eye(v2.size()); + if(H1) *H1 = -1 * alpha.asDiagonal(); + if(H2) *H2 = Matrix::Identity(v2.size(),v2.size()); return hx; } diff --git a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h index 30d3a216d..c3a67232a 100644 --- a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h +++ b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h @@ -278,9 +278,9 @@ public: static inline noiseModel::Gaussian::shared_ptr CalcEquivalentNoiseCov(const noiseModel::Gaussian::shared_ptr& gaussian_acc, const noiseModel::Gaussian::shared_ptr& gaussian_gyro, const noiseModel::Gaussian::shared_ptr& gaussian_process){ - Matrix cov_acc = inverse( gaussian_acc->R().transpose() * gaussian_acc->R() ); - Matrix cov_gyro = inverse( gaussian_gyro->R().transpose() * gaussian_gyro->R() ); - Matrix cov_process = inverse( gaussian_process->R().transpose() * gaussian_process->R() ); + Matrix cov_acc = ( gaussian_acc->R().transpose() * gaussian_acc->R() ).inverse(); + Matrix cov_gyro = ( gaussian_gyro->R().transpose() * gaussian_gyro->R() ).inverse(); + Matrix cov_process = ( gaussian_process->R().transpose() * gaussian_process->R() ).inverse(); cov_process.block(0,0, 3,3) += cov_gyro; cov_process.block(6,6, 3,3) += cov_acc; diff --git a/gtsam_unstable/slam/InvDepthFactor3.h b/gtsam_unstable/slam/InvDepthFactor3.h index ac481f979..3de6a1824 100644 --- a/gtsam_unstable/slam/InvDepthFactor3.h +++ b/gtsam_unstable/slam/InvDepthFactor3.h @@ -89,9 +89,9 @@ public: gtsam::Point2 reprojectionError(camera.project(point, invDepth, H1, H2, H3) - measured_); return reprojectionError.vector(); } catch( CheiralityException& e) { - if (H1) *H1 = gtsam::zeros(2,6); - if (H2) *H2 = gtsam::zeros(2,5); - if (H3) *H2 = gtsam::zeros(2,1); + if (H1) *H1 = Matrix::Zero(2,6); + if (H2) *H2 = Matrix::Zero(2,5); + if (H3) *H2 = Matrix::Zero(2,1); std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) << " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl; return gtsam::ones(2) * 2.0 * K_->fx(); diff --git a/gtsam_unstable/slam/MultiProjectionFactor.h b/gtsam_unstable/slam/MultiProjectionFactor.h index f1487f562..84d4ae0db 100644 --- a/gtsam_unstable/slam/MultiProjectionFactor.h +++ b/gtsam_unstable/slam/MultiProjectionFactor.h @@ -184,8 +184,8 @@ namespace gtsam { return reprojectionError.vector(); } } catch( CheiralityException& e) { - if (H1) *H1 = zeros(2,6); - if (H2) *H2 = zeros(2,3); + if (H1) *H1 = Matrix::Zero(2,6); + if (H2) *H2 = Matrix::Zero(2,3); if (verboseCheirality_) std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) << " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl; diff --git a/gtsam_unstable/slam/PartialPriorFactor.h b/gtsam_unstable/slam/PartialPriorFactor.h index e3080466f..a848620e3 100644 --- a/gtsam_unstable/slam/PartialPriorFactor.h +++ b/gtsam_unstable/slam/PartialPriorFactor.h @@ -70,7 +70,7 @@ namespace gtsam { /** Single Element Constructor: acts on a single parameter specified by idx */ PartialPriorFactor(Key key, size_t idx, double prior, const SharedNoiseModel& model) : - Base(model, key), prior_((Vector(1) << prior).finished()), mask_(1, idx), H_(zeros(1, T::dimension)) { + Base(model, key), prior_((Vector(1) << prior).finished()), mask_(1, idx), H_(Matrix::Zero(1, T::dimension)) { assert(model->dim() == 1); this->fillH(); } @@ -78,7 +78,7 @@ namespace gtsam { /** Indices Constructor: specify the mask with a set of indices */ PartialPriorFactor(Key key, const std::vector& mask, const Vector& prior, const SharedNoiseModel& model) : - Base(model, key), prior_(prior), mask_(mask), H_(zeros(mask.size(), T::dimension)) { + Base(model, key), prior_(prior), mask_(mask), H_(Matrix::Zero(mask.size(), T::dimension)) { assert((size_t)prior_.size() == mask.size()); assert(model->dim() == (size_t) prior.size()); this->fillH(); diff --git a/gtsam_unstable/slam/ProjectionFactorPPP.h b/gtsam_unstable/slam/ProjectionFactorPPP.h index 5b1540c83..3b331d66b 100644 --- a/gtsam_unstable/slam/ProjectionFactorPPP.h +++ b/gtsam_unstable/slam/ProjectionFactorPPP.h @@ -137,9 +137,9 @@ namespace gtsam { return reprojectionError.vector(); } } catch( CheiralityException& e) { - if (H1) *H1 = zeros(2,6); - if (H2) *H2 = zeros(2,6); - if (H3) *H3 = zeros(2,3); + if (H1) *H1 = Matrix::Zero(2,6); + if (H2) *H2 = Matrix::Zero(2,6); + if (H3) *H3 = Matrix::Zero(2,3); if (verboseCheirality_) std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) << " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl; diff --git a/gtsam_unstable/slam/ProjectionFactorPPPC.h b/gtsam_unstable/slam/ProjectionFactorPPPC.h index 069274065..1bd352a33 100644 --- a/gtsam_unstable/slam/ProjectionFactorPPPC.h +++ b/gtsam_unstable/slam/ProjectionFactorPPPC.h @@ -132,10 +132,10 @@ namespace gtsam { return reprojectionError.vector(); } } catch( CheiralityException& e) { - if (H1) *H1 = zeros(2,6); - if (H2) *H2 = zeros(2,6); - if (H3) *H3 = zeros(2,3); - if (H4) *H4 = zeros(2,CALIBRATION::Dim()); + if (H1) *H1 = Matrix::Zero(2,6); + if (H2) *H2 = Matrix::Zero(2,6); + if (H3) *H3 = Matrix::Zero(2,3); + if (H4) *H4 = Matrix::Zero(2,CALIBRATION::Dim()); if (verboseCheirality_) std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) << " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl; diff --git a/gtsam_unstable/slam/RelativeElevationFactor.cpp b/gtsam_unstable/slam/RelativeElevationFactor.cpp index 941a1db89..db077994f 100644 --- a/gtsam_unstable/slam/RelativeElevationFactor.cpp +++ b/gtsam_unstable/slam/RelativeElevationFactor.cpp @@ -21,7 +21,7 @@ Vector RelativeElevationFactor::evaluateError(const Pose3& pose, const Point3& p boost::optional H1, boost::optional H2) const { double hx = pose.z() - point.z(); if (H1) { - *H1 = zeros(1, 6); + *H1 = Matrix::Zero(1,6); // Use bottom row of rotation matrix for derivative of translation (*H1)(0, 3) = pose.rotation().r1().z(); (*H1)(0, 4) = pose.rotation().r2().z(); @@ -29,7 +29,7 @@ Vector RelativeElevationFactor::evaluateError(const Pose3& pose, const Point3& p } if (H2) { - *H2 = zeros(1, 3); + *H2 = Matrix::Zero(1,3); (*H2)(0, 2) = -1.0; } return (Vector(1) << hx - measured_).finished(); diff --git a/gtsam_unstable/slam/SmartRangeFactor.h b/gtsam_unstable/slam/SmartRangeFactor.h index 84aed6ca8..6e638cc77 100644 --- a/gtsam_unstable/slam/SmartRangeFactor.h +++ b/gtsam_unstable/slam/SmartRangeFactor.h @@ -141,7 +141,7 @@ public: if (H) // set Jacobians to zero for n<3 for (size_t j = 0; j < n; j++) - (*H)[j] = zeros(3, 1); + (*H)[j] = Matrix::Zero(3, 1); return zero(1); } else { Vector error = zero(1); diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index 7cbeaae65..7f1bf544a 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -340,7 +340,7 @@ public: if (params_.degeneracyMode == ZERO_ON_DEGENERACY && !result_) { // failed: return"empty" Hessian BOOST_FOREACH(Matrix& m, Gs) - m = zeros(Base::Dim, Base::Dim); + m = Matrix::Zero(Base::Dim, Base::Dim); BOOST_FOREACH(Vector& v, gs) v = zero(Base::Dim); return boost::make_shared >(this->keys_, diff --git a/gtsam_unstable/slam/tests/testDummyFactor.cpp b/gtsam_unstable/slam/tests/testDummyFactor.cpp index 86b4e5584..03a57b352 100644 --- a/gtsam_unstable/slam/tests/testDummyFactor.cpp +++ b/gtsam_unstable/slam/tests/testDummyFactor.cpp @@ -47,7 +47,7 @@ TEST( testDummyFactor, basics ) { CHECK(actLinearization); noiseModel::Diagonal::shared_ptr model3 = noiseModel::Unit::Create(3); GaussianFactor::shared_ptr expLinearization(new JacobianFactor( - key1, zeros(3,3), key2, zeros(3,3), zero(3), model3)); + key1, Matrix::Zero(3,3), key2, Matrix::Zero(3,3), zero(3), model3)); EXPECT(assert_equal(*expLinearization, *actLinearization, tol)); } diff --git a/gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp b/gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp index 476447f8b..35bf5790e 100644 --- a/gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp +++ b/gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp @@ -41,8 +41,8 @@ TEST( EquivInertialNavFactor_GlobalVel, Constructor) Vector delta_vel_in_t0 = Vector3(0.0, 0.0, 0.0); Vector delta_angles = Vector3(0.0, 0.0, 0.0); double delta_t = 0.0; - Matrix EquivCov_Overall = zeros(15,15); - Matrix Jacobian_wrt_t0_Overall = eye(15); + Matrix EquivCov_Overall = Matrix::Zero(15,15); + Matrix Jacobian_wrt_t0_Overall = Matrix::Identity(15,15); imuBias::ConstantBias bias1 = imuBias::ConstantBias(); // Earth Terms (gravity, etc) diff --git a/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp b/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp index ee06dbce2..9ae4aa928 100644 --- a/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp @@ -70,7 +70,7 @@ TEST( SmartRangeFactor, unwhitenedError ) { // Check Jacobian for n==1 vector H1(1); f.unwhitenedError(values, H1); // with H now ! - CHECK(assert_equal(zeros(3,1), H1.front())); + CHECK(assert_equal(Matrix::Zero(3,1), H1.front())); // Whenever there are two ranges or less, error should be zero Vector actual1 = f.unwhitenedError(values); diff --git a/gtsam_unstable/testing_tools/base/cholScalingTest.m b/gtsam_unstable/testing_tools/base/cholScalingTest.m index ed347a523..24f597ed7 100644 --- a/gtsam_unstable/testing_tools/base/cholScalingTest.m +++ b/gtsam_unstable/testing_tools/base/cholScalingTest.m @@ -19,8 +19,8 @@ badscale = 1e-8; Acoeffs = [ 1 11 badscale - (1:10)' (1:10)' -ones(10,1) - (1:10)' (2:11)' ones(10,1) + (1:10)' (1:10)' -Matrix::Ones(10,1) + (1:10)' (2:11)' Matrix::Ones(10,1) ]'; A = full(sparse(Acoeffs(1,:), Acoeffs(2,:), Acoeffs(3,:))); b = zeros(size(A,1), 1); diff --git a/matlab/gtsam_examples/IMUKittiExampleGPS.m b/matlab/gtsam_examples/IMUKittiExampleGPS.m index 32f61e47f..b335b54be 100644 --- a/matlab/gtsam_examples/IMUKittiExampleGPS.m +++ b/matlab/gtsam_examples/IMUKittiExampleGPS.m @@ -38,7 +38,7 @@ currentBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1)); sigma_init_x = noiseModel.Isotropic.Precisions([ 0.0; 0.0; 0.0; 1; 1; 1 ]); sigma_init_v = noiseModel.Isotropic.Sigma(3, 1000.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) ]; +sigma_between_b = [ IMU_metadata.AccelerometerBiasSigma * Matrix::Ones(3,1); IMU_metadata.GyroscopeBiasSigma * Matrix::Ones(3,1) ]; g = [0;0;-9.8]; w_coriolis = [0;0;0]; diff --git a/matlab/gtsam_examples/SBAExample.m b/matlab/gtsam_examples/SBAExample.m index b0f754044..808064f4c 100644 --- a/matlab/gtsam_examples/SBAExample.m +++ b/matlab/gtsam_examples/SBAExample.m @@ -29,7 +29,7 @@ options.showImages = false; measurementNoiseSigma = 1.0; pointNoiseSigma = 0.1; cameraNoiseSigmas = [0.001 0.001 0.001 0.1 0.1 0.1 ... - 0.001*ones(1,5)]'; + 0.001*Matrix::Ones(1,5)]'; %% Create the graph (defined in visualSLAM.h, derived from NonlinearFactorGraph) graph = NonlinearFactorGraph; diff --git a/matlab/unstable_examples/FlightCameraTransformIMU.m b/matlab/unstable_examples/FlightCameraTransformIMU.m index b0b37ee33..25d6e69bd 100644 --- a/matlab/unstable_examples/FlightCameraTransformIMU.m +++ b/matlab/unstable_examples/FlightCameraTransformIMU.m @@ -62,7 +62,7 @@ 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) ]; +sigma_between_b = [ IMU_metadata.AccelerometerBiasSigma * Matrix::Ones(3,1); IMU_metadata.GyroscopeBiasSigma * Matrix::Ones(3,1) ]; g = [0;0;-9.8]; w_coriolis = [0;0;0]; diff --git a/matlab/unstable_examples/IMUKittiExampleAdvanced.m b/matlab/unstable_examples/IMUKittiExampleAdvanced.m index cb13eacee..8e2295159 100644 --- a/matlab/unstable_examples/IMUKittiExampleAdvanced.m +++ b/matlab/unstable_examples/IMUKittiExampleAdvanced.m @@ -155,7 +155,7 @@ for measurementIndex = 1:length(timestamps) %% Create GPS factor if type == 2 newFactors.add(PriorFactorPose3(currentPoseKey, Pose3(currentPoseGlobal.rotation, GPS_data(measurementIndex).Position), ... - noiseModel.Diagonal.Precisions([ zeros(3,1); 1./(GPS_data(measurementIndex).PositionSigma).^2*ones(3,1) ]))); + noiseModel.Diagonal.Precisions([ zeros(3,1); 1./(GPS_data(measurementIndex).PositionSigma).^2*Matrix::Ones(3,1) ]))); end %% Create VO factor diff --git a/matlab/unstable_examples/IMUKittiExampleVO.m b/matlab/unstable_examples/IMUKittiExampleVO.m index 6434e750a..91f187301 100644 --- a/matlab/unstable_examples/IMUKittiExampleVO.m +++ b/matlab/unstable_examples/IMUKittiExampleVO.m @@ -51,7 +51,7 @@ currentBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1)); sigma_init_x = noiseModel.Isotropic.Sigmas([ 1.0; 1.0; 0.01; 0.01; 0.01; 0.01 ]); sigma_init_v = noiseModel.Isotropic.Sigma(3, 1000.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) ]; +sigma_between_b = [ IMU_metadata.AccelerometerBiasSigma * Matrix::Ones(3,1); IMU_metadata.GyroscopeBiasSigma * Matrix::Ones(3,1) ]; g = [0;0;-9.8]; w_coriolis = [0;0;0]; diff --git a/matlab/unstable_examples/TransformCalProjectionFactorIMUExampleISAM.m b/matlab/unstable_examples/TransformCalProjectionFactorIMUExampleISAM.m index fd4a17469..0999596c9 100644 --- a/matlab/unstable_examples/TransformCalProjectionFactorIMUExampleISAM.m +++ b/matlab/unstable_examples/TransformCalProjectionFactorIMUExampleISAM.m @@ -94,7 +94,7 @@ 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) ]; +sigma_between_b = [ IMU_metadata.AccelerometerBiasSigma * Matrix::Ones(3,1); IMU_metadata.GyroscopeBiasSigma * Matrix::Ones(3,1) ]; g = [0;0;-9.8]; w_coriolis = [0;0;0]; diff --git a/tests/simulated2D.h b/tests/simulated2D.h index a1080a6de..0012f5f45 100644 --- a/tests/simulated2D.h +++ b/tests/simulated2D.h @@ -90,7 +90,7 @@ namespace simulated2D { /// Prior on a single pose, optionally returns derivative inline Point2 prior(const Point2& x, boost::optional H = boost::none) { - if (H) *H = gtsam::eye(2); + if (H) *H = I_2x2; return x; } @@ -102,8 +102,8 @@ namespace simulated2D { /// odometry between two poses, optionally returns derivative inline Point2 odo(const Point2& x1, const Point2& x2, boost::optional H1 = boost::none, boost::optional H2 = boost::none) { - if (H1) *H1 = -gtsam::eye(2); - if (H2) *H2 = gtsam::eye(2); + if (H1) *H1 = -I_2x2; + if (H2) *H2 = I_2x2; return x2 - x1; } @@ -115,8 +115,8 @@ namespace simulated2D { /// measurement between landmark and pose, optionally returns derivative inline Point2 mea(const Point2& x, const Point2& l, boost::optional H1 = boost::none, boost::optional H2 = boost::none) { - if (H1) *H1 = -gtsam::eye(2); - if (H2) *H2 = gtsam::eye(2); + if (H1) *H1 = -I_2x2; + if (H2) *H2 = I_2x2; return l - x; } diff --git a/tests/simulated2DConstraints.h b/tests/simulated2DConstraints.h index f593ab23a..ccc734cfd 100644 --- a/tests/simulated2DConstraints.h +++ b/tests/simulated2DConstraints.h @@ -90,7 +90,7 @@ namespace simulated2D { virtual double value(const Point& x, boost::optional H = boost::none) const { if (H) { - Matrix D = zeros(1, traits::GetDimension(x)); + Matrix D = Matrix::Zero(1, traits::GetDimension(x)); D(0, IDX) = 1.0; *H = D; } diff --git a/tests/simulated2DOriented.h b/tests/simulated2DOriented.h index b05fb45c1..9e604cb3c 100644 --- a/tests/simulated2DOriented.h +++ b/tests/simulated2DOriented.h @@ -63,7 +63,7 @@ namespace simulated2DOriented { /// Prior on a single pose, optional derivative version Pose2 prior(const Pose2& x, boost::optional H = boost::none) { - if (H) *H = gtsam::eye(3); + if (H) *H = I_3x3; return x; } diff --git a/tests/simulated3D.h b/tests/simulated3D.h index 84d9ec8cd..3c92e733e 100644 --- a/tests/simulated3D.h +++ b/tests/simulated3D.h @@ -39,7 +39,7 @@ namespace simulated3D { * Prior on a single pose */ Point3 prior(const Point3& x, boost::optional H = boost::none) { - if (H) *H = eye(3); + if (H) *H = I_3x3; return x; } @@ -49,8 +49,8 @@ Point3 prior(const Point3& x, boost::optional H = boost::none) { Point3 odo(const Point3& x1, const Point3& x2, boost::optional H1 = boost::none, boost::optional H2 = boost::none) { - if (H1) *H1 = -1 * eye(3); - if (H2) *H2 = eye(3); + if (H1) *H1 = -1 * I_3x3; + if (H2) *H2 = I_3x3; return x2 - x1; } @@ -60,8 +60,8 @@ Point3 odo(const Point3& x1, const Point3& x2, Point3 mea(const Point3& x, const Point3& l, boost::optional H1 = boost::none, boost::optional H2 = boost::none) { - if (H1) *H1 = -1 * eye(3); - if (H2) *H2 = eye(3); + if (H1) *H1 = -1 * I_3x3; + if (H2) *H2 = I_3x3; return l - x; } diff --git a/tests/smallExample.h b/tests/smallExample.h index 9866d22aa..33f42da7f 100644 --- a/tests/smallExample.h +++ b/tests/smallExample.h @@ -274,16 +274,16 @@ inline GaussianFactorGraph createGaussianFactorGraph() { GaussianFactorGraph fg; // linearized prior on x1: c[_x1_]+x1=0 i.e. x1=-c[_x1_] - fg += JacobianFactor(X(1), 10*eye(2), -1.0*ones(2)); + fg += JacobianFactor(X(1), 10*I_2x2, -1.0*Matrix::Ones(2,2)); // odometry between x1 and x2: x2-x1=[0.2;-0.1] - fg += JacobianFactor(X(1), -10*eye(2), X(2), 10*eye(2), Vector2(2.0, -1.0)); + fg += JacobianFactor(X(1), -10*I_2x2, X(2), 10*I_2x2, Vector2(2.0, -1.0)); // measurement between x1 and l1: l1-x1=[0.0;0.2] - fg += JacobianFactor(X(1), -5*eye(2), L(1), 5*eye(2), Vector2(0.0, 1.0)); + fg += JacobianFactor(X(1), -5*I_2x2, L(1), 5*I_2x2, Vector2(0.0, 1.0)); // measurement between x2 and l1: l1-x2=[-0.2;0.3] - fg += JacobianFactor(X(2), -5*eye(2), L(1), 5*eye(2), Vector2(-1.0, 1.5)); + fg += JacobianFactor(X(2), -5*I_2x2, L(1), 5*I_2x2, Vector2(-1.0, 1.5)); return fg; } @@ -409,7 +409,7 @@ inline GaussianFactorGraph createSimpleConstraintGraph() { using namespace impl; // create unary factor // prior on _x_, mean = [1,-1], sigma=0.1 - Matrix Ax = eye(2); + Matrix Ax = I_2x2; Vector b1(2); b1(0) = 1.0; b1(1) = -1.0; @@ -419,8 +419,8 @@ inline GaussianFactorGraph createSimpleConstraintGraph() { // between _x_ and _y_, that is going to be the only factor on _y_ // |1 0||x_1| + |-1 0||y_1| = |0| // |0 1||x_2| | 0 -1||y_2| |0| - Matrix Ax1 = eye(2); - Matrix Ay1 = eye(2) * -1; + Matrix Ax1 = I_2x2; + Matrix Ay1 = I_2x2 * -1; Vector b2 = Vector2(0.0, 0.0); JacobianFactor::shared_ptr f2(new JacobianFactor(_x_, Ax1, _y_, Ay1, b2, constraintModel)); @@ -450,7 +450,7 @@ inline GaussianFactorGraph createSingleConstraintGraph() { using namespace impl; // create unary factor // prior on _x_, mean = [1,-1], sigma=0.1 - Matrix Ax = eye(2); + Matrix Ax = I_2x2; Vector b1(2); b1(0) = 1.0; b1(1) = -1.0; @@ -466,7 +466,7 @@ inline GaussianFactorGraph createSingleConstraintGraph() { Ax1(0, 1) = 2.0; Ax1(1, 0) = 2.0; Ax1(1, 1) = 1.0; - Matrix Ay1 = eye(2) * 10; + Matrix Ay1 = I_2x2 * 10; Vector b2 = Vector2(1.0, 2.0); JacobianFactor::shared_ptr f2(new JacobianFactor(_x_, Ax1, _y_, Ay1, b2, constraintModel)); @@ -492,7 +492,7 @@ inline VectorValues createSingleConstraintValues() { inline GaussianFactorGraph createMultiConstraintGraph() { using namespace impl; // unary factor 1 - Matrix A = eye(2); + Matrix A = I_2x2; Vector b = Vector2(-2.0, 2.0); JacobianFactor::shared_ptr lf1(new JacobianFactor(_x_, A, b, sigma0_1)); diff --git a/tests/testExpressionFactor.cpp b/tests/testExpressionFactor.cpp index 282a6b816..c20eca34a 100644 --- a/tests/testExpressionFactor.cpp +++ b/tests/testExpressionFactor.cpp @@ -140,7 +140,7 @@ typedef Eigen::Matrix Matrix93; Vector9 wide(const Point3& p, OptionalJacobian<9,3> H) { Vector9 v; v << p, p, p; - if (H) *H << eye(3), eye(3), eye(3); + if (H) *H << I_3x3, I_3x3, I_3x3; return v; } typedef Eigen::Matrix Matrix9; @@ -334,11 +334,11 @@ TEST(ExpressionFactor, Compose1) { // Check unwhitenedError std::vector H(2); Vector actual = f.unwhitenedError(values, H); - EXPECT( assert_equal(eye(3), H[0],1e-9)); - EXPECT( assert_equal(eye(3), H[1],1e-9)); + EXPECT( assert_equal(I_3x3, H[0],1e-9)); + EXPECT( assert_equal(I_3x3, H[1],1e-9)); // Check linearization - JacobianFactor expected(1, eye(3), 2, eye(3), zero(3)); + JacobianFactor expected(1, I_3x3, 2, I_3x3, zero(3)); boost::shared_ptr gf = f.linearize(values); boost::shared_ptr jf = // boost::dynamic_pointer_cast(gf); @@ -364,10 +364,10 @@ TEST(ExpressionFactor, compose2) { std::vector H(1); Vector actual = f.unwhitenedError(values, H); EXPECT_LONGS_EQUAL(1, H.size()); - EXPECT( assert_equal(2*eye(3), H[0],1e-9)); + EXPECT( assert_equal(2*I_3x3, H[0],1e-9)); // Check linearization - JacobianFactor expected(1, 2 * eye(3), zero(3)); + JacobianFactor expected(1, 2 * I_3x3, zero(3)); boost::shared_ptr gf = f.linearize(values); boost::shared_ptr jf = // boost::dynamic_pointer_cast(gf); @@ -393,10 +393,10 @@ TEST(ExpressionFactor, compose3) { std::vector H(1); Vector actual = f.unwhitenedError(values, H); EXPECT_LONGS_EQUAL(1, H.size()); - EXPECT( assert_equal(eye(3), H[0],1e-9)); + EXPECT( assert_equal(I_3x3, H[0],1e-9)); // Check linearization - JacobianFactor expected(3, eye(3), zero(3)); + JacobianFactor expected(3, I_3x3, zero(3)); boost::shared_ptr gf = f.linearize(values); boost::shared_ptr jf = // boost::dynamic_pointer_cast(gf); @@ -409,11 +409,11 @@ 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); + *H1 = I_3x3; if (H2) - *H2 = eye(3); + *H2 = I_3x3; if (H3) - *H3 = eye(3); + *H3 = I_3x3; return R1 * (R2 * R3); } @@ -436,12 +436,12 @@ TEST(ExpressionFactor, composeTernary) { std::vector H(3); Vector actual = f.unwhitenedError(values, H); EXPECT_LONGS_EQUAL(3, H.size()); - EXPECT( assert_equal(eye(3), H[0],1e-9)); - EXPECT( assert_equal(eye(3), H[1],1e-9)); - EXPECT( assert_equal(eye(3), H[2],1e-9)); + EXPECT( assert_equal(I_3x3, H[0],1e-9)); + EXPECT( assert_equal(I_3x3, H[1],1e-9)); + EXPECT( assert_equal(I_3x3, H[2],1e-9)); // Check linearization - JacobianFactor expected(1, eye(3), 2, eye(3), 3, eye(3), zero(3)); + JacobianFactor expected(1, I_3x3, 2, I_3x3, 3, I_3x3, zero(3)); boost::shared_ptr gf = f.linearize(values); boost::shared_ptr jf = // boost::dynamic_pointer_cast(gf); diff --git a/tests/testExtendedKalmanFilter.cpp b/tests/testExtendedKalmanFilter.cpp index a7bcf7153..00ab4a16c 100644 --- a/tests/testExtendedKalmanFilter.cpp +++ b/tests/testExtendedKalmanFilter.cpp @@ -223,7 +223,7 @@ public: *H1 = -F(p1); if(H2) - *H2 = eye(dim()); + *H2 = Matrix::Identity(dim(),dim()); // Return the error between the prediction and the supplied value of p2 return (p2 - prediction).vector(); diff --git a/tests/testGaussianBayesTreeB.cpp b/tests/testGaussianBayesTreeB.cpp index cf54ce96e..9b172d572 100644 --- a/tests/testGaussianBayesTreeB.cpp +++ b/tests/testGaussianBayesTreeB.cpp @@ -79,7 +79,7 @@ TEST( GaussianBayesTree, linear_smoother_shortcuts ) double sigma3 = 0.61808; Matrix A56 = (Matrix(2,2) << -0.382022,0.,0.,-0.382022).finished(); GaussianBayesNet expected3; - expected3 += GaussianConditional(X(5), zero(2), eye(2)/sigma3, X(6), A56/sigma3); + expected3 += GaussianConditional(X(5), zero(2), I_2x2/sigma3, X(6), A56/sigma3); GaussianBayesTree::sharedClique C3 = bayesTree[X(4)]; GaussianBayesNet actual3 = C3->shortcut(R); EXPECT(assert_equal(expected3,actual3,tol)); @@ -88,7 +88,7 @@ TEST( GaussianBayesTree, linear_smoother_shortcuts ) double sigma4 = 0.661968; Matrix A46 = (Matrix(2,2) << -0.146067,0.,0.,-0.146067).finished(); GaussianBayesNet expected4; - expected4 += GaussianConditional(X(4), zero(2), eye(2)/sigma4, X(6), A46/sigma4); + expected4 += GaussianConditional(X(4), zero(2), I_2x2/sigma4, X(6), A46/sigma4); GaussianBayesTree::sharedClique C4 = bayesTree[X(3)]; GaussianBayesNet actual4 = C4->shortcut(R); EXPECT(assert_equal(expected4,actual4,tol)); @@ -134,7 +134,7 @@ TEST( GaussianBayesTree, balanced_smoother_marginals ) // Check marginal on x1 JacobianFactor expected1 = GaussianDensity::FromMeanAndStddev(X(1), zero(2), sigmax1); JacobianFactor actual1 = *bayesTree.marginalFactor(X(1)); - Matrix expectedCovarianceX1 = eye(2,2) * (sigmax1 * sigmax1); + Matrix expectedCovarianceX1 = I_2x2 * (sigmax1 * sigmax1); Matrix actualCovarianceX1; GaussianFactor::shared_ptr m = bayesTree.marginalFactor(X(1), EliminateCholesky); actualCovarianceX1 = bayesTree.marginalFactor(X(1), EliminateCholesky)->information().inverse(); @@ -243,7 +243,7 @@ TEST( GaussianBayesTree, balanced_smoother_joint ) GaussianBayesTree bayesTree = *smoother.eliminateMultifrontal(ordering); // Conditional density elements reused by both tests - const Matrix I = eye(2), A = -0.00429185*I; + const Matrix I = I_2x2, A = -0.00429185*I; // Check the joint density P(x1,x7) factored as P(x1|x7)P(x7) GaussianBayesNet expected1 = list_of diff --git a/tests/testGaussianFactorGraphB.cpp b/tests/testGaussianFactorGraphB.cpp index 88dc91ce7..cde79e637 100644 --- a/tests/testGaussianFactorGraphB.cpp +++ b/tests/testGaussianFactorGraphB.cpp @@ -78,7 +78,7 @@ TEST( GaussianFactorGraph, eliminateOne_x1 ) conditional = result.first->front(); // create expected Conditional Gaussian - Matrix I = 15*eye(2), R11 = I, S12 = -0.111111*I, S13 = -0.444444*I; + Matrix I = 15*I_2x2, R11 = I, S12 = -0.111111*I, S13 = -0.444444*I; Vector d = Vector2(-0.133333, -0.0222222); GaussianConditional expected(X(1),15*d,R11,L(1),S12,X(2),S13); @@ -96,7 +96,7 @@ TEST( GaussianFactorGraph, eliminateOne_x2 ) // create expected Conditional Gaussian double sig = 0.0894427; - Matrix I = eye(2)/sig, R11 = I, S12 = -0.2*I, S13 = -0.8*I; + Matrix I = I_2x2/sig, R11 = I, S12 = -0.2*I, S13 = -0.8*I; Vector d = Vector2(0.2, -0.14)/sig, sigma = ones(2); GaussianConditional expected(ordering[X(2)],d,R11,ordering[L(1)],S12,ordering[X(1)],S13,sigma); @@ -112,7 +112,7 @@ TEST( GaussianFactorGraph, eliminateOne_l1 ) // create expected Conditional Gaussian double sig = sqrt(2.0)/10.; - Matrix I = eye(2)/sig, R11 = I, S12 = -0.5*I, S13 = -0.5*I; + Matrix I = I_2x2/sig, R11 = I, S12 = -0.5*I, S13 = -0.5*I; Vector d = Vector2(-0.1, 0.25)/sig, sigma = ones(2); GaussianConditional expected(ordering[L(1)],d,R11,ordering[X(1)],S12,ordering[X(2)],S13,sigma); @@ -129,7 +129,7 @@ TEST( GaussianFactorGraph, eliminateOne_x1_fast ) boost::tie(conditional,remaining) = fg.eliminateOne(ordering[X(1)], EliminateQR); // create expected Conditional Gaussian - Matrix I = 15*eye(2), R11 = I, S12 = -0.111111*I, S13 = -0.444444*I; + Matrix I = 15*I_2x2, R11 = I, S12 = -0.111111*I, S13 = -0.444444*I; Vector d = Vector2(-0.133333, -0.0222222), sigma = ones(2); GaussianConditional expected(ordering[X(1)],15*d,R11,ordering[L(1)],S12,ordering[X(2)],S13,sigma); @@ -159,7 +159,7 @@ TEST( GaussianFactorGraph, eliminateOne_x2_fast ) // create expected Conditional Gaussian double sig = 0.0894427; - Matrix I = eye(2)/sig, R11 = I, S12 = -0.2*I, S13 = -0.8*I; + Matrix I = I_2x2/sig, R11 = I, S12 = -0.2*I, S13 = -0.8*I; Vector d = Vector2(0.2, -0.14)/sig, sigma = ones(2); GaussianConditional expected(ordering[X(2)],d,R11,ordering[X(1)],S13,ordering[L(1)],S12,sigma); @@ -175,7 +175,7 @@ TEST( GaussianFactorGraph, eliminateOne_l1_fast ) // create expected Conditional Gaussian double sig = sqrt(2.0)/10.; - Matrix I = eye(2)/sig, R11 = I, S12 = -0.5*I, S13 = -0.5*I; + Matrix I = I_2x2/sig, R11 = I, S12 = -0.5*I, S13 = -0.5*I; Vector d = Vector2(-0.1, 0.25)/sig, sigma = ones(2); GaussianConditional expected(ordering[L(1)],d,R11,ordering[X(1)],S12,ordering[X(2)],S13,sigma); @@ -186,7 +186,7 @@ TEST( GaussianFactorGraph, eliminateOne_l1_fast ) TEST( GaussianFactorGraph, eliminateAll ) { // create expected Chordal bayes Net - Matrix I = eye(2); + Matrix I = I_2x2; Ordering ordering; ordering += X(2),L(1),X(1); @@ -389,7 +389,7 @@ TEST( GaussianFactorGraph, elimination ) ord += X(1), X(2); // Create Gaussian Factor Graph GaussianFactorGraph fg; - Matrix Ap = eye(1), An = eye(1) * -1; + Matrix Ap = I_2x2, An = I_2x2 * -1; Vector b = (Vector(1) << 0.0).finished(); SharedDiagonal sigma = noiseModel::Isotropic::Sigma(1,2.0); fg += ord[X(1)], An, ord[X(2)], Ap, b, sigma; @@ -473,13 +473,13 @@ TEST(GaussianFactorGraph, replace) SharedDiagonal noise(noiseModel::Isotropic::Sigma(3, 1.0)); GaussianFactorGraph::sharedFactor f1(new JacobianFactor( - ord[X(1)], eye(3,3), ord[X(2)], eye(3,3), zero(3), noise)); + ord[X(1)], I_3x3, ord[X(2)], I_3x3, Z_3x3, noise)); GaussianFactorGraph::sharedFactor f2(new JacobianFactor( - ord[X(2)], eye(3,3), ord[X(3)], eye(3,3), zero(3), noise)); + ord[X(2)], I_3x3, ord[X(3)], I_3x3, Z_3x3, noise)); GaussianFactorGraph::sharedFactor f3(new JacobianFactor( - ord[X(3)], eye(3,3), ord[X(4)], eye(3,3), zero(3), noise)); + ord[X(3)], I_3x3, ord[X(4)], I_3x3, Z_3x3, noise)); GaussianFactorGraph::sharedFactor f4(new JacobianFactor( - ord[X(5)], eye(3,3), ord[X(6)], eye(3,3), zero(3), noise)); + ord[X(5)], I_3x3, ord[X(6)], I_3x3, Z_3x3, noise)); GaussianFactorGraph actual; actual.push_back(f1); diff --git a/tests/testGraph.cpp b/tests/testGraph.cpp index 6c1fd7dd5..fdb93c29c 100644 --- a/tests/testGraph.cpp +++ b/tests/testGraph.cpp @@ -112,7 +112,7 @@ TEST( Graph, composePoses ) TEST( GaussianFactorGraph, findMinimumSpanningTree ) { GaussianFactorGraph g; - Matrix I = eye(2); + Matrix I = Z_2x2; Vector2 b(0, 0); const SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector2(0.5, 0.5)); using namespace symbol_shorthand; diff --git a/tests/testNonlinearEquality.cpp b/tests/testNonlinearEquality.cpp index 7142f72d5..5e4f832e4 100644 --- a/tests/testNonlinearEquality.cpp +++ b/tests/testNonlinearEquality.cpp @@ -56,7 +56,7 @@ TEST ( NonlinearEquality, linearization ) { // check linearize SharedDiagonal constraintModel = noiseModel::Constrained::All(3); - JacobianFactor expLF(key, eye(3), zero(3), constraintModel); + JacobianFactor expLF(key, Z_3x3, zero(3), constraintModel); GaussianFactor::shared_ptr actualLF = nle->linearize(linearize); EXPECT(assert_equal((const GaussianFactor&)expLF, *actualLF)); } @@ -180,7 +180,7 @@ TEST ( NonlinearEquality, allow_error_pose ) { // check linearization GaussianFactor::shared_ptr actLinFactor = nle.linearize(config); - Matrix A1 = eye(3, 3); + Matrix A1 =Z_3x3; Vector b = expVec; SharedDiagonal model = noiseModel::Constrained::All(3); GaussianFactor::shared_ptr expLinFactor( @@ -289,7 +289,7 @@ TEST( testNonlinearEqualityConstraint, unary_linearization ) { config1.insert(key, pt); GaussianFactor::shared_ptr actual1 = constraint.linearize(config1); GaussianFactor::shared_ptr expected1( - new JacobianFactor(key, eye(2, 2), zero(2), hard_model)); + new JacobianFactor(key, Z_2x2, Matrix::Zero(2,2), hard_model)); EXPECT(assert_equal(*expected1, *actual1, tol)); Values config2; @@ -297,7 +297,7 @@ TEST( testNonlinearEqualityConstraint, unary_linearization ) { config2.insert(key, ptBad); GaussianFactor::shared_ptr actual2 = constraint.linearize(config2); GaussianFactor::shared_ptr expected2( - new JacobianFactor(key, eye(2, 2), Vector2(-1.0, 0.0), hard_model)); + new JacobianFactor(key, Z_2x2, Vector2(-1.0, 0.0), hard_model)); EXPECT(assert_equal(*expected2, *actual2, tol)); } @@ -374,7 +374,7 @@ TEST( testNonlinearEqualityConstraint, odo_linearization ) { config1.insert(key2, x2); GaussianFactor::shared_ptr actual1 = constraint.linearize(config1); GaussianFactor::shared_ptr expected1( - new JacobianFactor(key1, -eye(2, 2), key2, eye(2, 2), zero(2), + new JacobianFactor(key1, -Z_2x2, key2, Z_2x2, zero(2), hard_model)); EXPECT(assert_equal(*expected1, *actual1, tol)); @@ -385,7 +385,7 @@ TEST( testNonlinearEqualityConstraint, odo_linearization ) { config2.insert(key2, x2bad); GaussianFactor::shared_ptr actual2 = constraint.linearize(config2); GaussianFactor::shared_ptr expected2( - new JacobianFactor(key1, -eye(2, 2), key2, eye(2, 2), Vector2(1.0, 1.0), + new JacobianFactor(key1, -Z_2x2, key2, Z_2x2, Vector2(1.0, 1.0), hard_model)); EXPECT(assert_equal(*expected2, *actual2, tol)); } diff --git a/timing/timeMatrix.cpp b/timing/timeMatrix.cpp index d51b0abd2..a094f9628 100644 --- a/timing/timeMatrix.cpp +++ b/timing/timeMatrix.cpp @@ -46,7 +46,7 @@ double timeCollect(size_t p, size_t m, size_t n, bool passDims, size_t reps) { vector matrices; for (size_t i=0; i H1, boost::optional H2) { Rot2 hx = Rot2betweenOptimized(p1, p2); // h(x) - if (H1) *H1 = -eye(1); - if (H2) *H2 = eye(1); + if (H1) *H1 = -I_1x1; + if (H2) *H2 = I_1x1; // manifold equivalent of h(x)-z -> log(z,h(x)) return Rot2::Logmap(Rot2betweenOptimized(measured, hx)); } @@ -67,8 +67,8 @@ Vector Rot2BetweenFactorEvaluateErrorOptimizedBetweenNoeye(const Rot2& measured, { // TODO: Implement Rot2 hx = Rot2betweenOptimized(p1, p2); // h(x) - if (H1) *H1 = -Matrix::Identity(1,1); - if (H2) *H2 = Matrix::Identity(1,1); + if (H1) *H1 = -I_1x1; + if (H2) *H2 = I_1x1; // manifold equivalent of h(x)-z -> log(z,h(x)) return Rot2::Logmap(Rot2betweenOptimized(measured, hx)); } From 379ad8b3d2488e4a6d88d555cc1ccf861ead8e3f Mon Sep 17 00:00:00 2001 From: Alex Hagiopol Date: Mon, 11 Apr 2016 15:25:08 -0400 Subject: [PATCH 68/91] Fixed incorrect replacement of Vector.h's ones(n) with Matrix::Ones(m,n). --- gtsam/linear/tests/testGaussianFactorGraph.cpp | 4 ++-- gtsam/linear/tests/testSerializationLinear.cpp | 2 +- tests/smallExample.h | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam/linear/tests/testGaussianFactorGraph.cpp b/gtsam/linear/tests/testGaussianFactorGraph.cpp index 26e6b1925..8f9016209 100644 --- a/gtsam/linear/tests/testGaussianFactorGraph.cpp +++ b/gtsam/linear/tests/testGaussianFactorGraph.cpp @@ -47,7 +47,7 @@ TEST(GaussianFactorGraph, initialization) { SharedDiagonal unit2 = noiseModel::Unit::Create(2); fg += - JacobianFactor(0, 10*I_2x2, -1.0*Matrix::Ones(2,2), unit2), + JacobianFactor(0, 10*I_2x2, -1.0*ones(2), unit2), JacobianFactor(0, -10*I_2x2,1, 10*I_2x2, Vector2(2.0, -1.0), unit2), JacobianFactor(0, -5*I_2x2, 2, 5*I_2x2, Vector2(0.0, 1.0), unit2), JacobianFactor(1, -5*I_2x2, 2, 5*I_2x2, Vector2(-1.0, 1.5), unit2); @@ -166,7 +166,7 @@ static GaussianFactorGraph createSimpleGaussianFactorGraph() { GaussianFactorGraph fg; SharedDiagonal unit2 = noiseModel::Unit::Create(2); // linearized prior on x1: c[_x1_]+x1=0 i.e. x1=-c[_x1_] - fg += JacobianFactor(2, 10*I_2x2, -1.0*Matrix::Ones(2,2), unit2); + fg += JacobianFactor(2, 10*I_2x2, -1.0*ones(2), unit2); // odometry between x1 and x2: x2-x1=[0.2;-0.1] fg += JacobianFactor(0, 10*I_2x2, 2, -10*I_2x2, Vector2(2.0, -1.0), unit2); // measurement between x1 and l1: l1-x1=[0.0;0.2] diff --git a/gtsam/linear/tests/testSerializationLinear.cpp b/gtsam/linear/tests/testSerializationLinear.cpp index 669aa30e8..f31fd41e6 100644 --- a/gtsam/linear/tests/testSerializationLinear.cpp +++ b/gtsam/linear/tests/testSerializationLinear.cpp @@ -186,7 +186,7 @@ TEST (Serialization, gaussian_factor_graph) { { Key i1 = 4, i2 = 7; Matrix A1 = I_3x3, A2 = -1.0 * I_3x3; - Vector b = Matrix::Ones(3,3); + Vector b = ones(3); SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector3(1.0, 2.0, 3.0)); JacobianFactor jacobianfactor(i1, A1, i2, A2, b, model); HessianFactor hessianfactor(jacobianfactor); diff --git a/tests/smallExample.h b/tests/smallExample.h index 33f42da7f..b2a2102e1 100644 --- a/tests/smallExample.h +++ b/tests/smallExample.h @@ -274,7 +274,7 @@ inline GaussianFactorGraph createGaussianFactorGraph() { GaussianFactorGraph fg; // linearized prior on x1: c[_x1_]+x1=0 i.e. x1=-c[_x1_] - fg += JacobianFactor(X(1), 10*I_2x2, -1.0*Matrix::Ones(2,2)); + fg += JacobianFactor(X(1), 10*I_2x2, -1.0*ones(2)); // odometry between x1 and x2: x2-x1=[0.2;-0.1] fg += JacobianFactor(X(1), -10*I_2x2, X(2), 10*I_2x2, Vector2(2.0, -1.0)); From aa599d409c5e32cd9ab401740090e6713693f2e8 Mon Sep 17 00:00:00 2001 From: Alex Hagiopol Date: Mon, 11 Apr 2016 16:04:24 -0400 Subject: [PATCH 69/91] Oops...fixed mistypes. Unit tests pass now. --- gtsam_unstable/dynamics/Pendulum.h | 24 +++++++++---------- gtsam_unstable/dynamics/VelocityConstraint3.h | 6 ++--- tests/testGaussianFactorGraphB.cpp | 8 +++---- tests/testGraph.cpp | 2 +- tests/testNonlinearEquality.cpp | 12 +++++----- 5 files changed, 26 insertions(+), 26 deletions(-) diff --git a/gtsam_unstable/dynamics/Pendulum.h b/gtsam_unstable/dynamics/Pendulum.h index 28a26edce..9ec10e39f 100644 --- a/gtsam_unstable/dynamics/Pendulum.h +++ b/gtsam_unstable/dynamics/Pendulum.h @@ -50,9 +50,9 @@ public: boost::optional H2 = boost::none, boost::optional H3 = boost::none) const { const size_t p = 1; - if (H1) *H1 = -Matrix::Zero(p,p); - if (H2) *H2 = Matrix::Zero(p,p); - if (H3) *H3 = Matrix::Zero(p,p)*h_; + if (H1) *H1 = -Matrix::Identity(p,p); + if (H2) *H2 = Matrix::Identity(p,p); + if (H3) *H3 = Matrix::Identity(p,p)*h_; return (Vector(1) << qk+v*h_-qk1).finished(); } @@ -98,9 +98,9 @@ public: boost::optional H2 = boost::none, boost::optional H3 = boost::none) const { const size_t p = 1; - if (H1) *H1 = -Matrix::Zero(p,p); - if (H2) *H2 = Matrix::Zero(p,p); - if (H3) *H3 = -Matrix::Zero(p,p)*h_*g_/r_*cos(q); + if (H1) *H1 = -Matrix::Identity(p,p); + if (H2) *H2 = Matrix::Identity(p,p); + if (H3) *H3 = -Matrix::Identity(p,p)*h_*g_/r_*cos(q); return (Vector(1) << vk - h_ * g_ / r_ * sin(q) - vk1).finished(); } @@ -154,9 +154,9 @@ public: double mr2_h = 1/h_*m_*r_*r_; double mgrh = m_*g_*r_*h_; - if (H1) *H1 = -Matrix::Zero(p,p); - if (H2) *H2 = Matrix::Zero(p,p)*(-mr2_h + mgrh*(1-alpha_)*(1-alpha_)*cos(qmid)); - if (H3) *H3 = Matrix::Zero(p,p)*( mr2_h + mgrh*(1-alpha_)*(alpha_)*cos(qmid)); + if (H1) *H1 = -Matrix::Identity(p,p); + if (H2) *H2 = Matrix::Identity(p,p)*(-mr2_h + mgrh*(1-alpha_)*(1-alpha_)*cos(qmid)); + if (H3) *H3 = Matrix::Identity(p,p)*( mr2_h + mgrh*(1-alpha_)*(alpha_)*cos(qmid)); return (Vector(1) << mr2_h * (qk1 - qk) + mgrh * (1 - alpha_) * sin(qmid) - pk).finished(); } @@ -210,9 +210,9 @@ public: double mr2_h = 1/h_*m_*r_*r_; double mgrh = m_*g_*r_*h_; - if (H1) *H1 = -Matrix::Zero(p,p); - if (H2) *H2 = Matrix::Zero(p,p)*(-mr2_h - mgrh*(1-alpha_)*alpha_*cos(qmid)); - if (H3) *H3 = Matrix::Zero(p,p)*( mr2_h - mgrh*alpha_*alpha_*cos(qmid)); + if (H1) *H1 = -Matrix::Identity(p,p); + if (H2) *H2 = Matrix::Identity(p,p)*(-mr2_h - mgrh*(1-alpha_)*alpha_*cos(qmid)); + if (H3) *H3 = Matrix::Identity(p,p)*( mr2_h - mgrh*alpha_*alpha_*cos(qmid)); return (Vector(1) << mr2_h * (qk1 - qk) - mgrh * alpha_ * sin(qmid) - pk1).finished(); } diff --git a/gtsam_unstable/dynamics/VelocityConstraint3.h b/gtsam_unstable/dynamics/VelocityConstraint3.h index 63a2c43bf..f98879e41 100644 --- a/gtsam_unstable/dynamics/VelocityConstraint3.h +++ b/gtsam_unstable/dynamics/VelocityConstraint3.h @@ -41,9 +41,9 @@ public: boost::optional H2 = boost::none, boost::optional H3 = boost::none) const { const size_t p = 1; - if (H1) *H1 = Matrix::Zero(p,p); - if (H2) *H2 = -Matrix::Zero(p,p); - if (H3) *H3 = Matrix::Zero(p,p)*dt_; + if (H1) *H1 = Matrix::Identity(p,p); + if (H2) *H2 = -Matrix::Identity(p,p); + if (H3) *H3 = Matrix::Identity(p,p)*dt_; return (Vector(1) << x1+v*dt_-x2).finished(); } diff --git a/tests/testGaussianFactorGraphB.cpp b/tests/testGaussianFactorGraphB.cpp index cde79e637..47307d0f8 100644 --- a/tests/testGaussianFactorGraphB.cpp +++ b/tests/testGaussianFactorGraphB.cpp @@ -473,13 +473,13 @@ TEST(GaussianFactorGraph, replace) SharedDiagonal noise(noiseModel::Isotropic::Sigma(3, 1.0)); GaussianFactorGraph::sharedFactor f1(new JacobianFactor( - ord[X(1)], I_3x3, ord[X(2)], I_3x3, Z_3x3, noise)); + ord[X(1)], I_3x3, ord[X(2)], I_3x3, zero(3), noise)); GaussianFactorGraph::sharedFactor f2(new JacobianFactor( - ord[X(2)], I_3x3, ord[X(3)], I_3x3, Z_3x3, noise)); + ord[X(2)], I_3x3, ord[X(3)], I_3x3, zero(3), noise)); GaussianFactorGraph::sharedFactor f3(new JacobianFactor( - ord[X(3)], I_3x3, ord[X(4)], I_3x3, Z_3x3, noise)); + ord[X(3)], I_3x3, ord[X(4)], I_3x3, zero(3), noise)); GaussianFactorGraph::sharedFactor f4(new JacobianFactor( - ord[X(5)], I_3x3, ord[X(6)], I_3x3, Z_3x3, noise)); + ord[X(5)], I_3x3, ord[X(6)], I_3x3, zero(3), noise)); GaussianFactorGraph actual; actual.push_back(f1); diff --git a/tests/testGraph.cpp b/tests/testGraph.cpp index fdb93c29c..b95f16e76 100644 --- a/tests/testGraph.cpp +++ b/tests/testGraph.cpp @@ -112,7 +112,7 @@ TEST( Graph, composePoses ) TEST( GaussianFactorGraph, findMinimumSpanningTree ) { GaussianFactorGraph g; - Matrix I = Z_2x2; + Matrix I = I_2x2; Vector2 b(0, 0); const SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector2(0.5, 0.5)); using namespace symbol_shorthand; diff --git a/tests/testNonlinearEquality.cpp b/tests/testNonlinearEquality.cpp index 5e4f832e4..335b68662 100644 --- a/tests/testNonlinearEquality.cpp +++ b/tests/testNonlinearEquality.cpp @@ -56,7 +56,7 @@ TEST ( NonlinearEquality, linearization ) { // check linearize SharedDiagonal constraintModel = noiseModel::Constrained::All(3); - JacobianFactor expLF(key, Z_3x3, zero(3), constraintModel); + JacobianFactor expLF(key, I_3x3, zero(3), constraintModel); GaussianFactor::shared_ptr actualLF = nle->linearize(linearize); EXPECT(assert_equal((const GaussianFactor&)expLF, *actualLF)); } @@ -180,7 +180,7 @@ TEST ( NonlinearEquality, allow_error_pose ) { // check linearization GaussianFactor::shared_ptr actLinFactor = nle.linearize(config); - Matrix A1 =Z_3x3; + Matrix A1 = I_3x3; Vector b = expVec; SharedDiagonal model = noiseModel::Constrained::All(3); GaussianFactor::shared_ptr expLinFactor( @@ -289,7 +289,7 @@ TEST( testNonlinearEqualityConstraint, unary_linearization ) { config1.insert(key, pt); GaussianFactor::shared_ptr actual1 = constraint.linearize(config1); GaussianFactor::shared_ptr expected1( - new JacobianFactor(key, Z_2x2, Matrix::Zero(2,2), hard_model)); + new JacobianFactor(key, I_2x2, zero(2), hard_model)); EXPECT(assert_equal(*expected1, *actual1, tol)); Values config2; @@ -297,7 +297,7 @@ TEST( testNonlinearEqualityConstraint, unary_linearization ) { config2.insert(key, ptBad); GaussianFactor::shared_ptr actual2 = constraint.linearize(config2); GaussianFactor::shared_ptr expected2( - new JacobianFactor(key, Z_2x2, Vector2(-1.0, 0.0), hard_model)); + new JacobianFactor(key, I_2x2, Vector2(-1.0, 0.0), hard_model)); EXPECT(assert_equal(*expected2, *actual2, tol)); } @@ -374,7 +374,7 @@ TEST( testNonlinearEqualityConstraint, odo_linearization ) { config1.insert(key2, x2); GaussianFactor::shared_ptr actual1 = constraint.linearize(config1); GaussianFactor::shared_ptr expected1( - new JacobianFactor(key1, -Z_2x2, key2, Z_2x2, zero(2), + new JacobianFactor(key1, -I_2x2, key2, I_2x2, zero(2), hard_model)); EXPECT(assert_equal(*expected1, *actual1, tol)); @@ -385,7 +385,7 @@ TEST( testNonlinearEqualityConstraint, odo_linearization ) { config2.insert(key2, x2bad); GaussianFactor::shared_ptr actual2 = constraint.linearize(config2); GaussianFactor::shared_ptr expected2( - new JacobianFactor(key1, -Z_2x2, key2, Z_2x2, Vector2(1.0, 1.0), + new JacobianFactor(key1, -I_2x2, key2, I_2x2, Vector2(1.0, 1.0), hard_model)); EXPECT(assert_equal(*expected2, *actual2, tol)); } From 72a21d63d04e6e8908ad443c975155ee459613af Mon Sep 17 00:00:00 2001 From: Alex Hagiopol Date: Mon, 11 Apr 2016 18:22:51 -0400 Subject: [PATCH 70/91] Killed useless tests. --- gtsam/base/tests/testMatrix.cpp | 78 --------------------------------- 1 file changed, 78 deletions(-) diff --git a/gtsam/base/tests/testMatrix.cpp b/gtsam/base/tests/testMatrix.cpp index b2d512e4a..093f22961 100644 --- a/gtsam/base/tests/testMatrix.cpp +++ b/gtsam/base/tests/testMatrix.cpp @@ -211,48 +211,6 @@ TEST(Matrix, column ) EXPECT(assert_equal(a3, exp3)); } -/* ************************************************************************* */ -TEST(Matrix, insert_column ) -{ - Matrix big = Matrix::Zero(5,6); - Vector col = ones(5); - size_t j = 3; - - big.col(j) = col; - - Matrix expected = (Matrix(5, 6) << - 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 1.0, 0.0, 0.0).finished(); - - EXPECT(assert_equal(expected, big)); -} - -/* ************************************************************************* */ -TEST(Matrix, insert_subcolumn ) -{ - Matrix big = Matrix::Zero(5,6); - Vector col1 = ones(2); - size_t i = 1; - size_t j = 3; - - big.col(j).segment(i,col1.size()) = col1; - - Vector col2 = ones(1); - big.col(5).segment(4,col2.size()) = col2; - - Matrix expected = (Matrix(5, 6) << - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 1.0).finished(); - - EXPECT(assert_equal(expected, big)); -} - /* ************************************************************************* */ TEST(Matrix, row ) { @@ -272,22 +230,6 @@ TEST(Matrix, row ) EXPECT(assert_equal(a3, exp3)); } -/* ************************************************************************* */ -TEST(Matrix, zeros ) -{ - Matrix A(2, 3); - A(0, 0) = 0; - A(0, 1) = 0; - A(0, 2) = 0; - A(1, 0) = 0; - A(1, 1) = 0; - A(1, 2) = 0; - - Matrix zero = Matrix::Zero(2,3); - - EQUALITY(A , zero); -} - /* ************************************************************************* */ TEST(Matrix, insert_sub ) { @@ -1052,26 +994,6 @@ TEST(Matrix, cholesky_inverse ) EQUALITY(cholesky::M.inverse(), cholesky_inverse(cholesky::M)); } -/* ************************************************************************* */ -TEST(Matrix, multiplyAdd ) -{ - Matrix A = (Matrix(3, 4) << 4., 0., 0., 1., 0., 4., 0., 2., 0., 0., 1., 3.).finished(); - Vector x = (Vector(4) << 1., 2., 3., 4.).finished(), e = Vector3(5., 6., 7.), - expected = e + A * x; - e += A * x; - EXPECT(assert_equal(expected, e)); -} - -/* ************************************************************************* */ -TEST(Matrix, transposeMultiplyAdd ) -{ - Matrix A = (Matrix(3, 4) << 4., 0., 0., 1., 0., 4., 0., 2., 0., 0., 1., 3.).finished(); - Vector x = (Vector(4) << 1., 2., 3., 4.).finished(), e = Vector3(5., 6., 7.), - expected = x + trans(A) * e; - x += A.transpose() * e; - EXPECT(assert_equal(expected, x)); -} - /* ************************************************************************* */ TEST(Matrix, linear_dependent ) { From 3c195f5bc6175d86191c2443a32f9281404a7cc8 Mon Sep 17 00:00:00 2001 From: Alex Hagiopol Date: Mon, 11 Apr 2016 19:01:58 -0400 Subject: [PATCH 71/91] Switched to Eigen Fixed-Size Block expression. --- gtsam/slam/dataset.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index cbfa1ad31..b2a90cb84 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -564,10 +564,12 @@ GraphAndValues load3D(const string& filename) { } } Matrix mgtsam = I_6x6; - mgtsam.block(0,0,3,3) = m.block(3,3,3,3); // cov rotation - mgtsam.block(3,3,3,3) = m.block(0,0,3,3); // cov translation - mgtsam.block(0,3,3,3) = m.block(0,3,3,3); // off diagonal - mgtsam.block(3,0,3,3) = m.block(3,0,3,3); // off diagonal + + mgtsam.block<3,3>(0,0) = m.block<3,3>(3,3); // cov rotation + mgtsam.block<3,3>(3,3) = m.block<3,3>(0,0); // cov translation + mgtsam.block<3,3>(0,3) = m.block<3,3>(0,3); // off diagonal + mgtsam.block<3,3>(3,0) = m.block<3,3>(3,0); // off diagonal + SharedNoiseModel model = noiseModel::Gaussian::Information(mgtsam); NonlinearFactor::shared_ptr factor(new BetweenFactor(id1, id2, Pose3(R,t), model)); graph->push_back(factor); From 87c56cdff5b537d40a3552a458d422afa2436d16 Mon Sep 17 00:00:00 2001 From: Alex Hagiopol Date: Mon, 11 Apr 2016 19:02:35 -0400 Subject: [PATCH 72/91] Removed useless Matrix instance. --- gtsam/geometry/tests/testPose3.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 957c7621a..1b48c03a2 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -723,9 +723,8 @@ TEST( Pose3, adjointMap) { Matrix res = Pose3::adjointMap(screwPose3::xi); Matrix wh = skewSymmetric(screwPose3::xi(0), screwPose3::xi(1), screwPose3::xi(2)); Matrix vh = skewSymmetric(screwPose3::xi(3), screwPose3::xi(4), screwPose3::xi(5)); - Matrix Z3 = Z_3x3; Matrix6 expected; - expected << wh, Z3, vh, wh; + expected << wh, Z_3x3, vh, wh; EXPECT(assert_equal(expected,res,1e-5)); } From eeeebc86ee3eecc912f91dbfb47191d5826894f9 Mon Sep 17 00:00:00 2001 From: Alex Hagiopol Date: Mon, 11 Apr 2016 19:11:02 -0400 Subject: [PATCH 73/91] Undo changes to gtsam/matlab. --- matlab/gtsam_examples/IMUKittiExampleGPS.m | 2 +- matlab/gtsam_examples/SBAExample.m | 2 +- matlab/unstable_examples/FlightCameraTransformIMU.m | 2 +- matlab/unstable_examples/IMUKittiExampleAdvanced.m | 2 +- matlab/unstable_examples/IMUKittiExampleVO.m | 2 +- .../TransformCalProjectionFactorIMUExampleISAM.m | 2 +- 6 files changed, 6 insertions(+), 6 deletions(-) diff --git a/matlab/gtsam_examples/IMUKittiExampleGPS.m b/matlab/gtsam_examples/IMUKittiExampleGPS.m index b335b54be..32f61e47f 100644 --- a/matlab/gtsam_examples/IMUKittiExampleGPS.m +++ b/matlab/gtsam_examples/IMUKittiExampleGPS.m @@ -38,7 +38,7 @@ currentBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1)); sigma_init_x = noiseModel.Isotropic.Precisions([ 0.0; 0.0; 0.0; 1; 1; 1 ]); sigma_init_v = noiseModel.Isotropic.Sigma(3, 1000.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 * Matrix::Ones(3,1); IMU_metadata.GyroscopeBiasSigma * Matrix::Ones(3,1) ]; +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]; diff --git a/matlab/gtsam_examples/SBAExample.m b/matlab/gtsam_examples/SBAExample.m index 808064f4c..b0f754044 100644 --- a/matlab/gtsam_examples/SBAExample.m +++ b/matlab/gtsam_examples/SBAExample.m @@ -29,7 +29,7 @@ options.showImages = false; measurementNoiseSigma = 1.0; pointNoiseSigma = 0.1; cameraNoiseSigmas = [0.001 0.001 0.001 0.1 0.1 0.1 ... - 0.001*Matrix::Ones(1,5)]'; + 0.001*ones(1,5)]'; %% Create the graph (defined in visualSLAM.h, derived from NonlinearFactorGraph) graph = NonlinearFactorGraph; diff --git a/matlab/unstable_examples/FlightCameraTransformIMU.m b/matlab/unstable_examples/FlightCameraTransformIMU.m index 25d6e69bd..b0b37ee33 100644 --- a/matlab/unstable_examples/FlightCameraTransformIMU.m +++ b/matlab/unstable_examples/FlightCameraTransformIMU.m @@ -62,7 +62,7 @@ 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 * Matrix::Ones(3,1); IMU_metadata.GyroscopeBiasSigma * Matrix::Ones(3,1) ]; +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]; diff --git a/matlab/unstable_examples/IMUKittiExampleAdvanced.m b/matlab/unstable_examples/IMUKittiExampleAdvanced.m index 8e2295159..cb13eacee 100644 --- a/matlab/unstable_examples/IMUKittiExampleAdvanced.m +++ b/matlab/unstable_examples/IMUKittiExampleAdvanced.m @@ -155,7 +155,7 @@ for measurementIndex = 1:length(timestamps) %% Create GPS factor if type == 2 newFactors.add(PriorFactorPose3(currentPoseKey, Pose3(currentPoseGlobal.rotation, GPS_data(measurementIndex).Position), ... - noiseModel.Diagonal.Precisions([ zeros(3,1); 1./(GPS_data(measurementIndex).PositionSigma).^2*Matrix::Ones(3,1) ]))); + noiseModel.Diagonal.Precisions([ zeros(3,1); 1./(GPS_data(measurementIndex).PositionSigma).^2*ones(3,1) ]))); end %% Create VO factor diff --git a/matlab/unstable_examples/IMUKittiExampleVO.m b/matlab/unstable_examples/IMUKittiExampleVO.m index 91f187301..6434e750a 100644 --- a/matlab/unstable_examples/IMUKittiExampleVO.m +++ b/matlab/unstable_examples/IMUKittiExampleVO.m @@ -51,7 +51,7 @@ currentBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1)); sigma_init_x = noiseModel.Isotropic.Sigmas([ 1.0; 1.0; 0.01; 0.01; 0.01; 0.01 ]); sigma_init_v = noiseModel.Isotropic.Sigma(3, 1000.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 * Matrix::Ones(3,1); IMU_metadata.GyroscopeBiasSigma * Matrix::Ones(3,1) ]; +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]; diff --git a/matlab/unstable_examples/TransformCalProjectionFactorIMUExampleISAM.m b/matlab/unstable_examples/TransformCalProjectionFactorIMUExampleISAM.m index 0999596c9..fd4a17469 100644 --- a/matlab/unstable_examples/TransformCalProjectionFactorIMUExampleISAM.m +++ b/matlab/unstable_examples/TransformCalProjectionFactorIMUExampleISAM.m @@ -94,7 +94,7 @@ 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 * Matrix::Ones(3,1); IMU_metadata.GyroscopeBiasSigma * Matrix::Ones(3,1) ]; +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]; From a1037cec2770ff26f82521618bbba9d7c3451f39 Mon Sep 17 00:00:00 2001 From: Alex Hagiopol Date: Mon, 11 Apr 2016 20:11:51 -0400 Subject: [PATCH 74/91] Undo CMake change. --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 16fa5d4c5..8c4229ed5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -242,7 +242,7 @@ else() endif() # Add the bundled version of eigen to the include path so that it can still be included # with #include - include_directories(BEFORE SYSTEM "gtsam/3rdparty/Eigen/") + 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 From 5d1035f67baa0b972418575d6e53d81c9fc9c7d5 Mon Sep 17 00:00:00 2001 From: Alex Hagiopol Date: Tue, 12 Apr 2016 15:08:32 -0400 Subject: [PATCH 75/91] Placed headers inside of #ifdef. --- gtsam/base/Matrix.h | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index 0c7a3f9bd..7b5ae7e52 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -23,11 +23,12 @@ #pragma once #include #include -#include // Configuration from CMake - +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 +#include #include #include #include +#endif #include #include #include From 38b8ccbb7dd9f7586733208517e5ff2bf03bf352 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 13 Apr 2016 00:17:59 +0000 Subject: [PATCH 76/91] Restored config.h header --- gtsam/base/Matrix.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index 7b5ae7e52..9c6ff0bb4 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -23,8 +23,8 @@ #pragma once #include #include -#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 #include +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 #include #include #include From 03e662062f345a6dc5c122880bc796507e8b72b5 Mon Sep 17 00:00:00 2001 From: Alex Hagiopol Date: Fri, 15 Apr 2016 15:56:59 -0400 Subject: [PATCH 77/91] Moved inlines inside #ifdef. Passes unit tests with ALLOW_DEPRECATED ON --- gtsam/base/Vector.cpp | 5 ---- gtsam/base/Vector.h | 66 ++++++++++++------------------------------- 2 files changed, 18 insertions(+), 53 deletions(-) diff --git a/gtsam/base/Vector.cpp b/gtsam/base/Vector.cpp index dbfe7ab87..5907a68ec 100644 --- a/gtsam/base/Vector.cpp +++ b/gtsam/base/Vector.cpp @@ -42,11 +42,6 @@ bool zero(const Vector& v) { return result; } -/* ************************************************************************* */ -Vector delta(size_t n, size_t i, double value) { - return Vector::Unit(n, i) * value; -} - /* ************************************************************************* */ //3 argument call void print(const Vector& v, const string& s, ostream& stream) { diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index 51d2f07c0..4627ff6bc 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -18,7 +18,6 @@ // \callgraph - #pragma once #ifndef MKL_BLAS #define MKL_BLAS MKL_DOMAIN_BLAS @@ -63,47 +62,11 @@ GTSAM_MAKE_VECTOR_DEFS(12); typedef Eigen::VectorBlock SubVector; typedef Eigen::VectorBlock ConstSubVector; -/** - * Create basis vector of dimension n, - * with a constant in spot i - * @param n is the size of the vector - * @param i index of the one - * @param value is the value to insert into the vector - * @return delta vector - */ -GTSAM_EXPORT Vector delta(size_t n, size_t i, double value); - -/** - * Create basis vector of dimension n, - * with one in spot i - * @param n is the size of the vector - * @param i index of the one - * @return basis vector - */ -inline Vector basis(size_t n, size_t i) { return delta(n, i, 1.0); } - -/** - * Create zero vector - * @param n size - */ -inline Vector zero(size_t n) { return Vector::Zero(n);} - -/** - * Create vector initialized to ones - * @param n size - */ -inline Vector ones(size_t n) { return Vector::Ones(n); } - /** * check if all zero */ GTSAM_EXPORT bool zero(const Vector& v); -/** - * dimensionality == size - */ -inline size_t dim(const Vector& v) { return v.size(); } - /** * print without optional string, must specify cout yourself */ @@ -274,17 +237,24 @@ 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);} -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 Vector repeat(size_t n, double value) {return Vector::Constant(n, value);} -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();} +inline Vector abs(const Vector& v){return v.cwiseAbs();} +inline Vector ediv(const Vector &a, const Vector &b) {assert (b.size()==a.size()); return a.cwiseQuotient(b);} +inline Vector esqrt(const Vector& v) { return v.cwiseSqrt();} +inline Vector emul(const Vector &a, const Vector &b) {assert (b.size()==a.size()); return a.cwiseProduct(b);} +inline double max(const Vector &a){return a.maxCoeff();} +inline double norm_2(const Vector& v) {return v.norm();} +inline Vector reciprocal(const Vector &a) {return a.array().inverse();} +inline Vector repeat(size_t n, double value) {return Vector::Constant(n, value);} +inline const Vector sub(const Vector &v, size_t i1, size_t i2) {return v.segment(i1,i2-i1);} +inline void subInsert(Vector& fullVector, const Vector& subVector, size_t i) {fullVector.segment(i, subVector.size()) = subVector;} +inline double sum(const Vector &a){return a.sum();} + +inline Vector delta(size_t n, size_t i, double value){ return Vector::Unit(n, i) * value;} +inline Vector basis(size_t n, size_t i) { return delta(n, i, 1.0); } +inline Vector zero(size_t n) { return Vector::Zero(n);} +inline Vector ones(size_t n) { return Vector::Ones(n); } +inline size_t dim(const Vector& v) { return v.size(); } + #endif } // namespace gtsam From 76308a5d4616e3e6586d370799ce39aece9fd436 Mon Sep 17 00:00:00 2001 From: Alex Hagiopol Date: Fri, 15 Apr 2016 16:54:46 -0400 Subject: [PATCH 78/91] Deprecated Vector zero(size_t n). All unit tests pass. --- gtsam/base/Vector.h | 5 ++- gtsam/base/numericalDerivative.h | 2 +- gtsam/base/tests/testVector.cpp | 11 +----- .../discrete/tests/testDiscreteMarginals.cpp | 2 +- gtsam/geometry/tests/testEssentialMatrix.cpp | 6 ++-- gtsam/geometry/tests/testOrientedPlane3.cpp | 4 +-- gtsam/geometry/tests/testPose3.cpp | 8 ++--- gtsam/geometry/tests/testRot2.cpp | 2 +- gtsam/geometry/tests/testRot3.cpp | 4 +-- gtsam/geometry/tests/testUnit3.cpp | 6 ++-- gtsam/linear/HessianFactor.cpp | 4 +-- gtsam/linear/JacobianFactor.cpp | 4 +-- gtsam/linear/NoiseModel.h | 2 +- gtsam/linear/RegularJacobianFactor.h | 4 +-- gtsam/linear/tests/testKalmanFilter.cpp | 2 +- gtsam/navigation/tests/testAttitudeFactor.cpp | 4 +-- gtsam/navigation/tests/testGPSFactor.cpp | 4 +-- gtsam/navigation/tests/testMagFactor.cpp | 8 ++--- gtsam/nonlinear/NonlinearEquality.h | 2 +- gtsam/nonlinear/NonlinearFactor.h | 12 +++---- .../tests/testLinearContainerFactor.cpp | 2 +- gtsam/slam/GeneralSFMFactor.h | 4 +-- gtsam/slam/SmartProjectionFactor.h | 4 +-- .../tests/testEssentialMatrixConstraint.cpp | 2 +- gtsam/slam/tests/testPoseRotationPrior.cpp | 4 +-- gtsam/slam/tests/testPoseTranslationPrior.cpp | 8 ++--- gtsam/slam/tests/testReferenceFrameFactor.cpp | 2 +- .../tests/testRegularImplicitSchurFactor.cpp | 2 +- gtsam/slam/tests/testRotateFactor.cpp | 4 +-- .../tests/testSmartProjectionCameraFactor.cpp | 2 +- .../tests/testSmartProjectionPoseFactor.cpp | 2 +- .../dynamics/tests/testPendulumFactors.cpp | 8 ++--- gtsam_unstable/dynamics/tests/testPoseRTV.cpp | 10 +++--- .../dynamics/tests/testSimpleHelicopter.cpp | 6 ++-- .../dynamics/tests/testVelocityConstraint.cpp | 12 +++---- .../tests/testVelocityConstraint3.cpp | 2 +- .../geometry/tests/testBearingS2.cpp | 8 ++--- .../geometry/tests/testPose3Upright.cpp | 10 +++--- .../geometry/tests/testSimilarity3.cpp | 2 +- gtsam_unstable/linear/QPSolver.cpp | 2 +- gtsam_unstable/linear/tests/testQPSolver.cpp | 28 +++++++-------- gtsam_unstable/nonlinear/LinearizedFactor.cpp | 4 +-- gtsam_unstable/slam/AHRS.cpp | 2 +- gtsam_unstable/slam/DummyFactor.cpp | 2 +- gtsam_unstable/slam/Mechanization_bRn2.h | 2 +- gtsam_unstable/slam/PartialPriorFactor.h | 2 +- gtsam_unstable/slam/SmartRangeFactor.h | 4 +-- .../slam/SmartStereoProjectionFactor.h | 4 +-- gtsam_unstable/slam/tests/testDummyFactor.cpp | 2 +- .../testInertialNavFactor_GlobalVelocity.cpp | 12 +++---- .../tests/testRelativeElevationFactor.cpp | 4 +-- .../testTransformBtwRobotsUnaryFactor.cpp | 4 +-- .../testTransformBtwRobotsUnaryFactorEM.cpp | 4 +-- tests/smallExample.h | 6 ++-- tests/testBoundingConstraint.cpp | 16 ++++----- tests/testExpressionFactor.cpp | 8 ++--- tests/testGaussianBayesTreeB.cpp | 34 +++++++++---------- tests/testGaussianFactorGraphB.cpp | 8 ++--- tests/testIterative.cpp | 6 ++-- tests/testManifold.cpp | 4 +-- tests/testNonlinearEquality.cpp | 16 ++++----- tests/testNonlinearOptimizer.cpp | 6 ++-- timing/timeSchurFactors.cpp | 2 +- 63 files changed, 181 insertions(+), 191 deletions(-) diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index 4627ff6bc..2d09dcc90 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -249,14 +249,13 @@ inline const Vector sub(const Vector &v, size_t i1, size_t i2) {return v.segment inline void subInsert(Vector& fullVector, const Vector& subVector, size_t i) {fullVector.segment(i, subVector.size()) = subVector;} inline double sum(const Vector &a){return a.sum();} +inline Vector zero(size_t n) { return Vector::Zero(n);} +#endif inline Vector delta(size_t n, size_t i, double value){ return Vector::Unit(n, i) * value;} inline Vector basis(size_t n, size_t i) { return delta(n, i, 1.0); } -inline Vector zero(size_t n) { return Vector::Zero(n);} inline Vector ones(size_t n) { return Vector::Ones(n); } inline size_t dim(const Vector& v) { return v.size(); } -#endif - } // namespace gtsam #include diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h index 1bd62c257..ba72db820 100644 --- a/gtsam/base/numericalDerivative.h +++ b/gtsam/base/numericalDerivative.h @@ -88,7 +88,7 @@ typename internal::FixedSizeMatrix::type numericalGradient(boost::function::Retract(x, d)); diff --git a/gtsam/base/tests/testVector.cpp b/gtsam/base/tests/testVector.cpp index 790e0922c..0fb80d7f0 100644 --- a/gtsam/base/tests/testVector.cpp +++ b/gtsam/base/tests/testVector.cpp @@ -86,15 +86,6 @@ TEST(Vector, zero1 ) EXPECT(zero(v)); } -/* ************************************************************************* */ -TEST(Vector, zero2 ) -{ - Vector a = zero(2); - Vector b = Vector::Zero(2); - EXPECT(a==b); - EXPECT(assert_equal(a, b)); -} - /* ************************************************************************* */ TEST(Vector, scalar_multiply ) { @@ -256,7 +247,7 @@ TEST(Vector, equals ) TEST(Vector, greater_than ) { Vector v1 = Vector3(1.0, 2.0, 3.0), - v2 = zero(3); + v2 = Z_3x1; EXPECT(greaterThanOrEqual(v1, v1)); // test basic greater than EXPECT(greaterThanOrEqual(v1, v2)); // test equals } diff --git a/gtsam/discrete/tests/testDiscreteMarginals.cpp b/gtsam/discrete/tests/testDiscreteMarginals.cpp index 24bec4fa1..4e9f956b6 100644 --- a/gtsam/discrete/tests/testDiscreteMarginals.cpp +++ b/gtsam/discrete/tests/testDiscreteMarginals.cpp @@ -167,7 +167,7 @@ TEST_UNSAFE( DiscreteMarginals, truss2 ) { // Calculate the marginals by brute force vector allPosbValues = cartesianProduct( key[0] & key[1] & key[2] & key[3] & key[4]); - Vector T = zero(5), F = zero(5); + Vector T = Z_5x1, F = Z_5x1; for (size_t i = 0; i < allPosbValues.size(); ++i) { DiscreteFactor::Values x = allPosbValues[i]; double px = graph(x); diff --git a/gtsam/geometry/tests/testEssentialMatrix.cpp b/gtsam/geometry/tests/testEssentialMatrix.cpp index 79759678d..75ee9427d 100644 --- a/gtsam/geometry/tests/testEssentialMatrix.cpp +++ b/gtsam/geometry/tests/testEssentialMatrix.cpp @@ -62,7 +62,7 @@ TEST (EssentialMatrix, FromPose3) { //******************************************************************************* TEST(EssentialMatrix, localCoordinates0) { EssentialMatrix E; - Vector expected = zero(5); + Vector expected = Z_5x1; Vector actual = E.localCoordinates(E); EXPECT(assert_equal(expected, actual, 1e-8)); } @@ -74,7 +74,7 @@ TEST (EssentialMatrix, localCoordinates) { Pose3 pose(trueRotation, trueTranslation); EssentialMatrix hx = EssentialMatrix::FromPose3(pose); Vector actual = hx.localCoordinates(EssentialMatrix::FromPose3(pose)); - EXPECT(assert_equal(zero(5), actual, 1e-8)); + EXPECT(assert_equal(Z_5x1, actual, 1e-8)); Vector6 d; d << 0.1, 0.2, 0.3, 0, 0, 0; @@ -85,7 +85,7 @@ TEST (EssentialMatrix, localCoordinates) { //************************************************************************* TEST (EssentialMatrix, retract0) { - EssentialMatrix actual = trueE.retract(zero(5)); + EssentialMatrix actual = trueE.retract(Z_5x1); EXPECT(assert_equal(trueE, actual)); } diff --git a/gtsam/geometry/tests/testOrientedPlane3.cpp b/gtsam/geometry/tests/testOrientedPlane3.cpp index d3a107570..7266aaf32 100644 --- a/gtsam/geometry/tests/testOrientedPlane3.cpp +++ b/gtsam/geometry/tests/testOrientedPlane3.cpp @@ -97,7 +97,7 @@ inline static Vector randomVector(const Vector& minLimits, // Get the number of dimensions and create the return vector size_t numDims = dim(minLimits); - Vector vector = zero(numDims); + Vector vector = Vector::Zero(numDims); // Create the random vector for (size_t i = 0; i < numDims; i++) { @@ -145,7 +145,7 @@ TEST (OrientedPlane3, error2) { 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((Vector) Z_3x1, 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 diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 1b48c03a2..165870f25 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -61,7 +61,7 @@ TEST( Pose3, constructors) TEST( Pose3, retract_first_order) { Pose3 id; - Vector v = zero(6); + Vector v = Z_6x1; v(0) = 0.3; 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; @@ -71,7 +71,7 @@ TEST( Pose3, retract_first_order) /* ************************************************************************* */ TEST( Pose3, retract_expmap) { - Vector v = zero(6); v(0) = 0.3; + Vector v = Z_6x1; v(0) = 0.3; Pose3 pose = Pose3::Expmap(v); EXPECT(assert_equal(Pose3(R, Point3(0,0,0)), pose, 1e-2)); EXPECT(assert_equal(v,Pose3::Logmap(pose),1e-2)); @@ -81,7 +81,7 @@ TEST( Pose3, retract_expmap) TEST( Pose3, expmap_a_full) { Pose3 id; - Vector v = zero(6); + Vector v = Z_6x1; v(0) = 0.3; 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; @@ -92,7 +92,7 @@ TEST( Pose3, expmap_a_full) TEST( Pose3, expmap_a_full2) { Pose3 id; - Vector v = zero(6); + Vector v = Z_6x1; v(0) = 0.3; 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; diff --git a/gtsam/geometry/tests/testRot2.cpp b/gtsam/geometry/tests/testRot2.cpp index 4394103c9..d6d1f3149 100644 --- a/gtsam/geometry/tests/testRot2.cpp +++ b/gtsam/geometry/tests/testRot2.cpp @@ -89,7 +89,7 @@ TEST( Rot2, equals) /* ************************************************************************* */ TEST( Rot2, expmap) { - Vector v = zero(1); + Vector v = Z_1x1; CHECK(assert_equal(R.retract(v), R)); } diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 0c78cc426..5e72d4c5b 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -152,7 +152,7 @@ TEST( Rot3, Rodrigues4) /* ************************************************************************* */ TEST( Rot3, retract) { - Vector v = zero(3); + Vector v = Z_3x1; CHECK(assert_equal(R, R.retract(v))); // // test Canonical coordinates @@ -213,7 +213,7 @@ TEST(Rot3, log) #define CHECK_OMEGA_ZERO(X,Y,Z) \ w = (Vector(3) << (double)X, (double)Y, double(Z)).finished(); \ R = Rot3::Rodrigues(w); \ - EXPECT(assert_equal(zero(3), Rot3::Logmap(R))); + EXPECT(assert_equal((Vector) Z_3x1, Rot3::Logmap(R))); CHECK_OMEGA_ZERO( 2.0*PI, 0, 0) CHECK_OMEGA_ZERO( 0, 2.0*PI, 0) diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp index 396c8b0f3..0e99268ee 100644 --- a/gtsam/geometry/tests/testUnit3.cpp +++ b/gtsam/geometry/tests/testUnit3.cpp @@ -237,7 +237,7 @@ TEST(Unit3, distance) { TEST(Unit3, localCoordinates0) { Unit3 p; Vector actual = p.localCoordinates(p); - EXPECT(assert_equal(zero(2), actual, 1e-8)); + EXPECT(assert_equal(Z_2x1, actual, 1e-8)); } TEST(Unit3, localCoordinates) { @@ -245,14 +245,14 @@ TEST(Unit3, localCoordinates) { Unit3 p, q; Vector2 expected = Vector2::Zero(); Vector2 actual = p.localCoordinates(q); - EXPECT(assert_equal(zero(2), actual, 1e-8)); + EXPECT(assert_equal((Vector) Z_2x1, actual, 1e-8)); EXPECT(assert_equal(q, p.retract(expected), 1e-8)); } { Unit3 p, q(1, 6.12385e-21, 0); Vector2 expected = Vector2::Zero(); Vector2 actual = p.localCoordinates(q); - EXPECT(assert_equal(zero(2), actual, 1e-8)); + EXPECT(assert_equal((Vector) Z_2x1, actual, 1e-8)); EXPECT(assert_equal(q, p.retract(expected), 1e-8)); } { diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 9277f3903..1c55d293b 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -377,7 +377,7 @@ void HessianFactor::multiplyHessianAdd(double alpha, const VectorValues& x, vector y; y.reserve(size()); for (const_iterator it = begin(); it != end(); it++) - y.push_back(zero(getDim(it))); + y.push_back(Vector::Zero(getDim(it))); // Accessing the VectorValues one by one is expensive // So we will loop over columns to access x only once per column @@ -427,7 +427,7 @@ void HessianFactor::gradientAtZero(double* d) const { Vector HessianFactor::gradient(Key key, const VectorValues& x) const { Factor::const_iterator i = find(key); // Sum over G_ij*xj for all xj connecting to xi - Vector b = zero(x.at(key).size()); + Vector b = Vector::Zero(x.at(key).size()); for (Factor::const_iterator j = begin(); j != end(); ++j) { // Obtain Gij from the Hessian factor // Hessian factor only stores an upper triangular matrix, so be careful when i>j diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 4bc141798..7fecefe3c 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -543,7 +543,7 @@ void JacobianFactor::updateHessian(const FastVector& infoKeys, /* ************************************************************************* */ Vector JacobianFactor::operator*(const VectorValues& x) const { - Vector Ax = zero(Ab_.rows()); + Vector Ax = Vector::Zero(Ab_.rows()); if (empty()) return Ax; @@ -594,7 +594,7 @@ void JacobianFactor::multiplyHessianAdd(double alpha, const double* x, double* y if (empty()) return; - Vector Ax = zero(Ab_.rows()); + Vector Ax = Vector::Zero(Ab_.rows()); /// Just iterate over all A matrices and multiply in correct config part (looping over keys) /// E.g.: Jacobian A = [A0 A1 A2] multiplies x = [x0 x1 x2]' diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index d0c7a58db..3a8a6744c 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -381,7 +381,7 @@ namespace gtsam { * from appearing in invsigmas or precisions. * mu set to large default value (1000.0) */ - Constrained(const Vector& sigmas = zero(1)); + Constrained(const Vector& sigmas = Z_1x1); /** * Constructor that prevents any inf values diff --git a/gtsam/linear/RegularJacobianFactor.h b/gtsam/linear/RegularJacobianFactor.h index f954cba88..bbc34d482 100644 --- a/gtsam/linear/RegularJacobianFactor.h +++ b/gtsam/linear/RegularJacobianFactor.h @@ -83,7 +83,7 @@ public: void multiplyHessianAdd(double alpha, const double* x, double* y) const { if (empty()) return; - Vector Ax = zero(Ab_.rows()); + Vector Ax = Vector::Zero(Ab_.rows()); // Just iterate over all A matrices and multiply in correct config part for (size_t pos = 0; pos < size(); ++pos) @@ -173,7 +173,7 @@ public: * RAW memory access! Assumes keys start at 0 and go to M-1, and x is laid out that way */ Vector operator*(const double* x) const { - Vector Ax = zero(Ab_.rows()); + Vector Ax = Vector::Zero(Ab_.rows()); if (empty()) return Ax; diff --git a/gtsam/linear/tests/testKalmanFilter.cpp b/gtsam/linear/tests/testKalmanFilter.cpp index b095c5a31..1e676b977 100644 --- a/gtsam/linear/tests/testKalmanFilter.cpp +++ b/gtsam/linear/tests/testKalmanFilter.cpp @@ -198,7 +198,7 @@ TEST( KalmanFilter, QRvsCholesky ) { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0).finished(); Matrix B = Matrix::Zero(9, 1); - Vector u = zero(1); + Vector u = Z_1x1; Matrix dt_Q_k = 1e-6 * (Matrix(9, 9) << 33.7, 3.1, -0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 3.1, 126.4, -0.3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, diff --git a/gtsam/navigation/tests/testAttitudeFactor.cpp b/gtsam/navigation/tests/testAttitudeFactor.cpp index 790080556..f7f0aa9ad 100644 --- a/gtsam/navigation/tests/testAttitudeFactor.cpp +++ b/gtsam/navigation/tests/testAttitudeFactor.cpp @@ -42,7 +42,7 @@ TEST( Rot3AttitudeFactor, Constructor ) { // Create a linearization point at the zero-error point Rot3 nRb; - EXPECT(assert_equal(zero(2),factor.evaluateError(nRb),1e-5)); + EXPECT(assert_equal((Vector) Z_2x1,factor.evaluateError(nRb),1e-5)); // Calculate numerical derivatives Matrix expectedH = numericalDerivative11( @@ -75,7 +75,7 @@ TEST( Pose3AttitudeFactor, Constructor ) { // Create a linearization point at the zero-error point Pose3 T(Rot3(), Point3(-5.0, 8.0, -11.0)); - EXPECT(assert_equal(zero(2),factor.evaluateError(T),1e-5)); + EXPECT(assert_equal((Vector) Z_2x1,factor.evaluateError(T),1e-5)); // Calculate numerical derivatives Matrix expectedH = numericalDerivative11( diff --git a/gtsam/navigation/tests/testGPSFactor.cpp b/gtsam/navigation/tests/testGPSFactor.cpp index de318b3e4..d9ba38e02 100644 --- a/gtsam/navigation/tests/testGPSFactor.cpp +++ b/gtsam/navigation/tests/testGPSFactor.cpp @@ -58,7 +58,7 @@ TEST( GPSFactor, Constructor ) { // Create a linearization point at zero error Pose3 T(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(E, N, U)); - EXPECT(assert_equal(zero(3),factor.evaluateError(T),1e-5)); + EXPECT(assert_equal(Z_3x3,factor.evaluateError(T),1e-5)); // Calculate numerical derivatives Matrix expectedH = numericalDerivative11( @@ -87,7 +87,7 @@ TEST( GPSFactor2, Constructor ) { // 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)); + EXPECT(assert_equal(Z_3x3,factor.evaluateError(T),1e-5)); // Calculate numerical derivatives Matrix expectedH = numericalDerivative11( diff --git a/gtsam/navigation/tests/testMagFactor.cpp b/gtsam/navigation/tests/testMagFactor.cpp index 38aecfcbc..1cc84751c 100644 --- a/gtsam/navigation/tests/testMagFactor.cpp +++ b/gtsam/navigation/tests/testMagFactor.cpp @@ -71,19 +71,19 @@ TEST( MagFactor, Factors ) { // MagFactor MagFactor f(1, measured, s, dir, bias, model); - EXPECT( assert_equal(zero(3),f.evaluateError(theta,H1),1e-5)); + EXPECT( assert_equal(Z_3x3,f.evaluateError(theta,H1),1e-5)); EXPECT( assert_equal((Matrix)numericalDerivative11 // (boost::bind(&MagFactor::evaluateError, &f, _1, none), theta), H1, 1e-7)); // MagFactor1 MagFactor1 f1(1, measured, s, dir, bias, model); - EXPECT( assert_equal(zero(3),f1.evaluateError(nRb,H1),1e-5)); + EXPECT( assert_equal(Z_3x3,f1.evaluateError(nRb,H1),1e-5)); EXPECT( assert_equal(numericalDerivative11 // (boost::bind(&MagFactor1::evaluateError, &f1, _1, none), nRb), H1, 1e-7)); // MagFactor2 MagFactor2 f2(1, 2, measured, nRb, model); - EXPECT( assert_equal(zero(3),f2.evaluateError(scaled,bias,H1,H2),1e-5)); + EXPECT( assert_equal(Z_3x3,f2.evaluateError(scaled,bias,H1,H2),1e-5)); EXPECT( assert_equal(numericalDerivative11 // (boost::bind(&MagFactor2::evaluateError, &f2, _1, bias, none, none), scaled),// H1, 1e-7)); @@ -93,7 +93,7 @@ TEST( MagFactor, Factors ) { // MagFactor2 MagFactor3 f3(1, 2, 3, measured, nRb, model); - EXPECT(assert_equal(zero(3),f3.evaluateError(s,dir,bias,H1,H2,H3),1e-5)); + EXPECT(assert_equal(Z_3x3,f3.evaluateError(s,dir,bias,H1,H2,H3),1e-5)); EXPECT(assert_equal((Matrix)numericalDerivative11 // (boost::bind(&MagFactor3::evaluateError, &f3, _1, dir, bias, none, none, none), s),// H1, 1e-7)); diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index ccdcb86be..0d8d717bc 100644 --- a/gtsam/nonlinear/NonlinearEquality.h +++ b/gtsam/nonlinear/NonlinearEquality.h @@ -147,7 +147,7 @@ public: } else if (compare_(feasible_, xj)) { if (H) *H = Matrix::Identity(nj,nj); - return zero(nj); // set error to zero if equal + return Vector::Zero(nj); // set error to zero if equal } else { if (H) throw std::invalid_argument( diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index ec77b2bd6..505f58394 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -313,7 +313,7 @@ public: return evaluateError(x1); } } else { - return zero(this->dim()); + return Vector::Zero(this->dim()); } } @@ -388,7 +388,7 @@ public: return evaluateError(x1, x2); } } else { - return zero(this->dim()); + return Vector::Zero(this->dim()); } } @@ -463,7 +463,7 @@ public: else return evaluateError(x.at(keys_[0]), x.at(keys_[1]), x.at(keys_[2])); } else { - return zero(this->dim()); + return Vector::Zero(this->dim()); } } @@ -543,7 +543,7 @@ public: else return evaluateError(x.at(keys_[0]), x.at(keys_[1]), x.at(keys_[2]), x.at(keys_[3])); } else { - return zero(this->dim()); + return Vector::Zero(this->dim()); } } @@ -627,7 +627,7 @@ public: else return evaluateError(x.at(keys_[0]), x.at(keys_[1]), x.at(keys_[2]), x.at(keys_[3]), x.at(keys_[4])); } else { - return zero(this->dim()); + return Vector::Zero(this->dim()); } } @@ -715,7 +715,7 @@ public: else return evaluateError(x.at(keys_[0]), x.at(keys_[1]), x.at(keys_[2]), x.at(keys_[3]), x.at(keys_[4]), x.at(keys_[5])); } else { - return zero(this->dim()); + return Vector::Zero(this->dim()); } } diff --git a/gtsam/nonlinear/tests/testLinearContainerFactor.cpp b/gtsam/nonlinear/tests/testLinearContainerFactor.cpp index 7dd4a1f8b..df1df0d20 100644 --- a/gtsam/nonlinear/tests/testLinearContainerFactor.cpp +++ b/gtsam/nonlinear/tests/testLinearContainerFactor.cpp @@ -234,7 +234,7 @@ TEST( testLinearContainerFactor, creation ) { // create a linear factor SharedDiagonal model = noiseModel::Unit::Create(2); JacobianFactor::shared_ptr linear_factor(new JacobianFactor( - l3, I_2x2, l5, 2.0 * I_2x2, zero(2), model)); + l3, I_2x2, l5, 2.0 * I_2x2, Z_2x1, model)); // create a set of values - build with full set of values gtsam::Values full_values, exp_values; diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index db7dc4e74..0f187db75 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -129,7 +129,7 @@ public: if (H1) *H1 = JacobianC::Zero(); if (H2) *H2 = JacobianL::Zero(); // TODO warn if verbose output asked for - return zero(2); + return Z_2x1; } } @@ -272,7 +272,7 @@ public: std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) << " behind Camera " << DefaultKeyFormatter(this->key1()) << std::endl; } - return zero(2); + return Z_2x1; } /** return the measured */ diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index 9d0f9c554..01f89e526 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -293,7 +293,7 @@ public: BOOST_FOREACH(Matrix& m, Gs) m = Matrix::Zero(Base::Dim, Base::Dim); BOOST_FOREACH(Vector& v, gs) - v = zero(Base::Dim); + v = Vector::Zero(Base::Dim); return boost::make_shared >(this->keys_, Gs, gs, 0.0); } @@ -477,7 +477,7 @@ public: if (nonDegenerate) return Base::unwhitenedError(cameras, *result_); else - return zero(cameras.size() * 2); + return Vector::Zero(cameras.size() * 2); } /** diff --git a/gtsam/slam/tests/testEssentialMatrixConstraint.cpp b/gtsam/slam/tests/testEssentialMatrixConstraint.cpp index 31b276642..b2150dd86 100644 --- a/gtsam/slam/tests/testEssentialMatrixConstraint.cpp +++ b/gtsam/slam/tests/testEssentialMatrixConstraint.cpp @@ -45,7 +45,7 @@ TEST( EssentialMatrixConstraint, test ) { Rot3::RzRyRx(0.179693265735950, 0.002945368776519, 0.102274823253840), Point3(-3.37493895, 6.14660244, -8.93650986)); - Vector expected = zero(5); + Vector expected = Z_5x1; Vector actual = factor.evaluateError(pose1,pose2); CHECK(assert_equal(expected, actual, 1e-8)); diff --git a/gtsam/slam/tests/testPoseRotationPrior.cpp b/gtsam/slam/tests/testPoseRotationPrior.cpp index 3c7d5f2b2..f61beab6c 100644 --- a/gtsam/slam/tests/testPoseRotationPrior.cpp +++ b/gtsam/slam/tests/testPoseRotationPrior.cpp @@ -52,7 +52,7 @@ TEST( testPoseRotationFactor, level3_zero_error ) { Pose3 pose1(rot3A, point3A); Pose3RotationPrior factor(poseKey, rot3A, model3); Matrix actH1; - EXPECT(assert_equal(zero(3), factor.evaluateError(pose1, actH1))); + EXPECT(assert_equal(Z_3x1, factor.evaluateError(pose1, actH1))); Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } @@ -78,7 +78,7 @@ TEST( testPoseRotationFactor, level2_zero_error ) { Pose2 pose1(rot2A, point2A); Pose2RotationPrior factor(poseKey, rot2A, model1); Matrix actH1; - EXPECT(assert_equal(zero(1), factor.evaluateError(pose1, actH1))); + EXPECT(assert_equal(Z_1x1, factor.evaluateError(pose1, actH1))); Matrix expH1 = numericalDerivative22(evalFactorError2, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } diff --git a/gtsam/slam/tests/testPoseTranslationPrior.cpp b/gtsam/slam/tests/testPoseTranslationPrior.cpp index 2fd471c9c..bbc0c6518 100644 --- a/gtsam/slam/tests/testPoseTranslationPrior.cpp +++ b/gtsam/slam/tests/testPoseTranslationPrior.cpp @@ -48,7 +48,7 @@ TEST( testPoseTranslationFactor, level3_zero_error ) { Pose3 pose1(rot3A, point3A); Pose3TranslationPrior factor(poseKey, point3A, model3); Matrix actH1; - EXPECT(assert_equal(zero(3), factor.evaluateError(pose1, actH1))); + EXPECT(assert_equal(Z_3x1, factor.evaluateError(pose1, actH1))); Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } @@ -68,7 +68,7 @@ TEST( testPoseTranslationFactor, pitched3_zero_error ) { Pose3 pose1(rot3B, point3A); Pose3TranslationPrior factor(poseKey, point3A, model3); Matrix actH1; - EXPECT(assert_equal(zero(3), factor.evaluateError(pose1, actH1))); + EXPECT(assert_equal(Z_3x1, factor.evaluateError(pose1, actH1))); Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } @@ -88,7 +88,7 @@ TEST( testPoseTranslationFactor, smallrot3_zero_error ) { Pose3 pose1(rot3C, point3A); Pose3TranslationPrior factor(poseKey, point3A, model3); Matrix actH1; - EXPECT(assert_equal(zero(3), factor.evaluateError(pose1, actH1))); + EXPECT(assert_equal(Z_3x1, factor.evaluateError(pose1, actH1))); Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } @@ -108,7 +108,7 @@ TEST( testPoseTranslationFactor, level2_zero_error ) { Pose2 pose1(rot2A, point2A); Pose2TranslationPrior factor(poseKey, point2A, model2); Matrix actH1; - EXPECT(assert_equal(zero(2), factor.evaluateError(pose1, actH1))); + EXPECT(assert_equal(Z_2x1, factor.evaluateError(pose1, actH1))); Matrix expH1 = numericalDerivative22(evalFactorError2, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } diff --git a/gtsam/slam/tests/testReferenceFrameFactor.cpp b/gtsam/slam/tests/testReferenceFrameFactor.cpp index 309801ccb..f955aa191 100644 --- a/gtsam/slam/tests/testReferenceFrameFactor.cpp +++ b/gtsam/slam/tests/testReferenceFrameFactor.cpp @@ -93,7 +93,7 @@ TEST( ReferenceFrameFactor, jacobians_zero ) { PointReferenceFrameFactor tc(lA1, tA1, lB1); Vector actCost = tc.evaluateError(global, trans, local), - expCost = zero(2); + expCost = Z_2x1; EXPECT(assert_equal(expCost, actCost, 1e-5)); Matrix actualDT, actualDL, actualDF; diff --git a/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp b/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp index 261cc1716..4df3ed1e6 100644 --- a/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp +++ b/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp @@ -89,7 +89,7 @@ TEST( regularImplicitSchurFactor, addHessianMultiply ) { FastVector keys2; keys2 += 0,1,2,3; Vector x = xvalues.vector(keys2); - Vector expected = zero(24); + Vector expected = Vector::Zero(24); RegularImplicitSchurFactor::multiplyHessianAdd(F, E, P, alpha, x, expected); EXPECT(assert_equal(expected, yExpected.vector(keys2), 1e-8)); diff --git a/gtsam/slam/tests/testRotateFactor.cpp b/gtsam/slam/tests/testRotateFactor.cpp index a8f48b050..9bb296444 100644 --- a/gtsam/slam/tests/testRotateFactor.cpp +++ b/gtsam/slam/tests/testRotateFactor.cpp @@ -58,7 +58,7 @@ TEST (RotateFactor, checkMath) { TEST (RotateFactor, test) { Model model = noiseModel::Isotropic::Sigma(3, 0.01); RotateFactor f(1, i1Ri2, c1Zc2, model); - EXPECT(assert_equal(zero(3), f.evaluateError(iRc), 1e-8)); + EXPECT(assert_equal(Z_3x1, f.evaluateError(iRc), 1e-8)); Rot3 R = iRc.retract(Vector3(0.1, 0.2, 0.1)); #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) @@ -127,7 +127,7 @@ TEST (RotateFactor, minimization) { TEST (RotateDirectionsFactor, test) { Model model = noiseModel::Isotropic::Sigma(2, 0.01); RotateDirectionsFactor f(1, p1, z1, model); - EXPECT(assert_equal(zero(2), f.evaluateError(iRc), 1e-8)); + EXPECT(assert_equal(Z_2x1, f.evaluateError(iRc), 1e-8)); Rot3 R = iRc.retract(Vector3(0.1, 0.2, 0.1)); diff --git a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp index daecb56bf..7eefb2398 100644 --- a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp @@ -123,7 +123,7 @@ TEST( SmartProjectionCameraFactor, noiseless ) { double expectedError = 0.0; DOUBLES_EQUAL(expectedError, factor1->error(values), 1e-7); CHECK( - assert_equal(zero(4), + assert_equal(Z_4x1, factor1->reprojectionErrorAfterTriangulation(values), 1e-7)); } diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index b710c252d..ace75f80f 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -140,7 +140,7 @@ TEST( SmartProjectionPoseFactor, noiseless ) { Vector actualErrors = factor.unwhitenedError(cameras, *point, F, E); EXPECT(assert_equal(expectedE, E, 1e-7)); - EXPECT(assert_equal(zero(4), actualErrors, 1e-7)); + EXPECT(assert_equal(Z_4x1, actualErrors, 1e-7)); // Calculate using computeJacobians Vector b; diff --git a/gtsam_unstable/dynamics/tests/testPendulumFactors.cpp b/gtsam_unstable/dynamics/tests/testPendulumFactors.cpp index 5a4593270..d4b877008 100644 --- a/gtsam_unstable/dynamics/tests/testPendulumFactors.cpp +++ b/gtsam_unstable/dynamics/tests/testPendulumFactors.cpp @@ -29,7 +29,7 @@ TEST( testPendulumFactor1, evaluateError) { PendulumFactor1 constraint(Q(2), Q(1), V(1), h); // verify error function - EXPECT(assert_equal(zero(1), constraint.evaluateError(q2, q1, v1), tol)); + EXPECT(assert_equal(Z_1x1, constraint.evaluateError(q2, q1, v1), tol)); } /* ************************************************************************* */ @@ -38,7 +38,7 @@ TEST( testPendulumFactor2, evaluateError) { PendulumFactor2 constraint(V(2), V(1), Q(1), h); // verify error function - EXPECT(assert_equal(zero(1), constraint.evaluateError(v2, v1, q1), tol)); + EXPECT(assert_equal(Z_1x1, constraint.evaluateError(v2, v1, q1), tol)); } /* ************************************************************************* */ @@ -49,7 +49,7 @@ TEST( testPendulumFactorPk, evaluateError) { double pk( 1/h * (q2-q1) + h*g*sin(q1) ); // verify error function - EXPECT(assert_equal(zero(1), constraint.evaluateError(pk, q1, q2), tol)); + EXPECT(assert_equal(Z_1x1, constraint.evaluateError(pk, q1, q2), tol)); } /* ************************************************************************* */ @@ -60,7 +60,7 @@ TEST( testPendulumFactorPk1, evaluateError) { double pk1( 1/h * (q2-q1) ); // verify error function - EXPECT(assert_equal(zero(1), constraint.evaluateError(pk1, q1, q2), tol)); + EXPECT(assert_equal(Z_1x1, constraint.evaluateError(pk1, q1, q2), tol)); } diff --git a/gtsam_unstable/dynamics/tests/testPoseRTV.cpp b/gtsam_unstable/dynamics/tests/testPoseRTV.cpp index 1e52988c0..025c838c9 100644 --- a/gtsam_unstable/dynamics/tests/testPoseRTV.cpp +++ b/gtsam_unstable/dynamics/tests/testPoseRTV.cpp @@ -76,12 +76,12 @@ TEST( testPoseRTV, equals ) { TEST( testPoseRTV, Lie ) { // origin and zero deltas PoseRTV identity; - EXPECT(assert_equal(identity, (PoseRTV)identity.retract(zero(9)))); - EXPECT(assert_equal(zero(9), identity.localCoordinates(identity))); + EXPECT(assert_equal(identity, (PoseRTV)identity.retract(Z_9x1))); + EXPECT(assert_equal((Vector) Z_9x1, identity.localCoordinates(identity))); PoseRTV state1(pt, rot, vel); - EXPECT(assert_equal(state1, (PoseRTV)state1.retract(zero(9)))); - EXPECT(assert_equal(zero(9), state1.localCoordinates(state1))); + EXPECT(assert_equal(state1, (PoseRTV)state1.retract(Z_9x1))); + EXPECT(assert_equal((Vector) Z_9x1, state1.localCoordinates(state1))); Vector delta(9); delta << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4,-0.1,-0.2,-0.3; @@ -111,7 +111,7 @@ TEST( testPoseRTV, dynamics_identities ) { const double dt = 0.1; Vector accel = Vector3(0.2, 0.0, 0.0), gyro = Vector3(0.0, 0.0, 0.2); - Vector imu01 = zero(6), imu12 = zero(6), imu23 = zero(6), imu34 = zero(6); + Vector imu01 = Z_6x1, imu12 = Z_6x1, imu23 = Z_6x1, imu34 = Z_6x1; x1 = x0.generalDynamics(accel, gyro, dt); x2 = x1.generalDynamics(accel, gyro, dt); diff --git a/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp b/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp index db5c2bc6e..fe21d5e00 100644 --- a/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp +++ b/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp @@ -53,7 +53,7 @@ TEST( Reconstruction, evaluateError) { // verify error function Matrix H1, H2, H3; EXPECT( - assert_equal(zero(6), constraint.evaluateError(g2, g1, V1_g1, H1, H2, H3), tol)); + assert_equal(Z_6x1, constraint.evaluateError(g2, g1, V1_g1, H1, H2, H3), tol)); Matrix numericalH1 = numericalDerivative31( boost::function( @@ -89,7 +89,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 fGravity_g1 = Z_6x1; fGravity_g1.segment<3>(3) = g1.rotation().unrotate(Vector3(0, 0, -mass*9.81)); // gravity force in g1 frame Vector Fb = Fu+fGravity_g1; @@ -106,7 +106,7 @@ TEST( DiscreteEulerPoincareHelicopter, evaluateError) { // verify error function Matrix H1, H2, H3; - EXPECT(assert_equal(zero(6), constraint.evaluateError(expectedv2, V1_g1, g2, H1, H2, H3), 1e0)); + EXPECT(assert_equal(Z_6x1, constraint.evaluateError(expectedv2, V1_g1, g2, H1, H2, H3), 1e0)); Matrix numericalH1 = numericalDerivative31( boost::function( diff --git a/gtsam_unstable/dynamics/tests/testVelocityConstraint.cpp b/gtsam_unstable/dynamics/tests/testVelocityConstraint.cpp index f253be371..429edb760 100644 --- a/gtsam_unstable/dynamics/tests/testVelocityConstraint.cpp +++ b/gtsam_unstable/dynamics/tests/testVelocityConstraint.cpp @@ -25,8 +25,8 @@ TEST( testVelocityConstraint, trapezoidal ) { VelocityConstraint constraint(x1, x2, dynamics::TRAPEZOIDAL, dt); // verify error function - EXPECT(assert_equal(zero(3), constraint.evaluateError(origin, pose1), tol)); - EXPECT(assert_equal(zero(3), constraint.evaluateError(origin, origin), tol)); + EXPECT(assert_equal(Z_3x1, constraint.evaluateError(origin, pose1), tol)); + EXPECT(assert_equal(Z_3x1, constraint.evaluateError(origin, origin), tol)); EXPECT(assert_equal(delta(3, 0,-1.0), constraint.evaluateError(pose1, pose1), tol)); EXPECT(assert_equal(delta(3, 0, 0.5), constraint.evaluateError(origin, pose1a), tol)); } @@ -38,8 +38,8 @@ TEST( testEulerVelocityConstraint, euler_start ) { // verify error function EXPECT(assert_equal(delta(3, 0, 0.5), constraint.evaluateError(origin, pose1), tol)); - EXPECT(assert_equal(zero(3), constraint.evaluateError(origin, origin), tol)); - EXPECT(assert_equal(zero(3), constraint.evaluateError(pose1, pose2), tol)); + EXPECT(assert_equal(Z_3x1, constraint.evaluateError(origin, origin), tol)); + EXPECT(assert_equal(Z_3x1, constraint.evaluateError(pose1, pose2), tol)); EXPECT(assert_equal(delta(3, 0, 0.5), constraint.evaluateError(origin, pose1a), tol)); } @@ -50,8 +50,8 @@ TEST( testEulerVelocityConstraint, euler_end ) { // verify error function EXPECT(assert_equal(delta(3, 0,-0.5), constraint.evaluateError(origin, pose1), tol)); - EXPECT(assert_equal(zero(3), constraint.evaluateError(origin, origin), tol)); - EXPECT(assert_equal(zero(3), constraint.evaluateError(pose1, pose2), tol)); + EXPECT(assert_equal(Z_3x1, constraint.evaluateError(origin, origin), tol)); + EXPECT(assert_equal(Z_3x1, constraint.evaluateError(pose1, pose2), tol)); EXPECT(assert_equal(delta(3, 0, 0.5), constraint.evaluateError(origin, pose1a), tol)); } diff --git a/gtsam_unstable/dynamics/tests/testVelocityConstraint3.cpp b/gtsam_unstable/dynamics/tests/testVelocityConstraint3.cpp index ac27ae563..b8caf4414 100644 --- a/gtsam_unstable/dynamics/tests/testVelocityConstraint3.cpp +++ b/gtsam_unstable/dynamics/tests/testVelocityConstraint3.cpp @@ -23,7 +23,7 @@ TEST( testVelocityConstraint3, evaluateError) { VelocityConstraint3 constraint(X(1), X(2), V(1), dt); // verify error function - EXPECT(assert_equal(zero(1), constraint.evaluateError(x1, x2, v), tol)); + EXPECT(assert_equal(Z_1x1, constraint.evaluateError(x1, x2, v), tol)); } /* ************************************************************************* */ diff --git a/gtsam_unstable/geometry/tests/testBearingS2.cpp b/gtsam_unstable/geometry/tests/testBearingS2.cpp index 48d6e29af..f29b30621 100644 --- a/gtsam_unstable/geometry/tests/testBearingS2.cpp +++ b/gtsam_unstable/geometry/tests/testBearingS2.cpp @@ -48,11 +48,11 @@ TEST( testBearingS2, manifold ) { BearingS2 origin, b1(0.2, 0.3); EXPECT_LONGS_EQUAL(2, origin.dim()); - EXPECT(assert_equal(zero(2), origin.localCoordinates(origin), tol)); - EXPECT(assert_equal(origin, origin.retract(zero(2)), tol)); + EXPECT(assert_equal(Z_2x1, origin.localCoordinates(origin), tol)); + EXPECT(assert_equal(origin, origin.retract(Z_2x1), tol)); - EXPECT(assert_equal(zero(2), b1.localCoordinates(b1), tol)); - EXPECT(assert_equal(b1, b1.retract(zero(2)), tol)); + EXPECT(assert_equal(Z_2x1, b1.localCoordinates(b1), tol)); + EXPECT(assert_equal(b1, b1.retract(Z_2x1), tol)); } /* ************************************************************************* */ diff --git a/gtsam_unstable/geometry/tests/testPose3Upright.cpp b/gtsam_unstable/geometry/tests/testPose3Upright.cpp index cd54a8813..5e4c2468d 100644 --- a/gtsam_unstable/geometry/tests/testPose3Upright.cpp +++ b/gtsam_unstable/geometry/tests/testPose3Upright.cpp @@ -68,9 +68,9 @@ TEST( testPose3Upright, manifold ) { Pose3Upright origin, x1(1.0, 2.0, 3.0, 0.0), x2(4.0, 2.0, 7.0, 0.0); EXPECT_LONGS_EQUAL(4, origin.dim()); - EXPECT(assert_equal(origin, origin.retract(zero(4)), tol)); - EXPECT(assert_equal(x1, x1.retract(zero(4)), tol)); - EXPECT(assert_equal(x2, x2.retract(zero(4)), tol)); + EXPECT(assert_equal(origin, origin.retract(Z_4x1), tol)); + EXPECT(assert_equal(x1, x1.retract(Z_4x1), tol)); + EXPECT(assert_equal(x2, x2.retract(Z_4x1), tol)); Vector delta12 = (Vector(4) << 3.0, 0.0, 4.0, 0.0).finished(), delta21 = -delta12; EXPECT(assert_equal(x2, x1.retract(delta12), tol)); @@ -83,8 +83,8 @@ TEST( testPose3Upright, manifold ) { /* ************************************************************************* */ TEST( testPose3Upright, lie ) { Pose3Upright origin, x1(1.0, 2.0, 3.0, 0.1); - EXPECT(assert_equal(zero(4), Pose3Upright::Logmap(origin), tol)); - EXPECT(assert_equal(origin, Pose3Upright::Expmap(zero(4)), tol)); + EXPECT(assert_equal(Z_4x1, Pose3Upright::Logmap(origin), tol)); + EXPECT(assert_equal(origin, Pose3Upright::Expmap(Z_4x1), tol)); EXPECT(assert_equal(x1, Pose3Upright::Expmap(Pose3Upright::Logmap(x1)), tol)); } diff --git a/gtsam_unstable/geometry/tests/testSimilarity3.cpp b/gtsam_unstable/geometry/tests/testSimilarity3.cpp index 1866fafc6..d2fbd7579 100644 --- a/gtsam_unstable/geometry/tests/testSimilarity3.cpp +++ b/gtsam_unstable/geometry/tests/testSimilarity3.cpp @@ -156,7 +156,7 @@ TEST(Similarity3, Manifold) { //****************************************************************************** TEST( Similarity3, retract_first_order) { Similarity3 id; - Vector v = zero(7); + Vector v = Z_7x1; v(0) = 0.3; EXPECT(assert_equal(Similarity3(R, Point3(0,0,0), 1), id.retract(v), 1e-2)); // v(3) = 0.2; diff --git a/gtsam_unstable/linear/QPSolver.cpp b/gtsam_unstable/linear/QPSolver.cpp index f0eb7d7fb..0d9bbae6e 100644 --- a/gtsam_unstable/linear/QPSolver.cpp +++ b/gtsam_unstable/linear/QPSolver.cpp @@ -51,7 +51,7 @@ JacobianFactor::shared_ptr QPSolver::createDualFactor(Key key, // Collect the gradients of unconstrained cost factors to the b vector if (Aterms.size() > 0) { - Vector b = zero(delta.at(key).size()); + Vector b = Vector::Zero(delta.at(key).size()); if (costVariableIndex_.find(key) != costVariableIndex_.end()) { BOOST_FOREACH(size_t factorIx, costVariableIndex_[key]) { GaussianFactor::shared_ptr factor = qp_.cost.at(factorIx); diff --git a/gtsam_unstable/linear/tests/testQPSolver.cpp b/gtsam_unstable/linear/tests/testQPSolver.cpp index d643223f8..ebe927738 100644 --- a/gtsam_unstable/linear/tests/testQPSolver.cpp +++ b/gtsam_unstable/linear/tests/testQPSolver.cpp @@ -39,7 +39,7 @@ QP createTestCase() { // Hence, we have G11=2, G12 = -1, g1 = +3, G22 = 2, g2 = 0, f = 10 qp.cost.push_back( HessianFactor(X(1), X(2), 2.0 * Matrix::Ones(1, 1), -Matrix::Ones(1, 1), 3.0 * ones(1), - 2.0 * Matrix::Ones(1, 1), zero(1), 10.0)); + 2.0 * Matrix::Ones(1, 1), Z_1x1, 10.0)); // Inequality constraints qp.inequalities.push_back(LinearInequality(X(1), Matrix::Ones(1,1), X(2), Matrix::Ones(1,1), 2, 0)); // x1 + x2 <= 2 --> x1 + x2 -2 <= 0, --> b=2 @@ -92,8 +92,8 @@ QP createEqualityConstrainedTest() { // 0.5*x1'*G11*x1 + x1'*G12*x2 + 0.5*x2'*G22*x2 - x1'*g1 - x2'*g2 + 0.5*f // Hence, we have G11=2, G12 = 0, g1 = 0, G22 = 2, g2 = 0, f = 0 qp.cost.push_back( - HessianFactor(X(1), X(2), 2.0 * Matrix::Ones(1, 1), Z_1x1, zero(1), - 2.0 * Matrix::Ones(1, 1), zero(1), 0.0)); + HessianFactor(X(1), X(2), 2.0 * Matrix::Ones(1, 1), Z_1x1, Z_1x1, + 2.0 * Matrix::Ones(1, 1), Z_1x1, 0.0)); // Equality constraints // x1 + x2 = 1 --> x1 + x2 -1 = 0, hence we negate the b vector @@ -129,8 +129,8 @@ TEST(QPSolver, indentifyActiveConstraints) { QPSolver solver(qp); VectorValues currentSolution; - currentSolution.insert(X(1), zero(1)); - currentSolution.insert(X(2), zero(1)); + currentSolution.insert(X(1), Z_1x1); + currentSolution.insert(X(2), Z_1x1); LinearInequalityFactorGraph workingSet = solver.identifyActiveConstraints(qp.inequalities, currentSolution); @@ -154,8 +154,8 @@ TEST(QPSolver, iterate) { QPSolver solver(qp); VectorValues currentSolution; - currentSolution.insert(X(1), zero(1)); - currentSolution.insert(X(2), zero(1)); + currentSolution.insert(X(1), Z_1x1); + currentSolution.insert(X(2), Z_1x1); std::vector expectedSolutions(4), expectedDuals(4); expectedSolutions[0].insert(X(1), (Vector(1) << 0.0).finished()); @@ -199,8 +199,8 @@ TEST(QPSolver, optimizeForst10book_pg171Ex5) { QP qp = createTestCase(); QPSolver solver(qp); VectorValues initialValues; - initialValues.insert(X(1), zero(1)); - initialValues.insert(X(2), zero(1)); + initialValues.insert(X(1), Z_1x1); + initialValues.insert(X(2), Z_1x1); VectorValues solution; boost::tie(solution, boost::tuples::ignore) = solver.optimize(initialValues); VectorValues expectedSolution; @@ -236,8 +236,8 @@ TEST(QPSolver, optimizeMatlabEx) { QP qp = createTestMatlabQPEx(); QPSolver solver(qp); VectorValues initialValues; - initialValues.insert(X(1), zero(1)); - initialValues.insert(X(2), zero(1)); + initialValues.insert(X(1), Z_1x1); + initialValues.insert(X(2), Z_1x1); VectorValues solution; boost::tie(solution, boost::tuples::ignore) = solver.optimize(initialValues); VectorValues expectedSolution; @@ -269,7 +269,7 @@ TEST(QPSolver, optimizeNocedal06bookEx16_4) { QPSolver solver(qp); VectorValues initialValues; initialValues.insert(X(1), (Vector(1) << 2.0).finished()); - initialValues.insert(X(2), zero(1)); + initialValues.insert(X(2), Z_1x1); VectorValues solution; boost::tie(solution, boost::tuples::ignore) = solver.optimize(initialValues); @@ -283,8 +283,8 @@ TEST(QPSolver, optimizeNocedal06bookEx16_4) { TEST(QPSolver, failedSubproblem) { QP qp; - qp.cost.push_back(JacobianFactor(X(1), I_2x2, zero(2))); - qp.cost.push_back(HessianFactor(X(1), Z_2x2, zero(2), 100.0)); + qp.cost.push_back(JacobianFactor(X(1), I_2x2, Z_2x1)); + qp.cost.push_back(HessianFactor(X(1), Z_2x2, Z_2x1, 100.0)); qp.inequalities.push_back( LinearInequality(X(1), (Matrix(1,2) << -1.0, 0.0).finished(), -1.0, 0)); diff --git a/gtsam_unstable/nonlinear/LinearizedFactor.cpp b/gtsam_unstable/nonlinear/LinearizedFactor.cpp index f8dca75a4..84dffe7be 100644 --- a/gtsam_unstable/nonlinear/LinearizedFactor.cpp +++ b/gtsam_unstable/nonlinear/LinearizedFactor.cpp @@ -194,7 +194,7 @@ bool LinearizedHessianFactor::equals(const NonlinearFactor& expected, double tol double LinearizedHessianFactor::error(const Values& c) const { // Construct an error vector in key-order from the Values - Vector dx = zero(dim()); + Vector dx = Vector::Zero(dim()); size_t index = 0; for(unsigned int i = 0; i < this->size(); ++i){ Key key = this->keys()[i]; @@ -217,7 +217,7 @@ boost::shared_ptr LinearizedHessianFactor::linearize(const Values& c) const { // Construct an error vector in key-order from the Values - Vector dx = zero(dim()); + Vector dx = Vector::Zero(dim()); size_t index = 0; for(unsigned int i = 0; i < this->size(); ++i){ Key key = this->keys()[i]; diff --git a/gtsam_unstable/slam/AHRS.cpp b/gtsam_unstable/slam/AHRS.cpp index a16a4f9c0..a0beef07f 100644 --- a/gtsam_unstable/slam/AHRS.cpp +++ b/gtsam_unstable/slam/AHRS.cpp @@ -101,7 +101,7 @@ std::pair AHRS::initialize(double g_e) P_plus_k2.block<3,3>(6, 3) = Z_3x3; P_plus_k2.block<3,3>(6, 6) = Pa; - Vector dx = zero(9); + Vector dx = Z_9x1; KalmanFilter::State state = KF_.init(dx, P_plus_k2); return std::make_pair(mech0_, state); } diff --git a/gtsam_unstable/slam/DummyFactor.cpp b/gtsam_unstable/slam/DummyFactor.cpp index 70aff0596..8519f6f8d 100644 --- a/gtsam_unstable/slam/DummyFactor.cpp +++ b/gtsam_unstable/slam/DummyFactor.cpp @@ -55,7 +55,7 @@ DummyFactor::linearize(const Values& c) const { noiseModel::Diagonal::shared_ptr model = noiseModel::Unit::Create(rowDim_); return GaussianFactor::shared_ptr( - new JacobianFactor(terms, zero(rowDim_), model)); + new JacobianFactor(terms, Vector::Zero(rowDim_), model)); } /* ************************************************************************* */ diff --git a/gtsam_unstable/slam/Mechanization_bRn2.h b/gtsam_unstable/slam/Mechanization_bRn2.h index 4268aa0e5..a228b2347 100644 --- a/gtsam_unstable/slam/Mechanization_bRn2.h +++ b/gtsam_unstable/slam/Mechanization_bRn2.h @@ -30,7 +30,7 @@ public: * @param r3 Z-axis of rotated frame */ Mechanization_bRn2(const Rot3& initial_bRn = Rot3(), - const Vector3& initial_x_g = zero(3), const Vector3& initial_x_a = zero(3)) : + const Vector3& initial_x_g = Z_3x1, const Vector3& initial_x_a = Z_3x1) : bRn_(initial_bRn), x_g_(initial_x_g), x_a_(initial_x_a) { } diff --git a/gtsam_unstable/slam/PartialPriorFactor.h b/gtsam_unstable/slam/PartialPriorFactor.h index a848620e3..fa06d47a3 100644 --- a/gtsam_unstable/slam/PartialPriorFactor.h +++ b/gtsam_unstable/slam/PartialPriorFactor.h @@ -113,7 +113,7 @@ namespace gtsam { // FIXME: this was originally the generic retraction - may not produce same results Vector full_logmap = T::Logmap(p); // Vector full_logmap = T::identity().localCoordinates(p); // Alternate implementation - Vector masked_logmap = zero(this->dim()); + Vector masked_logmap = Vector::Zero(this->dim()); for (size_t i=0; i >(this->keys_, Gs, gs, 0.0); } @@ -528,7 +528,7 @@ public: if (nonDegenerate) return Base::unwhitenedError(cameras, *result_); else - return zero(cameras.size() * Base::ZDim); + return Vector::Zero(cameras.size() * Base::ZDim); } /** diff --git a/gtsam_unstable/slam/tests/testDummyFactor.cpp b/gtsam_unstable/slam/tests/testDummyFactor.cpp index 03a57b352..743c7398c 100644 --- a/gtsam_unstable/slam/tests/testDummyFactor.cpp +++ b/gtsam_unstable/slam/tests/testDummyFactor.cpp @@ -47,7 +47,7 @@ TEST( testDummyFactor, basics ) { CHECK(actLinearization); noiseModel::Diagonal::shared_ptr model3 = noiseModel::Unit::Create(3); GaussianFactor::shared_ptr expLinearization(new JacobianFactor( - key1, Matrix::Zero(3,3), key2, Matrix::Zero(3,3), zero(3), model3)); + key1, Matrix::Zero(3,3), key2, Matrix::Zero(3,3), Z_3x1, model3)); EXPECT(assert_equal(*expLinearization, *actLinearization, tol)); } diff --git a/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp b/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp index 75535eebc..b84f7e080 100644 --- a/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp +++ b/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp @@ -152,7 +152,7 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, imuBias::ConstantBias Bias1; Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); - Vector ExpectedErr(zero(9)); + Vector ExpectedErr(Z_9x1); CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5)); } @@ -185,7 +185,7 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, imuBias::ConstantBias Bias1; Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); - Vector ExpectedErr(zero(9)); + Vector ExpectedErr(Z_9x1); CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5)); } @@ -225,7 +225,7 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, imuBias::ConstantBias Bias1; Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); - Vector ExpectedErr(zero(9)); + Vector ExpectedErr(Z_9x1); // TODO: Expected values need to be updated for global velocity version CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5)); @@ -488,7 +488,7 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, imuBias::ConstantBias Bias1; Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); - Vector ExpectedErr(zero(9)); + Vector ExpectedErr(Z_9x1); CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5)); } @@ -529,7 +529,7 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, imuBias::ConstantBias Bias1; Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); - Vector ExpectedErr(zero(9)); + Vector ExpectedErr(Z_9x1); CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5)); } @@ -579,7 +579,7 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, imuBias::ConstantBias Bias1; Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); - Vector ExpectedErr(zero(9)); + Vector ExpectedErr(Z_9x1); // TODO: Expected values need to be updated for global velocity version CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5)); diff --git a/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp b/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp index a06c39182..210018ed3 100644 --- a/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp +++ b/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp @@ -34,7 +34,7 @@ TEST( testRelativeElevationFactor, level_zero_error ) { double measured = 2.0; RelativeElevationFactor factor(poseKey, pointKey, measured, model1); Matrix actH1, actH2; - EXPECT(assert_equal(zero(1), factor.evaluateError(pose1, point1, actH1, actH2))); + EXPECT(assert_equal(Z_1x1, factor.evaluateError(pose1, point1, actH1, actH2))); Matrix expH1 = numericalDerivative21( boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5); Matrix expH2 = numericalDerivative22( @@ -79,7 +79,7 @@ TEST( testRelativeElevationFactor, rotated_zero_error ) { double measured = 2.0; RelativeElevationFactor factor(poseKey, pointKey, measured, model1); Matrix actH1, actH2; - EXPECT(assert_equal(zero(1), factor.evaluateError(pose2, point1, actH1, actH2))); + EXPECT(assert_equal(Z_1x1, factor.evaluateError(pose2, point1, actH1, actH2))); Matrix expH1 = numericalDerivative21( boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5); Matrix expH2 = numericalDerivative22( diff --git a/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp b/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp index d8d720e83..01d4b152d 100644 --- a/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp +++ b/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp @@ -97,7 +97,7 @@ TEST( TransformBtwRobotsUnaryFactor, unwhitenedError) Vector err = g.unwhitenedError(values); // Equals - CHECK(assert_equal(err, zero(3), 1e-5)); + CHECK(assert_equal(err, (Vector) Z_3x1, 1e-5)); } /* ************************************************************************* */ @@ -131,7 +131,7 @@ TEST( TransformBtwRobotsUnaryFactor, unwhitenedError2) Vector err = g.unwhitenedError(values); // Equals - CHECK(assert_equal(err, zero(3), 1e-5)); + CHECK(assert_equal(err, Z_3x1, 1e-5)); } /* ************************************************************************* */ diff --git a/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp b/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp index dbd90f3a3..b200a3e58 100644 --- a/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp +++ b/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp @@ -108,7 +108,7 @@ TEST( TransformBtwRobotsUnaryFactorEM, unwhitenedError) Vector err = g.unwhitenedError(values); // Equals - CHECK(assert_equal(err, zero(3), 1e-5)); + CHECK(assert_equal(err, Z_3x1, 1e-5)); } /* ************************************************************************* */ @@ -147,7 +147,7 @@ TEST( TransformBtwRobotsUnaryFactorEM, unwhitenedError2) Vector err = g.unwhitenedError(values); // Equals - CHECK(assert_equal(err, zero(3), 1e-5)); + CHECK(assert_equal(err, Z_3x1, 1e-5)); } /* ************************************************************************* */ diff --git a/tests/smallExample.h b/tests/smallExample.h index b2a2102e1..d46cc34e1 100644 --- a/tests/smallExample.h +++ b/tests/smallExample.h @@ -260,9 +260,9 @@ inline VectorValues createZeroDelta() { using symbol_shorthand::X; using symbol_shorthand::L; VectorValues c; - c.insert(L(1), zero(2)); - c.insert(X(1), zero(2)); - c.insert(X(2), zero(2)); + c.insert(L(1), Z_2x1); + c.insert(X(1), Z_2x1); + c.insert(X(2), Z_2x1); return c; } diff --git a/tests/testBoundingConstraint.cpp b/tests/testBoundingConstraint.cpp index d4cefd329..18dabf18b 100644 --- a/tests/testBoundingConstraint.cpp +++ b/tests/testBoundingConstraint.cpp @@ -56,8 +56,8 @@ TEST( testBoundingConstraint, unary_basics_inactive1 ) { EXPECT(constraint2.isGreaterThan()); EXPECT(assert_equal(ones(1), constraint1.evaluateError(pt1), tol)); EXPECT(assert_equal(ones(1), constraint2.evaluateError(pt1), tol)); - EXPECT(assert_equal(zero(1), constraint1.unwhitenedError(config), tol)); - EXPECT(assert_equal(zero(1), constraint2.unwhitenedError(config), tol)); + EXPECT(assert_equal(Z_1x1, constraint1.unwhitenedError(config), tol)); + EXPECT(assert_equal(Z_1x1, constraint2.unwhitenedError(config), tol)); EXPECT_DOUBLES_EQUAL(0.0, constraint1.error(config), tol); EXPECT_DOUBLES_EQUAL(0.0, constraint2.error(config), tol); } @@ -75,8 +75,8 @@ TEST( testBoundingConstraint, unary_basics_inactive2 ) { EXPECT(!constraint4.isGreaterThan()); 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(assert_equal(Z_1x1, constraint3.unwhitenedError(config), tol)); + EXPECT(assert_equal(Z_1x1, constraint4.unwhitenedError(config), tol)); EXPECT_DOUBLES_EQUAL(0.0, constraint3.error(config), tol); EXPECT_DOUBLES_EQUAL(0.0, constraint4.error(config), tol); } @@ -189,26 +189,26 @@ TEST( testBoundingConstraint, MaxDistance_basics) { EXPECT(assert_equal((Vector(1) << 2.0).finished(), rangeBound.evaluateError(pt1, pt1))); EXPECT(assert_equal(ones(1), rangeBound.evaluateError(pt1, pt2))); - EXPECT(assert_equal(zero(1), rangeBound.evaluateError(pt1, pt3))); + EXPECT(assert_equal(Z_1x1, rangeBound.evaluateError(pt1, pt3))); EXPECT(assert_equal(-1.0*ones(1), rangeBound.evaluateError(pt1, pt4))); Values config1; config1.insert(key1, pt1); config1.insert(key2, pt1); EXPECT(!rangeBound.active(config1)); - EXPECT(assert_equal(zero(1), rangeBound.unwhitenedError(config1))); + EXPECT(assert_equal(Z_1x1, rangeBound.unwhitenedError(config1))); EXPECT(!rangeBound.linearize(config1)); EXPECT_DOUBLES_EQUAL(0.0, rangeBound.error(config1), tol); config1.update(key2, pt2); EXPECT(!rangeBound.active(config1)); - EXPECT(assert_equal(zero(1), rangeBound.unwhitenedError(config1))); + EXPECT(assert_equal(Z_1x1, rangeBound.unwhitenedError(config1))); EXPECT(!rangeBound.linearize(config1)); EXPECT_DOUBLES_EQUAL(0.0, rangeBound.error(config1), tol); config1.update(key2, pt3); EXPECT(rangeBound.active(config1)); - EXPECT(assert_equal(zero(1), rangeBound.unwhitenedError(config1))); + EXPECT(assert_equal(Z_1x1, rangeBound.unwhitenedError(config1))); EXPECT_DOUBLES_EQUAL(0.0, rangeBound.error(config1), tol); config1.update(key2, pt4); diff --git a/tests/testExpressionFactor.cpp b/tests/testExpressionFactor.cpp index c20eca34a..4fda27cdb 100644 --- a/tests/testExpressionFactor.cpp +++ b/tests/testExpressionFactor.cpp @@ -338,7 +338,7 @@ TEST(ExpressionFactor, Compose1) { EXPECT( assert_equal(I_3x3, H[1],1e-9)); // Check linearization - JacobianFactor expected(1, I_3x3, 2, I_3x3, zero(3)); + JacobianFactor expected(1, I_3x3, 2, I_3x3, Z_3x1); boost::shared_ptr gf = f.linearize(values); boost::shared_ptr jf = // boost::dynamic_pointer_cast(gf); @@ -367,7 +367,7 @@ TEST(ExpressionFactor, compose2) { EXPECT( assert_equal(2*I_3x3, H[0],1e-9)); // Check linearization - JacobianFactor expected(1, 2 * I_3x3, zero(3)); + JacobianFactor expected(1, 2 * I_3x3, Z_3x1); boost::shared_ptr gf = f.linearize(values); boost::shared_ptr jf = // boost::dynamic_pointer_cast(gf); @@ -396,7 +396,7 @@ TEST(ExpressionFactor, compose3) { EXPECT( assert_equal(I_3x3, H[0],1e-9)); // Check linearization - JacobianFactor expected(3, I_3x3, zero(3)); + JacobianFactor expected(3, I_3x3, Z_3x1); boost::shared_ptr gf = f.linearize(values); boost::shared_ptr jf = // boost::dynamic_pointer_cast(gf); @@ -441,7 +441,7 @@ TEST(ExpressionFactor, composeTernary) { EXPECT( assert_equal(I_3x3, H[2],1e-9)); // Check linearization - JacobianFactor expected(1, I_3x3, 2, I_3x3, 3, I_3x3, zero(3)); + JacobianFactor expected(1, I_3x3, 2, I_3x3, 3, I_3x3, Z_3x1); boost::shared_ptr gf = f.linearize(values); boost::shared_ptr jf = // boost::dynamic_pointer_cast(gf); diff --git a/tests/testGaussianBayesTreeB.cpp b/tests/testGaussianBayesTreeB.cpp index 9b172d572..e3c17fa3f 100644 --- a/tests/testGaussianBayesTreeB.cpp +++ b/tests/testGaussianBayesTreeB.cpp @@ -79,7 +79,7 @@ TEST( GaussianBayesTree, linear_smoother_shortcuts ) double sigma3 = 0.61808; Matrix A56 = (Matrix(2,2) << -0.382022,0.,0.,-0.382022).finished(); GaussianBayesNet expected3; - expected3 += GaussianConditional(X(5), zero(2), I_2x2/sigma3, X(6), A56/sigma3); + expected3 += GaussianConditional(X(5), Z_2x1, I_2x2/sigma3, X(6), A56/sigma3); GaussianBayesTree::sharedClique C3 = bayesTree[X(4)]; GaussianBayesNet actual3 = C3->shortcut(R); EXPECT(assert_equal(expected3,actual3,tol)); @@ -88,7 +88,7 @@ TEST( GaussianBayesTree, linear_smoother_shortcuts ) double sigma4 = 0.661968; Matrix A46 = (Matrix(2,2) << -0.146067,0.,0.,-0.146067).finished(); GaussianBayesNet expected4; - expected4 += GaussianConditional(X(4), zero(2), I_2x2/sigma4, X(6), A46/sigma4); + expected4 += GaussianConditional(X(4), Z_2x1, I_2x2/sigma4, X(6), A46/sigma4); GaussianBayesTree::sharedClique C4 = bayesTree[X(3)]; GaussianBayesNet actual4 = C4->shortcut(R); EXPECT(assert_equal(expected4,actual4,tol)); @@ -132,7 +132,7 @@ TEST( GaussianBayesTree, balanced_smoother_marginals ) double tol=1e-5; // Check marginal on x1 - JacobianFactor expected1 = GaussianDensity::FromMeanAndStddev(X(1), zero(2), sigmax1); + JacobianFactor expected1 = GaussianDensity::FromMeanAndStddev(X(1), Z_2x1, sigmax1); JacobianFactor actual1 = *bayesTree.marginalFactor(X(1)); Matrix expectedCovarianceX1 = I_2x2 * (sigmax1 * sigmax1); Matrix actualCovarianceX1; @@ -143,22 +143,22 @@ TEST( GaussianBayesTree, balanced_smoother_marginals ) // Check marginal on x2 double sigx2 = 0.68712938; // FIXME: this should be corrected analytically - JacobianFactor expected2 = GaussianDensity::FromMeanAndStddev(X(2), zero(2), sigx2); + JacobianFactor expected2 = GaussianDensity::FromMeanAndStddev(X(2), Z_2x1, sigx2); JacobianFactor actual2 = *bayesTree.marginalFactor(X(2)); EXPECT(assert_equal(expected2,actual2,tol)); // Check marginal on x3 - JacobianFactor expected3 = GaussianDensity::FromMeanAndStddev(X(3), zero(2), sigmax3); + JacobianFactor expected3 = GaussianDensity::FromMeanAndStddev(X(3), Z_2x1, sigmax3); JacobianFactor actual3 = *bayesTree.marginalFactor(X(3)); EXPECT(assert_equal(expected3,actual3,tol)); // Check marginal on x4 - JacobianFactor expected4 = GaussianDensity::FromMeanAndStddev(X(4), zero(2), sigmax4); + JacobianFactor expected4 = GaussianDensity::FromMeanAndStddev(X(4), Z_2x1, sigmax4); JacobianFactor actual4 = *bayesTree.marginalFactor(X(4)); EXPECT(assert_equal(expected4,actual4,tol)); // Check marginal on x7 (should be equal to x1) - JacobianFactor expected7 = GaussianDensity::FromMeanAndStddev(X(7), zero(2), sigmax7); + JacobianFactor expected7 = GaussianDensity::FromMeanAndStddev(X(7), Z_2x1, sigmax7); JacobianFactor actual7 = *bayesTree.marginalFactor(X(7)); EXPECT(assert_equal(expected7,actual7,tol)); } @@ -212,8 +212,8 @@ TEST( GaussianBayesTree, balanced_smoother_shortcuts ) // // // Check the clique marginal P(C3) // double sigmax2_alt = 1/1.45533; // THIS NEEDS TO BE CHECKED! -// GaussianBayesNet expected = simpleGaussian(ordering[X(2)],zero(2),sigmax2_alt); -// push_front(expected,ordering[X(1)], zero(2), eye(2)*sqrt(2), ordering[X(2)], -eye(2)*sqrt(2)/2, ones(2)); +// GaussianBayesNet expected = simpleGaussian(ordering[X(2)],Z_2x1,sigmax2_alt); +// push_front(expected,ordering[X(1)], Z_2x1, eye(2)*sqrt(2), ordering[X(2)], -eye(2)*sqrt(2)/2, ones(2)); // GaussianISAM::sharedClique R = bayesTree.root(), C3 = bayesTree[ordering[X(1)]]; // GaussianFactorGraph marginal = C3->marginal(R); // GaussianVariableIndex varIndex(marginal); @@ -248,17 +248,17 @@ TEST( GaussianBayesTree, balanced_smoother_joint ) // Check the joint density P(x1,x7) factored as P(x1|x7)P(x7) GaussianBayesNet expected1 = list_of // Why does the sign get flipped on the prior? - (GaussianConditional(X(1), zero(2), I/sigmax7, X(7), A/sigmax7)) - (GaussianConditional(X(7), zero(2), -1*I/sigmax7)); + (GaussianConditional(X(1), Z_2x1, I/sigmax7, X(7), A/sigmax7)) + (GaussianConditional(X(7), Z_2x1, -1*I/sigmax7)); GaussianBayesNet actual1 = *bayesTree.jointBayesNet(X(1),X(7)); EXPECT(assert_equal(expected1, actual1, tol)); // // Check the joint density P(x7,x1) factored as P(x7|x1)P(x1) // GaussianBayesNet expected2; // GaussianConditional::shared_ptr - // parent2(new GaussianConditional(X(1), zero(2), -1*I/sigmax1, ones(2))); + // parent2(new GaussianConditional(X(1), Z_2x1, -1*I/sigmax1, ones(2))); // expected2.push_front(parent2); - // push_front(expected2,X(7), zero(2), I/sigmax1, X(1), A/sigmax1, sigma); + // push_front(expected2,X(7), Z_2x1, I/sigmax1, X(1), A/sigmax1, sigma); // GaussianBayesNet actual2 = *bayesTree.jointBayesNet(X(7),X(1)); // EXPECT(assert_equal(expected2,actual2,tol)); @@ -266,19 +266,19 @@ TEST( GaussianBayesTree, balanced_smoother_joint ) double sig14 = 0.784465; Matrix A14 = -0.0769231*I; GaussianBayesNet expected3 = list_of - (GaussianConditional(X(1), zero(2), I/sig14, X(4), A14/sig14)) - (GaussianConditional(X(4), zero(2), I/sigmax4)); + (GaussianConditional(X(1), Z_2x1, I/sig14, X(4), A14/sig14)) + (GaussianConditional(X(4), Z_2x1, I/sigmax4)); GaussianBayesNet actual3 = *bayesTree.jointBayesNet(X(1),X(4)); EXPECT(assert_equal(expected3,actual3,tol)); // // Check the joint density P(x4,x1), i.e. with a root variable, factored the other way // GaussianBayesNet expected4; // GaussianConditional::shared_ptr - // parent4(new GaussianConditional(X(1), zero(2), -1.0*I/sigmax1, ones(2))); + // parent4(new GaussianConditional(X(1), Z_2x1, -1.0*I/sigmax1, ones(2))); // expected4.push_front(parent4); // double sig41 = 0.668096; // Matrix A41 = -0.055794*I; - // push_front(expected4,X(4), zero(2), I/sig41, X(1), A41/sig41, sigma); + // push_front(expected4,X(4), Z_2x1, I/sig41, X(1), A41/sig41, sigma); // GaussianBayesNet actual4 = *bayesTree.jointBayesNet(X(4),X(1)); // EXPECT(assert_equal(expected4,actual4,tol)); } diff --git a/tests/testGaussianFactorGraphB.cpp b/tests/testGaussianFactorGraphB.cpp index 47307d0f8..becd407aa 100644 --- a/tests/testGaussianFactorGraphB.cpp +++ b/tests/testGaussianFactorGraphB.cpp @@ -473,13 +473,13 @@ TEST(GaussianFactorGraph, replace) SharedDiagonal noise(noiseModel::Isotropic::Sigma(3, 1.0)); GaussianFactorGraph::sharedFactor f1(new JacobianFactor( - ord[X(1)], I_3x3, ord[X(2)], I_3x3, zero(3), noise)); + ord[X(1)], I_3x3, ord[X(2)], I_3x3, Z_3x1, noise)); GaussianFactorGraph::sharedFactor f2(new JacobianFactor( - ord[X(2)], I_3x3, ord[X(3)], I_3x3, zero(3), noise)); + ord[X(2)], I_3x3, ord[X(3)], I_3x3, Z_3x1, noise)); GaussianFactorGraph::sharedFactor f3(new JacobianFactor( - ord[X(3)], I_3x3, ord[X(4)], I_3x3, zero(3), noise)); + ord[X(3)], I_3x3, ord[X(4)], I_3x3, Z_3x1, noise)); GaussianFactorGraph::sharedFactor f4(new JacobianFactor( - ord[X(5)], I_3x3, ord[X(6)], I_3x3, zero(3), noise)); + ord[X(5)], I_3x3, ord[X(6)], I_3x3, Z_3x1, noise)); GaussianFactorGraph actual; actual.push_back(f1); diff --git a/tests/testIterative.cpp b/tests/testIterative.cpp index 071b9d12d..e0c2b7b66 100644 --- a/tests/testIterative.cpp +++ b/tests/testIterative.cpp @@ -62,7 +62,7 @@ TEST( Iterative, conjugateGradientDescent ) // get matrices Matrix A; Vector b; - Vector x0 = gtsam::zero(6); + Vector x0 = Z_6x1; boost::tie(A, b) = fg.jacobian(); Vector expectedX = (Vector(6) << -0.1, 0.1, -0.1, -0.1, 0.1, -0.2).finished(); @@ -104,7 +104,7 @@ TEST( Iterative, conjugateGradientDescent_hard_constraint ) VectorValues actual = conjugateGradientDescent(*fg, zeros, parameters); VectorValues expected; - expected.insert(X(1), zero(3)); + expected.insert(X(1), Z_3x1); expected.insert(X(2), Vector3(-0.5,0.,0.)); CHECK(assert_equal(expected, actual)); } @@ -131,7 +131,7 @@ TEST( Iterative, conjugateGradientDescent_soft_constraint ) VectorValues actual = conjugateGradientDescent(*fg, zeros, parameters); VectorValues expected; - expected.insert(X(1), zero(3)); + expected.insert(X(1), Z_3x1); expected.insert(X(2), Vector3(-0.5,0.,0.)); CHECK(assert_equal(expected, actual)); } diff --git a/tests/testManifold.cpp b/tests/testManifold.cpp index 89b296824..65d26eb98 100644 --- a/tests/testManifold.cpp +++ b/tests/testManifold.cpp @@ -131,7 +131,7 @@ TEST(Manifold, DefaultChart) { EXPECT_DOUBLES_EQUAL(traits::Retract(0, v1), 1, 1e-9); // Dynamic does not work yet ! - Vector z = zero(2), v(2); + Vector z = Z_2x1, v(2); v << 1, 0; //DefaultChart chart4; // EXPECT(assert_equal(traits::Local(z, v), v)); @@ -146,7 +146,7 @@ TEST(Manifold, DefaultChart) { EXPECT(assert_equal(traits::Retract(I, v3), R)); // Check zero vector //DefaultChart chart6; - EXPECT(assert_equal(zero(3), traits::Local(R, R))); + EXPECT(assert_equal((Vector) Z_3x1, traits::Local(R, R))); } //****************************************************************************** diff --git a/tests/testNonlinearEquality.cpp b/tests/testNonlinearEquality.cpp index 335b68662..86080b673 100644 --- a/tests/testNonlinearEquality.cpp +++ b/tests/testNonlinearEquality.cpp @@ -56,7 +56,7 @@ TEST ( NonlinearEquality, linearization ) { // check linearize SharedDiagonal constraintModel = noiseModel::Constrained::All(3); - JacobianFactor expLF(key, I_3x3, zero(3), constraintModel); + JacobianFactor expLF(key, I_3x3, Z_3x1, constraintModel); GaussianFactor::shared_ptr actualLF = nle->linearize(linearize); EXPECT(assert_equal((const GaussianFactor&)expLF, *actualLF)); } @@ -133,7 +133,7 @@ TEST ( NonlinearEquality, error ) { // check error function outputs Vector actual = nle->unwhitenedError(feasible); - EXPECT(assert_equal(actual, zero(3))); + EXPECT(assert_equal(actual, Z_3x1)); actual = nle->unwhitenedError(bad_linearize); EXPECT( @@ -263,8 +263,8 @@ TEST( testNonlinearEqualityConstraint, unary_basics ) { Values config1; config1.insert(key, pt); EXPECT(constraint.active(config1)); - EXPECT(assert_equal(zero(2), constraint.evaluateError(pt), tol)); - EXPECT(assert_equal(zero(2), constraint.unwhitenedError(config1), tol)); + EXPECT(assert_equal(Z_2x1, constraint.evaluateError(pt), tol)); + EXPECT(assert_equal(Z_2x1, constraint.unwhitenedError(config1), tol)); EXPECT_DOUBLES_EQUAL(0.0, constraint.error(config1), tol); Values config2; @@ -289,7 +289,7 @@ TEST( testNonlinearEqualityConstraint, unary_linearization ) { config1.insert(key, pt); GaussianFactor::shared_ptr actual1 = constraint.linearize(config1); GaussianFactor::shared_ptr expected1( - new JacobianFactor(key, I_2x2, zero(2), hard_model)); + new JacobianFactor(key, I_2x2, Z_2x1, hard_model)); EXPECT(assert_equal(*expected1, *actual1, tol)); Values config2; @@ -345,8 +345,8 @@ TEST( testNonlinearEqualityConstraint, odo_basics ) { config1.insert(key1, x1); config1.insert(key2, x2); EXPECT(constraint.active(config1)); - EXPECT(assert_equal(zero(2), constraint.evaluateError(x1, x2), tol)); - EXPECT(assert_equal(zero(2), constraint.unwhitenedError(config1), tol)); + EXPECT(assert_equal(Z_2x1, constraint.evaluateError(x1, x2), tol)); + EXPECT(assert_equal(Z_2x1, constraint.unwhitenedError(config1), tol)); EXPECT_DOUBLES_EQUAL(0.0, constraint.error(config1), tol); Values config2; @@ -374,7 +374,7 @@ TEST( testNonlinearEqualityConstraint, odo_linearization ) { config1.insert(key2, x2); GaussianFactor::shared_ptr actual1 = constraint.linearize(config1); GaussianFactor::shared_ptr expected1( - new JacobianFactor(key1, -I_2x2, key2, I_2x2, zero(2), + new JacobianFactor(key1, -I_2x2, key2, I_2x2, Z_2x1, hard_model)); EXPECT(assert_equal(*expected1, *actual1, tol)); diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index f6cdd6ee5..7a22abc67 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -257,9 +257,9 @@ TEST_UNSAFE(NonlinearOptimizer, MoreOptimization) { expected.insert(2, Pose2(1, 1, M_PI)); VectorValues expectedGradient; - expectedGradient.insert(0,zero(3)); - expectedGradient.insert(1,zero(3)); - expectedGradient.insert(2,zero(3)); + expectedGradient.insert(0,Z_3x1); + expectedGradient.insert(1,Z_3x1); + expectedGradient.insert(2,Z_3x1); // Try LM and Dogleg LevenbergMarquardtParams params = LevenbergMarquardtParams::LegacyDefaults(); diff --git a/timing/timeSchurFactors.cpp b/timing/timeSchurFactors.cpp index 29e8e9916..9be55f831 100644 --- a/timing/timeSchurFactors.cpp +++ b/timing/timeSchurFactors.cpp @@ -109,7 +109,7 @@ void timeAll(size_t m, size_t N) { double* xdata = x.data(); // create a y - Vector y = zero(m * D); + Vector y = Vector::Zero(m * D); TIME(RawImplicit, implicitFactor, xdata, y.data()) TIME(RawJacobianQ, jf, xdata, y.data()) TIME(RawJacobianQR, jqr, xdata, y.data()) From b6728ef6203b20222fc73f4d1f2aaefceb845ae2 Mon Sep 17 00:00:00 2001 From: Alex Hagiopol Date: Fri, 15 Apr 2016 17:30:54 -0400 Subject: [PATCH 79/91] Deprecated Vector ones(size_t n). All unit tests pass. --- gtsam/base/Vector.h | 2 +- gtsam/geometry/Rot2.h | 4 ++-- gtsam/linear/NoiseModel.cpp | 2 +- gtsam/linear/tests/testGaussianFactorGraph.cpp | 4 ++-- gtsam/linear/tests/testHessianFactor.cpp | 2 +- gtsam/linear/tests/testJacobianFactor.cpp | 6 +++--- gtsam/linear/tests/testKalmanFilter.cpp | 2 +- gtsam/linear/tests/testSerializationLinear.cpp | 4 ++-- gtsam/slam/ProjectionFactor.h | 2 +- gtsam/slam/StereoFactor.h | 2 +- gtsam/slam/TriangulationFactor.h | 2 +- .../tests/testRegularImplicitSchurFactor.cpp | 18 +++++++++--------- .../dynamics/tests/testIMUSystem.cpp | 4 ++-- gtsam_unstable/linear/tests/testQPSolver.cpp | 14 +++++++------- gtsam_unstable/slam/AHRS.cpp | 6 +++--- gtsam_unstable/slam/InvDepthFactor3.h | 2 +- gtsam_unstable/slam/InvDepthFactorVariant1.h | 2 +- gtsam_unstable/slam/InvDepthFactorVariant2.h | 2 +- gtsam_unstable/slam/InvDepthFactorVariant3.h | 4 ++-- gtsam_unstable/slam/MultiProjectionFactor.h | 2 +- gtsam_unstable/slam/ProjectionFactorPPP.h | 2 +- gtsam_unstable/slam/ProjectionFactorPPPC.h | 2 +- .../slam/tests/testSerialization.cpp | 6 +++--- tests/smallExample.h | 2 +- tests/testBoundingConstraint.cpp | 18 +++++++++--------- tests/testGaussianFactorGraphB.cpp | 16 ++++++++-------- tests/testNonlinearFactor.cpp | 2 +- 27 files changed, 67 insertions(+), 67 deletions(-) diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index 2d09dcc90..3d854916c 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -250,10 +250,10 @@ inline void subInsert(Vector& fullVector, const Vector& subVector, size_t i) {fu inline double sum(const Vector &a){return a.sum();} inline Vector zero(size_t n) { return Vector::Zero(n);} +inline Vector ones(size_t n) { return Vector::Ones(n); } #endif inline Vector delta(size_t n, size_t i, double value){ return Vector::Unit(n, i) * value;} inline Vector basis(size_t n, size_t i) { return delta(n, i, 1.0); } -inline Vector ones(size_t n) { return Vector::Ones(n); } inline size_t dim(const Vector& v) { return v.size(); } } // namespace gtsam diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index 198cd5862..fb54156df 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -119,12 +119,12 @@ namespace gtsam { /// Left-trivialized derivative of the exponential map static Matrix ExpmapDerivative(const Vector& /*v*/) { - return ones(1); + return Vector::Ones(1); } /// Left-trivialized derivative inverse of the exponential map static Matrix LogmapDerivative(const Vector& /*v*/) { - return ones(1); + return Vector::Ones(1); } // Chart at origin simply uses exponential map and its inverse diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index 31b059989..5bcf3d635 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -405,7 +405,7 @@ void Constrained::WhitenInPlace(Eigen::Block H) const { /* ************************************************************************* */ Constrained::shared_ptr Constrained::unit() const { - Vector sigmas = ones(dim()); + Vector sigmas = Vector::Ones(dim()); for (size_t i=0; ifx(); + return Vector::Ones(2) * 2.0 * K_->fx(); } /** return the measurement */ diff --git a/gtsam/slam/StereoFactor.h b/gtsam/slam/StereoFactor.h index 528479f80..c330276d3 100644 --- a/gtsam/slam/StereoFactor.h +++ b/gtsam/slam/StereoFactor.h @@ -146,7 +146,7 @@ public: if (throwCheirality_) throw e; } - return ones(3) * 2.0 * K_->fx(); + return Vector::Ones(3) * 2.0 * K_->fx(); } /** return the measured */ diff --git a/gtsam/slam/TriangulationFactor.h b/gtsam/slam/TriangulationFactor.h index 2748599c4..4f86023ca 100644 --- a/gtsam/slam/TriangulationFactor.h +++ b/gtsam/slam/TriangulationFactor.h @@ -131,7 +131,7 @@ public: << std::endl; if (throwCheirality_) throw e; - return ones(Measurement::dimension) * 2.0 * camera_.calibration().fx(); + return Vector::Ones(Measurement::dimension) * 2.0 * camera_.calibration().fx(); } } diff --git a/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp b/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp index 4df3ed1e6..4184d6769 100644 --- a/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp +++ b/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp @@ -170,9 +170,9 @@ TEST( regularImplicitSchurFactor, addHessianMultiply ) { } VectorValues expectedVV; - expectedVV.insert(0,-3.5*ones(6)); - expectedVV.insert(1,10*ones(6)/3); - expectedVV.insert(3,-19.5*ones(6)); + expectedVV.insert(0,-3.5*Vector::Ones(6)); + expectedVV.insert(1,10*Vector::Ones(6)/3); + expectedVV.insert(3,-19.5*Vector::Ones(6)); { // Check gradientAtZero VectorValues actual = implicitFactor.gradientAtZero(); EXPECT(assert_equal(expectedVV, jfQ.gradientAtZero(), 1e-8)); @@ -210,9 +210,9 @@ TEST( regularImplicitSchurFactor, addHessianMultiply ) { TEST(regularImplicitSchurFactor, hessianDiagonal) { /* TESTED AGAINST MATLAB - * F = [ones(2,6) zeros(2,6) zeros(2,6) - zeros(2,6) 2*ones(2,6) zeros(2,6) - zeros(2,6) zeros(2,6) 3*ones(2,6)] + * F = [Vector::Ones(2,6) zeros(2,6) zeros(2,6) + zeros(2,6) 2*Vector::Ones(2,6) zeros(2,6) + zeros(2,6) zeros(2,6) 3*Vector::Ones(2,6)] E = [[1:6] [1:6] [0.5 1:5]]; E = reshape(E',3,6)' P = inv(E' * E) @@ -228,9 +228,9 @@ TEST(regularImplicitSchurFactor, hessianDiagonal) // hessianDiagonal VectorValues expected; - expected.insert(0, 1.195652*ones(6)); - expected.insert(1, 4.782608*ones(6)); - expected.insert(3, 7.043478*ones(6)); + expected.insert(0, 1.195652*Vector::Ones(6)); + expected.insert(1, 4.782608*Vector::Ones(6)); + expected.insert(3, 7.043478*Vector::Ones(6)); EXPECT(assert_equal(expected, factor.hessianDiagonal(),1e-5)); // hessianBlockDiagonal diff --git a/gtsam_unstable/dynamics/tests/testIMUSystem.cpp b/gtsam_unstable/dynamics/tests/testIMUSystem.cpp index 3fc6a0197..5fe29a57f 100644 --- a/gtsam_unstable/dynamics/tests/testIMUSystem.cpp +++ b/gtsam_unstable/dynamics/tests/testIMUSystem.cpp @@ -38,7 +38,7 @@ TEST(testIMUSystem, instantiations) { gtsam::SharedNoiseModel model6 = gtsam::noiseModel::Unit::Create(6); gtsam::SharedNoiseModel model9 = gtsam::noiseModel::Unit::Create(9); - Vector accel = ones(3), gyro = ones(3); + Vector accel = Vector::Ones(3), gyro = Vector::Ones(3); IMUFactor imu(accel, gyro, 0.01, x1, x2, model6); FullIMUFactor full_imu(accel, gyro, 0.01, x1, x2, model9); @@ -48,7 +48,7 @@ TEST(testIMUSystem, instantiations) { VelocityConstraint constraint(x1, x2, 0.1, 10000); PriorFactor posePrior(x1, x1_v, model9); DHeightPrior heightPrior(x1, 0.1, model1); - VelocityPrior velPrior(x1, ones(3), model3); + VelocityPrior velPrior(x1, Vector::Ones(3), model3); } /* ************************************************************************* */ diff --git a/gtsam_unstable/linear/tests/testQPSolver.cpp b/gtsam_unstable/linear/tests/testQPSolver.cpp index ebe927738..9a67cbb53 100644 --- a/gtsam_unstable/linear/tests/testQPSolver.cpp +++ b/gtsam_unstable/linear/tests/testQPSolver.cpp @@ -38,7 +38,7 @@ QP createTestCase() { // 0.5*x1'*G11*x1 + x1'*G12*x2 + 0.5*x2'*G22*x2 - x1'*g1 - x2'*g2 + 0.5*f // Hence, we have G11=2, G12 = -1, g1 = +3, G22 = 2, g2 = 0, f = 10 qp.cost.push_back( - HessianFactor(X(1), X(2), 2.0 * Matrix::Ones(1, 1), -Matrix::Ones(1, 1), 3.0 * ones(1), + HessianFactor(X(1), X(2), 2.0 * Matrix::Ones(1, 1), -Matrix::Ones(1, 1), 3.0 * Vector::Ones(1), 2.0 * Matrix::Ones(1, 1), Z_1x1, 10.0)); // Inequality constraints @@ -110,8 +110,8 @@ TEST(QPSolver, dual) { // Initials values VectorValues initialValues; - initialValues.insert(X(1), ones(1)); - initialValues.insert(X(2), ones(1)); + initialValues.insert(X(1), Vector::Ones(1)); + initialValues.insert(X(2), Vector::Ones(1)); QPSolver solver(qp); @@ -219,8 +219,8 @@ QP createTestMatlabQPEx() { // 0.5*x1'*G11*x1 + x1'*G12*x2 + 0.5*x2'*G22*x2 - x1'*g1 - x2'*g2 + 0.5*f // Hence, we have G11=1, G12 = -1, g1 = +2, G22 = 2, g2 = +6, f = 0 qp.cost.push_back( - HessianFactor(X(1), X(2), 1.0 * Matrix::Ones(1,1), -Matrix::Ones(1, 1), 2.0 * ones(1), - 2.0 * Matrix::Ones(1, 1), 6 * ones(1), 1000.0)); + HessianFactor(X(1), X(2), 1.0 * Matrix::Ones(1,1), -Matrix::Ones(1, 1), 2.0 * Vector::Ones(1), + 2.0 * Matrix::Ones(1, 1), 6 * Vector::Ones(1), 1000.0)); // Inequality constraints qp.inequalities.push_back(LinearInequality(X(1), One, X(2), One, 2, 0)); // x1 + x2 <= 2 @@ -251,8 +251,8 @@ TEST(QPSolver, optimizeMatlabEx) { QP createTestNocedal06bookEx16_4() { QP qp; - qp.cost.push_back(JacobianFactor(X(1), Matrix::Ones(1, 1), ones(1))); - qp.cost.push_back(JacobianFactor(X(2), Matrix::Ones(1, 1), 2.5 * ones(1))); + qp.cost.push_back(JacobianFactor(X(1), Matrix::Ones(1, 1), Vector::Ones(1))); + qp.cost.push_back(JacobianFactor(X(2), Matrix::Ones(1, 1), 2.5 * Vector::Ones(1))); // Inequality constraints qp.inequalities.push_back(LinearInequality(X(1), -One, X(2), 2 * One, 2, 0)); diff --git a/gtsam_unstable/slam/AHRS.cpp b/gtsam_unstable/slam/AHRS.cpp index a0beef07f..0a3fa0283 100644 --- a/gtsam_unstable/slam/AHRS.cpp +++ b/gtsam_unstable/slam/AHRS.cpp @@ -45,9 +45,9 @@ AHRS::AHRS(const Matrix& stationaryU, const Matrix& stationaryF, double g_e, F_g_ = -I_3x3 / tau_g; F_a_ = -I_3x3 / tau_a; - 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 var_omega_w = 0 * Vector::Ones(3); // TODO + Vector3 var_omega_g = (0.0034 * 0.0034) * Vector::Ones(3); + Vector3 var_omega_a = (0.034 * 0.034) * Vector::Ones(3); 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; diff --git a/gtsam_unstable/slam/InvDepthFactor3.h b/gtsam_unstable/slam/InvDepthFactor3.h index 3de6a1824..7509fe3b2 100644 --- a/gtsam_unstable/slam/InvDepthFactor3.h +++ b/gtsam_unstable/slam/InvDepthFactor3.h @@ -94,7 +94,7 @@ public: if (H3) *H2 = Matrix::Zero(2,1); std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) << " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl; - return gtsam::ones(2) * 2.0 * K_->fx(); + return gtsam::Vector::Ones(2) * 2.0 * K_->fx(); } return (gtsam::Vector(1) << 0.0).finished(); } diff --git a/gtsam_unstable/slam/InvDepthFactorVariant1.h b/gtsam_unstable/slam/InvDepthFactorVariant1.h index 2fd964a35..e9f894faf 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant1.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant1.h @@ -93,7 +93,7 @@ public: << ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key2()) << "]" << " moved behind camera [" << DefaultKeyFormatter(this->key1()) <<"]" << std::endl; - return gtsam::ones(2) * 2.0 * K_->fx(); + return gtsam::Vector::Ones(2) * 2.0 * K_->fx(); } return (gtsam::Vector(1) << 0.0).finished(); } diff --git a/gtsam_unstable/slam/InvDepthFactorVariant2.h b/gtsam_unstable/slam/InvDepthFactorVariant2.h index fdba78445..ec2615ed6 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant2.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant2.h @@ -96,7 +96,7 @@ public: << ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key2()) << "]" << " moved behind camera [" << DefaultKeyFormatter(this->key1()) <<"]" << std::endl; - return gtsam::ones(2) * 2.0 * K_->fx(); + return gtsam::Vector::Ones(2) * 2.0 * K_->fx(); } return (gtsam::Vector(1) << 0.0).finished(); } diff --git a/gtsam_unstable/slam/InvDepthFactorVariant3.h b/gtsam_unstable/slam/InvDepthFactorVariant3.h index feff0b62c..cc5878d85 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant3.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant3.h @@ -96,7 +96,7 @@ public: << ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key1()) << "," << DefaultKeyFormatter(this->key2()) << "]" << " moved behind camera [" << DefaultKeyFormatter(this->key1()) << "]" << std::endl; - return gtsam::ones(2) * 2.0 * K_->fx(); + return gtsam::Vector::Ones(2) * 2.0 * K_->fx(); } return (Vector(1) << 0.0).finished(); } @@ -215,7 +215,7 @@ public: << ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key1()) << "," << DefaultKeyFormatter(this->key3()) << "]" << " moved behind camera " << DefaultKeyFormatter(this->key2()) << std::endl; - return gtsam::ones(2) * 2.0 * K_->fx(); + return gtsam::Vector::Ones(2) * 2.0 * K_->fx(); } return (Vector(1) << 0.0).finished(); } diff --git a/gtsam_unstable/slam/MultiProjectionFactor.h b/gtsam_unstable/slam/MultiProjectionFactor.h index 84d4ae0db..dc250fd9d 100644 --- a/gtsam_unstable/slam/MultiProjectionFactor.h +++ b/gtsam_unstable/slam/MultiProjectionFactor.h @@ -192,7 +192,7 @@ namespace gtsam { if (throwCheirality_) throw e; } - return ones(2) * 2.0 * K_->fx(); + return Vector::Ones(2) * 2.0 * K_->fx(); } /** return the measurements */ diff --git a/gtsam_unstable/slam/ProjectionFactorPPP.h b/gtsam_unstable/slam/ProjectionFactorPPP.h index 3b331d66b..adfc1d108 100644 --- a/gtsam_unstable/slam/ProjectionFactorPPP.h +++ b/gtsam_unstable/slam/ProjectionFactorPPP.h @@ -146,7 +146,7 @@ namespace gtsam { if (throwCheirality_) throw e; } - return ones(2) * 2.0 * K_->fx(); + return Vector::Ones(2) * 2.0 * K_->fx(); } /** return the measurement */ diff --git a/gtsam_unstable/slam/ProjectionFactorPPPC.h b/gtsam_unstable/slam/ProjectionFactorPPPC.h index 1bd352a33..2fd622ea1 100644 --- a/gtsam_unstable/slam/ProjectionFactorPPPC.h +++ b/gtsam_unstable/slam/ProjectionFactorPPPC.h @@ -142,7 +142,7 @@ namespace gtsam { if (throwCheirality_) throw e; } - return ones(2) * 2.0 * K.fx(); + return Vector::Ones(2) * 2.0 * K.fx(); } /** return the measurement */ diff --git a/gtsam_unstable/slam/tests/testSerialization.cpp b/gtsam_unstable/slam/tests/testSerialization.cpp index 1d8b30850..dc05711e3 100644 --- a/gtsam_unstable/slam/tests/testSerialization.cpp +++ b/gtsam_unstable/slam/tests/testSerialization.cpp @@ -48,9 +48,9 @@ Values exampleValues() { NonlinearFactorGraph exampleGraph() { NonlinearFactorGraph graph; - graph.add(PriorFactor(234, Pose2(1.0, 2.0, 0.3), noiseModel::Diagonal::Sigmas(ones(3)))); - graph.add(BetweenFactor(234, 567, Pose2(1.0, 2.0, 0.3), noiseModel::Diagonal::Sigmas(ones(3)))); - graph.add(BearingRangeFactor(234, 567, Rot2::fromAngle(0.3), 2.0, noiseModel::Diagonal::Sigmas(ones(2)))); + graph.add(PriorFactor(234, Pose2(1.0, 2.0, 0.3), noiseModel::Diagonal::Sigmas(Vector::Ones(3)))); + graph.add(BetweenFactor(234, 567, Pose2(1.0, 2.0, 0.3), noiseModel::Diagonal::Sigmas(Vector::Ones(3)))); + graph.add(BearingRangeFactor(234, 567, Rot2::fromAngle(0.3), 2.0, noiseModel::Diagonal::Sigmas(Vector::Ones(2)))); return graph; } diff --git a/tests/smallExample.h b/tests/smallExample.h index d46cc34e1..215655593 100644 --- a/tests/smallExample.h +++ b/tests/smallExample.h @@ -274,7 +274,7 @@ inline GaussianFactorGraph createGaussianFactorGraph() { GaussianFactorGraph fg; // linearized prior on x1: c[_x1_]+x1=0 i.e. x1=-c[_x1_] - fg += JacobianFactor(X(1), 10*I_2x2, -1.0*ones(2)); + fg += JacobianFactor(X(1), 10*I_2x2, -1.0*Vector::Ones(2)); // odometry between x1 and x2: x2-x1=[0.2;-0.1] fg += JacobianFactor(X(1), -10*I_2x2, X(2), 10*I_2x2, Vector2(2.0, -1.0)); diff --git a/tests/testBoundingConstraint.cpp b/tests/testBoundingConstraint.cpp index 18dabf18b..064deca5b 100644 --- a/tests/testBoundingConstraint.cpp +++ b/tests/testBoundingConstraint.cpp @@ -54,8 +54,8 @@ TEST( testBoundingConstraint, unary_basics_inactive1 ) { EXPECT_DOUBLES_EQUAL(2.0, constraint2.threshold(), tol); EXPECT(constraint1.isGreaterThan()); EXPECT(constraint2.isGreaterThan()); - EXPECT(assert_equal(ones(1), constraint1.evaluateError(pt1), tol)); - EXPECT(assert_equal(ones(1), constraint2.evaluateError(pt1), tol)); + EXPECT(assert_equal(Vector::Ones(1), constraint1.evaluateError(pt1), tol)); + EXPECT(assert_equal(Vector::Ones(1), constraint2.evaluateError(pt1), tol)); EXPECT(assert_equal(Z_1x1, constraint1.unwhitenedError(config), tol)); EXPECT(assert_equal(Z_1x1, constraint2.unwhitenedError(config), tol)); EXPECT_DOUBLES_EQUAL(0.0, constraint1.error(config), tol); @@ -103,10 +103,10 @@ TEST( testBoundingConstraint, unary_basics_active2 ) { config.insert(key, pt1); EXPECT(constraint3.active(config)); EXPECT(constraint4.active(config)); - EXPECT(assert_equal(-1.0 * ones(1), constraint3.evaluateError(pt1), tol)); - EXPECT(assert_equal(-1.0 * ones(1), constraint4.evaluateError(pt1), tol)); - EXPECT(assert_equal(-1.0 * ones(1), constraint3.unwhitenedError(config), tol)); - EXPECT(assert_equal(-1.0 * ones(1), constraint4.unwhitenedError(config), tol)); + EXPECT(assert_equal(-1.0 * Vector::Ones(1), constraint3.evaluateError(pt1), tol)); + EXPECT(assert_equal(-1.0 * Vector::Ones(1), constraint4.evaluateError(pt1), tol)); + EXPECT(assert_equal(-1.0 * Vector::Ones(1), constraint3.unwhitenedError(config), tol)); + EXPECT(assert_equal(-1.0 * Vector::Ones(1), constraint4.unwhitenedError(config), tol)); EXPECT_DOUBLES_EQUAL(5.0, constraint3.error(config), tol); EXPECT_DOUBLES_EQUAL(5.0, constraint4.error(config), tol); } @@ -188,9 +188,9 @@ TEST( testBoundingConstraint, MaxDistance_basics) { EXPECT(rangeBound.dim() == 1); EXPECT(assert_equal((Vector(1) << 2.0).finished(), rangeBound.evaluateError(pt1, pt1))); - EXPECT(assert_equal(ones(1), rangeBound.evaluateError(pt1, pt2))); + EXPECT(assert_equal(Vector::Ones(1), rangeBound.evaluateError(pt1, pt2))); EXPECT(assert_equal(Z_1x1, rangeBound.evaluateError(pt1, pt3))); - EXPECT(assert_equal(-1.0*ones(1), rangeBound.evaluateError(pt1, pt4))); + EXPECT(assert_equal(-1.0*Vector::Ones(1), rangeBound.evaluateError(pt1, pt4))); Values config1; config1.insert(key1, pt1); @@ -213,7 +213,7 @@ TEST( testBoundingConstraint, MaxDistance_basics) { config1.update(key2, pt4); EXPECT(rangeBound.active(config1)); - EXPECT(assert_equal(-1.0*ones(1), rangeBound.unwhitenedError(config1))); + EXPECT(assert_equal(-1.0*Vector::Ones(1), rangeBound.unwhitenedError(config1))); EXPECT_DOUBLES_EQUAL(0.5*mu, rangeBound.error(config1), tol); } diff --git a/tests/testGaussianFactorGraphB.cpp b/tests/testGaussianFactorGraphB.cpp index becd407aa..10de57435 100644 --- a/tests/testGaussianFactorGraphB.cpp +++ b/tests/testGaussianFactorGraphB.cpp @@ -97,7 +97,7 @@ TEST( GaussianFactorGraph, eliminateOne_x2 ) // create expected Conditional Gaussian double sig = 0.0894427; Matrix I = I_2x2/sig, R11 = I, S12 = -0.2*I, S13 = -0.8*I; - Vector d = Vector2(0.2, -0.14)/sig, sigma = ones(2); + Vector d = Vector2(0.2, -0.14)/sig, sigma = Vector::Ones(2); GaussianConditional expected(ordering[X(2)],d,R11,ordering[L(1)],S12,ordering[X(1)],S13,sigma); EXPECT(assert_equal(expected,*actual,tol)); @@ -113,7 +113,7 @@ TEST( GaussianFactorGraph, eliminateOne_l1 ) // create expected Conditional Gaussian double sig = sqrt(2.0)/10.; Matrix I = I_2x2/sig, R11 = I, S12 = -0.5*I, S13 = -0.5*I; - Vector d = Vector2(-0.1, 0.25)/sig, sigma = ones(2); + Vector d = Vector2(-0.1, 0.25)/sig, sigma = Vector::Ones(2); GaussianConditional expected(ordering[L(1)],d,R11,ordering[X(1)],S12,ordering[X(2)],S13,sigma); EXPECT(assert_equal(expected,*actual,tol)); @@ -130,7 +130,7 @@ TEST( GaussianFactorGraph, eliminateOne_x1_fast ) // create expected Conditional Gaussian Matrix I = 15*I_2x2, R11 = I, S12 = -0.111111*I, S13 = -0.444444*I; - Vector d = Vector2(-0.133333, -0.0222222), sigma = ones(2); + Vector d = Vector2(-0.133333, -0.0222222), sigma = Vector::Ones(2); GaussianConditional expected(ordering[X(1)],15*d,R11,ordering[L(1)],S12,ordering[X(2)],S13,sigma); // Create expected remaining new factor @@ -160,7 +160,7 @@ TEST( GaussianFactorGraph, eliminateOne_x2_fast ) // create expected Conditional Gaussian double sig = 0.0894427; Matrix I = I_2x2/sig, R11 = I, S12 = -0.2*I, S13 = -0.8*I; - Vector d = Vector2(0.2, -0.14)/sig, sigma = ones(2); + Vector d = Vector2(0.2, -0.14)/sig, sigma = Vector::Ones(2); GaussianConditional expected(ordering[X(2)],d,R11,ordering[X(1)],S13,ordering[L(1)],S12,sigma); EXPECT(assert_equal(expected,*actual,tol)); @@ -176,7 +176,7 @@ TEST( GaussianFactorGraph, eliminateOne_l1_fast ) // create expected Conditional Gaussian double sig = sqrt(2.0)/10.; Matrix I = I_2x2/sig, R11 = I, S12 = -0.5*I, S13 = -0.5*I; - Vector d = Vector2(-0.1, 0.25)/sig, sigma = ones(2); + Vector d = Vector2(-0.1, 0.25)/sig, sigma = Vector::Ones(2); GaussianConditional expected(ordering[L(1)],d,R11,ordering[X(1)],S12,ordering[X(2)],S13,sigma); EXPECT(assert_equal(expected,*actual,tol)); @@ -195,11 +195,11 @@ TEST( GaussianFactorGraph, eliminateAll ) GaussianBayesNet expected = simpleGaussian(ordering[X(1)],d1,0.1); double sig1 = 0.149071; - Vector d2 = Vector2(0.0, 0.2)/sig1, sigma2 = ones(2); + Vector d2 = Vector2(0.0, 0.2)/sig1, sigma2 = Vector::Ones(2); push_front(expected,ordering[L(1)],d2, I/sig1,ordering[X(1)], (-1)*I/sig1,sigma2); double sig2 = 0.0894427; - Vector d3 = Vector2(0.2, -0.14)/sig2, sigma3 = ones(2); + Vector d3 = Vector2(0.2, -0.14)/sig2, sigma3 = Vector::Ones(2); push_front(expected,ordering[X(2)],d3, I/sig2,ordering[L(1)], (-0.2)*I/sig2, ordering[X(1)], (-0.8)*I/sig2, sigma3); // Check one ordering @@ -588,7 +588,7 @@ TEST( GaussianFactorGraph, conditional_sigma_failure) { BOOST_FOREACH(const GaussianBayesTree::sharedClique& clique, actBT.nodes() | br::map_values) { GaussianConditional::shared_ptr conditional = clique->conditional(); //size_t dim = conditional->rows(); - //EXPECT(assert_equal(gtsam::ones(dim), conditional->get_model()->sigmas(), tol)); + //EXPECT(assert_equal(gtsam::Vector::Ones(dim), conditional->get_model()->sigmas(), tol)); EXPECT(!conditional->get_model()); } } diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp index 05cb9c4ad..fb85c3742 100644 --- a/tests/testNonlinearFactor.cpp +++ b/tests/testNonlinearFactor.cpp @@ -91,7 +91,7 @@ TEST( NonlinearFactor, NonlinearFactor ) // calculate the error_vector from the factor "f1" // error_vector = [0.1 0.1] Vector actual_e = boost::dynamic_pointer_cast(factor)->unwhitenedError(cfg); - CHECK(assert_equal(0.1*ones(2),actual_e)); + CHECK(assert_equal(0.1*Vector::Ones(2),actual_e)); // error = 0.5 * [1 1] * [1;1] = 1 double expected = 1.0; From c2183eedb711287735f804441f1c2103c2a848e4 Mon Sep 17 00:00:00 2001 From: Alex Hagiopol Date: Fri, 15 Apr 2016 19:38:28 -0400 Subject: [PATCH 80/91] Deprecated size_t dim(const Vector&). All unit tests pass. --- gtsam/base/Vector.h | 2 +- gtsam/geometry/tests/testOrientedPlane3.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index 3d854916c..16b0643ae 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -251,10 +251,10 @@ inline double sum(const Vector &a){return a.sum();} inline Vector zero(size_t n) { return Vector::Zero(n);} inline Vector ones(size_t n) { return Vector::Ones(n); } +inline size_t dim(const Vector& v) { return v.size(); } #endif inline Vector delta(size_t n, size_t i, double value){ return Vector::Unit(n, i) * value;} inline Vector basis(size_t n, size_t i) { return delta(n, i, 1.0); } -inline size_t dim(const Vector& v) { return v.size(); } } // namespace gtsam diff --git a/gtsam/geometry/tests/testOrientedPlane3.cpp b/gtsam/geometry/tests/testOrientedPlane3.cpp index 7266aaf32..b3d87f18c 100644 --- a/gtsam/geometry/tests/testOrientedPlane3.cpp +++ b/gtsam/geometry/tests/testOrientedPlane3.cpp @@ -96,7 +96,7 @@ inline static Vector randomVector(const Vector& minLimits, const Vector& maxLimits) { // Get the number of dimensions and create the return vector - size_t numDims = dim(minLimits); + size_t numDims = minLimits.size(); Vector vector = Vector::Zero(numDims); // Create the random vector From 2fe0c26f4e998a1277650e81a3a89cf2c8d4a6a9 Mon Sep 17 00:00:00 2001 From: Alex Hagiopol Date: Fri, 15 Apr 2016 20:01:22 -0400 Subject: [PATCH 81/91] Deprecated delta() and basis(). All unit tests pass. --- gtsam/base/Matrix.cpp | 2 +- gtsam/base/Vector.cpp | 2 +- gtsam/base/Vector.h | 4 +--- gtsam/geometry/tests/testPose3.cpp | 2 +- gtsam_unstable/dynamics/DynamicsPriors.h | 2 +- gtsam_unstable/dynamics/PoseRTV.cpp | 4 ++-- gtsam_unstable/dynamics/tests/testIMUSystem.cpp | 6 +++--- .../dynamics/tests/testVelocityConstraint.cpp | 12 ++++++------ gtsam_unstable/geometry/SimPolygon2D.cpp | 2 +- 9 files changed, 17 insertions(+), 19 deletions(-) diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp index ff509b41e..a9d521a7f 100644 --- a/gtsam/base/Matrix.cpp +++ b/gtsam/base/Matrix.cpp @@ -290,7 +290,7 @@ weighted_eliminate(Matrix& A, Vector& b, const Vector& sigmas) { if (precision < 1e-8) continue; // create solution and copy into r - Vector r(basis(n, j)); + Vector r(Vector::Unit(n,j)); for (size_t j2=j+1; j2& vs); */ GTSAM_EXPORT Vector concatVectors(size_t nrVectors, ...); - #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 inline Vector abs(const Vector& v){return v.cwiseAbs();} inline Vector ediv(const Vector &a, const Vector &b) {assert (b.size()==a.size()); return a.cwiseQuotient(b);} @@ -252,10 +251,9 @@ inline double sum(const Vector &a){return a.sum();} inline Vector zero(size_t n) { return Vector::Zero(n);} inline Vector ones(size_t n) { return Vector::Ones(n); } inline size_t dim(const Vector& v) { return v.size(); } -#endif inline Vector delta(size_t n, size_t i, double value){ return Vector::Unit(n, i) * value;} inline Vector basis(size_t n, size_t i) { return delta(n, i, 1.0); } - +#endif } // namespace gtsam #include diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 165870f25..97ccbcb34 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -712,7 +712,7 @@ TEST(Pose3, Bearing2) { TEST( Pose3, unicycle ) { // velocity in X should be X in inertial frame, rather than global frame - Vector x_step = delta(6,3,1.0); + Vector x_step = Vector::Unit(6,3)*1.0; EXPECT(assert_equal(Pose3(Rot3::Ypr(0,0,0), l1), expmap_default(x1, x_step), tol)); EXPECT(assert_equal(Pose3(Rot3::Ypr(0,0,0), Point3(2,1,0)), expmap_default(x2, x_step), tol)); EXPECT(assert_equal(Pose3(Rot3::Ypr(M_PI/4.0,0,0), Point3(2,2,0)), expmap_default(x3, sqrt(2.0) * x_step), tol)); diff --git a/gtsam_unstable/dynamics/DynamicsPriors.h b/gtsam_unstable/dynamics/DynamicsPriors.h index b4dfa7dc8..1e768ef2a 100644 --- a/gtsam_unstable/dynamics/DynamicsPriors.h +++ b/gtsam_unstable/dynamics/DynamicsPriors.h @@ -75,7 +75,7 @@ struct DGroundConstraint : public gtsam::PartialPriorFactor { */ DGroundConstraint(Key key, double height, const gtsam::SharedNoiseModel& model) : Base(key, model) { - this->prior_ = delta(4, 0, height); // [z, vz, roll, pitch] + this->prior_ = Vector::Unit(4,0)*height; // [z, vz, roll, pitch] this->mask_.resize(4); this->mask_[0] = 5; // z = height this->mask_[1] = 8; // vz diff --git a/gtsam_unstable/dynamics/PoseRTV.cpp b/gtsam_unstable/dynamics/PoseRTV.cpp index b6fc61411..c1afe3882 100644 --- a/gtsam_unstable/dynamics/PoseRTV.cpp +++ b/gtsam_unstable/dynamics/PoseRTV.cpp @@ -11,7 +11,7 @@ namespace gtsam { using namespace std; -static const Vector kGravity = delta(3, 2, 9.81); +static const Vector kGravity = Vector::Unit(3,2)*9.81; /* ************************************************************************* */ double bound(double a, double min, double max) { @@ -105,7 +105,7 @@ PoseRTV PoseRTV::flyingDynamics( Vector Acc_n = 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 + + Vector::Unit(3,2)*(loss_lift - lift_control); // falling due to lift lost from pitch // Update Vehicle Position and Velocity Velocity3 v2 = v1 + Velocity3(Acc_n * dt); diff --git a/gtsam_unstable/dynamics/tests/testIMUSystem.cpp b/gtsam_unstable/dynamics/tests/testIMUSystem.cpp index 5fe29a57f..494f2731f 100644 --- a/gtsam_unstable/dynamics/tests/testIMUSystem.cpp +++ b/gtsam_unstable/dynamics/tests/testIMUSystem.cpp @@ -26,7 +26,7 @@ using namespace gtsam; const double tol=1e-5; static const Key x0 = 0, x1 = 1, x2 = 2, x3 = 3, x4 = 4; -static const Vector g = delta(3, 2, -9.81); +static const Vector g = Vector::Unit(3,2)*(-9.81); /* ************************************************************************* */ TEST(testIMUSystem, instantiations) { @@ -149,8 +149,8 @@ TEST( testIMUSystem, linear_trajectory) { const double dt = 1.0; PoseRTV start; - Vector accel = delta(3, 0, 0.5); // forward force - Vector gyro = delta(3, 0, 0.1); // constant rotation + Vector accel = Vector::Unit(3,0)*0.5; // forward force + Vector gyro = Vector::Unit(3,0)*0.1; // constant rotation SharedDiagonal model = noiseModel::Unit::Create(9); Values true_traj, init_traj; diff --git a/gtsam_unstable/dynamics/tests/testVelocityConstraint.cpp b/gtsam_unstable/dynamics/tests/testVelocityConstraint.cpp index 429edb760..6d8206177 100644 --- a/gtsam_unstable/dynamics/tests/testVelocityConstraint.cpp +++ b/gtsam_unstable/dynamics/tests/testVelocityConstraint.cpp @@ -27,8 +27,8 @@ TEST( testVelocityConstraint, trapezoidal ) { // verify error function EXPECT(assert_equal(Z_3x1, constraint.evaluateError(origin, pose1), tol)); EXPECT(assert_equal(Z_3x1, constraint.evaluateError(origin, origin), tol)); - EXPECT(assert_equal(delta(3, 0,-1.0), constraint.evaluateError(pose1, pose1), tol)); - EXPECT(assert_equal(delta(3, 0, 0.5), constraint.evaluateError(origin, pose1a), tol)); + EXPECT(assert_equal(Vector::Unit(3,0)*(-1.0), constraint.evaluateError(pose1, pose1), tol)); + EXPECT(assert_equal(Vector::Unit(3,0)*0.5, constraint.evaluateError(origin, pose1a), tol)); } /* ************************************************************************* */ @@ -37,10 +37,10 @@ TEST( testEulerVelocityConstraint, euler_start ) { VelocityConstraint constraint(x1, x2, dynamics::EULER_START, dt); // verify error function - EXPECT(assert_equal(delta(3, 0, 0.5), constraint.evaluateError(origin, pose1), tol)); + EXPECT(assert_equal(Vector::Unit(3,0)*0.5, constraint.evaluateError(origin, pose1), tol)); EXPECT(assert_equal(Z_3x1, constraint.evaluateError(origin, origin), tol)); EXPECT(assert_equal(Z_3x1, constraint.evaluateError(pose1, pose2), tol)); - EXPECT(assert_equal(delta(3, 0, 0.5), constraint.evaluateError(origin, pose1a), tol)); + EXPECT(assert_equal(Vector::Unit(3,0)*0.5, constraint.evaluateError(origin, pose1a), tol)); } /* ************************************************************************* */ @@ -49,10 +49,10 @@ TEST( testEulerVelocityConstraint, euler_end ) { VelocityConstraint constraint(x1, x2, dynamics::EULER_END, dt); // verify error function - EXPECT(assert_equal(delta(3, 0,-0.5), constraint.evaluateError(origin, pose1), tol)); + EXPECT(assert_equal(Vector::Unit(3,0)*(-0.5), constraint.evaluateError(origin, pose1), tol)); EXPECT(assert_equal(Z_3x1, constraint.evaluateError(origin, origin), tol)); EXPECT(assert_equal(Z_3x1, constraint.evaluateError(pose1, pose2), tol)); - EXPECT(assert_equal(delta(3, 0, 0.5), constraint.evaluateError(origin, pose1a), tol)); + EXPECT(assert_equal(Vector::Unit(3,0)*0.5, constraint.evaluateError(origin, pose1a), tol)); } /* ************************************************************************* */ diff --git a/gtsam_unstable/geometry/SimPolygon2D.cpp b/gtsam_unstable/geometry/SimPolygon2D.cpp index f60ea107c..90e3eeea6 100644 --- a/gtsam_unstable/geometry/SimPolygon2D.cpp +++ b/gtsam_unstable/geometry/SimPolygon2D.cpp @@ -149,7 +149,7 @@ SimPolygon2D SimPolygon2D::randomTriangle( // extend from B to find C double dBC = randomDistance(mean_side_len, sigma_side_len, min_side_len); - Pose2 xC = xB.retract(delta(3, 0, dBC)); + Pose2 xC = xB.retract(Vector::Unit(3,0)*dBC); // use triangle equality to verify non-degenerate triangle double dAC = xA.t().distance(xC.t()); From e2a0110f3bf5ca188bd0049c8350df326b0b4629 Mon Sep 17 00:00:00 2001 From: Alex Hagiopol Date: Fri, 15 Apr 2016 20:21:34 -0400 Subject: [PATCH 82/91] Deprecated bool zero(). All unit tests pass. --- gtsam/base/Vector.cpp | 9 --------- gtsam/base/Vector.h | 17 ++++++----------- gtsam/base/tests/testVector.cpp | 7 ------- gtsam/geometry/Rot2.cpp | 2 +- 4 files changed, 7 insertions(+), 28 deletions(-) diff --git a/gtsam/base/Vector.cpp b/gtsam/base/Vector.cpp index b3945c6a6..40d819de9 100644 --- a/gtsam/base/Vector.cpp +++ b/gtsam/base/Vector.cpp @@ -33,15 +33,6 @@ using namespace std; namespace gtsam { -/* ************************************************************************* */ -bool zero(const Vector& v) { - bool result = true; - size_t n = v.size(); - for( size_t j = 0 ; j < n ; j++) - result = result && (v(j) == 0.0); - return result; -} - /* ************************************************************************* */ //3 argument call void print(const Vector& v, const string& s, ostream& stream) { diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index 4b15300b2..ec1c4dcd2 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -62,11 +62,6 @@ GTSAM_MAKE_VECTOR_DEFS(12); typedef Eigen::VectorBlock SubVector; typedef Eigen::VectorBlock ConstSubVector; -/** - * check if all zero - */ -GTSAM_EXPORT bool zero(const Vector& v); - /** * print without optional string, must specify cout yourself */ @@ -237,22 +232,22 @@ GTSAM_EXPORT Vector concatVectors(size_t nrVectors, ...); #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 inline Vector abs(const Vector& v){return v.cwiseAbs();} +inline Vector basis(size_t n, size_t i) { return delta(n, i, 1.0); } +inline Vector delta(size_t n, size_t i, double value){ return Vector::Unit(n, i) * value;} +inline size_t dim(const Vector& v) { return v.size(); } inline Vector ediv(const Vector &a, const Vector &b) {assert (b.size()==a.size()); return a.cwiseQuotient(b);} inline Vector esqrt(const Vector& v) { return v.cwiseSqrt();} inline Vector emul(const Vector &a, const Vector &b) {assert (b.size()==a.size()); return a.cwiseProduct(b);} inline double max(const Vector &a){return a.maxCoeff();} inline double norm_2(const Vector& v) {return v.norm();} +inline Vector ones(size_t n) { return Vector::Ones(n); } inline Vector reciprocal(const Vector &a) {return a.array().inverse();} inline Vector repeat(size_t n, double value) {return Vector::Constant(n, value);} inline const Vector sub(const Vector &v, size_t i1, size_t i2) {return v.segment(i1,i2-i1);} inline void subInsert(Vector& fullVector, const Vector& subVector, size_t i) {fullVector.segment(i, subVector.size()) = subVector;} inline double sum(const Vector &a){return a.sum();} - -inline Vector zero(size_t n) { return Vector::Zero(n);} -inline Vector ones(size_t n) { return Vector::Ones(n); } -inline size_t dim(const Vector& v) { return v.size(); } -inline Vector delta(size_t n, size_t i, double value){ return Vector::Unit(n, i) * value;} -inline Vector basis(size_t n, size_t i) { return delta(n, i, 1.0); } +inline bool zero(const Vector& v){ return v.isZero(); } +inline Vector zero(size_t n) { return Vector::Zero(n); } #endif } // namespace gtsam diff --git a/gtsam/base/tests/testVector.cpp b/gtsam/base/tests/testVector.cpp index 0fb80d7f0..8a54d5469 100644 --- a/gtsam/base/tests/testVector.cpp +++ b/gtsam/base/tests/testVector.cpp @@ -79,13 +79,6 @@ TEST(Vector, copy ) EXPECT(assert_equal(a, b)); } -/* ************************************************************************* */ -TEST(Vector, zero1 ) -{ - Vector v = Vector::Zero(2); - EXPECT(zero(v)); -} - /* ************************************************************************* */ TEST(Vector, scalar_multiply ) { diff --git a/gtsam/geometry/Rot2.cpp b/gtsam/geometry/Rot2.cpp index 41d56b6e3..9780cb49a 100644 --- a/gtsam/geometry/Rot2.cpp +++ b/gtsam/geometry/Rot2.cpp @@ -63,7 +63,7 @@ Rot2& Rot2::normalize() { Rot2 Rot2::Expmap(const Vector1& v, OptionalJacobian<1, 1> H) { if (H) *H = I_1x1; - if (zero(v)) + if (v.isZero()) return (Rot2()); else return Rot2::fromAngle(v(0)); From d9d8e4be6e29964d4ebc3c41dc2a2cec71fe59a9 Mon Sep 17 00:00:00 2001 From: Alex Hagiopol Date: Fri, 15 Apr 2016 20:39:32 -0400 Subject: [PATCH 83/91] Removed missed usage of delta() in Vector.h. --- gtsam/base/Vector.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index ec1c4dcd2..7fbc7dae6 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -232,7 +232,7 @@ GTSAM_EXPORT Vector concatVectors(size_t nrVectors, ...); #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 inline Vector abs(const Vector& v){return v.cwiseAbs();} -inline Vector basis(size_t n, size_t i) { return delta(n, i, 1.0); } +inline Vector basis(size_t n, size_t i) { return Vector::Unit(n,i); } inline Vector delta(size_t n, size_t i, double value){ return Vector::Unit(n, i) * value;} inline size_t dim(const Vector& v) { return v.size(); } inline Vector ediv(const Vector &a, const Vector &b) {assert (b.size()==a.size()); return a.cwiseQuotient(b);} From df2693d2aec3712b3268e656db2403922c2bb7ea Mon Sep 17 00:00:00 2001 From: Alex Hagiopol Date: Sat, 16 Apr 2016 12:20:28 -0400 Subject: [PATCH 84/91] Use Matrix::Constant or Vector::Constant where appropriate. --- gtsam/base/numericalDerivative.h | 2 +- gtsam/slam/ProjectionFactor.h | 2 +- gtsam/slam/StereoFactor.h | 2 +- gtsam/slam/TriangulationFactor.h | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h index ba72db820..6cd28b951 100644 --- a/gtsam/base/numericalDerivative.h +++ b/gtsam/base/numericalDerivative.h @@ -88,7 +88,7 @@ typename internal::FixedSizeMatrix::type numericalGradient(boost::function g; g.setZero(); // Can be fixed size for (int j = 0; j < N; j++) { d(j) = delta; double hxplus = h(traits::Retract(x, d)); diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h index 5f0c3b1d3..dee8e925f 100644 --- a/gtsam/slam/ProjectionFactor.h +++ b/gtsam/slam/ProjectionFactor.h @@ -154,7 +154,7 @@ namespace gtsam { if (throwCheirality_) throw e; } - return Vector::Ones(2) * 2.0 * K_->fx(); + return Vector2::Constant(2.0 * K_->fx()); } /** return the measurement */ diff --git a/gtsam/slam/StereoFactor.h b/gtsam/slam/StereoFactor.h index c330276d3..59fc372cb 100644 --- a/gtsam/slam/StereoFactor.h +++ b/gtsam/slam/StereoFactor.h @@ -146,7 +146,7 @@ public: if (throwCheirality_) throw e; } - return Vector::Ones(3) * 2.0 * K_->fx(); + return Vector3::Constant(2.0 * K_->fx()); } /** return the measured */ diff --git a/gtsam/slam/TriangulationFactor.h b/gtsam/slam/TriangulationFactor.h index 4f86023ca..e97cd2730 100644 --- a/gtsam/slam/TriangulationFactor.h +++ b/gtsam/slam/TriangulationFactor.h @@ -131,7 +131,7 @@ public: << std::endl; if (throwCheirality_) throw e; - return Vector::Ones(Measurement::dimension) * 2.0 * camera_.calibration().fx(); + return Eigen::Matrix::Constant(2.0 * camera_.calibration().fx()); } } From b021ccef36403fb5afdb4c64ed8a80bec31a533e Mon Sep 17 00:00:00 2001 From: Alex Hagiopol Date: Sat, 16 Apr 2016 12:32:48 -0400 Subject: [PATCH 85/91] Global replace of Vector::Ones(1) and Matrix::Ones(1,1) with I_1x1 --- gtsam/geometry/Rot2.h | 4 ++-- gtsam_unstable/linear/tests/testQPSolver.cpp | 24 ++++++++++---------- tests/testBoundingConstraint.cpp | 18 +++++++-------- 3 files changed, 23 insertions(+), 23 deletions(-) diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index fb54156df..65dd5f609 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -119,12 +119,12 @@ namespace gtsam { /// Left-trivialized derivative of the exponential map static Matrix ExpmapDerivative(const Vector& /*v*/) { - return Vector::Ones(1); + return I_1x1; } /// Left-trivialized derivative inverse of the exponential map static Matrix LogmapDerivative(const Vector& /*v*/) { - return Vector::Ones(1); + return I_1x1; } // Chart at origin simply uses exponential map and its inverse diff --git a/gtsam_unstable/linear/tests/testQPSolver.cpp b/gtsam_unstable/linear/tests/testQPSolver.cpp index 9a67cbb53..7442540f5 100644 --- a/gtsam_unstable/linear/tests/testQPSolver.cpp +++ b/gtsam_unstable/linear/tests/testQPSolver.cpp @@ -26,7 +26,7 @@ using namespace std; using namespace gtsam; using namespace gtsam::symbol_shorthand; -const Matrix One = Matrix::Ones(1,1); +const Matrix One = I_1x1; /* ************************************************************************* */ // Create test graph according to Forst10book_pg171Ex5 @@ -38,14 +38,14 @@ QP createTestCase() { // 0.5*x1'*G11*x1 + x1'*G12*x2 + 0.5*x2'*G22*x2 - x1'*g1 - x2'*g2 + 0.5*f // Hence, we have G11=2, G12 = -1, g1 = +3, G22 = 2, g2 = 0, f = 10 qp.cost.push_back( - HessianFactor(X(1), X(2), 2.0 * Matrix::Ones(1, 1), -Matrix::Ones(1, 1), 3.0 * Vector::Ones(1), + HessianFactor(X(1), X(2), 2.0 * Matrix::Ones(1, 1), -Matrix::Ones(1, 1), 3.0 * I_1x1, 2.0 * Matrix::Ones(1, 1), Z_1x1, 10.0)); // Inequality constraints - qp.inequalities.push_back(LinearInequality(X(1), Matrix::Ones(1,1), X(2), Matrix::Ones(1,1), 2, 0)); // x1 + x2 <= 2 --> x1 + x2 -2 <= 0, --> b=2 - qp.inequalities.push_back(LinearInequality(X(1), -Matrix::Ones(1,1), 0, 1)); // -x1 <= 0 - qp.inequalities.push_back(LinearInequality(X(2), -Matrix::Ones(1,1), 0, 2)); // -x2 <= 0 - qp.inequalities.push_back(LinearInequality(X(1), Matrix::Ones(1,1), 1.5, 3)); // x1 <= 3/2 + qp.inequalities.push_back(LinearInequality(X(1), I_1x1, X(2), I_1x1, 2, 0)); // x1 + x2 <= 2 --> x1 + x2 -2 <= 0, --> b=2 + qp.inequalities.push_back(LinearInequality(X(1), -I_1x1, 0, 1)); // -x1 <= 0 + qp.inequalities.push_back(LinearInequality(X(2), -I_1x1, 0, 2)); // -x2 <= 0 + qp.inequalities.push_back(LinearInequality(X(1), I_1x1, 1.5, 3)); // x1 <= 3/2 return qp; } @@ -110,8 +110,8 @@ TEST(QPSolver, dual) { // Initials values VectorValues initialValues; - initialValues.insert(X(1), Vector::Ones(1)); - initialValues.insert(X(2), Vector::Ones(1)); + initialValues.insert(X(1), I_1x1); + initialValues.insert(X(2), I_1x1); QPSolver solver(qp); @@ -219,8 +219,8 @@ QP createTestMatlabQPEx() { // 0.5*x1'*G11*x1 + x1'*G12*x2 + 0.5*x2'*G22*x2 - x1'*g1 - x2'*g2 + 0.5*f // Hence, we have G11=1, G12 = -1, g1 = +2, G22 = 2, g2 = +6, f = 0 qp.cost.push_back( - HessianFactor(X(1), X(2), 1.0 * Matrix::Ones(1,1), -Matrix::Ones(1, 1), 2.0 * Vector::Ones(1), - 2.0 * Matrix::Ones(1, 1), 6 * Vector::Ones(1), 1000.0)); + HessianFactor(X(1), X(2), 1.0 * I_1x1, -Matrix::Ones(1, 1), 2.0 * I_1x1, + 2.0 * Matrix::Ones(1, 1), 6 * I_1x1, 1000.0)); // Inequality constraints qp.inequalities.push_back(LinearInequality(X(1), One, X(2), One, 2, 0)); // x1 + x2 <= 2 @@ -251,8 +251,8 @@ TEST(QPSolver, optimizeMatlabEx) { QP createTestNocedal06bookEx16_4() { QP qp; - qp.cost.push_back(JacobianFactor(X(1), Matrix::Ones(1, 1), Vector::Ones(1))); - qp.cost.push_back(JacobianFactor(X(2), Matrix::Ones(1, 1), 2.5 * Vector::Ones(1))); + qp.cost.push_back(JacobianFactor(X(1), Matrix::Ones(1, 1), I_1x1)); + qp.cost.push_back(JacobianFactor(X(2), Matrix::Ones(1, 1), 2.5 * I_1x1)); // Inequality constraints qp.inequalities.push_back(LinearInequality(X(1), -One, X(2), 2 * One, 2, 0)); diff --git a/tests/testBoundingConstraint.cpp b/tests/testBoundingConstraint.cpp index 064deca5b..b0b748d95 100644 --- a/tests/testBoundingConstraint.cpp +++ b/tests/testBoundingConstraint.cpp @@ -54,8 +54,8 @@ TEST( testBoundingConstraint, unary_basics_inactive1 ) { EXPECT_DOUBLES_EQUAL(2.0, constraint2.threshold(), tol); EXPECT(constraint1.isGreaterThan()); EXPECT(constraint2.isGreaterThan()); - EXPECT(assert_equal(Vector::Ones(1), constraint1.evaluateError(pt1), tol)); - EXPECT(assert_equal(Vector::Ones(1), constraint2.evaluateError(pt1), tol)); + EXPECT(assert_equal(I_1x1, constraint1.evaluateError(pt1), tol)); + EXPECT(assert_equal(I_1x1, constraint2.evaluateError(pt1), tol)); EXPECT(assert_equal(Z_1x1, constraint1.unwhitenedError(config), tol)); EXPECT(assert_equal(Z_1x1, constraint2.unwhitenedError(config), tol)); EXPECT_DOUBLES_EQUAL(0.0, constraint1.error(config), tol); @@ -103,10 +103,10 @@ TEST( testBoundingConstraint, unary_basics_active2 ) { config.insert(key, pt1); EXPECT(constraint3.active(config)); EXPECT(constraint4.active(config)); - EXPECT(assert_equal(-1.0 * Vector::Ones(1), constraint3.evaluateError(pt1), tol)); - EXPECT(assert_equal(-1.0 * Vector::Ones(1), constraint4.evaluateError(pt1), tol)); - EXPECT(assert_equal(-1.0 * Vector::Ones(1), constraint3.unwhitenedError(config), tol)); - EXPECT(assert_equal(-1.0 * Vector::Ones(1), constraint4.unwhitenedError(config), tol)); + EXPECT(assert_equal(-1.0 * I_1x1, constraint3.evaluateError(pt1), tol)); + EXPECT(assert_equal(-1.0 * I_1x1, constraint4.evaluateError(pt1), tol)); + EXPECT(assert_equal(-1.0 * I_1x1, constraint3.unwhitenedError(config), tol)); + EXPECT(assert_equal(-1.0 * I_1x1, constraint4.unwhitenedError(config), tol)); EXPECT_DOUBLES_EQUAL(5.0, constraint3.error(config), tol); EXPECT_DOUBLES_EQUAL(5.0, constraint4.error(config), tol); } @@ -188,9 +188,9 @@ TEST( testBoundingConstraint, MaxDistance_basics) { EXPECT(rangeBound.dim() == 1); EXPECT(assert_equal((Vector(1) << 2.0).finished(), rangeBound.evaluateError(pt1, pt1))); - EXPECT(assert_equal(Vector::Ones(1), rangeBound.evaluateError(pt1, pt2))); + EXPECT(assert_equal(I_1x1, rangeBound.evaluateError(pt1, pt2))); EXPECT(assert_equal(Z_1x1, rangeBound.evaluateError(pt1, pt3))); - EXPECT(assert_equal(-1.0*Vector::Ones(1), rangeBound.evaluateError(pt1, pt4))); + EXPECT(assert_equal(-1.0*I_1x1, rangeBound.evaluateError(pt1, pt4))); Values config1; config1.insert(key1, pt1); @@ -213,7 +213,7 @@ TEST( testBoundingConstraint, MaxDistance_basics) { config1.update(key2, pt4); EXPECT(rangeBound.active(config1)); - EXPECT(assert_equal(-1.0*Vector::Ones(1), rangeBound.unwhitenedError(config1))); + EXPECT(assert_equal(-1.0*I_1x1, rangeBound.unwhitenedError(config1))); EXPECT_DOUBLES_EQUAL(0.5*mu, rangeBound.error(config1), tol); } From f78523b1b74d03ebe099899339c33568acf38f6a Mon Sep 17 00:00:00 2001 From: Alex Hagiopol Date: Sat, 16 Apr 2016 12:36:03 -0400 Subject: [PATCH 86/91] Edit authors. --- gtsam/base/Matrix.h | 1 + gtsam/base/Vector.h | 1 + 2 files changed, 2 insertions(+) diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index 9c6ff0bb4..235cd30f3 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -16,6 +16,7 @@ * @author Kai Ni * @author Frank Dellaert * @author Alex Cunningham + * @author Alex Hagiopol */ // \callgraph diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index 7fbc7dae6..91aec85b8 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -14,6 +14,7 @@ * @brief typedef and functions to augment Eigen's VectorXd * @author Kai Ni * @author Frank Dellaert + * @author Alex Hagiopol */ // \callgraph From 2c7d2dfff42bf326f37a6084c8860ddf297117a6 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 16 Apr 2016 12:49:08 -0700 Subject: [PATCH 87/91] Ingested CCOLAMD 2.9.5 from April 1, 2016 --- gtsam/3rdparty/CCOLAMD/Demo/Makefile | 49 ++ gtsam/3rdparty/CCOLAMD/Demo/ccolamd_example.c | 2 - .../3rdparty/CCOLAMD/Demo/ccolamd_example.out | 4 +- .../3rdparty/CCOLAMD/Demo/ccolamd_l_example.c | 22 +- .../CCOLAMD/Demo/ccolamd_l_example.out | 4 +- gtsam/3rdparty/CCOLAMD/Doc/ChangeLog | 34 + gtsam/3rdparty/CCOLAMD/Doc/License.txt | 21 + gtsam/3rdparty/CCOLAMD/Include/ccolamd.h | 283 ++++----- gtsam/3rdparty/CCOLAMD/Lib/Makefile | 71 +++ gtsam/3rdparty/CCOLAMD/MATLAB/ccolamd_demo.m | 29 +- gtsam/3rdparty/CCOLAMD/MATLAB/ccolamd_make.m | 23 +- gtsam/3rdparty/CCOLAMD/MATLAB/ccolamd_test.m | 9 +- gtsam/3rdparty/CCOLAMD/MATLAB/ccolamdmex.c | 52 +- .../3rdparty/CCOLAMD/MATLAB/ccolamdtestmex.c | 68 +- gtsam/3rdparty/CCOLAMD/MATLAB/csymamd.m | 4 +- gtsam/3rdparty/CCOLAMD/MATLAB/csymamdmex.c | 46 +- .../3rdparty/CCOLAMD/MATLAB/csymamdtestmex.c | 66 +- gtsam/3rdparty/CCOLAMD/Makefile | 51 ++ gtsam/3rdparty/CCOLAMD/README.txt | 58 +- gtsam/3rdparty/CCOLAMD/Source/ccolamd.c | 180 +++--- gtsam/3rdparty/CMakeLists.txt | 2 +- gtsam/3rdparty/SuiteSparse_config/Makefile | 70 ++ gtsam/3rdparty/SuiteSparse_config/README.txt | 51 ++ .../SuiteSparse_config/SuiteSparse_config.c | 531 ++++++++++++++++ .../SuiteSparse_config/SuiteSparse_config.h | 248 ++++++++ .../SuiteSparse_config/SuiteSparse_config.mk | 600 ++++++++++++++++++ .../SuiteSparse_config/xerbla/Makefile | 73 +++ .../xerbla/xerbla.c | 0 .../xerbla/xerbla.f | 0 .../xerbla/xerbla.h | 0 gtsam/3rdparty/UFconfig/README.txt | 35 - gtsam/3rdparty/UFconfig/UFconfig.c | 71 --- gtsam/3rdparty/UFconfig/UFconfig.h | 152 ----- gtsam/3rdparty/UFconfig/UFconfig.mk | 386 ----------- gtsam/CMakeLists.txt | 2 +- 35 files changed, 2202 insertions(+), 1095 deletions(-) create mode 100644 gtsam/3rdparty/CCOLAMD/Demo/Makefile create mode 100644 gtsam/3rdparty/CCOLAMD/Doc/License.txt create mode 100644 gtsam/3rdparty/CCOLAMD/Lib/Makefile create mode 100644 gtsam/3rdparty/CCOLAMD/Makefile create mode 100644 gtsam/3rdparty/SuiteSparse_config/Makefile create mode 100644 gtsam/3rdparty/SuiteSparse_config/README.txt create mode 100644 gtsam/3rdparty/SuiteSparse_config/SuiteSparse_config.c create mode 100644 gtsam/3rdparty/SuiteSparse_config/SuiteSparse_config.h create mode 100644 gtsam/3rdparty/SuiteSparse_config/SuiteSparse_config.mk create mode 100644 gtsam/3rdparty/SuiteSparse_config/xerbla/Makefile rename gtsam/3rdparty/{UFconfig => SuiteSparse_config}/xerbla/xerbla.c (100%) rename gtsam/3rdparty/{UFconfig => SuiteSparse_config}/xerbla/xerbla.f (100%) rename gtsam/3rdparty/{UFconfig => SuiteSparse_config}/xerbla/xerbla.h (100%) delete mode 100644 gtsam/3rdparty/UFconfig/README.txt delete mode 100644 gtsam/3rdparty/UFconfig/UFconfig.c delete mode 100644 gtsam/3rdparty/UFconfig/UFconfig.h delete mode 100644 gtsam/3rdparty/UFconfig/UFconfig.mk diff --git a/gtsam/3rdparty/CCOLAMD/Demo/Makefile b/gtsam/3rdparty/CCOLAMD/Demo/Makefile new file mode 100644 index 000000000..c0ad95683 --- /dev/null +++ b/gtsam/3rdparty/CCOLAMD/Demo/Makefile @@ -0,0 +1,49 @@ +#----------------------------------------------------------------------------- +# compile the CCOLAMD demo +#----------------------------------------------------------------------------- + +default: all + +include ../../SuiteSparse_config/SuiteSparse_config.mk + +I = -I../../include + +C = $(CC) $(CF) $(I) + +LIB2 = $(LDFLAGS) -L../../lib -lccolamd -lsuitesparseconfig $(LDLIBS) + +all: library ccolamd_example ccolamd_l_example + +library: + ( cd ../../SuiteSparse_config ; $(MAKE) ) + ( cd ../Lib ; $(MAKE) ) + +#------------------------------------------------------------------------------ +# Create the demo program, run it, and compare the output +#------------------------------------------------------------------------------ + +dist: + +ccolamd_example: ccolamd_example.c + $(C) -o ccolamd_example ccolamd_example.c $(LIB2) + - ./ccolamd_example > my_ccolamd_example.out + - diff ccolamd_example.out my_ccolamd_example.out + +ccolamd_l_example: ccolamd_l_example.c + $(C) -o ccolamd_l_example ccolamd_l_example.c $(LIB2) + - ./ccolamd_l_example > my_ccolamd_l_example.out + - diff ccolamd_l_example.out my_ccolamd_l_example.out + +#------------------------------------------------------------------------------ +# Remove all but the files in the original distribution +#------------------------------------------------------------------------------ + +clean: + - $(RM) -r $(CLEAN) + +purge: distclean + +distclean: clean + - $(RM) ccolamd_example ccolamd_l_example + - $(RM) my_ccolamd_example.out my_ccolamd_l_example.out + - $(RM) -r $(PURGE) diff --git a/gtsam/3rdparty/CCOLAMD/Demo/ccolamd_example.c b/gtsam/3rdparty/CCOLAMD/Demo/ccolamd_example.c index 253fdc988..44423574e 100644 --- a/gtsam/3rdparty/CCOLAMD/Demo/ccolamd_example.c +++ b/gtsam/3rdparty/CCOLAMD/Demo/ccolamd_example.c @@ -5,8 +5,6 @@ /* ---------------------------------------------------------------------------- * CCOLAMD Copyright (C), Univ. of Florida. Authors: Timothy A. Davis, * Sivasankaran Rajamanickam, and Stefan Larimore - * See License.txt for the Version 2.1 of the GNU Lesser General Public License - * http://www.cise.ufl.edu/research/sparse * -------------------------------------------------------------------------- */ /* diff --git a/gtsam/3rdparty/CCOLAMD/Demo/ccolamd_example.out b/gtsam/3rdparty/CCOLAMD/Demo/ccolamd_example.out index dd2dc4955..b456b0f11 100644 --- a/gtsam/3rdparty/CCOLAMD/Demo/ccolamd_example.out +++ b/gtsam/3rdparty/CCOLAMD/Demo/ccolamd_example.out @@ -15,7 +15,7 @@ Column 3, with 2 entries: row 1 row 3 -ccolamd version 2.7, Jan 25, 2011: OK. +ccolamd version 2.9, Apr 1, 2016: OK. ccolamd: number of dense or empty rows ignored: 0 ccolamd: number of dense or empty columns ignored: 0 ccolamd: number of garbage collections performed: 0 @@ -38,7 +38,7 @@ Column 3, with 1 entries: row 4 Column 4, with 0 entries: -csymamd version 2.7, Jan 25, 2011: OK. +csymamd version 2.9, Apr 1, 2016: OK. csymamd: number of dense or empty rows ignored: 0 csymamd: number of dense or empty columns ignored: 0 csymamd: number of garbage collections performed: 0 diff --git a/gtsam/3rdparty/CCOLAMD/Demo/ccolamd_l_example.c b/gtsam/3rdparty/CCOLAMD/Demo/ccolamd_l_example.c index b213aad0e..b68e1d0d6 100644 --- a/gtsam/3rdparty/CCOLAMD/Demo/ccolamd_l_example.c +++ b/gtsam/3rdparty/CCOLAMD/Demo/ccolamd_l_example.c @@ -1,12 +1,10 @@ /* ========================================================================== */ -/* === ccolamd and csymamd example (UF_long integer version) ================ */ +/* === ccolamd and csymamd example (long integer version) =================== */ /* ========================================================================== */ /* ---------------------------------------------------------------------------- * CCOLAMD Copyright (C), Univ. of Florida. Authors: Timothy A. Davis, * Sivasankaran Rajamanickam, and Stefan Larimore - * See License.txt for the Version 2.1 of the GNU Lesser General Public License - * http://www.cise.ufl.edu/research/sparse * -------------------------------------------------------------------------- */ /* @@ -46,9 +44,6 @@ #define B_NNZ 4 #define B_N 5 -/* define UF_long */ -#include "UFconfig.h" - int main (void) { @@ -56,14 +51,14 @@ int main (void) /* input matrix A definition */ /* ====================================================================== */ - UF_long A [ALEN] = { + SuiteSparse_long A [ALEN] = { 0, 1, 4, /* row indices of nonzeros in column 0 */ 2, 4, /* row indices of nonzeros in column 1 */ 0, 1, 2, 3, /* row indices of nonzeros in column 2 */ 1, 3} ; /* row indices of nonzeros in column 3 */ - UF_long p [ ] = { + SuiteSparse_long p [ ] = { 0, /* column 0 is in A [0..2] */ 3, /* column 1 is in A [3..4] */ @@ -75,7 +70,7 @@ int main (void) /* input matrix B definition */ /* ====================================================================== */ - UF_long B [ ] = { /* Note: only strictly lower triangular part */ + SuiteSparse_long B [ ] = { /* Note: only strictly lower triangular part */ /* is included, since symamd ignores the */ /* diagonal and upper triangular part of B. */ @@ -85,7 +80,7 @@ int main (void) 4 /* row indices of nonzeros in column 3 */ } ; /* row indices of nonzeros in column 4 (none) */ - UF_long q [ ] = { + SuiteSparse_long q [ ] = { 0, /* column 0 is in B [0] */ 1, /* column 1 is in B [1..2] */ @@ -98,10 +93,9 @@ int main (void) /* other variable definitions */ /* ====================================================================== */ - UF_long perm [B_N+1] ; /* note the size is N+1 */ - UF_long stats [CCOLAMD_STATS] ; /* for ccolamd and csymamd output stats */ - - UF_long row, col, pp, length, ok ; + SuiteSparse_long perm [B_N+1] ; /* note the size is N+1 */ + SuiteSparse_long stats [CCOLAMD_STATS] ; /* ccolamd/csymamd output stats */ + SuiteSparse_long row, col, pp, length, ok ; /* ====================================================================== */ /* dump the input matrix A */ diff --git a/gtsam/3rdparty/CCOLAMD/Demo/ccolamd_l_example.out b/gtsam/3rdparty/CCOLAMD/Demo/ccolamd_l_example.out index fc87f5474..559c66098 100644 --- a/gtsam/3rdparty/CCOLAMD/Demo/ccolamd_l_example.out +++ b/gtsam/3rdparty/CCOLAMD/Demo/ccolamd_l_example.out @@ -15,7 +15,7 @@ Column 3, with 2 entries: row 1 row 3 -ccolamd version 2.7, Jan 25, 2011: OK. +ccolamd version 2.9, Apr 1, 2016: OK. ccolamd: number of dense or empty rows ignored: 0 ccolamd: number of dense or empty columns ignored: 0 ccolamd: number of garbage collections performed: 0 @@ -38,7 +38,7 @@ Column 3, with 1 entries: row 4 Column 4, with 0 entries: -csymamd version 2.7, Jan 25, 2011: OK. +csymamd version 2.9, Apr 1, 2016: OK. csymamd: number of dense or empty rows ignored: 0 csymamd: number of dense or empty columns ignored: 0 csymamd: number of garbage collections performed: 0 diff --git a/gtsam/3rdparty/CCOLAMD/Doc/ChangeLog b/gtsam/3rdparty/CCOLAMD/Doc/ChangeLog index 499d2fc69..85f375c7a 100644 --- a/gtsam/3rdparty/CCOLAMD/Doc/ChangeLog +++ b/gtsam/3rdparty/CCOLAMD/Doc/ChangeLog @@ -1,3 +1,37 @@ +Apr 1, 2016: version 2.9.5 + + * licensing simplified (no other change); refer to CCOLAMD/Doc/License.txt + +Feb 1, 2016: version 2.9.4 + + * update to Makefiles + +Jan 30, 2016: version 2.9.3 + + * modifications to Makefiles + +Jan 1, 2016: version 2.9.2 + + * modified Makefile to create shared libraries + No change to C code except version number. + The empty file ccolamd_global.c removed. + +Oct 10, 2014: version 2.9.1 + + modified MATLAB/ccolamd_make.m. No change to C code except version number. + +July 31, 2013: version 2.9.0 + + * changed malloc and printf pointers to use SuiteSparse_config + +Jun 1, 2012: version 2.8.0 + + * changed from UFconfig to SuiteSparse_config + +Dec 7, 2011: version 2.7.4 + + * fixed the Makefile to better align with CFLAGS and other standards + Jan 25, 2011: version 2.7.3 * minor fix to "make install" diff --git a/gtsam/3rdparty/CCOLAMD/Doc/License.txt b/gtsam/3rdparty/CCOLAMD/Doc/License.txt new file mode 100644 index 000000000..089509a6b --- /dev/null +++ b/gtsam/3rdparty/CCOLAMD/Doc/License.txt @@ -0,0 +1,21 @@ +CCOLAMD: constrained column approximate minimum degree ordering +Copyright (C) 2005-2016, Univ. of Florida. Authors: Timothy A. Davis, +Sivasankaran Rajamanickam, and Stefan Larimore. Closely based on COLAMD by +Davis, Stefan Larimore, in collaboration with Esmond Ng, and John Gilbert. +http://www.suitesparse.com + +-------------------------------------------------------------------------------- + +CCOLAMD is free software; you can redistribute it and/or +modify it under the terms of the GNU Lesser General Public +License as published by the Free Software Foundation; either +version 2.1 of the License, or (at your option) any later version. + +CCOLAMD is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +Lesser General Public License for more details. + +You should have received a copy of the GNU Lesser General Public +License along with this Module; if not, write to the Free Software +Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA diff --git a/gtsam/3rdparty/CCOLAMD/Include/ccolamd.h b/gtsam/3rdparty/CCOLAMD/Include/ccolamd.h index 55693d47d..b4ee829be 100644 --- a/gtsam/3rdparty/CCOLAMD/Include/ccolamd.h +++ b/gtsam/3rdparty/CCOLAMD/Include/ccolamd.h @@ -5,8 +5,6 @@ /* ---------------------------------------------------------------------------- * CCOLAMD Copyright (C), Univ. of Florida. Authors: Timothy A. Davis, * Sivasankaran Rajamanickam, and Stefan Larimore - * See License.txt for the Version 2.1 of the GNU Lesser General Public License - * http://www.cise.ufl.edu/research/sparse * -------------------------------------------------------------------------- */ /* @@ -32,24 +30,24 @@ extern "C" { /* All versions of CCOLAMD will include the following definitions. * As an example, to test if the version you are using is 1.3 or later: * - * if (CCOLAMD_VERSION >= CCOLAMD_VERSION_CODE (1,3)) ... + * if (CCOLAMD_VERSION >= CCOLAMD_VERSION_CODE (1,3)) ... * * This also works during compile-time: * - * #if CCOLAMD_VERSION >= CCOLAMD_VERSION_CODE (1,3) - * printf ("This is version 1.3 or later\n") ; - * #else - * printf ("This is an early version\n") ; - * #endif + * #if CCOLAMD_VERSION >= CCOLAMD_VERSION_CODE (1,3) + * printf ("This is version 1.3 or later\n") ; + * #else + * printf ("This is an early version\n") ; + * #endif */ -#define CCOLAMD_DATE "Jan 25, 2011" +#define CCOLAMD_DATE "Apr 1, 2016" #define CCOLAMD_VERSION_CODE(main,sub) ((main) * 1000 + (sub)) #define CCOLAMD_MAIN_VERSION 2 -#define CCOLAMD_SUB_VERSION 7 -#define CCOLAMD_SUBSUB_VERSION 3 +#define CCOLAMD_SUB_VERSION 9 +#define CCOLAMD_SUBSUB_VERSION 5 #define CCOLAMD_VERSION \ - CCOLAMD_VERSION_CODE(CCOLAMD_MAIN_VERSION,CCOLAMD_SUB_VERSION) + CCOLAMD_VERSION_CODE(CCOLAMD_MAIN_VERSION,CCOLAMD_SUB_VERSION) /* ========================================================================== */ /* === Knob and statistics definitions ====================================== */ @@ -94,106 +92,105 @@ extern "C" { #define CCOLAMD_NEWLY_EMPTY_COL 10 /* error codes returned in stats [3]: */ -#define CCOLAMD_OK (0) -#define CCOLAMD_OK_BUT_JUMBLED (1) -#define CCOLAMD_ERROR_A_not_present (-1) -#define CCOLAMD_ERROR_p_not_present (-2) -#define CCOLAMD_ERROR_nrow_negative (-3) -#define CCOLAMD_ERROR_ncol_negative (-4) -#define CCOLAMD_ERROR_nnz_negative (-5) -#define CCOLAMD_ERROR_p0_nonzero (-6) -#define CCOLAMD_ERROR_A_too_small (-7) -#define CCOLAMD_ERROR_col_length_negative (-8) -#define CCOLAMD_ERROR_row_index_out_of_bounds (-9) -#define CCOLAMD_ERROR_out_of_memory (-10) -#define CCOLAMD_ERROR_invalid_cmember (-11) -#define CCOLAMD_ERROR_internal_error (-999) +#define CCOLAMD_OK (0) +#define CCOLAMD_OK_BUT_JUMBLED (1) +#define CCOLAMD_ERROR_A_not_present (-1) +#define CCOLAMD_ERROR_p_not_present (-2) +#define CCOLAMD_ERROR_nrow_negative (-3) +#define CCOLAMD_ERROR_ncol_negative (-4) +#define CCOLAMD_ERROR_nnz_negative (-5) +#define CCOLAMD_ERROR_p0_nonzero (-6) +#define CCOLAMD_ERROR_A_too_small (-7) +#define CCOLAMD_ERROR_col_length_negative (-8) +#define CCOLAMD_ERROR_row_index_out_of_bounds (-9) +#define CCOLAMD_ERROR_out_of_memory (-10) +#define CCOLAMD_ERROR_invalid_cmember (-11) +#define CCOLAMD_ERROR_internal_error (-999) /* ========================================================================== */ /* === Prototypes of user-callable routines ================================= */ /* ========================================================================== */ -/* define UF_long */ -#include "UFconfig.h" +#include "SuiteSparse_config.h" -size_t ccolamd_recommended /* returns recommended value of Alen, */ - /* or 0 if input arguments are erroneous */ +size_t ccolamd_recommended /* returns recommended value of Alen, */ + /* or 0 if input arguments are erroneous */ ( - int nnz, /* nonzeros in A */ - int n_row, /* number of rows in A */ - int n_col /* number of columns in A */ + int nnz, /* nonzeros in A */ + int n_row, /* number of rows in A */ + int n_col /* number of columns in A */ ) ; -size_t ccolamd_l_recommended /* returns recommended value of Alen, */ - /* or 0 if input arguments are erroneous */ +size_t ccolamd_l_recommended /* returns recommended value of Alen, */ + /* or 0 if input arguments are erroneous */ ( - UF_long nnz, /* nonzeros in A */ - UF_long n_row, /* number of rows in A */ - UF_long n_col /* number of columns in A */ + SuiteSparse_long nnz, /* nonzeros in A */ + SuiteSparse_long n_row, /* number of rows in A */ + SuiteSparse_long n_col /* number of columns in A */ ) ; -void ccolamd_set_defaults /* sets default parameters */ -( /* knobs argument is modified on output */ - double knobs [CCOLAMD_KNOBS] /* parameter settings for ccolamd */ +void ccolamd_set_defaults /* sets default parameters */ +( /* knobs argument is modified on output */ + double knobs [CCOLAMD_KNOBS] /* parameter settings for ccolamd */ ) ; -void ccolamd_l_set_defaults /* sets default parameters */ -( /* knobs argument is modified on output */ - double knobs [CCOLAMD_KNOBS] /* parameter settings for ccolamd */ +void ccolamd_l_set_defaults /* sets default parameters */ +( /* knobs argument is modified on output */ + double knobs [CCOLAMD_KNOBS] /* parameter settings for ccolamd */ ) ; -int ccolamd /* returns (1) if successful, (0) otherwise*/ -( /* A and p arguments are modified on output */ - int n_row, /* number of rows in A */ - int n_col, /* number of columns in A */ - int Alen, /* size of the array A */ - int A [ ], /* row indices of A, of size Alen */ - int p [ ], /* column pointers of A, of size n_col+1 */ +int ccolamd /* returns (1) if successful, (0) otherwise*/ +( /* A and p arguments are modified on output */ + int n_row, /* number of rows in A */ + int n_col, /* number of columns in A */ + int Alen, /* size of the array A */ + int A [ ], /* row indices of A, of size Alen */ + int p [ ], /* column pointers of A, of size n_col+1 */ double knobs [CCOLAMD_KNOBS],/* parameter settings for ccolamd */ - int stats [CCOLAMD_STATS], /* ccolamd output statistics and error codes */ - int cmember [ ] /* Constraint set of A, of size n_col */ + int stats [CCOLAMD_STATS], /* ccolamd output statistics and error codes */ + int cmember [ ] /* Constraint set of A, of size n_col */ ) ; -UF_long ccolamd_l /* same as ccolamd, but with UF_long integers */ +SuiteSparse_long ccolamd_l /* as ccolamd w/ SuiteSparse_long integers */ ( - UF_long n_row, - UF_long n_col, - UF_long Alen, - UF_long A [ ], - UF_long p [ ], + SuiteSparse_long n_row, + SuiteSparse_long n_col, + SuiteSparse_long Alen, + SuiteSparse_long A [ ], + SuiteSparse_long p [ ], double knobs [CCOLAMD_KNOBS], - UF_long stats [CCOLAMD_STATS], - UF_long cmember [ ] + SuiteSparse_long stats [CCOLAMD_STATS], + SuiteSparse_long cmember [ ] ) ; -int csymamd /* return (1) if OK, (0) otherwise */ +int csymamd /* return (1) if OK, (0) otherwise */ ( - int n, /* number of rows and columns of A */ - int A [ ], /* row indices of A */ - int p [ ], /* column pointers of A */ - int perm [ ], /* output permutation, size n_col+1 */ + int n, /* number of rows and columns of A */ + int A [ ], /* row indices of A */ + int p [ ], /* column pointers of A */ + int perm [ ], /* output permutation, size n_col+1 */ double knobs [CCOLAMD_KNOBS],/* parameters (uses defaults if NULL) */ - int stats [CCOLAMD_STATS], /* output statistics and error codes */ + int stats [CCOLAMD_STATS], /* output statistics and error codes */ void * (*allocate) (size_t, size_t), /* pointer to calloc (ANSI C) or */ - /* mxCalloc (for MATLAB mexFunction) */ - void (*release) (void *), /* pointer to free (ANSI C) or */ - /* mxFree (for MATLAB mexFunction) */ - int cmember [ ], /* Constraint set of A */ - int stype /* 0: use both parts, >0: upper, <0: lower */ + /* mxCalloc (for MATLAB mexFunction) */ + void (*release) (void *), /* pointer to free (ANSI C) or */ + /* mxFree (for MATLAB mexFunction) */ + int cmember [ ], /* Constraint set of A */ + int stype /* 0: use both parts, >0: upper, <0: lower */ ) ; -UF_long csymamd_l /* same as csymamd, but with UF_long integers */ +SuiteSparse_long csymamd_l /* as csymamd, w/ SuiteSparse_long integers */ ( - UF_long n, - UF_long A [ ], - UF_long p [ ], - UF_long perm [ ], + SuiteSparse_long n, + SuiteSparse_long A [ ], + SuiteSparse_long p [ ], + SuiteSparse_long perm [ ], double knobs [CCOLAMD_KNOBS], - UF_long stats [CCOLAMD_STATS], + SuiteSparse_long stats [CCOLAMD_STATS], void * (*allocate) (size_t, size_t), void (*release) (void *), - UF_long cmember [ ], - UF_long stype + SuiteSparse_long cmember [ ], + SuiteSparse_long stype ) ; void ccolamd_report @@ -203,7 +200,7 @@ void ccolamd_report void ccolamd_l_report ( - UF_long stats [CCOLAMD_STATS] + SuiteSparse_long stats [CCOLAMD_STATS] ) ; void csymamd_report @@ -213,7 +210,7 @@ void csymamd_report void csymamd_l_report ( - UF_long stats [CCOLAMD_STATS] + SuiteSparse_long stats [CCOLAMD_STATS] ) ; @@ -227,42 +224,42 @@ void csymamd_l_report */ int ccolamd2 -( /* A and p arguments are modified on output */ - int n_row, /* number of rows in A */ - int n_col, /* number of columns in A */ - int Alen, /* size of the array A */ - int A [ ], /* row indices of A, of size Alen */ - int p [ ], /* column pointers of A, of size n_col+1 */ +( /* A and p arguments are modified on output */ + int n_row, /* number of rows in A */ + int n_col, /* number of columns in A */ + int Alen, /* size of the array A */ + int A [ ], /* row indices of A, of size Alen */ + int p [ ], /* column pointers of A, of size n_col+1 */ double knobs [CCOLAMD_KNOBS],/* parameter settings for ccolamd */ - int stats [CCOLAMD_STATS], /* ccolamd output statistics and error codes */ + int stats [CCOLAMD_STATS], /* ccolamd output statistics and error codes */ /* each Front_ array is of size n_col+1: */ - int Front_npivcol [ ], /* # pivot cols in each front */ - int Front_nrows [ ], /* # of rows in each front (incl. pivot rows) */ - int Front_ncols [ ], /* # of cols in each front (incl. pivot cols) */ - int Front_parent [ ], /* parent of each front */ - int Front_cols [ ], /* link list of pivot columns for each front */ - int *p_nfr, /* total number of frontal matrices */ - int InFront [ ], /* InFront [row] = f if row in front f */ - int cmember [ ] /* Constraint set of A */ + int Front_npivcol [ ], /* # pivot cols in each front */ + int Front_nrows [ ], /* # of rows in each front (incl. pivot rows) */ + int Front_ncols [ ], /* # of cols in each front (incl. pivot cols) */ + int Front_parent [ ], /* parent of each front */ + int Front_cols [ ], /* link list of pivot columns for each front */ + int *p_nfr, /* total number of frontal matrices */ + int InFront [ ], /* InFront [row] = f if row in front f */ + int cmember [ ] /* Constraint set of A */ ) ; -UF_long ccolamd2_l /* same as ccolamd2, but with UF_long integers */ +SuiteSparse_long ccolamd2_l /* as ccolamd2, w/ SuiteSparse_long integers */ ( - UF_long n_row, - UF_long n_col, - UF_long Alen, - UF_long A [ ], - UF_long p [ ], + SuiteSparse_long n_row, + SuiteSparse_long n_col, + SuiteSparse_long Alen, + SuiteSparse_long A [ ], + SuiteSparse_long p [ ], double knobs [CCOLAMD_KNOBS], - UF_long stats [CCOLAMD_STATS], - UF_long Front_npivcol [ ], - UF_long Front_nrows [ ], - UF_long Front_ncols [ ], - UF_long Front_parent [ ], - UF_long Front_cols [ ], - UF_long *p_nfr, - UF_long InFront [ ], - UF_long cmember [ ] + SuiteSparse_long stats [CCOLAMD_STATS], + SuiteSparse_long Front_npivcol [ ], + SuiteSparse_long Front_nrows [ ], + SuiteSparse_long Front_ncols [ ], + SuiteSparse_long Front_parent [ ], + SuiteSparse_long Front_cols [ ], + SuiteSparse_long *p_nfr, + SuiteSparse_long InFront [ ], + SuiteSparse_long cmember [ ] ) ; void ccolamd_apply_order @@ -276,11 +273,11 @@ void ccolamd_apply_order void ccolamd_l_apply_order ( - UF_long Front [ ], - const UF_long Order [ ], - UF_long Temp [ ], - UF_long nn, - UF_long nfr + SuiteSparse_long Front [ ], + const SuiteSparse_long Order [ ], + SuiteSparse_long Temp [ ], + SuiteSparse_long nn, + SuiteSparse_long nfr ) ; @@ -296,12 +293,12 @@ void ccolamd_fsize void ccolamd_l_fsize ( - UF_long nn, - UF_long MaxFsize [ ], - UF_long Fnrows [ ], - UF_long Fncols [ ], - UF_long Parent [ ], - UF_long Npiv [ ] + SuiteSparse_long nn, + SuiteSparse_long MaxFsize [ ], + SuiteSparse_long Fnrows [ ], + SuiteSparse_long Fncols [ ], + SuiteSparse_long Parent [ ], + SuiteSparse_long Npiv [ ] ) ; void ccolamd_postorder @@ -320,16 +317,16 @@ void ccolamd_postorder void ccolamd_l_postorder ( - UF_long nn, - UF_long Parent [ ], - UF_long Npiv [ ], - UF_long Fsize [ ], - UF_long Order [ ], - UF_long Child [ ], - UF_long Sibling [ ], - UF_long Stack [ ], - UF_long Front_cols [ ], - UF_long cmember [ ] + SuiteSparse_long nn, + SuiteSparse_long Parent [ ], + SuiteSparse_long Npiv [ ], + SuiteSparse_long Fsize [ ], + SuiteSparse_long Order [ ], + SuiteSparse_long Child [ ], + SuiteSparse_long Sibling [ ], + SuiteSparse_long Stack [ ], + SuiteSparse_long Front_cols [ ], + SuiteSparse_long cmember [ ] ) ; int ccolamd_post_tree @@ -342,22 +339,16 @@ int ccolamd_post_tree int Stack [ ] ) ; -UF_long ccolamd_l_post_tree +SuiteSparse_long ccolamd_l_post_tree ( - UF_long root, - UF_long k, - UF_long Child [ ], - const UF_long Sibling [ ], - UF_long Order [ ], - UF_long Stack [ ] + SuiteSparse_long root, + SuiteSparse_long k, + SuiteSparse_long Child [ ], + const SuiteSparse_long Sibling [ ], + SuiteSparse_long Order [ ], + SuiteSparse_long Stack [ ] ) ; -#ifndef EXTERN -#define EXTERN extern -#endif - -EXTERN int (*ccolamd_printf) (const char *, ...) ; - #ifdef __cplusplus } #endif diff --git a/gtsam/3rdparty/CCOLAMD/Lib/Makefile b/gtsam/3rdparty/CCOLAMD/Lib/Makefile new file mode 100644 index 000000000..c2352c90e --- /dev/null +++ b/gtsam/3rdparty/CCOLAMD/Lib/Makefile @@ -0,0 +1,71 @@ +#------------------------------------------------------------------------------- +# CCOLAMD Lib/Makefile +#------------------------------------------------------------------------------- + +LIBRARY = libccolamd +VERSION = 2.9.5 +SO_VERSION = 2 + +default: library + +include ../../SuiteSparse_config/SuiteSparse_config.mk + +# CCOLAMD depends on SuiteSparse_config +LDLIBS += -lsuitesparseconfig + +# compile and install in SuiteSparse/lib +library: + $(MAKE) install INSTALL=$(SUITESPARSE) + +I = -I../Include -I../../SuiteSparse_config + +INC = ../Include/ccolamd.h ../../SuiteSparse_config/SuiteSparse_config.h + +SRC = ../Source/ccolamd.c + +OBJ = ccolamd.o ccolamd_l.o + +ccolamd.o: $(SRC) $(INC) + $(CC) $(CF) $(I) -c ../Source/ccolamd.c + +ccolamd_l.o: $(SRC) $(INC) + $(CC) $(CF) $(I) -c ../Source/ccolamd.c -DDLONG -o ccolamd_l.o + +# creates libccolamd.a, a C-callable CCOLAMD library +$(AR_TARGET): $(OBJ) + $(ARCHIVE) $@ $^ + - $(RANLIB) $@ + +ccode: library + +clean: + - $(RM) -r $(CLEAN) + +purge: distclean + +distclean: clean + - $(RM) -r $(PURGE) + +# install CCOLAMD +install: $(AR_TARGET) $(INSTALL_LIB)/$(SO_TARGET) + +$(INSTALL_LIB)/$(SO_TARGET): $(OBJ) + @mkdir -p $(INSTALL_LIB) + @mkdir -p $(INSTALL_INCLUDE) + @mkdir -p $(INSTALL_DOC) + $(CC) $(SO_OPTS) $^ -o $@ $(LDLIBS) + ( cd $(INSTALL_LIB) ; ln -sf $(SO_TARGET) $(SO_PLAIN) ) + ( cd $(INSTALL_LIB) ; ln -sf $(SO_TARGET) $(SO_MAIN) ) + $(CP) ../Include/ccolamd.h $(INSTALL_INCLUDE) + $(CP) ../README.txt $(INSTALL_DOC)/CCOLAMD_README.txt + chmod 755 $(INSTALL_LIB)/$(SO_TARGET) + chmod 644 $(INSTALL_INCLUDE)/ccolamd.h + chmod 644 $(INSTALL_DOC)/CCOLAMD_README.txt + +uninstall: + $(RM) $(INSTALL_LIB)/$(SO_TARGET) + $(RM) $(INSTALL_LIB)/$(SO_PLAIN) + $(RM) $(INSTALL_LIB)/$(SO_MAIN) + $(RM) $(INSTALL_INCLUDE)/ccolamd.h + $(RM) $(INSTALL_DOC)/CCOLAMD_README.txt + diff --git a/gtsam/3rdparty/CCOLAMD/MATLAB/ccolamd_demo.m b/gtsam/3rdparty/CCOLAMD/MATLAB/ccolamd_demo.m index e2fd68457..2c69a502e 100644 --- a/gtsam/3rdparty/CCOLAMD/MATLAB/ccolamd_demo.m +++ b/gtsam/3rdparty/CCOLAMD/MATLAB/ccolamd_demo.m @@ -47,9 +47,8 @@ spparms ('default') ; A = sprandn (n, n, 2/n) + speye (n) ; b = (1:n)' ; -figure (1) clf ; -subplot (2,2,1) +subplot (3,4,1) spy (A) title ('original matrix') @@ -62,7 +61,7 @@ fl = luflops (L, U) ; x = Q * (U \ (L \ (P * b))) ; fprintf (1, '\nFlop count for [L,U,P] = lu (A*Q): %d\n', fl) ; fprintf (1, 'residual: %e\n', norm (A*x-b)); -subplot (2,2,2) ; +subplot (3,4,2) ; spy (L|U) ; title ('LU with ccolamd') ; @@ -76,7 +75,7 @@ fl = luflops (L, U) ; x = Q * (U \ (L \ (P * b))) ; fprintf (1, '\nFlop count for [L,U,P] = lu (A*Q): %d\n', fl) ; fprintf (1, 'residual: %e\n', norm (A*x-b)); -subplot (2,2,3) ; +subplot (3,4,3) ; spy (L|U) ; title ('LU with colamd') ; catch @@ -89,7 +88,7 @@ fl = luflops (L, U) ; x = U \ (L \ (P * b)) ; fprintf (1, '\nFlop count for [L,U,P] = lu (A*Q): %d\n', fl) ; fprintf (1, 'residual: %e\n', norm (A*x-b)); -subplot (2,2,4) ; +subplot (3,4,4) ; spy (L|U) ; title ('LU with no ordering') ; @@ -111,9 +110,7 @@ n = 1000 ; fprintf (1, 'Generating a random %d-by-%d sparse matrix.\n', n, n) ; A = sprandn (n, n, 2/n) + speye (n) ; -figure (2) -clf ; -subplot (2,2,1) +subplot (3,4,5) spy (A) title ('original matrix') @@ -121,7 +118,7 @@ fprintf (1, '\n\nUnordered matrix:\n') ; [lnz,h,parent,post,R] = symbfact (A, 'col') ; fprintf (1, 'nz in Cholesky factors of A''A: %d\n', sum (lnz)) ; fprintf (1, 'flop count for Cholesky of A''A: %d\n', sum (lnz.^2)) ; -subplot (2,2,4) ; +subplot (3,4,6) ; spy (R) ; title ('Cholesky with no ordering') ; @@ -133,7 +130,7 @@ fprintf (1, '\n\nccolamd run time: %f\n', t) ; fprintf (1, 'ccolamd ordering quality: \n') ; fprintf (1, 'nz in Cholesky factors of A(:,p)''A(:,p): %d\n', sum (lnz)) ; fprintf (1, 'flop count for Cholesky of A(:,p)''A(:,p): %d\n', sum (lnz.^2)) ; -subplot (2,2,2) ; +subplot (3,4,7) ; spy (R) ; title ('Cholesky with ccolamd') ; @@ -146,7 +143,7 @@ fprintf (1, '\n\ncolamd run time: %f\n', t) ; fprintf (1, 'colamd ordering quality: \n') ; fprintf (1, 'nz in Cholesky factors of A(:,p)''A(:,p): %d\n', sum (lnz)) ; fprintf (1, 'flop count for Cholesky of A(:,p)''A(:,p): %d\n', sum (lnz.^2)) ; -subplot (2,2,3) ; +subplot (3,4,8) ; spy (R) ; title ('Cholesky with colamd') ; catch @@ -164,9 +161,7 @@ fprintf (1, '\n-----------------------------------------------------------\n') ; fprintf (1, 'Generating a random symmetric %d-by-%d sparse matrix.\n', n, n) ; A = A+A' ; -figure (3) -clf ; -subplot (2,2,1) +subplot (3,4,9) ; spy (A) title ('original matrix') @@ -174,7 +169,7 @@ fprintf (1, '\n\nUnordered matrix:\n') ; [lnz,h,parent,post,R] = symbfact (A, 'sym') ; fprintf (1, 'nz in Cholesky factors of A: %d\n', sum (lnz)) ; fprintf (1, 'flop count for Cholesky of A: %d\n', sum (lnz.^2)) ; -subplot (2,2,4) ; +subplot (3,4,10) ; spy (R) ; title ('Cholesky with no ordering') ; @@ -186,7 +181,7 @@ fprintf (1, '\n\ncsymamd run time: %f\n', t) ; fprintf (1, 'csymamd ordering quality: \n') ; fprintf (1, 'nz in Cholesky factors of A(p,p): %d\n', sum (lnz)) ; fprintf (1, 'flop count for Cholesky of A(p,p): %d\n', sum (lnz.^2)) ; -subplot (2,2,2) ; +subplot (3,4,11) ; spy (R) ; title ('Cholesky with csymamd') ; @@ -199,7 +194,7 @@ fprintf (1, '\n\nsymamd run time: %f\n', t) ; fprintf (1, 'symamd ordering quality: \n') ; fprintf (1, 'nz in Cholesky factors of A(p,p): %d\n', sum (lnz)) ; fprintf (1, 'flop count for Cholesky of A(p,p): %d\n', sum (lnz.^2)) ; -subplot (2,2,3) ; +subplot (3,4,12) ; spy (R) ; title ('Cholesky with symamd') ; catch diff --git a/gtsam/3rdparty/CCOLAMD/MATLAB/ccolamd_make.m b/gtsam/3rdparty/CCOLAMD/MATLAB/ccolamd_make.m index 4637f3884..9a84da909 100644 --- a/gtsam/3rdparty/CCOLAMD/MATLAB/ccolamd_make.m +++ b/gtsam/3rdparty/CCOLAMD/MATLAB/ccolamd_make.m @@ -14,14 +14,33 @@ d = '' ; if (~isempty (strfind (computer, '64'))) d = '-largeArrayDims' ; end -src = '../Source/ccolamd.c ../Source/ccolamd_global.c' ; -cmd = sprintf ('mex -DDLONG -O %s -I../../UFconfig -I../Include -output ', d) ; + +% MATLAB 8.3.0 now has a -silent option to keep 'mex' from burbling too much +if (~verLessThan ('matlab', '8.3.0')) + d = ['-silent ' d] ; +end + +src = '../Source/ccolamd.c ../../SuiteSparse_config/SuiteSparse_config.c' ; +cmd = sprintf ( ... + 'mex -DDLONG -O %s -I../../SuiteSparse_config -I../Include -output ', d) ; s = [cmd 'ccolamd ccolamdmex.c ' src] ; + +if (~(ispc || ismac)) + % for POSIX timing routine + s = [s ' -lrt'] ; +end if (details) fprintf ('%s\n', s) ; end eval (s) ; + s = [cmd 'csymamd csymamdmex.c ' src] ; + +if (~(ispc || ismac)) + % for POSIX timing routine + s = [s ' -lrt'] ; +end + if (details) fprintf ('%s\n', s) ; end diff --git a/gtsam/3rdparty/CCOLAMD/MATLAB/ccolamd_test.m b/gtsam/3rdparty/CCOLAMD/MATLAB/ccolamd_test.m index e359b9fea..0514ad9c7 100644 --- a/gtsam/3rdparty/CCOLAMD/MATLAB/ccolamd_test.m +++ b/gtsam/3rdparty/CCOLAMD/MATLAB/ccolamd_test.m @@ -22,8 +22,13 @@ csymamd_default_knobs = [10 1 0] ; if (~isempty (strfind (computer, '64'))) d = '-largeArrayDims' ; end - src = '../Source/ccolamd.c ../Source/ccolamd_global.c' ; - cmd = sprintf ('mex -DDLONG -O %s -I../../UFconfig -I../Include ', d) ; + cmd = sprintf ( ... + 'mex -DDLONG -O %s -I../../SuiteSparse_config -I../Include ', d) ; + src = '../Source/ccolamd.c ../../SuiteSparse_config/SuiteSparse_config.c' ; + if (~(ispc || ismac)) + % for POSIX timing routine + src = [src ' -lrt'] ; + end eval ([cmd 'ccolamdtestmex.c ' src]) ; eval ([cmd 'csymamdtestmex.c ' src]) ; fprintf ('Done compiling.\n') ; diff --git a/gtsam/3rdparty/CCOLAMD/MATLAB/ccolamdmex.c b/gtsam/3rdparty/CCOLAMD/MATLAB/ccolamdmex.c index fbcb87873..09d21efee 100644 --- a/gtsam/3rdparty/CCOLAMD/MATLAB/ccolamdmex.c +++ b/gtsam/3rdparty/CCOLAMD/MATLAB/ccolamdmex.c @@ -5,8 +5,6 @@ /* ---------------------------------------------------------------------------- * CCOLAMD, Copyright (C), Univ. of Florida. Authors: Timothy A. Davis, * Sivasankaran Rajamanickam, and Stefan Larimore - * See License.txt for the Version 2.1 of the GNU Lesser General Public License - * http://www.cise.ufl.edu/research/sparse * -------------------------------------------------------------------------- */ /* @@ -26,7 +24,7 @@ #include "matrix.h" #include #include -#include "UFconfig.h" +#define Long SuiteSparse_long /* ========================================================================== */ /* === ccolamd mexFunction ================================================== */ @@ -44,24 +42,24 @@ void mexFunction { /* === Local variables ================================================== */ - UF_long *A ; /* ccolamd's copy of the matrix and workspace */ - UF_long *cmember ; /* ccolamd's copy of the constraint set */ - double *in_cmember ; /* input constraint set */ - UF_long *p ; /* ccolamd's copy of the column pointers */ - UF_long Alen ; /* size of A */ - UF_long cslen ; /* size of CS */ - UF_long n_col ; /* number of columns of A */ - UF_long n_row ; /* number of rows of A */ - UF_long nnz ; /* number of entries in A */ - UF_long full ; /* TRUE if input matrix full, FALSE if sparse */ + Long *A ; /* ccolamd's copy of the matrix and workspace */ + Long *cmember ; /* ccolamd's copy of the constraint set */ + double *in_cmember ; /* input constraint set */ + Long *p ; /* ccolamd's copy of the column pointers */ + Long Alen ; /* size of A */ + Long cslen ; /* size of CS */ + Long n_col ; /* number of columns of A */ + Long n_row ; /* number of rows of A */ + Long nnz ; /* number of entries in A */ + Long full ; /* TRUE if input matrix full, FALSE if sparse */ double knobs [CCOLAMD_KNOBS] ; /* ccolamd user-controllable parameters */ - double *out_perm ; /* output permutation vector */ - double *out_stats ; /* output stats vector */ - double *in_knobs ; /* input knobs vector */ - UF_long i ; /* loop counter */ - mxArray *Ainput ; /* input matrix handle */ - UF_long spumoni ; /* verbosity variable */ - UF_long stats [CCOLAMD_STATS] ; /* stats for ccolamd */ + double *out_perm ; /* output permutation vector */ + double *out_stats ; /* output stats vector */ + double *in_knobs ; /* input knobs vector */ + Long i ; /* loop counter */ + mxArray *Ainput ; /* input matrix handle */ + Long spumoni ; /* verbosity variable */ + Long stats [CCOLAMD_STATS] ;/* stats for ccolamd */ /* === Check inputs ===================================================== */ @@ -80,11 +78,11 @@ void mexFunction cslen = mxGetNumberOfElements (pargin [2]) ; if (cslen != 0) { - cmember = (UF_long *) mxCalloc (cslen, sizeof (UF_long)) ; + cmember = (Long *) mxCalloc (cslen, sizeof (Long)) ; for (i = 0 ; i < cslen ; i++) { /* convert cmember from 1-based to 0-based */ - cmember[i] = ((UF_long) in_cmember [i] - 1) ; + cmember[i] = ((Long) in_cmember [i] - 1) ; } } } @@ -157,10 +155,10 @@ void mexFunction n_col = mxGetN (Ainput) ; /* get column pointer vector */ - p = (UF_long *) mxCalloc (n_col+1, sizeof (UF_long)) ; - (void) memcpy (p, mxGetJc (Ainput), (n_col+1)*sizeof (UF_long)) ; + p = (Long *) mxCalloc (n_col+1, sizeof (Long)) ; + (void) memcpy (p, mxGetJc (Ainput), (n_col+1)*sizeof (Long)) ; nnz = p [n_col] ; - Alen = (UF_long) ccolamd_l_recommended (nnz, n_row, n_col) ; + Alen = (Long) ccolamd_l_recommended (nnz, n_row, n_col) ; if (Alen == 0) { mexErrMsgTxt ("ccolamd: problem too large") ; @@ -168,8 +166,8 @@ void mexFunction /* === Copy input matrix into workspace ================================= */ - A = (UF_long *) mxCalloc (Alen, sizeof (UF_long)) ; - (void) memcpy (A, mxGetIr (Ainput), nnz*sizeof (UF_long)) ; + A = (Long *) mxCalloc (Alen, sizeof (Long)) ; + (void) memcpy (A, mxGetIr (Ainput), nnz*sizeof (Long)) ; if (full) { diff --git a/gtsam/3rdparty/CCOLAMD/MATLAB/ccolamdtestmex.c b/gtsam/3rdparty/CCOLAMD/MATLAB/ccolamdtestmex.c index b5b04da73..18c850dc6 100644 --- a/gtsam/3rdparty/CCOLAMD/MATLAB/ccolamdtestmex.c +++ b/gtsam/3rdparty/CCOLAMD/MATLAB/ccolamdtestmex.c @@ -5,8 +5,6 @@ /* ---------------------------------------------------------------------------- * CCOLAMD Copyright (C), Univ. of Florida. Authors: Timothy A. Davis, * Sivasankaran Rajamanickam, and Stefan Larimore - * See License.txt for the Version 2.1 of the GNU Lesser General Public License - * http://www.cise.ufl.edu/research/sparse * -------------------------------------------------------------------------- */ /* @@ -43,7 +41,7 @@ #include "matrix.h" #include #include -#include "UFconfig.h" +#define Long SuiteSparse_long /* Here only for testing */ #undef MIN @@ -61,15 +59,15 @@ static void dump_matrix ( - UF_long A [ ], - UF_long p [ ], - UF_long n_row, - UF_long n_col, - UF_long Alen, - UF_long limit + Long A [ ], + Long p [ ], + Long n_row, + Long n_col, + Long Alen, + Long limit ) { - UF_long col, k, row ; + Long col, k, row ; mexPrintf ("dump matrix: nrow %d ncol %d Alen %d\n", n_row, n_col, Alen) ; @@ -102,24 +100,24 @@ void mexFunction { /* === Local variables ================================================== */ - UF_long *A ; /* ccolamd's copy of the matrix and workspace */ - UF_long *p ; /* ccolamd's copy of the column pointers */ - UF_long Alen ; /* size of A */ - UF_long n_col ; /* number of columns of A */ - UF_long n_row ; /* number of rows of A */ - UF_long nnz ; /* number of entries in A */ - UF_long full ; /* TRUE if input matrix full, FALSE if sparse */ + Long *A ; /* ccolamd's copy of the matrix and workspace */ + Long *p ; /* ccolamd's copy of the column pointers */ + Long Alen ; /* size of A */ + Long n_col ; /* number of columns of A */ + Long n_row ; /* number of rows of A */ + Long nnz ; /* number of entries in A */ + Long full ; /* TRUE if input matrix full, FALSE if sparse */ double knobs [CCOLAMD_KNOBS] ; /* ccolamd user-controllable parameters */ - double *out_perm ; /* output permutation vector */ - double *out_stats ; /* output stats vector */ - double *in_knobs ; /* input knobs vector */ - UF_long i ; /* loop counter */ - mxArray *Ainput ; /* input matrix handle */ - UF_long spumoni ; /* verbosity variable */ - UF_long stats2 [CCOLAMD_STATS] ; /* stats for ccolamd */ + double *out_perm ; /* output permutation vector */ + double *out_stats ; /* output stats vector */ + double *in_knobs ; /* input knobs vector */ + Long i ; /* loop counter */ + mxArray *Ainput ; /* input matrix handle */ + Long spumoni ; /* verbosity variable */ + Long stats2 [CCOLAMD_STATS] ; /* stats for ccolamd */ - UF_long *cp, *cp_end, result, col, length, ok ; - UF_long *stats ; + Long *cp, *cp_end, result, col, length, ok ; + Long *stats ; stats = stats2 ; /* === Check inputs ===================================================== */ @@ -199,10 +197,10 @@ void mexFunction n_col = mxGetN (Ainput) ; /* get column pointer vector so we can find nnz */ - p = (UF_long *) mxCalloc (n_col+1, sizeof (UF_long)) ; - (void) memcpy (p, mxGetJc (Ainput), (n_col+1)*sizeof (UF_long)) ; + p = (Long *) mxCalloc (n_col+1, sizeof (Long)) ; + (void) memcpy (p, mxGetJc (Ainput), (n_col+1)*sizeof (Long)) ; nnz = p [n_col] ; - Alen = (UF_long) ccolamd_l_recommended (nnz, n_row, n_col) ; + Alen = (Long) ccolamd_l_recommended (nnz, n_row, n_col) ; if (Alen == 0) { mexErrMsgTxt ("ccolamd: problem too large") ; @@ -230,8 +228,8 @@ void mexFunction /* === Copy input matrix into workspace ================================= */ - A = (UF_long *) mxCalloc (Alen, sizeof (UF_long)) ; - (void) memcpy (A, mxGetIr (Ainput), nnz*sizeof (UF_long)) ; + A = (Long *) mxCalloc (Alen, sizeof (Long)) ; + (void) memcpy (A, mxGetIr (Ainput), nnz*sizeof (Long)) ; if (full) { @@ -261,7 +259,7 @@ void mexFunction */ /* jumble appropriately */ - switch ((UF_long) in_knobs [6]) + switch ((Long) in_knobs [6]) { case 0 : @@ -359,7 +357,7 @@ void mexFunction mexPrintf ("ccolamdtest: A not present\n") ; } result = 0 ; /* A not present */ - A = (UF_long *) NULL ; + A = (Long *) NULL ; break ; case 8 : @@ -368,7 +366,7 @@ void mexFunction mexPrintf ("ccolamdtest: p not present\n") ; } result = 0 ; /* p not present */ - p = (UF_long *) NULL ; + p = (Long *) NULL ; break ; case 9 : @@ -456,7 +454,7 @@ void mexFunction mexPrintf ("ccolamdtest: stats not present\n") ; } result = 0 ; /* stats not present */ - stats = (UF_long *) NULL ; + stats = (Long *) NULL ; break ; case 13 : diff --git a/gtsam/3rdparty/CCOLAMD/MATLAB/csymamd.m b/gtsam/3rdparty/CCOLAMD/MATLAB/csymamd.m index 901040df6..4308f9934 100644 --- a/gtsam/3rdparty/CCOLAMD/MATLAB/csymamd.m +++ b/gtsam/3rdparty/CCOLAMD/MATLAB/csymamd.m @@ -34,10 +34,10 @@ function [p, stats] = csymamd (S, knobs, cmember) %#ok % p = csymamd(S) is about the same as p = symamd(S). knobs and its default % values differ. % -% Authors: S. Larimore, T. Davis (Univ of Florida), and S. Rajamanickam, in +% Authors: S. Larimore, T. Davis, and S. Rajamanickam, in % collaboration with J. Gilbert and E. Ng. Supported by the National % Science Foundation (DMS-9504974, DMS-9803599, CCR-0203270), and a grant -% from Sandia National Lab. See http://www.cise.ufl.edu/research/sparse +% from Sandia National Lab. See http://www.suitesparse.com % for ccolamd, csymamd, amd, colamd, symamd, and other related orderings. % % See also AMD, CCOLAMD, COLAMD, SYMAMD, SYMRCM. diff --git a/gtsam/3rdparty/CCOLAMD/MATLAB/csymamdmex.c b/gtsam/3rdparty/CCOLAMD/MATLAB/csymamdmex.c index e52d92619..41a3b4346 100644 --- a/gtsam/3rdparty/CCOLAMD/MATLAB/csymamdmex.c +++ b/gtsam/3rdparty/CCOLAMD/MATLAB/csymamdmex.c @@ -5,8 +5,6 @@ /* ---------------------------------------------------------------------------- * CCOLAMD, Copyright (C), Univ. of Florida. Authors: Timothy A. Davis, * Sivasankaran Rajamanickam, and Stefan Larimore - * See License.txt for the Version 2.1 of the GNU Lesser General Public License - * http://www.cise.ufl.edu/research/sparse * -------------------------------------------------------------------------- */ /* @@ -25,7 +23,7 @@ #include "mex.h" #include "matrix.h" #include -#include "UFconfig.h" +#define Long SuiteSparse_long /* ========================================================================== */ /* === csymamd mexFunction ================================================== */ @@ -43,23 +41,23 @@ void mexFunction { /* === Local variables ================================================== */ - UF_long *A ; /* row indices of input matrix A */ - UF_long *perm ; /* column ordering of M and ordering of A */ - UF_long *cmember ; /* csymamd's copy of the constraint set */ - double *in_cmember ; /* input constraint set */ - UF_long *p ; /* column pointers of input matrix A */ - UF_long cslen ; /* size of constraint set */ - UF_long n_col ; /* number of columns of A */ - UF_long n_row ; /* number of rows of A */ - UF_long full ; /* TRUE if input matrix full, FALSE if sparse */ + Long *A ; /* row indices of input matrix A */ + Long *perm ; /* column ordering of M and ordering of A */ + Long *cmember ; /* csymamd's copy of the constraint set */ + double *in_cmember ; /* input constraint set */ + Long *p ; /* column pointers of input matrix A */ + Long cslen ; /* size of constraint set */ + Long n_col ; /* number of columns of A */ + Long n_row ; /* number of rows of A */ + Long full ; /* TRUE if input matrix full, FALSE if sparse */ double knobs [CCOLAMD_KNOBS] ; /* csymamd user-controllable parameters */ - double *out_perm ; /* output permutation vector */ - double *out_stats ; /* output stats vector */ - double *in_knobs ; /* input knobs vector */ - UF_long i ; /* loop counter */ - mxArray *Ainput ; /* input matrix handle */ - UF_long spumoni ; /* verbosity variable */ - UF_long stats [CCOLAMD_STATS] ; /* stats for symamd */ + double *out_perm ; /* output permutation vector */ + double *out_stats ; /* output stats vector */ + double *in_knobs ; /* input knobs vector */ + Long i ; /* loop counter */ + mxArray *Ainput ; /* input matrix handle */ + Long spumoni ; /* verbosity variable */ + Long stats [CCOLAMD_STATS] ;/* stats for symamd */ /* === Check inputs ===================================================== */ @@ -78,11 +76,11 @@ void mexFunction cslen = mxGetNumberOfElements (pargin [2]) ; if (cslen != 0) { - cmember = (UF_long *) mxCalloc (cslen, sizeof (UF_long)) ; + cmember = (Long *) mxCalloc (cslen, sizeof (Long)) ; for (i = 0 ; i < cslen ; i++) { /* convert cmember from 1-based to 0-based */ - cmember[i] = ((UF_long) in_cmember [i] - 1) ; + cmember[i] = ((Long) in_cmember [i] - 1) ; } } } @@ -153,9 +151,9 @@ void mexFunction mexErrMsgTxt ("csymamd: cmember must be of length equal to #cols of A"); } - A = (UF_long *) mxGetIr (Ainput) ; - p = (UF_long *) mxGetJc (Ainput) ; - perm = (UF_long *) mxCalloc (n_col+1, sizeof (UF_long)) ; + A = (Long *) mxGetIr (Ainput) ; + p = (Long *) mxGetJc (Ainput) ; + perm = (Long *) mxCalloc (n_col+1, sizeof (Long)) ; /* === Order the rows and columns of A (does not destroy A) ============= */ diff --git a/gtsam/3rdparty/CCOLAMD/MATLAB/csymamdtestmex.c b/gtsam/3rdparty/CCOLAMD/MATLAB/csymamdtestmex.c index 428b41185..60b0d6c04 100644 --- a/gtsam/3rdparty/CCOLAMD/MATLAB/csymamdtestmex.c +++ b/gtsam/3rdparty/CCOLAMD/MATLAB/csymamdtestmex.c @@ -5,8 +5,6 @@ /* ---------------------------------------------------------------------------- * CCOLAMD Copyright (C), Univ. of Florida. Authors: Timothy A. Davis, * Sivasankaran Rajamanickam, and Stefan Larimore - * See License.txt for the Version 2.1 of the GNU Lesser General Public License - * http://www.cise.ufl.edu/research/sparse * -------------------------------------------------------------------------- */ /* @@ -37,7 +35,7 @@ #include "matrix.h" #include #include -#include "UFconfig.h" +#define Long SuiteSparse_long #ifdef MIN #undef MIN @@ -47,15 +45,15 @@ static void dump_matrix ( - UF_long A [ ], - UF_long p [ ], - UF_long n_row, - UF_long n_col, - UF_long Alen, - UF_long limit + Long A [ ], + Long p [ ], + Long n_row, + Long n_col, + Long Alen, + Long limit ) { - UF_long col, k, row ; + Long col, k, row ; mexPrintf ("dump matrix: nrow %d ncol %d Alen %d\n", n_row, n_col, Alen) ; @@ -100,23 +98,23 @@ void mexFunction { /* === Local variables ================================================== */ - UF_long *perm ; /* column ordering of M and ordering of A */ - UF_long *A ; /* row indices of input matrix A */ - UF_long *p ; /* column pointers of input matrix A */ - UF_long n_col ; /* number of columns of A */ - UF_long n_row ; /* number of rows of A */ - UF_long full ; /* TRUE if input matrix full, FALSE if sparse */ + Long *perm ; /* column ordering of M and ordering of A */ + Long *A ; /* row indices of input matrix A */ + Long *p ; /* column pointers of input matrix A */ + Long n_col ; /* number of columns of A */ + Long n_row ; /* number of rows of A */ + Long full ; /* TRUE if input matrix full, FALSE if sparse */ double knobs [CCOLAMD_KNOBS] ; /* ccolamd user-controllable parameters */ - double *out_perm ; /* output permutation vector */ - double *out_stats ; /* output stats vector */ - double *in_knobs ; /* input knobs vector */ - UF_long i ; /* loop counter */ - mxArray *Ainput ; /* input matrix handle */ - UF_long spumoni ; /* verbosity variable */ - UF_long stats2 [CCOLAMD_STATS] ;/* stats for csymamd */ + double *out_perm ; /* output permutation vector */ + double *out_stats ; /* output stats vector */ + double *in_knobs ; /* input knobs vector */ + Long i ; /* loop counter */ + mxArray *Ainput ; /* input matrix handle */ + Long spumoni ; /* verbosity variable */ + Long stats2 [CCOLAMD_STATS] ;/* stats for csymamd */ - UF_long *cp, *cp_end, result, nnz, col, length, ok ; - UF_long *stats ; + Long *cp, *cp_end, result, nnz, col, length, ok ; + Long *stats ; stats = stats2 ; /* === Check inputs ===================================================== */ @@ -192,8 +190,8 @@ void mexFunction } /* p = mxGetJc (Ainput) ; */ - p = (UF_long *) mxCalloc (n_col+1, sizeof (UF_long)) ; - (void) memcpy (p, mxGetJc (Ainput), (n_col+1)*sizeof (UF_long)) ; + p = (Long *) mxCalloc (n_col+1, sizeof (Long)) ; + (void) memcpy (p, mxGetJc (Ainput), (n_col+1)*sizeof (Long)) ; nnz = p [n_col] ; if (spumoni) @@ -202,10 +200,10 @@ void mexFunction } /* A = mxGetIr (Ainput) ; */ - A = (UF_long *) mxCalloc (nnz+1, sizeof (UF_long)) ; - (void) memcpy (A, mxGetIr (Ainput), nnz*sizeof (UF_long)) ; + A = (Long *) mxCalloc (nnz+1, sizeof (Long)) ; + (void) memcpy (A, mxGetIr (Ainput), nnz*sizeof (Long)) ; - perm = (UF_long *) mxCalloc (n_col+1, sizeof (UF_long)) ; + perm = (Long *) mxCalloc (n_col+1, sizeof (Long)) ; /* === Jumble matrix ==================================================== */ @@ -230,7 +228,7 @@ void mexFunction */ /* jumble appropriately */ - switch ((UF_long) in_knobs [3]) + switch ((Long) in_knobs [3]) { case 0 : @@ -321,7 +319,7 @@ void mexFunction mexPrintf ("csymamdtest: A not present\n") ; } result = 0 ; /* A not present */ - A = (UF_long *) NULL ; + A = (Long *) NULL ; break ; case 8 : @@ -330,7 +328,7 @@ void mexFunction mexPrintf ("csymamdtest: p not present\n") ; } result = 0 ; /* p not present */ - p = (UF_long *) NULL ; + p = (Long *) NULL ; break ; case 9 : @@ -418,7 +416,7 @@ void mexFunction mexPrintf ("csymamdtest: stats not present\n") ; } result = 0 ; /* stats not present */ - stats = (UF_long *) NULL ; + stats = (Long *) NULL ; break ; case 13 : diff --git a/gtsam/3rdparty/CCOLAMD/Makefile b/gtsam/3rdparty/CCOLAMD/Makefile new file mode 100644 index 000000000..f04181d60 --- /dev/null +++ b/gtsam/3rdparty/CCOLAMD/Makefile @@ -0,0 +1,51 @@ +#------------------------------------------------------------------------------ +# CCOLAMD Makefile +#------------------------------------------------------------------------------ + +SUITESPARSE ?= $(realpath $(CURDIR)/..) +export SUITESPARSE + +default: all + +include ../SuiteSparse_config/SuiteSparse_config.mk + +demos: all + +# Compile all C code +all: + ( cd Lib ; $(MAKE) ) + ( cd Demo ; $(MAKE) ) + +# compile just the C-callable libraries (not Demos) +library: + ( cd Lib ; $(MAKE) ) + +# remove object files, but keep the compiled programs and library archives +clean: + ( cd Lib ; $(MAKE) clean ) + ( cd Demo ; $(MAKE) clean ) + ( cd MATLAB ; $(RM) $(CLEAN) ) + +# clean, and then remove compiled programs and library archives +purge: + ( cd Lib ; $(MAKE) purge ) + ( cd Demo ; $(MAKE) purge ) + ( cd MATLAB ; $(RM) $(CLEAN) ; $(RM) *.mex* ) + +distclean: purge + +# get ready for distribution +dist: purge + ( cd Demo ; $(MAKE) dist ) + +ccode: library + +lib: library + +# install CCOLAMD +install: + ( cd Lib ; $(MAKE) install ) + +# uninstall CCOLAMD +uninstall: + ( cd Lib ; $(MAKE) uninstall ) diff --git a/gtsam/3rdparty/CCOLAMD/README.txt b/gtsam/3rdparty/CCOLAMD/README.txt index ccc5a19a5..6d8f1ce83 100644 --- a/gtsam/3rdparty/CCOLAMD/README.txt +++ b/gtsam/3rdparty/CCOLAMD/README.txt @@ -1,8 +1,8 @@ CCOLAMD: constrained column approximate minimum degree ordering -Copyright (C) 2005-2011, Univ. of Florida. Authors: Timothy A. Davis, +Copyright (C) 2005-2016, Univ. of Florida. Authors: Timothy A. Davis, Sivasankaran Rajamanickam, and Stefan Larimore. Closely based on COLAMD by Davis, Stefan Larimore, in collaboration with Esmond Ng, and John Gilbert. -http://www.cise.ufl.edu/research/sparse +http://www.suitesparse.com ------------------------------------------------------------------------------- The CCOLAMD column approximate minimum degree ordering algorithm computes @@ -14,7 +14,8 @@ available as a MATLAB-callable function. It constructs a matrix M such that M'*M has the same pattern as A, and then uses CCOLAMD to compute a column ordering of M. -Requires UFconfig, in the ../UFconfig directory relative to this directory. +Requires SuiteSparse_config, in the ../SuiteSparse_config directory relative to +this directory. To compile and install the ccolamd m-files and mexFunctions, just cd to CCOLAMD/MATLAB and type ccolamd_install in the MATLAB command window. @@ -22,47 +23,27 @@ A short demo will run. Optionally, type ccolamd_test to run an extensive tests. Type "make" in Unix in the CCOLAMD directory to compile the C-callable library and to run a short demo. -If you have MATLAB 7.2 or earlier, you must first edit UFconfig/UFconfig.h to -remove the "-largeArrayDims" option from the MEX command (or just use -ccolamd_install.m inside MATLAB). - Other "make" targets: - make mex compiles MATLAB mexFunctions only - make libccolamd.a compiles a C-callable library containing ccolamd - make clean removes all files not in the distribution, except for - libccolamd.a + make library compiles a C-callable library containing ccolamd + make clean removes all files not in the distribution + but keeps the compiled libraries. make distclean removes all files not in the distribution + make install installs the library in /usr/local/lib and + /usr/local/include + make uninstall uninstalls the library from /usr/local/lib and + /usr/local/include To use ccolamd and csymamd within an application written in C, all you need are ccolamd.c and ccolamd.h, which are the C-callable ccolamd/csymamd codes. See ccolamd.c for more information on how to call ccolamd from a C program. It contains a complete description of the C-interface to CCOLAMD and CSYMAMD. - Copyright (c) 1998-2007 by the University of Florida. - All Rights Reserved. - Licensed under the GNU LESSER GENERAL PUBLIC LICENSE. +See CCOLAMD/Doc/License.txt for the license. ------------------------------------------------------------------------------- -This library is free software; you can redistribute it and/or -modify it under the terms of the GNU Lesser General Public -License as published by the Free Software Foundation; either -version 2.1 of the License, or (at your option) any later version. - -This library is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -Lesser General Public License for more details. - -You should have received a copy of the GNU Lesser General Public -License along with this library; if not, write to the Free Software -Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA - -------------------------------------------------------------------------------- - - Related papers: T. A. Davis and W. W. Hager, Rajamanickam, Multiple-rank updates @@ -86,21 +67,18 @@ Related papers: "An approximate minimum degree column ordering algorithm", S. I. Larimore, MS Thesis, Dept. of Computer and Information Science and Engineering, University of Florida, Gainesville, FL, - 1998. CISE Tech Report TR-98-016. Available at - ftp://ftp.cise.ufl.edu/cis/tech-reports/tr98/tr98-016.ps - via anonymous ftp. + 1998. CISE Tech Report TR-98-016. Approximate Deficiency for Ordering the Columns of a Matrix, J. L. Kern, Senior Thesis, Dept. of Computer and Information Science and Engineering, University of Florida, Gainesville, FL, - 1999. Available at http://www.cise.ufl.edu/~davis/Kern/kern.ps + 1999. Authors: Timothy A. Davis, Sivasankaran Rajamanickam, and Stefan Larimore. Closely based on COLAMD by Stefan I. Larimore and Timothy A. Davis, - University of Florida, in collaboration with John Gilbert, Xerox PARC - (now at UC Santa Barbara), and Esmong Ng, Lawrence Berkeley National - Laboratory (much of this work he did while at Oak Ridge National - Laboratory). + in collaboration with John Gilbert, Xerox PARC (now at UC Santa + Barbara), and Esmong Ng, Lawrence Berkeley National Laboratory (much of + this work he did while at Oak Ridge National Laboratory). CCOLAMD files: @@ -122,7 +100,6 @@ CCOLAMD files: ./Doc: ChangeLog change log - lesser.txt license ./Include: ccolamd.h include file @@ -147,4 +124,3 @@ CCOLAMD files: ./Source: ccolamd.c primary source code - ccolamd_global.c globally defined function pointers (malloc, free, ...) diff --git a/gtsam/3rdparty/CCOLAMD/Source/ccolamd.c b/gtsam/3rdparty/CCOLAMD/Source/ccolamd.c index 1c35ffa41..9a08e3ea8 100644 --- a/gtsam/3rdparty/CCOLAMD/Source/ccolamd.c +++ b/gtsam/3rdparty/CCOLAMD/Source/ccolamd.c @@ -5,8 +5,6 @@ /* ---------------------------------------------------------------------------- * CCOLAMD, Copyright (C) Univ. of Florida. Authors: Timothy A. Davis, * Sivasankaran Rajamanickam, and Stefan Larimore - * See License.txt for the Version 2.1 of the GNU Lesser General Public License - * http://www.cise.ufl.edu/research/sparse * -------------------------------------------------------------------------- */ /* @@ -58,39 +56,13 @@ * COLAMD is also available under alternate licenses, contact T. Davis * for details. * - * This library is free software; you can redistribute it and/or - * modify it under the terms of the GNU Lesser General Public - * License as published by the Free Software Foundation; either - * version 2.1 of the License, or (at your option) any later version. - * - * This library is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - * Lesser General Public License for more details. - * - * You should have received a copy of the GNU Lesser General Public - * License along with this library; if not, write to the Free Software - * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 - * USA - * - * Permission is hereby granted to use or copy this program under the - * terms of the GNU LGPL, provided that the Copyright, this License, - * and the Availability of the original version is retained on all copies. - * User documentation of any code that uses this code or any modified - * version of this code must cite the Copyright, this License, the - * Availability note, and "Used by permission." Permission to modify - * the code and to distribute modified code is granted, provided the - * Copyright, this License, and the Availability note are retained, - * and a notice that the code was modified is included. + * See CCOLAMD/Doc/License.txt for the license. * * Availability: * * The CCOLAMD/CSYMAMD library is available at * - * http://www.cise.ufl.edu/research/sparse/ccolamd/ - * - * This is the http://www.cise.ufl.edu/research/sparse/ccolamd/ccolamd.c - * file. + * http://www.suitesparse.com * * See the ChangeLog file for changes since Version 1.0. */ @@ -99,10 +71,10 @@ /* === Description of user-callable routines ================================ */ /* ========================================================================== */ -/* CCOLAMD includes both int and UF_long versions of all its routines. The - * description below is for the int version. For UF_long, all int arguments - * become UF_long integers. UF_long is normally defined as long, except for - * WIN64 */ +/* CCOLAMD includes both int and SuiteSparse_long versions of all its routines. + * The description below is for the int version. For SuiteSparse_long, all + * int arguments become SuiteSparse_long integers. SuiteSparse_long is + * normally defined as long, except for WIN64 */ /* ---------------------------------------------------------------------------- * ccolamd_recommended: @@ -112,8 +84,8 @@ * * #include "ccolamd.h" * size_t ccolamd_recommended (int nnz, int n_row, int n_col) ; - * size_t ccolamd_l_recommended (UF_long nnz, UF_long n_row, - * UF_long n_col) ; + * size_t ccolamd_l_recommended (SuiteSparse_long nnz, + * SuiteSparse_long n_row, SuiteSparse_long n_col) ; * * Purpose: * @@ -209,9 +181,12 @@ * double knobs [CCOLAMD_KNOBS], int stats [CCOLAMD_STATS], * int *cmember) ; * - * UF_long ccolamd_l (UF_long n_row, UF_long n_col, UF_long Alen, - * UF_long *A, UF_long *p, double knobs [CCOLAMD_KNOBS], - * UF_long stats [CCOLAMD_STATS], UF_long *cmember) ; + * SuiteSparse_long ccolamd_l (SuiteSparse_long n_row, + * SuiteSparse_long n_col, SuiteSparse_long Alen, + * SuiteSparse_long *A, SuiteSparse_long *p, + * double knobs [CCOLAMD_KNOBS], + * SuiteSparse_long stats [CCOLAMD_STATS], + * SuiteSparse_long *cmember) ; * * Purpose: * @@ -385,9 +360,7 @@ * * Example: * - * See - * http://www.cise.ufl.edu/research/sparse/ccolamd/ccolamd_example.c - * for a complete example. + * See ccolamd_example.c for a complete example. * * To order the columns of a 5-by-4 matrix with 11 nonzero entries in * the following nonzero pattern @@ -423,10 +396,12 @@ * void (*allocate) (size_t, size_t), void (*release) (void *), * int *cmember, int stype) ; * - * UF_long csymamd_l (UF_long n, UF_long *A, UF_long *p, UF_long *perm, - * double knobs [CCOLAMD_KNOBS], UF_long stats [CCOLAMD_STATS], - * void (*allocate) (size_t, size_t), void (*release) (void *), - * UF_long *cmember, UF_long stype) ; + * SuiteSparse_long csymamd_l (SuiteSparse_long n, + * SuiteSparse_long *A, SuiteSparse_long *p, + * SuiteSparse_long *perm, double knobs [CCOLAMD_KNOBS], + * SuiteSparse_long stats [CCOLAMD_STATS], void (*allocate) + * (size_t, size_t), void (*release) (void *), + * SuiteSparse_long *cmember, SuiteSparse_long stype) ; * * Purpose: * @@ -562,7 +537,7 @@ * * #include "ccolamd.h" * ccolamd_report (int stats [CCOLAMD_STATS]) ; - * ccolamd_l_report (UF_long stats [CCOLAMD_STATS]) ; + * ccolamd_l_report (SuiteSparse_long stats [CCOLAMD_STATS]) ; * * Purpose: * @@ -583,7 +558,7 @@ * * #include "ccolamd.h" * csymamd_report (int stats [CCOLAMD_STATS]) ; - * csymamd_l_report (UF_long stats [CCOLAMD_STATS]) ; + * csymamd_l_report (SuiteSparse_long stats [CCOLAMD_STATS]) ; * * Purpose: * @@ -617,12 +592,11 @@ #include "ccolamd.h" +#include #include #include #ifdef MATLAB_MEX_FILE -#include -typedef uint16_t char16_t; #include "mex.h" #include "matrix.h" #endif @@ -636,17 +610,14 @@ typedef uint16_t char16_t; #endif /* ========================================================================== */ -/* === int or UF_long ======================================================= */ +/* === int or SuiteSparse_long ============================================== */ /* ========================================================================== */ -/* define UF_long */ -#include "UFconfig.h" - #ifdef DLONG -#define Int UF_long -#define ID UF_long_id -#define Int_MAX UF_long_max +#define Int SuiteSparse_long +#define ID SuiteSparse_long_id +#define Int_MAX SuiteSparse_long_max #define CCOLAMD_recommended ccolamd_l_recommended #define CCOLAMD_set_defaults ccolamd_l_set_defaults @@ -811,9 +782,6 @@ typedef struct CColamd_Row_struct #define INDEX(i) (i) #endif -/* All output goes through the PRINTF macro. */ -#define PRINTF(params) { if (ccolamd_printf != NULL) (void) ccolamd_printf params ; } - /* ========================================================================== */ /* === Debugging prototypes and definitions ================================= */ @@ -827,11 +795,11 @@ typedef struct CColamd_Row_struct PRIVATE Int ccolamd_debug ; /* debug print statements */ -#define DEBUG0(params) { PRINTF (params) ; } -#define DEBUG1(params) { if (ccolamd_debug >= 1) PRINTF (params) ; } -#define DEBUG2(params) { if (ccolamd_debug >= 2) PRINTF (params) ; } -#define DEBUG3(params) { if (ccolamd_debug >= 3) PRINTF (params) ; } -#define DEBUG4(params) { if (ccolamd_debug >= 4) PRINTF (params) ; } +#define DEBUG0(params) { SUITESPARSE_PRINTF (params) ; } +#define DEBUG1(params) { if (ccolamd_debug >= 1) SUITESPARSE_PRINTF (params) ; } +#define DEBUG2(params) { if (ccolamd_debug >= 2) SUITESPARSE_PRINTF (params) ; } +#define DEBUG3(params) { if (ccolamd_debug >= 3) SUITESPARSE_PRINTF (params) ; } +#define DEBUG4(params) { if (ccolamd_debug >= 4) SUITESPARSE_PRINTF (params) ; } #ifdef MATLAB_MEX_FILE #define ASSERT(expression) (mxAssert ((expression), "")) @@ -3752,12 +3720,12 @@ PRIVATE void print_report Int i1, i2, i3 ; - PRINTF (("\n%s version %d.%d, %s: ", method, + SUITESPARSE_PRINTF (("\n%s version %d.%d, %s: ", method, CCOLAMD_MAIN_VERSION, CCOLAMD_SUB_VERSION, CCOLAMD_DATE)) ; if (!stats) { - PRINTF (("No statistics available.\n")) ; + SUITESPARSE_PRINTF (("No statistics available.\n")) ; return ; } @@ -3767,11 +3735,11 @@ PRIVATE void print_report if (stats [CCOLAMD_STATUS] >= 0) { - PRINTF(("OK. ")) ; + SUITESPARSE_PRINTF(("OK. ")) ; } else { - PRINTF(("ERROR. ")) ; + SUITESPARSE_PRINTF(("ERROR. ")) ; } switch (stats [CCOLAMD_STATUS]) @@ -3779,91 +3747,105 @@ PRIVATE void print_report case CCOLAMD_OK_BUT_JUMBLED: - PRINTF(("Matrix has unsorted or duplicate row indices.\n")) ; + SUITESPARSE_PRINTF(( + "Matrix has unsorted or duplicate row indices.\n")) ; - PRINTF(("%s: duplicate or out-of-order row indices: "ID"\n", - method, i3)) ; + SUITESPARSE_PRINTF(( + "%s: duplicate or out-of-order row indices: "ID"\n", + method, i3)) ; - PRINTF(("%s: last seen duplicate or out-of-order row: "ID"\n", - method, INDEX (i2))) ; + SUITESPARSE_PRINTF(( + "%s: last seen duplicate or out-of-order row: "ID"\n", + method, INDEX (i2))) ; - PRINTF(("%s: last seen in column: "ID"", - method, INDEX (i1))) ; + SUITESPARSE_PRINTF(( + "%s: last seen in column: "ID"", + method, INDEX (i1))) ; /* no break - fall through to next case instead */ case CCOLAMD_OK: - PRINTF(("\n")) ; + SUITESPARSE_PRINTF(("\n")) ; - PRINTF(("%s: number of dense or empty rows ignored: "ID"\n", - method, stats [CCOLAMD_DENSE_ROW])) ; + SUITESPARSE_PRINTF(( + "%s: number of dense or empty rows ignored: "ID"\n", + method, stats [CCOLAMD_DENSE_ROW])) ; - PRINTF(("%s: number of dense or empty columns ignored: "ID"\n", - method, stats [CCOLAMD_DENSE_COL])) ; + SUITESPARSE_PRINTF(( + "%s: number of dense or empty columns ignored: "ID"\n", + method, stats [CCOLAMD_DENSE_COL])) ; - PRINTF(("%s: number of garbage collections performed: "ID"\n", - method, stats [CCOLAMD_DEFRAG_COUNT])) ; + SUITESPARSE_PRINTF(( + "%s: number of garbage collections performed: "ID"\n", + method, stats [CCOLAMD_DEFRAG_COUNT])) ; break ; case CCOLAMD_ERROR_A_not_present: - PRINTF(("Array A (row indices of matrix) not present.\n")) ; + SUITESPARSE_PRINTF(( + "Array A (row indices of matrix) not present.\n")) ; break ; case CCOLAMD_ERROR_p_not_present: - PRINTF(("Array p (column pointers for matrix) not present.\n")) ; + SUITESPARSE_PRINTF(( + "Array p (column pointers for matrix) not present.\n")) ; break ; case CCOLAMD_ERROR_nrow_negative: - PRINTF(("Invalid number of rows ("ID").\n", i1)) ; + SUITESPARSE_PRINTF(("Invalid number of rows ("ID").\n", i1)) ; break ; case CCOLAMD_ERROR_ncol_negative: - PRINTF(("Invalid number of columns ("ID").\n", i1)) ; + SUITESPARSE_PRINTF(("Invalid number of columns ("ID").\n", i1)) ; break ; case CCOLAMD_ERROR_nnz_negative: - PRINTF(("Invalid number of nonzero entries ("ID").\n", i1)) ; + SUITESPARSE_PRINTF(( + "Invalid number of nonzero entries ("ID").\n", i1)) ; break ; case CCOLAMD_ERROR_p0_nonzero: - PRINTF(("Invalid column pointer, p [0] = "ID", must be 0.\n", i1)) ; + SUITESPARSE_PRINTF(( + "Invalid column pointer, p [0] = "ID", must be 0.\n", i1)) ; break ; case CCOLAMD_ERROR_A_too_small: - PRINTF(("Array A too small.\n")) ; - PRINTF((" Need Alen >= "ID", but given only Alen = "ID".\n", - i1, i2)) ; + SUITESPARSE_PRINTF(("Array A too small.\n")) ; + SUITESPARSE_PRINTF(( + " Need Alen >= "ID", but given only Alen = "ID".\n", + i1, i2)) ; break ; case CCOLAMD_ERROR_col_length_negative: - PRINTF(("Column "ID" has a negative number of entries ("ID").\n", - INDEX (i1), i2)) ; + SUITESPARSE_PRINTF(( + "Column "ID" has a negative number of entries ("ID").\n", + INDEX (i1), i2)) ; break ; case CCOLAMD_ERROR_row_index_out_of_bounds: - PRINTF(("Row index (row "ID") out of bounds ("ID" to "ID") in" - "column "ID".\n", INDEX (i2), INDEX (0), INDEX (i3-1), - INDEX (i1))) ; + SUITESPARSE_PRINTF(( + "Row index (row "ID") out of bounds ("ID" to "ID") in" + "column "ID".\n", INDEX (i2), INDEX (0), INDEX (i3-1), + INDEX (i1))) ; break ; case CCOLAMD_ERROR_out_of_memory: - PRINTF(("Out of memory.\n")) ; + SUITESPARSE_PRINTF(("Out of memory.\n")) ; break ; case CCOLAMD_ERROR_invalid_cmember: - PRINTF(("cmember invalid\n")) ; + SUITESPARSE_PRINTF(("cmember invalid\n")) ; break ; } } diff --git a/gtsam/3rdparty/CMakeLists.txt b/gtsam/3rdparty/CMakeLists.txt index 8534a8d7e..b890bc4af 100644 --- a/gtsam/3rdparty/CMakeLists.txt +++ b/gtsam/3rdparty/CMakeLists.txt @@ -1,6 +1,6 @@ # install CCOLAMD headers install(FILES CCOLAMD/Include/ccolamd.h DESTINATION include/gtsam/3rdparty/CCOLAMD) -install(FILES UFconfig/UFconfig.h DESTINATION include/gtsam/3rdparty/UFconfig) +install(FILES SuiteSparse_config/SuiteSparse_config.h DESTINATION include/gtsam/3rdparty/SuiteSparse_config) if(NOT GTSAM_USE_SYSTEM_EIGEN) # Find plain .h files diff --git a/gtsam/3rdparty/SuiteSparse_config/Makefile b/gtsam/3rdparty/SuiteSparse_config/Makefile new file mode 100644 index 000000000..96db5772f --- /dev/null +++ b/gtsam/3rdparty/SuiteSparse_config/Makefile @@ -0,0 +1,70 @@ +#------------------------------------------------------------------------------- +# SuiteSparse_config Makefile +#------------------------------------------------------------------------------- + +SUITESPARSE ?= $(realpath $(CURDIR)/..) +export SUITESPARSE + +# version of SuiteSparse_config is also version of SuiteSparse meta-package +LIBRARY = libsuitesparseconfig +VERSION = 4.5.2 +SO_VERSION = 4 + +default: library + +include SuiteSparse_config.mk + +ccode: all + +all: library + +# compile and install in SuiteSparse/lib +library: $(AR_TARGET) + $(MAKE) install INSTALL=$(SUITESPARSE) + +OBJ = SuiteSparse_config.o + +SuiteSparse_config.o: SuiteSparse_config.c SuiteSparse_config.h + $(CC) $(CF) -c SuiteSparse_config.c + +$(AR_TARGET): $(OBJ) + $(ARCHIVE) $(AR_TARGET) SuiteSparse_config.o + $(RANLIB) $(AR_TARGET) + +distclean: purge + +purge: clean + ( cd xerbla ; $(MAKE) purge ) + - $(RM) -r $(PURGE) + +clean: + ( cd xerbla ; $(MAKE) clean ) + - $(RM) -r $(CLEAN) + +# install SuiteSparse_config +install: $(AR_TARGET) $(INSTALL_LIB)/$(SO_TARGET) + +$(INSTALL_LIB)/$(SO_TARGET): $(OBJ) + @mkdir -p $(INSTALL_LIB) + @mkdir -p $(INSTALL_INCLUDE) + @mkdir -p $(INSTALL_DOC) + $(CC) $(SO_OPTS) $^ -o $@ $(LDLIBS) + ( cd $(INSTALL_LIB) ; ln -sf $(SO_TARGET) $(SO_PLAIN) ) + ( cd $(INSTALL_LIB) ; ln -sf $(SO_TARGET) $(SO_MAIN) ) + $(CP) SuiteSparse_config.h $(INSTALL_INCLUDE) + $(CP) README.txt $(INSTALL_DOC)/SUITESPARSECONFIG_README.txt + chmod 755 $(INSTALL_LIB)/$(SO_TARGET) + chmod 755 $(INSTALL_LIB)/$(SO_PLAIN) + chmod 644 $(INSTALL_INCLUDE)/SuiteSparse_config.h + chmod 644 $(INSTALL_DOC)/SUITESPARSECONFIG_README.txt + +# uninstall SuiteSparse_config +uninstall: + $(RM) $(INSTALL_LIB)/$(SO_TARGET) + $(RM) $(INSTALL_LIB)/$(SO_PLAIN) + $(RM) $(INSTALL_LIB)/$(SO_MAIN) + $(RM) $(INSTALL_INCLUDE)/SuiteSparse_config.h + $(RM) $(INSTALL_DOC)/SUITESPARSECONFIG_README.txt + ( cd xerbla ; $(MAKE) uninstall ) + + diff --git a/gtsam/3rdparty/SuiteSparse_config/README.txt b/gtsam/3rdparty/SuiteSparse_config/README.txt new file mode 100644 index 000000000..a76a5fab6 --- /dev/null +++ b/gtsam/3rdparty/SuiteSparse_config/README.txt @@ -0,0 +1,51 @@ +SuiteSparse_config, 2016, Timothy A. Davis, http://www.suitesparse.com +(formerly the UFconfig package) + +This directory contains a default SuiteSparse_config.mk file. It tries to +detect your system (Linux, SunOS, or Mac), which compiler to use (icc or cc), +which BLAS and LAPACK library to use (OpenBLAS or MKL), and whether or not to +compile with CUDA. + +For alternatives, see the comments in the SuiteSparse_config.mk file. + +License: No licensing restrictions apply to this file or to the +SuiteSparse_config directory. + +-------------------------------------------------------------------------------- + +SuiteSparse_config contains configuration settings for all many of the software +packages that I develop or co-author. Note that older versions of some of +these packages do not require SuiteSparse_config. + + Package Description + ------- ----------- + AMD approximate minimum degree ordering + CAMD constrained AMD + COLAMD column approximate minimum degree ordering + CCOLAMD constrained approximate minimum degree ordering + UMFPACK sparse LU factorization, with the BLAS + CXSparse int/long/real/complex version of CSparse + CHOLMOD sparse Cholesky factorization, update/downdate + KLU sparse LU factorization, BLAS-free + BTF permutation to block triangular form + LDL concise sparse LDL' + LPDASA LP Dual Active Set Algorithm + RBio read/write files in Rutherford/Boeing format + SPQR sparse QR factorization (full name: SuiteSparseQR) + +SuiteSparse_config is not required by these packages: + + CSparse a Concise Sparse matrix package + MATLAB_Tools toolboxes for use in MATLAB + +In addition, the xerbla/ directory contains Fortan and C versions of the +BLAS/LAPACK xerbla routine, which is called when an invalid input is passed to +the BLAS or LAPACK. The xerbla provided here does not print any message, so +the entire Fortran I/O library does not need to be linked into a C application. +Most versions of the BLAS contain xerbla, but those from K. Goto do not. Use +this if you need too. + +If you edit this directory (SuiteSparse_config.mk in particular) then you +must do "make purge ; make" in the parent directory to recompile all of +SuiteSparse. Otherwise, the changes will not necessarily be applied. + diff --git a/gtsam/3rdparty/SuiteSparse_config/SuiteSparse_config.c b/gtsam/3rdparty/SuiteSparse_config/SuiteSparse_config.c new file mode 100644 index 000000000..b491539fe --- /dev/null +++ b/gtsam/3rdparty/SuiteSparse_config/SuiteSparse_config.c @@ -0,0 +1,531 @@ +/* ========================================================================== */ +/* === SuiteSparse_config =================================================== */ +/* ========================================================================== */ + +/* SuiteSparse configuration : memory manager and printf functions. */ + +/* Copyright (c) 2013, Timothy A. Davis. No licensing restrictions + * apply to this file or to the SuiteSparse_config directory. + * Author: Timothy A. Davis. + */ + +#include +#include + +#ifndef NPRINT +#include +#endif + +#ifdef MATLAB_MEX_FILE +#include "mex.h" +#include "matrix.h" +#endif + +#ifndef NULL +#define NULL ((void *) 0) +#endif + +#include "SuiteSparse_config.h" + +/* -------------------------------------------------------------------------- */ +/* SuiteSparse_config : a global extern struct */ +/* -------------------------------------------------------------------------- */ + +/* The SuiteSparse_config struct is available to all SuiteSparse functions and + to all applications that use those functions. It must be modified with + care, particularly in a multithreaded context. Normally, the application + will initialize this object once, via SuiteSparse_start, possibily followed + by application-specific modifications if the applications wants to use + alternative memory manager functions. + + The user can redefine these global pointers at run-time to change the + memory manager and printf function used by SuiteSparse. + + If -DNMALLOC is defined at compile-time, then no memory-manager is + specified. You must define them at run-time, after calling + SuiteSparse_start. + + If -DPRINT is defined a compile time, then printf is disabled, and + SuiteSparse will not use printf. + */ + +struct SuiteSparse_config_struct SuiteSparse_config = +{ + + /* memory management functions */ + #ifndef NMALLOC + #ifdef MATLAB_MEX_FILE + /* MATLAB mexFunction: */ + mxMalloc, mxCalloc, mxRealloc, mxFree, + #else + /* standard ANSI C: */ + malloc, calloc, realloc, free, + #endif + #else + /* no memory manager defined; you must define one at run-time: */ + NULL, NULL, NULL, NULL, + #endif + + /* printf function */ + #ifndef NPRINT + #ifdef MATLAB_MEX_FILE + /* MATLAB mexFunction: */ + mexPrintf, + #else + /* standard ANSI C: */ + printf, + #endif + #else + /* printf is disabled */ + NULL, + #endif + + SuiteSparse_hypot, + SuiteSparse_divcomplex + +} ; + +/* -------------------------------------------------------------------------- */ +/* SuiteSparse_start */ +/* -------------------------------------------------------------------------- */ + +/* All applications that use SuiteSparse should call SuiteSparse_start prior + to using any SuiteSparse function. Only a single thread should call this + function, in a multithreaded application. Currently, this function is + optional, since all this function currently does is to set the four memory + function pointers to NULL (which tells SuiteSparse to use the default + functions). In a multi- threaded application, only a single thread should + call this function. + + Future releases of SuiteSparse might enforce a requirement that + SuiteSparse_start be called prior to calling any SuiteSparse function. + */ + +void SuiteSparse_start ( void ) +{ + + /* memory management functions */ + #ifndef NMALLOC + #ifdef MATLAB_MEX_FILE + /* MATLAB mexFunction: */ + SuiteSparse_config.malloc_func = mxMalloc ; + SuiteSparse_config.calloc_func = mxCalloc ; + SuiteSparse_config.realloc_func = mxRealloc ; + SuiteSparse_config.free_func = mxFree ; + #else + /* standard ANSI C: */ + SuiteSparse_config.malloc_func = malloc ; + SuiteSparse_config.calloc_func = calloc ; + SuiteSparse_config.realloc_func = realloc ; + SuiteSparse_config.free_func = free ; + #endif + #else + /* no memory manager defined; you must define one after calling + SuiteSparse_start */ + SuiteSparse_config.malloc_func = NULL ; + SuiteSparse_config.calloc_func = NULL ; + SuiteSparse_config.realloc_func = NULL ; + SuiteSparse_config.free_func = NULL ; + #endif + + /* printf function */ + #ifndef NPRINT + #ifdef MATLAB_MEX_FILE + /* MATLAB mexFunction: */ + SuiteSparse_config.printf_func = mexPrintf ; + #else + /* standard ANSI C: */ + SuiteSparse_config.printf_func = printf ; + #endif + #else + /* printf is disabled */ + SuiteSparse_config.printf_func = NULL ; + #endif + + /* math functions */ + SuiteSparse_config.hypot_func = SuiteSparse_hypot ; + SuiteSparse_config.divcomplex_func = SuiteSparse_divcomplex ; +} + +/* -------------------------------------------------------------------------- */ +/* SuiteSparse_finish */ +/* -------------------------------------------------------------------------- */ + +/* This currently does nothing, but in the future, applications should call + SuiteSparse_start before calling any SuiteSparse function, and then + SuiteSparse_finish after calling the last SuiteSparse function, just before + exiting. In a multithreaded application, only a single thread should call + this function. + + Future releases of SuiteSparse might use this function for any + SuiteSparse-wide cleanup operations or finalization of statistics. + */ + +void SuiteSparse_finish ( void ) +{ + /* do nothing */ ; +} + +/* -------------------------------------------------------------------------- */ +/* SuiteSparse_malloc: malloc wrapper */ +/* -------------------------------------------------------------------------- */ + +void *SuiteSparse_malloc /* pointer to allocated block of memory */ +( + size_t nitems, /* number of items to malloc */ + size_t size_of_item /* sizeof each item */ +) +{ + void *p ; + size_t size ; + if (nitems < 1) nitems = 1 ; + if (size_of_item < 1) size_of_item = 1 ; + size = nitems * size_of_item ; + + if (size != ((double) nitems) * size_of_item) + { + /* size_t overflow */ + p = NULL ; + } + else + { + p = (void *) (SuiteSparse_config.malloc_func) (size) ; + } + return (p) ; +} + + +/* -------------------------------------------------------------------------- */ +/* SuiteSparse_calloc: calloc wrapper */ +/* -------------------------------------------------------------------------- */ + +void *SuiteSparse_calloc /* pointer to allocated block of memory */ +( + size_t nitems, /* number of items to calloc */ + size_t size_of_item /* sizeof each item */ +) +{ + void *p ; + size_t size ; + if (nitems < 1) nitems = 1 ; + if (size_of_item < 1) size_of_item = 1 ; + size = nitems * size_of_item ; + + if (size != ((double) nitems) * size_of_item) + { + /* size_t overflow */ + p = NULL ; + } + else + { + p = (void *) (SuiteSparse_config.calloc_func) (nitems, size_of_item) ; + } + return (p) ; +} + +/* -------------------------------------------------------------------------- */ +/* SuiteSparse_realloc: realloc wrapper */ +/* -------------------------------------------------------------------------- */ + +/* If p is non-NULL on input, it points to a previously allocated object of + size nitems_old * size_of_item. The object is reallocated to be of size + nitems_new * size_of_item. If p is NULL on input, then a new object of that + size is allocated. On success, a pointer to the new object is returned, + and ok is returned as 1. If the allocation fails, ok is set to 0 and a + pointer to the old (unmodified) object is returned. + */ + +void *SuiteSparse_realloc /* pointer to reallocated block of memory, or + to original block if the realloc failed. */ +( + size_t nitems_new, /* new number of items in the object */ + size_t nitems_old, /* old number of items in the object */ + size_t size_of_item, /* sizeof each item */ + void *p, /* old object to reallocate */ + int *ok /* 1 if successful, 0 otherwise */ +) +{ + size_t size ; + if (nitems_old < 1) nitems_old = 1 ; + if (nitems_new < 1) nitems_new = 1 ; + if (size_of_item < 1) size_of_item = 1 ; + size = nitems_new * size_of_item ; + + if (size != ((double) nitems_new) * size_of_item) + { + /* size_t overflow */ + (*ok) = 0 ; + } + else if (p == NULL) + { + /* a fresh object is being allocated */ + p = SuiteSparse_malloc (nitems_new, size_of_item) ; + (*ok) = (p != NULL) ; + } + else if (nitems_old == nitems_new) + { + /* the object does not change; do nothing */ + (*ok) = 1 ; + } + else + { + /* change the size of the object from nitems_old to nitems_new */ + void *pnew ; + pnew = (void *) (SuiteSparse_config.realloc_func) (p, size) ; + if (pnew == NULL) + { + if (nitems_new < nitems_old) + { + /* the attempt to reduce the size of the block failed, but + the old block is unchanged. So pretend to succeed. */ + (*ok) = 1 ; + } + else + { + /* out of memory */ + (*ok) = 0 ; + } + } + else + { + /* success */ + p = pnew ; + (*ok) = 1 ; + } + } + return (p) ; +} + +/* -------------------------------------------------------------------------- */ +/* SuiteSparse_free: free wrapper */ +/* -------------------------------------------------------------------------- */ + +void *SuiteSparse_free /* always returns NULL */ +( + void *p /* block to free */ +) +{ + if (p) + { + (SuiteSparse_config.free_func) (p) ; + } + return (NULL) ; +} + + +/* -------------------------------------------------------------------------- */ +/* SuiteSparse_tic: return current wall clock time */ +/* -------------------------------------------------------------------------- */ + +/* Returns the number of seconds (tic [0]) and nanoseconds (tic [1]) since some + * unspecified but fixed time in the past. If no timer is installed, zero is + * returned. A scalar double precision value for 'tic' could be used, but this + * might cause loss of precision because clock_getttime returns the time from + * some distant time in the past. Thus, an array of size 2 is used. + * + * The timer is enabled by default. To disable the timer, compile with + * -DNTIMER. If enabled on a POSIX C 1993 system, the timer requires linking + * with the -lrt library. + * + * example: + * + * double tic [2], r, s, t ; + * SuiteSparse_tic (tic) ; // start the timer + * // do some work A + * t = SuiteSparse_toc (tic) ; // t is time for work A, in seconds + * // do some work B + * s = SuiteSparse_toc (tic) ; // s is time for work A and B, in seconds + * SuiteSparse_tic (tic) ; // restart the timer + * // do some work C + * r = SuiteSparse_toc (tic) ; // s is time for work C, in seconds + * + * A double array of size 2 is used so that this routine can be more easily + * ported to non-POSIX systems. The caller does not rely on the POSIX + * include file. + */ + +#ifdef SUITESPARSE_TIMER_ENABLED + +#include + +void SuiteSparse_tic +( + double tic [2] /* output, contents undefined on input */ +) +{ + /* POSIX C 1993 timer, requires -librt */ + struct timespec t ; + clock_gettime (CLOCK_MONOTONIC, &t) ; + tic [0] = (double) (t.tv_sec) ; + tic [1] = (double) (t.tv_nsec) ; +} + +#else + +void SuiteSparse_tic +( + double tic [2] /* output, contents undefined on input */ +) +{ + /* no timer installed */ + tic [0] = 0 ; + tic [1] = 0 ; +} + +#endif + + +/* -------------------------------------------------------------------------- */ +/* SuiteSparse_toc: return time since last tic */ +/* -------------------------------------------------------------------------- */ + +/* Assuming SuiteSparse_tic is accurate to the nanosecond, this function is + * accurate down to the nanosecond for 2^53 nanoseconds since the last call to + * SuiteSparse_tic, which is sufficient for SuiteSparse (about 104 days). If + * additional accuracy is required, the caller can use two calls to + * SuiteSparse_tic and do the calculations differently. + */ + +double SuiteSparse_toc /* returns time in seconds since last tic */ +( + double tic [2] /* input, not modified from last call to SuiteSparse_tic */ +) +{ + double toc [2] ; + SuiteSparse_tic (toc) ; + return ((toc [0] - tic [0]) + 1e-9 * (toc [1] - tic [1])) ; +} + + +/* -------------------------------------------------------------------------- */ +/* SuiteSparse_time: return current wallclock time in seconds */ +/* -------------------------------------------------------------------------- */ + +/* This function might not be accurate down to the nanosecond. */ + +double SuiteSparse_time /* returns current wall clock time in seconds */ +( + void +) +{ + double toc [2] ; + SuiteSparse_tic (toc) ; + return (toc [0] + 1e-9 * toc [1]) ; +} + + +/* -------------------------------------------------------------------------- */ +/* SuiteSparse_version: return the current version of SuiteSparse */ +/* -------------------------------------------------------------------------- */ + +int SuiteSparse_version +( + int version [3] +) +{ + if (version != NULL) + { + version [0] = SUITESPARSE_MAIN_VERSION ; + version [1] = SUITESPARSE_SUB_VERSION ; + version [2] = SUITESPARSE_SUBSUB_VERSION ; + } + return (SUITESPARSE_VERSION) ; +} + +/* -------------------------------------------------------------------------- */ +/* SuiteSparse_hypot */ +/* -------------------------------------------------------------------------- */ + +/* There is an equivalent routine called hypot in , which conforms + * to ANSI C99. However, SuiteSparse does not assume that ANSI C99 is + * available. You can use the ANSI C99 hypot routine with: + * + * #include + *i SuiteSparse_config.hypot_func = hypot ; + * + * Default value of the SuiteSparse_config.hypot_func pointer is + * SuiteSparse_hypot, defined below. + * + * s = hypot (x,y) computes s = sqrt (x*x + y*y) but does so more accurately. + * The NaN cases for the double relops x >= y and x+y == x are safely ignored. + * + * Source: Algorithm 312, "Absolute value and square root of a complex number," + * P. Friedland, Comm. ACM, vol 10, no 10, October 1967, page 665. + */ + +double SuiteSparse_hypot (double x, double y) +{ + double s, r ; + x = fabs (x) ; + y = fabs (y) ; + if (x >= y) + { + if (x + y == x) + { + s = x ; + } + else + { + r = y / x ; + s = x * sqrt (1.0 + r*r) ; + } + } + else + { + if (y + x == y) + { + s = y ; + } + else + { + r = x / y ; + s = y * sqrt (1.0 + r*r) ; + } + } + return (s) ; +} + +/* -------------------------------------------------------------------------- */ +/* SuiteSparse_divcomplex */ +/* -------------------------------------------------------------------------- */ + +/* c = a/b where c, a, and b are complex. The real and imaginary parts are + * passed as separate arguments to this routine. The NaN case is ignored + * for the double relop br >= bi. Returns 1 if the denominator is zero, + * 0 otherwise. + * + * This uses ACM Algo 116, by R. L. Smith, 1962, which tries to avoid + * underflow and overflow. + * + * c can be the same variable as a or b. + * + * Default value of the SuiteSparse_config.divcomplex_func pointer is + * SuiteSparse_divcomplex. + */ + +int SuiteSparse_divcomplex +( + double ar, double ai, /* real and imaginary parts of a */ + double br, double bi, /* real and imaginary parts of b */ + double *cr, double *ci /* real and imaginary parts of c */ +) +{ + double tr, ti, r, den ; + if (fabs (br) >= fabs (bi)) + { + r = bi / br ; + den = br + r * bi ; + tr = (ar + ai * r) / den ; + ti = (ai - ar * r) / den ; + } + else + { + r = br / bi ; + den = r * br + bi ; + tr = (ar * r + ai) / den ; + ti = (ai * r - ar) / den ; + } + *cr = tr ; + *ci = ti ; + return (den == 0.) ; +} diff --git a/gtsam/3rdparty/SuiteSparse_config/SuiteSparse_config.h b/gtsam/3rdparty/SuiteSparse_config/SuiteSparse_config.h new file mode 100644 index 000000000..49296fc5a --- /dev/null +++ b/gtsam/3rdparty/SuiteSparse_config/SuiteSparse_config.h @@ -0,0 +1,248 @@ +/* ========================================================================== */ +/* === SuiteSparse_config =================================================== */ +/* ========================================================================== */ + +/* Configuration file for SuiteSparse: a Suite of Sparse matrix packages + * (AMD, COLAMD, CCOLAMD, CAMD, CHOLMOD, UMFPACK, CXSparse, and others). + * + * SuiteSparse_config.h provides the definition of the long integer. On most + * systems, a C program can be compiled in LP64 mode, in which long's and + * pointers are both 64-bits, and int's are 32-bits. Windows 64, however, uses + * the LLP64 model, in which int's and long's are 32-bits, and long long's and + * pointers are 64-bits. + * + * SuiteSparse packages that include long integer versions are + * intended for the LP64 mode. However, as a workaround for Windows 64 + * (and perhaps other systems), the long integer can be redefined. + * + * If _WIN64 is defined, then the __int64 type is used instead of long. + * + * The long integer can also be defined at compile time. For example, this + * could be added to SuiteSparse_config.mk: + * + * CFLAGS = -O -D'SuiteSparse_long=long long' \ + * -D'SuiteSparse_long_max=9223372036854775801' -D'SuiteSparse_long_idd="lld"' + * + * This file defines SuiteSparse_long as either long (on all but _WIN64) or + * __int64 on Windows 64. The intent is that a SuiteSparse_long is always a + * 64-bit integer in a 64-bit code. ptrdiff_t might be a better choice than + * long; it is always the same size as a pointer. + * + * This file also defines the SUITESPARSE_VERSION and related definitions. + * + * Copyright (c) 2012, Timothy A. Davis. No licensing restrictions apply + * to this file or to the SuiteSparse_config directory. + * Author: Timothy A. Davis. + */ + +#ifndef SUITESPARSE_CONFIG_H +#define SUITESPARSE_CONFIG_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + +/* ========================================================================== */ +/* === SuiteSparse_long ===================================================== */ +/* ========================================================================== */ + +#ifndef SuiteSparse_long + +#ifdef _WIN64 + +#define SuiteSparse_long __int64 +#define SuiteSparse_long_max _I64_MAX +#define SuiteSparse_long_idd "I64d" + +#else + +#define SuiteSparse_long long +#define SuiteSparse_long_max LONG_MAX +#define SuiteSparse_long_idd "ld" + +#endif +#define SuiteSparse_long_id "%" SuiteSparse_long_idd +#endif + +/* ========================================================================== */ +/* === SuiteSparse_config parameters and functions ========================== */ +/* ========================================================================== */ + +/* SuiteSparse-wide parameters are placed in this struct. It is meant to be + an extern, globally-accessible struct. It is not meant to be updated + frequently by multiple threads. Rather, if an application needs to modify + SuiteSparse_config, it should do it once at the beginning of the application, + before multiple threads are launched. + + The intent of these function pointers is that they not be used in your + application directly, except to assign them to the desired user-provided + functions. Rather, you should use the + */ + +struct SuiteSparse_config_struct +{ + void *(*malloc_func) (size_t) ; /* pointer to malloc */ + void *(*calloc_func) (size_t, size_t) ; /* pointer to calloc */ + void *(*realloc_func) (void *, size_t) ; /* pointer to realloc */ + void (*free_func) (void *) ; /* pointer to free */ + int (*printf_func) (const char *, ...) ; /* pointer to printf */ + double (*hypot_func) (double, double) ; /* pointer to hypot */ + int (*divcomplex_func) (double, double, double, double, double *, double *); +} ; + +extern struct SuiteSparse_config_struct SuiteSparse_config ; + +void SuiteSparse_start ( void ) ; /* called to start SuiteSparse */ + +void SuiteSparse_finish ( void ) ; /* called to finish SuiteSparse */ + +void *SuiteSparse_malloc /* pointer to allocated block of memory */ +( + size_t nitems, /* number of items to malloc (>=1 is enforced) */ + size_t size_of_item /* sizeof each item */ +) ; + +void *SuiteSparse_calloc /* pointer to allocated block of memory */ +( + size_t nitems, /* number of items to calloc (>=1 is enforced) */ + size_t size_of_item /* sizeof each item */ +) ; + +void *SuiteSparse_realloc /* pointer to reallocated block of memory, or + to original block if the realloc failed. */ +( + size_t nitems_new, /* new number of items in the object */ + size_t nitems_old, /* old number of items in the object */ + size_t size_of_item, /* sizeof each item */ + void *p, /* old object to reallocate */ + int *ok /* 1 if successful, 0 otherwise */ +) ; + +void *SuiteSparse_free /* always returns NULL */ +( + void *p /* block to free */ +) ; + +void SuiteSparse_tic /* start the timer */ +( + double tic [2] /* output, contents undefined on input */ +) ; + +double SuiteSparse_toc /* return time in seconds since last tic */ +( + double tic [2] /* input: from last call to SuiteSparse_tic */ +) ; + +double SuiteSparse_time /* returns current wall clock time in seconds */ +( + void +) ; + +/* returns sqrt (x^2 + y^2), computed reliably */ +double SuiteSparse_hypot (double x, double y) ; + +/* complex division of c = a/b */ +int SuiteSparse_divcomplex +( + double ar, double ai, /* real and imaginary parts of a */ + double br, double bi, /* real and imaginary parts of b */ + double *cr, double *ci /* real and imaginary parts of c */ +) ; + +/* determine which timer to use, if any */ +#ifndef NTIMER +#ifdef _POSIX_C_SOURCE +#if _POSIX_C_SOURCE >= 199309L +#define SUITESPARSE_TIMER_ENABLED +#endif +#endif +#endif + +/* SuiteSparse printf macro */ +#define SUITESPARSE_PRINTF(params) \ +{ \ + if (SuiteSparse_config.printf_func != NULL) \ + { \ + (void) (SuiteSparse_config.printf_func) params ; \ + } \ +} + +/* ========================================================================== */ +/* === SuiteSparse version ================================================== */ +/* ========================================================================== */ + +/* SuiteSparse is not a package itself, but a collection of packages, some of + * which must be used together (UMFPACK requires AMD, CHOLMOD requires AMD, + * COLAMD, CAMD, and CCOLAMD, etc). A version number is provided here for the + * collection itself. The versions of packages within each version of + * SuiteSparse are meant to work together. Combining one package from one + * version of SuiteSparse, with another package from another version of + * SuiteSparse, may or may not work. + * + * SuiteSparse contains the following packages: + * + * SuiteSparse_config version 4.5.2 (version always the same as SuiteSparse) + * AMD version 2.4.5 + * BTF version 1.2.5 + * CAMD version 2.4.5 + * CCOLAMD version 2.9.5 + * CHOLMOD version 3.0.10 + * COLAMD version 2.9.5 + * CSparse version 3.1.8 + * CXSparse version 3.1.8 + * GPUQREngine version 1.0.4 + * KLU version 1.3.7 + * LDL version 2.2.5 + * RBio version 2.2.5 + * SPQR version 2.0.6 + * SuiteSparse_GPURuntime version 1.0.4 + * UMFPACK version 5.7.5 + * MATLAB_Tools various packages & M-files + * xerbla version 1.0.2 + * + * Other package dependencies: + * BLAS required by CHOLMOD and UMFPACK + * LAPACK required by CHOLMOD + * METIS 5.1.0 required by CHOLMOD (optional) and KLU (optional) + * CUBLAS, CUDART NVIDIA libraries required by CHOLMOD and SPQR when + * they are compiled with GPU acceleration. + */ + + +int SuiteSparse_version /* returns SUITESPARSE_VERSION */ +( + /* output, not defined on input. Not used if NULL. Returns + the three version codes in version [0..2]: + version [0] is SUITESPARSE_MAIN_VERSION + version [1] is SUITESPARSE_SUB_VERSION + version [2] is SUITESPARSE_SUBSUB_VERSION + */ + int version [3] +) ; + +/* Versions prior to 4.2.0 do not have the above function. The following + code fragment will work with any version of SuiteSparse: + + #ifdef SUITESPARSE_HAS_VERSION_FUNCTION + v = SuiteSparse_version (NULL) ; + #else + v = SUITESPARSE_VERSION ; + #endif +*/ +#define SUITESPARSE_HAS_VERSION_FUNCTION + +#define SUITESPARSE_DATE "Apr 1, 2016" +#define SUITESPARSE_VER_CODE(main,sub) ((main) * 1000 + (sub)) +#define SUITESPARSE_MAIN_VERSION 4 +#define SUITESPARSE_SUB_VERSION 5 +#define SUITESPARSE_SUBSUB_VERSION 2 +#define SUITESPARSE_VERSION \ + SUITESPARSE_VER_CODE(SUITESPARSE_MAIN_VERSION,SUITESPARSE_SUB_VERSION) + +#ifdef __cplusplus +} +#endif +#endif diff --git a/gtsam/3rdparty/SuiteSparse_config/SuiteSparse_config.mk b/gtsam/3rdparty/SuiteSparse_config/SuiteSparse_config.mk new file mode 100644 index 000000000..40ad6b9af --- /dev/null +++ b/gtsam/3rdparty/SuiteSparse_config/SuiteSparse_config.mk @@ -0,0 +1,600 @@ +#=============================================================================== +# SuiteSparse_config.mk: common configuration file for the SuiteSparse +#=============================================================================== + +# This file contains all configuration settings for all packages in SuiteSparse, +# except for CSparse (which is stand-alone) and the packages in MATLAB_Tools. + +SUITESPARSE_VERSION = 4.5.2 + +#=============================================================================== +# Options you can change without editing this file: +#=============================================================================== + + # To list the options you can modify at the 'make' command line, type + # 'make config', which also lists their default values. You can then + # change them with 'make OPTION=value'. For example, to use an INSTALL + # path of /my/path, and to use your own BLAS and LAPACK libraries, do: + # + # make install INSTALL=/my/path BLAS=-lmyblas LAPACK=-lmylapackgoeshere + # + # which will install the package into /my/path/lib and /my/path/include, + # and use -lmyblas -lmylapackgoes here when building the demo program. + +#=============================================================================== +# Defaults for any system +#=============================================================================== + + #--------------------------------------------------------------------------- + # SuiteSparse root directory + #--------------------------------------------------------------------------- + + # Most Makefiles are in SuiteSparse/Pkg/Lib or SuiteSparse/Pkg/Demo, so + # the top-level of SuiteSparse is in ../.. unless otherwise specified. + # This is true for all but the SuiteSparse_config package. + SUITESPARSE ?= $(realpath $(CURDIR)/../..) + + #--------------------------------------------------------------------------- + # installation location + #--------------------------------------------------------------------------- + + # For "make install" and "make uninstall", the default location is + # SuiteSparse/lib, SuiteSparse/include, and + # SuiteSparse/share/doc/suitesparse-x.y.z + # If you do this: + # make install INSTALL=/usr/local + # then the libraries are installed in /usr/local/lib, include files in + # /usr/local/include, and documentation in + # /usr/local/share/doc/suitesparse-x.y.z. + # You can instead specify the install location of each of these 3 components + # separately, via (for example): + # make install INSTALL_LIB=/yada/mylibs INSTALL_INCLUDE=/yoda/myinc \ + # INSTALL_DOC=/solo/mydox + # which puts the libraries in /yada/mylibs, include files in /yoda/myinc, + # and documentation in /solo/mydox. + INSTALL ?= $(SUITESPARSE) + INSTALL_LIB ?= $(INSTALL)/lib + INSTALL_INCLUDE ?= $(INSTALL)/include + INSTALL_DOC ?= $(INSTALL)/share/doc/suitesparse-$(SUITESPARSE_VERSION) + + #--------------------------------------------------------------------------- + # optimization level + #--------------------------------------------------------------------------- + + OPTIMIZATION ?= -O3 + + #--------------------------------------------------------------------------- + # statement coverage for */Tcov + #--------------------------------------------------------------------------- + + ifeq ($(TCOV),yes) + # Each package has a */Tcov directory for extensive testing, including + # statement coverage. The Tcov tests require Linux and gcc, and use + # the vanilla BLAS. For those tests, the packages use 'make TCOV=yes', + # which overrides the following settings: + MKLROOT = + AUTOCC = no + CC = gcc + CXX = g++ + BLAS = -lrefblas -lgfortran -lstdc++ + LAPACK = -llapack + CFLAGS += --coverage + OPTIMIZATION = -g + LDFLAGS += --coverage + endif + + #--------------------------------------------------------------------------- + # CFLAGS for the C/C++ compiler + #--------------------------------------------------------------------------- + + # The CF macro is used by SuiteSparse Makefiles as a combination of + # CFLAGS, CPPFLAGS, TARGET_ARCH, and system-dependent settings. + CF ?= $(CFLAGS) $(CPPFLAGS) $(TARGET_ARCH) $(OPTIMIZATION) -fexceptions -fPIC + + #--------------------------------------------------------------------------- + # OpenMP is used in CHOLMOD + #--------------------------------------------------------------------------- + + # with gcc, enable OpenMP directives via -fopenmp + # This is not supported on Darwin, so this string is cleared, below. + CFOPENMP ?= -fopenmp + + #--------------------------------------------------------------------------- + # compiler + #--------------------------------------------------------------------------- + + # By default, look for the Intel compilers. If present, they are used + # instead of $(CC), $(CXX), and $(F77). To disable this feature and + # use the $(CC), $(CXX), and $(F77) compilers, use 'make AUTOCC=no' + + AUTOCC ?= yes + + ifneq ($(AUTOCC),no) + ifneq ($(shell which icc 2>/dev/null),) + # use the Intel icc compiler for C codes, and -qopenmp for OpenMP + CC = icc -D_GNU_SOURCE + CXX = $(CC) + CFOPENMP = -qopenmp -I$(MKLROOT)/include + endif + ifneq ($(shell which ifort 2>/dev/null),) + # use the Intel ifort compiler for Fortran codes + F77 = ifort + endif + endif + + #--------------------------------------------------------------------------- + # code formatting (for Tcov only) + #--------------------------------------------------------------------------- + + PRETTY ?= grep -v "^\#" | indent -bl -nce -bli0 -i4 -sob -l120 + + #--------------------------------------------------------------------------- + # required libraries + #--------------------------------------------------------------------------- + + # SuiteSparse requires the BLAS, LAPACK, and -lm (Math) libraries. + # It places its shared *.so libraries in SuiteSparse/lib. + # Linux also requires the -lrt library (see below) + LDLIBS ?= -lm + LDFLAGS += -L$(INSTALL_LIB) + + # See http://www.openblas.net for a recent and freely available optimzed + # BLAS. LAPACK is at http://www.netlib.org/lapack/ . You can use the + # standard Fortran LAPACK along with OpenBLAS to obtain very good + # performance. This script can also detect if the Intel MKL BLAS is + # installed. + + LAPACK ?= -llapack + + ifndef BLAS + ifdef MKLROOT + # use the Intel MKL for BLAS and LAPACK + # using static linking: + # BLAS = -Wl,--start-group \ + # $(MKLROOT)/lib/intel64/libmkl_intel_lp64.a \ + # $(MKLROOT)/lib/intel64/libmkl_core.a \ + # $(MKLROOT)/lib/intel64/libmkl_intel_thread.a \ + # -Wl,--end-group -lpthread -lm + # using dynamic linking: + BLAS = -lmkl_intel_lp64 -lmkl_core -lmkl_intel_thread -lpthread -lm + LAPACK = + else + # use the OpenBLAS at http://www.openblas.net + BLAS = -lopenblas + endif + endif + + # For ACML, use this instead: + # make BLAS='-lacml -lgfortran' + + #--------------------------------------------------------------------------- + # shell commands + #--------------------------------------------------------------------------- + + # ranlib, and ar, for generating libraries. If you don't need ranlib, + # just change it to RANLAB = echo + RANLIB ?= ranlib + ARCHIVE ?= $(AR) $(ARFLAGS) + CP ?= cp -f + MV ?= mv -f + + #--------------------------------------------------------------------------- + # Fortran compiler (not required for 'make' or 'make library') + #--------------------------------------------------------------------------- + + # A Fortran compiler is optional. Only required for the optional Fortran + # interfaces to AMD and UMFPACK. Not needed by 'make' or 'make install' + F77 ?= gfortran + F77FLAGS ?= $(FFLAGS) $(OPTIMIZATION) + + #--------------------------------------------------------------------------- + # NVIDIA CUDA configuration for CHOLMOD and SPQR + #--------------------------------------------------------------------------- + + # CUDA is detected automatically, and used if found. To disable CUDA, + # use CUDA=no + + ifneq ($(CUDA),no) + CUDA_PATH = $(shell which nvcc 2>/dev/null | sed "s/\/bin\/nvcc//") + endif + + ifeq ($(wildcard $(CUDA_PATH)),) + # CUDA is not present + CUDA_PATH = + GPU_BLAS_PATH = + GPU_CONFIG = + CUDART_LIB = + CUBLAS_LIB = + CUDA_INC_PATH = + CUDA_INC = + NVCC = echo + NVCCFLAGS = + else + # with CUDA for CHOLMOD and SPQR + GPU_BLAS_PATH = $(CUDA_PATH) + # GPU_CONFIG must include -DGPU_BLAS to compile SuiteSparse for the + # GPU. You can add additional GPU-related flags to it as well. + # with 4 cores (default): + GPU_CONFIG = -DGPU_BLAS + # For example, to compile CHOLMOD for 10 CPU cores when using the GPU: + # GPU_CONFIG = -DGPU_BLAS -DCHOLMOD_OMP_NUM_THREADS=10 + CUDART_LIB = $(CUDA_PATH)/lib64/libcudart.so + CUBLAS_LIB = $(CUDA_PATH)/lib64/libcublas.so + CUDA_INC_PATH = $(CUDA_PATH)/include/ + CUDA_INC = -I$(CUDA_INC_PATH) + NVCC = $(CUDA_PATH)/bin/nvcc + NVCCFLAGS = -Xcompiler -fPIC -O3 \ + -gencode=arch=compute_20,code=sm_20 \ + -gencode=arch=compute_30,code=sm_30 \ + -gencode=arch=compute_35,code=sm_35 \ + -gencode=arch=compute_50,code=sm_50 \ + -gencode=arch=compute_50,code=compute_50 + endif + + #--------------------------------------------------------------------------- + # UMFPACK configuration: + #--------------------------------------------------------------------------- + + # Configuration for UMFPACK. See UMFPACK/Source/umf_config.h for details. + # + # -DNBLAS do not use the BLAS. UMFPACK will be very slow. + # -D'LONGBLAS=long' or -DLONGBLAS='long long' defines the integers used by + # LAPACK and the BLAS (defaults to 'int') + # -DNSUNPERF do not use the Sun Perf. Library on Solaris + # -DNRECIPROCAL do not multiply by the reciprocal + # -DNO_DIVIDE_BY_ZERO do not divide by zero + # -DNCHOLMOD do not use CHOLMOD as a ordering method. If -DNCHOLMOD is + # included in UMFPACK_CONFIG, then UMFPACK does not rely on + # CHOLMOD, CAMD, CCOLAMD, COLAMD, and METIS. + + UMFPACK_CONFIG ?= + + # For example, uncomment this line to compile UMFPACK without CHOLMOD: + # UMFPACK_CONFIG = -DNCHOLMOD + # or use 'make UMFPACK_CONFIG=-DNCHOLMOD' + + #--------------------------------------------------------------------------- + # CHOLMOD configuration + #--------------------------------------------------------------------------- + + # CHOLMOD Library Modules, which appear in -lcholmod + # Core requires: none + # Check requires: Core + # Cholesky requires: Core, AMD, COLAMD. optional: Partition, Supernodal + # MatrixOps requires: Core + # Modify requires: Core + # Partition requires: Core, CCOLAMD, METIS. optional: Cholesky + # Supernodal requires: Core, BLAS, LAPACK + # + # CHOLMOD test/demo Modules (these do not appear in -lcholmod): + # Tcov requires: Core, Check, Cholesky, MatrixOps, Modify, Supernodal + # optional: Partition + # Valgrind same as Tcov + # Demo requires: Core, Check, Cholesky, MatrixOps, Supernodal + # optional: Partition + # + # Configuration flags: + # -DNCHECK do not include the Check module. + # -DNCHOLESKY do not include the Cholesky module. + # -DNPARTITION do not include the Partition module. + # also do not include METIS. + # -DNCAMD do not use CAMD & CCOLAMD in Parition Module. + # -DNMATRIXOPS do not include the MatrixOps module. + # -DNMODIFY do not include the Modify module. + # -DNSUPERNODAL do not include the Supernodal module. + # + # -DNPRINT do not print anything. + # -D'LONGBLAS=long' or -DLONGBLAS='long long' defines the integers used by + # LAPACK and the BLAS (defaults to 'int') + # -DNSUNPERF for Solaris only. If defined, do not use the Sun + # Performance Library + # -DGPU_BLAS enable the use of the CUDA BLAS + + CHOLMOD_CONFIG ?= $(GPU_CONFIG) + + #--------------------------------------------------------------------------- + # SuiteSparseQR configuration: + #--------------------------------------------------------------------------- + + # The SuiteSparseQR library can be compiled with the following options: + # + # -DNPARTITION do not include the CHOLMOD partition module + # -DNEXPERT do not include the functions in SuiteSparseQR_expert.cpp + # -DHAVE_TBB enable the use of Intel's Threading Building Blocks + # -DGPU_BLAS enable the use of the CUDA BLAS + + SPQR_CONFIG ?= $(GPU_CONFIG) + + # to compile with Intel's TBB, use TBB=-ltbb SPQR_CONFIG=-DHAVE_TBB + TBB ?= + + # TODO: this *mk file should auto-detect the presence of Intel's TBB, + # and set the compiler flags accordingly. + +#=============================================================================== +# System-dependent configurations +#=============================================================================== + + #--------------------------------------------------------------------------- + # determine what system we are on + #--------------------------------------------------------------------------- + + # To disable these auto configurations, use 'make UNAME=custom' + + ifndef UNAME + ifeq ($(OS),Windows_NT) + # Cygwin Make on Windows has an $(OS) variable, but not uname. + # Note that this option is untested. + UNAME = Windows + else + # Linux and Darwin (Mac OSX) have been tested. + UNAME := $(shell uname) + endif + endif + + #--------------------------------------------------------------------------- + # Linux + #--------------------------------------------------------------------------- + + ifeq ($(UNAME),Linux) + # add the realtime library, librt, and SuiteSparse/lib + LDLIBS += -lrt -Wl,-rpath=$(INSTALL_LIB) + endif + + #--------------------------------------------------------------------------- + # Mac + #--------------------------------------------------------------------------- + + ifeq ($(UNAME), Darwin) + # To compile on the Mac, you must install Xcode. Then do this at the + # command line in the Terminal, before doing 'make': + # xcode-select --install + CF += -fno-common + BLAS = -framework Accelerate + LAPACK = -framework Accelerate + # OpenMP is not yet supported by default in clang + CFOPENMP = + endif + + #--------------------------------------------------------------------------- + # Solaris + #--------------------------------------------------------------------------- + + ifeq ($(UNAME), SunOS) + # Using the Sun compiler and the Sun Performance Library + # This hasn't been tested recently. + # I leave it here in case you need it. It likely needs updating. + CF += -fast -KPIC -xc99=%none -xlibmieee -xlibmil -m64 -Xc + F77FLAGS = -O -fast -KPIC -dalign -xlibmil -m64 + BLAS = -xlic_lib=sunperf + LAPACK = + # Using the GCC compiler and the reference BLAS + ## CC = gcc + ## CXX = g++ + ## MAKE = gmake + ## BLAS = -lrefblas -lgfortran + ## LAPACK = -llapack + endif + + #--------------------------------------------------------------------------- + # IBM AIX + #--------------------------------------------------------------------------- + + ifeq ($(UNAME), AIX) + # hasn't been tested for a very long time... + # I leave it here in case you need it. It likely needs updating. + CF += -O4 -qipa -qmaxmem=16384 -q64 -qproto -DBLAS_NO_UNDERSCORE + F77FLAGS = -O4 -qipa -qmaxmem=16384 -q64 + BLAS = -lessl + LAPACK = + endif + +#=============================================================================== +# finalize the CF compiler flags +#=============================================================================== + + CF += $(CFOPENMP) + +#=============================================================================== +# internal configuration +#=============================================================================== + + # The user should not have to change these definitions, and they are + # not displayed by 'make config' + + #--------------------------------------------------------------------------- + # for removing files not in the distribution + #--------------------------------------------------------------------------- + + # remove object files, but keep compiled libraries via 'make clean' + CLEAN = *.o *.obj *.ln *.bb *.bbg *.da *.tcov *.gcov gmon.out *.bak *.d \ + *.gcda *.gcno *.aux *.bbl *.blg *.log *.toc *.dvi *.lof *.lot + + # also remove compiled libraries, via 'make distclean' + PURGE = *.so* *.a *.dll *.dylib *.dSYM + + # location of TCOV test output + TCOV_TMP ?= /tmp + +#=============================================================================== +# Building the shared and static libraries +#=============================================================================== + +# How to build/install shared and static libraries for Mac and Linux/Unix. +# This assumes that LIBRARY and VERSION have already been defined by the +# Makefile that includes this file. + +SO_OPTS = $(LDFLAGS) + +ifeq ($(UNAME),Windows) + # Cygwin Make on Windows (untested) + AR_TARGET = $(LIBRARY).lib + SO_PLAIN = $(LIBRARY).dll + SO_MAIN = $(LIBRARY).$(SO_VERSION).dll + SO_TARGET = $(LIBRARY).$(VERSION).dll + SO_INSTALL_NAME = echo +else + # Mac or Linux/Unix + AR_TARGET = $(LIBRARY).a + ifeq ($(UNAME),Darwin) + # Mac + SO_PLAIN = $(LIBRARY).dylib + SO_MAIN = $(LIBRARY).$(SO_VERSION).dylib + SO_TARGET = $(LIBRARY).$(VERSION).dylib + SO_OPTS += -dynamiclib -compatibility_version $(SO_VERSION) \ + -current_version $(VERSION) \ + -shared -undefined dynamic_lookup + # When a Mac *.dylib file is moved, this command is required + # to change its internal name to match its location in the filesystem: + SO_INSTALL_NAME = install_name_tool -id + else + # Linux and other variants of Unix + SO_PLAIN = $(LIBRARY).so + SO_MAIN = $(LIBRARY).so.$(SO_VERSION) + SO_TARGET = $(LIBRARY).so.$(VERSION) + SO_OPTS += -shared -Wl,-soname -Wl,$(SO_MAIN) -Wl,--no-undefined + # Linux/Unix *.so files can be moved without modification: + SO_INSTALL_NAME = echo + endif +endif + +#=============================================================================== +# Configure CHOLMOD/Partition module with METIS, CAMD, and CCOLAMD +#=============================================================================== + +# By default, SuiteSparse uses METIS 5.1.0 in the SuiteSparse/metis-5.1.0 +# directory. SuiteSparse's interface to METIS is only through the +# SuiteSparse/CHOLMOD/Partition module, which also requires SuiteSparse/CAMD +# and SuiteSparse/CCOLAMD. +# +# If you wish to use your own pre-installed copy of METIS, use the MY_METIS_LIB +# and MY_METIS_INC options passed to 'make'. For example: +# make MY_METIS_LIB=-lmetis +# make MY_METIS_LIB=/home/myself/mylibraries/libmetis.so +# make MY_METIS_LIB='-L/home/myself/mylibraries -lmetis' +# If you need to tell the compiler where to find the metis.h include file, +# then add MY_METIS_INC=/home/myself/metis-5.1.0/include as well, which points +# to the directory containing metis.h. If metis.h is already installed in +# a location known to the compiler (/usr/local/include/metis.h for example) +# then you do not need to add MY_METIS_INC. + +I_WITH_PARTITION = +LIB_WITH_PARTITION = +CONFIG_PARTITION = -DNPARTITION -DNCAMD +# check if CAMD/CCOLAMD and METIS are requested and available +ifeq (,$(findstring -DNCAMD, $(CHOLMOD_CONFIG))) + # CAMD and CCOLAMD are requested. See if they are available in + # SuiteSparse/CAMD and SuiteSparse/CCOLAMD + ifneq (, $(wildcard $(SUITESPARSE)/CAMD)) + ifneq (, $(wildcard $(SUITESPARSE)/CCOLAMD)) + # CAMD and CCOLAMD are requested and available + LIB_WITH_PARTITION = -lccolamd -lcamd + I_WITH_PARTITION = -I$(SUITESPARSE)/CCOLAMD/Include -I$(SUITESPARSE)/CAMD/Include + CONFIG_PARTITION = -DNPARTITION + # check if METIS is requested and available + ifeq (,$(findstring -DNPARTITION, $(CHOLMOD_CONFIG))) + # METIS is requested. See if it is available. + ifneq (,$(MY_METIS_LIB)) + # METIS 5.1.0 is provided elsewhere, and we are not using + # SuiteSparse/metis-5.1.0. To do so, we link with + # $(MY_METIS_LIB) and add the -I$(MY_METIS_INC) option for + # the compiler. The latter can be empty if you have METIS + # installed in a place where the compiler can find the + # metis.h include file by itself without any -I option + # (/usr/local/include/metis.h for example). + LIB_WITH_PARTITION += $(MY_METIS_LIB) + ifneq (,$(MY_METIS_INC)) + I_WITH_PARTITION += -I$(MY_METIS_INC) + endif + CONFIG_PARTITION = + else + # see if METIS is in SuiteSparse/metis-5.1.0 + ifneq (, $(wildcard $(SUITESPARSE)/metis-5.1.0)) + # SuiteSparse/metis5.1.0 is available + ifeq ($(UNAME), Darwin) + LIB_WITH_PARTITION += $(SUITESPARSE)/lib/libmetis.dylib + else + LIB_WITH_PARTITION += -lmetis + endif + I_WITH_PARTITION += -I$(SUITESPARSE)/metis-5.1.0/include + CONFIG_PARTITION = + endif + endif + endif + endif + endif +endif + +#=============================================================================== +# display configuration +#=============================================================================== + +ifeq ($(LIBRARY),) + # placeholders, for 'make config' in the top-level SuiteSparse + LIBRARY=PackageNameWillGoHere + VERSION=x.y.z + SO_VERSION=x +endif + +# 'make config' lists the primary installation options +config: + @echo ' ' + @echo '----------------------------------------------------------------' + @echo 'SuiteSparse package compilation options:' + @echo '----------------------------------------------------------------' + @echo ' ' + @echo 'SuiteSparse Version: ' '$(SUITESPARSE_VERSION)' + @echo 'SuiteSparse top folder: ' '$(SUITESPARSE)' + @echo 'Package: LIBRARY= ' '$(LIBRARY)' + @echo 'Version: VERSION= ' '$(VERSION)' + @echo 'SO version: SO_VERSION= ' '$(SO_VERSION)' + @echo 'System: UNAME= ' '$(UNAME)' + @echo 'Install directory: INSTALL= ' '$(INSTALL)' + @echo 'Install libraries in: INSTALL_LIB= ' '$(INSTALL_LIB)' + @echo 'Install include files in: INSTALL_INCLUDE=' '$(INSTALL_INCLUDE)' + @echo 'Install documentation in: INSTALL_DOC= ' '$(INSTALL_DOC)' + @echo 'Optimization level: OPTIMIZATION= ' '$(OPTIMIZATION)' + @echo 'BLAS library: BLAS= ' '$(BLAS)' + @echo 'LAPACK library: LAPACK= ' '$(LAPACK)' + @echo 'Intel TBB library: TBB= ' '$(TBB)' + @echo 'Other libraries: LDLIBS= ' '$(LDLIBS)' + @echo 'static library: AR_TARGET= ' '$(AR_TARGET)' + @echo 'shared library (full): SO_TARGET= ' '$(SO_TARGET)' + @echo 'shared library (main): SO_MAIN= ' '$(SO_MAIN)' + @echo 'shared library (short): SO_PLAIN= ' '$(SO_PLAIN)' + @echo 'shared library options: SO_OPTS= ' '$(SO_OPTS)' + @echo 'shared library name tool: SO_INSTALL_NAME=' '$(SO_INSTALL_NAME)' + @echo 'ranlib, for static libs: RANLIB= ' '$(RANLIB)' + @echo 'static library command: ARCHIVE= ' '$(ARCHIVE)' + @echo 'copy file: CP= ' '$(CP)' + @echo 'move file: MV= ' '$(MV)' + @echo 'remove file: RM= ' '$(RM)' + @echo 'pretty (for Tcov tests): PRETTY= ' '$(PRETTY)' + @echo 'C compiler: CC= ' '$(CC)' + @echo 'C++ compiler: CXX= ' '$(CXX)' + @echo 'CUDA compiler: NVCC= ' '$(NVCC)' + @echo 'CUDA root directory: CUDA_PATH= ' '$(CUDA_PATH)' + @echo 'OpenMP flags: CFOPENMP= ' '$(CFOPENMP)' + @echo 'C/C++ compiler flags: CF= ' '$(CF)' + @echo 'LD flags: LDFLAGS= ' '$(LDFLAGS)' + @echo 'Fortran compiler: F77= ' '$(F77)' + @echo 'Fortran flags: F77FLAGS= ' '$(F77FLAGS)' + @echo 'Intel MKL root: MKLROOT= ' '$(MKLROOT)' + @echo 'Auto detect Intel icc: AUTOCC= ' '$(AUTOCC)' + @echo 'UMFPACK config: UMFPACK_CONFIG= ' '$(UMFPACK_CONFIG)' + @echo 'CHOLMOD config: CHOLMOD_CONFIG= ' '$(CHOLMOD_CONFIG)' + @echo 'SuiteSparseQR config: SPQR_CONFIG= ' '$(SPQR_CONFIG)' + @echo 'CUDA library: CUDART_LIB= ' '$(CUDART_LIB)' + @echo 'CUBLAS library: CUBLAS_LIB= ' '$(CUBLAS_LIB)' + @echo 'METIS and CHOLMOD/Partition configuration:' + @echo 'Your METIS library: MY_METIS_LIB= ' '$(MY_METIS_LIB)' + @echo 'Your metis.h is in: MY_METIS_INC= ' '$(MY_METIS_INC)' + @echo 'METIS is used via the CHOLMOD/Partition module, configured as follows.' + @echo 'If the next line has -DNPARTITION then METIS will not be used:' + @echo 'CHOLMOD Partition config: ' '$(CONFIG_PARTITION)' + @echo 'CHOLMOD Partition libs: ' '$(LIB_WITH_PARTITION)' + @echo 'CHOLMOD Partition include:' '$(I_WITH_PARTITION)' +ifeq ($(TCOV),yes) + @echo 'TCOV=yes, for extensive testing only (gcc, g++, vanilla BLAS)' +endif + diff --git a/gtsam/3rdparty/SuiteSparse_config/xerbla/Makefile b/gtsam/3rdparty/SuiteSparse_config/xerbla/Makefile new file mode 100644 index 000000000..420c50e88 --- /dev/null +++ b/gtsam/3rdparty/SuiteSparse_config/xerbla/Makefile @@ -0,0 +1,73 @@ +# Makefile for null-output xerbla, both C and Fortran versions. +# By default, the C version (libcerbla.a and *.so) is compiled and installed. +# Set the USE_FORTRAN option to 1 to create the Fortran instead (libxerbla): + USE_FORTRAN = 0 +# USE_FORTRAN = 1 + +VERSION = 1.0.2 +SO_VERSION = 1 + +default: library + +# compile and install in SuiteSparse/lib +library: + $(MAKE) install INSTALL=$(SUITESPARSE) + +all: library + +ifeq ($(USE_FORTRAN),0) + LIBRARY = libcerbla +else + LIBRARY = libxerbla +endif + +include ../SuiteSparse_config.mk + +ifeq ($(USE_FORTRAN),0) + COMPILE = $(CC) $(CF) -c xerbla.c + DEPENDS = xerbla.c xerbla.h +else + COMPILE = $(F77) $(F77FLAGS) -c xerbla.f + DEPENDS = xerbla.f +endif + +ccode: all + +fortran: all + +$(AR_TARGET): $(DEPENDS) + $(COMPILE) + $(ARCHIVE) $(AR_TARGET) xerbla.o + - $(RANLIB) $(AR_TARGET) + - $(RM) xerbla.o + +# install libcerbla / libxerbla +install: $(AR_TARGET) $(INSTALL_LIB)/$(SO_TARGET) + +$(INSTALL_LIB)/$(SO_TARGET): $(DEPENDS) + @mkdir -p $(INSTALL_LIB) + @mkdir -p $(INSTALL_INCLUDE) + @mkdir -p $(INSTALL_DOC) + $(COMPILE) + $(CC) $(SO_OPTS) xerbla.o -o $@ + - $(RM) xerbla.o + ( cd $(INSTALL_LIB) ; ln -sf $(SO_TARGET) $(SO_PLAIN) ) + ( cd $(INSTALL_LIB) ; ln -sf $(SO_TARGET) $(SO_MAIN) ) + $(CP) xerbla.h $(INSTALL_INCLUDE) + chmod 755 $(INSTALL_LIB)/$(SO_TARGET) + chmod 644 $(INSTALL_INCLUDE)/xerbla.h + +# uninstall libcerbla / libxerbla +uninstall: + $(RM) $(INSTALL_LIB)/$(SO_TARGET) + $(RM) $(INSTALL_LIB)/$(SO_PLAIN) + $(RM) $(INSTALL_INCLUDE)/xerbla.h + +distclean: purge + +purge: clean + - $(RM) -r $(PURGE) + +clean: + - $(RM) -r $(CLEAN) + diff --git a/gtsam/3rdparty/UFconfig/xerbla/xerbla.c b/gtsam/3rdparty/SuiteSparse_config/xerbla/xerbla.c similarity index 100% rename from gtsam/3rdparty/UFconfig/xerbla/xerbla.c rename to gtsam/3rdparty/SuiteSparse_config/xerbla/xerbla.c diff --git a/gtsam/3rdparty/UFconfig/xerbla/xerbla.f b/gtsam/3rdparty/SuiteSparse_config/xerbla/xerbla.f similarity index 100% rename from gtsam/3rdparty/UFconfig/xerbla/xerbla.f rename to gtsam/3rdparty/SuiteSparse_config/xerbla/xerbla.f diff --git a/gtsam/3rdparty/UFconfig/xerbla/xerbla.h b/gtsam/3rdparty/SuiteSparse_config/xerbla/xerbla.h similarity index 100% rename from gtsam/3rdparty/UFconfig/xerbla/xerbla.h rename to gtsam/3rdparty/SuiteSparse_config/xerbla/xerbla.h diff --git a/gtsam/3rdparty/UFconfig/README.txt b/gtsam/3rdparty/UFconfig/README.txt deleted file mode 100644 index d9edc239a..000000000 --- a/gtsam/3rdparty/UFconfig/README.txt +++ /dev/null @@ -1,35 +0,0 @@ -UFconfig contains configuration settings for all many of the software packages -that I develop or co-author. Note that older versions of some of these packages -do not require UFconfig. - - Package Description - ------- ----------- - AMD approximate minimum degree ordering - CAMD constrained AMD - COLAMD column approximate minimum degree ordering - CCOLAMD constrained approximate minimum degree ordering - UMFPACK sparse LU factorization, with the BLAS - CXSparse int/long/real/complex version of CSparse - CHOLMOD sparse Cholesky factorization, update/downdate - KLU sparse LU factorization, BLAS-free - BTF permutation to block triangular form - LDL concise sparse LDL' - LPDASA LP Dual Active Set Algorithm - SuiteSparseQR sparse QR factorization - -UFconfig is not required by: - - CSparse a Concise Sparse matrix package - RBio read/write files in Rutherford/Boeing format - UFcollection tools for managing the UF Sparse Matrix Collection - LINFACTOR simple m-file to show how to use LU and CHOL to solve Ax=b - MESHND 2D and 3D mesh generation and nested dissection ordering - MATLAB_Tools misc collection of m-files - SSMULT sparse matrix times sparse matrix, for use in MATLAB - -In addition, the xerbla/ directory contains Fortan and C versions of the -BLAS/LAPACK xerbla routine, which is called when an invalid input is passed to -the BLAS or LAPACK. The xerbla provided here does not print any message, so -the entire Fortran I/O library does not need to be linked into a C application. -Most versions of the BLAS contain xerbla, but those from K. Goto do not. Use -this if you need too. diff --git a/gtsam/3rdparty/UFconfig/UFconfig.c b/gtsam/3rdparty/UFconfig/UFconfig.c deleted file mode 100644 index 8b8d45ef7..000000000 --- a/gtsam/3rdparty/UFconfig/UFconfig.c +++ /dev/null @@ -1,71 +0,0 @@ -/* ========================================================================== */ -/* === UFconfig ============================================================= */ -/* ========================================================================== */ - -/* Copyright (c) 2009, University of Florida. No licensing restrictions - * apply to this file or to the UFconfig directory. Author: Timothy A. Davis. - */ - -#include "UFconfig.h" - -/* -------------------------------------------------------------------------- */ -/* UFmalloc: malloc wrapper */ -/* -------------------------------------------------------------------------- */ - -void *UFmalloc /* pointer to allocated block of memory */ -( - size_t nitems, /* number of items to malloc (>=1 is enforced) */ - size_t size_of_item, /* sizeof each item */ - int *ok, /* TRUE if successful, FALSE otherwise */ - UFconfig *config /* SuiteSparse-wide configuration */ -) -{ - void *p ; - if (nitems < 1) nitems = 1 ; - if (nitems * size_of_item != ((double) nitems) * size_of_item) - { - /* Int overflow */ - *ok = 0 ; - return (NULL) ; - } - if (!config || config->malloc_memory == NULL) - { - /* use malloc by default */ - p = (void *) malloc (nitems * size_of_item) ; - } - else - { - /* use the pointer to malloc in the config */ - p = (void *) (config->malloc_memory) (nitems * size_of_item) ; - } - *ok = (p != NULL) ; - return (p) ; -} - - -/* -------------------------------------------------------------------------- */ -/* UFfree: free wrapper */ -/* -------------------------------------------------------------------------- */ - -void *UFfree /* always returns NULL */ -( - void *p, /* block to free */ - UFconfig *config /* SuiteSparse-wide configuration */ -) -{ - if (p) - { - if (!config || config->free_memory == NULL) - { - /* use free by default */ - free (p) ; - } - else - { - /* use the pointer to free in the config */ - (config->free_memory) (p) ; - } - } - return (NULL) ; -} - diff --git a/gtsam/3rdparty/UFconfig/UFconfig.h b/gtsam/3rdparty/UFconfig/UFconfig.h deleted file mode 100644 index 795f5668c..000000000 --- a/gtsam/3rdparty/UFconfig/UFconfig.h +++ /dev/null @@ -1,152 +0,0 @@ -/* ========================================================================== */ -/* === UFconfig.h =========================================================== */ -/* ========================================================================== */ - -/* Configuration file for SuiteSparse: a Suite of Sparse matrix packages - * (AMD, COLAMD, CCOLAMD, CAMD, CHOLMOD, UMFPACK, CXSparse, and others). - * - * UFconfig.h provides the definition of the long integer. On most systems, - * a C program can be compiled in LP64 mode, in which long's and pointers are - * both 64-bits, and int's are 32-bits. Windows 64, however, uses the LLP64 - * model, in which int's and long's are 32-bits, and long long's and pointers - * are 64-bits. - * - * SuiteSparse packages that include long integer versions are - * intended for the LP64 mode. However, as a workaround for Windows 64 - * (and perhaps other systems), the long integer can be redefined. - * - * If _WIN64 is defined, then the __int64 type is used instead of long. - * - * The long integer can also be defined at compile time. For example, this - * could be added to UFconfig.mk: - * - * CFLAGS = -O -D'UF_long=long long' -D'UF_long_max=9223372036854775801' \ - * -D'UF_long_idd="lld"' - * - * This file defines UF_long as either long (on all but _WIN64) or - * __int64 on Windows 64. The intent is that a UF_long is always a 64-bit - * integer in a 64-bit code. ptrdiff_t might be a better choice than long; - * it is always the same size as a pointer. - * - * This file also defines the SUITESPARSE_VERSION and related definitions. - * - * Copyright (c) 2007, University of Florida. No licensing restrictions - * apply to this file or to the UFconfig directory. Author: Timothy A. Davis. - */ - -#ifndef _UFCONFIG_H -#define _UFCONFIG_H - -#ifdef __cplusplus -extern "C" { -#endif - -#include -#include - -/* ========================================================================== */ -/* === UF_long ============================================================== */ -/* ========================================================================== */ - -#ifndef UF_long - -#ifdef _WIN64 - -#define UF_long __int64 -#define UF_long_max _I64_MAX -#define UF_long_idd "I64d" - -#else - -#define UF_long long -#define UF_long_max LONG_MAX -#define UF_long_idd "ld" - -#endif -#define UF_long_id "%" UF_long_idd -#endif - -/* ========================================================================== */ -/* === UFconfig parameters and functions ==================================== */ -/* ========================================================================== */ - -/* SuiteSparse-wide parameters will be placed in this struct. So far, they - are only used by RBio. */ - -typedef struct UFconfig_struct -{ - void *(*malloc_memory) (size_t) ; /* pointer to malloc */ - void *(*realloc_memory) (void *, size_t) ; /* pointer to realloc */ - void (*free_memory) (void *) ; /* pointer to free */ - void *(*calloc_memory) (size_t, size_t) ; /* pointer to calloc */ - -} UFconfig ; - -void *UFmalloc /* pointer to allocated block of memory */ -( - size_t nitems, /* number of items to malloc (>=1 is enforced) */ - size_t size_of_item, /* sizeof each item */ - int *ok, /* TRUE if successful, FALSE otherwise */ - UFconfig *config /* SuiteSparse-wide configuration */ -) ; - -void *UFfree /* always returns NULL */ -( - void *p, /* block to free */ - UFconfig *config /* SuiteSparse-wide configuration */ -) ; - - -/* ========================================================================== */ -/* === SuiteSparse version ================================================== */ -/* ========================================================================== */ - -/* SuiteSparse is not a package itself, but a collection of packages, some of - * which must be used together (UMFPACK requires AMD, CHOLMOD requires AMD, - * COLAMD, CAMD, and CCOLAMD, etc). A version number is provided here for the - * collection itself. The versions of packages within each version of - * SuiteSparse are meant to work together. Combining one packge from one - * version of SuiteSparse, with another package from another version of - * SuiteSparse, may or may not work. - * - * SuiteSparse Version 3.6.1 contains the following packages: - * - * AMD version 2.2.2 - * BTF version 1.1.2 - * CAMD version 2.2.2 - * CCOLAMD version 2.7.3 - * CHOLMOD version 1.7.3 - * COLAMD version 2.7.3 - * CSparse version 2.2.5 - * CSparse3 version 3.0.1 - * CXSparse version 2.2.5 - * KLU version 1.1.2 - * LDL version 2.0.3 - * RBio version 2.0.1 - * SPQR version 1.2.2 (also called SuiteSparseQR) - * UFcollection version 1.5.0 - * UFconfig version number is the same as SuiteSparse - * UMFPACK version 5.5.1 - * LINFACTOR version 1.1.0 - * MESHND version 1.1.1 - * SSMULT version 2.0.2 - * MATLAB_Tools no specific version number - * - * Other package dependencies: - * BLAS required by CHOLMOD and UMFPACK - * LAPACK required by CHOLMOD - * METIS 4.0.1 required by CHOLMOD (optional) and KLU (optional) - */ - -#define SUITESPARSE_DATE "May 10, 2011" -#define SUITESPARSE_VER_CODE(main,sub) ((main) * 1000 + (sub)) -#define SUITESPARSE_MAIN_VERSION 3 -#define SUITESPARSE_SUB_VERSION 6 -#define SUITESPARSE_SUBSUB_VERSION 1 -#define SUITESPARSE_VERSION \ - SUITESPARSE_VER_CODE(SUITESPARSE_MAIN_VERSION,SUITESPARSE_SUB_VERSION) - -#ifdef __cplusplus -} -#endif -#endif diff --git a/gtsam/3rdparty/UFconfig/UFconfig.mk b/gtsam/3rdparty/UFconfig/UFconfig.mk deleted file mode 100644 index 60c951b6a..000000000 --- a/gtsam/3rdparty/UFconfig/UFconfig.mk +++ /dev/null @@ -1,386 +0,0 @@ -#=============================================================================== -# UFconfig.mk: common configuration file for the SuiteSparse -#=============================================================================== - -# This file contains all configuration settings for all packages authored or -# co-authored by Tim Davis at the University of Florida: -# -# Package Version Description -# ------- ------- ----------- -# AMD 1.2 or later approximate minimum degree ordering -# COLAMD 2.4 or later column approximate minimum degree ordering -# CCOLAMD 1.0 or later constrained column approximate minimum degree ordering -# CAMD any constrained approximate minimum degree ordering -# UMFPACK 4.5 or later sparse LU factorization, with the BLAS -# CHOLMOD any sparse Cholesky factorization, update/downdate -# KLU 0.8 or later sparse LU factorization, BLAS-free -# BTF 0.8 or later permutation to block triangular form -# LDL 1.2 or later concise sparse LDL' -# LPDASA any linear program solve (dual active set algorithm) -# CXSparse any extended version of CSparse (int/long, real/complex) -# SuiteSparseQR any sparse QR factorization -# -# The UFconfig directory and the above packages should all appear in a single -# directory, in order for the Makefile's within each package to find this file. -# -# To enable an option of the form "# OPTION = ...", edit this file and -# delete the "#" in the first column of the option you wish to use. - -#------------------------------------------------------------------------------ -# Generic configuration -#------------------------------------------------------------------------------ - -# C compiler and compiler flags: These will normally not give you optimal -# performance. You should select the optimization parameters that are best -# for your system. On Linux, use "CFLAGS = -O3 -fexceptions" for example. -CC = cc -CFLAGS = -O3 -fexceptions - -# C++ compiler (also uses CFLAGS) -CPLUSPLUS = g++ - -# ranlib, and ar, for generating libraries -RANLIB = ranlib -AR = ar cr - -# copy, delete, and rename a file -CP = cp -f -RM = rm -f -MV = mv -f - -# Fortran compiler (not normally required) -F77 = f77 -F77FLAGS = -O -F77LIB = - -# C and Fortran libraries -LIB = -lm - -# For compiling MATLAB mexFunctions (MATLAB 7.5 or later) -MEX = mex -O -largeArrayDims -lmwlapack -lmwblas - -# For compiling MATLAB mexFunctions (MATLAB 7.3 and 7.4) -# MEX = mex -O -largeArrayDims -lmwlapack - -# For MATLAB 7.2 or earlier, you must use one of these options: -# MEX = mex -O -lmwlapack -# MEX = mex -O - -# Which version of MAKE you are using (default is "make") -# MAKE = make -# MAKE = gmake - -# For "make install" -INSTALL_LIB = /usr/local/lib -INSTALL_INCLUDE = /usr/local/include - -#------------------------------------------------------------------------------ -# BLAS and LAPACK configuration: -#------------------------------------------------------------------------------ - -# UMFPACK and CHOLMOD both require the BLAS. CHOLMOD also requires LAPACK. -# See Kazushige Goto's BLAS at http://www.cs.utexas.edu/users/flame/goto/ or -# http://www.tacc.utexas.edu/~kgoto/ for the best BLAS to use with CHOLMOD. -# LAPACK is at http://www.netlib.org/lapack/ . You can use the standard -# Fortran LAPACK along with Goto's BLAS to obtain very good performance. -# CHOLMOD gets a peak numeric factorization rate of 3.6 Gflops on a 3.2 GHz -# Pentium 4 (512K cache, 4GB main memory) with the Goto BLAS, and 6 Gflops -# on a 2.5Ghz dual-core AMD Opteron. - -# These settings will probably not work, since there is no fixed convention for -# naming the BLAS and LAPACK library (*.a or *.so) files. - -# This is probably slow ... it might connect to the Standard Reference BLAS: -BLAS = -lblas -lgfortran -LAPACK = -llapack - -# NOTE: this next option for the "Goto BLAS" has nothing to do with a "goto" -# statement. Rather, the Goto BLAS is written by Dr. Kazushige Goto. -# Using the Goto BLAS: -# BLAS = -lgoto -lgfortran -lgfortranbegin - -# Using non-optimized versions: -# BLAS = -lblas_plain -lgfortran -lgfortranbegin -# LAPACK = -llapack_plain - -# BLAS = -lblas_plain -lgfortran -lgfortranbegin -# LAPACK = -llapack - -# The BLAS might not contain xerbla, an error-handling routine for LAPACK and -# the BLAS. Also, the standard xerbla requires the Fortran I/O library, and -# stops the application program if an error occurs. A C version of xerbla -# distributed with this software (UFconfig/xerbla/libcerbla.a) includes a -# Fortran-callable xerbla routine that prints nothing and does not stop the -# application program. This is optional. -# XERBLA = ../../UFconfig/xerbla/libcerbla.a - -# If you wish to use the XERBLA in LAPACK and/or the BLAS instead, -# use this option: -XERBLA = - -# If you wish to use the Fortran UFconfig/xerbla/xerbla.f instead, use this: -# XERBLA = ../../UFconfig/xerbla/libxerbla.a - -#------------------------------------------------------------------------------ -# METIS, optionally used by CHOLMOD -#------------------------------------------------------------------------------ - -# If you do not have METIS, or do not wish to use it in CHOLMOD, you must -# compile CHOLMOD with the -DNPARTITION flag. You must also use the -# "METIS =" option, below. - -# The path is relative to where it is used, in CHOLMOD/Lib, CHOLMOD/MATLAB, etc. -# You may wish to use an absolute path. METIS is optional. Compile -# CHOLMOD with -DNPARTITION if you do not wish to use METIS. -METIS_PATH = ../../metis-4.0 -METIS = ../../metis-4.0/libmetis.a - -# If you use CHOLMOD_CONFIG = -DNPARTITION then you must use the following -# options: -# METIS_PATH = -# METIS = - -#------------------------------------------------------------------------------ -# UMFPACK configuration: -#------------------------------------------------------------------------------ - -# Configuration flags for UMFPACK. See UMFPACK/Source/umf_config.h for details. -# -# -DNBLAS do not use the BLAS. UMFPACK will be very slow. -# -D'LONGBLAS=long' or -DLONGBLAS='long long' defines the integers used by -# LAPACK and the BLAS (defaults to 'int') -# -DNSUNPERF do not use the Sun Perf. Library (default is use it on Solaris) -# -DNPOSIX do not use POSIX routines sysconf and times. -# -DGETRUSAGE use getrusage -# -DNO_TIMER do not use any timing routines -# -DNRECIPROCAL do not multiply by the reciprocal -# -DNO_DIVIDE_BY_ZERO do not divide by zero - -UMFPACK_CONFIG = - -#------------------------------------------------------------------------------ -# CHOLMOD configuration -#------------------------------------------------------------------------------ - -# CHOLMOD Library Modules, which appear in libcholmod.a: -# Core requires: none -# Check requires: Core -# Cholesky requires: Core, AMD, COLAMD. optional: Partition, Supernodal -# MatrixOps requires: Core -# Modify requires: Core -# Partition requires: Core, CCOLAMD, METIS. optional: Cholesky -# Supernodal requires: Core, BLAS, LAPACK -# -# CHOLMOD test/demo Modules (all are GNU GPL, do not appear in libcholmod.a): -# Tcov requires: Core, Check, Cholesky, MatrixOps, Modify, Supernodal -# optional: Partition -# Valgrind same as Tcov -# Demo requires: Core, Check, Cholesky, MatrixOps, Supernodal -# optional: Partition -# -# Configuration flags: -# -DNCHECK do not include the Check module. License GNU LGPL -# -DNCHOLESKY do not include the Cholesky module. License GNU LGPL -# -DNPARTITION do not include the Partition module. License GNU LGPL -# also do not include METIS. -# -DNGPL do not include any GNU GPL Modules in the CHOLMOD library: -# -DNMATRIXOPS do not include the MatrixOps module. License GNU GPL -# -DNMODIFY do not include the Modify module. License GNU GPL -# -DNSUPERNODAL do not include the Supernodal module. License GNU GPL -# -# -DNPRINT do not print anything. -# -D'LONGBLAS=long' or -DLONGBLAS='long long' defines the integers used by -# LAPACK and the BLAS (defaults to 'int') -# -DNSUNPERF for Solaris only. If defined, do not use the Sun -# Performance Library - -CHOLMOD_CONFIG = - -#------------------------------------------------------------------------------ -# SuiteSparseQR configuration: -#------------------------------------------------------------------------------ - -# The SuiteSparseQR library can be compiled with the following options: -# -# -DNPARTITION do not include the CHOLMOD partition module -# -DNEXPERT do not include the functions in SuiteSparseQR_expert.cpp -# -DTIMING enable timing and flop counts -# -DHAVE_TBB enable the use of Intel's Threading Building Blocks (TBB) - -# default, without timing, without TBB: -SPQR_CONFIG = -# with timing and TBB: -# SPQR_CONFIG = -DTIMING -DHAVE_TBB -# with timing -# SPQR_CONFIG = -DTIMING - -# This is needed for IBM AIX: (but not for and C codes, just C++) -# SPQR_CONFIG = -DBLAS_NO_UNDERSCORE - -# with TBB, you must select this: -# TBB = -ltbb -# without TBB: -TBB = - -# with timing, you must include the timing library: -# RTLIB = -lrt -# without timing -RTLIB = - -#------------------------------------------------------------------------------ -# Linux -#------------------------------------------------------------------------------ - -# Using default compilers: -# CC = gcc -# CFLAGS = -O3 -fexceptions - -# alternatives: -# CFLAGS = -g -fexceptions \ - -Wall -W -Wshadow -Wmissing-prototypes -Wstrict-prototypes \ - -Wredundant-decls -Wnested-externs -Wdisabled-optimization -ansi \ - -funit-at-a-time -# CFLAGS = -O3 -fexceptions \ - -Wall -W -Werror -Wshadow -Wmissing-prototypes -Wstrict-prototypes \ - -Wredundant-decls -Wnested-externs -Wdisabled-optimization -ansi -# CFLAGS = -O3 -fexceptions -D_FILE_OFFSET_BITS=64 -D_LARGEFILE64_SOURCE -# CFLAGS = -O3 -# CFLAGS = -O3 -g -fexceptions -# CFLAGS = -g -fexceptions \ - -Wall -W -Wshadow \ - -Wredundant-decls -Wdisabled-optimization -ansi - -# consider: -# -fforce-addr -fmove-all-movables -freduce-all-givs -ftsp-ordering -# -frename-registers -ffast-math -funroll-loops - -# Using the Goto BLAS: -# BLAS = -lgoto -lfrtbegin -lg2c $(XERBLA) -lpthread - -# Using Intel's icc and ifort compilers: -# (does not work for mexFunctions unless you add a mexopts.sh file) -# F77 = ifort -# CC = icc -# CFLAGS = -O3 -xN -vec_report=0 -# CFLAGS = -g -# old (broken): CFLAGS = -ansi -O3 -ip -tpp7 -xW -vec_report0 - -# 64bit: -# F77FLAGS = -O -m64 -# CFLAGS = -O3 -fexceptions -m64 -# BLAS = -lgoto64 -lfrtbegin -lg2c -lpthread $(XERBLA) -# LAPACK = -llapack64 - - -# SUSE Linux 10.1, AMD Opteron, with GOTO Blas -# F77 = gfortran -# BLAS = -lgoto_opteron64 -lgfortran - -# SUSE Linux 10.1, Intel Pentium, with GOTO Blas -# F77 = gfortran -# BLAS = -lgoto -lgfortran - -#------------------------------------------------------------------------------ -# Mac -#------------------------------------------------------------------------------ - -# As recommended by macports, http://suitesparse.darwinports.com/ -# I've tested them myself on Mac OSX 10.6.1 (Snow Leopard), on my MacBook Air. -# F77 = gfortran -# CFLAGS = -O3 -fno-common -no-cpp-precomp -fexceptions -# BLAS = -framework Accelerate -# LAPACK = -framework Accelerate - -# Using netlib.org LAPACK and BLAS compiled by gfortran, with and without -# optimzation: -# BLAS = -lblas_plain -lgfortran -# LAPACK = -llapack_plain -# BLAS = -lblas_optimized -lgfortran -# LAPACK = -llapack_optimized - -#------------------------------------------------------------------------------ -# Solaris -#------------------------------------------------------------------------------ - -# 32-bit -# CFLAGS = -KPIC -dalign -xc99=%none -Xc -xlibmieee -xO5 -xlibmil -m32 - -# 64-bit -# CFLAGS = -fast -KPIC -xc99=%none -xlibmieee -xlibmil -m64 -Xc - -# FFLAGS = -fast -KPIC -dalign -xlibmil -m64 - -# The Sun Performance Library includes both LAPACK and the BLAS: -# BLAS = -xlic_lib=sunperf -# LAPACK = - - -#------------------------------------------------------------------------------ -# Compaq Alpha -#------------------------------------------------------------------------------ - -# 64-bit mode only -# CFLAGS = -O2 -std1 -# BLAS = -ldxml -# LAPACK = - -#------------------------------------------------------------------------------ -# Macintosh -#------------------------------------------------------------------------------ - -# CC = gcc -# CFLAGS = -O3 -fno-common -no-cpp-precomp -fexceptions -# LIB = -lstdc++ -# BLAS = -framework Accelerate -# LAPACK = -framework Accelerate - -#------------------------------------------------------------------------------ -# IBM RS 6000 -#------------------------------------------------------------------------------ - -# BLAS = -lessl -# LAPACK = - -# 32-bit mode: -# CFLAGS = -O4 -qipa -qmaxmem=16384 -qproto -# F77FLAGS = -O4 -qipa -qmaxmem=16384 - -# 64-bit mode: -# CFLAGS = -O4 -qipa -qmaxmem=16384 -q64 -qproto -# F77FLAGS = -O4 -qipa -qmaxmem=16384 -q64 -# AR = ar -X64 - -#------------------------------------------------------------------------------ -# SGI IRIX -#------------------------------------------------------------------------------ - -# BLAS = -lscsl -# LAPACK = - -# 32-bit mode -# CFLAGS = -O - -# 64-bit mode (32 bit int's and 64-bit long's): -# CFLAGS = -64 -# F77FLAGS = -64 - -# SGI doesn't have ranlib -# RANLIB = echo - -#------------------------------------------------------------------------------ -# AMD Opteron (64 bit) -#------------------------------------------------------------------------------ - -# BLAS = -lgoto_opteron64 -lg2c -# LAPACK = -llapack_opteron64 - -# SUSE Linux 10.1, AMD Opteron -# F77 = gfortran -# BLAS = -lgoto_opteron64 -lgfortran -# LAPACK = -llapack_opteron64 - -#------------------------------------------------------------------------------ -# remove object files and profile output -#------------------------------------------------------------------------------ - -CLEAN = *.o *.obj *.ln *.bb *.bbg *.da *.tcov *.gcov gmon.out *.bak *.d *.gcda *.gcno diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index d3229427e..1801e30b4 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -26,7 +26,7 @@ set (3rdparty_srcs ${eigen_headers} # Set by 3rdparty/CMakeLists.txt ${CMAKE_CURRENT_SOURCE_DIR}/3rdparty/CCOLAMD/Source/ccolamd.c ${CMAKE_CURRENT_SOURCE_DIR}/3rdparty/CCOLAMD/Source/ccolamd_global.c - ${CMAKE_CURRENT_SOURCE_DIR}/3rdparty/UFconfig/UFconfig.c) + ${CMAKE_CURRENT_SOURCE_DIR}/3rdparty/SuiteSparse_config/SuiteSparse_config.c) gtsam_assign_source_folders("${3rdparty_srcs}") # Create MSVC structure # To exclude a source from the library build (in any subfolder) From 83d87d09614d48765538dc383d124a284961f128 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 16 Apr 2016 13:12:36 -0700 Subject: [PATCH 88/91] A bit of cleanup --- gtsam/inference/Ordering.cpp | 27 ++++++++++++-------------- gtsam/inference/tests/testOrdering.cpp | 11 ++++++++--- 2 files changed, 20 insertions(+), 18 deletions(-) diff --git a/gtsam/inference/Ordering.cpp b/gtsam/inference/Ordering.cpp index 3e3b40f8f..931610639 100644 --- a/gtsam/inference/Ordering.cpp +++ b/gtsam/inference/Ordering.cpp @@ -53,10 +53,10 @@ Ordering Ordering::ColamdConstrained(const VariableIndex& variableIndex, gttic(Ordering_COLAMDConstrained); gttic(Prepare); - size_t nEntries = variableIndex.nEntries(), nFactors = + const size_t nEntries = variableIndex.nEntries(), nFactors = variableIndex.nFactors(), nVars = variableIndex.size(); // Convert to compressed column major format colamd wants it in (== MATLAB format!) - size_t Alen = ccolamd_recommended((int) nEntries, (int) nFactors, + const size_t Alen = ccolamd_recommended((int) nEntries, (int) nFactors, (int) nVars); /* colamd arg 3: size of the array A */ vector A = vector(Alen); /* colamd arg 4: row indices of A, of size Alen */ vector p = vector(nVars + 1); /* colamd arg 5: column pointers of A, of size n_col+1 */ @@ -66,13 +66,10 @@ Ordering Ordering::ColamdConstrained(const VariableIndex& variableIndex, int count = 0; vector keys(nVars); // Array to store the keys in the order we add them so we can retrieve them in permuted order size_t index = 0; - BOOST_FOREACH(const VariableIndex::value_type key_factors, variableIndex) { + for (auto key_factors: variableIndex) { // Arrange factor indices into COLAMD format const VariableIndex::Factors& column = key_factors.second; - size_t lastFactorId = numeric_limits::max(); - BOOST_FOREACH(size_t factorIndex, column) { - if (lastFactorId != numeric_limits::max()) - assert(factorIndex > lastFactorId); + for(size_t factorIndex: column) { A[count++] = (int) factorIndex; // copy sparse column } p[index + 1] = count; // column j (base 1) goes from A[j-1] to A[j]-1 @@ -106,8 +103,8 @@ Ordering Ordering::ColamdConstrained(const VariableIndex& variableIndex, // ccolamd_report(stats); - gttic(Fill_Ordering); // Convert elimination ordering in p to an ordering + gttic(Fill_Ordering); Ordering result; result.resize(nVars); for (size_t j = 0; j < nVars; ++j) @@ -128,13 +125,13 @@ Ordering Ordering::ColamdConstrainedLast(const VariableIndex& variableIndex, // Build a mapping to look up sorted Key indices by Key FastMap keyIndices; size_t j = 0; - BOOST_FOREACH(const VariableIndex::value_type key_factors, variableIndex) + for (auto key_factors: variableIndex) keyIndices.insert(keyIndices.end(), make_pair(key_factors.first, j++)); // If at least some variables are not constrained to be last, constrain the // ones that should be constrained. int group = (constrainLast.size() != n ? 1 : 0); - BOOST_FOREACH(Key key, constrainLast) { + for (Key key: constrainLast) { cmember[keyIndices.at(key)] = group; if (forceOrder) ++group; @@ -155,13 +152,13 @@ Ordering Ordering::ColamdConstrainedFirst(const VariableIndex& variableIndex, // Build a mapping to look up sorted Key indices by Key FastMap keyIndices; size_t j = 0; - BOOST_FOREACH(const VariableIndex::value_type key_factors, variableIndex) + for (auto key_factors: variableIndex) keyIndices.insert(keyIndices.end(), make_pair(key_factors.first, j++)); // If at least some variables are not constrained to be last, constrain the // ones that should be constrained. int group = 0; - BOOST_FOREACH(Key key, constrainFirst) { + for (Key key: constrainFirst) { cmember[keyIndices.at(key)] = group; if (forceOrder) ++group; @@ -169,7 +166,7 @@ Ordering Ordering::ColamdConstrainedFirst(const VariableIndex& variableIndex, if (!forceOrder && !constrainFirst.empty()) ++group; - BOOST_FOREACH(int& c, cmember) + for(int& c: cmember) if (c == none) c = group; @@ -186,12 +183,12 @@ Ordering Ordering::ColamdConstrained(const VariableIndex& variableIndex, // Build a mapping to look up sorted Key indices by Key FastMap keyIndices; size_t j = 0; - BOOST_FOREACH(const VariableIndex::value_type key_factors, variableIndex) + for (auto key_factors: variableIndex) keyIndices.insert(keyIndices.end(), make_pair(key_factors.first, j++)); // Assign groups typedef FastMap::value_type key_group; - BOOST_FOREACH(const key_group& p, groups) { + for(const key_group& p: groups) { // FIXME: check that no groups are skipped cmember[keyIndices.at(p.first)] = p.second; } diff --git a/gtsam/inference/tests/testOrdering.cpp b/gtsam/inference/tests/testOrdering.cpp index c209d086b..52528ad7f 100644 --- a/gtsam/inference/tests/testOrdering.cpp +++ b/gtsam/inference/tests/testOrdering.cpp @@ -172,6 +172,7 @@ TEST(Ordering, csr_format_3) { } /* ************************************************************************* */ +#ifdef GTSAM_SUPPORT_NESTED_DISSECTION TEST(Ordering, csr_format_4) { SymbolicFactorGraph sfg; @@ -206,8 +207,9 @@ TEST(Ordering, csr_format_4) { Ordering metOrder2 = Ordering::Metis(sfg); } - +#endif /* ************************************************************************* */ +#ifdef GTSAM_SUPPORT_NESTED_DISSECTION TEST(Ordering, metis) { SymbolicFactorGraph sfg; @@ -228,8 +230,9 @@ TEST(Ordering, metis) { Ordering metis = Ordering::Metis(sfg); } - +#endif /* ************************************************************************* */ +#ifdef GTSAM_SUPPORT_NESTED_DISSECTION TEST(Ordering, MetisLoop) { // create linear graph @@ -261,7 +264,7 @@ TEST(Ordering, MetisLoop) { } #endif } - +#endif /* ************************************************************************* */ TEST(Ordering, Create) { @@ -280,6 +283,7 @@ TEST(Ordering, Create) { EXPECT(assert_equal(expected, actual)); } +#ifdef GTSAM_SUPPORT_NESTED_DISSECTION // METIS { Ordering actual = Ordering::Create(Ordering::METIS, sfg); @@ -289,6 +293,7 @@ TEST(Ordering, Create) { Ordering expected = Ordering(list_of(5)(3)(4)(1)(0)(2)); EXPECT(assert_equal(expected, actual)); } +#endif // CUSTOM CHECK_EXCEPTION(Ordering::Create(Ordering::CUSTOM, sfg), runtime_error); From 621aefb9a5b8e870fa6073541d749ab50f17152c Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 16 Apr 2016 13:22:49 -0700 Subject: [PATCH 89/91] Forgot to commit cmake change --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 8c4229ed5..3ea81c85e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -305,7 +305,7 @@ endif() # 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/SuiteSparse_config gtsam/3rdparty/CCOLAMD/Include ${METIS_INCLUDE_DIRECTORIES} ${PROJECT_SOURCE_DIR} From 99f1a950a824bec793a49fa2094d8d6428d3ee3f Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 16 Apr 2016 13:23:10 -0700 Subject: [PATCH 90/91] disable metis test if flag not set --- gtsam/symbolic/tests/testSymbolicBayesTree.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/gtsam/symbolic/tests/testSymbolicBayesTree.cpp b/gtsam/symbolic/tests/testSymbolicBayesTree.cpp index 82d8fb3ec..13849895d 100644 --- a/gtsam/symbolic/tests/testSymbolicBayesTree.cpp +++ b/gtsam/symbolic/tests/testSymbolicBayesTree.cpp @@ -726,6 +726,7 @@ TEST(SymbolicBayesTree, COLAMDvsMETIS) { EXPECT(assert_equal(expected, actual)); } +#ifdef GTSAM_SUPPORT_NESTED_DISSECTION // METIS { Ordering ordering = Ordering::Create(Ordering::METIS, sfg); @@ -759,6 +760,7 @@ TEST(SymbolicBayesTree, COLAMDvsMETIS) { SymbolicBayesTree actual = *sfg.eliminateMultifrontal(ordering); EXPECT(assert_equal(expected, actual)); } +#endif } /* ************************************************************************* */ From 63e6ed0401e94cc1c8d04813a0bc34fc1402c37c Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 16 Apr 2016 13:24:28 -0700 Subject: [PATCH 91/91] Fixed compile errors with tests on Mac --- gtsam/navigation/tests/testGPSFactor.cpp | 4 ++-- gtsam/navigation/tests/testMagFactor.cpp | 8 ++++---- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/gtsam/navigation/tests/testGPSFactor.cpp b/gtsam/navigation/tests/testGPSFactor.cpp index d9ba38e02..293bffa00 100644 --- a/gtsam/navigation/tests/testGPSFactor.cpp +++ b/gtsam/navigation/tests/testGPSFactor.cpp @@ -58,7 +58,7 @@ TEST( GPSFactor, Constructor ) { // Create a linearization point at zero error Pose3 T(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(E, N, U)); - EXPECT(assert_equal(Z_3x3,factor.evaluateError(T),1e-5)); + EXPECT(assert_equal(Z_3x1,factor.evaluateError(T),1e-5)); // Calculate numerical derivatives Matrix expectedH = numericalDerivative11( @@ -87,7 +87,7 @@ TEST( GPSFactor2, Constructor ) { // 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(Z_3x3,factor.evaluateError(T),1e-5)); + EXPECT(assert_equal(Z_3x1,factor.evaluateError(T),1e-5)); // Calculate numerical derivatives Matrix expectedH = numericalDerivative11( diff --git a/gtsam/navigation/tests/testMagFactor.cpp b/gtsam/navigation/tests/testMagFactor.cpp index 1cc84751c..45db58e33 100644 --- a/gtsam/navigation/tests/testMagFactor.cpp +++ b/gtsam/navigation/tests/testMagFactor.cpp @@ -71,19 +71,19 @@ TEST( MagFactor, Factors ) { // MagFactor MagFactor f(1, measured, s, dir, bias, model); - EXPECT( assert_equal(Z_3x3,f.evaluateError(theta,H1),1e-5)); + EXPECT( assert_equal(Z_3x1,f.evaluateError(theta,H1),1e-5)); EXPECT( assert_equal((Matrix)numericalDerivative11 // (boost::bind(&MagFactor::evaluateError, &f, _1, none), theta), H1, 1e-7)); // MagFactor1 MagFactor1 f1(1, measured, s, dir, bias, model); - EXPECT( assert_equal(Z_3x3,f1.evaluateError(nRb,H1),1e-5)); + EXPECT( assert_equal(Z_3x1,f1.evaluateError(nRb,H1),1e-5)); EXPECT( assert_equal(numericalDerivative11 // (boost::bind(&MagFactor1::evaluateError, &f1, _1, none), nRb), H1, 1e-7)); // MagFactor2 MagFactor2 f2(1, 2, measured, nRb, model); - EXPECT( assert_equal(Z_3x3,f2.evaluateError(scaled,bias,H1,H2),1e-5)); + EXPECT( assert_equal(Z_3x1,f2.evaluateError(scaled,bias,H1,H2),1e-5)); EXPECT( assert_equal(numericalDerivative11 // (boost::bind(&MagFactor2::evaluateError, &f2, _1, bias, none, none), scaled),// H1, 1e-7)); @@ -93,7 +93,7 @@ TEST( MagFactor, Factors ) { // MagFactor2 MagFactor3 f3(1, 2, 3, measured, nRb, model); - EXPECT(assert_equal(Z_3x3,f3.evaluateError(s,dir,bias,H1,H2,H3),1e-5)); + EXPECT(assert_equal(Z_3x1,f3.evaluateError(s,dir,bias,H1,H2,H3),1e-5)); EXPECT(assert_equal((Matrix)numericalDerivative11 // (boost::bind(&MagFactor3::evaluateError, &f3, _1, dir, bias, none, none, none), s),// H1, 1e-7));