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); } /* ************************************************************************* */