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

NonlinearFactorValuePair typedef

release/4.3a0
Varun Agrawal 2024-09-15 09:16:51 -04:00
parent 3eb91a4ba0
commit c2dc1fcdb2
6 changed files with 18 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
gtsam/hybrid/HybridNonlinearFactor.h
View File

@ -33,6 +33,9 @@
namespace gtsam { namespace gtsam {
/// Alias for a NonlinearFactor shared pointer and double scalar pair.
using NonlinearFactorValuePair = std::pair<NonlinearFactor::shared_ptr, double>;
/** /**
* @brief Implementation of a discrete conditional mixture factor. * @brief Implementation of a discrete conditional mixture factor.
* *
@ -55,7 +58,7 @@ class HybridNonlinearFactor : public HybridFactor {
* @brief typedef for DecisionTree which has Keys as node labels and * @brief typedef for DecisionTree which has Keys as node labels and
* pairs of NonlinearFactor & an arbitrary scalar as leaf nodes. * pairs of NonlinearFactor & an arbitrary scalar as leaf nodes.
*/ */
using Factors = DecisionTree<Key, std::pair<sharedFactor, double>>; using Factors = DecisionTree<Key, NonlinearFactorValuePair>;
private: private:
/// Decision tree of Gaussian factors indexed by discrete keys. /// Decision tree of Gaussian factors indexed by discrete keys.
@ -100,8 +103,7 @@ class HybridNonlinearFactor : public HybridFactor {
const std::vector<std::pair<std::shared_ptr<FACTOR>, double>>& factors, const std::vector<std::pair<std::shared_ptr<FACTOR>, double>>& factors,
bool normalized = false) bool normalized = false)
: Base(keys, discreteKeys), normalized_(normalized) { : Base(keys, discreteKeys), normalized_(normalized) {
std::vector<std::pair<NonlinearFactor::shared_ptr, double>> std::vector<NonlinearFactorValuePair> nonlinear_factors;
nonlinear_factors;
KeySet continuous_keys_set(keys.begin(), keys.end()); KeySet continuous_keys_set(keys.begin(), keys.end());
KeySet factor_keys_set; KeySet factor_keys_set;
for (auto&& [f, val] : factors) { for (auto&& [f, val] : factors) {

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

@ -162,7 +162,7 @@ struct Switching {
for (size_t k = 0; k < K - 1; k++) { for (size_t k = 0; k < K - 1; k++) {
KeyVector keys = {X(k), X(k + 1)}; KeyVector keys = {X(k), X(k + 1)};
auto motion_models = motionModels(k, between_sigma); auto motion_models = motionModels(k, between_sigma);
std::vector<std::pair<NonlinearFactor::shared_ptr, double>> components; std::vector<NonlinearFactorValuePair> components;
for (auto &&f : motion_models) { for (auto &&f : motion_models) {
components.push_back( components.push_back(
{std::dynamic_pointer_cast<NonlinearFactor>(f), 0.0}); {std::dynamic_pointer_cast<NonlinearFactor>(f), 0.0});

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

@ -387,8 +387,8 @@ TEST(HybridBayesNet, Sampling) {
std::make_shared<BetweenFactor<double>>(X(0), X(1), 0, noise_model); std::make_shared<BetweenFactor<double>>(X(0), X(1), 0, noise_model);
auto one_motion = auto one_motion =
std::make_shared<BetweenFactor<double>>(X(0), X(1), 1, noise_model); std::make_shared<BetweenFactor<double>>(X(0), X(1), 1, noise_model);
std::vector<std::pair<NonlinearFactor::shared_ptr, double>> factors = { std::vector<NonlinearFactorValuePair> factors = {{zero_motion, 0.0},
{zero_motion, 0.0}, {one_motion, 0.0}}; {one_motion, 0.0}};
nfg.emplace_shared<PriorFactor<double>>(X(0), 0.0, noise_model); nfg.emplace_shared<PriorFactor<double>>(X(0), 0.0, noise_model);
nfg.emplace_shared<HybridNonlinearFactor>( nfg.emplace_shared<HybridNonlinearFactor>(
KeyVector{X(0), X(1)}, DiscreteKeys{DiscreteKey(M(0), 2)}, factors); KeyVector{X(0), X(1)}, DiscreteKeys{DiscreteKey(M(0), 2)}, factors);

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

@ -435,8 +435,8 @@ static HybridNonlinearFactorGraph createHybridNonlinearFactorGraph() {
std::make_shared<BetweenFactor<double>>(X(0), X(1), 0, noise_model); std::make_shared<BetweenFactor<double>>(X(0), X(1), 0, noise_model);
const auto one_motion = const auto one_motion =
std::make_shared<BetweenFactor<double>>(X(0), X(1), 1, noise_model); std::make_shared<BetweenFactor<double>>(X(0), X(1), 1, noise_model);
std::vector<std::pair<NonlinearFactor::shared_ptr, double>> components = { std::vector<NonlinearFactorValuePair> components = {{zero_motion, 0.0},
{zero_motion, 0.0}, {one_motion, 0.0}}; {one_motion, 0.0}};
nfg.emplace_shared<HybridNonlinearFactor>(KeyVector{X(0), X(1)}, nfg.emplace_shared<HybridNonlinearFactor>(KeyVector{X(0), X(1)},
DiscreteKeys{m}, components); DiscreteKeys{m}, components);
@ -595,8 +595,8 @@ TEST(HybridEstimation, ModeSelection) {
model1 = std::make_shared<MotionModel>( model1 = std::make_shared<MotionModel>(
X(0), X(1), 0.0, noiseModel::Isotropic::Sigma(d, noise_tight)); X(0), X(1), 0.0, noiseModel::Isotropic::Sigma(d, noise_tight));
std::vector<std::pair<NonlinearFactor::shared_ptr, double>> components = { std::vector<NonlinearFactorValuePair> components = {{model0, 0.0},
{model0, 0.0}, {model1, 0.0}}; {model1, 0.0}};
KeyVector keys = {X(0), X(1)}; KeyVector keys = {X(0), X(1)};
HybridNonlinearFactor mf(keys, modes, components); HybridNonlinearFactor mf(keys, modes, components);
@ -687,8 +687,8 @@ TEST(HybridEstimation, ModeSelection2) {
model1 = std::make_shared<BetweenFactor<Vector3>>( model1 = std::make_shared<BetweenFactor<Vector3>>(
X(0), X(1), Z_3x1, noiseModel::Isotropic::Sigma(d, noise_tight)); X(0), X(1), Z_3x1, noiseModel::Isotropic::Sigma(d, noise_tight));
std::vector<std::pair<NonlinearFactor::shared_ptr, double>> components = { std::vector<NonlinearFactorValuePair> components = {{model0, 0.0},
{model0, 0.0}, {model1, 0.0}}; {model1, 0.0}};
KeyVector keys = {X(0), X(1)}; KeyVector keys = {X(0), X(1)};
HybridNonlinearFactor mf(keys, modes, components); HybridNonlinearFactor mf(keys, modes, components);

6
gtsam/hybrid/tests/testHybridNonlinearFactor.cpp
View File

@ -58,8 +58,7 @@ TEST(HybridNonlinearFactor, Printing) {
std::make_shared<BetweenFactor<double>>(X(1), X(2), between0, model); std::make_shared<BetweenFactor<double>>(X(1), X(2), between0, model);
auto f1 = auto f1 =
std::make_shared<BetweenFactor<double>>(X(1), X(2), between1, model); std::make_shared<BetweenFactor<double>>(X(1), X(2), between1, model);
std::vector<std::pair<NonlinearFactor::shared_ptr, double>> factors{ std::vector<NonlinearFactorValuePair> factors{{f0, 0.0}, {f1, 0.0}};
{f0, 0.0}, {f1, 0.0}};
HybridNonlinearFactor mixtureFactor({X(1), X(2)}, {m1}, factors); HybridNonlinearFactor mixtureFactor({X(1), X(2)}, {m1}, factors);
@ -87,8 +86,7 @@ static HybridNonlinearFactor getHybridNonlinearFactor() {
std::make_shared<BetweenFactor<double>>(X(1), X(2), between0, model); std::make_shared<BetweenFactor<double>>(X(1), X(2), between0, model);
auto f1 = auto f1 =
std::make_shared<BetweenFactor<double>>(X(1), X(2), between1, model); std::make_shared<BetweenFactor<double>>(X(1), X(2), between1, model);
std::vector<std::pair<NonlinearFactor::shared_ptr, double>> factors{ std::vector<NonlinearFactorValuePair> factors{{f0, 0.0}, {f1, 0.0}};
{f0, 0.0}, {f1, 0.0}};
return HybridNonlinearFactor({X(1), X(2)}, {m1}, factors); return HybridNonlinearFactor({X(1), X(2)}, {m1}, factors);
} }

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

@ -27,6 +27,7 @@
#include <gtsam/hybrid/HybridNonlinearFactorGraph.h> #include <gtsam/hybrid/HybridNonlinearFactorGraph.h>
#include <gtsam/linear/GaussianBayesNet.h> #include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianFactorGraph.h> #include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/PriorFactor.h> #include <gtsam/nonlinear/PriorFactor.h>
#include <gtsam/sam/BearingRangeFactor.h> #include <gtsam/sam/BearingRangeFactor.h>
@ -867,7 +868,7 @@ static HybridNonlinearFactorGraph CreateFactorGraph(
std::make_shared<BetweenFactor<double>>(X(0), X(1), means[1], model1); std::make_shared<BetweenFactor<double>>(X(0), X(1), means[1], model1);
// Create HybridNonlinearFactor // Create HybridNonlinearFactor
std::vector<std::pair<NonlinearFactor::shared_ptr, double>> factors{ std::vector<NonlinearFactorValuePair> factors{
{f0, ComputeLogNormalizer(model0)}, {f1, ComputeLogNormalizer(model1)}}; {f0, ComputeLogNormalizer(model0)}, {f1, ComputeLogNormalizer(model1)}};
HybridNonlinearFactor mixtureFactor({X(0), X(1)}, {m1}, factors); HybridNonlinearFactor mixtureFactor({X(0), X(1)}, {m1}, factors);