From 4bfde71d8aa4889eb82f7b2764e418a914992796 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 20 Jan 2023 12:43:09 -0800 Subject: [PATCH 01/51] Deprecate `filter` in favor of `extract`, which copies to std::map without boost. --- gtsam/nonlinear/Values-inl.h | 29 +++-- gtsam/nonlinear/Values.h | 156 +++++++++++---------------- gtsam/nonlinear/tests/testValues.cpp | 20 +++- gtsam/nonlinear/utilities.h | 85 ++++++++------- gtsam/sfm/ShonanAveraging.cpp | 26 ++--- gtsam/sfm/ShonanAveraging.h | 4 +- 6 files changed, 160 insertions(+), 160 deletions(-) diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h index 0370c5cee..598963761 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -90,7 +90,25 @@ namespace gtsam { } }; - /* ************************************************************************* */ +/* ************************************************************************* */ + template + std::map + Values::extract(const std::function& filterFcn) const { + std::map result; + for (const auto& key_value : *this) { + // Check if key matches + if (filterFcn(key_value.key)) { + // Check if type matches (typically does as symbols matched with types) + if (auto t = + dynamic_cast*>(&key_value.value)) + result[key_value.key] = t->value(); + } + } + return result; + } + +/* ************************************************************************* */ +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 template class Values::Filtered { public: @@ -164,7 +182,6 @@ namespace gtsam { const_const_iterator constEnd_; }; - /* ************************************************************************* */ template class Values::ConstFiltered { public: @@ -215,8 +232,6 @@ namespace gtsam { } }; - /* ************************************************************************* */ - /** Constructor from a Filtered view copies out all values */ template Values::Values(const Values::Filtered& view) { for(const auto key_value: view) { @@ -225,7 +240,6 @@ namespace gtsam { } } - /* ************************************************************************* */ template Values::Values(const Values::ConstFiltered& view) { for(const auto key_value: view) { @@ -234,13 +248,11 @@ namespace gtsam { } } - /* ************************************************************************* */ Values::Filtered inline Values::filter(const std::function& filterFcn) { return filter(filterFcn); } - /* ************************************************************************* */ template Values::Filtered Values::filter(const std::function& filterFcn) { @@ -248,19 +260,18 @@ namespace gtsam { std::placeholders::_1), *this); } - /* ************************************************************************* */ Values::ConstFiltered inline Values::filter(const std::function& filterFcn) const { return filter(filterFcn); } - /* ************************************************************************* */ template Values::ConstFiltered Values::filter(const std::function& filterFcn) const { return ConstFiltered(std::bind(&filterHelper, filterFcn, std::placeholders::_1), *this); } +#endif /* ************************************************************************* */ template<> diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index cfe6347b5..79ddb0267 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -29,9 +29,11 @@ #include #include #include -#include #include #include +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 +#include +#endif #include #include @@ -126,14 +128,6 @@ namespace gtsam { typedef KeyValuePair value_type; - /** A filtered view of a Values, returned from Values::filter. */ - template - class Filtered; - - /** A filtered view of a const Values, returned from Values::filter. */ - template - class ConstFiltered; - /** Default constructor creates an empty Values class */ Values() {} @@ -153,14 +147,6 @@ namespace gtsam { /** Construct from a Values and an update vector: identical to other.retract(delta) */ Values(const Values& other, const VectorValues& delta); - /** Constructor from a Filtered view copies out all values */ - template - Values(const Filtered& view); - - /** Constructor from a Filtered or ConstFiltered view */ - template - Values(const ConstFiltered& view); - /// @name Testable /// @{ @@ -322,83 +308,6 @@ namespace gtsam { /** Return a VectorValues of zero vectors for each variable in this Values */ VectorValues zeroVectors() const; - /** - * Return a filtered view of this Values class, without copying any data. - * When iterating over the filtered view, only the key-value pairs - * with a key causing \c filterFcn to return \c true are visited. Because - * the object Filtered returned from filter() is only a - * view the original Values object must not be deallocated or - * go out of scope as long as the view is needed. - * @param filterFcn The function that determines which key-value pairs are - * included in the filtered view, for which this function returns \c true - * on their keys. - * @return A filtered view of the original Values class, which references - * the original Values class. - */ - Filtered - filter(const std::function& filterFcn); - - /** - * Return a filtered view of this Values class, without copying any data. - * In this templated version, only key-value pairs whose value matches the - * template argument \c ValueType and whose key causes the function argument - * \c filterFcn to return true are visited when iterating over the filtered - * view. Because the object Filtered returned from filter() is only - * a view the original Values object must not be deallocated or - * go out of scope as long as the view is needed. - * @tparam ValueType The type that the value in a key-value pair must match - * to be included in the filtered view. Currently, base classes are not - * resolved so the type must match exactly, except if ValueType = Value, in - * which case no type filtering is done. - * @param filterFcn The function that determines which key-value pairs are - * included in the filtered view, for which this function returns \c true - * on their keys (default: always return true so that filter() only - * filters by type, matching \c ValueType). - * @return A filtered view of the original Values class, which references - * the original Values class. - */ - template - Filtered - filter(const std::function& filterFcn = &_truePredicate); - - /** - * Return a filtered view of this Values class, without copying any data. - * When iterating over the filtered view, only the key-value pairs - * with a key causing \c filterFcn to return \c true are visited. Because - * the object Filtered returned from filter() is only a - * view the original Values object must not be deallocated or - * go out of scope as long as the view is needed. - * @param filterFcn The function that determines which key-value pairs are - * included in the filtered view, for which this function returns \c true - * on their keys. - * @return A filtered view of the original Values class, which references - * the original Values class. - */ - ConstFiltered - filter(const std::function& filterFcn) const; - - /** - * Return a filtered view of this Values class, without copying any data. - * In this templated version, only key-value pairs whose value matches the - * template argument \c ValueType and whose key causes the function argument - * \c filterFcn to return true are visited when iterating over the filtered - * view. Because the object Filtered returned from filter() is only - * a view the original Values object must not be deallocated or - * go out of scope as long as the view is needed. - * @tparam ValueType The type that the value in a key-value pair must match - * to be included in the filtered view. Currently, base classes are not - * resolved so the type must match exactly, except if ValueType = Value, in - * which case no type filtering is done. - * @param filterFcn The function that determines which key-value pairs are - * included in the filtered view, for which this function returns \c true - * on their keys. - * @return A filtered view of the original Values class, which references - * the original Values class. - */ - template - ConstFiltered - filter(const std::function& filterFcn = &_truePredicate) const; - // Count values of given type \c ValueType template size_t count() const { @@ -410,6 +319,65 @@ namespace gtsam { return i; } + /** + * Extract a subset of values of the given type \c ValueType. + * + * In this templated version, only key-value pairs whose value matches the + * template argument \c ValueType and whose key causes the function argument + * \c filterFcn to return true are visited when iterating over the filtered + * view. This replaces the fancier but very boost-dependent \c filter methods + * that were previously available up to GTSAM 4.2. + * + * @tparam ValueType The type that the value in a key-value pair must match + * to be included in the filtered view. Currently, base classes are not + * resolved so the type must match exactly, except if ValueType = Value, in + * which case no type filtering is done. + * @param filterFcn The function that determines which key-value pairs are + * included in the filtered view, for which this function returns \c true + * on their keys (default: always return true so that filter() only + * filters by type, matching \c ValueType). + * @return An Eigen aligned map on Key with the filtered values. + */ + template + std::map // , std::less, Eigen::aligned_allocator + extract(const std::function& filterFcn = &_truePredicate) const; + +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 + /** A filtered view of a Values, returned from Values::filter. */ + template + class Filtered; + + /** A filtered view of a const Values, returned from Values::filter. */ + template + class ConstFiltered; + + /** Constructor from a Filtered view copies out all values */ + template + Values(const Filtered& view); + + /** Constructor from a Filtered or ConstFiltered view */ + template + Values(const ConstFiltered& view); + + /// A filtered view of the original Values class. + Filtered GTSAM_DEPRECATED + filter(const std::function& filterFcn); + + /// A filtered view of the original Values class, also filter on type. + template + Filtered GTSAM_DEPRECATED + filter(const std::function& filterFcn = &_truePredicate); + + /// A filtered view of the original Values class, const version. + ConstFiltered GTSAM_DEPRECATED + filter(const std::function& filterFcn) const; + + /// A filtered view of the original Values class, also on type, const. + template + ConstFiltered GTSAM_DEPRECATED filter( + const std::function& filterFcn = &_truePredicate) const; +#endif + private: // Filters based on ValueType (if not Value) and also based on the user- // supplied \c filter function. diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index 2465903db..85dd2f4b3 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -343,6 +343,7 @@ TEST(Values, filter) { values.insert(2, pose2); values.insert(3, pose3); +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 // Filter by key int i = 0; Values::Filtered filtered = values.filter(std::bind(std::greater_equal(), std::placeholders::_1, 2)); @@ -395,8 +396,6 @@ TEST(Values, filter) { ++ i; } EXPECT_LONGS_EQUAL(2, (long)i); - EXPECT_LONGS_EQUAL(2, (long)values.count()); - EXPECT_LONGS_EQUAL(2, (long)values.count()); // construct a values with the view Values actualSubValues2(pose_filtered); @@ -404,6 +403,16 @@ TEST(Values, filter) { expectedSubValues2.insert(1, pose1); expectedSubValues2.insert(3, pose3); EXPECT(assert_equal(expectedSubValues2, actualSubValues2)); +#endif + + // Test counting by type. + EXPECT_LONGS_EQUAL(2, (long)values.count()); + EXPECT_LONGS_EQUAL(2, (long)values.count()); + + // Filter by type using extract. + auto extracted_pose3s = values.extract(); + EXPECT_LONGS_EQUAL(2, (long)extracted_pose3s.size()); + } /* ************************************************************************* */ @@ -419,6 +428,7 @@ TEST(Values, Symbol_filter) { values.insert(X(2), pose2); values.insert(Symbol('y', 3), pose3); +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 int i = 0; for(const auto key_value: values.filter(Symbol::ChrTest('y'))) { if(i == 0) { @@ -433,6 +443,12 @@ TEST(Values, Symbol_filter) { ++ i; } LONGS_EQUAL(2, (long)i); +#endif + +// Test extract with filter on symbol: + auto extracted_pose3s = values.extract(Symbol::ChrTest('y')); + EXPECT_LONGS_EQUAL(2, (long)extracted_pose3s.size()); + } /* ************************************************************************* */ diff --git a/gtsam/nonlinear/utilities.h b/gtsam/nonlinear/utilities.h index d2b38d374..985e1a5f4 100644 --- a/gtsam/nonlinear/utilities.h +++ b/gtsam/nonlinear/utilities.h @@ -90,19 +90,19 @@ KeySet createKeySet(std::string s, const Vector& I) { /// Extract all Point2 values into a single matrix [x y] Matrix extractPoint2(const Values& values) { - Values::ConstFiltered points = values.filter(); + const auto points = values.extract(); // Point2 is aliased as a gtsam::Vector in the wrapper - Values::ConstFiltered points2 = values.filter(); + const auto points2 = values.extract(); Matrix result(points.size() + points2.size(), 2); size_t j = 0; for (const auto& key_value : points) { - result.row(j++) = key_value.value; + result.row(j++) = key_value.second; } for (const auto& key_value : points2) { - if (key_value.value.rows() == 2) { - result.row(j++) = key_value.value; + if (key_value.second.rows() == 2) { + result.row(j++) = key_value.second; } } return result; @@ -110,19 +110,19 @@ Matrix extractPoint2(const Values& values) { /// Extract all Point3 values into a single matrix [x y z] Matrix extractPoint3(const Values& values) { - Values::ConstFiltered points = values.filter(); + const auto points = values.extract(); // Point3 is aliased as a gtsam::Vector in the wrapper - Values::ConstFiltered points2 = values.filter(); + const auto points2 = values.extract(); Matrix result(points.size() + points2.size(), 3); size_t j = 0; for (const auto& key_value : points) { - result.row(j++) = key_value.value; + result.row(j++) = key_value.second; } for (const auto& key_value : points2) { - if (key_value.value.rows() == 3) { - result.row(j++) = key_value.value; + if (key_value.second.rows() == 3) { + result.row(j++) = key_value.second; } } return result; @@ -130,34 +130,40 @@ Matrix extractPoint3(const Values& values) { /// Extract all Pose3 values Values allPose2s(const Values& values) { - return values.filter(); + Values result; + for(const auto& key_value: values.extract()) + result.insert(key_value.first, key_value.second); + return result; } /// Extract all Pose2 values into a single matrix [x y theta] Matrix extractPose2(const Values& values) { - Values::ConstFiltered poses = values.filter(); + const auto poses = values.extract(); Matrix result(poses.size(), 3); size_t j = 0; for(const auto& key_value: poses) - result.row(j++) << key_value.value.x(), key_value.value.y(), key_value.value.theta(); + result.row(j++) << key_value.second.x(), key_value.second.y(), key_value.second.theta(); return result; } /// Extract all Pose3 values Values allPose3s(const Values& values) { - return values.filter(); + Values result; + for(const auto& key_value: values.extract()) + result.insert(key_value.first, key_value.second); + return result; } /// Extract all Pose3 values into a single matrix [r11 r12 r13 r21 r22 r23 r31 r32 r33 x y z] Matrix extractPose3(const Values& values) { - Values::ConstFiltered poses = values.filter(); + const auto poses = values.extract(); Matrix result(poses.size(), 12); size_t j = 0; for(const auto& key_value: poses) { - result.row(j).segment(0, 3) << key_value.value.rotation().matrix().row(0); - result.row(j).segment(3, 3) << key_value.value.rotation().matrix().row(1); - result.row(j).segment(6, 3) << key_value.value.rotation().matrix().row(2); - result.row(j).tail(3) = key_value.value.translation(); + result.row(j).segment(0, 3) << key_value.second.rotation().matrix().row(0); + result.row(j).segment(3, 3) << key_value.second.rotation().matrix().row(1); + result.row(j).segment(6, 3) << key_value.second.rotation().matrix().row(2); + result.row(j).tail(3) = key_value.second.translation(); j++; } return result; @@ -172,20 +178,19 @@ Matrix extractPose3(const Values& values) { /// variables x1, x2, ..., x200 of type Vector each 5-dimensional, will return a /// 200x5 matrix with row i containing xi. Matrix extractVectors(const Values& values, char c) { - Values::ConstFiltered vectors = - values.filter(Symbol::ChrTest(c)); + const auto vectors = values.extract(Symbol::ChrTest(c)); if (vectors.size() == 0) { return Matrix(); } - auto dim = vectors.begin()->value.size(); + auto dim = vectors.begin()->second.size(); Matrix result(vectors.size(), dim); Eigen::Index rowi = 0; for (const auto& kv : vectors) { - if (kv.value.size() != dim) { + if (kv.second.size() != dim) { throw std::runtime_error( "Tried to extract different-sized vectors into a single matrix"); } - result.row(rowi) = kv.value; + result.row(rowi) = kv.second; ++rowi; } return result; @@ -196,14 +201,14 @@ void perturbPoint2(Values& values, double sigma, int32_t seed = 42u) { noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(2, sigma); Sampler sampler(model, seed); - for (const auto& key_value : values.filter()) { - values.update(key_value.key, - key_value.value + Point2(sampler.sample())); + for (const auto& key_value : values.extract()) { + values.update(key_value.first, + key_value.second + Point2(sampler.sample())); } - for (const auto& key_value : values.filter()) { - if (key_value.value.rows() == 2) { - values.update(key_value.key, - key_value.value + Point2(sampler.sample())); + for (const auto& key_value : values.extract()) { + if (key_value.second.rows() == 2) { + values.update(key_value.first, + key_value.second + Point2(sampler.sample())); } } } @@ -214,8 +219,8 @@ void perturbPose2(Values& values, double sigmaT, double sigmaR, int32_t seed = noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas( Vector3(sigmaT, sigmaT, sigmaR)); Sampler sampler(model, seed); - for(const auto& key_value: values.filter()) { - values.update(key_value.key, key_value.value.retract(sampler.sample())); + for(const auto& key_value: values.extract()) { + values.update(key_value.first, key_value.second.retract(sampler.sample())); } } @@ -224,14 +229,14 @@ void perturbPoint3(Values& values, double sigma, int32_t seed = 42u) { noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3, sigma); Sampler sampler(model, seed); - for (const auto& key_value : values.filter()) { - values.update(key_value.key, - key_value.value + Point3(sampler.sample())); + for (const auto& key_value : values.extract()) { + values.update(key_value.first, + key_value.second + Point3(sampler.sample())); } - for (const auto& key_value : values.filter()) { - if (key_value.value.rows() == 3) { - values.update(key_value.key, - key_value.value + Point3(sampler.sample())); + for (const auto& key_value : values.extract()) { + if (key_value.second.rows() == 3) { + values.update(key_value.first, + key_value.second + Point3(sampler.sample())); } } } diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp index c00669e36..ea4171238 100644 --- a/gtsam/sfm/ShonanAveraging.cpp +++ b/gtsam/sfm/ShonanAveraging.cpp @@ -207,9 +207,9 @@ Matrix ShonanAveraging::StiefelElementMatrix(const Values &values) { const size_t N = values.size(); const size_t p = values.at(0).rows(); Matrix S(p, N * d); - for (const auto it : values.filter()) { - S.middleCols(it.key * d) = - it.value.matrix().leftCols(); // project Qj to Stiefel + for (const auto& it : values.extract()) { + S.middleCols(it.first * d) = + it.second.matrix().leftCols(); // project Qj to Stiefel } return S; } @@ -218,11 +218,11 @@ Matrix ShonanAveraging::StiefelElementMatrix(const Values &values) { template <> Values ShonanAveraging<2>::projectFrom(size_t p, const Values &values) const { Values result; - for (const auto it : values.filter()) { - assert(it.value.rows() == p); - const auto &M = it.value.matrix(); + for (const auto& it : values.extract()) { + assert(it.second.rows() == p); + const auto &M = it.second.matrix(); const Rot2 R = Rot2::atan2(M(1, 0), M(0, 0)); - result.insert(it.key, R); + result.insert(it.first, R); } return result; } @@ -230,11 +230,11 @@ Values ShonanAveraging<2>::projectFrom(size_t p, const Values &values) const { template <> Values ShonanAveraging<3>::projectFrom(size_t p, const Values &values) const { Values result; - for (const auto it : values.filter()) { - assert(it.value.rows() == p); - const auto &M = it.value.matrix(); + for (const auto& it : values.extract()) { + assert(it.second.rows() == p); + const auto &M = it.second.matrix(); const Rot3 R = Rot3::ClosestTo(M.topLeftCorner<3, 3>()); - result.insert(it.key, R); + result.insert(it.first, R); } return result; } @@ -326,8 +326,8 @@ double ShonanAveraging::cost(const Values &values) const { } // Finally, project each dxd rotation block to SO(d) Values result; - for (const auto it : values.filter()) { - result.insert(it.key, SO(it.value.matrix())); + for (const auto& it : values.extract()) { + result.insert(it.first, SO(it.second.matrix())); } return graph.error(result); } diff --git a/gtsam/sfm/ShonanAveraging.h b/gtsam/sfm/ShonanAveraging.h index e035da4c7..b40916828 100644 --- a/gtsam/sfm/ShonanAveraging.h +++ b/gtsam/sfm/ShonanAveraging.h @@ -366,8 +366,8 @@ class GTSAM_EXPORT ShonanAveraging { template static Values LiftTo(size_t p, const Values &values) { Values result; - for (const auto it : values.filter()) { - result.insert(it.key, SOn::Lift(p, it.value.matrix())); + for (const auto it : values.extract()) { + result.insert(it.first, SOn::Lift(p, it.second.matrix())); } return result; } From f1dcc6bede5f1aed59f53bb8b2dfd3e4c1b9f7cd Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 21 Jan 2023 11:39:42 -0800 Subject: [PATCH 02/51] Removed filter from examples and timing scripts --- examples/StereoVOExample_large.cpp | 3 +- .../examples/ConcurrentCalibration.cpp | 46 ++++++++++--------- .../examples/SmartProjectionFactorExample.cpp | 3 +- .../examples/SmartRangeExample_plaza1.cpp | 10 ++-- .../examples/SmartRangeExample_plaza2.cpp | 10 ++-- .../SmartStereoProjectionFactorExample.cpp | 24 +++++----- timing/timeLago.cpp | 13 +++--- 7 files changed, 56 insertions(+), 53 deletions(-) diff --git a/examples/StereoVOExample_large.cpp b/examples/StereoVOExample_large.cpp index af81db703..c4a3a8de4 100644 --- a/examples/StereoVOExample_large.cpp +++ b/examples/StereoVOExample_large.cpp @@ -27,6 +27,7 @@ #include #include #include +#include #include #include #include @@ -113,7 +114,7 @@ int main(int argc, char** argv) { Values result = optimizer.optimize(); cout << "Final result sample:" << endl; - Values pose_values = result.filter(); + Values pose_values = utilities::allPose3s(result); pose_values.print("Final camera poses:\n"); return 0; diff --git a/gtsam_unstable/examples/ConcurrentCalibration.cpp b/gtsam_unstable/examples/ConcurrentCalibration.cpp index 95629fb43..75dc49eee 100644 --- a/gtsam_unstable/examples/ConcurrentCalibration.cpp +++ b/gtsam_unstable/examples/ConcurrentCalibration.cpp @@ -20,6 +20,7 @@ #include #include +#include #include #include #include @@ -35,12 +36,15 @@ using namespace std; using namespace gtsam; +using symbol_shorthand::K; +using symbol_shorthand::L; +using symbol_shorthand::X; int main(int argc, char** argv){ Values initial_estimate; NonlinearFactorGraph graph; - const noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(2,1); + const auto model = noiseModel::Isotropic::Sigma(2,1); string calibration_loc = findExampleDataFile("VO_calibration00s.txt"); string pose_loc = findExampleDataFile("VO_camera_poses00s.txt"); @@ -54,13 +58,13 @@ int main(int argc, char** argv){ calibration_file >> fx >> fy >> s >> u0 >> v0 >> b; //create stereo camera calibration object - const Cal3_S2::shared_ptr K(new Cal3_S2(fx,fy,s,u0,v0)); - const Cal3_S2::shared_ptr noisy_K(new Cal3_S2(fx*1.2,fy*1.2,s,u0-10,v0+10)); + const Cal3_S2 true_K(fx,fy,s,u0,v0); + const Cal3_S2 noisy_K(fx*1.2,fy*1.2,s,u0-10,v0+10); - initial_estimate.insert(Symbol('K', 0), *noisy_K); + initial_estimate.insert(K(0), noisy_K); - noiseModel::Diagonal::shared_ptr calNoise = noiseModel::Diagonal::Sigmas((Vector(5) << 500, 500, 1e-5, 100, 100).finished()); - graph.addPrior(Symbol('K', 0), *noisy_K, calNoise); + auto calNoise = noiseModel::Diagonal::Sigmas((Vector(5) << 500, 500, 1e-5, 100, 100).finished()); + graph.addPrior(K(0), noisy_K, calNoise); ifstream pose_file(pose_loc.c_str()); @@ -75,7 +79,7 @@ int main(int argc, char** argv){ initial_estimate.insert(Symbol('x', pose_id), Pose3(m)); } - noiseModel::Isotropic::shared_ptr poseNoise = noiseModel::Isotropic::Sigma(6, 0.01); + auto poseNoise = noiseModel::Isotropic::Sigma(6, 0.01); graph.addPrior(Symbol('x', pose_id), Pose3(m), poseNoise); // camera and landmark keys @@ -83,22 +87,22 @@ int main(int argc, char** argv){ // pixel coordinates uL, uR, v (same for left/right images due to rectification) // landmark coordinates X, Y, Z in camera frame, resulting from triangulation - double uL, uR, v, X, Y, Z; + double uL, uR, v, _X, Y, Z; ifstream factor_file(factor_loc.c_str()); cout << "Reading stereo factors" << endl; //read stereo measurement details from file and use to create and add GenericStereoFactor objects to the graph representation - while (factor_file >> x >> l >> uL >> uR >> v >> X >> Y >> Z) { -// graph.emplace_shared >(StereoPoint2(uL, uR, v), model, Symbol('x', x), Symbol('l', l), K); + while (factor_file >> x >> l >> uL >> uR >> v >> _X >> Y >> Z) { +// graph.emplace_shared >(StereoPoint2(uL, uR, v), model, X(x), L(l), K); - graph.emplace_shared >(Point2(uL,v), model, Symbol('x', x), Symbol('l', l), Symbol('K', 0)); + graph.emplace_shared >(Point2(uL,v), model, X(x), L(l), K(0)); //if the landmark variable included in this factor has not yet been added to the initial variable value estimate, add it - if (!initial_estimate.exists(Symbol('l', l))) { - Pose3 camPose = initial_estimate.at(Symbol('x', x)); + if (!initial_estimate.exists(L(l))) { + Pose3 camPose = initial_estimate.at(X(x)); //transformFrom() transforms the input Point3 from the camera pose space, camPose, to the global space - Point3 worldPoint = camPose.transformFrom(Point3(X, Y, Z)); - initial_estimate.insert(Symbol('l', l), worldPoint); + Point3 worldPoint = camPose.transformFrom(Point3(_X, Y, Z)); + initial_estimate.insert(L(l), worldPoint); } } @@ -123,7 +127,7 @@ int main(int argc, char** argv){ double currentError; - stream_K << optimizer.iterations() << " " << optimizer.values().at(Symbol('K',0)).vector().transpose() << endl; + stream_K << optimizer.iterations() << " " << optimizer.values().at(K(0)).vector().transpose() << endl; // Iterative loop @@ -132,7 +136,7 @@ int main(int argc, char** argv){ currentError = optimizer.error(); optimizer.iterate(); - stream_K << optimizer.iterations() << " " << optimizer.values().at(Symbol('K',0)).vector().transpose() << endl; + stream_K << optimizer.iterations() << " " << optimizer.values().at(K(0)).vector().transpose() << endl; if(params.verbosity >= NonlinearOptimizerParams::ERROR) cout << "newError: " << optimizer.error() << endl; } while(optimizer.iterations() < params.maxIterations && @@ -142,13 +146,13 @@ int main(int argc, char** argv){ Values result = optimizer.values(); cout << "Final result sample:" << endl; - Values pose_values = result.filter(); + Values pose_values = utilities::allPose3s(result); pose_values.print("Final camera poses:\n"); - Values(result.filter()).print("Final K\n"); + result.at(K(0)).print("Final K\n"); - noisy_K->print("Initial noisy K\n"); - K->print("Initial correct K\n"); + noisy_K.print("Initial noisy K\n"); + true_K.print("Initial correct K\n"); return 0; } diff --git a/gtsam_unstable/examples/SmartProjectionFactorExample.cpp b/gtsam_unstable/examples/SmartProjectionFactorExample.cpp index e290cef7e..b3b04cca3 100644 --- a/gtsam_unstable/examples/SmartProjectionFactorExample.cpp +++ b/gtsam_unstable/examples/SmartProjectionFactorExample.cpp @@ -30,6 +30,7 @@ #include #include #include +#include #include #include #include @@ -114,7 +115,7 @@ int main(int argc, char** argv){ Values result = optimizer.optimize(); cout << "Final result sample:" << endl; - Values pose_values = result.filter(); + Values pose_values = utilities::allPose3s(result); pose_values.print("Final camera poses:\n"); return 0; diff --git a/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp b/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp index 64afa8030..2fccf6b18 100644 --- a/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp +++ b/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp @@ -234,9 +234,8 @@ int main(int argc, char** argv) { } } countK = 0; - for(const auto it: result.filter()) - os2 << it.key << "\t" << it.value.x() << "\t" << it.value.y() << "\t1" - << endl; + for (const auto& [key, point] : result.extract()) + os2 << key << "\t" << point.x() << "\t" << point.y() << "\t1" << endl; if (smart) { for(size_t jj: ids) { Point2 landmark = smartFactors[jj]->triangulate(result); @@ -257,9 +256,8 @@ int main(int argc, char** argv) { // Write result to file Values result = isam.calculateEstimate(); ofstream os("rangeResult.txt"); - for(const auto it: result.filter()) - os << it.key << "\t" << it.value.x() << "\t" << it.value.y() << "\t" - << it.value.theta() << endl; + for (const auto& [key, pose] : result.extract()) + os << key << "\t" << pose.x() << "\t" << pose.y() << "\t" << pose.theta() << endl; exit(0); } diff --git a/gtsam_unstable/examples/SmartRangeExample_plaza2.cpp b/gtsam_unstable/examples/SmartRangeExample_plaza2.cpp index 95aff85a7..1e6f77b31 100644 --- a/gtsam_unstable/examples/SmartRangeExample_plaza2.cpp +++ b/gtsam_unstable/examples/SmartRangeExample_plaza2.cpp @@ -202,13 +202,11 @@ int main(int argc, char** argv) { // Write result to file Values result = isam.calculateEstimate(); ofstream os2("rangeResultLM.txt"); - for(const auto it: result.filter()) - os2 << it.key << "\t" << it.value.x() << "\t" << it.value.y() << "\t1" - << endl; + for (const auto& [key, point] : result.extract()) + os2 << key << "\t" << point.x() << "\t" << point.y() << "\t1" << endl; ofstream os("rangeResult.txt"); - for(const auto it: result.filter()) - os << it.key << "\t" << it.value.x() << "\t" << it.value.y() << "\t" - << it.value.theta() << endl; + for (const auto& [key, pose] : result.extract()) + os << key << "\t" << pose.x() << "\t" << pose.y() << "\t" << pose.theta() << endl; exit(0); } diff --git a/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp b/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp index 2426ca201..804e4cf5b 100644 --- a/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp +++ b/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp @@ -29,6 +29,7 @@ #include #include #include +#include #include #include #include @@ -43,6 +44,7 @@ using namespace std; using namespace gtsam; +using symbol_shorthand::X; int main(int argc, char** argv){ @@ -84,16 +86,16 @@ int main(int argc, char** argv){ if(add_initial_noise){ m(1,3) += (pose_id % 10)/10.0; } - initial_estimate.insert(Symbol('x', pose_id), Pose3(m)); + initial_estimate.insert(X(pose_id), Pose3(m)); } - Values initial_pose_values = initial_estimate.filter(); + const auto initialPoses = initial_estimate.extract(); if (output_poses) { init_pose3Out.open(init_poseOutput.c_str(), ios::out); - for (size_t i = 1; i <= initial_pose_values.size(); i++) { + for (size_t i = 1; i <= initialPoses.size(); i++) { init_pose3Out << i << " " - << initial_pose_values.at(Symbol('x', i)).matrix().format( + << initialPoses.at(X(i)).matrix().format( Eigen::IOFormat(Eigen::StreamPrecision, 0, " ", " ")) << endl; } } @@ -103,7 +105,7 @@ int main(int argc, char** argv){ // pixel coordinates uL, uR, v (same for left/right images due to rectification) // landmark coordinates X, Y, Z in camera frame, resulting from triangulation - double uL, uR, v, X, Y, Z; + double uL, uR, v, _X, Y, Z; ifstream factor_file(factor_loc.c_str()); cout << "Reading stereo factors" << endl; @@ -112,21 +114,21 @@ int main(int argc, char** argv){ SmartFactor::shared_ptr factor(new SmartFactor(model)); size_t current_l = 3; // hardcoded landmark ID from first measurement - while (factor_file >> x >> l >> uL >> uR >> v >> X >> Y >> Z) { + while (factor_file >> x >> l >> uL >> uR >> v >> _X >> Y >> Z) { if(current_l != l) { graph.push_back(factor); factor = SmartFactor::shared_ptr(new SmartFactor(model)); current_l = l; } - factor->add(StereoPoint2(uL,uR,v), Symbol('x',x), K); + factor->add(StereoPoint2(uL,uR,v), X(x), K); } - Pose3 first_pose = initial_estimate.at(Symbol('x',1)); + Pose3 first_pose = initial_estimate.at(X(1)); //constrain the first pose such that it cannot change from its original value during optimization // NOTE: NonlinearEquality forces the optimizer to use QR rather than Cholesky // QR is much slower than Cholesky, but numerically more stable - graph.emplace_shared >(Symbol('x',1),first_pose); + graph.emplace_shared >(X(1),first_pose); LevenbergMarquardtParams params; params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; @@ -138,13 +140,13 @@ int main(int argc, char** argv){ Values result = optimizer.optimize(); cout << "Final result sample:" << endl; - Values pose_values = result.filter(); + Values pose_values = utilities::allPose3s(result); pose_values.print("Final camera poses:\n"); if(output_poses){ pose3Out.open(poseOutput.c_str(),ios::out); for(size_t i = 1; i<=pose_values.size(); i++){ - pose3Out << i << " " << pose_values.at(Symbol('x',i)).matrix().format(Eigen::IOFormat(Eigen::StreamPrecision, 0, + pose3Out << i << " " << pose_values.at(X(i)).matrix().format(Eigen::IOFormat(Eigen::StreamPrecision, 0, " ", " ")) << endl; } cout << "Writing output" << endl; diff --git a/timing/timeLago.cpp b/timing/timeLago.cpp index c3ee6ff4b..bb506de36 100644 --- a/timing/timeLago.cpp +++ b/timing/timeLago.cpp @@ -10,8 +10,8 @@ * -------------------------------------------------------------------------- */ /** - * @file timeVirtual.cpp - * @brief Time the overhead of using virtual destructors and methods + * @file timeLago.cpp + * @brief Time the LAGO initialization method * @author Richard Roberts * @date Dec 3, 2010 */ @@ -36,16 +36,15 @@ int main(int argc, char *argv[]) { Values::shared_ptr solution; NonlinearFactorGraph::shared_ptr g; string inputFile = findExampleDataFile("w10000"); - SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.05, 0.05, 5.0 * M_PI / 180.0).finished()); + auto model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.05, 0.05, 5.0 * M_PI / 180.0).finished()); boost::tie(g, solution) = load2D(inputFile, model); // add noise to create initial estimate Values initial; - Values::ConstFiltered poses = solution->filter(); - SharedDiagonal noise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 15.0 * M_PI / 180.0).finished()); + auto noise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 15.0 * M_PI / 180.0).finished()); Sampler sampler(noise); - for(const Values::ConstFiltered::KeyValuePair& it: poses) - initial.insert(it.key, it.value.retract(sampler.sample())); + for(const auto& [key,pose]: solution->extract()) + initial.insert(key, pose.retract(sampler.sample())); // Add prior on the pose having index (key) = 0 noiseModel::Diagonal::shared_ptr priorModel = // From 9bca36fd2c5d359be673755ca8e4168c1b0c7e51 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 22 Jan 2023 09:05:04 -0800 Subject: [PATCH 03/51] Move some Eigen methods to cpp --- gtsam/base/SymmetricBlockMatrix.cpp | 18 +++++++++++ gtsam/base/SymmetricBlockMatrix.h | 20 ++---------- gtsam/geometry/Point2.h | 2 +- gtsam/geometry/Pose3.cpp | 6 ++++ gtsam/geometry/Pose3.h | 5 +-- gtsam/geometry/Unit3.cpp | 8 +++++ gtsam/geometry/Unit3.h | 13 ++------ gtsam/linear/LossFunctions.cpp | 4 +++ gtsam/linear/LossFunctions.h | 4 +-- gtsam/linear/NoiseModel.cpp | 49 ++++++++++++++++++++++++++--- gtsam/linear/NoiseModel.h | 40 +++++++---------------- gtsam/linear/PCGSolver.cpp | 11 +++++++ gtsam/linear/PCGSolver.h | 12 ++----- 13 files changed, 115 insertions(+), 77 deletions(-) diff --git a/gtsam/base/SymmetricBlockMatrix.cpp b/gtsam/base/SymmetricBlockMatrix.cpp index 7f41da137..d37bbf7d1 100644 --- a/gtsam/base/SymmetricBlockMatrix.cpp +++ b/gtsam/base/SymmetricBlockMatrix.cpp @@ -24,6 +24,12 @@ namespace gtsam { +/* ************************************************************************* */ +SymmetricBlockMatrix::SymmetricBlockMatrix() : blockStart_(0) { + variableColOffsets_.push_back(0); + assertInvariants(); +} + /* ************************************************************************* */ SymmetricBlockMatrix SymmetricBlockMatrix::LikeActiveViewOf( const SymmetricBlockMatrix& other) { @@ -61,6 +67,18 @@ Matrix SymmetricBlockMatrix::block(DenseIndex I, DenseIndex J) const { } } +/* ************************************************************************* */ +void SymmetricBlockMatrix::negate() { + full().triangularView() *= -1.0; +} + +/* ************************************************************************* */ +void SymmetricBlockMatrix::invertInPlace() { + const auto identity = Matrix::Identity(rows(), rows()); + full().triangularView() = + selfadjointView().llt().solve(identity).triangularView(); +} + /* ************************************************************************* */ void SymmetricBlockMatrix::choleskyPartial(DenseIndex nFrontals) { gttic(VerticalBlockMatrix_choleskyPartial); diff --git a/gtsam/base/SymmetricBlockMatrix.h b/gtsam/base/SymmetricBlockMatrix.h index 302a1ec34..e8afe6e38 100644 --- a/gtsam/base/SymmetricBlockMatrix.h +++ b/gtsam/base/SymmetricBlockMatrix.h @@ -63,12 +63,7 @@ namespace gtsam { public: /// Construct from an empty matrix (asserts that the matrix is empty) - SymmetricBlockMatrix() : - blockStart_(0) - { - variableColOffsets_.push_back(0); - assertInvariants(); - } + SymmetricBlockMatrix(); /// Construct from a container of the sizes of each block. template @@ -265,19 +260,10 @@ namespace gtsam { } /// Negate the entire active matrix. - void negate() { - full().triangularView() *= -1.0; - } + void negate(); /// Invert the entire active matrix in place. - void invertInPlace() { - const auto identity = Matrix::Identity(rows(), rows()); - full().triangularView() = - selfadjointView() - .llt() - .solve(identity) - .triangularView(); - } + void invertInPlace(); /// @} diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index d8b6daca8..21b44ada5 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -45,7 +45,7 @@ typedef std::vector > Point2Vector; /// multiply with scalar inline Point2 operator*(double s, const Point2& p) { - return p * s; + return Point2(s * p.x(), s * p.y()); } /* diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 2652fc073..6a8c61560 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -158,6 +158,12 @@ bool Pose3::equals(const Pose3& pose, double tol) const { return R_.equals(pose.R_, tol) && traits::Equals(t_, pose.t_, tol); } +/* ************************************************************************* */ +Pose3 Pose3::interpolateRt(const Pose3& T, double t) const { + return Pose3(interpolate(R_, T.R_, t), + interpolate(t_, T.t_, t)); +} + /* ************************************************************************* */ /** Modified from Murray94book version (which assumes w and v normalized?) */ Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi) { diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 80741e1c3..678df8376 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -129,10 +129,7 @@ public: * @param T End point of interpolation. * @param t A value in [0, 1]. */ - Pose3 interpolateRt(const Pose3& T, double t) const { - return Pose3(interpolate(R_, T.R_, t), - interpolate(t_, T.t_, t)); - } + Pose3 interpolateRt(const Pose3& T, double t) const; /// @} /// @name Lie Group diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index 6f70d840e..17169a5f5 100644 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -32,6 +32,14 @@ using namespace std; namespace gtsam { +/* ************************************************************************* */ +Unit3::Unit3(const Vector3& p) : p_(p.normalized()) {} + +Unit3::Unit3(double x, double y, double z) : p_(x, y, z) { p_.normalize(); } + +Unit3::Unit3(const Point2& p, double f) : p_(p.x(), p.y(), f) { + p_.normalize(); +} /* ************************************************************************* */ Unit3 Unit3::FromPoint3(const Point3& point, OptionalJacobian<2, 3> H) { // 3*3 Derivative of representation with respect to point is 3*3: diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h index ebd5c63c9..2989159a3 100644 --- a/gtsam/geometry/Unit3.h +++ b/gtsam/geometry/Unit3.h @@ -67,21 +67,14 @@ public: } /// Construct from point - explicit Unit3(const Vector3& p) : - p_(p.normalized()) { - } + explicit Unit3(const Vector3& p); /// Construct from x,y,z - Unit3(double x, double y, double z) : - p_(x, y, z) { - p_.normalize(); - } + Unit3(double x, double y, double z); /// Construct from 2D point in plane at focal length f /// Unit3(p,1) can be viewed as normalized homogeneous coordinates of 2D point - explicit Unit3(const Point2& p, double f) : p_(p.x(), p.y(), f) { - p_.normalize(); - } + explicit Unit3(const Point2& p, double f); /// Copy constructor Unit3(const Unit3& u) { diff --git a/gtsam/linear/LossFunctions.cpp b/gtsam/linear/LossFunctions.cpp index 7307c4a68..64ded7cc3 100644 --- a/gtsam/linear/LossFunctions.cpp +++ b/gtsam/linear/LossFunctions.cpp @@ -40,6 +40,10 @@ Vector Base::weight(const Vector& error) const { return w; } +Vector Base::sqrtWeight(const Vector &error) const { + return weight(error).cwiseSqrt(); +} + // The following three functions re-weight block matrices and a vector // according to their weight implementation diff --git a/gtsam/linear/LossFunctions.h b/gtsam/linear/LossFunctions.h index d9cfc1f3c..ee05cb987 100644 --- a/gtsam/linear/LossFunctions.h +++ b/gtsam/linear/LossFunctions.h @@ -115,9 +115,7 @@ class GTSAM_EXPORT Base { Vector weight(const Vector &error) const; /** square root version of the weight function */ - Vector sqrtWeight(const Vector &error) const { - return weight(error).cwiseSqrt(); - } + Vector sqrtWeight(const Vector &error) const; /** reweight block matrices and a vector according to their weight * implementation */ diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index 7108a7660..8c6b5fd55 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -239,6 +239,8 @@ void Gaussian::WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const whitenInPlace(b); } +Matrix Gaussian::information() const { return R().transpose() * R(); } + /* ************************************************************************* */ // Diagonal /* ************************************************************************* */ @@ -284,6 +286,11 @@ Diagonal::shared_ptr Diagonal::Sigmas(const Vector& sigmas, bool smart) { full: return Diagonal::shared_ptr(new Diagonal(sigmas)); } +/* ************************************************************************* */ +Diagonal::shared_ptr Diagonal::Precisions(const Vector& precisions, + bool smart) { + return Variances(precisions.array().inverse(), smart); +} /* ************************************************************************* */ void Diagonal::print(const string& name) const { gtsam::print(sigmas_, name + "diagonal sigmas "); @@ -294,22 +301,18 @@ Vector Diagonal::whiten(const Vector& v) const { return v.cwiseProduct(invsigmas_); } -/* ************************************************************************* */ Vector Diagonal::unwhiten(const Vector& v) const { return v.cwiseProduct(sigmas_); } -/* ************************************************************************* */ Matrix Diagonal::Whiten(const Matrix& H) const { return vector_scale(invsigmas(), H); } -/* ************************************************************************* */ void Diagonal::WhitenInPlace(Matrix& H) const { vector_scale_inplace(invsigmas(), H); } -/* ************************************************************************* */ void Diagonal::WhitenInPlace(Eigen::Block H) const { H = invsigmas().asDiagonal() * H; } @@ -377,6 +380,32 @@ Vector Constrained::whiten(const Vector& v) const { return c; } +/* ************************************************************************* */ +Constrained::shared_ptr Constrained::MixedSigmas(const Vector& sigmas) { + return MixedSigmas(Vector::Constant(sigmas.size(), 1000.0), sigmas); +} + +Constrained::shared_ptr Constrained::MixedSigmas(double m, + const Vector& sigmas) { + return MixedSigmas(Vector::Constant(sigmas.size(), m), sigmas); +} + +Constrained::shared_ptr Constrained::MixedVariances(const Vector& mu, + const Vector& variances) { + return shared_ptr(new Constrained(mu, variances.cwiseSqrt())); +} +Constrained::shared_ptr Constrained::MixedVariances(const Vector& variances) { + return shared_ptr(new Constrained(variances.cwiseSqrt())); +} + +Constrained::shared_ptr Constrained::MixedPrecisions(const Vector& mu, + const Vector& precisions) { + return MixedVariances(mu, precisions.array().inverse()); +} +Constrained::shared_ptr Constrained::MixedPrecisions(const Vector& precisions) { + return MixedVariances(precisions.array().inverse()); +} + /* ************************************************************************* */ double Constrained::squaredMahalanobisDistance(const Vector& v) const { Vector w = Diagonal::whiten(v); // get noisemodel for constrained elements @@ -623,6 +652,11 @@ void Unit::print(const std::string& name) const { cout << name << "unit (" << dim_ << ") " << endl; } +/* ************************************************************************* */ +double Unit::squaredMahalanobisDistance(const Vector& v) const { + return v.dot(v); +} + /* ************************************************************************* */ // Robust /* ************************************************************************* */ @@ -663,6 +697,13 @@ void Robust::WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const{ robust_->reweight(A1,A2,A3,b); } +Vector Robust::unweightedWhiten(const Vector& v) const { + return noise_->unweightedWhiten(v); +} +double Robust::weight(const Vector& v) const { + return robust_->weight(v.norm()); +} + Robust::shared_ptr Robust::Create( const RobustModel::shared_ptr &robust, const NoiseModel::shared_ptr noise){ return shared_ptr(new Robust(robust,noise)); diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 7bcf808e5..447121848 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -255,7 +255,7 @@ namespace gtsam { virtual Matrix R() const { return thisR();} /// Compute information matrix - virtual Matrix information() const { return R().transpose() * R(); } + virtual Matrix information() const; /// Compute covariance matrix virtual Matrix covariance() const; @@ -319,9 +319,7 @@ namespace gtsam { * A diagonal noise model created by specifying a Vector of precisions, i.e. * i.e. the diagonal of the information matrix, i.e., weights */ - static shared_ptr Precisions(const Vector& precisions, bool smart = true) { - return Variances(precisions.array().inverse(), smart); - } + static shared_ptr Precisions(const Vector& precisions, bool smart = true); void print(const std::string& name) const override; Vector sigmas() const override { return sigmas_; } @@ -426,39 +424,27 @@ namespace gtsam { * A diagonal noise model created by specifying a Vector of * standard devations, some of which might be zero */ - static shared_ptr MixedSigmas(const Vector& sigmas) { - return MixedSigmas(Vector::Constant(sigmas.size(), 1000.0), sigmas); - } + static shared_ptr MixedSigmas(const Vector& sigmas); /** * A diagonal noise model created by specifying a Vector of * standard devations, some of which might be zero */ - static shared_ptr MixedSigmas(double m, const Vector& sigmas) { - return MixedSigmas(Vector::Constant(sigmas.size(), m), sigmas); - } + static shared_ptr MixedSigmas(double m, const Vector& sigmas); /** * A diagonal noise model created by specifying a Vector of * standard devations, some of which might be zero */ - static shared_ptr MixedVariances(const Vector& mu, const Vector& variances) { - return shared_ptr(new Constrained(mu, variances.cwiseSqrt())); - } - static shared_ptr MixedVariances(const Vector& variances) { - return shared_ptr(new Constrained(variances.cwiseSqrt())); - } + static shared_ptr MixedVariances(const Vector& mu, const Vector& variances); + static shared_ptr MixedVariances(const Vector& variances); /** * A diagonal noise model created by specifying a Vector of * precisions, some of which might be inf */ - static shared_ptr MixedPrecisions(const Vector& mu, const Vector& precisions) { - return MixedVariances(mu, precisions.array().inverse()); - } - static shared_ptr MixedPrecisions(const Vector& precisions) { - return MixedVariances(precisions.array().inverse()); - } + static shared_ptr MixedPrecisions(const Vector& mu, const Vector& precisions); + static shared_ptr MixedPrecisions(const Vector& precisions); /** * The squaredMahalanobisDistance function for a constrained noisemodel, @@ -616,7 +602,7 @@ namespace gtsam { bool isUnit() const override { return true; } void print(const std::string& name) const override; - double squaredMahalanobisDistance(const Vector& v) const override {return v.dot(v); } + double squaredMahalanobisDistance(const Vector& v) const override; Vector whiten(const Vector& v) const override { return v; } Vector unwhiten(const Vector& v) const override { return v; } Matrix Whiten(const Matrix& H) const override { return H; } @@ -710,12 +696,8 @@ namespace gtsam { void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const override; void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const override; - Vector unweightedWhiten(const Vector& v) const override { - return noise_->unweightedWhiten(v); - } - double weight(const Vector& v) const override { - return robust_->weight(v.norm()); - } + Vector unweightedWhiten(const Vector& v) const override; + double weight(const Vector& v) const override; static shared_ptr Create( const RobustModel::shared_ptr &robust, const NoiseModel::shared_ptr noise); diff --git a/gtsam/linear/PCGSolver.cpp b/gtsam/linear/PCGSolver.cpp index a7af7d8d8..141412159 100644 --- a/gtsam/linear/PCGSolver.cpp +++ b/gtsam/linear/PCGSolver.cpp @@ -136,6 +136,17 @@ void GaussianFactorGraphSystem::rightPrecondition(const Vector &x, preconditioner_.transposeSolve(x, y); } +/**********************************************************************************/ +void GaussianFactorGraphSystem::scal(const double alpha, Vector &x) const { + x *= alpha; +} +double GaussianFactorGraphSystem::dot(const Vector &x, const Vector &y) const { + return x.dot(y); +} +void GaussianFactorGraphSystem::axpy(const double alpha, const Vector &x, + Vector &y) const { + y += alpha * x; +} /**********************************************************************************/ VectorValues buildVectorValues(const Vector &v, const Ordering &ordering, const map & dimensions) { diff --git a/gtsam/linear/PCGSolver.h b/gtsam/linear/PCGSolver.h index 198b77ec8..cb90ae520 100644 --- a/gtsam/linear/PCGSolver.h +++ b/gtsam/linear/PCGSolver.h @@ -102,15 +102,9 @@ public: void multiply(const Vector &x, Vector& y) const; void leftPrecondition(const Vector &x, Vector &y) const; void rightPrecondition(const Vector &x, Vector &y) const; - inline void scal(const double alpha, Vector &x) const { - x *= alpha; - } - inline double dot(const Vector &x, const Vector &y) const { - return x.dot(y); - } - inline void axpy(const double alpha, const Vector &x, Vector &y) const { - y += alpha * x; - } + void scal(const double alpha, Vector &x) const; + double dot(const Vector &x, const Vector &y) const; + void axpy(const double alpha, const Vector &x, Vector &y) const; void getb(Vector &b) const; }; From ae63321d1b653bda412fae88833299c6e0d4db9f Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 22 Jan 2023 12:18:09 -0800 Subject: [PATCH 04/51] Deprecated boost iterators in Values --- examples/Pose3Localization.cpp | 10 +- examples/Pose3SLAMExample_changeKeys.cpp | 10 +- examples/Pose3SLAMExample_g2o.cpp | 4 +- ...ose3SLAMExample_initializePose3Chordal.cpp | 4 +- ...se3SLAMExample_initializePose3Gradient.cpp | 4 +- examples/SolverComparer.cpp | 6 +- gtsam/hybrid/HybridValues.h | 2 +- gtsam/hybrid/tests/testHybridBayesNet.cpp | 10 +- gtsam/nonlinear/ISAM2-impl.h | 30 ---- gtsam/nonlinear/ISAM2.cpp | 2 +- gtsam/nonlinear/NonlinearFactorGraph.cpp | 4 +- gtsam/nonlinear/Values-inl.h | 16 +- gtsam/nonlinear/Values.cpp | 119 ++++++++++----- gtsam/nonlinear/Values.h | 144 ++++++++++-------- .../internal/LevenbergMarquardtState.h | 7 +- .../tests/testLinearContainerFactor.cpp | 4 +- gtsam/nonlinear/tests/testValues.cpp | 27 +++- gtsam/slam/InitializePose.h | 16 +- gtsam/slam/InitializePose3.cpp | 50 +++--- gtsam/slam/dataset.cpp | 42 ++--- gtsam/slam/tests/testLago.cpp | 14 +- ...ConcurrentFilteringAndSmoothingExample.cpp | 8 +- .../nonlinear/BatchFixedLagSmoother.cpp | 20 +-- .../nonlinear/BatchFixedLagSmoother.h | 4 +- .../nonlinear/ConcurrentBatchFilter.cpp | 23 ++- .../nonlinear/ConcurrentBatchSmoother.cpp | 49 +++--- .../nonlinear/ConcurrentIncrementalFilter.cpp | 12 +- .../ConcurrentIncrementalSmoother.cpp | 23 ++- .../tests/testConcurrentBatchFilter.cpp | 4 +- .../tests/testConcurrentBatchSmoother.cpp | 4 +- .../tests/testConcurrentIncrementalFilter.cpp | 4 +- .../testConcurrentIncrementalSmootherDL.cpp | 8 +- .../testConcurrentIncrementalSmootherGN.cpp | 7 +- 33 files changed, 359 insertions(+), 332 deletions(-) diff --git a/examples/Pose3Localization.cpp b/examples/Pose3Localization.cpp index c18a9e9ce..44076ab38 100644 --- a/examples/Pose3Localization.cpp +++ b/examples/Pose3Localization.cpp @@ -43,9 +43,9 @@ int main(const int argc, const char* argv[]) { auto priorModel = noiseModel::Diagonal::Variances( (Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished()); Key firstKey = 0; - for (const auto key_value : *initial) { + for (const auto key : initial->keys()) { std::cout << "Adding prior to g2o file " << std::endl; - firstKey = key_value.key; + firstKey = key; graph->addPrior(firstKey, Pose3(), priorModel); break; } @@ -74,10 +74,8 @@ int main(const int argc, const char* argv[]) { // Calculate and print marginal covariances for all variables Marginals marginals(*graph, result); - for (const auto key_value : result) { - auto p = dynamic_cast*>(&key_value.value); - if (!p) continue; - std::cout << marginals.marginalCovariance(key_value.key) << endl; + for (const auto& key_pose : result.extract()) { + std::cout << marginals.marginalCovariance(key_pose.first) << endl; } return 0; } diff --git a/examples/Pose3SLAMExample_changeKeys.cpp b/examples/Pose3SLAMExample_changeKeys.cpp index 9aa697f14..7da83d211 100644 --- a/examples/Pose3SLAMExample_changeKeys.cpp +++ b/examples/Pose3SLAMExample_changeKeys.cpp @@ -55,14 +55,14 @@ int main(const int argc, const char *argv[]) { std::cout << "Rewriting input to file: " << inputFileRewritten << std::endl; // Additional: rewrite input with simplified keys 0,1,... Values simpleInitial; - for(const auto key_value: *initial) { + for (const auto k : initial->keys()) { Key key; - if(add) - key = key_value.key + firstKey; + if (add) + key = k + firstKey; else - key = key_value.key - firstKey; + key = k - firstKey; - simpleInitial.insert(key, initial->at(key_value.key)); + simpleInitial.insert(key, initial->at(k)); } NonlinearFactorGraph simpleGraph; for(const boost::shared_ptr& factor: *graph) { diff --git a/examples/Pose3SLAMExample_g2o.cpp b/examples/Pose3SLAMExample_g2o.cpp index 367964307..7ae2978ce 100644 --- a/examples/Pose3SLAMExample_g2o.cpp +++ b/examples/Pose3SLAMExample_g2o.cpp @@ -42,9 +42,9 @@ int main(const int argc, const char* argv[]) { auto priorModel = noiseModel::Diagonal::Variances( (Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished()); Key firstKey = 0; - for (const auto key_value : *initial) { + for (const auto key : initial->keys()) { std::cout << "Adding prior to g2o file " << std::endl; - firstKey = key_value.key; + firstKey = key; graph->addPrior(firstKey, Pose3(), priorModel); break; } diff --git a/examples/Pose3SLAMExample_initializePose3Chordal.cpp b/examples/Pose3SLAMExample_initializePose3Chordal.cpp index 992750fed..03db9fc77 100644 --- a/examples/Pose3SLAMExample_initializePose3Chordal.cpp +++ b/examples/Pose3SLAMExample_initializePose3Chordal.cpp @@ -42,9 +42,9 @@ int main(const int argc, const char* argv[]) { auto priorModel = noiseModel::Diagonal::Variances( (Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished()); Key firstKey = 0; - for (const auto key_value : *initial) { + for (const auto key : initial->keys()) { std::cout << "Adding prior to g2o file " << std::endl; - firstKey = key_value.key; + firstKey = key; graph->addPrior(firstKey, Pose3(), priorModel); break; } diff --git a/examples/Pose3SLAMExample_initializePose3Gradient.cpp b/examples/Pose3SLAMExample_initializePose3Gradient.cpp index 384f290a1..31693c5ff 100644 --- a/examples/Pose3SLAMExample_initializePose3Gradient.cpp +++ b/examples/Pose3SLAMExample_initializePose3Gradient.cpp @@ -42,9 +42,9 @@ int main(const int argc, const char* argv[]) { auto priorModel = noiseModel::Diagonal::Variances( (Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished()); Key firstKey = 0; - for (const auto key_value : *initial) { + for (const auto key : initial->keys()) { std::cout << "Adding prior to g2o file " << std::endl; - firstKey = key_value.key; + firstKey = key; graph->addPrior(firstKey, Pose3(), priorModel); break; } diff --git a/examples/SolverComparer.cpp b/examples/SolverComparer.cpp index 69975b620..3fffc31d0 100644 --- a/examples/SolverComparer.cpp +++ b/examples/SolverComparer.cpp @@ -559,12 +559,12 @@ void runPerturb() // Perturb values VectorValues noise; - for(const Values::KeyValuePair key_val: initial) + for(const auto& key_dim: initial.dims()) { - Vector noisev(key_val.value.dim()); + Vector noisev(key_dim.second); for(Vector::Index i = 0; i < noisev.size(); ++i) noisev(i) = normal(rng); - noise.insert(key_val.key, noisev); + noise.insert(key_dim.first, noisev); } Values perturbed = initial.retract(noise); diff --git a/gtsam/hybrid/HybridValues.h b/gtsam/hybrid/HybridValues.h index 005e3534b..94365db75 100644 --- a/gtsam/hybrid/HybridValues.h +++ b/gtsam/hybrid/HybridValues.h @@ -102,7 +102,7 @@ class GTSAM_EXPORT HybridValues { /// Check whether a variable with key \c j exists in values. bool existsNonlinear(Key j) { - return (nonlinear_.find(j) != nonlinear_.end()); + return nonlinear_.exists(j); }; /// Check whether a variable with key \c j exists. diff --git a/gtsam/hybrid/tests/testHybridBayesNet.cpp b/gtsam/hybrid/tests/testHybridBayesNet.cpp index 068a2031c..425e93415 100644 --- a/gtsam/hybrid/tests/testHybridBayesNet.cpp +++ b/gtsam/hybrid/tests/testHybridBayesNet.cpp @@ -129,10 +129,7 @@ TEST(HybridBayesNet, evaluateHybrid) { TEST(HybridBayesNet, Choose) { Switching s(4); - Ordering ordering; - for (auto&& kvp : s.linearizationPoint) { - ordering += kvp.key; - } + const Ordering ordering(s.linearizationPoint.keys()); HybridBayesNet::shared_ptr hybridBayesNet; HybridGaussianFactorGraph::shared_ptr remainingFactorGraph; @@ -163,10 +160,7 @@ TEST(HybridBayesNet, Choose) { TEST(HybridBayesNet, OptimizeAssignment) { Switching s(4); - Ordering ordering; - for (auto&& kvp : s.linearizationPoint) { - ordering += kvp.key; - } + const Ordering ordering(s.linearizationPoint.keys()); HybridBayesNet::shared_ptr hybridBayesNet; HybridGaussianFactorGraph::shared_ptr remainingFactorGraph; diff --git a/gtsam/nonlinear/ISAM2-impl.h b/gtsam/nonlinear/ISAM2-impl.h index eb2285b28..7b7630582 100644 --- a/gtsam/nonlinear/ISAM2-impl.h +++ b/gtsam/nonlinear/ISAM2-impl.h @@ -436,36 +436,6 @@ struct GTSAM_EXPORT UpdateImpl { } } - /** - * Apply expmap to the given values, but only for indices appearing in - * \c mask. Values are expmapped in-place. - * \param mask Mask on linear indices, only \c true entries are expmapped - */ - static void ExpmapMasked(const VectorValues& delta, const KeySet& mask, - Values* theta) { - gttic(ExpmapMasked); - assert(theta->size() == delta.size()); - Values::iterator key_value; - VectorValues::const_iterator key_delta; -#ifdef GTSAM_USE_TBB - for (key_value = theta->begin(); key_value != theta->end(); ++key_value) { - key_delta = delta.find(key_value->key); -#else - for (key_value = theta->begin(), key_delta = delta.begin(); - key_value != theta->end(); ++key_value, ++key_delta) { - assert(key_value->key == key_delta->first); -#endif - Key var = key_value->key; - assert(static_cast(delta[var].size()) == key_value->value.dim()); - assert(delta[var].allFinite()); - if (mask.exists(var)) { - Value* retracted = key_value->value.retract_(delta[var]); - key_value->value = *retracted; - retracted->deallocate_(); - } - } - } - // Linearize new factors void linearizeNewFactors(const NonlinearFactorGraph& newFactors, const Values& theta, size_t numNonlinearFactors, diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index f56b23777..1c15469cc 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -454,7 +454,7 @@ ISAM2Result ISAM2::update(const NonlinearFactorGraph& newFactors, update.findFluid(roots_, relinKeys, &result.markedKeys, result.details()); // 6. Update linearization point for marked variables: // \Theta_{J}:=\Theta_{J}+\Delta_{J}. - UpdateImpl::ExpmapMasked(delta_, relinKeys, &theta_); + theta_.retractMasked(delta_, relinKeys); } result.variablesRelinearized = result.markedKeys.size(); } diff --git a/gtsam/nonlinear/NonlinearFactorGraph.cpp b/gtsam/nonlinear/NonlinearFactorGraph.cpp index dfa54f26f..84c15c004 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.cpp +++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp @@ -285,8 +285,8 @@ static Scatter scatterFromValues(const Values& values) { scatter.reserve(values.size()); // use "natural" ordering with keys taken from the initial values - for (const auto key_value : values) { - scatter.add(key_value.key, key_value.value.dim()); + for (const auto& key_dim : values.dims()) { + scatter.add(key_dim.first, key_dim.second); } return scatter; diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h index 598963761..474394f7b 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -24,12 +24,10 @@ #pragma once +#include + #include -#include - -#include // Only so Eclipse finds class definition - namespace gtsam { @@ -95,13 +93,13 @@ namespace gtsam { std::map Values::extract(const std::function& filterFcn) const { std::map result; - for (const auto& key_value : *this) { + for (const auto& key_value : values_) { // Check if key matches - if (filterFcn(key_value.key)) { + if (filterFcn(key_value.first)) { // Check if type matches (typically does as symbols matched with types) if (auto t = - dynamic_cast*>(&key_value.value)) - result[key_value.key] = t->value(); + dynamic_cast*>(key_value.second)) + result[key_value.first] = t->value(); } } return result; @@ -109,6 +107,8 @@ namespace gtsam { /* ************************************************************************* */ #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 +#include + template class Values::Filtered { public: diff --git a/gtsam/nonlinear/Values.cpp b/gtsam/nonlinear/Values.cpp index adadc99c0..e3adcc1bf 100644 --- a/gtsam/nonlinear/Values.cpp +++ b/gtsam/nonlinear/Values.cpp @@ -25,8 +25,6 @@ #include #include -#include - #include #include #include @@ -52,15 +50,15 @@ namespace gtsam { /* ************************************************************************* */ Values::Values(const Values& other, const VectorValues& delta) { - for (const_iterator key_value = other.begin(); key_value != other.end(); ++key_value) { - VectorValues::const_iterator it = delta.find(key_value->key); - Key key = key_value->key; // Non-const duplicate to deal with non-const insert argument + for (const auto& key_value : other.values_) { + VectorValues::const_iterator it = delta.find(key_value.first); + Key key = key_value.first; // Non-const duplicate to deal with non-const insert argument if (it != delta.end()) { const Vector& v = it->second; - Value* retractedValue(key_value->value.retract_(v)); // Retract + Value* retractedValue(key_value.second->retract_(v)); // Retract values_.insert(key, retractedValue); // Add retracted result directly to result values } else { - values_.insert(key, key_value->value.clone_()); // Add original version to result values + values_.insert(key, key_value.second->clone_()); // Add original version to result values } } } @@ -69,9 +67,9 @@ namespace gtsam { void Values::print(const string& str, const KeyFormatter& keyFormatter) const { cout << str << (str.empty() ? "" : "\n"); cout << "Values with " << size() << " values:\n"; - for(const_iterator key_value = begin(); key_value != end(); ++key_value) { - cout << "Value " << keyFormatter(key_value->key) << ": "; - key_value->value.print(""); + for (const auto& key_value : values_) { + cout << "Value " << keyFormatter(key_value.first) << ": "; + key_value.second->print(""); cout << "\n"; } } @@ -80,12 +78,12 @@ namespace gtsam { bool Values::equals(const Values& other, double tol) const { if (this->size() != other.size()) return false; - for (const_iterator it1 = this->begin(), it2 = other.begin(); - it1 != this->end(); ++it1, ++it2) { - const Value& value1 = it1->value; - const Value& value2 = it2->value; - if (typeid(value1) != typeid(value2) || it1->key != it2->key - || !value1.equals_(value2, tol)) { + for (auto it1 = values_.begin(), it2 = other.values_.begin(); + it1 != values_.end(); ++it1, ++it2) { + const Value* value1 = it1->second; + const Value* value2 = it2->second; + if (typeid(*value1) != typeid(*value2) || it1->first != it2->first + || !value1->equals_(*value2, tol)) { return false; } } @@ -102,17 +100,44 @@ namespace gtsam { return Values(*this, delta); } + /* ************************************************************************* */ + void Values::retractMasked(const VectorValues& delta, const KeySet& mask) { + gttic(retractMasked); + assert(theta->size() == delta.size()); + auto key_value = values_.begin(); + VectorValues::const_iterator key_delta; +#ifdef GTSAM_USE_TBB + for (; key_value != values_.end(); ++key_value) { + key_delta = delta.find(key_value.first); +#else + for (key_delta = delta.begin(); key_value != values_.end(); + ++key_value, ++key_delta) { + assert(key_value.first == key_delta->first); +#endif + Key var = key_value->first; + assert(static_cast(delta[var].size()) == key_value->second->dim()); + assert(delta[var].allFinite()); + if (mask.exists(var)) { + Value* retracted = key_value->second->retract_(delta[var]); + // TODO(dellaert): can we use std::move here? + *(key_value->second) = *retracted; + retracted->deallocate_(); + } + } + } + /* ************************************************************************* */ VectorValues Values::localCoordinates(const Values& cp) const { if(this->size() != cp.size()) throw DynamicValuesMismatched(); VectorValues result; - for(const_iterator it1=this->begin(), it2=cp.begin(); it1!=this->end(); ++it1, ++it2) { - if(it1->key != it2->key) + for (auto it1 = values_.begin(), it2 = cp.values_.begin(); + it1 != values_.end(); ++it1, ++it2) { + if(it1->first != it2->first) throw DynamicValuesMismatched(); // If keys do not match // Will throw a dynamic_cast exception if types do not match // NOTE: this is separate from localCoordinates(cp, ordering, result) due to at() vs. insert - result.insert(it1->key, it1->value.localCoordinates_(it2->value)); + result.insert(it1->first, it1->second->localCoordinates_(*it2->second)); } return result; } @@ -130,24 +155,26 @@ namespace gtsam { /* ************************************************************************* */ void Values::insert(Key j, const Value& val) { - std::pair insertResult = tryInsert(j, val); + auto insertResult = values_.insert(j, val.clone_()); if(!insertResult.second) throw ValuesKeyAlreadyExists(j); } /* ************************************************************************* */ - void Values::insert(const Values& values) { - for(const_iterator key_value = values.begin(); key_value != values.end(); ++key_value) { - Key key = key_value->key; // Non-const duplicate to deal with non-const insert argument - insert(key, key_value->value); + void Values::insert(const Values& other) { + for (auto key_value = other.values_.begin(); + key_value != other.values_.end(); ++key_value) { + insert(key_value->first, *(key_value->second)); } } /* ************************************************************************* */ +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 std::pair Values::tryInsert(Key j, const Value& value) { std::pair result = values_.insert(j, value.clone_()); return std::make_pair(boost::make_transform_iterator(result.first, &make_deref_pair), result.second); } +#endif /* ************************************************************************* */ void Values::update(Key j, const Value& val) { @@ -165,9 +192,10 @@ namespace gtsam { } /* ************************************************************************* */ - void Values::update(const Values& values) { - for(const_iterator key_value = values.begin(); key_value != values.end(); ++key_value) { - this->update(key_value->key, key_value->value); + void Values::update(const Values& other) { + for (auto key_value = other.values_.begin(); + key_value != other.values_.end(); ++key_value) { + this->update(key_value->first, *(key_value->second)); } } @@ -183,10 +211,10 @@ namespace gtsam { } /* ************************************************************************ */ - void Values::insert_or_assign(const Values& values) { - for (const_iterator key_value = values.begin(); key_value != values.end(); - ++key_value) { - this->insert_or_assign(key_value->key, key_value->value); + void Values::insert_or_assign(const Values& other) { + for (auto key_value = other.values_.begin(); + key_value != other.values_.end(); ++key_value) { + this->insert_or_assign(key_value->first, *(key_value->second)); } } @@ -202,8 +230,16 @@ namespace gtsam { KeyVector Values::keys() const { KeyVector result; result.reserve(size()); - for(const_iterator key_value = begin(); key_value != end(); ++key_value) - result.push_back(key_value->key); + for(const auto& key_value: values_) + result.push_back(key_value.first); + return result; + } + + /* ************************************************************************* */ + KeySet Values::keySet() const { + KeySet result; + for(const auto& key_value: values_) + result.insert(key_value.first); return result; } @@ -217,8 +253,17 @@ namespace gtsam { /* ************************************************************************* */ size_t Values::dim() const { size_t result = 0; - for(const auto key_value: *this) { - result += key_value.value.dim(); + for (const auto key_value : values_) { + result += key_value->second->dim(); + } + return result; + } + + /* ************************************************************************* */ + std::map Values::dims() const { + std::map result; + for (const auto key_value : values_) { + result.emplace(key_value->first, key_value->second->dim()); } return result; } @@ -226,8 +271,8 @@ namespace gtsam { /* ************************************************************************* */ VectorValues Values::zeroVectors() const { VectorValues result; - for(const auto key_value: *this) - result.insert(key_value.key, Vector::Zero(key_value.value.dim())); + for (const auto key_value : values_) + result.insert(key_value->first, Vector::Zero(key_value->second->dim())); return result; } diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index 79ddb0267..299435677 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -28,10 +28,10 @@ #include #include #include -#include #include #include #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 +#include #include #endif @@ -81,10 +81,6 @@ namespace gtsam { // The member to store the values, see just above KeyValueMap values_; - // Types obtained by iterating - typedef KeyValueMap::const_iterator::value_type ConstKeyValuePtrPair; - typedef KeyValueMap::iterator::value_type KeyValuePtrPair; - public: /// A shared_ptr to this class @@ -110,22 +106,6 @@ namespace gtsam { ConstKeyValuePair(const KeyValuePair& kv) : key(kv.key), value(kv.value) {} }; - /// Mutable forward iterator, with value type KeyValuePair - typedef boost::transform_iterator< - std::function, KeyValueMap::iterator> iterator; - - /// Const forward iterator, with value type ConstKeyValuePair - typedef boost::transform_iterator< - std::function, KeyValueMap::const_iterator> const_iterator; - - /// Mutable reverse iterator, with value type KeyValuePair - typedef boost::transform_iterator< - std::function, KeyValueMap::reverse_iterator> reverse_iterator; - - /// Const reverse iterator, with value type ConstKeyValuePair - typedef boost::transform_iterator< - std::function, KeyValueMap::const_reverse_iterator> const_reverse_iterator; - typedef KeyValuePair value_type; /** Default constructor creates an empty Values class */ @@ -191,47 +171,24 @@ namespace gtsam { template boost::optional exists(Key j) const; - /** Find an element by key, returning an iterator, or end() if the key was - * not found. */ - iterator find(Key j) { return boost::make_transform_iterator(values_.find(j), &make_deref_pair); } - - /** Find an element by key, returning an iterator, or end() if the key was - * not found. */ - const_iterator find(Key j) const { return boost::make_transform_iterator(values_.find(j), &make_const_deref_pair); } - - /** Find the element greater than or equal to the specified key. */ - iterator lower_bound(Key j) { return boost::make_transform_iterator(values_.lower_bound(j), &make_deref_pair); } - - /** Find the element greater than or equal to the specified key. */ - const_iterator lower_bound(Key j) const { return boost::make_transform_iterator(values_.lower_bound(j), &make_const_deref_pair); } - - /** Find the lowest-ordered element greater than the specified key. */ - iterator upper_bound(Key j) { return boost::make_transform_iterator(values_.upper_bound(j), &make_deref_pair); } - - /** Find the lowest-ordered element greater than the specified key. */ - const_iterator upper_bound(Key j) const { return boost::make_transform_iterator(values_.upper_bound(j), &make_const_deref_pair); } - /** The number of variables in this config */ size_t size() const { return values_.size(); } /** whether the config is empty */ bool empty() const { return values_.empty(); } - const_iterator begin() const { return boost::make_transform_iterator(values_.begin(), &make_const_deref_pair); } - const_iterator end() const { return boost::make_transform_iterator(values_.end(), &make_const_deref_pair); } - iterator begin() { return boost::make_transform_iterator(values_.begin(), &make_deref_pair); } - iterator end() { return boost::make_transform_iterator(values_.end(), &make_deref_pair); } - const_reverse_iterator rbegin() const { return boost::make_transform_iterator(values_.rbegin(), &make_const_deref_pair); } - const_reverse_iterator rend() const { return boost::make_transform_iterator(values_.rend(), &make_const_deref_pair); } - reverse_iterator rbegin() { return boost::make_transform_iterator(values_.rbegin(), &make_deref_pair); } - reverse_iterator rend() { return boost::make_transform_iterator(values_.rend(), &make_deref_pair); } - /// @name Manifold Operations /// @{ /** Add a delta config to current config and returns a new config */ Values retract(const VectorValues& delta) const; + /** + * Retract, but only for Keys appearing in \c mask. In-place. + * \param mask Mask on Keys where to apply retract. + */ + void retractMasked(const VectorValues& delta, const KeySet& mask); + /** Get a delta config about a linearization point c0 (*this) */ VectorValues localCoordinates(const Values& cp) const; @@ -252,12 +209,6 @@ namespace gtsam { /// version for double void insertDouble(Key j, double c) { insert(j,c); } - /** insert that mimics the STL map insert - if the value already exists, the map is not modified - * and an iterator to the existing value is returned, along with 'false'. If the value did not - * exist, it is inserted and an iterator pointing to the new element, along with 'true', is - * returned. */ - std::pair tryInsert(Key j, const Value& value); - /** single element change of existing element */ void update(Key j, const Value& val); @@ -288,11 +239,16 @@ namespace gtsam { void erase(Key j); /** - * Returns a set of keys in the config + * Returns a vector of keys in the config. * Note: by construction, the list is ordered */ KeyVector keys() const; + /** + * Returns a set of keys in the config. + */ + KeySet keySet() const; + /** Replace all keys and variables */ Values& operator=(const Values& rhs); @@ -305,6 +261,9 @@ namespace gtsam { /** Compute the total dimensionality of all values (\f$ O(n) \f$) */ size_t dim() const; + /** Return all dimensions in a map (\f$ O(n log n) \f$) */ + std::map dims() const; + /** Return a VectorValues of zero vectors for each variable in this Values */ VectorValues zeroVectors() const; @@ -312,8 +271,8 @@ namespace gtsam { template size_t count() const { size_t i = 0; - for (const auto key_value : *this) { - if (dynamic_cast*>(&key_value.value)) + for (const auto key_value : values_) { + if (dynamic_cast*>(key_value.second)) ++i; } return i; @@ -343,6 +302,67 @@ namespace gtsam { extract(const std::function& filterFcn = &_truePredicate) const; #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 + // Types obtained by iterating + typedef KeyValueMap::const_iterator::value_type ConstKeyValuePtrPair; + typedef KeyValueMap::iterator::value_type KeyValuePtrPair; + + /** insert that mimics the STL map insert - if the value already exists, the map is not modified + * and an iterator to the existing value is returned, along with 'false'. If the value did not + * exist, it is inserted and an iterator pointing to the new element, along with 'true', is + * returned. */ + std::pair tryInsert(Key j, const Value& value); + + /// Mutable forward iterator, with value type KeyValuePair + typedef boost::transform_iterator< + std::function, KeyValueMap::iterator> iterator; + + /// Const forward iterator, with value type ConstKeyValuePair + typedef boost::transform_iterator< + std::function, KeyValueMap::const_iterator> const_iterator; + + /// Mutable reverse iterator, with value type KeyValuePair + typedef boost::transform_iterator< + std::function, KeyValueMap::reverse_iterator> reverse_iterator; + + /// Const reverse iterator, with value type ConstKeyValuePair + typedef boost::transform_iterator< + std::function, KeyValueMap::const_reverse_iterator> const_reverse_iterator; + + static ConstKeyValuePair make_const_deref_pair(const KeyValueMap::const_iterator::value_type& key_value) { + return ConstKeyValuePair(key_value.first, *key_value.second); } + + static KeyValuePair make_deref_pair(const KeyValueMap::iterator::value_type& key_value) { + return KeyValuePair(key_value.first, *key_value.second); } + + const_iterator begin() const { return boost::make_transform_iterator(values_.begin(), &make_const_deref_pair); } + const_iterator end() const { return boost::make_transform_iterator(values_.end(), &make_const_deref_pair); } + iterator begin() { return boost::make_transform_iterator(values_.begin(), &make_deref_pair); } + iterator end() { return boost::make_transform_iterator(values_.end(), &make_deref_pair); } + const_reverse_iterator rbegin() const { return boost::make_transform_iterator(values_.rbegin(), &make_const_deref_pair); } + const_reverse_iterator rend() const { return boost::make_transform_iterator(values_.rend(), &make_const_deref_pair); } + reverse_iterator rbegin() { return boost::make_transform_iterator(values_.rbegin(), &make_deref_pair); } + reverse_iterator rend() { return boost::make_transform_iterator(values_.rend(), &make_deref_pair); } + + /** Find an element by key, returning an iterator, or end() if the key was + * not found. */ + iterator find(Key j) { return boost::make_transform_iterator(values_.find(j), &make_deref_pair); } + + /** Find an element by key, returning an iterator, or end() if the key was + * not found. */ + const_iterator find(Key j) const { return boost::make_transform_iterator(values_.find(j), &make_const_deref_pair); } + + /** Find the element greater than or equal to the specified key. */ + iterator lower_bound(Key j) { return boost::make_transform_iterator(values_.lower_bound(j), &make_deref_pair); } + + /** Find the element greater than or equal to the specified key. */ + const_iterator lower_bound(Key j) const { return boost::make_transform_iterator(values_.lower_bound(j), &make_const_deref_pair); } + + /** Find the lowest-ordered element greater than the specified key. */ + iterator upper_bound(Key j) { return boost::make_transform_iterator(values_.upper_bound(j), &make_deref_pair); } + + /** Find the lowest-ordered element greater than the specified key. */ + const_iterator upper_bound(Key j) const { return boost::make_transform_iterator(values_.upper_bound(j), &make_const_deref_pair); } + /** A filtered view of a Values, returned from Values::filter. */ template class Filtered; @@ -395,12 +415,6 @@ namespace gtsam { ar & BOOST_SERIALIZATION_NVP(values_); } - static ConstKeyValuePair make_const_deref_pair(const KeyValueMap::const_iterator::value_type& key_value) { - return ConstKeyValuePair(key_value.first, *key_value.second); } - - static KeyValuePair make_deref_pair(const KeyValueMap::iterator::value_type& key_value) { - return KeyValuePair(key_value.first, *key_value.second); } - }; /* ************************************************************************* */ diff --git a/gtsam/nonlinear/internal/LevenbergMarquardtState.h b/gtsam/nonlinear/internal/LevenbergMarquardtState.h index 75e5a5135..a055f3060 100644 --- a/gtsam/nonlinear/internal/LevenbergMarquardtState.h +++ b/gtsam/nonlinear/internal/LevenbergMarquardtState.h @@ -128,9 +128,10 @@ struct LevenbergMarquardtState : public NonlinearOptimizerState { noiseModelCache.resize(0); // for each of the variables, add a prior damped.reserve(damped.size() + values.size()); - for (const auto key_value : values) { - const Key key = key_value.key; - const size_t dim = key_value.value.dim(); + std::map dims = values.dims(); + for (const auto& key_dim : dims) { + const Key& key = key_dim.first; + const size_t& dim = key_dim.second; const CachedModel* item = getCachedModel(dim); damped += boost::make_shared(key, item->A, item->b, item->model); } diff --git a/gtsam/nonlinear/tests/testLinearContainerFactor.cpp b/gtsam/nonlinear/tests/testLinearContainerFactor.cpp index 22ae4d73e..9bd9ace98 100644 --- a/gtsam/nonlinear/tests/testLinearContainerFactor.cpp +++ b/gtsam/nonlinear/tests/testLinearContainerFactor.cpp @@ -350,8 +350,8 @@ TEST(TestLinearContainerFactor, Rekey) { // For extra fun lets try linearizing this LCF gtsam::Values linearization_pt_rekeyed; - for (auto key_val : linearization_pt) { - linearization_pt_rekeyed.insert(key_map.at(key_val.key), key_val.value); + for (auto key : linearization_pt.keys()) { + linearization_pt_rekeyed.insert(key_map.at(key), linearization_pt.at(key)); } // Check independent values since we don't want to unnecessarily sort diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index 85dd2f4b3..644b8c84f 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -195,6 +195,7 @@ TEST(Values, basic_functions) values.insert(6, M1); values.insert(8, M2); +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 // find EXPECT_LONGS_EQUAL(4, values.find(4)->key); EXPECT_LONGS_EQUAL(4, values_c.find(4)->key); @@ -210,7 +211,7 @@ TEST(Values, basic_functions) EXPECT_LONGS_EQUAL(6, values_c.upper_bound(4)->key); EXPECT_LONGS_EQUAL(4, values.upper_bound(3)->key); EXPECT_LONGS_EQUAL(4, values_c.upper_bound(3)->key); - +#endif } /* ************************************************************************* */ @@ -220,8 +221,8 @@ TEST(Values, retract_full) config0.insert(key1, Vector3(1.0, 2.0, 3.0)); config0.insert(key2, Vector3(5.0, 6.0, 7.0)); - VectorValues delta {{key1, Vector3(1.0, 1.1, 1.2)}, - {key2, Vector3(1.3, 1.4, 1.5)}}; + const VectorValues delta{{key1, Vector3(1.0, 1.1, 1.2)}, + {key2, Vector3(1.3, 1.4, 1.5)}}; Values expected; expected.insert(key1, Vector3(2.0, 3.1, 4.2)); @@ -238,7 +239,7 @@ TEST(Values, retract_partial) config0.insert(key1, Vector3(1.0, 2.0, 3.0)); config0.insert(key2, Vector3(5.0, 6.0, 7.0)); - VectorValues delta {{key2, Vector3(1.3, 1.4, 1.5)}}; + const VectorValues delta{{key2, Vector3(1.3, 1.4, 1.5)}}; Values expected; expected.insert(key1, Vector3(1.0, 2.0, 3.0)); @@ -248,6 +249,24 @@ TEST(Values, retract_partial) CHECK(assert_equal(expected, Values(config0, delta))); } +/* ************************************************************************* */ +TEST(Values, retract_masked) +{ + Values config0; + config0.insert(key1, Vector3(1.0, 2.0, 3.0)); + config0.insert(key2, Vector3(5.0, 6.0, 7.0)); + + const VectorValues delta{{key1, Vector3(1.0, 1.1, 1.2)}, + {key2, Vector3(1.3, 1.4, 1.5)}}; + + Values expected; + expected.insert(key1, Vector3(1.0, 2.0, 3.0)); + expected.insert(key2, Vector3(6.3, 7.4, 8.5)); + + config0.retractMasked(delta, {key2}); + CHECK(assert_equal(expected, config0)); +} + /* ************************************************************************* */ TEST(Values, equals) { diff --git a/gtsam/slam/InitializePose.h b/gtsam/slam/InitializePose.h index f9b99e47e..79e3fe813 100644 --- a/gtsam/slam/InitializePose.h +++ b/gtsam/slam/InitializePose.h @@ -62,10 +62,10 @@ static Values computePoses(const Values& initialRot, // Upgrade rotations to full poses Values initialPose; - for (const auto key_value : initialRot) { - Key key = key_value.key; - const auto& rot = initialRot.at(key); - Pose initializedPose = Pose(rot, origin); + for (const auto& key_rot : initialRot.extract()) { + const Key& key = key_rot.first; + const auto& rot = key_rot.second; + const Pose initializedPose(rot, origin); initialPose.insert(key, initializedPose); } @@ -82,14 +82,14 @@ static Values computePoses(const Values& initialRot, params.setVerbosity("TERMINATION"); } GaussNewtonOptimizer optimizer(*posegraph, initialPose, params); - Values GNresult = optimizer.optimize(); + const Values GNresult = optimizer.optimize(); // put into Values structure Values estimate; - for (const auto key_value : GNresult) { - Key key = key_value.key; + for (const auto& key_pose : GNresult.extract()) { + const Key& key = key_pose.first; if (key != kAnchorKey) { - const Pose& pose = GNresult.at(key); + const Pose& pose = key_pose.second; estimate.insert(key, pose); } } diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp index 6bb1b0f36..e8ec9181c 100644 --- a/gtsam/slam/InitializePose3.cpp +++ b/gtsam/slam/InitializePose3.cpp @@ -122,12 +122,12 @@ Values InitializePose3::computeOrientationsGradient( gttic(InitializePose3_computeOrientationsGradient); // this works on the inverse rotations, according to Tron&Vidal,2011 - Values inverseRot; - inverseRot.insert(initialize::kAnchorKey, Rot3()); - for(const auto key_value: givenGuess) { - Key key = key_value.key; - const Pose3& pose = givenGuess.at(key); - inverseRot.insert(key, pose.rotation().inverse()); + std::map inverseRot; + inverseRot.emplace(initialize::kAnchorKey, Rot3()); + for(const auto& key_pose: givenGuess.extract()) { + const Key& key = key_pose.first; + const Pose3& pose = key_pose.second; + inverseRot.emplace(key, pose.rotation().inverse()); } // Create the map of edges incident on each node @@ -138,10 +138,8 @@ Values InitializePose3::computeOrientationsGradient( // calculate max node degree & allocate gradient size_t maxNodeDeg = 0; - VectorValues grad; - for(const auto key_value: inverseRot) { - Key key = key_value.key; - grad.insert(key,Z_3x1); + for (const auto& key_R : inverseRot) { + const Key& key = key_R.first; size_t currNodeDeg = (adjEdgesMap.at(key)).size(); if(currNodeDeg > maxNodeDeg) maxNodeDeg = currNodeDeg; @@ -162,28 +160,29 @@ Values InitializePose3::computeOrientationsGradient( ////////////////////////////////////////////////////////////////////////// // compute the gradient at each node maxGrad = 0; - for (const auto key_value : inverseRot) { - Key key = key_value.key; + VectorValues grad; + for (const auto& key_R : inverseRot) { + const Key& key = key_R.first; + const Rot3& Ri = key_R.second; Vector gradKey = Z_3x1; // collect the gradient for each edge incident on key for (const size_t& factorId : adjEdgesMap.at(key)) { - Rot3 Rij = factorId2RotMap.at(factorId); - Rot3 Ri = inverseRot.at(key); + const Rot3& Rij = factorId2RotMap.at(factorId); auto factor = pose3Graph.at(factorId); const auto& keys = factor->keys(); if (key == keys[0]) { Key key1 = keys[1]; - Rot3 Rj = inverseRot.at(key1); + const Rot3& Rj = inverseRot.at(key1); gradKey = gradKey + gradientTron(Ri, Rij * Rj, a, b); } else if (key == keys[1]) { Key key0 = keys[0]; - Rot3 Rj = inverseRot.at(key0); + const Rot3& Rj = inverseRot.at(key0); gradKey = gradKey + gradientTron(Ri, Rij.between(Rj), a, b); } else { cout << "Error in gradient computation" << endl; } } // end of i-th gradient computation - grad.at(key) = stepsize * gradKey; + grad.insert(key, stepsize * gradKey); double normGradKey = (gradKey).norm(); if(normGradKey>maxGrad) @@ -192,8 +191,12 @@ Values InitializePose3::computeOrientationsGradient( ////////////////////////////////////////////////////////////////////////// // update estimates - inverseRot = inverseRot.retract(grad); - + for (auto& key_R : inverseRot) { + const Key& key = key_R.first; + Rot3& Ri = key_R.second; + Ri = Ri.retract(grad.at(key)); + } + ////////////////////////////////////////////////////////////////////////// // check stopping condition if (it>20 && maxGrad < 5e-3) @@ -201,12 +204,13 @@ Values InitializePose3::computeOrientationsGradient( } // enf of gradient iterations // Return correct rotations - const Rot3& Rref = inverseRot.at(initialize::kAnchorKey); // This will be set to the identity as so far we included no prior + const Rot3& Rref = inverseRot.at(initialize::kAnchorKey); // This will be set to the identity as so far we included no prior Values estimateRot; - for(const auto key_value: inverseRot) { - Key key = key_value.key; + for (const auto key_R : inverseRot) { + const Key& key = key_R.first; + const Rot3& Ri = key_R.second; if (key != initialize::kAnchorKey) { - const Rot3& R = inverseRot.at(key); + const Rot3& R = inverseRot.at(key); if(setRefFrame) estimateRot.insert(key, Rref.compose(R.inverse())); else diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index db79fcd3c..9cb12aefb 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -586,9 +586,9 @@ void save2D(const NonlinearFactorGraph &graph, const Values &config, fstream stream(filename.c_str(), fstream::out); // save poses - for (const auto key_value : config) { - const Pose2 &pose = key_value.value.cast(); - stream << "VERTEX2 " << key_value.key << " " << pose.x() << " " << pose.y() + for (const auto &key_pose : config.extract()) { + const Pose2 &pose = key_pose.second; + stream << "VERTEX2 " << key_pose.first << " " << pose.x() << " " << pose.y() << " " << pose.theta() << endl; } @@ -635,45 +635,33 @@ void writeG2o(const NonlinearFactorGraph &graph, const Values &estimate, auto index = [](gtsam::Key key) { return Symbol(key).index(); }; // save 2D poses - for (const auto key_value : estimate) { - auto p = dynamic_cast *>(&key_value.value); - if (!p) - continue; - const Pose2 &pose = p->value(); - stream << "VERTEX_SE2 " << index(key_value.key) << " " << pose.x() << " " + for (const auto &pair : estimate.extract()) { + const Pose2 &pose = pair.second; + stream << "VERTEX_SE2 " << index(pair.first) << " " << pose.x() << " " << pose.y() << " " << pose.theta() << endl; } // save 3D poses - for (const auto key_value : estimate) { - auto p = dynamic_cast *>(&key_value.value); - if (!p) - continue; - const Pose3 &pose = p->value(); + for (const auto &pair : estimate.extract()) { + const Pose3 &pose = pair.second; const Point3 t = pose.translation(); const auto q = pose.rotation().toQuaternion(); - stream << "VERTEX_SE3:QUAT " << index(key_value.key) << " " << t.x() << " " + stream << "VERTEX_SE3:QUAT " << index(pair.first) << " " << t.x() << " " << t.y() << " " << t.z() << " " << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << endl; } // save 2D landmarks - for (const auto key_value : estimate) { - auto p = dynamic_cast *>(&key_value.value); - if (!p) - continue; - const Point2 &point = p->value(); - stream << "VERTEX_XY " << index(key_value.key) << " " << point.x() << " " + for (const auto &pair : estimate.extract()) { + const Point2 &point = pair.second; + stream << "VERTEX_XY " << index(pair.first) << " " << point.x() << " " << point.y() << endl; } // save 3D landmarks - for (const auto key_value : estimate) { - auto p = dynamic_cast *>(&key_value.value); - if (!p) - continue; - const Point3 &point = p->value(); - stream << "VERTEX_TRACKXYZ " << index(key_value.key) << " " << point.x() + for (const auto &pair : estimate.extract()) { + const Point3 &point = pair.second; + stream << "VERTEX_TRACKXYZ " << index(pair.first) << " " << point.x() << " " << point.y() << " " << point.z() << endl; } diff --git a/gtsam/slam/tests/testLago.cpp b/gtsam/slam/tests/testLago.cpp index 0b74f06d7..5d429796d 100644 --- a/gtsam/slam/tests/testLago.cpp +++ b/gtsam/slam/tests/testLago.cpp @@ -283,9 +283,10 @@ TEST( Lago, largeGraphNoisy_orientations ) { Values::shared_ptr expected; boost::tie(gmatlab, expected) = readG2o(matlabFile); - for(const auto key_val: *expected){ - Key k = key_val.key; - EXPECT(assert_equal(expected->at(k), actual.at(k), 1e-5)); + for(const auto key_pose: expected->extract()){ + const Key& k = key_pose.first; + const Pose2& pose = key_pose.second; + EXPECT(assert_equal(pose, actual.at(k), 1e-5)); } } @@ -309,9 +310,10 @@ TEST( Lago, largeGraphNoisy ) { Values::shared_ptr expected; boost::tie(gmatlab, expected) = readG2o(matlabFile); - for(const auto key_val: *expected){ - Key k = key_val.key; - EXPECT(assert_equal(expected->at(k), actual.at(k), 1e-2)); + for(const auto key_pose: expected->extract()){ + const Key& k = key_pose.first; + const Pose2& pose = key_pose.second; + EXPECT(assert_equal(pose, actual.at(k), 1e-2)); } } diff --git a/gtsam_unstable/examples/ConcurrentFilteringAndSmoothingExample.cpp b/gtsam_unstable/examples/ConcurrentFilteringAndSmoothingExample.cpp index 1494d784b..52a45b6d0 100644 --- a/gtsam_unstable/examples/ConcurrentFilteringAndSmoothingExample.cpp +++ b/gtsam_unstable/examples/ConcurrentFilteringAndSmoothingExample.cpp @@ -308,12 +308,12 @@ int main(int argc, char** argv) { // And to demonstrate the fixed-lag aspect, print the keys contained in each smoother after 3.0 seconds cout << "After 15.0 seconds, each version contains to the following keys:" << endl; cout << " Concurrent Filter Keys: " << endl; - for(const auto key_value: concurrentFilter.getLinearizationPoint()) { - cout << setprecision(5) << " Key: " << key_value.key << endl; + for(const auto key: concurrentFilter.getLinearizationPoint().keys()) { + cout << setprecision(5) << " Key: " << key << endl; } cout << " Concurrent Smoother Keys: " << endl; - for(const auto key_value: concurrentSmoother.getLinearizationPoint()) { - cout << setprecision(5) << " Key: " << key_value.key << endl; + for(const auto key: concurrentSmoother.getLinearizationPoint().keys()) { + cout << setprecision(5) << " Key: " << key << endl; } cout << " Fixed-Lag Smoother Keys: " << endl; for(const auto& key_timestamp: fixedlagSmoother.timestamps()) { diff --git a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp index 18c664934..f5280ceff 100644 --- a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp @@ -59,8 +59,8 @@ FixedLagSmoother::Result BatchFixedLagSmoother::update( // Add the new variables to theta theta_.insert(newTheta); // Add new variables to the end of the ordering - for (const auto key_value : newTheta) { - ordering_.push_back(key_value.key); + for (const auto key : newTheta.keys()) { + ordering_.push_back(key); } // Augment Delta delta_.insert(newTheta.zeroVectors()); @@ -161,8 +161,8 @@ void BatchFixedLagSmoother::eraseKeys(const KeyVector& keys) { factorIndex_.erase(key); // Erase the key from the set of linearized keys - if (linearKeys_.exists(key)) { - linearKeys_.erase(key); + if (linearValues_.exists(key)) { + linearValues_.erase(key); } } @@ -186,8 +186,8 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() { // Create output result structure Result result; - result.nonlinearVariables = theta_.size() - linearKeys_.size(); - result.linearVariables = linearKeys_.size(); + result.nonlinearVariables = theta_.size() - linearValues_.size(); + result.linearVariables = linearValues_.size(); // Set optimization parameters double lambda = parameters_.lambdaInitial; @@ -265,10 +265,10 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() { // Reset the deltas to zeros delta_.setZero(); // Put the linearization points and deltas back for specific variables - if (enforceConsistency_ && (linearKeys_.size() > 0)) { - theta_.update(linearKeys_); - for(const auto key_value: linearKeys_) { - delta_.at(key_value.key) = newDelta.at(key_value.key); + if (enforceConsistency_ && (linearValues_.size() > 0)) { + theta_.update(linearValues_); + for(const auto key: linearValues_.keys()) { + delta_.at(key) = newDelta.at(key); } } // Decrease lambda for next time diff --git a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h index 94cf130cf..79bd22f9d 100644 --- a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h +++ b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h @@ -136,8 +136,8 @@ protected: /** The current linearization point **/ Values theta_; - /** The set of keys involved in current linear factors. These keys should not be relinearized. **/ - Values linearKeys_; + /** The set of values involved in current linear factors. **/ + Values linearValues_; /** The current ordering */ Ordering ordering_; diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp index 83d0ab719..a31a75920 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp @@ -139,8 +139,8 @@ ConcurrentBatchFilter::Result ConcurrentBatchFilter::update(const NonlinearFacto // Add the new variables to theta theta_.insert(newTheta); // Add new variables to the end of the ordering - for(const auto key_value: newTheta) { - ordering_.push_back(key_value.key); + for (const auto key : newTheta.keys()) { + ordering_.push_back(key); } // Augment Delta delta_.insert(newTheta.zeroVectors()); @@ -221,10 +221,7 @@ void ConcurrentBatchFilter::synchronize(const NonlinearFactorGraph& smootherSumm if(debug) { PrintNonlinearFactorGraph(smootherSummarization_, "ConcurrentBatchFilter::synchronize ", "Updated Smoother Summarization:", DefaultKeyFormatter); } // Find the set of new separator keys - KeySet newSeparatorKeys; - for(const auto key_value: separatorValues_) { - newSeparatorKeys.insert(key_value.key); - } + const KeySet newSeparatorKeys = separatorValues_.keySet(); if(debug) { PrintKeys(newSeparatorKeys, "ConcurrentBatchFilter::synchronize ", "Current Separator Keys:"); } @@ -236,9 +233,9 @@ void ConcurrentBatchFilter::synchronize(const NonlinearFactorGraph& smootherSumm graph.push_back(smootherShortcut_); Values values; values.insert(smootherSummarizationValues); - for(const auto key_value: separatorValues_) { - if(!values.exists(key_value.key)) { - values.insert(key_value.key, key_value.value); + for(const auto key: newSeparatorKeys) { + if(!values.exists(key)) { + values.insert(key, separatorValues_.at(key)); } } // Calculate the summarized factor on just the new separator keys @@ -471,8 +468,8 @@ void ConcurrentBatchFilter::optimize(const NonlinearFactorGraph& factors, Values // Put the linearization points and deltas back for specific variables if(linearValues.size() > 0) { theta.update(linearValues); - for(const auto key_value: linearValues) { - delta.at(key_value.key) = newDelta.at(key_value.key); + for(const auto key: linearValues.keys()) { + delta.at(key) = newDelta.at(key); } } @@ -574,9 +571,7 @@ void ConcurrentBatchFilter::moveSeparator(const FastList& keysToMove) { // Calculate the set of new separator keys: AffectedKeys + PreviousSeparatorKeys - KeysToMove KeySet newSeparatorKeys = removedFactors.keys(); - for(const auto key_value: separatorValues_) { - newSeparatorKeys.insert(key_value.key); - } + newSeparatorKeys.merge(separatorValues_.keySet()); for(Key key: keysToMove) { newSeparatorKeys.erase(key); } diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp index 75d491bde..94a7d4c22 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp @@ -61,8 +61,8 @@ ConcurrentBatchSmoother::Result ConcurrentBatchSmoother::update(const NonlinearF theta_.insert(newTheta); // Add new variables to the end of the ordering - for(const auto key_value: newTheta) { - ordering_.push_back(key_value.key); + for(const auto key: newTheta.keys()) { + ordering_.push_back(key); } // Augment Delta @@ -135,26 +135,30 @@ void ConcurrentBatchSmoother::synchronize(const NonlinearFactorGraph& smootherFa removeFactors(filterSummarizationSlots_); // Insert new linpoints into the values, augment the ordering, and store new dims to augment delta - for(const auto key_value: smootherValues) { - std::pair iter_inserted = theta_.tryInsert(key_value.key, key_value.value); - if(iter_inserted.second) { - // If the insert succeeded - ordering_.push_back(key_value.key); - delta_.insert(key_value.key, Vector::Zero(key_value.value.dim())); + for(const auto key: smootherValues.keys()) { + if(!theta_.exists(key)) { + // If this a new key for theta_, also add to ordering and delta. + const auto& value = smootherValues.at(key); + delta_.insert(key, Vector::Zero(value.dim())); + theta_.insert(key, value); + ordering_.push_back(key); } else { - // If the element already existed in theta_ - iter_inserted.first->value = key_value.value; + // If the key already existed in theta_, just update. + const auto& value = smootherValues.at(key); + theta_.update(key, value); } } - for(const auto key_value: separatorValues) { - std::pair iter_inserted = theta_.tryInsert(key_value.key, key_value.value); - if(iter_inserted.second) { - // If the insert succeeded - ordering_.push_back(key_value.key); - delta_.insert(key_value.key, Vector::Zero(key_value.value.dim())); + for(const auto key: separatorValues.keys()) { + if(!theta_.exists(key)) { + // If this a new key for theta_, also add to ordering and delta. + const auto& value = separatorValues.at(key); + delta_.insert(key, Vector::Zero(value.dim())); + theta_.insert(key, value); + ordering_.push_back(key); } else { - // If the element already existed in theta_ - iter_inserted.first->value = key_value.value; + // If the key already existed in theta_, just update. + const auto& value = separatorValues.at(key); + theta_.update(key, value); } } @@ -322,8 +326,8 @@ ConcurrentBatchSmoother::Result ConcurrentBatchSmoother::optimize() { // Put the linearization points and deltas back for specific variables if(separatorValues_.size() > 0) { theta_.update(separatorValues_); - for(const auto key_value: separatorValues_) { - delta_.at(key_value.key) = newDelta.at(key_value.key); + for(const auto key: separatorValues_.keys()) { + delta_.at(key) = newDelta.at(key); } } @@ -371,10 +375,7 @@ void ConcurrentBatchSmoother::updateSmootherSummarization() { } // Get the set of separator keys - gtsam::KeySet separatorKeys; - for(const auto key_value: separatorValues_) { - separatorKeys.insert(key_value.key); - } + const KeySet separatorKeys = separatorValues_.keySet(); // Calculate the marginal factors on the separator smootherSummarization_ = internal::calculateMarginalFactors(graph, theta_, separatorKeys, parameters_.getEliminationFunction()); diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp index b9cf66a97..947c02cc0 100644 --- a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp @@ -69,14 +69,14 @@ ConcurrentIncrementalFilter::Result ConcurrentIncrementalFilter::update(const No int group = 1; // Set all existing variables to Group1 if(isam2_.getLinearizationPoint().size() > 0) { - for(const auto key_value: isam2_.getLinearizationPoint()) { - orderingConstraints->operator[](key_value.key) = group; + for(const auto key: isam2_.getLinearizationPoint().keys()) { + orderingConstraints->operator[](key) = group; } ++group; } // Assign new variables to the root - for(const auto key_value: newTheta) { - orderingConstraints->operator[](key_value.key) = group; + for(const auto key: newTheta.keys()) { + orderingConstraints->operator[](key) = group; } // Set marginalizable variables to Group0 for(Key key: *keysToMove){ @@ -201,8 +201,8 @@ void ConcurrentIncrementalFilter::synchronize(const NonlinearFactorGraph& smooth // Force iSAM2 not to relinearize anything during this iteration FastList noRelinKeys; - for(const auto key_value: isam2_.getLinearizationPoint()) { - noRelinKeys.push_back(key_value.key); + for(const auto key: isam2_.getLinearizationPoint().keys()) { + noRelinKeys.push_back(key); } // Calculate the summarized factor on just the new separator keys diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp b/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp index 3886d0e42..b82b08048 100644 --- a/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp @@ -66,9 +66,9 @@ ConcurrentIncrementalSmoother::Result ConcurrentIncrementalSmoother::update(cons // Also, mark the separator keys as fixed linearization points FastMap constrainedKeys; FastList noRelinKeys; - for(const auto key_value: separatorValues_) { - constrainedKeys[key_value.key] = 1; - noRelinKeys.push_back(key_value.key); + for (const auto key : separatorValues_.keys()) { + constrainedKeys[key] = 1; + noRelinKeys.push_back(key); } // Use iSAM2 to perform an update @@ -82,14 +82,14 @@ ConcurrentIncrementalSmoother::Result ConcurrentIncrementalSmoother::update(cons Values values(newTheta); // Unfortunately, we must be careful here, as some of the smoother values // and/or separator values may have been added previously - for(const auto key_value: smootherValues_) { - if(!isam2_.getLinearizationPoint().exists(key_value.key)) { - values.insert(key_value.key, smootherValues_.at(key_value.key)); + for(const auto key: smootherValues_.keys()) { + if(!isam2_.getLinearizationPoint().exists(key)) { + values.insert(key, smootherValues_.at(key)); } } - for(const auto key_value: separatorValues_) { - if(!isam2_.getLinearizationPoint().exists(key_value.key)) { - values.insert(key_value.key, separatorValues_.at(key_value.key)); + for(const auto key: separatorValues_.keys()) { + if(!isam2_.getLinearizationPoint().exists(key)) { + values.insert(key, separatorValues_.at(key)); } } @@ -245,10 +245,7 @@ void ConcurrentIncrementalSmoother::updateSmootherSummarization() { } // Get the set of separator keys - KeySet separatorKeys; - for(const auto key_value: separatorValues_) { - separatorKeys.insert(key_value.key); - } + const KeySet separatorKeys = separatorValues_.keySet(); // Calculate the marginal factors on the separator smootherSummarization_ = internal::calculateMarginalFactors(graph, isam2_.getLinearizationPoint(), separatorKeys, diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp index 61db05167..15038c23f 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp @@ -64,8 +64,8 @@ NonlinearFactorGraph CalculateMarginals(const NonlinearFactorGraph& factorGraph, std::set KeysToKeep; - for(const auto key_value: linPoint) { // we cycle over all the keys of factorGraph - KeysToKeep.insert(key_value.key); + for(const auto key: linPoint.keys()) { // we cycle over all the keys of factorGraph + KeysToKeep.insert(key); } // so far we are keeping all keys, but we want to delete the ones that we are going to marginalize for(Key key: keysToMarginalize) { KeysToKeep.erase(key); diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp index b5fb61428..a2733d509 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp @@ -560,8 +560,8 @@ TEST( ConcurrentBatchSmoother, synchronize_3 ) GaussianFactorGraph::shared_ptr linearFactors = allFactors.linearize(allValues); KeySet eliminateKeys = linearFactors->keys(); - for(const auto key_value: filterSeparatorValues) { - eliminateKeys.erase(key_value.key); + for(const auto key: filterSeparatorValues.keys()) { + eliminateKeys.erase(key); } KeyVector variables(eliminateKeys.begin(), eliminateKeys.end()); GaussianFactorGraph result = *linearFactors->eliminatePartialMultifrontal(variables, EliminateCholesky).second; diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp index 08d71a420..7c6a08278 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp @@ -80,8 +80,8 @@ NonlinearFactorGraph CalculateMarginals(const NonlinearFactorGraph& factorGraph, std::set KeysToKeep; - for(const auto key_value: linPoint) { // we cycle over all the keys of factorGraph - KeysToKeep.insert(key_value.key); + for(const auto key: linPoint.keys()) { // we cycle over all the keys of factorGraph + KeysToKeep.insert(key); } // so far we are keeping all keys, but we want to delete the ones that we are going to marginalize for(Key key: keysToMarginalize) { KeysToKeep.erase(key); diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp index ccb5a104e..0e01112eb 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp @@ -512,8 +512,8 @@ TEST( ConcurrentIncrementalSmootherDL, synchronize_2 ) // Values expectedLinearizationPoint = BatchOptimize(allFactors, allValues, 1); Values expectedLinearizationPoint = filterSeparatorValues; Values actualLinearizationPoint; - for(const auto key_value: filterSeparatorValues) { - actualLinearizationPoint.insert(key_value.key, smoother.getLinearizationPoint().at(key_value.key)); + for(const auto key: filterSeparatorValues.keys()) { + actualLinearizationPoint.insert(key, smoother.getLinearizationPoint().at(key)); } CHECK(assert_equal(expectedLinearizationPoint, actualLinearizationPoint, 1e-6)); } @@ -580,8 +580,8 @@ TEST( ConcurrentIncrementalSmootherDL, synchronize_3 ) // GaussianBayesNet::shared_ptr GBNsptr = GSS.eliminate(); KeySet allkeys = LinFactorGraph->keys(); - for(const auto key_value: filterSeparatorValues) { - allkeys.erase(key_value.key); + for(const auto key: filterSeparatorValues.keys()) { + allkeys.erase(key); } KeyVector variables(allkeys.begin(), allkeys.end()); std::pair result = LinFactorGraph->eliminatePartialSequential(variables, EliminateCholesky); diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp index 53c3d1610..a33fcc481 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp @@ -513,8 +513,8 @@ TEST( ConcurrentIncrementalSmootherGN, synchronize_2 ) // Values expectedLinearizationPoint = BatchOptimize(allFactors, allValues, 1); Values expectedLinearizationPoint = filterSeparatorValues; Values actualLinearizationPoint; - for(const auto key_value: filterSeparatorValues) { - actualLinearizationPoint.insert(key_value.key, smoother.getLinearizationPoint().at(key_value.key)); + for(const auto key: filterSeparatorValues.keys()) { + actualLinearizationPoint.insert(key, smoother.getLinearizationPoint().at(key)); } CHECK(assert_equal(expectedLinearizationPoint, actualLinearizationPoint, 1e-6)); } @@ -582,8 +582,7 @@ TEST( ConcurrentIncrementalSmootherGN, synchronize_3 ) // GaussianBayesNet::shared_ptr GBNsptr = GSS.eliminate(); KeySet allkeys = LinFactorGraph->keys(); - for(const auto key_value: filterSeparatorValues) - allkeys.erase(key_value.key); + for (const auto key : filterSeparatorValues.keys()) allkeys.erase(key); KeyVector variables(allkeys.begin(), allkeys.end()); std::pair result = LinFactorGraph->eliminatePartialSequential(variables, EliminateCholesky); From c681628cd08412a315bfda7188d43509e96ac67f Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 21 Jan 2023 20:26:06 -0800 Subject: [PATCH 05/51] Fixed some stragglers in timing --- gtsam_unstable/timing/timeDSFvariants.cpp | 3 +-- timing/timeCameraExpression.cpp | 1 - timing/timeSchurFactors.cpp | 4 ++-- 3 files changed, 3 insertions(+), 5 deletions(-) diff --git a/gtsam_unstable/timing/timeDSFvariants.cpp b/gtsam_unstable/timing/timeDSFvariants.cpp index 3da01bfaf..35ae7d4d5 100644 --- a/gtsam_unstable/timing/timeDSFvariants.cpp +++ b/gtsam_unstable/timing/timeDSFvariants.cpp @@ -40,8 +40,7 @@ int main(int argc, char* argv[]) { os << "images,points,matches,Base,Map,BTree" << endl; // loop over number of images - vector ms; - ms += 10, 20, 30, 40, 50, 100, 200, 300, 400, 500, 1000; + vector ms {10, 20, 30, 40, 50, 100, 200, 300, 400, 500, 1000}; for(size_t m: ms) { // We use volatile here to make these appear to the optimizing compiler as // if their values are only known at run-time. diff --git a/timing/timeCameraExpression.cpp b/timing/timeCameraExpression.cpp index 07eeb1bcb..509294999 100644 --- a/timing/timeCameraExpression.cpp +++ b/timing/timeCameraExpression.cpp @@ -100,7 +100,6 @@ int main() { // Oct 3, 2014, Macbook Air // 9.0 musecs/call typedef PinholeCamera Camera; - typedef Expression Camera_; NonlinearFactor::shared_ptr g3 = boost::make_shared >(model, z, Point2_(myProject, x, p)); diff --git a/timing/timeSchurFactors.cpp b/timing/timeSchurFactors.cpp index 370486c5f..95296e80e 100644 --- a/timing/timeSchurFactors.cpp +++ b/timing/timeSchurFactors.cpp @@ -101,7 +101,7 @@ void timeAll(size_t m, size_t N) { { // Raw memory Version FastVector < Key > keys; for (size_t i = 0; i < m; i++) - keys += i; + keys.push_back(i); Vector x = xvalues.vector(keys); double* xdata = x.data(); @@ -144,7 +144,7 @@ int main(void) { // ms += 20, 30, 40, 50; // ms += 20,30,40,50,60,70,80,90,100; for (size_t m = 2; m <= 50; m += 2) - ms += m; + ms.push_back(m); //for (size_t m=10;m<=100;m+=10) ms += m; // loop over number of images for(size_t m: ms) From fdc0068c00a22d0a98d4397ddb9735c1b979b98e Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 22 Jan 2023 13:20:37 -0800 Subject: [PATCH 06/51] Fix some CI issues (running in Debug, with TBB) --- gtsam/nonlinear/Values-inl.h | 4 ++-- gtsam/nonlinear/Values.cpp | 6 +++--- gtsam/nonlinear/Values.h | 12 ++++++------ 3 files changed, 11 insertions(+), 11 deletions(-) diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h index 474394f7b..a354e0139 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -24,9 +24,9 @@ #pragma once -#include - #include +#include +#include namespace gtsam { diff --git a/gtsam/nonlinear/Values.cpp b/gtsam/nonlinear/Values.cpp index e3adcc1bf..c7321c904 100644 --- a/gtsam/nonlinear/Values.cpp +++ b/gtsam/nonlinear/Values.cpp @@ -103,16 +103,16 @@ namespace gtsam { /* ************************************************************************* */ void Values::retractMasked(const VectorValues& delta, const KeySet& mask) { gttic(retractMasked); - assert(theta->size() == delta.size()); + assert(this->size() == delta.size()); auto key_value = values_.begin(); VectorValues::const_iterator key_delta; #ifdef GTSAM_USE_TBB for (; key_value != values_.end(); ++key_value) { - key_delta = delta.find(key_value.first); + key_delta = delta.find(key_value->first); #else for (key_delta = delta.begin(); key_value != values_.end(); ++key_value, ++key_delta) { - assert(key_value.first == key_delta->first); + assert(key_value->first == key_delta->first); #endif Key var = key_value->first; assert(static_cast(delta[var].size()) == key_value->second->dim()); diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index 299435677..161df2cba 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -306,12 +306,6 @@ namespace gtsam { typedef KeyValueMap::const_iterator::value_type ConstKeyValuePtrPair; typedef KeyValueMap::iterator::value_type KeyValuePtrPair; - /** insert that mimics the STL map insert - if the value already exists, the map is not modified - * and an iterator to the existing value is returned, along with 'false'. If the value did not - * exist, it is inserted and an iterator pointing to the new element, along with 'true', is - * returned. */ - std::pair tryInsert(Key j, const Value& value); - /// Mutable forward iterator, with value type KeyValuePair typedef boost::transform_iterator< std::function, KeyValueMap::iterator> iterator; @@ -328,6 +322,12 @@ namespace gtsam { typedef boost::transform_iterator< std::function, KeyValueMap::const_reverse_iterator> const_reverse_iterator; + /** insert that mimics the STL map insert - if the value already exists, the map is not modified + * and an iterator to the existing value is returned, along with 'false'. If the value did not + * exist, it is inserted and an iterator pointing to the new element, along with 'true', is + * returned. */ + std::pair tryInsert(Key j, const Value& value); + static ConstKeyValuePair make_const_deref_pair(const KeyValueMap::const_iterator::value_type& key_value) { return ConstKeyValuePair(key_value.first, *key_value.second); } From b281f62e24f10dccedc3bafa98d3077e5eeb4888 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 25 Jan 2023 19:46:09 -0800 Subject: [PATCH 07/51] Added a const_iterator back in --- gtsam/nonlinear/Values.h | 31 +++++++++++++++++++++++++++- gtsam/nonlinear/tests/testValues.cpp | 8 +++++++ 2 files changed, 38 insertions(+), 1 deletion(-) diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index 161df2cba..f93323202 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -108,8 +108,11 @@ namespace gtsam { typedef KeyValuePair value_type; + /// @name Constructors + /// @{ + /** Default constructor creates an empty Values class */ - Values() {} + Values() = default; /** Copy constructor duplicates all keys and values */ Values(const Values& other); @@ -127,6 +130,7 @@ namespace gtsam { /** Construct from a Values and an update vector: identical to other.retract(delta) */ Values(const Values& other, const VectorValues& delta); + /// @} /// @name Testable /// @{ @@ -137,6 +141,8 @@ namespace gtsam { bool equals(const Values& other, double tol=1e-9) const; /// @} + /// @name Standard Interface + /// @{ /** 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. @@ -177,6 +183,29 @@ namespace gtsam { /** whether the config is empty */ bool empty() const { return values_.empty(); } + /// @} + /// @name Iterator + /// @{ + + struct const_iterator { + using const_iterator_type = typename KeyValueMap::const_iterator; + const_iterator_type it_; + const_iterator(const_iterator_type it) : it_(it) {} + std::pair operator*() const { + return {it_->first, *(it_->second)}; + } + bool operator==(const const_iterator& other) const { return it_ == other.it_; } + bool operator!=(const const_iterator& other) const { return it_ != other.it_; } + const_iterator& operator++() { + ++it_; + return *this; + } + }; + + const_iterator begin() { return const_iterator(values_.begin()); } + const_iterator end() { return const_iterator(values_.end()); } + + /// @} /// @name Manifold Operations /// @{ diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index 644b8c84f..e554a28b1 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -195,6 +195,14 @@ TEST(Values, basic_functions) values.insert(6, M1); values.insert(8, M2); + size_t count = 0; + for (const auto& [key, value] : values) { + count += 1; + if (key == 2 || key == 4) EXPECT_LONGS_EQUAL(3, value.dim()); + if (key == 6 || key == 8) EXPECT_LONGS_EQUAL(6, value.dim()); + } + EXPECT_LONGS_EQUAL(4, count); + #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 // find EXPECT_LONGS_EQUAL(4, values.find(4)->key); From f2fff1936b590e7cea6ab549fba193a1ac025487 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 25 Jan 2023 21:17:03 -0800 Subject: [PATCH 08/51] Added some more functions and fixed both flag paths. --- gtsam/nonlinear/Values-inl.h | 8 ++--- gtsam/nonlinear/Values.h | 45 +++++++++++++++------------- gtsam/nonlinear/tests/testValues.cpp | 2 -- 3 files changed, 28 insertions(+), 27 deletions(-) diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h index a354e0139..04c6440a2 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -163,14 +163,14 @@ namespace gtsam { &ValuesCastHelper::cast)), constBegin_( boost::make_transform_iterator( boost::make_filter_iterator(filter, - ((const Values&) values).begin(), - ((const Values&) values).end()), + values._begin(), + values._end()), &ValuesCastHelper::cast)), constEnd_( boost::make_transform_iterator( boost::make_filter_iterator(filter, - ((const Values&) values).end(), - ((const Values&) values).end()), + values._end(), + values._end()), &ValuesCastHelper::cast)) { } diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index f93323202..48c9bd1ad 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -187,23 +187,36 @@ namespace gtsam { /// @name Iterator /// @{ - struct const_iterator { + struct deref_iterator { using const_iterator_type = typename KeyValueMap::const_iterator; const_iterator_type it_; - const_iterator(const_iterator_type it) : it_(it) {} - std::pair operator*() const { - return {it_->first, *(it_->second)}; + deref_iterator(const_iterator_type it) : it_(it) {} + ConstKeyValuePair operator*() const { return {it_->first, *(it_->second)}; } + std::unique_ptr operator->() { + return std::make_unique(it_->first, *(it_->second)); } - bool operator==(const const_iterator& other) const { return it_ == other.it_; } - bool operator!=(const const_iterator& other) const { return it_ != other.it_; } - const_iterator& operator++() { + bool operator==(const deref_iterator& other) const { + return it_ == other.it_; + } + bool operator!=(const deref_iterator& other) const { return it_ != other.it_; } + deref_iterator& operator++() { ++it_; return *this; } }; - const_iterator begin() { return const_iterator(values_.begin()); } - const_iterator end() { return const_iterator(values_.end()); } + deref_iterator begin() const { return deref_iterator(values_.begin()); } + deref_iterator end() const { return deref_iterator(values_.end()); } + + /** Find an element by key, returning an iterator, or end() if the key was + * not found. */ + deref_iterator find(Key j) const { return deref_iterator(values_.find(j)); } + + /** Find the element greater than or equal to the specified key. */ + deref_iterator lower_bound(Key j) const { return deref_iterator(values_.lower_bound(j)); } + + /** Find the lowest-ordered element greater than the specified key. */ + deref_iterator upper_bound(Key j) const { return deref_iterator(values_.upper_bound(j)); } /// @} /// @name Manifold Operations @@ -363,8 +376,8 @@ namespace gtsam { static KeyValuePair make_deref_pair(const KeyValueMap::iterator::value_type& key_value) { return KeyValuePair(key_value.first, *key_value.second); } - const_iterator begin() const { return boost::make_transform_iterator(values_.begin(), &make_const_deref_pair); } - const_iterator end() const { return boost::make_transform_iterator(values_.end(), &make_const_deref_pair); } + const_iterator _begin() const { return boost::make_transform_iterator(values_.begin(), &make_const_deref_pair); } + const_iterator _end() const { return boost::make_transform_iterator(values_.end(), &make_const_deref_pair); } iterator begin() { return boost::make_transform_iterator(values_.begin(), &make_deref_pair); } iterator end() { return boost::make_transform_iterator(values_.end(), &make_deref_pair); } const_reverse_iterator rbegin() const { return boost::make_transform_iterator(values_.rbegin(), &make_const_deref_pair); } @@ -376,22 +389,12 @@ namespace gtsam { * not found. */ iterator find(Key j) { return boost::make_transform_iterator(values_.find(j), &make_deref_pair); } - /** Find an element by key, returning an iterator, or end() if the key was - * not found. */ - const_iterator find(Key j) const { return boost::make_transform_iterator(values_.find(j), &make_const_deref_pair); } - /** Find the element greater than or equal to the specified key. */ iterator lower_bound(Key j) { return boost::make_transform_iterator(values_.lower_bound(j), &make_deref_pair); } - /** Find the element greater than or equal to the specified key. */ - const_iterator lower_bound(Key j) const { return boost::make_transform_iterator(values_.lower_bound(j), &make_const_deref_pair); } - /** Find the lowest-ordered element greater than the specified key. */ iterator upper_bound(Key j) { return boost::make_transform_iterator(values_.upper_bound(j), &make_deref_pair); } - /** Find the lowest-ordered element greater than the specified key. */ - const_iterator upper_bound(Key j) const { return boost::make_transform_iterator(values_.upper_bound(j), &make_const_deref_pair); } - /** A filtered view of a Values, returned from Values::filter. */ template class Filtered; diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index e554a28b1..41bd6aead 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -203,7 +203,6 @@ TEST(Values, basic_functions) } EXPECT_LONGS_EQUAL(4, count); -#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 // find EXPECT_LONGS_EQUAL(4, values.find(4)->key); EXPECT_LONGS_EQUAL(4, values_c.find(4)->key); @@ -219,7 +218,6 @@ TEST(Values, basic_functions) EXPECT_LONGS_EQUAL(6, values_c.upper_bound(4)->key); EXPECT_LONGS_EQUAL(4, values.upper_bound(3)->key); EXPECT_LONGS_EQUAL(4, values_c.upper_bound(3)->key); -#endif } /* ************************************************************************* */ From e2f69e0afe4cd1dd8a1a74193e3fa66f5a89c1d5 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 25 Jan 2023 21:47:33 -0800 Subject: [PATCH 09/51] Forgot [key, value] only works for c++17 --- gtsam/nonlinear/tests/testValues.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index 41bd6aead..758d9a5a3 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -196,10 +196,10 @@ TEST(Values, basic_functions) values.insert(8, M2); size_t count = 0; - for (const auto& [key, value] : values) { + for (const auto& it : values) { count += 1; - if (key == 2 || key == 4) EXPECT_LONGS_EQUAL(3, value.dim()); - if (key == 6 || key == 8) EXPECT_LONGS_EQUAL(6, value.dim()); + if (it.key == 2 || it.key == 4) EXPECT_LONGS_EQUAL(3, it.value.dim()); + if (it.key == 6 || it.key == 8) EXPECT_LONGS_EQUAL(6, it.value.dim()); } EXPECT_LONGS_EQUAL(4, count); From d00971d2c9b43c0dc060551463353e9f2c849f73 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 25 Jan 2023 22:59:22 -0800 Subject: [PATCH 10/51] Use boost::shared ptr --- gtsam/nonlinear/Values.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index 48c9bd1ad..74f22a27d 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -192,8 +192,8 @@ namespace gtsam { const_iterator_type it_; deref_iterator(const_iterator_type it) : it_(it) {} ConstKeyValuePair operator*() const { return {it_->first, *(it_->second)}; } - std::unique_ptr operator->() { - return std::make_unique(it_->first, *(it_->second)); + boost::shared_ptr operator->() { + return boost::make_shared(it_->first, *(it_->second)); } bool operator==(const deref_iterator& other) const { return it_ == other.it_; From a7b42dc8787aa8ea6944b81a9ba25c9b18570a64 Mon Sep 17 00:00:00 2001 From: chris Date: Tue, 24 Jan 2023 23:44:44 -1000 Subject: [PATCH 11/51] Fix (issue #1336) dangling pointer access violation. --- gtsam/nonlinear/Expression-inl.h | 16 +++++++++++----- 1 file changed, 11 insertions(+), 5 deletions(-) diff --git a/gtsam/nonlinear/Expression-inl.h b/gtsam/nonlinear/Expression-inl.h index b2ef6f055..620e6afca 100644 --- a/gtsam/nonlinear/Expression-inl.h +++ b/gtsam/nonlinear/Expression-inl.h @@ -19,6 +19,13 @@ #pragma once +// The MSVC compiler workaround for the unsupported variable length array +// utilizes the std::unique_ptr<> custom deleter. +// See Expression::valueAndJacobianMap() below. +#ifdef _MSC_VER +#include +#endif + #include #include @@ -208,7 +215,10 @@ T Expression::valueAndJacobianMap(const Values& values, // allocated on Visual Studio. For more information see the issue below // https://bitbucket.org/gtborg/gtsam/issue/178/vlas-unsupported-in-visual-studio #ifdef _MSC_VER - auto traceStorage = static_cast(_aligned_malloc(size, internal::TraceAlignment)); + std::unique_ptr traceStorageDeleter( + _aligned_malloc(size, internal::TraceAlignment), + [](void *ptr){ _aligned_free(ptr); }); + auto traceStorage = static_cast(traceStorageDeleter.get()); #else internal::ExecutionTraceStorage traceStorage[size]; #endif @@ -217,10 +227,6 @@ T Expression::valueAndJacobianMap(const Values& values, T value(this->traceExecution(values, trace, traceStorage)); trace.startReverseAD1(jacobians); -#ifdef _MSC_VER - _aligned_free(traceStorage); -#endif - return value; } From a132a36ff2d5a90aeb7ffe1ec7e1109ed4e4570e Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 28 Jan 2023 12:57:47 -0800 Subject: [PATCH 12/51] Avoid warning --- gtsam/slam/InitializePose3.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp index e8ec9181c..3e2ad7ebe 100644 --- a/gtsam/slam/InitializePose3.cpp +++ b/gtsam/slam/InitializePose3.cpp @@ -206,11 +206,10 @@ Values InitializePose3::computeOrientationsGradient( // Return correct rotations const Rot3& Rref = inverseRot.at(initialize::kAnchorKey); // This will be set to the identity as so far we included no prior Values estimateRot; - for (const auto key_R : inverseRot) { + for (const auto& key_R : inverseRot) { const Key& key = key_R.first; - const Rot3& Ri = key_R.second; if (key != initialize::kAnchorKey) { - const Rot3& R = inverseRot.at(key); + const Rot3& R = key_R.second; if(setRefFrame) estimateRot.insert(key, Rref.compose(R.inverse())); else From faae928eae102e41e329d652c98981347d6464bd Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 28 Jan 2023 13:02:54 -0800 Subject: [PATCH 13/51] Fix version --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index b81479cb8..399259685 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -10,7 +10,7 @@ endif() set (GTSAM_VERSION_MAJOR 4) set (GTSAM_VERSION_MINOR 2) set (GTSAM_VERSION_PATCH 0) -set (GTSAM_PRERELEASE_VERSION "a8") +set (GTSAM_PRERELEASE_VERSION "") math (EXPR GTSAM_VERSION_NUMERIC "10000 * ${GTSAM_VERSION_MAJOR} + 100 * ${GTSAM_VERSION_MINOR} + ${GTSAM_VERSION_PATCH}") if (${GTSAM_VERSION_PATCH} EQUAL 0) From be9b00e408ccedfe9f5295be132f1de700a17430 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 28 Jan 2023 13:03:19 -0800 Subject: [PATCH 14/51] Fix docs --- .github/workflows/build-special.yml | 2 +- README.md | 4 +--- cmake/HandleGeneralOptions.cmake | 2 +- cmake/HandlePrintConfiguration.cmake | 2 +- 4 files changed, 4 insertions(+), 6 deletions(-) diff --git a/.github/workflows/build-special.yml b/.github/workflows/build-special.yml index ef7d7723d..96e7174ae 100644 --- a/.github/workflows/build-special.yml +++ b/.github/workflows/build-special.yml @@ -111,7 +111,7 @@ jobs: if: matrix.flag == 'deprecated' run: | echo "GTSAM_ALLOW_DEPRECATED_SINCE_V42=ON" >> $GITHUB_ENV - echo "Allow deprecated since version 4.1" + echo "Allow deprecated since version 4.2" - name: Set Use Quaternions Flag if: matrix.flag == 'quaternions' diff --git a/README.md b/README.md index dbbba8c2b..25b3b98ea 100644 --- a/README.md +++ b/README.md @@ -55,9 +55,7 @@ Optional prerequisites - used automatically if findable by CMake: GTSAM 4 introduces several new features, most notably Expressions and a Python toolbox. It also introduces traits, a C++ technique that allows optimizing with non-GTSAM types. That opens the door to retiring geometric types such as Point2 and Point3 to pure Eigen types, which we also do. A significant change which will not trigger a compile error is that zero-initializing of Point2 and Point3 is deprecated, so please be aware that this might render functions using their default constructor incorrect. -GTSAM 4 also deprecated some legacy functionality and wrongly named methods. If you are on a 4.0.X release, you can define the flag `GTSAM_ALLOW_DEPRECATED_SINCE_V4` to use the deprecated methods. - -GTSAM 4.1 added a new pybind wrapper, and **removed** the deprecated functionality. There is a flag `GTSAM_ALLOW_DEPRECATED_SINCE_V42` for newly deprecated methods since the 4.1 release, which is on by default, allowing anyone to just pull version 4.1 and compile. + There is a flag `GTSAM_ALLOW_DEPRECATED_SINCE_V42` for newly deprecated methods since the 4.2 release, which is on by default, allowing anyone to just pull version 4.2 and compile. ## Wrappers diff --git a/cmake/HandleGeneralOptions.cmake b/cmake/HandleGeneralOptions.cmake index 7c8f8533f..00ec65ba7 100644 --- a/cmake/HandleGeneralOptions.cmake +++ b/cmake/HandleGeneralOptions.cmake @@ -25,7 +25,7 @@ option(GTSAM_WITH_EIGEN_MKL_OPENMP "Eigen, when using Intel MKL, will a option(GTSAM_THROW_CHEIRALITY_EXCEPTION "Throw exception when a triangulated point is behind a camera" ON) option(GTSAM_BUILD_PYTHON "Enable/Disable building & installation of Python module with pybind11" OFF) option(GTSAM_INSTALL_MATLAB_TOOLBOX "Enable/Disable installation of matlab toolbox" OFF) -option(GTSAM_ALLOW_DEPRECATED_SINCE_V42 "Allow use of methods/functions deprecated in GTSAM 4.1" ON) +option(GTSAM_ALLOW_DEPRECATED_SINCE_V42 "Allow use of methods/functions deprecated in GTSAM 4.2" ON) option(GTSAM_SUPPORT_NESTED_DISSECTION "Support Metis-based nested dissection" ON) option(GTSAM_TANGENT_PREINTEGRATION "Use new ImuFactor with integration on tangent space" ON) option(GTSAM_SLOW_BUT_CORRECT_BETWEENFACTOR "Use the slower but correct version of BetweenFactor" OFF) diff --git a/cmake/HandlePrintConfiguration.cmake b/cmake/HandlePrintConfiguration.cmake index 04d27c27f..5b7ef237a 100644 --- a/cmake/HandlePrintConfiguration.cmake +++ b/cmake/HandlePrintConfiguration.cmake @@ -87,7 +87,7 @@ print_enabled_config(${GTSAM_USE_QUATERNIONS} "Quaternions as defaul print_enabled_config(${GTSAM_ENABLE_CONSISTENCY_CHECKS} "Runtime consistency checking ") print_enabled_config(${GTSAM_ROT3_EXPMAP} "Rot3 retract is full ExpMap ") print_enabled_config(${GTSAM_POSE3_EXPMAP} "Pose3 retract is full ExpMap ") -print_enabled_config(${GTSAM_ALLOW_DEPRECATED_SINCE_V42} "Allow features deprecated in GTSAM 4.1") +print_enabled_config(${GTSAM_ALLOW_DEPRECATED_SINCE_V42} "Allow features deprecated in GTSAM 4.2") print_enabled_config(${GTSAM_SUPPORT_NESTED_DISSECTION} "Metis-based Nested Dissection ") print_enabled_config(${GTSAM_TANGENT_PREINTEGRATION} "Use tangent-space preintegration") From f2f1bbaf8ce2a6538e801af3ea76d13ddb6291a8 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 28 Jan 2023 16:27:07 -0500 Subject: [PATCH 15/51] update examples to use C++11 --- gtsam_unstable/examples/SmartRangeExample_plaza1.cpp | 10 ++++++++-- gtsam_unstable/examples/SmartRangeExample_plaza2.cpp | 10 ++++++++-- 2 files changed, 16 insertions(+), 4 deletions(-) diff --git a/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp b/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp index 2fccf6b18..984637ef0 100644 --- a/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp +++ b/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp @@ -234,8 +234,11 @@ int main(int argc, char** argv) { } } countK = 0; - for (const auto& [key, point] : result.extract()) + for (const auto& key_point : result.extract()) { + auto key = key_point.first; + const Point2 point = key_point.second; os2 << key << "\t" << point.x() << "\t" << point.y() << "\t1" << endl; + } if (smart) { for(size_t jj: ids) { Point2 landmark = smartFactors[jj]->triangulate(result); @@ -256,8 +259,11 @@ int main(int argc, char** argv) { // Write result to file Values result = isam.calculateEstimate(); ofstream os("rangeResult.txt"); - for (const auto& [key, pose] : result.extract()) + for (const auto& key_pose : result.extract()) { + auto key = key_pose.first; + const Pose2 pose = key_pose.second; os << key << "\t" << pose.x() << "\t" << pose.y() << "\t" << pose.theta() << endl; + } exit(0); } diff --git a/gtsam_unstable/examples/SmartRangeExample_plaza2.cpp b/gtsam_unstable/examples/SmartRangeExample_plaza2.cpp index 1e6f77b31..b7f1fb1e3 100644 --- a/gtsam_unstable/examples/SmartRangeExample_plaza2.cpp +++ b/gtsam_unstable/examples/SmartRangeExample_plaza2.cpp @@ -202,11 +202,17 @@ int main(int argc, char** argv) { // Write result to file Values result = isam.calculateEstimate(); ofstream os2("rangeResultLM.txt"); - for (const auto& [key, point] : result.extract()) + for (const auto& key_point : result.extract()) { + auto key = key_point.first; + const Point2 point = key_point.second; os2 << key << "\t" << point.x() << "\t" << point.y() << "\t1" << endl; + } ofstream os("rangeResult.txt"); - for (const auto& [key, pose] : result.extract()) + for (const auto& key_pose : result.extract()) { + auto key = key_pose.first; + const Pose2 pose = key_pose.second; os << key << "\t" << pose.x() << "\t" << pose.y() << "\t" << pose.theta() << endl; + } exit(0); } From a1ed2f9866202f35cef66057a8cb37603e8b932b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 28 Jan 2023 16:27:27 -0500 Subject: [PATCH 16/51] fix warnings --- gtsam/slam/InitializePose3.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp index e8ec9181c..878bbd44c 100644 --- a/gtsam/slam/InitializePose3.cpp +++ b/gtsam/slam/InitializePose3.cpp @@ -206,12 +206,11 @@ Values InitializePose3::computeOrientationsGradient( // Return correct rotations const Rot3& Rref = inverseRot.at(initialize::kAnchorKey); // This will be set to the identity as so far we included no prior Values estimateRot; - for (const auto key_R : inverseRot) { + for (const auto& key_R : inverseRot) { const Key& key = key_R.first; - const Rot3& Ri = key_R.second; if (key != initialize::kAnchorKey) { const Rot3& R = inverseRot.at(key); - if(setRefFrame) + if (setRefFrame) estimateRot.insert(key, Rref.compose(R.inverse())); else estimateRot.insert(key, R.inverse()); From 09d5380514d744a77db605ad815021699e483312 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 28 Jan 2023 16:27:41 -0500 Subject: [PATCH 17/51] only print cmake compile options for current build version --- cmake/GtsamPrinting.cmake | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/cmake/GtsamPrinting.cmake b/cmake/GtsamPrinting.cmake index c68679667..2181652e5 100644 --- a/cmake/GtsamPrinting.cmake +++ b/cmake/GtsamPrinting.cmake @@ -51,11 +51,10 @@ function(print_build_options_for_target target_name_) # print_padded(GTSAM_COMPILE_DEFINITIONS_PRIVATE) print_padded(GTSAM_COMPILE_DEFINITIONS_PUBLIC) - foreach(build_type ${GTSAM_CMAKE_CONFIGURATION_TYPES}) - string(TOUPPER "${build_type}" build_type_toupper) - # print_padded(GTSAM_COMPILE_OPTIONS_PRIVATE_${build_type_toupper}) - print_padded(GTSAM_COMPILE_OPTIONS_PUBLIC_${build_type_toupper}) - # print_padded(GTSAM_COMPILE_DEFINITIONS_PRIVATE_${build_type_toupper}) - print_padded(GTSAM_COMPILE_DEFINITIONS_PUBLIC_${build_type_toupper}) - endforeach() + string(TOUPPER "${CMAKE_BUILD_TYPE}" build_type_toupper) + # print_padded(GTSAM_COMPILE_OPTIONS_PRIVATE_${build_type_toupper}) + print_padded(GTSAM_COMPILE_OPTIONS_PUBLIC_${build_type_toupper}) + # print_padded(GTSAM_COMPILE_DEFINITIONS_PRIVATE_${build_type_toupper}) + print_padded(GTSAM_COMPILE_DEFINITIONS_PUBLIC_${build_type_toupper}) + endfunction() From 60ed2422267f49f8436a1d4f8f6de8be635d1e64 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 28 Jan 2023 16:27:56 -0500 Subject: [PATCH 18/51] remove commented Cmake commands --- cmake/HandleTBB.cmake | 4 ---- 1 file changed, 4 deletions(-) diff --git a/cmake/HandleTBB.cmake b/cmake/HandleTBB.cmake index 52ee75494..fb944ba5b 100644 --- a/cmake/HandleTBB.cmake +++ b/cmake/HandleTBB.cmake @@ -7,10 +7,6 @@ if (GTSAM_WITH_TBB) if(TBB_FOUND) set(GTSAM_USE_TBB 1) # This will go into config.h -# if ((${TBB_VERSION} VERSION_GREATER "2021.1") OR (${TBB_VERSION} VERSION_EQUAL "2021.1")) -# message(FATAL_ERROR "TBB version greater than 2021.1 (oneTBB API) is not yet supported. Use an older version instead.") -# endif() - if ((${TBB_VERSION_MAJOR} GREATER 2020) OR (${TBB_VERSION_MAJOR} EQUAL 2020)) set(TBB_GREATER_EQUAL_2020 1) else() From 0b7a2be2ea2c40b67716ea05a94abced706b2053 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco Claraco Date: Sun, 29 Jan 2023 00:31:48 +0100 Subject: [PATCH 19/51] Update ROS package.xml format to v3 and update gtsam version --- package.xml | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/package.xml b/package.xml index 341c78ba3..9e402d2c4 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,8 @@ - + + gtsam - 4.1.0 + 4.2.0 gtsam Frank Dellaert From b3e87360d50fe7c8b0f7b5800fd99fd6684d0200 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 28 Jan 2023 16:39:57 -0800 Subject: [PATCH 20/51] Add missing header to fix gtbook. --- python/gtsam/preamble/inference.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/python/gtsam/preamble/inference.h b/python/gtsam/preamble/inference.h index d07a75f6f..4106c794a 100644 --- a/python/gtsam/preamble/inference.h +++ b/python/gtsam/preamble/inference.h @@ -10,3 +10,5 @@ * Without this they will be automatically converted to a Python object, and all * mutations on Python side will not be reflected on C++. */ + +#include \ No newline at end of file From c57988fe554e7213c77fe379c1d7c483de26ad33 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 28 Jan 2023 18:09:38 -0800 Subject: [PATCH 21/51] Switch to 4.2a9 version --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 399259685..5bad53988 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -10,7 +10,7 @@ endif() set (GTSAM_VERSION_MAJOR 4) set (GTSAM_VERSION_MINOR 2) set (GTSAM_VERSION_PATCH 0) -set (GTSAM_PRERELEASE_VERSION "") +set (GTSAM_PRERELEASE_VERSION "a9") math (EXPR GTSAM_VERSION_NUMERIC "10000 * ${GTSAM_VERSION_MAJOR} + 100 * ${GTSAM_VERSION_MINOR} + ${GTSAM_VERSION_PATCH}") if (${GTSAM_VERSION_PATCH} EQUAL 0) From b5a3f11993d430f6ff26cc319c943f87077ab817 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 13 Feb 2023 15:28:35 -0500 Subject: [PATCH 22/51] Add better hybrid support --- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 12 ++-- gtsam/hybrid/HybridNonlinearFactorGraph.cpp | 7 ++ .../tests/testGaussianMixtureFactor.cpp | 2 + .../tests/testHybridGaussianFactorGraph.cpp | 67 +++++++++++++++++-- 4 files changed, 75 insertions(+), 13 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index c912a74fc..20950d4f9 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -106,7 +106,9 @@ GaussianFactorGraphTree HybridGaussianFactorGraph::assembleGraphTree() const { // TODO(dellaert): just use a virtual method defined in HybridFactor. if (auto gf = dynamic_pointer_cast(f)) { result = addGaussian(result, gf); - } else if (auto gm = dynamic_pointer_cast(f)) { + } else if (auto gmf = dynamic_pointer_cast(f)) { + result = gmf->add(result); + } else if (auto gm = dynamic_pointer_cast(f)) { result = gm->add(result); } else if (auto hc = dynamic_pointer_cast(f)) { if (auto gm = hc->asMixture()) { @@ -283,17 +285,15 @@ hybridElimination(const HybridGaussianFactorGraph &factors, // taking care to correct for conditional constant. // Correct for the normalization constant used up by the conditional - auto correct = [&](const Result &pair) -> GaussianFactor::shared_ptr { + auto correct = [&](const Result &pair) { const auto &factor = pair.second; - if (!factor) return factor; // TODO(dellaert): not loving this. + if (!factor) return; auto hf = boost::dynamic_pointer_cast(factor); if (!hf) throw std::runtime_error("Expected HessianFactor!"); hf->constantTerm() += 2.0 * pair.first->logNormalizationConstant(); - return hf; }; + eliminationResults.visit(correct); - GaussianMixtureFactor::Factors correctedFactors(eliminationResults, - correct); const auto mixtureFactor = boost::make_shared( continuousSeparator, discreteSeparator, newFactors); diff --git a/gtsam/hybrid/HybridNonlinearFactorGraph.cpp b/gtsam/hybrid/HybridNonlinearFactorGraph.cpp index 71b064eb6..ab64b1ec3 100644 --- a/gtsam/hybrid/HybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/HybridNonlinearFactorGraph.cpp @@ -17,6 +17,7 @@ */ #include +#include #include #include #include @@ -69,6 +70,12 @@ HybridGaussianFactorGraph::shared_ptr HybridNonlinearFactorGraph::linearize( } else if (dynamic_pointer_cast(f)) { // If discrete-only: doesn't need linearization. linearFG->push_back(f); + } else if (auto gmf = dynamic_pointer_cast(f)) { + linearFG->push_back(gmf); + } else if (auto gm = dynamic_pointer_cast(f)) { + linearFG->push_back(gm); + } else if (dynamic_pointer_cast(f)) { + linearFG->push_back(f); } else { auto& fr = *f; throw std::invalid_argument( diff --git a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp index 962d238a8..4ef2af471 100644 --- a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp +++ b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp @@ -93,6 +93,7 @@ TEST(GaussianMixtureFactor, Sum) { EXPECT(actual.at(1) == f22); } +/* ************************************************************************* */ TEST(GaussianMixtureFactor, Printing) { DiscreteKey m1(1, 2); auto A1 = Matrix::Zero(2, 1); @@ -136,6 +137,7 @@ TEST(GaussianMixtureFactor, Printing) { EXPECT(assert_print_equal(expected, mixtureFactor)); } +/* ************************************************************************* */ TEST(GaussianMixtureFactor, GaussianMixture) { KeyVector keys; keys.push_back(X(0)); diff --git a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp index e5c11bf0c..3302994c0 100644 --- a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp @@ -612,7 +612,6 @@ TEST(HybridGaussianFactorGraph, ErrorAndProbPrimeTree) { // Check that assembleGraphTree assembles Gaussian factor graphs for each // assignment. TEST(HybridGaussianFactorGraph, assembleGraphTree) { - using symbol_shorthand::Z; const int num_measurements = 1; auto fg = tiny::createHybridGaussianFactorGraph( num_measurements, VectorValues{{Z(0), Vector1(5.0)}}); @@ -694,7 +693,6 @@ bool ratioTest(const HybridBayesNet &bn, const VectorValues &measurements, /* ****************************************************************************/ // Check that eliminating tiny net with 1 measurement yields correct result. TEST(HybridGaussianFactorGraph, EliminateTiny1) { - using symbol_shorthand::Z; const int num_measurements = 1; const VectorValues measurements{{Z(0), Vector1(5.0)}}; auto bn = tiny::createHybridBayesNet(num_measurements); @@ -726,11 +724,67 @@ TEST(HybridGaussianFactorGraph, EliminateTiny1) { EXPECT(ratioTest(bn, measurements, *posterior)); } +/* ****************************************************************************/ +// Check that eliminating tiny net with 1 measurement with mode order swapped +// yields correct result. +TEST(HybridGaussianFactorGraph, EliminateTiny1Swapped) { + const VectorValues measurements{{Z(0), Vector1(5.0)}}; + + // Create mode key: 1 is low-noise, 0 is high-noise. + const DiscreteKey mode{M(0), 2}; + HybridBayesNet bn; + + // Create Gaussian mixture z_0 = x0 + noise for each measurement. + bn.emplace_back(new GaussianMixture( + {Z(0)}, {X(0)}, {mode}, + {GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), Z_1x1, 3), + GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), Z_1x1, + 0.5)})); + + // Create prior on X(0). + bn.push_back( + GaussianConditional::sharedMeanAndStddev(X(0), Vector1(5.0), 0.5)); + + // Add prior on mode. + bn.emplace_back(new DiscreteConditional(mode, "1/1")); + + // bn.print(); + auto fg = bn.toFactorGraph(measurements); + EXPECT_LONGS_EQUAL(3, fg.size()); + + // fg.print(); + + EXPECT(ratioTest(bn, measurements, fg)); + + // Create expected Bayes Net: + HybridBayesNet expectedBayesNet; + + // Create Gaussian mixture on X(0). + // regression, but mean checked to be 5.0 in both cases: + const auto conditional0 = boost::make_shared( + X(0), Vector1(10.1379), I_1x1 * 2.02759), + conditional1 = boost::make_shared( + X(0), Vector1(14.1421), I_1x1 * 2.82843); + expectedBayesNet.emplace_back( + new GaussianMixture({X(0)}, {}, {mode}, {conditional0, conditional1})); + + // Add prior on mode. + expectedBayesNet.emplace_back(new DiscreteConditional(mode, "1/1")); + + // Test elimination + const auto posterior = fg.eliminateSequential(); + // EXPECT(assert_equal(expectedBayesNet, *posterior, 0.01)); + + EXPECT(ratioTest(bn, measurements, *posterior)); + + // posterior->print(); + // posterior->optimize().print(); +} + /* ****************************************************************************/ // Check that eliminating tiny net with 2 measurements yields correct result. TEST(HybridGaussianFactorGraph, EliminateTiny2) { // Create factor graph with 2 measurements such that posterior mean = 5.0. - using symbol_shorthand::Z; const int num_measurements = 2; const VectorValues measurements{{Z(0), Vector1(4.0)}, {Z(1), Vector1(6.0)}}; auto bn = tiny::createHybridBayesNet(num_measurements); @@ -764,7 +818,6 @@ TEST(HybridGaussianFactorGraph, EliminateTiny2) { // Test eliminating tiny net with 1 mode per measurement. TEST(HybridGaussianFactorGraph, EliminateTiny22) { // Create factor graph with 2 measurements such that posterior mean = 5.0. - using symbol_shorthand::Z; const int num_measurements = 2; const bool manyModes = true; @@ -835,12 +888,12 @@ TEST(HybridGaussianFactorGraph, EliminateSwitchingNetwork) { // D D // | | // m1 m2 - // | | + // | | // C-x0-HC-x1-HC-x2 // | | | // HF HF HF // | | | - // n0 n1 n2 + // n0 n1 n2 // | | | // D D D EXPECT_LONGS_EQUAL(11, fg.size()); @@ -853,7 +906,7 @@ TEST(HybridGaussianFactorGraph, EliminateSwitchingNetwork) { EXPECT(ratioTest(bn, measurements, fg1)); // Create ordering that eliminates in time order, then discrete modes: - Ordering ordering {X(2), X(1), X(0), N(0), N(1), N(2), M(1), M(2)}; + Ordering ordering{X(2), X(1), X(0), N(0), N(1), N(2), M(1), M(2)}; // Do elimination: const HybridBayesNet::shared_ptr posterior = fg.eliminateSequential(ordering); From 2714dc562550b5c6f0b774f2b215172900ef6dc3 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 13 Feb 2023 15:59:30 -0500 Subject: [PATCH 23/51] add ordering method for HybridSmoother --- gtsam/hybrid/HybridSmoother.cpp | 32 +++++++++++++++++++++++++++++++- gtsam/hybrid/HybridSmoother.h | 2 ++ 2 files changed, 33 insertions(+), 1 deletion(-) diff --git a/gtsam/hybrid/HybridSmoother.cpp b/gtsam/hybrid/HybridSmoother.cpp index 35dd5f88b..fcee7833a 100644 --- a/gtsam/hybrid/HybridSmoother.cpp +++ b/gtsam/hybrid/HybridSmoother.cpp @@ -23,6 +23,37 @@ namespace gtsam { +/* ************************************************************************* */ +Ordering HybridSmoother::getOrdering( + const HybridGaussianFactorGraph &newFactors) { + HybridGaussianFactorGraph factors(hybridBayesNet()); + factors += newFactors; + // Get all the discrete keys from the factors + KeySet allDiscrete = factors.discreteKeySet(); + + // Create KeyVector with continuous keys followed by discrete keys. + KeyVector newKeysDiscreteLast; + const KeySet newFactorKeys = newFactors.keys(); + // Insert continuous keys first. + for (auto &k : newFactorKeys) { + if (!allDiscrete.exists(k)) { + newKeysDiscreteLast.push_back(k); + } + } + + // Insert discrete keys at the end + std::copy(allDiscrete.begin(), allDiscrete.end(), + std::back_inserter(newKeysDiscreteLast)); + + const VariableIndex index(newFactors); + + // Get an ordering where the new keys are eliminated last + Ordering ordering = Ordering::ColamdConstrainedLast( + index, KeyVector(newKeysDiscreteLast.begin(), newKeysDiscreteLast.end()), + true); + return ordering; +} + /* ************************************************************************* */ void HybridSmoother::update(HybridGaussianFactorGraph graph, const Ordering &ordering, @@ -92,7 +123,6 @@ HybridSmoother::addConditionals(const HybridGaussianFactorGraph &originalGraph, } graph.push_back(newConditionals); - // newConditionals.print("\n\n\nNew Conditionals to add back"); } return {graph, hybridBayesNet}; } diff --git a/gtsam/hybrid/HybridSmoother.h b/gtsam/hybrid/HybridSmoother.h index 7e90f9425..9f14a7002 100644 --- a/gtsam/hybrid/HybridSmoother.h +++ b/gtsam/hybrid/HybridSmoother.h @@ -50,6 +50,8 @@ class HybridSmoother { void update(HybridGaussianFactorGraph graph, const Ordering& ordering, boost::optional maxNrLeaves = boost::none); + Ordering getOrdering(const HybridGaussianFactorGraph& newFactors); + /** * @brief Add conditionals from previous timestep as part of liquefication. * From febeacd68686ed0b7ced72458eb0b31a196bdab7 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 4 Jun 2023 15:40:02 +0100 Subject: [PATCH 24/51] Improved documentation and tests --- gtsam/discrete/AlgebraicDecisionTree.h | 64 +++++++++++++++++++--- gtsam/discrete/DecisionTree-inl.h | 2 +- gtsam/discrete/DecisionTree.h | 31 +++++++++-- gtsam/discrete/tests/testDecisionTree.cpp | 66 ++++++++++++++++++----- 4 files changed, 137 insertions(+), 26 deletions(-) diff --git a/gtsam/discrete/AlgebraicDecisionTree.h b/gtsam/discrete/AlgebraicDecisionTree.h index b3f0d69b0..cd77e41f8 100644 --- a/gtsam/discrete/AlgebraicDecisionTree.h +++ b/gtsam/discrete/AlgebraicDecisionTree.h @@ -28,9 +28,9 @@ namespace gtsam { /** - * Algebraic Decision Trees fix the range to double - * Just has some nice constructors and some syntactic sugar - * TODO: consider eliminating this class altogether? + * An algebraic decision tree fixes the range of a DecisionTree to double. + * Just has some nice constructors and some syntactic sugar. + * TODO(dellaert): consider eliminating this class altogether? * * @ingroup discrete */ @@ -80,20 +80,62 @@ namespace gtsam { AlgebraicDecisionTree(const L& label, double y1, double y2) : Base(label, y1, y2) {} - /** Create a new leaf function splitting on a variable */ + /** + * @brief Create a new leaf function splitting on a variable + * + * @param labelC: The label with cardinality 2 + * @param y1: The value for the first key + * @param y2: The value for the second key + * + * Example: + * @code{.cpp} + * std::pair A {"a", 2}; + * AlgebraicDecisionTree a(A, 0.6, 0.4); + * @endcode + */ AlgebraicDecisionTree(const typename Base::LabelC& labelC, double y1, double y2) : Base(labelC, y1, y2) {} - /** Create from keys and vector table */ + /** + * @brief Create from keys with cardinalities and a vector table + * + * @param labelCs: The keys, with cardinalities, given as pairs + * @param ys: The vector table + * + * Example with three keys, A, B, and C, with cardinalities 2, 3, and 2, + * respectively, and a vector table of size 12: + * @code{.cpp} + * DiscreteKey A(0, 2), B(1, 3), C(2, 2); + * const vector cpt{ + * 1.0 / 3, 2.0 / 3, 3.0 / 7, 4.0 / 7, 5.0 / 11, 6.0 / 11, // + * 1.0 / 9, 8.0 / 9, 3.0 / 6, 3.0 / 6, 5.0 / 10, 5.0 / 10}; + * AlgebraicDecisionTree expected(A & B & C, cpt); + * @endcode + * The table is given in the following order: + * A=0, B=0, C=0 + * A=0, B=0, C=1 + * ... + * A=1, B=1, C=1 + * Hence, the first line in the table is for A==0, and the second for A==1. + * In each line, the first two entries are for B==0, the next two for B==1, + * and the last two for B==2. Each pair is for a C value of 0 and 1. + */ AlgebraicDecisionTree // (const std::vector& labelCs, - const std::vector& ys) { + const std::vector& ys) { this->root_ = Base::create(labelCs.begin(), labelCs.end(), ys.begin(), ys.end()); } - /** Create from keys and string table */ + /** + * @brief Create from keys and string table + * + * @param labelCs: The keys, with cardinalities, given as pairs + * @param table: The string table, given as a string of doubles. + * + * @note Table needs to be in same order as the vector table in the other constructor. + */ AlgebraicDecisionTree // (const std::vector& labelCs, const std::string& table) { @@ -108,7 +150,13 @@ namespace gtsam { Base::create(labelCs.begin(), labelCs.end(), ys.begin(), ys.end()); } - /** Create a new function splitting on a variable */ + /** + * @brief Create a range of decision trees, splitting on a single variable. + * + * @param begin: Iterator to beginning of a range of decision trees + * @param end: Iterator to end of a range of decision trees + * @param label: The label to split on + */ template AlgebraicDecisionTree(Iterator begin, Iterator end, const L& label) : Base(nullptr) { diff --git a/gtsam/discrete/DecisionTree-inl.h b/gtsam/discrete/DecisionTree-inl.h index 9f3d5e8f9..4d1670bb7 100644 --- a/gtsam/discrete/DecisionTree-inl.h +++ b/gtsam/discrete/DecisionTree-inl.h @@ -622,7 +622,7 @@ namespace gtsam { // B=1 // A=0: 3 // A=1: 4 - // Note, through the magic of "compose", create([A B],[1 2 3 4]) will produce + // Note, through the magic of "compose", create([A B],[1 3 2 4]) will produce // exactly the same tree as above: the highest label is always the root. // However, it will be *way* faster if labels are given highest to lowest. template diff --git a/gtsam/discrete/DecisionTree.h b/gtsam/discrete/DecisionTree.h index a8764a98f..06e945cf9 100644 --- a/gtsam/discrete/DecisionTree.h +++ b/gtsam/discrete/DecisionTree.h @@ -37,9 +37,23 @@ namespace gtsam { /** - * Decision Tree - * L = label for variables - * Y = function range (any algebra), e.g., bool, int, double + * @brief a decision tree is a function from assignments to values. + * @tparam L label for variables + * @tparam Y function range (any algebra), e.g., bool, int, double + * + * After creating a decision tree on some variables, the tree can be evaluated + * on an assignment to those variables. Example: + * + * @code{.cpp} + * // Create a decision stump one one variable 'a' with values 10 and 20. + * DecisionTree tree('a', 10, 20); + * + * // Evaluate the tree on an assignment to the variable. + * int value0 = tree({{'a', 0}}); // value0 = 10 + * int value1 = tree({{'a', 1}}); // value1 = 20 + * @endcode + * + * More examples can be found in testDecisionTree.cpp * * @ingroup discrete */ @@ -132,7 +146,8 @@ namespace gtsam { NodePtr root_; protected: - /** Internal recursive function to create from keys, cardinalities, + /** + * Internal recursive function to create from keys, cardinalities, * and Y values */ template @@ -163,7 +178,13 @@ namespace gtsam { /** Create a constant */ explicit DecisionTree(const Y& y); - /// Create tree with 2 assignments `y1`, `y2`, splitting on variable `label` + /** + * @brief Create tree with 2 assignments `y1`, `y2`, splitting on variable `label` + * + * @param label The variable to split on. + * @param y1 The value for the first assignment. + * @param y2 The value for the second assignment. + */ DecisionTree(const L& label, const Y& y1, const Y& y2); /** Allow Label+Cardinality for convenience */ diff --git a/gtsam/discrete/tests/testDecisionTree.cpp b/gtsam/discrete/tests/testDecisionTree.cpp index 2f385263c..fbcecb5ab 100644 --- a/gtsam/discrete/tests/testDecisionTree.cpp +++ b/gtsam/discrete/tests/testDecisionTree.cpp @@ -71,6 +71,19 @@ struct traits : public Testable {}; GTSAM_CONCEPT_TESTABLE_INST(CrazyDecisionTree) +/* ************************************************************************** */ +// Test char labels and int range +/* ************************************************************************** */ + +// Create a decision stump one one variable 'a' with values 10 and 20. +TEST(DecisionTree, constructor) { + DecisionTree tree('a', 10, 20); + + // Evaluate the tree on an assignment to the variable. + EXPECT_LONGS_EQUAL(10, tree({{'a', 0}})); + EXPECT_LONGS_EQUAL(20, tree({{'a', 1}})); +} + /* ************************************************************************** */ // Test string labels and int range /* ************************************************************************** */ @@ -114,18 +127,47 @@ struct Ring { static inline int mul(const int& a, const int& b) { return a * b; } }; +/* ************************************************************************** */ +// Check that creating decision trees respects key order. +TEST(DecisionTree, constructor_order) { + // Create labels + string A("A"), B("B"); + + const std::vector ys1 = {1, 2, 3, 4}; + DT tree1({{B, 2}, {A, 2}}, ys1); // faster version, as B is "higher" than A! + + const std::vector ys2 = {1, 3, 2, 4}; + DT tree2({{A, 2}, {B, 2}}, ys2); // slower version ! + + // Both trees will be the same, tree is order from high to low labels. + // Choice(B) + // 0 Choice(A) + // 0 0 Leaf 1 + // 0 1 Leaf 2 + // 1 Choice(A) + // 1 0 Leaf 3 + // 1 1 Leaf 4 + + EXPECT(tree2.equals(tree1)); + + // Check the values are as expected by calling the () operator: + EXPECT_LONGS_EQUAL(1, tree1({{A, 0}, {B, 0}})); + EXPECT_LONGS_EQUAL(3, tree1({{A, 0}, {B, 1}})); + EXPECT_LONGS_EQUAL(2, tree1({{A, 1}, {B, 0}})); + EXPECT_LONGS_EQUAL(4, tree1({{A, 1}, {B, 1}})); +} + /* ************************************************************************** */ // test DT TEST(DecisionTree, example) { // Create labels string A("A"), B("B"), C("C"); - // create a value - Assignment x00, x01, x10, x11; - x00[A] = 0, x00[B] = 0; - x01[A] = 0, x01[B] = 1; - x10[A] = 1, x10[B] = 0; - x11[A] = 1, x11[B] = 1; + // Create assignments using brace initialization: + Assignment x00{{A, 0}, {B, 0}}; + Assignment x01{{A, 0}, {B, 1}}; + Assignment x10{{A, 1}, {B, 0}}; + Assignment x11{{A, 1}, {B, 1}}; // empty DT empty; @@ -237,8 +279,7 @@ TEST(DecisionTree, ConvertValuesOnly) { StringBoolTree f2(f1, bool_of_int); // Check a value - Assignment x00; - x00["A"] = 0, x00["B"] = 0; + Assignment x00 {{A, 0}, {B, 0}}; EXPECT(!f2(x00)); } @@ -262,10 +303,11 @@ TEST(DecisionTree, ConvertBoth) { // Check some values Assignment