From 18f4d8b24c3f36d072fb68fe75ce17869f708444 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 3 Jan 2023 12:34:41 -0500 Subject: [PATCH 1/6] Repro issue #1341 --- tests/testNonlinearFactorGraph.cpp | 78 ++++++++++++++++++++++++++++++ 1 file changed, 78 insertions(+) diff --git a/tests/testNonlinearFactorGraph.cpp b/tests/testNonlinearFactorGraph.cpp index e1a88d6169..f8d0b5fdcc 100644 --- a/tests/testNonlinearFactorGraph.cpp +++ b/tests/testNonlinearFactorGraph.cpp @@ -25,6 +25,7 @@ #include #include #include +#include #include #include #include @@ -388,6 +389,83 @@ TEST(NonlinearFactorGraph, dot_extra) { EXPECT(ss.str() == expected); } +/* ************************************************************************* */ +template +class MyPrior : public gtsam::NoiseModelFactorN { + private: + VALUE prior_; + + public: + MyPrior(gtsam::Key key, const VALUE &prior, + const gtsam::SharedNoiseModel &model) + : gtsam::NoiseModelFactorN(model, key), prior_(prior) {} + + gtsam::Vector evaluateError( + const VALUE &val, + boost::optional H = boost::none) const override { + if (H) + (*H) = gtsam::Matrix::Identity(gtsam::traits::GetDimension(val), + gtsam::traits::GetDimension(val)); + // manifold equivalent of z-x -> Local(x,z) + return -gtsam::traits::Local(val, prior_); + } + + virtual gtsam::NonlinearFactor::shared_ptr clone() const override { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new MyPrior(*this))); + } +}; + +template +class MyPriorPrint : public gtsam::NoiseModelFactorN { + private: + VALUE prior_; + + public: + MyPriorPrint(gtsam::Key key, const VALUE &prior, + const gtsam::SharedNoiseModel &model) + : gtsam::NoiseModelFactorN(model, key), prior_(prior) {} + + gtsam::Vector evaluateError( + const VALUE &val, + boost::optional H = boost::none) const override { + if (H) + (*H) = gtsam::Matrix::Identity(gtsam::traits::GetDimension(val), + gtsam::traits::GetDimension(val)); + // manifold equivalent of z-x -> Local(x,z) + auto error = -gtsam::traits::Local(val, prior_); + val.print(); + prior_.print(); + return error; + } + + virtual gtsam::NonlinearFactor::shared_ptr clone() const override { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new MyPriorPrint(*this))); + } +}; + +TEST(NonlinearFactorGraph, NoPrintSideEffects) { + NonlinearFactorGraph fg; + Values vals; + const auto model = noiseModel::Unit::Create(3); + fg.emplace_shared>(0, Pose2(0, 0, 0), model); + vals.insert(0, Pose2(1, 1, 1)); + + NonlinearFactorGraph fg_print; + Values vals_print; + fg_print.emplace_shared>(0, Pose2(0, 0, 0), model); + vals_print.insert(0, Pose2(1, 1, 1)); + + std::cout << "Without Prints:" << std::endl; + GaussNewtonOptimizer optimizer(fg, vals); + optimizer.optimize().print(); + + std::cout << "With Prints:" << std::endl; + GaussNewtonOptimizer optimizer_print(fg_print, vals_print); + optimizer_print.optimize().print(); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ From 549e0b1e64d4c4d9d7cc6d87323e892e0e6f0cd1 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 3 Jan 2023 15:14:41 -0500 Subject: [PATCH 2/6] Fix argument to const & --- gtsam/linear/NoiseModel.cpp | 7 ++++--- gtsam/linear/NoiseModel.h | 2 +- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index 8bcef6fcce..7108a7660f 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -47,8 +47,9 @@ void updateAb(MATRIX& Ab, int j, const Vector& a, const Vector& rd) { /* ************************************************************************* */ // check *above the diagonal* for non-zero entries -boost::optional checkIfDiagonal(const Matrix M) { +boost::optional checkIfDiagonal(const Matrix& M) { size_t m = M.rows(), n = M.cols(); + assert(m > 0); // check all non-diagonal entries bool full = false; size_t i, j; @@ -92,7 +93,7 @@ Gaussian::shared_ptr Gaussian::SqrtInformation(const Matrix& R, bool smart) { return Diagonal::Sigmas(diagonal->array().inverse(), true); } // NOTE(frank): only reaches here if !(smart && diagonal) - return shared_ptr(new Gaussian(R.rows(), R)); + return boost::make_shared(R.rows(), R); } /* ************************************************************************* */ @@ -108,7 +109,7 @@ Gaussian::shared_ptr Gaussian::Information(const Matrix& information, bool smart else { Eigen::LLT llt(information); Matrix R = llt.matrixU(); - return shared_ptr(new Gaussian(n, R)); + return boost::make_shared(n, R); } } diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 5a29e5d7dd..7bcf808e50 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -732,7 +732,7 @@ namespace gtsam { }; // Helper function - GTSAM_EXPORT boost::optional checkIfDiagonal(const Matrix M); + GTSAM_EXPORT boost::optional checkIfDiagonal(const Matrix& M); } // namespace noiseModel From a69c0864457a8c36303fd36a5088621e2384ac31 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 3 Jan 2023 15:15:07 -0500 Subject: [PATCH 3/6] Fix count issue --- gtsam/nonlinear/ISAM2Clique.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/nonlinear/ISAM2Clique.cpp b/gtsam/nonlinear/ISAM2Clique.cpp index f7a1c14a03..b3dc49342b 100644 --- a/gtsam/nonlinear/ISAM2Clique.cpp +++ b/gtsam/nonlinear/ISAM2Clique.cpp @@ -245,7 +245,7 @@ bool ISAM2Clique::optimizeWildfireNode(const KeySet& replaced, double threshold, // Back-substitute fastBackSubstitute(delta); - count += conditional_->nrFrontals(); + *count += conditional_->nrFrontals(); if (valuesChanged(replaced, originalValues, *delta, threshold)) { markFrontalsAsChanged(changed); From 4f2615aeeb557c194bf59793c120f10f258c1ef1 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 3 Jan 2023 15:15:21 -0500 Subject: [PATCH 4/6] Fix loop variable --- gtsam/base/Vector.cpp | 11 ++++------- 1 file changed, 4 insertions(+), 7 deletions(-) diff --git a/gtsam/base/Vector.cpp b/gtsam/base/Vector.cpp index 658ab9a0d4..16d913a967 100644 --- a/gtsam/base/Vector.cpp +++ b/gtsam/base/Vector.cpp @@ -299,17 +299,14 @@ weightedPseudoinverse(const Vector& a, const Vector& weights) { } /* ************************************************************************* */ -Vector concatVectors(const std::list& vs) -{ +Vector concatVectors(const std::list& vs) { size_t dim = 0; - for(Vector v: vs) - dim += v.size(); + for (const Vector& v : vs) dim += v.size(); Vector A(dim); size_t index = 0; - for(Vector v: vs) { - for(int d = 0; d < v.size(); d++) - A(d+index) = v(d); + for (const Vector& v : vs) { + for (int d = 0; d < v.size(); d++) A(d + index) = v(d); index += v.size(); } From 65aaf47a1cae1b341aae07fd8220a37ed2beaeab Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 3 Jan 2023 15:21:44 -0500 Subject: [PATCH 5/6] Remove obsolete test --- tests/testNonlinearFactorGraph.cpp | 77 ------------------------------ 1 file changed, 77 deletions(-) diff --git a/tests/testNonlinearFactorGraph.cpp b/tests/testNonlinearFactorGraph.cpp index f8d0b5fdcc..c9b42ff9ae 100644 --- a/tests/testNonlinearFactorGraph.cpp +++ b/tests/testNonlinearFactorGraph.cpp @@ -389,83 +389,6 @@ TEST(NonlinearFactorGraph, dot_extra) { EXPECT(ss.str() == expected); } -/* ************************************************************************* */ -template -class MyPrior : public gtsam::NoiseModelFactorN { - private: - VALUE prior_; - - public: - MyPrior(gtsam::Key key, const VALUE &prior, - const gtsam::SharedNoiseModel &model) - : gtsam::NoiseModelFactorN(model, key), prior_(prior) {} - - gtsam::Vector evaluateError( - const VALUE &val, - boost::optional H = boost::none) const override { - if (H) - (*H) = gtsam::Matrix::Identity(gtsam::traits::GetDimension(val), - gtsam::traits::GetDimension(val)); - // manifold equivalent of z-x -> Local(x,z) - return -gtsam::traits::Local(val, prior_); - } - - virtual gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new MyPrior(*this))); - } -}; - -template -class MyPriorPrint : public gtsam::NoiseModelFactorN { - private: - VALUE prior_; - - public: - MyPriorPrint(gtsam::Key key, const VALUE &prior, - const gtsam::SharedNoiseModel &model) - : gtsam::NoiseModelFactorN(model, key), prior_(prior) {} - - gtsam::Vector evaluateError( - const VALUE &val, - boost::optional H = boost::none) const override { - if (H) - (*H) = gtsam::Matrix::Identity(gtsam::traits::GetDimension(val), - gtsam::traits::GetDimension(val)); - // manifold equivalent of z-x -> Local(x,z) - auto error = -gtsam::traits::Local(val, prior_); - val.print(); - prior_.print(); - return error; - } - - virtual gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new MyPriorPrint(*this))); - } -}; - -TEST(NonlinearFactorGraph, NoPrintSideEffects) { - NonlinearFactorGraph fg; - Values vals; - const auto model = noiseModel::Unit::Create(3); - fg.emplace_shared>(0, Pose2(0, 0, 0), model); - vals.insert(0, Pose2(1, 1, 1)); - - NonlinearFactorGraph fg_print; - Values vals_print; - fg_print.emplace_shared>(0, Pose2(0, 0, 0), model); - vals_print.insert(0, Pose2(1, 1, 1)); - - std::cout << "Without Prints:" << std::endl; - GaussNewtonOptimizer optimizer(fg, vals); - optimizer.optimize().print(); - - std::cout << "With Prints:" << std::endl; - GaussNewtonOptimizer optimizer_print(fg_print, vals_print); - optimizer_print.optimize().print(); -} - /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ From 7b56c167dea95354c8afada8537a59860d8ab9c5 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 3 Jan 2023 15:22:21 -0500 Subject: [PATCH 6/6] remove obsolete include --- tests/testNonlinearFactorGraph.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/tests/testNonlinearFactorGraph.cpp b/tests/testNonlinearFactorGraph.cpp index c9b42ff9ae..e1a88d6169 100644 --- a/tests/testNonlinearFactorGraph.cpp +++ b/tests/testNonlinearFactorGraph.cpp @@ -25,7 +25,6 @@ #include #include #include -#include #include #include #include