From 187935407c27974c0e61e8a6b22eec43d2b622b4 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 13 Sep 2024 05:40:00 -0400 Subject: [PATCH] rename MixtureFactor to HybridNonlinearFactor --- gtsam/hybrid/GaussianMixture.cpp | 8 ++--- gtsam/hybrid/GaussianMixtureFactor.h | 2 +- gtsam/hybrid/HybridFactor.h | 2 +- gtsam/hybrid/HybridNonlinearFactorGraph.cpp | 6 ++-- gtsam/hybrid/MixtureFactor.h | 28 ++++++++--------- gtsam/hybrid/hybrid.i | 10 +++---- gtsam/hybrid/tests/Switching.h | 4 +-- gtsam/hybrid/tests/testHybridBayesNet.cpp | 2 +- gtsam/hybrid/tests/testHybridEstimation.cpp | 10 +++---- gtsam/hybrid/tests/testHybridGaussianISAM.cpp | 6 ++-- .../tests/testHybridNonlinearFactorGraph.cpp | 18 +++++------ .../hybrid/tests/testHybridNonlinearISAM.cpp | 6 ++-- gtsam/hybrid/tests/testMixtureFactor.cpp | 30 +++++++++---------- .../tests/test_HybridNonlinearFactorGraph.py | 4 +-- 14 files changed, 68 insertions(+), 68 deletions(-) diff --git a/gtsam/hybrid/GaussianMixture.cpp b/gtsam/hybrid/GaussianMixture.cpp index feb49b6a2..f1f1ce429 100644 --- a/gtsam/hybrid/GaussianMixture.cpp +++ b/gtsam/hybrid/GaussianMixture.cpp @@ -72,7 +72,7 @@ GaussianMixture::GaussianMixture( /* *******************************************************************************/ // TODO(dellaert): This is copy/paste: GaussianMixture should be derived from -// HybridGaussianFactor, no? +// GaussianMixtureFactor, no? GaussianFactorGraphTree GaussianMixture::add( const GaussianFactorGraphTree &sum) const { using Y = GaussianFactorGraph; @@ -203,7 +203,7 @@ bool GaussianMixture::allFrontalsGiven(const VectorValues &given) const { } /* ************************************************************************* */ -std::shared_ptr GaussianMixture::likelihood( +std::shared_ptr GaussianMixture::likelihood( const VectorValues &given) const { if (!allFrontalsGiven(given)) { throw std::runtime_error( @@ -212,7 +212,7 @@ std::shared_ptr GaussianMixture::likelihood( const DiscreteKeys discreteParentKeys = discreteKeys(); const KeyVector continuousParentKeys = continuousParents(); - const HybridGaussianFactor::Factors likelihoods( + const GaussianMixtureFactor::Factors likelihoods( conditionals_, [&](const GaussianConditional::shared_ptr &conditional) { const auto likelihood_m = conditional->likelihood(given); const double Cgm_Kgcm = @@ -231,7 +231,7 @@ std::shared_ptr GaussianMixture::likelihood( return std::make_shared(gfg); } }); - return std::make_shared( + return std::make_shared( continuousParentKeys, discreteParentKeys, likelihoods); } diff --git a/gtsam/hybrid/GaussianMixtureFactor.h b/gtsam/hybrid/GaussianMixtureFactor.h index 467412b1d..9915e24e6 100644 --- a/gtsam/hybrid/GaussianMixtureFactor.h +++ b/gtsam/hybrid/GaussianMixtureFactor.h @@ -146,7 +146,7 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor { /// Getter for GaussianFactor decision tree const Factors &factors() const { return factors_; } - /// Add MixtureFactor to a Sum, syntactic sugar. + /// Add HybridNonlinearFactor to a Sum, syntactic sugar. friend GaussianFactorGraphTree &operator+=( GaussianFactorGraphTree &sum, const HybridGaussianFactor &factor) { sum = factor.add(sum); diff --git a/gtsam/hybrid/HybridFactor.h b/gtsam/hybrid/HybridFactor.h index 4a68f851b..2a572ea65 100644 --- a/gtsam/hybrid/HybridFactor.h +++ b/gtsam/hybrid/HybridFactor.h @@ -45,7 +45,7 @@ DiscreteKeys CollectDiscreteKeys(const DiscreteKeys &key1, * Base class for *truly* hybrid probabilistic factors * * Examples: - * - MixtureFactor + * - HybridNonlinearFactor * - HybridGaussianFactor * - GaussianMixture * diff --git a/gtsam/hybrid/HybridNonlinearFactorGraph.cpp b/gtsam/hybrid/HybridNonlinearFactorGraph.cpp index 923058d05..35a8ea355 100644 --- a/gtsam/hybrid/HybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/HybridNonlinearFactorGraph.cpp @@ -21,7 +21,7 @@ #include #include #include -#include +#include #include namespace gtsam { @@ -59,7 +59,7 @@ void HybridNonlinearFactorGraph::printErrors( // Clear the stringstream ss.str(std::string()); - if (auto mf = std::dynamic_pointer_cast(factor)) { + if (auto mf = std::dynamic_pointer_cast(factor)) { if (factor == nullptr) { std::cout << "nullptr" << "\n"; @@ -151,7 +151,7 @@ HybridGaussianFactorGraph::shared_ptr HybridNonlinearFactorGraph::linearize( continue; } // Check if it is a nonlinear mixture factor - if (auto mf = dynamic_pointer_cast(f)) { + if (auto mf = dynamic_pointer_cast(f)) { const HybridGaussianFactor::shared_ptr& gmf = mf->linearize(continuousValues); linearFG->push_back(gmf); diff --git a/gtsam/hybrid/MixtureFactor.h b/gtsam/hybrid/MixtureFactor.h index d2e0e7822..d3d673dda 100644 --- a/gtsam/hybrid/MixtureFactor.h +++ b/gtsam/hybrid/MixtureFactor.h @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file MixtureFactor.h + * @file HybridNonlinearFactor.h * @brief Nonlinear Mixture factor of continuous and discrete. * @author Kevin Doherty, kdoherty@mit.edu * @author Varun Agrawal @@ -44,11 +44,11 @@ namespace gtsam { * one of (NonlinearFactor, GaussianFactor) which can then be checked to perform * the correct operation. */ -class MixtureFactor : public HybridFactor { +class HybridNonlinearFactor : public HybridFactor { public: using Base = HybridFactor; - using This = MixtureFactor; - using shared_ptr = std::shared_ptr; + using This = HybridNonlinearFactor; + using shared_ptr = std::shared_ptr; using sharedFactor = std::shared_ptr; /** @@ -63,7 +63,7 @@ class MixtureFactor : public HybridFactor { bool normalized_; public: - MixtureFactor() = default; + HybridNonlinearFactor() = default; /** * @brief Construct from Decision tree. @@ -74,7 +74,7 @@ class MixtureFactor : public HybridFactor { * @param normalized Flag indicating if the factor error is already * normalized. */ - MixtureFactor(const KeyVector& keys, const DiscreteKeys& discreteKeys, + HybridNonlinearFactor(const KeyVector& keys, const DiscreteKeys& discreteKeys, const Factors& factors, bool normalized = false) : Base(keys, discreteKeys), factors_(factors), normalized_(normalized) {} @@ -95,7 +95,7 @@ class MixtureFactor : public HybridFactor { * normalized. */ template - MixtureFactor(const KeyVector& keys, const DiscreteKeys& discreteKeys, + HybridNonlinearFactor(const KeyVector& keys, const DiscreteKeys& discreteKeys, const std::vector>& factors, bool normalized = false) : Base(keys, discreteKeys), normalized_(normalized) { @@ -111,7 +111,7 @@ class MixtureFactor : public HybridFactor { nonlinear_factors.push_back(nf); } else { throw std::runtime_error( - "Factors passed into MixtureFactor need to be nonlinear!"); + "Factors passed into HybridNonlinearFactor need to be nonlinear!"); } } factors_ = Factors(discreteKeys, nonlinear_factors); @@ -124,7 +124,7 @@ class MixtureFactor : public HybridFactor { } /** - * @brief Compute error of the MixtureFactor as a tree. + * @brief Compute error of the HybridNonlinearFactor as a tree. * * @param continuousValues The continuous values for which to compute the * error. @@ -201,15 +201,15 @@ class MixtureFactor : public HybridFactor { /// Check equality bool equals(const HybridFactor& other, double tol = 1e-9) const override { - // We attempt a dynamic cast from HybridFactor to MixtureFactor. If it + // We attempt a dynamic cast from HybridFactor to HybridNonlinearFactor. If it // fails, return false. - if (!dynamic_cast(&other)) return false; + if (!dynamic_cast(&other)) return false; - // If the cast is successful, we'll properly construct a MixtureFactor + // If the cast is successful, we'll properly construct a HybridNonlinearFactor // object from `other` - const MixtureFactor& f(static_cast(other)); + const HybridNonlinearFactor& f(static_cast(other)); - // Ensure that this MixtureFactor and `f` have the same `factors_`. + // Ensure that this HybridNonlinearFactor and `f` have the same `factors_`. auto compare = [tol](const sharedFactor& a, const sharedFactor& b) { return traits::Equals(*a, *b, tol); }; diff --git a/gtsam/hybrid/hybrid.i b/gtsam/hybrid/hybrid.i index 0c72d5046..e5be00c6f 100644 --- a/gtsam/hybrid/hybrid.i +++ b/gtsam/hybrid/hybrid.i @@ -235,15 +235,15 @@ class HybridNonlinearFactorGraph { gtsam::DefaultKeyFormatter) const; }; -#include -class MixtureFactor : gtsam::HybridFactor { - MixtureFactor( +#include +class HybridNonlinearFactor : gtsam::HybridFactor { + HybridNonlinearFactor( const gtsam::KeyVector& keys, const gtsam::DiscreteKeys& discreteKeys, const gtsam::DecisionTree& factors, bool normalized = false); template - MixtureFactor(const gtsam::KeyVector& keys, const gtsam::DiscreteKeys& discreteKeys, + HybridNonlinearFactor(const gtsam::KeyVector& keys, const gtsam::DiscreteKeys& discreteKeys, const std::vector& factors, bool normalized = false); @@ -256,7 +256,7 @@ class MixtureFactor : gtsam::HybridFactor { HybridGaussianFactor* linearize( const gtsam::Values& continuousValues) const; - void print(string s = "MixtureFactor\n", + void print(string s = "HybridNonlinearFactor\n", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; }; diff --git a/gtsam/hybrid/tests/Switching.h b/gtsam/hybrid/tests/Switching.h index 126b15c6e..0e8680aa9 100644 --- a/gtsam/hybrid/tests/Switching.h +++ b/gtsam/hybrid/tests/Switching.h @@ -22,7 +22,7 @@ #include #include #include -#include +#include #include #include #include @@ -163,7 +163,7 @@ struct Switching { for (auto &&f : motion_models) { components.push_back(std::dynamic_pointer_cast(f)); } - nonlinearFactorGraph.emplace_shared( + nonlinearFactorGraph.emplace_shared( keys, DiscreteKeys{modes[k]}, components); } diff --git a/gtsam/hybrid/tests/testHybridBayesNet.cpp b/gtsam/hybrid/tests/testHybridBayesNet.cpp index 3a7e008d8..9b7ba9744 100644 --- a/gtsam/hybrid/tests/testHybridBayesNet.cpp +++ b/gtsam/hybrid/tests/testHybridBayesNet.cpp @@ -389,7 +389,7 @@ TEST(HybridBayesNet, Sampling) { std::make_shared>(X(0), X(1), 1, noise_model); std::vector factors = {zero_motion, one_motion}; nfg.emplace_shared>(X(0), 0.0, noise_model); - nfg.emplace_shared( + nfg.emplace_shared( KeyVector{X(0), X(1)}, DiscreteKeys{DiscreteKey(M(0), 2)}, factors); DiscreteKey mode(M(0), 2); diff --git a/gtsam/hybrid/tests/testHybridEstimation.cpp b/gtsam/hybrid/tests/testHybridEstimation.cpp index 2ec9c514a..295633916 100644 --- a/gtsam/hybrid/tests/testHybridEstimation.cpp +++ b/gtsam/hybrid/tests/testHybridEstimation.cpp @@ -22,7 +22,7 @@ #include #include #include -#include +#include #include #include #include @@ -435,7 +435,7 @@ static HybridNonlinearFactorGraph createHybridNonlinearFactorGraph() { std::make_shared>(X(0), X(1), 0, noise_model); const auto one_motion = std::make_shared>(X(0), X(1), 1, noise_model); - nfg.emplace_shared( + nfg.emplace_shared( KeyVector{X(0), X(1)}, DiscreteKeys{m}, std::vector{zero_motion, one_motion}); @@ -532,7 +532,7 @@ TEST(HybridEstimation, CorrectnessViaSampling) { * the difference in noise models. */ std::shared_ptr mixedVarianceFactor( - const MixtureFactor& mf, const Values& initial, const Key& mode, + const HybridNonlinearFactor& mf, const Values& initial, const Key& mode, double noise_tight, double noise_loose, size_t d, size_t tight_index) { HybridGaussianFactor::shared_ptr gmf = mf.linearize(initial); @@ -592,7 +592,7 @@ TEST(HybridEstimation, ModeSelection) { std::vector components = {model0, model1}; KeyVector keys = {X(0), X(1)}; - MixtureFactor mf(keys, modes, components); + HybridNonlinearFactor mf(keys, modes, components); initial.insert(X(0), 0.0); initial.insert(X(1), 0.0); @@ -683,7 +683,7 @@ TEST(HybridEstimation, ModeSelection2) { std::vector components = {model0, model1}; KeyVector keys = {X(0), X(1)}; - MixtureFactor mf(keys, modes, components); + HybridNonlinearFactor mf(keys, modes, components); initial.insert(X(0), Z_3x1); initial.insert(X(1), Z_3x1); diff --git a/gtsam/hybrid/tests/testHybridGaussianISAM.cpp b/gtsam/hybrid/tests/testHybridGaussianISAM.cpp index 249cbf9c3..d2b0fded3 100644 --- a/gtsam/hybrid/tests/testHybridGaussianISAM.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianISAM.cpp @@ -418,7 +418,7 @@ TEST(HybridGaussianISAM, NonTrivial) { moving = std::make_shared(W(0), W(1), odometry, noise_model); std::vector components = {moving, still}; - auto mixtureFactor = std::make_shared( + auto mixtureFactor = std::make_shared( contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components); fg.push_back(mixtureFactor); @@ -458,7 +458,7 @@ TEST(HybridGaussianISAM, NonTrivial) { moving = std::make_shared(W(1), W(2), odometry, noise_model); components = {moving, still}; - mixtureFactor = std::make_shared( + mixtureFactor = std::make_shared( contKeys, DiscreteKeys{gtsam::DiscreteKey(M(2), 2)}, components); fg.push_back(mixtureFactor); @@ -501,7 +501,7 @@ TEST(HybridGaussianISAM, NonTrivial) { moving = std::make_shared(W(2), W(3), odometry, noise_model); components = {moving, still}; - mixtureFactor = std::make_shared( + mixtureFactor = std::make_shared( contKeys, DiscreteKeys{gtsam::DiscreteKey(M(3), 2)}, components); fg.push_back(mixtureFactor); diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp index 67b820c33..9e2816865 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp @@ -24,7 +24,7 @@ #include #include #include -#include +#include #include #include #include @@ -105,7 +105,7 @@ TEST(HybridNonlinearFactorGraph, Resize) { auto discreteFactor = std::make_shared(); fg.push_back(discreteFactor); - auto dcFactor = std::make_shared(); + auto dcFactor = std::make_shared(); fg.push_back(dcFactor); EXPECT_LONGS_EQUAL(fg.size(), 3); @@ -132,7 +132,7 @@ TEST(HybridGaussianFactorGraph, Resize) { moving = std::make_shared(X(0), X(1), 1.0, noise_model); std::vector components = {still, moving}; - auto dcFactor = std::make_shared( + auto dcFactor = std::make_shared( contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components); nhfg.push_back(dcFactor); @@ -150,10 +150,10 @@ TEST(HybridGaussianFactorGraph, Resize) { } /*************************************************************************** - * Test that the MixtureFactor reports correctly if the number of continuous + * Test that the HybridNonlinearFactor reports correctly if the number of continuous * keys provided do not match the keys in the factors. */ -TEST(HybridGaussianFactorGraph, MixtureFactor) { +TEST(HybridGaussianFactorGraph, HybridNonlinearFactor) { auto nonlinearFactor = std::make_shared>( X(0), X(1), 0.0, Isotropic::Sigma(1, 0.1)); auto discreteFactor = std::make_shared(); @@ -166,12 +166,12 @@ TEST(HybridGaussianFactorGraph, MixtureFactor) { // Check for exception when number of continuous keys are under-specified. KeyVector contKeys = {X(0)}; - THROWS_EXCEPTION(std::make_shared( + THROWS_EXCEPTION(std::make_shared( contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components)); // Check for exception when number of continuous keys are too many. contKeys = {X(0), X(1), X(2)}; - THROWS_EXCEPTION(std::make_shared( + THROWS_EXCEPTION(std::make_shared( contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components)); } @@ -195,7 +195,7 @@ TEST(HybridFactorGraph, PushBack) { fg = HybridNonlinearFactorGraph(); - auto dcFactor = std::make_shared(); + auto dcFactor = std::make_shared(); fg.push_back(dcFactor); EXPECT_LONGS_EQUAL(fg.size(), 1); @@ -800,7 +800,7 @@ TEST(HybridFactorGraph, DefaultDecisionTree) { moving = std::make_shared(X(0), X(1), odometry, noise_model); std::vector motion_models = {still, moving}; - fg.emplace_shared( + fg.emplace_shared( contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, motion_models); // Add Range-Bearing measurements to from X0 to L0 and X1 to L1. diff --git a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp index 2fb6fd8ba..e2ed4cbad 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp @@ -437,7 +437,7 @@ TEST(HybridNonlinearISAM, NonTrivial) { moving = std::make_shared(W(0), W(1), odometry, noise_model); std::vector components = {moving, still}; - auto mixtureFactor = std::make_shared( + auto mixtureFactor = std::make_shared( contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components); fg.push_back(mixtureFactor); @@ -477,7 +477,7 @@ TEST(HybridNonlinearISAM, NonTrivial) { moving = std::make_shared(W(1), W(2), odometry, noise_model); components = {moving, still}; - mixtureFactor = std::make_shared( + mixtureFactor = std::make_shared( contKeys, DiscreteKeys{gtsam::DiscreteKey(M(2), 2)}, components); fg.push_back(mixtureFactor); @@ -520,7 +520,7 @@ TEST(HybridNonlinearISAM, NonTrivial) { moving = std::make_shared(W(2), W(3), odometry, noise_model); components = {moving, still}; - mixtureFactor = std::make_shared( + mixtureFactor = std::make_shared( contKeys, DiscreteKeys{gtsam::DiscreteKey(M(3), 2)}, components); fg.push_back(mixtureFactor); diff --git a/gtsam/hybrid/tests/testMixtureFactor.cpp b/gtsam/hybrid/tests/testMixtureFactor.cpp index a58a4767f..dead8cd34 100644 --- a/gtsam/hybrid/tests/testMixtureFactor.cpp +++ b/gtsam/hybrid/tests/testMixtureFactor.cpp @@ -11,7 +11,7 @@ /** * @file testMixtureFactor.cpp - * @brief Unit tests for MixtureFactor + * @brief Unit tests for HybridNonlinearFactor * @author Varun Agrawal * @date October 2022 */ @@ -21,7 +21,7 @@ #include #include #include -#include +#include #include #include @@ -36,17 +36,17 @@ using symbol_shorthand::X; /* ************************************************************************* */ // Check iterators of empty mixture. -TEST(MixtureFactor, Constructor) { - MixtureFactor factor; - MixtureFactor::const_iterator const_it = factor.begin(); +TEST(HybridNonlinearFactor, Constructor) { + HybridNonlinearFactor factor; + HybridNonlinearFactor::const_iterator const_it = factor.begin(); CHECK(const_it == factor.end()); - MixtureFactor::iterator it = factor.begin(); + HybridNonlinearFactor::iterator it = factor.begin(); CHECK(it == factor.end()); } /* ************************************************************************* */ // Test .print() output. -TEST(MixtureFactor, Printing) { +TEST(HybridNonlinearFactor, Printing) { DiscreteKey m1(1, 2); double between0 = 0.0; double between1 = 1.0; @@ -60,11 +60,11 @@ TEST(MixtureFactor, Printing) { std::make_shared>(X(1), X(2), between1, model); std::vector factors{f0, f1}; - MixtureFactor mixtureFactor({X(1), X(2)}, {m1}, factors); + HybridNonlinearFactor mixtureFactor({X(1), X(2)}, {m1}, factors); std::string expected = R"(Hybrid [x1 x2; 1] -MixtureFactor +HybridNonlinearFactor Choice(1) 0 Leaf Nonlinear factor on 2 keys 1 Leaf Nonlinear factor on 2 keys @@ -73,7 +73,7 @@ MixtureFactor } /* ************************************************************************* */ -static MixtureFactor getMixtureFactor() { +static HybridNonlinearFactor getMixtureFactor() { DiscreteKey m1(1, 2); double between0 = 0.0; @@ -88,12 +88,12 @@ static MixtureFactor getMixtureFactor() { std::make_shared>(X(1), X(2), between1, model); std::vector factors{f0, f1}; - return MixtureFactor({X(1), X(2)}, {m1}, factors); + return HybridNonlinearFactor({X(1), X(2)}, {m1}, factors); } /* ************************************************************************* */ -// Test the error of the MixtureFactor -TEST(MixtureFactor, Error) { +// Test the error of the HybridNonlinearFactor +TEST(HybridNonlinearFactor, Error) { auto mixtureFactor = getMixtureFactor(); Values continuousValues; @@ -112,8 +112,8 @@ TEST(MixtureFactor, Error) { } /* ************************************************************************* */ -// Test dim of the MixtureFactor -TEST(MixtureFactor, Dim) { +// Test dim of the HybridNonlinearFactor +TEST(HybridNonlinearFactor, Dim) { auto mixtureFactor = getMixtureFactor(); EXPECT_LONGS_EQUAL(1, mixtureFactor.dim()); } diff --git a/python/gtsam/tests/test_HybridNonlinearFactorGraph.py b/python/gtsam/tests/test_HybridNonlinearFactorGraph.py index ed26a0c81..3c2a70f73 100644 --- a/python/gtsam/tests/test_HybridNonlinearFactorGraph.py +++ b/python/gtsam/tests/test_HybridNonlinearFactorGraph.py @@ -17,9 +17,9 @@ import unittest import numpy as np from gtsam.symbol_shorthand import C, X from gtsam.utils.test_case import GtsamTestCase -from gtsam import BetweenFactorPoint3, noiseModel, PriorFactorPoint3, Point3 import gtsam +from gtsam import BetweenFactorPoint3, Point3, PriorFactorPoint3, noiseModel class TestHybridGaussianFactorGraph(GtsamTestCase): @@ -34,7 +34,7 @@ class TestHybridGaussianFactorGraph(GtsamTestCase): PriorFactorPoint3(2, Point3(1, 2, 3), noiseModel.Diagonal.Variances([0.5, 0.5, 0.5]))) nlfg.push_back( - gtsam.MixtureFactor([1], dk, [ + gtsam.HybridNonlinearFactor([1], dk, [ PriorFactorPoint3(1, Point3(0, 0, 0), noiseModel.Unit.Create(3)), PriorFactorPoint3(1, Point3(1, 2, 1),