From f5e046fd109f291d1f4b22797e04a83fb7e4e62c Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 8 Aug 2022 17:17:22 -0400 Subject: [PATCH] split HybridNonlinearFactorGraph to .h and .cpp --- gtsam/hybrid/HybridNonlinearFactorGraph.cpp | 100 ++++++++++++++++++++ gtsam/hybrid/HybridNonlinearFactorGraph.h | 72 ++------------ 2 files changed, 108 insertions(+), 64 deletions(-) create mode 100644 gtsam/hybrid/HybridNonlinearFactorGraph.cpp diff --git a/gtsam/hybrid/HybridNonlinearFactorGraph.cpp b/gtsam/hybrid/HybridNonlinearFactorGraph.cpp new file mode 100644 index 000000000..233badc55 --- /dev/null +++ b/gtsam/hybrid/HybridNonlinearFactorGraph.cpp @@ -0,0 +1,100 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file HybridNonlinearFactorGraph.cpp + * @brief Nonlinear hybrid factor graph that uses type erasure + * @author Varun Agrawal + * @date May 28, 2022 + */ + +#include + +namespace gtsam { + +/* ************************************************************************* */ +void HybridNonlinearFactorGraph::add( + boost::shared_ptr factor) { + FactorGraph::add(boost::make_shared(factor)); +} + +/* ************************************************************************* */ +void HybridNonlinearFactorGraph::print(const std::string& s, + const KeyFormatter& keyFormatter) const { + // Base::print(str, keyFormatter); + std::cout << (s.empty() ? "" : s + " ") << std::endl; + std::cout << "size: " << size() << std::endl; + for (size_t i = 0; i < factors_.size(); i++) { + std::stringstream ss; + ss << "factor " << i << ": "; + if (factors_[i]) { + factors_[i]->print(ss.str(), keyFormatter); + std::cout << std::endl; + } + } +} + +/* ************************************************************************* */ +bool HybridNonlinearFactorGraph::equals(const HybridNonlinearFactorGraph& other, + double tol) const { + return false; +} + +/* ************************************************************************* */ +HybridGaussianFactorGraph HybridNonlinearFactorGraph::linearize( + const Values& continuousValues) const { + // create an empty linear FG + HybridGaussianFactorGraph linearFG; + + linearFG.reserve(size()); + + // linearize all hybrid factors + for (auto&& factor : factors_) { + // First check if it is a valid factor + if (factor) { + // Check if the factor is a hybrid factor. + // It can be either a nonlinear MixtureFactor or a linear + // GaussianMixtureFactor. + if (factor->isHybrid()) { + // Check if it is a nonlinear mixture factor + if (auto nlmf = boost::dynamic_pointer_cast(factor)) { + linearFG.push_back(nlmf->linearize(continuousValues)); + } else { + linearFG.push_back(factor); + } + + // Now check if the factor is a continuous only factor. + } else if (factor->isContinuous()) { + // In this case, we check if factor->inner() is nonlinear since + // HybridFactors wrap over continuous factors. + auto nlhf = boost::dynamic_pointer_cast(factor); + if (auto nlf = + boost::dynamic_pointer_cast(nlhf->inner())) { + auto hgf = boost::make_shared( + nlf->linearize(continuousValues)); + linearFG.push_back(hgf); + } else { + linearFG.push_back(factor); + } + // Finally if nothing else, we are discrete-only which doesn't need + // lineariztion. + } else { + linearFG.push_back(factor); + } + + } else { + linearFG.push_back(GaussianFactor::shared_ptr()); + } + } + return linearFG; +} + +} // namespace gtsam diff --git a/gtsam/hybrid/HybridNonlinearFactorGraph.h b/gtsam/hybrid/HybridNonlinearFactorGraph.h index 6c5dd515f..073581da2 100644 --- a/gtsam/hybrid/HybridNonlinearFactorGraph.h +++ b/gtsam/hybrid/HybridNonlinearFactorGraph.h @@ -18,9 +18,9 @@ #pragma once -#include #include #include +#include #include #include #include @@ -115,25 +115,15 @@ class GTSAM_EXPORT HybridNonlinearFactorGraph : public HybridFactorGraph { // } /// Add a nonlinear factor as a shared ptr. - void add(boost::shared_ptr factor) { - FactorGraph::add(boost::make_shared(factor)); - } + void add(boost::shared_ptr factor); - /** - * Simply prints the factor graph. - */ + /// Print the factor graph. void print( - const std::string& str = "HybridNonlinearFactorGraph", - const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {} + const std::string& s = "HybridNonlinearFactorGraph", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override; - /** - * @return true if all internal graphs of `this` are equal to those of - * `other` - */ - bool equals(const HybridNonlinearFactorGraph& other, - double tol = 1e-9) const { - return false; - } + /// Check if this factor graph is equal to `other`. + bool equals(const HybridNonlinearFactorGraph& other, double tol = 1e-9) const; /** * @brief Linearize all the continuous factors in the @@ -142,53 +132,7 @@ class GTSAM_EXPORT HybridNonlinearFactorGraph : public HybridFactorGraph { * @param continuousValues: Dictionary of continuous values. * @return HybridGaussianFactorGraph::shared_ptr */ - HybridGaussianFactorGraph linearize(const Values& continuousValues) const { - // create an empty linear FG - HybridGaussianFactorGraph linearFG; - - linearFG.reserve(size()); - - // linearize all hybrid factors - for (auto&& factor : factors_) { - // First check if it is a valid factor - if (factor) { - // Check if the factor is a hybrid factor. - // It can be either a nonlinear MixtureFactor or a linear - // GaussianMixtureFactor. - if (factor->isHybrid()) { - // Check if it is a nonlinear mixture factor - if (auto nlmf = boost::dynamic_pointer_cast(factor)) { - linearFG.push_back(nlmf->linearize(continuousValues)); - } else { - linearFG.push_back(factor); - } - - // Now check if the factor is a continuous only factor. - } else if (factor->isContinuous()) { - // In this case, we check if factor->inner() is nonlinear since - // HybridFactors wrap over continuous factors. - auto nlhf = - boost::dynamic_pointer_cast(factor); - if (auto nlf = - boost::dynamic_pointer_cast(nlhf->inner())) { - auto hgf = boost::make_shared( - nlf->linearize(continuousValues)); - linearFG.push_back(hgf); - } else { - linearFG.push_back(factor); - } - // Finally if nothing else, we are discrete-only which doesn't need - // lineariztion. - } else { - linearFG.push_back(factor); - } - - } else { - linearFG.push_back(GaussianFactor::shared_ptr()); - } - } - return linearFG; - } + HybridGaussianFactorGraph linearize(const Values& continuousValues) const; }; template <>