12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Remove HybridNonlinearFactor wrapper class as no longer needed

release/4.3a0
Frank Dellaert 2023-01-06 19:01:52 -08:00
parent 1538452d5a
commit 88129d9f45
12 changed files with 49 additions and 271 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
gtsam/hybrid/HybridFactorGraph.h
View File

@ -99,10 +99,6 @@ class HybridFactorGraph : public FactorGraph<Factor> {
Base::push_back(hybridFactor); Base::push_back(hybridFactor);
} }
/// delete emplace_shared.
template <class FACTOR, class... Args>
void emplace_shared(Args&&... args) = delete;
/// Construct a factor and add (shared pointer to it) to factor graph. /// Construct a factor and add (shared pointer to it) to factor graph.
template <class FACTOR, class... Args> template <class FACTOR, class... Args>
IsDiscrete<FACTOR> emplace_discrete(Args&&... args) { IsDiscrete<FACTOR> emplace_discrete(Args&&... args) {
@ -119,22 +115,6 @@ class HybridFactorGraph : public FactorGraph<Factor> {
push_hybrid(factor); push_hybrid(factor);
} }
/**
* @brief Add a single factor shared pointer to the hybrid factor graph.
* Dynamically handles the factor type and assigns it to the correct
* underlying container.
*
* @param sharedFactor The factor to add to this factor graph.
*/
void push_back(const SharedFactor& sharedFactor) {
if (auto p = boost::dynamic_pointer_cast<DiscreteFactor>(sharedFactor)) {
push_discrete(p);
}
if (auto p = boost::dynamic_pointer_cast<HybridFactor>(sharedFactor)) {
push_hybrid(p);
}
}
/// Get all the discrete keys in the factor graph. /// Get all the discrete keys in the factor graph.
const KeySet discreteKeys() const { const KeySet discreteKeys() const {
KeySet discrete_keys; KeySet discrete_keys;

41
gtsam/hybrid/HybridNonlinearFactor.cpp
View File

@ -1,41 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 HybridNonlinearFactor.cpp
* @date May 28, 2022
* @author Varun Agrawal
*/
#include <gtsam/hybrid/HybridNonlinearFactor.h>
#include <boost/make_shared.hpp>
namespace gtsam {
/* ************************************************************************* */
HybridNonlinearFactor::HybridNonlinearFactor(
const NonlinearFactor::shared_ptr &other)
: Base(other->keys()), inner_(other) {}
/* ************************************************************************* */
bool HybridNonlinearFactor::equals(const HybridFactor &lf, double tol) const {
return Base::equals(lf, tol);
}
/* ************************************************************************* */
void HybridNonlinearFactor::print(const std::string &s,
const KeyFormatter &formatter) const {
HybridFactor::print(s, formatter);
inner_->print("\n", formatter);
};
} // namespace gtsam

73
gtsam/hybrid/HybridNonlinearFactor.h
View File

@ -1,73 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 HybridNonlinearFactor.h
* @date May 28, 2022
* @author Varun Agrawal
*/
#pragma once
#include <gtsam/hybrid/HybridFactor.h>
#include <gtsam/hybrid/HybridGaussianFactor.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
namespace gtsam {
/**
* A HybridNonlinearFactor is a layer over NonlinearFactor so that we do not
* have a diamond inheritance.
*/
class HybridNonlinearFactor : public HybridFactor {
private:
NonlinearFactor::shared_ptr inner_;
public:
using Base = HybridFactor;
using This = HybridNonlinearFactor;
using shared_ptr = boost::shared_ptr<This>;
// Explicit conversion from a shared ptr of NonlinearFactor
explicit HybridNonlinearFactor(const NonlinearFactor::shared_ptr &other);
/// @name Testable
/// @{
/// Check equality.
virtual bool equals(const HybridFactor &lf, double tol) const override;
/// GTSAM print utility.
void print(
const std::string &s = "HybridNonlinearFactor\n",
const KeyFormatter &formatter = DefaultKeyFormatter) const override;
/// @}
/// @name Standard Interface
/// @{
/// Return pointer to the internal nonlinear factor.
NonlinearFactor::shared_ptr inner() const { return inner_; }
/// Error for HybridValues is not provided for nonlinear factor.
double error(const HybridValues &values) const override {
throw std::runtime_error(
"HybridNonlinearFactor::error(HybridValues) not implemented.");
}
/// Linearize to a HybridGaussianFactor at the linearization point `c`.
boost::shared_ptr<HybridGaussianFactor> linearize(const Values &c) const {
return boost::make_shared<HybridGaussianFactor>(inner_->linearize(c));
}
/// @}
};
} // namespace gtsam

21
gtsam/hybrid/HybridNonlinearFactorGraph.cpp
View File

@ -20,17 +20,6 @@
namespace gtsam { namespace gtsam {
/* ************************************************************************* */
void HybridNonlinearFactorGraph::add(
boost::shared_ptr<NonlinearFactor> factor) {
FactorGraph::add(boost::make_shared<HybridNonlinearFactor>(factor));
}
/* ************************************************************************* */
void HybridNonlinearFactorGraph::add(boost::shared_ptr<DiscreteFactor> factor) {
FactorGraph::add(boost::make_shared<HybridDiscreteFactor>(factor));
}
/* ************************************************************************* */ /* ************************************************************************* */
void HybridNonlinearFactorGraph::print(const std::string& s, void HybridNonlinearFactorGraph::print(const std::string& s,
const KeyFormatter& keyFormatter) const { const KeyFormatter& keyFormatter) const {
@ -66,14 +55,12 @@ HybridGaussianFactorGraph::shared_ptr HybridNonlinearFactorGraph::linearize(
continue; continue;
} }
// Check if it is a nonlinear mixture factor // Check if it is a nonlinear mixture factor
if (auto nlmf = dynamic_pointer_cast<MixtureFactor>(f)) { if (auto mf = dynamic_pointer_cast<MixtureFactor>(f)) {
const GaussianMixtureFactor::shared_ptr& gmf = const GaussianMixtureFactor::shared_ptr& gmf =
nlmf->linearize(continuousValues); mf->linearize(continuousValues);
linearFG->push_back(gmf); linearFG->push_back(gmf);
} else if (auto nlhf = dynamic_pointer_cast<HybridNonlinearFactor>(f)) { } else if (auto nlf = dynamic_pointer_cast<NonlinearFactor>(f)) {
// Nonlinear wrapper case: const GaussianFactor::shared_ptr& gf = nlf->linearize(continuousValues);
const GaussianFactor::shared_ptr& gf =
nlhf->inner()->linearize(continuousValues);
const auto hgf = boost::make_shared<HybridGaussianFactor>(gf); const auto hgf = boost::make_shared<HybridGaussianFactor>(gf);
linearFG->push_back(hgf); linearFG->push_back(hgf);
} else if (dynamic_pointer_cast<DiscreteFactor>(f) || } else if (dynamic_pointer_cast<DiscreteFactor>(f) ||

75
gtsam/hybrid/HybridNonlinearFactorGraph.h
View File

@ -21,7 +21,6 @@
#include <gtsam/hybrid/HybridFactor.h> #include <gtsam/hybrid/HybridFactor.h>
#include <gtsam/hybrid/HybridFactorGraph.h> #include <gtsam/hybrid/HybridFactorGraph.h>
#include <gtsam/hybrid/HybridGaussianFactorGraph.h> #include <gtsam/hybrid/HybridGaussianFactorGraph.h>
#include <gtsam/hybrid/HybridNonlinearFactor.h>
#include <gtsam/hybrid/MixtureFactor.h> #include <gtsam/hybrid/MixtureFactor.h>
#include <gtsam/inference/Ordering.h> #include <gtsam/inference/Ordering.h>
#include <gtsam/nonlinear/NonlinearFactor.h> #include <gtsam/nonlinear/NonlinearFactor.h>
@ -37,16 +36,6 @@ namespace gtsam {
*/ */
class GTSAM_EXPORT HybridNonlinearFactorGraph : public HybridFactorGraph { class GTSAM_EXPORT HybridNonlinearFactorGraph : public HybridFactorGraph {
protected: protected:
/// Check if FACTOR type is derived from NonlinearFactor.
template <typename FACTOR>
using IsNonlinear = typename std::enable_if<
std::is_base_of<NonlinearFactor, FACTOR>::value>::type;
/// Check if T has a value_type derived from FactorType.
template <typename T>
using HasDerivedValueType = typename std::enable_if<
std::is_base_of<HybridFactor, typename T::value_type>::value>::type;
public: public:
using Base = HybridFactorGraph; using Base = HybridFactorGraph;
using This = HybridNonlinearFactorGraph; ///< this class using This = HybridNonlinearFactorGraph; ///< this class
@ -71,70 +60,6 @@ class GTSAM_EXPORT HybridNonlinearFactorGraph : public HybridFactorGraph {
/// @} /// @}
// Allow use of selected FactorGraph methods:
using Base::empty;
using Base::reserve;
using Base::size;
using Base::operator[];
using Base::add;
using Base::resize;
/**
* Add a nonlinear factor *pointer* to the internal nonlinear factor graph
* @param nonlinearFactor - boost::shared_ptr to the factor to add
*/
template <typename FACTOR>
IsNonlinear<FACTOR> push_nonlinear(
const boost::shared_ptr<FACTOR>& nonlinearFactor) {
Base::push_back(boost::make_shared<HybridNonlinearFactor>(nonlinearFactor));
}
/// Construct a factor and add (shared pointer to it) to factor graph.
template <class FACTOR, class... Args>
IsNonlinear<FACTOR> emplace_nonlinear(Args&&... args) {
auto factor = boost::allocate_shared<FACTOR>(
Eigen::aligned_allocator<FACTOR>(), std::forward<Args>(args)...);
push_nonlinear(factor);
}
/**
* @brief Add a single factor shared pointer to the hybrid factor graph.
* Dynamically handles the factor type and assigns it to the correct
* underlying container.
*
* @tparam FACTOR The factor type template
* @param sharedFactor The factor to add to this factor graph.
*/
template <typename FACTOR>
void push_back(const boost::shared_ptr<FACTOR>& sharedFactor) {
if (auto p = boost::dynamic_pointer_cast<NonlinearFactor>(sharedFactor)) {
push_nonlinear(p);
} else {
Base::push_back(sharedFactor);
}
}
/**
* Push back many factors as shared_ptr's in a container (factors are not
* copied)
*/
template <typename CONTAINER>
void push_back(const CONTAINER& container) {
Base::push_back(container.begin(), container.end());
}
/// Push back non-pointer objects in a container (factors are copied).
template <typename CONTAINER>
HasDerivedValueType<CONTAINER> push_back(const CONTAINER& container) {
Base::push_back(container.begin(), container.end());
}
/// Add a nonlinear factor as a shared ptr.
void add(boost::shared_ptr<NonlinearFactor> factor);
/// Add a discrete factor as a shared ptr.
void add(boost::shared_ptr<DiscreteFactor> factor);
/// Print the factor graph. /// Print the factor graph.
void print( void print(
const std::string& s = "HybridNonlinearFactorGraph", const std::string& s = "HybridNonlinearFactorGraph",

1
gtsam/hybrid/MixtureFactor.h
View File

@ -21,7 +21,6 @@
#include <gtsam/discrete/DiscreteValues.h> #include <gtsam/discrete/DiscreteValues.h>
#include <gtsam/hybrid/GaussianMixtureFactor.h> #include <gtsam/hybrid/GaussianMixtureFactor.h>
#include <gtsam/hybrid/HybridNonlinearFactor.h>
#include <gtsam/nonlinear/NonlinearFactor.h> #include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Symbol.h> #include <gtsam/nonlinear/Symbol.h>

11
gtsam/hybrid/tests/Switching.h
View File

@ -136,6 +136,8 @@ struct Switching {
std::vector<double> measurements = {}, std::vector<double> measurements = {},
std::string discrete_transition_prob = "1/2 3/2") std::string discrete_transition_prob = "1/2 3/2")
: K(K) { : K(K) {
using noiseModel::Isotropic;
// Create DiscreteKeys for binary K modes. // Create DiscreteKeys for binary K modes.
for (size_t k = 0; k < K; k++) { for (size_t k = 0; k < K; k++) {
modes.emplace_back(M(k), 2); modes.emplace_back(M(k), 2);
@ -150,9 +152,8 @@ struct Switching {
// Create hybrid factor graph. // Create hybrid factor graph.
// Add a prior on X(0). // Add a prior on X(0).
auto prior = boost::make_shared<PriorFactor<double>>( nonlinearFactorGraph.emplace_shared<PriorFactor<double>>(
X(0), measurements.at(0), noiseModel::Isotropic::Sigma(1, prior_sigma)); X(0), measurements.at(0), Isotropic::Sigma(1, prior_sigma));
nonlinearFactorGraph.push_nonlinear(prior);
// Add "motion models". // Add "motion models".
for (size_t k = 0; k < K - 1; k++) { for (size_t k = 0; k < K - 1; k++) {
@ -167,9 +168,9 @@ struct Switching {
} }
// Add measurement factors // Add measurement factors
auto measurement_noise = noiseModel::Isotropic::Sigma(1, prior_sigma); auto measurement_noise = Isotropic::Sigma(1, prior_sigma);
for (size_t k = 1; k < K; k++) { for (size_t k = 1; k < K; k++) {
nonlinearFactorGraph.emplace_nonlinear<PriorFactor<double>>( nonlinearFactorGraph.emplace_shared<PriorFactor<double>>(
X(k), measurements.at(k), measurement_noise); X(k), measurements.at(k), measurement_noise);
} }

2
gtsam/hybrid/tests/testHybridBayesNet.cpp
View File

@ -336,7 +336,7 @@ TEST(HybridBayesNet, Sampling) {
auto one_motion = auto one_motion =
boost::make_shared<BetweenFactor<double>>(X(0), X(1), 1, noise_model); boost::make_shared<BetweenFactor<double>>(X(0), X(1), 1, noise_model);
std::vector<NonlinearFactor::shared_ptr> factors = {zero_motion, one_motion}; std::vector<NonlinearFactor::shared_ptr> factors = {zero_motion, one_motion};
nfg.emplace_nonlinear<PriorFactor<double>>(X(0), 0.0, noise_model); nfg.emplace_shared<PriorFactor<double>>(X(0), 0.0, noise_model);
nfg.emplace_hybrid<MixtureFactor>( nfg.emplace_hybrid<MixtureFactor>(
KeyVector{X(0), X(1)}, DiscreteKeys{DiscreteKey(M(0), 2)}, factors); KeyVector{X(0), X(1)}, DiscreteKeys{DiscreteKey(M(0), 2)}, factors);

4
gtsam/hybrid/tests/testHybridEstimation.cpp
View File

@ -407,8 +407,8 @@ static HybridNonlinearFactorGraph createHybridNonlinearFactorGraph() {
const auto noise_model = noiseModel::Isotropic::Sigma(1, sigma); const auto noise_model = noiseModel::Isotropic::Sigma(1, sigma);
// Add "measurement" factors: // Add "measurement" factors:
nfg.emplace_nonlinear<PriorFactor<double>>(X(0), 0.0, noise_model); nfg.emplace_shared<PriorFactor<double>>(X(0), 0.0, noise_model);
nfg.emplace_nonlinear<PriorFactor<double>>(X(1), 1.0, noise_model); nfg.emplace_shared<PriorFactor<double>>(X(1), 1.0, noise_model);
// Add mixture factor: // Add mixture factor:
DiscreteKey m(M(0), 2); DiscreteKey m(M(0), 2);

32
gtsam/hybrid/tests/testHybridGaussianISAM.cpp
View File

@ -380,7 +380,7 @@ TEST(HybridGaussianISAM, NonTrivial) {
Pose2 prior(0.0, 0.0, 0.0); // prior mean is at origin Pose2 prior(0.0, 0.0, 0.0); // prior mean is at origin
auto priorNoise = noiseModel::Diagonal::Sigmas( auto priorNoise = noiseModel::Diagonal::Sigmas(
Vector3(0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta Vector3(0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta
fg.emplace_nonlinear<PriorFactor<Pose2>>(X(0), prior, priorNoise); fg.emplace_shared<PriorFactor<Pose2>>(X(0), prior, priorNoise);
// create a noise model for the landmark measurements // create a noise model for the landmark measurements
auto poseNoise = noiseModel::Isotropic::Sigma(3, 0.1); auto poseNoise = noiseModel::Isotropic::Sigma(3, 0.1);
@ -389,11 +389,11 @@ TEST(HybridGaussianISAM, NonTrivial) {
// where X is the base link and W is the foot link. // where X is the base link and W is the foot link.
// Add connecting poses similar to PoseFactors in GTD // Add connecting poses similar to PoseFactors in GTD
fg.emplace_nonlinear<BetweenFactor<Pose2>>(X(0), Y(0), Pose2(0, 1.0, 0), fg.emplace_shared<BetweenFactor<Pose2>>(X(0), Y(0), Pose2(0, 1.0, 0),
poseNoise); poseNoise);
fg.emplace_nonlinear<BetweenFactor<Pose2>>(Y(0), Z(0), Pose2(0, 1.0, 0), fg.emplace_shared<BetweenFactor<Pose2>>(Y(0), Z(0), Pose2(0, 1.0, 0),
poseNoise); poseNoise);
fg.emplace_nonlinear<BetweenFactor<Pose2>>(Z(0), W(0), Pose2(0, 1.0, 0), fg.emplace_shared<BetweenFactor<Pose2>>(Z(0), W(0), Pose2(0, 1.0, 0),
poseNoise); poseNoise);
// Create initial estimate // Create initial estimate
@ -432,14 +432,14 @@ TEST(HybridGaussianISAM, NonTrivial) {
fg.push_back(mixtureFactor); fg.push_back(mixtureFactor);
// Add equivalent of ImuFactor // Add equivalent of ImuFactor
fg.emplace_nonlinear<BetweenFactor<Pose2>>(X(0), X(1), Pose2(1.0, 0.0, 0), fg.emplace_shared<BetweenFactor<Pose2>>(X(0), X(1), Pose2(1.0, 0.0, 0),
poseNoise); poseNoise);
// PoseFactors-like at k=1 // PoseFactors-like at k=1
fg.emplace_nonlinear<BetweenFactor<Pose2>>(X(1), Y(1), Pose2(0, 1, 0), fg.emplace_shared<BetweenFactor<Pose2>>(X(1), Y(1), Pose2(0, 1, 0),
poseNoise); poseNoise);
fg.emplace_nonlinear<BetweenFactor<Pose2>>(Y(1), Z(1), Pose2(0, 1, 0), fg.emplace_shared<BetweenFactor<Pose2>>(Y(1), Z(1), Pose2(0, 1, 0),
poseNoise); poseNoise);
fg.emplace_nonlinear<BetweenFactor<Pose2>>(Z(1), W(1), Pose2(-1, 1, 0), fg.emplace_shared<BetweenFactor<Pose2>>(Z(1), W(1), Pose2(-1, 1, 0),
poseNoise); poseNoise);
initial.insert(X(1), Pose2(1.0, 0.0, 0.0)); initial.insert(X(1), Pose2(1.0, 0.0, 0.0));
@ -472,14 +472,14 @@ TEST(HybridGaussianISAM, NonTrivial) {
fg.push_back(mixtureFactor); fg.push_back(mixtureFactor);
// Add equivalent of ImuFactor // Add equivalent of ImuFactor
fg.emplace_nonlinear<BetweenFactor<Pose2>>(X(1), X(2), Pose2(1.0, 0.0, 0), fg.emplace_shared<BetweenFactor<Pose2>>(X(1), X(2), Pose2(1.0, 0.0, 0),
poseNoise); poseNoise);
// PoseFactors-like at k=1 // PoseFactors-like at k=1
fg.emplace_nonlinear<BetweenFactor<Pose2>>(X(2), Y(2), Pose2(0, 1, 0), fg.emplace_shared<BetweenFactor<Pose2>>(X(2), Y(2), Pose2(0, 1, 0),
poseNoise); poseNoise);
fg.emplace_nonlinear<BetweenFactor<Pose2>>(Y(2), Z(2), Pose2(0, 1, 0), fg.emplace_shared<BetweenFactor<Pose2>>(Y(2), Z(2), Pose2(0, 1, 0),
poseNoise); poseNoise);
fg.emplace_nonlinear<BetweenFactor<Pose2>>(Z(2), W(2), Pose2(-2, 1, 0), fg.emplace_shared<BetweenFactor<Pose2>>(Z(2), W(2), Pose2(-2, 1, 0),
poseNoise); poseNoise);
initial.insert(X(2), Pose2(2.0, 0.0, 0.0)); initial.insert(X(2), Pose2(2.0, 0.0, 0.0));
@ -515,14 +515,14 @@ TEST(HybridGaussianISAM, NonTrivial) {
fg.push_back(mixtureFactor); fg.push_back(mixtureFactor);
// Add equivalent of ImuFactor // Add equivalent of ImuFactor
fg.emplace_nonlinear<BetweenFactor<Pose2>>(X(2), X(3), Pose2(1.0, 0.0, 0), fg.emplace_shared<BetweenFactor<Pose2>>(X(2), X(3), Pose2(1.0, 0.0, 0),
poseNoise); poseNoise);
// PoseFactors-like at k=3 // PoseFactors-like at k=3
fg.emplace_nonlinear<BetweenFactor<Pose2>>(X(3), Y(3), Pose2(0, 1, 0), fg.emplace_shared<BetweenFactor<Pose2>>(X(3), Y(3), Pose2(0, 1, 0),
poseNoise); poseNoise);
fg.emplace_nonlinear<BetweenFactor<Pose2>>(Y(3), Z(3), Pose2(0, 1, 0), fg.emplace_shared<BetweenFactor<Pose2>>(Y(3), Z(3), Pose2(0, 1, 0),
poseNoise); poseNoise);
fg.emplace_nonlinear<BetweenFactor<Pose2>>(Z(3), W(3), Pose2(-3, 1, 0), fg.emplace_shared<BetweenFactor<Pose2>>(Z(3), W(3), Pose2(-3, 1, 0),
poseNoise); poseNoise);
initial.insert(X(3), Pose2(3.0, 0.0, 0.0)); initial.insert(X(3), Pose2(3.0, 0.0, 0.0));

8
gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp
View File

@ -54,7 +54,7 @@ TEST(HybridFactorGraph, GaussianFactorGraph) {
HybridNonlinearFactorGraph fg; HybridNonlinearFactorGraph fg;
// Add a simple prior factor to the nonlinear factor graph // Add a simple prior factor to the nonlinear factor graph
fg.emplace_nonlinear<PriorFactor<double>>(X(0), 0, Isotropic::Sigma(1, 0.1)); fg.emplace_shared<PriorFactor<double>>(X(0), 0, Isotropic::Sigma(1, 0.1));
// Linearization point // Linearization point
Values linearizationPoint; Values linearizationPoint;
@ -683,7 +683,7 @@ TEST(HybridFactorGraph, DefaultDecisionTree) {
Pose2 prior(0.0, 0.0, 0.0); // prior mean is at origin Pose2 prior(0.0, 0.0, 0.0); // prior mean is at origin
auto priorNoise = noiseModel::Diagonal::Sigmas( auto priorNoise = noiseModel::Diagonal::Sigmas(
Vector3(0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta Vector3(0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta
fg.emplace_nonlinear<PriorFactor<Pose2>>(X(0), prior, priorNoise); fg.emplace_shared<PriorFactor<Pose2>>(X(0), prior, priorNoise);
using PlanarMotionModel = BetweenFactor<Pose2>; using PlanarMotionModel = BetweenFactor<Pose2>;
@ -708,9 +708,9 @@ TEST(HybridFactorGraph, DefaultDecisionTree) {
double range11 = std::sqrt(4.0 + 4.0), range22 = 2.0; double range11 = std::sqrt(4.0 + 4.0), range22 = 2.0;
// Add Bearing-Range factors // Add Bearing-Range factors
fg.emplace_nonlinear<BearingRangeFactor<Pose2, Point2>>( fg.emplace_shared<BearingRangeFactor<Pose2, Point2>>(
X(0), L(0), bearing11, range11, measurementNoise); X(0), L(0), bearing11, range11, measurementNoise);
fg.emplace_nonlinear<BearingRangeFactor<Pose2, Point2>>( fg.emplace_shared<BearingRangeFactor<Pose2, Point2>>(
X(1), L(1), bearing22, range22, measurementNoise); X(1), L(1), bearing22, range22, measurementNoise);
// Create (deliberately inaccurate) initial estimate // Create (deliberately inaccurate) initial estimate

32
gtsam/hybrid/tests/testHybridNonlinearISAM.cpp
View File

@ -409,7 +409,7 @@ TEST(HybridNonlinearISAM, NonTrivial) {
Pose2 prior(0.0, 0.0, 0.0); // prior mean is at origin Pose2 prior(0.0, 0.0, 0.0); // prior mean is at origin
auto priorNoise = noiseModel::Diagonal::Sigmas( auto priorNoise = noiseModel::Diagonal::Sigmas(
Vector3(0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta Vector3(0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta
fg.emplace_nonlinear<PriorFactor<Pose2>>(X(0), prior, priorNoise); fg.emplace_shared<PriorFactor<Pose2>>(X(0), prior, priorNoise);
// create a noise model for the landmark measurements // create a noise model for the landmark measurements
auto poseNoise = noiseModel::Isotropic::Sigma(3, 0.1); auto poseNoise = noiseModel::Isotropic::Sigma(3, 0.1);
@ -418,11 +418,11 @@ TEST(HybridNonlinearISAM, NonTrivial) {
// where X is the base link and W is the foot link. // where X is the base link and W is the foot link.
// Add connecting poses similar to PoseFactors in GTD // Add connecting poses similar to PoseFactors in GTD
fg.emplace_nonlinear<BetweenFactor<Pose2>>(X(0), Y(0), Pose2(0, 1.0, 0), fg.emplace_shared<BetweenFactor<Pose2>>(X(0), Y(0), Pose2(0, 1.0, 0),
poseNoise); poseNoise);
fg.emplace_nonlinear<BetweenFactor<Pose2>>(Y(0), Z(0), Pose2(0, 1.0, 0), fg.emplace_shared<BetweenFactor<Pose2>>(Y(0), Z(0), Pose2(0, 1.0, 0),
poseNoise); poseNoise);
fg.emplace_nonlinear<BetweenFactor<Pose2>>(Z(0), W(0), Pose2(0, 1.0, 0), fg.emplace_shared<BetweenFactor<Pose2>>(Z(0), W(0), Pose2(0, 1.0, 0),
poseNoise); poseNoise);
// Create initial estimate // Create initial estimate
@ -451,14 +451,14 @@ TEST(HybridNonlinearISAM, NonTrivial) {
fg.push_back(mixtureFactor); fg.push_back(mixtureFactor);
// Add equivalent of ImuFactor // Add equivalent of ImuFactor
fg.emplace_nonlinear<BetweenFactor<Pose2>>(X(0), X(1), Pose2(1.0, 0.0, 0), fg.emplace_shared<BetweenFactor<Pose2>>(X(0), X(1), Pose2(1.0, 0.0, 0),
poseNoise); poseNoise);
// PoseFactors-like at k=1 // PoseFactors-like at k=1
fg.emplace_nonlinear<BetweenFactor<Pose2>>(X(1), Y(1), Pose2(0, 1, 0), fg.emplace_shared<BetweenFactor<Pose2>>(X(1), Y(1), Pose2(0, 1, 0),
poseNoise); poseNoise);
fg.emplace_nonlinear<BetweenFactor<Pose2>>(Y(1), Z(1), Pose2(0, 1, 0), fg.emplace_shared<BetweenFactor<Pose2>>(Y(1), Z(1), Pose2(0, 1, 0),
poseNoise); poseNoise);
fg.emplace_nonlinear<BetweenFactor<Pose2>>(Z(1), W(1), Pose2(-1, 1, 0), fg.emplace_shared<BetweenFactor<Pose2>>(Z(1), W(1), Pose2(-1, 1, 0),
poseNoise); poseNoise);
initial.insert(X(1), Pose2(1.0, 0.0, 0.0)); initial.insert(X(1), Pose2(1.0, 0.0, 0.0));
@ -491,14 +491,14 @@ TEST(HybridNonlinearISAM, NonTrivial) {
fg.push_back(mixtureFactor); fg.push_back(mixtureFactor);
// Add equivalent of ImuFactor // Add equivalent of ImuFactor
fg.emplace_nonlinear<BetweenFactor<Pose2>>(X(1), X(2), Pose2(1.0, 0.0, 0), fg.emplace_shared<BetweenFactor<Pose2>>(X(1), X(2), Pose2(1.0, 0.0, 0),
poseNoise); poseNoise);
// PoseFactors-like at k=1 // PoseFactors-like at k=1
fg.emplace_nonlinear<BetweenFactor<Pose2>>(X(2), Y(2), Pose2(0, 1, 0), fg.emplace_shared<BetweenFactor<Pose2>>(X(2), Y(2), Pose2(0, 1, 0),
poseNoise); poseNoise);
fg.emplace_nonlinear<BetweenFactor<Pose2>>(Y(2), Z(2), Pose2(0, 1, 0), fg.emplace_shared<BetweenFactor<Pose2>>(Y(2), Z(2), Pose2(0, 1, 0),
poseNoise); poseNoise);
fg.emplace_nonlinear<BetweenFactor<Pose2>>(Z(2), W(2), Pose2(-2, 1, 0), fg.emplace_shared<BetweenFactor<Pose2>>(Z(2), W(2), Pose2(-2, 1, 0),
poseNoise); poseNoise);
initial.insert(X(2), Pose2(2.0, 0.0, 0.0)); initial.insert(X(2), Pose2(2.0, 0.0, 0.0));
@ -534,14 +534,14 @@ TEST(HybridNonlinearISAM, NonTrivial) {
fg.push_back(mixtureFactor); fg.push_back(mixtureFactor);
// Add equivalent of ImuFactor // Add equivalent of ImuFactor
fg.emplace_nonlinear<BetweenFactor<Pose2>>(X(2), X(3), Pose2(1.0, 0.0, 0), fg.emplace_shared<BetweenFactor<Pose2>>(X(2), X(3), Pose2(1.0, 0.0, 0),
poseNoise); poseNoise);
// PoseFactors-like at k=3 // PoseFactors-like at k=3
fg.emplace_nonlinear<BetweenFactor<Pose2>>(X(3), Y(3), Pose2(0, 1, 0), fg.emplace_shared<BetweenFactor<Pose2>>(X(3), Y(3), Pose2(0, 1, 0),
poseNoise); poseNoise);
fg.emplace_nonlinear<BetweenFactor<Pose2>>(Y(3), Z(3), Pose2(0, 1, 0), fg.emplace_shared<BetweenFactor<Pose2>>(Y(3), Z(3), Pose2(0, 1, 0),
poseNoise); poseNoise);
fg.emplace_nonlinear<BetweenFactor<Pose2>>(Z(3), W(3), Pose2(-3, 1, 0), fg.emplace_shared<BetweenFactor<Pose2>>(Z(3), W(3), Pose2(-3, 1, 0),
poseNoise); poseNoise);
initial.insert(X(3), Pose2(3.0, 0.0, 0.0)); initial.insert(X(3), Pose2(3.0, 0.0, 0.0));