diff --git a/.github/workflows/build-linux.yml b/.github/workflows/build-linux.yml index 4502dbe0e..da398ad23 100644 --- a/.github/workflows/build-linux.yml +++ b/.github/workflows/build-linux.yml @@ -56,7 +56,7 @@ jobs: steps: - name: Checkout - uses: actions/checkout@v3 + uses: actions/checkout@v4 - name: Install Dependencies run: | diff --git a/.github/workflows/build-macos.yml b/.github/workflows/build-macos.yml index 72e27e3b6..e4c78bf67 100644 --- a/.github/workflows/build-macos.yml +++ b/.github/workflows/build-macos.yml @@ -26,6 +26,7 @@ jobs: # See https://help.github.com/en/articles/workflow-syntax-for-github-actions. name: [ macos-12-xcode-14.2, + macos-14-xcode-15.4, ] build_type: [Debug, Release] @@ -36,9 +37,14 @@ jobs: compiler: xcode version: "14.2" + - name: macos-14-xcode-15.4 + os: macos-14 + compiler: xcode + version: "15.4" + steps: - name: Checkout - uses: actions/checkout@v3 + uses: actions/checkout@v4 - name: Install Dependencies run: | diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index 037704a36..cb465309e 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -32,6 +32,7 @@ jobs: ubuntu-20.04-gcc-9-tbb, ubuntu-20.04-clang-9, macos-12-xcode-14.2, + macos-14-xcode-15.4, windows-2022-msbuild, ] @@ -59,13 +60,18 @@ jobs: compiler: xcode version: "14.2" + - name: macos-14-xcode-15.4 + os: macos-14 + compiler: xcode + version: "15.4" + - name: windows-2022-msbuild os: windows-2022 platform: 64 steps: - name: Checkout - uses: actions/checkout@v3 + uses: actions/checkout@v4 - name: Install (Linux) if: runner.os == 'Linux' @@ -162,6 +168,14 @@ jobs: run: | bash .github/scripts/python.sh -d + - name: Create virtual on MacOS + if: runner.os == 'macOS' + run: | + python$PYTHON_VERSION -m venv venv + source venv/bin/activate + echo "PATH=$(pwd)/venv/bin:$PATH" >> $GITHUB_ENV + python -m pip install --upgrade pip + - name: Install Python Dependencies shell: bash run: python$PYTHON_VERSION -m pip install -r python/dev_requirements.txt diff --git a/.github/workflows/build-special.yml b/.github/workflows/build-special.yml index 164646e3e..3a7dd974d 100644 --- a/.github/workflows/build-special.yml +++ b/.github/workflows/build-special.yml @@ -83,7 +83,7 @@ jobs: steps: - name: Checkout - uses: actions/checkout@v3 + uses: actions/checkout@v4 - name: Install (Linux) if: runner.os == 'Linux' diff --git a/.github/workflows/build-windows.yml b/.github/workflows/build-windows.yml index dcf742c05..1bda7e40a 100644 --- a/.github/workflows/build-windows.yml +++ b/.github/workflows/build-windows.yml @@ -44,7 +44,7 @@ jobs: steps: - name: Checkout - uses: actions/checkout@v3 + uses: actions/checkout@v4 - name: Setup msbuild uses: ilammy/msvc-dev-cmd@v1 diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/TriangularSolver.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/TriangularSolver.h index f9c56ba79..6b5fdb3e6 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/TriangularSolver.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/TriangularSolver.h @@ -270,11 +270,9 @@ struct sparse_solve_triangular_sparse_selector } - Index count = 0; // FIXME compute a reference value to filter zeros for (typename AmbiVector::Iterator it(tempVector/*,1e-12*/); it; ++it) { - ++ count; // std::cerr << "fill " << it.index() << ", " << col << "\n"; // std::cout << it.value() << " "; // FIXME use insertBack diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_heap_relax_snode.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_heap_relax_snode.h index 6f75d500e..7aecbcad8 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_heap_relax_snode.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_heap_relax_snode.h @@ -75,8 +75,6 @@ void SparseLUImpl::heap_relax_snode (const Index n, IndexVe // Identify the relaxed supernodes by postorder traversal of the etree Index snode_start; // beginning of a snode StorageIndex k; - Index nsuper_et_post = 0; // Number of relaxed snodes in postordered etree - Index nsuper_et = 0; // Number of relaxed snodes in the original etree StorageIndex l; for (j = 0; j < n; ) { @@ -88,7 +86,6 @@ void SparseLUImpl::heap_relax_snode (const Index n, IndexVe parent = et(j); } // Found a supernode in postordered etree, j is the last column - ++nsuper_et_post; k = StorageIndex(n); for (Index i = snode_start; i <= j; ++i) k = (std::min)(k, inv_post(i)); @@ -97,7 +94,6 @@ void SparseLUImpl::heap_relax_snode (const Index n, IndexVe { // This is also a supernode in the original etree relax_end(k) = l; // Record last column - ++nsuper_et; } else { @@ -107,7 +103,6 @@ void SparseLUImpl::heap_relax_snode (const Index n, IndexVe if (descendants(i) == 0) { relax_end(l) = l; - ++nsuper_et; } } } diff --git a/gtsam/3rdparty/metis/GKlib/pdb.c b/gtsam/3rdparty/metis/GKlib/pdb.c index b4d222653..018846604 100644 --- a/gtsam/3rdparty/metis/GKlib/pdb.c +++ b/gtsam/3rdparty/metis/GKlib/pdb.c @@ -131,7 +131,7 @@ that structure. /************************************************************************/ pdbf *gk_readpdbfile(char *fname) { /* {{{ */ int i=0, res=0; - char linetype[6]; + char linetype[7]; int aserial; char aname[5] = " \0"; char altLoc = ' '; diff --git a/gtsam/discrete/tests/testDiscreteConditional.cpp b/gtsam/discrete/tests/testDiscreteConditional.cpp index 172dd0fa1..2482a86a2 100644 --- a/gtsam/discrete/tests/testDiscreteConditional.cpp +++ b/gtsam/discrete/tests/testDiscreteConditional.cpp @@ -292,7 +292,7 @@ TEST(DiscreteConditional, choose) { /* ************************************************************************* */ // Check argmax on P(C|D) and P(D), plus tie-breaking for P(B) TEST(DiscreteConditional, Argmax) { - DiscreteKey B(2, 2), C(2, 2), D(4, 2); + DiscreteKey C(2, 2), D(4, 2); DiscreteConditional B_prior(D, "1/1"); DiscreteConditional D_prior(D, "1/3"); DiscreteConditional C_given_D((C | D) = "1/4 1/1"); diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index 63b914434..6bb97ff6c 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -68,7 +68,7 @@ namespace gtsam { return fromAngle(theta * degree); } - /// Named constructor from cos(theta),sin(theta) pair, will *not* normalize! + /// Named constructor from cos(theta),sin(theta) pair static Rot2 fromCosSin(double c, double s); /** diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 93cf99972..c9851f761 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -1207,6 +1207,31 @@ TEST(Pose3, Print) { EXPECT(assert_print_equal(expected, pose)); } +/* ************************************************************************* */ +TEST(Pose3, ExpmapChainRule) { + // Muliply with an arbitrary matrix and exponentiate + Matrix6 M; + M << 1, 2, 3, 4, 5, 6, // + 7, 8, 9, 1, 2, 3, // + 4, 5, 6, 7, 8, 9, // + 1, 2, 3, 4, 5, 6, // + 7, 8, 9, 1, 2, 3, // + 4, 5, 6, 7, 8, 9; + auto g = [&](const Vector6& omega) { + return Pose3::Expmap(M*omega); + }; + + // Test the derivatives at zero + const Matrix6 expected = numericalDerivative11(g, Z_6x1); + EXPECT(assert_equal(expected, M, 1e-5)); // Pose3::ExpmapDerivative(Z_6x1) is identity + + // Test the derivatives at another value + const Vector6 delta{0.1, 0.2, 0.3, 0.4, 0.5, 0.6}; + const Matrix6 expected2 = numericalDerivative11(g, delta); + const Matrix6 analytic = Pose3::ExpmapDerivative(M*delta) * M; + EXPECT(assert_equal(expected2, analytic, 1e-5)); // note tolerance +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 1232348f0..ce056edb6 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -956,6 +956,46 @@ TEST(Rot3, determinant) { } } +/* ************************************************************************* */ +TEST(Rot3, ExpmapChainRule) { + // Multiply with an arbitrary matrix and exponentiate + Matrix3 M; + M << 1, 2, 3, 4, 5, 6, 7, 8, 9; + auto g = [&](const Vector3& omega) { + return Rot3::Expmap(M*omega); + }; + + // Test the derivatives at zero + const Matrix3 expected = numericalDerivative11(g, Z_3x1); + EXPECT(assert_equal(expected, M, 1e-5)); // SO3::ExpmapDerivative(Z_3x1) is identity + + // Test the derivatives at another value + const Vector3 delta{0.1,0.2,0.3}; + const Matrix3 expected2 = numericalDerivative11(g, delta); + EXPECT(assert_equal(expected2, SO3::ExpmapDerivative(M*delta) * M, 1e-5)); +} + +/* ************************************************************************* */ +TEST(Rot3, expmapChainRule) { + // Multiply an arbitrary rotation with exp(M*x) + // Perhaps counter-intuitively, this has the same derivatives as above + Matrix3 M; + M << 1, 2, 3, 4, 5, 6, 7, 8, 9; + const Rot3 R = Rot3::Expmap({1, 2, 3}); + auto g = [&](const Vector3& omega) { + return R.expmap(M*omega); + }; + + // Test the derivatives at zero + const Matrix3 expected = numericalDerivative11(g, Z_3x1); + EXPECT(assert_equal(expected, M, 1e-5)); + + // Test the derivatives at another value + const Vector3 delta{0.1,0.2,0.3}; + const Matrix3 expected2 = numericalDerivative11(g, delta); + EXPECT(assert_equal(expected2, SO3::ExpmapDerivative(M*delta) * M, 1e-5)); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/hybrid/GaussianMixture.cpp b/gtsam/hybrid/GaussianMixture.cpp index 0a0332af8..fa4248921 100644 --- a/gtsam/hybrid/GaussianMixture.cpp +++ b/gtsam/hybrid/GaussianMixture.cpp @@ -87,7 +87,22 @@ GaussianFactorGraphTree GaussianMixture::add( /* *******************************************************************************/ GaussianFactorGraphTree GaussianMixture::asGaussianFactorGraphTree() const { - auto wrap = [](const GaussianConditional::shared_ptr &gc) { + auto wrap = [this](const GaussianConditional::shared_ptr &gc) { + // First check if conditional has not been pruned + if (gc) { + const double Cgm_Kgcm = + this->logConstant_ - gc->logNormalizationConstant(); + // If there is a difference in the covariances, we need to account for + // that since the error is dependent on the mode. + if (Cgm_Kgcm > 0.0) { + // We add a constant factor which will be used when computing + // the probability of the discrete variables. + Vector c(1); + c << std::sqrt(2.0 * Cgm_Kgcm); + auto constantFactor = std::make_shared(c); + return GaussianFactorGraph{gc, constantFactor}; + } + } return GaussianFactorGraph{gc}; }; return {conditionals_, wrap}; diff --git a/gtsam/hybrid/HybridConditional.h b/gtsam/hybrid/HybridConditional.h index 64bdcb2c1..fb6542822 100644 --- a/gtsam/hybrid/HybridConditional.h +++ b/gtsam/hybrid/HybridConditional.h @@ -61,7 +61,7 @@ class GTSAM_EXPORT HybridConditional public Conditional { public: // typedefs needed to play nice with gtsam - typedef HybridConditional This; ///< Typedef to this class + typedef HybridConditional This; ///< Typedef to this class typedef std::shared_ptr shared_ptr; ///< shared_ptr to this class typedef HybridFactor BaseFactor; ///< Typedef to our factor base class typedef Conditional @@ -185,7 +185,7 @@ class GTSAM_EXPORT HybridConditional * Return the log normalization constant. * Note this is 0.0 for discrete and hybrid conditionals, but depends * on the continuous parameters for Gaussian conditionals. - */ + */ double logNormalizationConstant() const override; /// Return the probability (or density) of the underlying conditional. diff --git a/gtsam/hybrid/HybridFactorGraph.h b/gtsam/hybrid/HybridFactorGraph.h index 8b59fd4f9..1830d44ab 100644 --- a/gtsam/hybrid/HybridFactorGraph.h +++ b/gtsam/hybrid/HybridFactorGraph.h @@ -66,7 +66,7 @@ class GTSAM_EXPORT HybridFactorGraph : public FactorGraph { /// Get all the discrete keys in the factor graph. std::set discreteKeys() const; - /// Get all the discrete keys in the factor graph, as a set. + /// Get all the discrete keys in the factor graph, as a set of Keys. KeySet discreteKeySet() const; /// Get a map from Key to corresponding DiscreteKey. diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index cb8ceed20..c44fc4f1d 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -97,29 +97,27 @@ void HybridGaussianFactorGraph::printErrors( std::cout << "nullptr" << "\n"; } else { - factor->print(ss.str(), keyFormatter); - std::cout << "error = "; - gmf->errorTree(values.continuous()).print("", keyFormatter); - std::cout << std::endl; + gmf->operator()(values.discrete())->print(ss.str(), keyFormatter); + std::cout << "error = " << gmf->error(values) << std::endl; } } else if (auto hc = std::dynamic_pointer_cast(factor)) { if (factor == nullptr) { std::cout << "nullptr" << "\n"; } else { - factor->print(ss.str(), keyFormatter); - if (hc->isContinuous()) { + factor->print(ss.str(), keyFormatter); std::cout << "error = " << hc->asGaussian()->error(values) << "\n"; } else if (hc->isDiscrete()) { - std::cout << "error = "; - hc->asDiscrete()->errorTree().print("", keyFormatter); - std::cout << "\n"; + factor->print(ss.str(), keyFormatter); + std::cout << "error = " << hc->asDiscrete()->error(values.discrete()) + << "\n"; } else { // Is hybrid - std::cout << "error = "; - hc->asMixture()->errorTree(values.continuous()).print(); - std::cout << "\n"; + auto mixtureComponent = + hc->asMixture()->operator()(values.discrete()); + mixtureComponent->print(ss.str(), keyFormatter); + std::cout << "error = " << mixtureComponent->error(values) << "\n"; } } } else if (auto gf = std::dynamic_pointer_cast(factor)) { @@ -140,8 +138,7 @@ void HybridGaussianFactorGraph::printErrors( << "\n"; } else { factor->print(ss.str(), keyFormatter); - std::cout << "error = "; - df->errorTree().print("", keyFormatter); + std::cout << "error = " << df->error(values.discrete()) << std::endl; } } else { @@ -233,6 +230,25 @@ continuousElimination(const HybridGaussianFactorGraph &factors, return {std::make_shared(result.first), result.second}; } +/* ************************************************************************ */ +/** + * @brief Exponentiate log-values, not necessarily normalized, normalize, and + * return as AlgebraicDecisionTree. + * + * @param logValues DecisionTree of (unnormalized) log values. + * @return AlgebraicDecisionTree + */ +static AlgebraicDecisionTree probabilitiesFromLogValues( + const AlgebraicDecisionTree &logValues) { + // Perform normalization + double max_log = logValues.max(); + AlgebraicDecisionTree probabilities = DecisionTree( + logValues, [&max_log](const double x) { return exp(x - max_log); }); + probabilities = probabilities.normalize(probabilities.sum()); + + return probabilities; +} + /* ************************************************************************ */ static std::pair> discreteElimination(const HybridGaussianFactorGraph &factors, @@ -245,14 +261,18 @@ discreteElimination(const HybridGaussianFactorGraph &factors, } else if (auto gmf = dynamic_pointer_cast(f)) { // Case where we have a GaussianMixtureFactor with no continuous keys. // In this case, compute discrete probabilities. - auto probability = + auto logProbability = [&](const GaussianFactor::shared_ptr &factor) -> double { if (!factor) return 0.0; - return exp(-factor->error(VectorValues())); + return -factor->error(VectorValues()); }; - dfg.emplace_shared( - gmf->discreteKeys(), - DecisionTree(gmf->factors(), probability)); + AlgebraicDecisionTree logProbabilities = + DecisionTree(gmf->factors(), logProbability); + + AlgebraicDecisionTree probabilities = + probabilitiesFromLogValues(logProbabilities); + dfg.emplace_shared(gmf->discreteKeys(), + probabilities); } else if (auto orphan = dynamic_pointer_cast(f)) { // Ignore orphaned clique. @@ -310,18 +330,13 @@ static std::shared_ptr createDiscreteFactor( // exp(-factor->error(kEmpty)) / conditional->normalizationConstant(); // We take negative of the logNormalizationConstant `log(1/k)` // to get `log(k)`. - return -factor->error(kEmpty) + (-conditional->logNormalizationConstant()); + return -factor->error(kEmpty) - conditional->logNormalizationConstant(); }; AlgebraicDecisionTree logProbabilities( DecisionTree(eliminationResults, logProbability)); - - // Perform normalization - double max_log = logProbabilities.max(); - AlgebraicDecisionTree probabilities = DecisionTree( - logProbabilities, - [&max_log](const double x) { return exp(x - max_log); }); - probabilities = probabilities.normalize(probabilities.sum()); + AlgebraicDecisionTree probabilities = + probabilitiesFromLogValues(logProbabilities); return std::make_shared(discreteSeparator, probabilities); } @@ -508,14 +523,15 @@ EliminateHybrid(const HybridGaussianFactorGraph &factors, std::inserter(continuousSeparator, continuousSeparator.begin())); // Similarly for the discrete separator. - KeySet discreteSeparatorSet; - std::set discreteSeparator; auto discreteKeySet = factors.discreteKeySet(); + // Use set-difference to get the difference in keys + KeySet discreteSeparatorSet; std::set_difference( discreteKeySet.begin(), discreteKeySet.end(), frontalKeysSet.begin(), frontalKeysSet.end(), std::inserter(discreteSeparatorSet, discreteSeparatorSet.begin())); // Convert from set of keys to set of DiscreteKeys + std::set discreteSeparator; auto discreteKeyMap = factors.discreteKeyMap(); for (auto key : discreteSeparatorSet) { discreteSeparator.insert(discreteKeyMap.at(key)); @@ -532,10 +548,15 @@ AlgebraicDecisionTree HybridGaussianFactorGraph::errorTree( AlgebraicDecisionTree error_tree(0.0); // Iterate over each factor. - for (auto &f : factors_) { + for (auto &factor : factors_) { // TODO(dellaert): just use a virtual method defined in HybridFactor. AlgebraicDecisionTree factor_error; + auto f = factor; + if (auto hc = dynamic_pointer_cast(factor)) { + f = hc->inner(); + } + if (auto gaussianMixture = dynamic_pointer_cast(f)) { // Compute factor error and add it. error_tree = error_tree + gaussianMixture->errorTree(continuousValues); diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.h b/gtsam/hybrid/HybridGaussianFactorGraph.h index 1708ff64b..2ca6e4c95 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.h +++ b/gtsam/hybrid/HybridGaussianFactorGraph.h @@ -144,6 +144,14 @@ class GTSAM_EXPORT HybridGaussianFactorGraph // const std::string& s = "HybridGaussianFactorGraph", // const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override; + /** + * @brief Print the errors of each factor in the hybrid factor graph. + * + * @param values The HybridValues for the variables used to compute the error. + * @param str String that is output before the factor graph and errors. + * @param keyFormatter Formatter function for the keys in the factors. + * @param printCondition A condition to check if a factor should be printed. + */ void printErrors( const HybridValues& values, const std::string& str = "HybridGaussianFactorGraph: ", diff --git a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp index a854f20d8..6efa21f84 100644 --- a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp +++ b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp @@ -36,6 +36,7 @@ using namespace std; using namespace gtsam; using noiseModel::Isotropic; +using symbol_shorthand::F; using symbol_shorthand::M; using symbol_shorthand::X; using symbol_shorthand::Z; @@ -200,67 +201,29 @@ TEST(GaussianMixtureFactor, Error) { 4.0, mixtureFactor.error({continuousValues, discreteValues}), 1e-9); } -/* ************************************************************************* */ -/** - * Test a simple Gaussian Mixture Model represented as P(m)P(z|m) - * where m is a discrete variable and z is a continuous variable. - * m is binary and depending on m, we have 2 different means - * μ1 and μ2 for the Gaussian distribution around which we sample z. - * - * The resulting factor graph should eliminate to a Bayes net - * which represents a sigmoid function. - */ -TEST(GaussianMixtureFactor, GaussianMixtureModel) { - double mu0 = 1.0, mu1 = 3.0; - double sigma = 2.0; - auto model = noiseModel::Isotropic::Sigma(1, sigma); +namespace test_gmm { +/** + * Function to compute P(m=1|z). For P(m=0|z), swap mus and sigmas. + * If sigma0 == sigma1, it simplifies to a sigmoid function. + * + * Follows equation 7.108 since it is more generic. + */ +double prob_m_z(double mu0, double mu1, double sigma0, double sigma1, + double z) { + double x1 = ((z - mu0) / sigma0), x2 = ((z - mu1) / sigma1); + double d = sigma1 / sigma0; + double e = d * std::exp(-0.5 * (x1 * x1 - x2 * x2)); + return 1 / (1 + e); +}; + +static HybridBayesNet GetGaussianMixtureModel(double mu0, double mu1, + double sigma0, double sigma1) { DiscreteKey m(M(0), 2); Key z = Z(0); - auto c0 = make_shared(z, Vector1(mu0), I_1x1, model), - c1 = make_shared(z, Vector1(mu1), I_1x1, model); - - GaussianMixture gm({z}, {}, {m}, {c0, c1}); - DiscreteConditional mixing(m, "0.5/0.5"); - - HybridBayesNet hbn; - hbn.push_back(gm); - hbn.push_back(mixing); - - // The result should be a sigmoid. - // So should be m = 0.5 at z=3.0 - 1.0=2.0 - VectorValues given; - given.insert(z, Vector1(mu1 - mu0)); - - HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given); - HybridBayesNet::shared_ptr bn = gfg.eliminateSequential(); - - HybridBayesNet expected; - expected.push_back(DiscreteConditional(m, "0.5/0.5")); - - EXPECT(assert_equal(expected, *bn)); -} - -/* ************************************************************************* */ -/** - * Test a simple Gaussian Mixture Model represented as P(m)P(z|m) - * where m is a discrete variable and z is a continuous variable. - * m is binary and depending on m, we have 2 different means - * and covariances each for the - * Gaussian distribution around which we sample z. - * - * The resulting factor graph should eliminate to a Bayes net - * which represents a sigmoid function leaning towards - * the tighter covariance Gaussian. - */ -TEST(GaussianMixtureFactor, GaussianMixtureModel2) { - double mu0 = 1.0, mu1 = 3.0; - auto model0 = noiseModel::Isotropic::Sigma(1, 8.0); - auto model1 = noiseModel::Isotropic::Sigma(1, 4.0); - - DiscreteKey m(M(0), 2); - Key z = Z(0); + auto model0 = noiseModel::Isotropic::Sigma(1, sigma0); + auto model1 = noiseModel::Isotropic::Sigma(1, sigma1); auto c0 = make_shared(z, Vector1(mu0), I_1x1, model0), c1 = make_shared(z, Vector1(mu1), I_1x1, model1); @@ -272,125 +235,283 @@ TEST(GaussianMixtureFactor, GaussianMixtureModel2) { hbn.push_back(gm); hbn.push_back(mixing); - // The result should be a sigmoid leaning towards model1 - // since it has the tighter covariance. - // So should be m = 0.34/0.66 at z=3.0 - 1.0=2.0 - VectorValues given; - given.insert(z, Vector1(mu1 - mu0)); - HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given); - HybridBayesNet::shared_ptr bn = gfg.eliminateSequential(); + return hbn; +} - HybridBayesNet expected; - expected.push_back(DiscreteConditional(m, "0.338561851224/0.661438148776")); +} // namespace test_gmm - EXPECT(assert_equal(expected, *bn)); +/* ************************************************************************* */ +/** + * Test a simple Gaussian Mixture Model represented as P(m)P(z|m) + * where m is a discrete variable and z is a continuous variable. + * m is binary and depending on m, we have 2 different means + * μ1 and μ2 for the Gaussian distribution around which we sample z. + * + * The resulting factor graph should eliminate to a Bayes net + * which represents a sigmoid function. + */ +TEST(GaussianMixtureFactor, GaussianMixtureModel) { + using namespace test_gmm; + + double mu0 = 1.0, mu1 = 3.0; + double sigma = 2.0; + + DiscreteKey m(M(0), 2); + Key z = Z(0); + + auto hbn = GetGaussianMixtureModel(mu0, mu1, sigma, sigma); + + // The result should be a sigmoid. + // So should be P(m=1|z) = 0.5 at z=3.0 - 1.0=2.0 + double midway = mu1 - mu0, lambda = 4; + { + VectorValues given; + given.insert(z, Vector1(midway)); + + HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given); + HybridBayesNet::shared_ptr bn = gfg.eliminateSequential(); + + EXPECT_DOUBLES_EQUAL( + prob_m_z(mu0, mu1, sigma, sigma, midway), + bn->at(0)->asDiscrete()->operator()(DiscreteValues{{m.first, 1}}), + 1e-8); + + // At the halfway point between the means, we should get P(m|z)=0.5 + HybridBayesNet expected; + expected.emplace_back(new DiscreteConditional(m, "0.5/0.5")); + + EXPECT(assert_equal(expected, *bn)); + } + { + // Shift by -lambda + VectorValues given; + given.insert(z, Vector1(midway - lambda)); + + HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given); + HybridBayesNet::shared_ptr bn = gfg.eliminateSequential(); + + EXPECT_DOUBLES_EQUAL( + prob_m_z(mu0, mu1, sigma, sigma, midway - lambda), + bn->at(0)->asDiscrete()->operator()(DiscreteValues{{m.first, 1}}), + 1e-8); + } + { + // Shift by lambda + VectorValues given; + given.insert(z, Vector1(midway + lambda)); + + HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given); + HybridBayesNet::shared_ptr bn = gfg.eliminateSequential(); + + EXPECT_DOUBLES_EQUAL( + prob_m_z(mu0, mu1, sigma, sigma, midway + lambda), + bn->at(0)->asDiscrete()->operator()(DiscreteValues{{m.first, 1}}), + 1e-8); + } } /* ************************************************************************* */ /** - * Test a model P(x0)P(z0|x0)p(x1|m1)p(z1|x1)p(m1). + * Test a simple Gaussian Mixture Model represented as P(m)P(z|m) + * where m is a discrete variable and z is a continuous variable. + * m is binary and depending on m, we have 2 different means + * and covariances each for the + * Gaussian distribution around which we sample z. * - * p(x1|m1) has different means and same covariance. + * The resulting factor graph should eliminate to a Bayes net + * which represents a Gaussian-like function + * where m1>m0 close to 3.1333. + */ +TEST(GaussianMixtureFactor, GaussianMixtureModel2) { + using namespace test_gmm; + + double mu0 = 1.0, mu1 = 3.0; + double sigma0 = 8.0, sigma1 = 4.0; + + DiscreteKey m(M(0), 2); + Key z = Z(0); + + auto hbn = GetGaussianMixtureModel(mu0, mu1, sigma0, sigma1); + + double m1_high = 3.133, lambda = 4; + { + // The result should be a bell curve like function + // with m1 > m0 close to 3.1333. + // We get 3.1333 by finding the maximum value of the function. + VectorValues given; + given.insert(z, Vector1(3.133)); + + HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given); + HybridBayesNet::shared_ptr bn = gfg.eliminateSequential(); + + EXPECT_DOUBLES_EQUAL( + prob_m_z(mu0, mu1, sigma0, sigma1, m1_high), + bn->at(0)->asDiscrete()->operator()(DiscreteValues{{M(0), 1}}), 1e-8); + + // At the halfway point between the means + HybridBayesNet expected; + expected.emplace_back(new DiscreteConditional( + m, {}, + vector{prob_m_z(mu1, mu0, sigma1, sigma0, m1_high), + prob_m_z(mu0, mu1, sigma0, sigma1, m1_high)})); + + EXPECT(assert_equal(expected, *bn)); + } + { + // Shift by -lambda + VectorValues given; + given.insert(z, Vector1(m1_high - lambda)); + + HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given); + HybridBayesNet::shared_ptr bn = gfg.eliminateSequential(); + + EXPECT_DOUBLES_EQUAL( + prob_m_z(mu0, mu1, sigma0, sigma1, m1_high - lambda), + bn->at(0)->asDiscrete()->operator()(DiscreteValues{{m.first, 1}}), + 1e-8); + } + { + // Shift by lambda + VectorValues given; + given.insert(z, Vector1(m1_high + lambda)); + + HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given); + HybridBayesNet::shared_ptr bn = gfg.eliminateSequential(); + + EXPECT_DOUBLES_EQUAL( + prob_m_z(mu0, mu1, sigma0, sigma1, m1_high + lambda), + bn->at(0)->asDiscrete()->operator()(DiscreteValues{{m.first, 1}}), + 1e-8); + } +} + +namespace test_two_state_estimation { + +/// Create Two State Bayes Network with measurements +static HybridBayesNet CreateBayesNet(double mu0, double mu1, double sigma0, + double sigma1, + bool add_second_measurement = false, + double prior_sigma = 1e-3, + double measurement_sigma = 3.0) { + DiscreteKey m1(M(1), 2); + Key z0 = Z(0), z1 = Z(1); + Key x0 = X(0), x1 = X(1); + + HybridBayesNet hbn; + + auto measurement_model = noiseModel::Isotropic::Sigma(1, measurement_sigma); + // Add measurement P(z0 | x0) + auto p_z0 = new GaussianConditional(z0, Vector1(0.0), -I_1x1, x0, I_1x1, + measurement_model); + hbn.emplace_back(p_z0); + + // Add hybrid motion model + auto model0 = noiseModel::Isotropic::Sigma(1, sigma0); + auto model1 = noiseModel::Isotropic::Sigma(1, sigma1); + auto c0 = make_shared(x1, Vector1(mu0), I_1x1, x0, + -I_1x1, model0), + c1 = make_shared(x1, Vector1(mu1), I_1x1, x0, + -I_1x1, model1); + + auto motion = new GaussianMixture({x1}, {x0}, {m1}, {c0, c1}); + hbn.emplace_back(motion); + + if (add_second_measurement) { + // Add second measurement + auto p_z1 = new GaussianConditional(z1, Vector1(0.0), -I_1x1, x1, I_1x1, + measurement_model); + hbn.emplace_back(p_z1); + } + + // Discrete uniform prior. + auto p_m1 = new DiscreteConditional(m1, "0.5/0.5"); + hbn.emplace_back(p_m1); + + return hbn; +} + +} // namespace test_two_state_estimation + +/* ************************************************************************* */ +/** + * Test a model P(x0)P(z0|x0)P(x1|x0,m1)P(z1|x1)P(m1). + * + * P(f01|x1,x0,m1) has different means and same covariance. * * Converting to a factor graph gives us - * P(x0)ϕ(x0)P(x1|m1)ϕ(x1)P(m1) + * P(x0)ϕ(x0)ϕ(x1,x0,m1)ϕ(x1)P(m1) * * If we only have a measurement on z0, then - * the probability of x1 should be 0.5/0.5. + * the probability of m1 should be 0.5/0.5. * Getting a measurement on z1 gives use more information. */ TEST(GaussianMixtureFactor, TwoStateModel) { + using namespace test_two_state_estimation; + double mu0 = 1.0, mu1 = 3.0; - auto model = noiseModel::Isotropic::Sigma(1, 2.0); + double sigma = 2.0; DiscreteKey m1(M(1), 2); - Key z0 = Z(0), z1 = Z(1), x0 = X(0), x1 = X(1); + Key z0 = Z(0), z1 = Z(1); - auto c0 = make_shared(x1, Vector1(mu0), I_1x1, model), - c1 = make_shared(x1, Vector1(mu1), I_1x1, model); - - auto p_x0 = new GaussianConditional(x0, Vector1(0.0), I_1x1, - noiseModel::Isotropic::Sigma(1, 1.0)); - auto p_z0x0 = new GaussianConditional(z0, Vector1(0.0), I_1x1, x0, -I_1x1, - noiseModel::Isotropic::Sigma(1, 1.0)); - auto p_x1m1 = new GaussianMixture({x1}, {}, {m1}, {c0, c1}); - auto p_z1x1 = new GaussianConditional(z1, Vector1(0.0), I_1x1, x1, -I_1x1, - noiseModel::Isotropic::Sigma(1, 3.0)); - auto p_m1 = new DiscreteConditional(m1, "0.5/0.5"); - - HybridBayesNet hbn; - hbn.emplace_back(p_x0); - hbn.emplace_back(p_z0x0); - hbn.emplace_back(p_x1m1); - hbn.emplace_back(p_m1); + // Start with no measurement on x1, only on x0 + HybridBayesNet hbn = CreateBayesNet(mu0, mu1, sigma, sigma, false); VectorValues given; given.insert(z0, Vector1(0.5)); { - // Start with no measurement on x1, only on x0 HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given); HybridBayesNet::shared_ptr bn = gfg.eliminateSequential(); // Since no measurement on x1, we hedge our bets DiscreteConditional expected(m1, "0.5/0.5"); - EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()))); } { // Now we add a measurement z1 on x1 - hbn.emplace_back(p_z1x1); + hbn = CreateBayesNet(mu0, mu1, sigma, sigma, true); - given.insert(z1, Vector1(2.2)); + // If we see z1=2.6 (> 2.5 which is the halfway point), + // discrete mode should say m1=1 + given.insert(z1, Vector1(2.6)); HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given); HybridBayesNet::shared_ptr bn = gfg.eliminateSequential(); // Since we have a measurement on z2, we get a definite result - DiscreteConditional expected(m1, "0.4923083/0.5076917"); - + DiscreteConditional expected(m1, "0.49772729/0.50227271"); + // regression EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), 1e-6)); } } /* ************************************************************************* */ /** - * Test a model P(x0)P(z0|x0)p(x1|m1)p(z1|x1)p(m1). + * Test a model P(x0)P(z0|x0)P(x1|x0,m1)P(z1|x1)P(m1). * - * p(x1|m1) has different means and different covariances. + * P(f01|x1,x0,m1) has different means and different covariances. * * Converting to a factor graph gives us - * P(x0)ϕ(x0)P(x1|m1)ϕ(x1)P(m1) + * P(x0)ϕ(x0)ϕ(x1,x0,m1)ϕ(x1)P(m1) * * If we only have a measurement on z0, then - * the probability of x1 should be the ratio of covariances. + * the P(m1) should be 0.5/0.5. * Getting a measurement on z1 gives use more information. */ TEST(GaussianMixtureFactor, TwoStateModel2) { + using namespace test_two_state_estimation; + double mu0 = 1.0, mu1 = 3.0; - auto model0 = noiseModel::Isotropic::Sigma(1, 6.0); - auto model1 = noiseModel::Isotropic::Sigma(1, 4.0); + double sigma0 = 6.0, sigma1 = 4.0; + auto model0 = noiseModel::Isotropic::Sigma(1, sigma0); + auto model1 = noiseModel::Isotropic::Sigma(1, sigma1); DiscreteKey m1(M(1), 2); - Key z0 = Z(0), z1 = Z(1), x0 = X(0), x1 = X(1); + Key z0 = Z(0), z1 = Z(1); - auto c0 = make_shared(x1, Vector1(mu0), I_1x1, model0), - c1 = make_shared(x1, Vector1(mu1), I_1x1, model1); - - auto p_x0 = new GaussianConditional(x0, Vector1(0.0), I_1x1, - noiseModel::Isotropic::Sigma(1, 1.0)); - auto p_z0x0 = new GaussianConditional(z0, Vector1(0.0), I_1x1, x0, -I_1x1, - noiseModel::Isotropic::Sigma(1, 1.0)); - auto p_x1m1 = new GaussianMixture({x1}, {}, {m1}, {c0, c1}); - auto p_z1x1 = new GaussianConditional(z1, Vector1(0.0), I_1x1, x1, -I_1x1, - noiseModel::Isotropic::Sigma(1, 3.0)); - auto p_m1 = new DiscreteConditional(m1, "0.5/0.5"); - - HybridBayesNet hbn; - hbn.emplace_back(p_x0); - hbn.emplace_back(p_z0x0); - hbn.emplace_back(p_x1m1); - hbn.emplace_back(p_m1); + // Start with no measurement on x1, only on x0 + HybridBayesNet hbn = CreateBayesNet(mu0, mu1, sigma0, sigma1, false); VectorValues given; given.insert(z0, Vector1(0.5)); @@ -398,225 +519,74 @@ TEST(GaussianMixtureFactor, TwoStateModel2) { { // Start with no measurement on x1, only on x0 HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given); + + { + VectorValues vv{ + {X(0), Vector1(0.0)}, {X(1), Vector1(1.0)}, {Z(0), Vector1(0.5)}}; + HybridValues hv0(vv, DiscreteValues{{M(1), 0}}), + hv1(vv, DiscreteValues{{M(1), 1}}); + EXPECT_DOUBLES_EQUAL(gfg.error(hv0) / hbn.error(hv0), + gfg.error(hv1) / hbn.error(hv1), 1e-9); + } + { + VectorValues vv{ + {X(0), Vector1(0.5)}, {X(1), Vector1(3.0)}, {Z(0), Vector1(0.5)}}; + HybridValues hv0(vv, DiscreteValues{{M(1), 0}}), + hv1(vv, DiscreteValues{{M(1), 1}}); + EXPECT_DOUBLES_EQUAL(gfg.error(hv0) / hbn.error(hv0), + gfg.error(hv1) / hbn.error(hv1), 1e-9); + } + HybridBayesNet::shared_ptr bn = gfg.eliminateSequential(); - // Since no measurement on x1, we get the ratio of covariances. - DiscreteConditional expected(m1, "0.6/0.4"); - - EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()))); + // Since no measurement on x1, we a 50/50 probability + auto p_m = bn->at(2)->asDiscrete(); + EXPECT_DOUBLES_EQUAL(0.5, p_m->operator()(DiscreteValues{{m1.first, 0}}), + 1e-9); + EXPECT_DOUBLES_EQUAL(0.5, p_m->operator()(DiscreteValues{{m1.first, 1}}), + 1e-9); } { // Now we add a measurement z1 on x1 - hbn.emplace_back(p_z1x1); + hbn = CreateBayesNet(mu0, mu1, sigma0, sigma1, true); given.insert(z1, Vector1(2.2)); HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given); + + { + VectorValues vv{{X(0), Vector1(0.0)}, + {X(1), Vector1(1.0)}, + {Z(0), Vector1(0.5)}, + {Z(1), Vector1(2.2)}}; + HybridValues hv0(vv, DiscreteValues{{M(1), 0}}), + hv1(vv, DiscreteValues{{M(1), 1}}); + EXPECT_DOUBLES_EQUAL(gfg.error(hv0) / hbn.error(hv0), + gfg.error(hv1) / hbn.error(hv1), 1e-9); + } + { + VectorValues vv{{X(0), Vector1(0.5)}, + {X(1), Vector1(3.0)}, + {Z(0), Vector1(0.5)}, + {Z(1), Vector1(2.2)}}; + HybridValues hv0(vv, DiscreteValues{{M(1), 0}}), + hv1(vv, DiscreteValues{{M(1), 1}}); + EXPECT_DOUBLES_EQUAL(gfg.error(hv0) / hbn.error(hv0), + gfg.error(hv1) / hbn.error(hv1), 1e-9); + } + HybridBayesNet::shared_ptr bn = gfg.eliminateSequential(); // Since we have a measurement on z2, we get a definite result - DiscreteConditional expected(m1, "0.52706646/0.47293354"); - + DiscreteConditional expected(m1, "0.44744586/0.55255414"); + // regression EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), 1e-6)); } } -/** - * @brief Helper function to specify a Hybrid Bayes Net - * {P(X1) P(Z1 | X1, X2, M1)} and convert it to a Hybrid Factor Graph - * {P(X1)L(X1, X2, M1; Z1)} by converting to likelihoods given Z1. - * - * We can specify either different means or different sigmas, - * or both for each hybrid factor component. - * - * @param values Initial values for linearization. - * @param means The mean values for the conditional components. - * @param sigmas Noise model sigma values (standard deviation). - * @param m1 The discrete mode key. - * @param z1 The measurement value. - * @return HybridGaussianFactorGraph - */ -HybridGaussianFactorGraph GetFactorGraphFromBayesNet( - const gtsam::Values &values, const std::vector &means, - const std::vector &sigmas, DiscreteKey &m1, double z1 = 0.0) { - // Noise models - auto model0 = noiseModel::Isotropic::Sigma(1, sigmas[0]); - auto model1 = noiseModel::Isotropic::Sigma(1, sigmas[1]); - auto prior_noise = noiseModel::Isotropic::Sigma(1, 1e-3); - - // GaussianMixtureFactor component factors - auto f0 = - std::make_shared>(X(1), X(2), means[0], model0); - auto f1 = - std::make_shared>(X(1), X(2), means[1], model1); - std::vector factors{f0, f1}; - - /// Get terms for each p^m(z1 | x1, x2) - Matrix H0_1, H0_2, H1_1, H1_2; - double x1 = values.at(X(1)), x2 = values.at(X(2)); - Vector d0 = f0->evaluateError(x1, x2, &H0_1, &H0_2); - std::vector> terms0 = {{Z(1), gtsam::I_1x1 /*Rx*/}, - // - {X(1), H0_1 /*Sp1*/}, - {X(2), H0_2 /*Tp2*/}}; - - Vector d1 = f1->evaluateError(x1, x2, &H1_1, &H1_2); - std::vector> terms1 = {{Z(1), gtsam::I_1x1 /*Rx*/}, - // - {X(1), H1_1 /*Sp1*/}, - {X(2), H1_2 /*Tp2*/}}; - // Create conditional P(Z1 | X1, X2, M1) - auto gm = new gtsam::GaussianMixture( - {Z(1)}, {X(1), X(2)}, {m1}, - {std::make_shared(terms0, 1, -d0, model0), - std::make_shared(terms1, 1, -d1, model1)}); - gtsam::HybridBayesNet bn; - bn.emplace_back(gm); - // bn.print(); - - // Create FG via toFactorGraph - gtsam::VectorValues measurements; - measurements.insert(Z(1), gtsam::I_1x1 * z1); // Set Z1 = 0 - HybridGaussianFactorGraph mixture_fg = bn.toFactorGraph(measurements); - - // Linearized prior factor on X1 - auto prior = PriorFactor(X(1), x1, prior_noise).linearize(values); - mixture_fg.push_back(prior); - - return mixture_fg; -} - -/* ************************************************************************* */ -/** - * @brief Test components with differing means. - * - * We specify a hybrid Bayes network P(Z | X, M) =p(X1)p(Z1 | X1, X2, M1), - * which is then converted to a factor graph by specifying Z1. - * This is a different case since now we have a hybrid factor - * with 2 continuous variables ϕ(x1, x2, m1). - * p(Z1 | X1, X2, M1) has 2 factors each for the binary - * mode m1, with only the means being different. - */ -TEST(GaussianMixtureFactor, DifferentMeans) { - DiscreteKey m1(M(1), 2); - - Values values; - double x1 = 0.0, x2 = 1.75; - values.insert(X(1), x1); - values.insert(X(2), x2); - - // Different means, same sigma - std::vector means{0.0, 2.0}, sigmas{1e-0, 1e-0}; - - HybridGaussianFactorGraph hfg = - GetFactorGraphFromBayesNet(values, means, sigmas, m1, 0.0); - - { - // With no measurement on X2, each mode should be equally likely - auto bn = hfg.eliminateSequential(); - HybridValues actual = bn->optimize(); - - HybridValues expected( - VectorValues{{X(1), Vector1(0.0)}, {X(2), Vector1(-1.75)}}, - DiscreteValues{{M(1), 0}}); - - EXPECT(assert_equal(expected, actual)); - - { - DiscreteValues dv{{M(1), 0}}; - VectorValues cont = bn->optimize(dv); - double error = bn->error(HybridValues(cont, dv)); - // regression - EXPECT_DOUBLES_EQUAL(0.69314718056, error, 1e-9); - } - { - DiscreteValues dv{{M(1), 1}}; - VectorValues cont = bn->optimize(dv); - double error = bn->error(HybridValues(cont, dv)); - // regression - EXPECT_DOUBLES_EQUAL(0.69314718056, error, 1e-9); - } - } - { - // If we add a measurement on X2, we have more information to work with. - // Add a measurement on X2 - auto prior_noise = noiseModel::Isotropic::Sigma(1, 1e-3); - GaussianConditional meas_z2(Z(2), Vector1(2.0), I_1x1, X(2), I_1x1, - prior_noise); - auto prior_x2 = meas_z2.likelihood(Vector1(x2)); - - hfg.push_back(prior_x2); - - auto bn = hfg.eliminateSequential(); - HybridValues actual = bn->optimize(); - - HybridValues expected( - VectorValues{{X(1), Vector1(0.0)}, {X(2), Vector1(0.25)}}, - DiscreteValues{{M(1), 1}}); - - EXPECT(assert_equal(expected, actual)); - - { - DiscreteValues dv{{M(1), 0}}; - VectorValues cont = bn->optimize(dv); - double error = bn->error(HybridValues(cont, dv)); - // regression - EXPECT_DOUBLES_EQUAL(2.12692448787, error, 1e-9); - } - { - DiscreteValues dv{{M(1), 1}}; - VectorValues cont = bn->optimize(dv); - double error = bn->error(HybridValues(cont, dv)); - // regression - EXPECT_DOUBLES_EQUAL(0.126928487854, error, 1e-9); - } - } -} - -/* ************************************************************************* */ -/** - * @brief Test components with differing covariances - * but with a Bayes net P(Z|X, M) converted to a FG. - * Same as the DifferentMeans example but in this case, - * we keep the means the same and vary the covariances. - */ -TEST(GaussianMixtureFactor, DifferentCovariances) { - DiscreteKey m1(M(1), 2); - - Values values; - double x1 = 1.0, x2 = 1.0; - values.insert(X(1), x1); - values.insert(X(2), x2); - - std::vector means{0.0, 0.0}, sigmas{1e2, 1e-2}; - HybridGaussianFactorGraph mixture_fg = - GetFactorGraphFromBayesNet(values, means, sigmas, m1); - - auto hbn = mixture_fg.eliminateSequential(); - - VectorValues cv; - cv.insert(X(1), Vector1(0.0)); - cv.insert(X(2), Vector1(0.0)); - - // Check that the error values at the MLE point μ. - AlgebraicDecisionTree errorTree = hbn->errorTree(cv); - - DiscreteValues dv0{{M(1), 0}}; - DiscreteValues dv1{{M(1), 1}}; - - // regression - EXPECT_DOUBLES_EQUAL(9.90348755254, errorTree(dv0), 1e-9); - EXPECT_DOUBLES_EQUAL(0.69314718056, errorTree(dv1), 1e-9); - - DiscreteConditional expected_m1(m1, "0.5/0.5"); - DiscreteConditional actual_m1 = *(hbn->at(2)->asDiscrete()); - - EXPECT(assert_equal(expected_m1, actual_m1)); -} - /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */ \ No newline at end of file +/* ************************************************************************* */ diff --git a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp index 5be2f2742..68b3b8215 100644 --- a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp @@ -598,6 +598,57 @@ TEST(HybridGaussianFactorGraph, ErrorAndProbPrimeTree) { EXPECT(assert_equal(expected_probs, probs, 1e-7)); } +/* ****************************************************************************/ +// Test hybrid gaussian factor graph errorTree when there is a HybridConditional in the graph +TEST(HybridGaussianFactorGraph, ErrorTreeWithConditional) { + using symbol_shorthand::F; + + DiscreteKey m1(M(1), 2); + Key z0 = Z(0), f01 = F(0); + Key x0 = X(0), x1 = X(1); + + HybridBayesNet hbn; + + auto prior_model = noiseModel::Isotropic::Sigma(1, 1e-1); + auto measurement_model = noiseModel::Isotropic::Sigma(1, 2.0); + + // Set a prior P(x0) at x0=0 + hbn.emplace_back( + new GaussianConditional(x0, Vector1(0.0), I_1x1, prior_model)); + + // Add measurement P(z0 | x0) + hbn.emplace_back(new GaussianConditional(z0, Vector1(0.0), -I_1x1, x0, I_1x1, + measurement_model)); + + // Add hybrid motion model + double mu = 0.0; + double sigma0 = 1e2, sigma1 = 1e-2; + auto model0 = noiseModel::Isotropic::Sigma(1, sigma0); + auto model1 = noiseModel::Isotropic::Sigma(1, sigma1); + auto c0 = make_shared(f01, Vector1(mu), I_1x1, x1, I_1x1, + x0, -I_1x1, model0), + c1 = make_shared(f01, Vector1(mu), I_1x1, x1, I_1x1, + x0, -I_1x1, model1); + hbn.emplace_back(new GaussianMixture({f01}, {x0, x1}, {m1}, {c0, c1})); + + // Discrete uniform prior. + hbn.emplace_back(new DiscreteConditional(m1, "0.5/0.5")); + + VectorValues given; + given.insert(z0, Vector1(0.0)); + given.insert(f01, Vector1(0.0)); + auto gfg = hbn.toFactorGraph(given); + + VectorValues vv; + vv.insert(x0, Vector1(1.0)); + vv.insert(x1, Vector1(2.0)); + AlgebraicDecisionTree errorTree = gfg.errorTree(vv); + + // regression + AlgebraicDecisionTree expected(m1, 59.335390372, 5050.125); + EXPECT(assert_equal(expected, errorTree, 1e-9)); +} + /* ****************************************************************************/ // Check that assembleGraphTree assembles Gaussian factor graphs for each // assignment. diff --git a/gtsam/linear/linear.i b/gtsam/linear/linear.i index 3a629f349..ed19eaa3b 100644 --- a/gtsam/linear/linear.i +++ b/gtsam/linear/linear.i @@ -375,7 +375,8 @@ virtual class JacobianFactor : gtsam::GaussianFactor { void serialize() const; }; -pair EliminateQR(const gtsam::GaussianFactorGraph& factors, const gtsam::Ordering& keys); +pair EliminateQR( + const gtsam::GaussianFactorGraph& factors, const gtsam::Ordering& keys); #include virtual class HessianFactor : gtsam::GaussianFactor { diff --git a/gtsam/navigation/PreintegratedRotation.cpp b/gtsam/navigation/PreintegratedRotation.cpp index 04e201a34..6c8e510e5 100644 --- a/gtsam/navigation/PreintegratedRotation.cpp +++ b/gtsam/navigation/PreintegratedRotation.cpp @@ -75,13 +75,11 @@ Rot3 IncrementalRotation::operator()( Vector3 correctedOmega = measuredOmega - bias; // Then compensate for sensor-body displacement: we express the quantities - // (originally in the IMU frame) into the body frame. If Jacobian is - // requested, the rotation matrix is obtained as `rotate` Jacobian. - Matrix3 body_R_sensor; + // (originally in the IMU frame) into the body frame. + // Note that the rotate Jacobian is just body_P_sensor->rotation().matrix(). if (body_P_sensor) { // rotation rate vector in the body frame - correctedOmega = body_P_sensor->rotation().rotate( - correctedOmega, {}, H_bias ? &body_R_sensor : nullptr); + correctedOmega = body_P_sensor->rotation() * correctedOmega; } // rotation vector describing rotation increment computed from the @@ -90,7 +88,7 @@ Rot3 IncrementalRotation::operator()( Rot3 incrR = Rot3::Expmap(integratedOmega, H_bias); // expensive !! if (H_bias) { *H_bias *= -deltaT; // Correct so accurately reflects bias derivative - if (body_P_sensor) *H_bias *= body_R_sensor; + if (body_P_sensor) *H_bias *= body_P_sensor->rotation().matrix(); } return incrR; } diff --git a/gtsam/navigation/tests/testManifoldPreintegration.cpp b/gtsam/navigation/tests/testManifoldPreintegration.cpp index 4016240cf..82f9876fb 100644 --- a/gtsam/navigation/tests/testManifoldPreintegration.cpp +++ b/gtsam/navigation/tests/testManifoldPreintegration.cpp @@ -118,17 +118,18 @@ TEST(ManifoldPreintegration, CompareWithPreintegratedRotation) { // Integrate a single measurement const double omega = 0.1; const Vector3 trueOmega(omega, 0, 0); - const Vector3 bias(1, 2, 3); - const Vector3 measuredOmega = trueOmega + bias; + const Vector3 biasOmega(1, 2, 3); + const Vector3 measuredOmega = trueOmega + biasOmega; const double deltaT = 0.5; // Check the accumulated rotation. Rot3 expected = Rot3::Roll(omega * deltaT); - pim.integrateGyroMeasurement(measuredOmega, bias, deltaT); + const Vector biasOmegaHat = biasOmega; + pim.integrateGyroMeasurement(measuredOmega, biasOmegaHat, deltaT); EXPECT(assert_equal(expected, pim.deltaRij(), 1e-9)); // Now do the same for a ManifoldPreintegration object - imuBias::ConstantBias biasHat {Z_3x1, bias}; + imuBias::ConstantBias biasHat {Z_3x1, biasOmega}; ManifoldPreintegration manifoldPim(testing::Params(), biasHat); manifoldPim.integrateMeasurement(Z_3x1, measuredOmega, deltaT); EXPECT(assert_equal(expected, manifoldPim.deltaRij(), 1e-9)); @@ -136,17 +137,21 @@ TEST(ManifoldPreintegration, CompareWithPreintegratedRotation) { // Check their internal Jacobians are the same: EXPECT(assert_equal(pim.delRdelBiasOmega(), manifoldPim.delRdelBiasOmega())); - // Check PreintegratedRotation::biascorrectedDeltaRij. - Matrix3 H; + // Let's check the derivatives a delta away from the bias hat const double delta = 0.05; const Vector3 biasOmegaIncr(delta, 0, 0); + imuBias::ConstantBias bias_i {Z_3x1, biasOmegaHat + biasOmegaIncr}; + + // Check PreintegratedRotation::biascorrectedDeltaRij. + // Note that biascorrectedDeltaRij expects the biasHat to be subtracted already + Matrix3 H; Rot3 corrected = pim.biascorrectedDeltaRij(biasOmegaIncr, H); EQUALITY(Vector3(-deltaT * delta, 0, 0), expected.logmap(corrected)); const Rot3 expected2 = Rot3::Roll((omega - delta) * deltaT); EXPECT(assert_equal(expected2, corrected, 1e-9)); // Check ManifoldPreintegration::biasCorrectedDelta. - imuBias::ConstantBias bias_i {Z_3x1, bias + biasOmegaIncr}; + // Note that, confusingly, biasCorrectedDelta will subtract biasHat inside Matrix96 H2; Vector9 biasCorrected = manifoldPim.biasCorrectedDelta(bias_i, H2); Vector9 expected3; @@ -154,12 +159,11 @@ TEST(ManifoldPreintegration, CompareWithPreintegratedRotation) { EXPECT(assert_equal(expected3, biasCorrected, 1e-9)); // Check that this one is sane: - auto g = [&](const Vector3& increment) { - return manifoldPim.biasCorrectedDelta({Z_3x1, bias + increment}, {}); + auto g = [&](const Vector3& b) { + return manifoldPim.biasCorrectedDelta({Z_3x1, b}, {}); }; - EXPECT(assert_equal(numericalDerivative11(g, Z_3x1), - H2.rightCols<3>(), - 1e-4)); // NOTE(frank): reduced tolerance + EXPECT(assert_equal(numericalDerivative11(g, bias_i.gyroscope()), + H2.rightCols<3>())); } /* ************************************************************************* */ diff --git a/gtsam/navigation/tests/testPreintegratedRotation.cpp b/gtsam/navigation/tests/testPreintegratedRotation.cpp index 1e27ca1e4..468701e3c 100644 --- a/gtsam/navigation/tests/testPreintegratedRotation.cpp +++ b/gtsam/navigation/tests/testPreintegratedRotation.cpp @@ -36,40 +36,33 @@ const Vector3 measuredOmega = trueOmega + bias; const double deltaT = 0.5; } // namespace biased_x_rotation -// Create params where x and y axes are exchanged. -static std::shared_ptr paramsWithTransform() { - auto p = std::make_shared(); - p->setBodyPSensor({Rot3::Yaw(M_PI_2), {0, 0, 0}}); - return p; -} - //****************************************************************************** TEST(PreintegratedRotation, integrateGyroMeasurement) { // Example where IMU is identical to body frame, then omega is roll using namespace biased_x_rotation; auto p = std::make_shared(); - PreintegratedRotation pim(p); // Check the value. Matrix3 H_bias; - internal::IncrementalRotation f{measuredOmega, deltaT, p->getBodyPSensor()}; + const internal::IncrementalRotation f{measuredOmega, deltaT, p->getBodyPSensor()}; const Rot3 incrR = f(bias, H_bias); - Rot3 expected = Rot3::Roll(omega * deltaT); - EXPECT(assert_equal(expected, incrR, 1e-9)); + const Rot3 expected = Rot3::Roll(omega * deltaT); + EXPECT(assert_equal(expected, incrR, 1e-9)) // Check the derivative: - EXPECT(assert_equal(numericalDerivative11(f, bias), H_bias)); + EXPECT(assert_equal(numericalDerivative11(f, bias), H_bias)) // Check value of deltaRij() after integration. Matrix3 F; + PreintegratedRotation pim(p); pim.integrateGyroMeasurement(measuredOmega, bias, deltaT, F); - EXPECT(assert_equal(expected, pim.deltaRij(), 1e-9)); + EXPECT(assert_equal(expected, pim.deltaRij(), 1e-9)) // Check that system matrix F is the first derivative of compose: - EXPECT(assert_equal(pim.deltaRij().inverse().AdjointMap(), F)); + EXPECT(assert_equal(pim.deltaRij().inverse().AdjointMap(), F)) // Make sure delRdelBiasOmega is H_bias after integration. - EXPECT(assert_equal(H_bias, pim.delRdelBiasOmega())); + EXPECT(assert_equal(H_bias, pim.delRdelBiasOmega())) // Check if we make a correction to the bias, the value and Jacobian are // correct. Note that the bias is subtracted from the measurement, and the @@ -78,56 +71,127 @@ TEST(PreintegratedRotation, integrateGyroMeasurement) { const double delta = 0.05; const Vector3 biasOmegaIncr(delta, 0, 0); Rot3 corrected = pim.biascorrectedDeltaRij(biasOmegaIncr, H); - EQUALITY(Vector3(-deltaT * delta, 0, 0), expected.logmap(corrected)); - EXPECT(assert_equal(Rot3::Roll((omega - delta) * deltaT), corrected, 1e-9)); + EQUALITY(Vector3(-deltaT * delta, 0, 0), expected.logmap(corrected)) + EXPECT(assert_equal(Rot3::Roll((omega - delta) * deltaT), corrected, 1e-9)) - // TODO(frank): again the derivative is not the *sane* one! - // auto g = [&](const Vector3& increment) { - // return pim.biascorrectedDeltaRij(increment, {}); - // }; - // EXPECT(assert_equal(numericalDerivative11(g, Z_3x1), H)); + // Check the derivative matches the numerical one + auto g = [&](const Vector3& increment) { + return pim.biascorrectedDeltaRij(increment, {}); + }; + Matrix3 expectedH = numericalDerivative11(g, biasOmegaIncr); + EXPECT(assert_equal(expectedH, H)); + + // Let's integrate a second IMU measurement and check the Jacobian update: + pim.integrateGyroMeasurement(measuredOmega, bias, deltaT); + expectedH = numericalDerivative11(g, biasOmegaIncr); + corrected = pim.biascorrectedDeltaRij(biasOmegaIncr, H); + EXPECT(assert_equal(expectedH, H)); } //****************************************************************************** + +// Create params where x and y axes are exchanged. +static std::shared_ptr paramsWithTransform() { + auto p = std::make_shared(); + p->setBodyPSensor({Rot3::Yaw(M_PI_2), {0, 0, 0}}); + return p; +} + TEST(PreintegratedRotation, integrateGyroMeasurementWithTransform) { // Example where IMU is rotated, so measured omega indicates pitch. using namespace biased_x_rotation; auto p = paramsWithTransform(); - PreintegratedRotation pim(p); // Check the value. Matrix3 H_bias; - internal::IncrementalRotation f{measuredOmega, deltaT, p->getBodyPSensor()}; - Rot3 expected = Rot3::Pitch(omega * deltaT); - EXPECT(assert_equal(expected, f(bias, H_bias), 1e-9)); + const internal::IncrementalRotation f{measuredOmega, deltaT, p->getBodyPSensor()}; + const Rot3 expected = Rot3::Pitch(omega * deltaT); // Pitch, because of sensor-IMU rotation! + EXPECT(assert_equal(expected, f(bias, H_bias), 1e-9)) // Check the derivative: - EXPECT(assert_equal(numericalDerivative11(f, bias), H_bias)); + EXPECT(assert_equal(numericalDerivative11(f, bias), H_bias)) // Check value of deltaRij() after integration. Matrix3 F; + PreintegratedRotation pim(p); pim.integrateGyroMeasurement(measuredOmega, bias, deltaT, F); - EXPECT(assert_equal(expected, pim.deltaRij(), 1e-9)); + EXPECT(assert_equal(expected, pim.deltaRij(), 1e-9)) // Check that system matrix F is the first derivative of compose: - EXPECT(assert_equal(pim.deltaRij().inverse().AdjointMap(), F)); + EXPECT(assert_equal(pim.deltaRij().inverse().AdjointMap(), F)) // Make sure delRdelBiasOmega is H_bias after integration. - EXPECT(assert_equal(H_bias, pim.delRdelBiasOmega())); + EXPECT(assert_equal(H_bias, pim.delRdelBiasOmega())) // Check the bias correction in same way, but will now yield pitch change. Matrix3 H; const double delta = 0.05; const Vector3 biasOmegaIncr(delta, 0, 0); Rot3 corrected = pim.biascorrectedDeltaRij(biasOmegaIncr, H); - EQUALITY(Vector3(0, -deltaT * delta, 0), expected.logmap(corrected)); - EXPECT(assert_equal(Rot3::Pitch((omega - delta) * deltaT), corrected, 1e-9)); + EQUALITY(Vector3(0, -deltaT * delta, 0), expected.logmap(corrected)) + EXPECT(assert_equal(Rot3::Pitch((omega - delta) * deltaT), corrected, 1e-9)) - // TODO(frank): again the derivative is not the *sane* one! - // auto g = [&](const Vector3& increment) { - // return pim.biascorrectedDeltaRij(increment, {}); - // }; - // EXPECT(assert_equal(numericalDerivative11(g, Z_3x1), H)); + // Check the derivative matches the *expectedH* one + auto g = [&](const Vector3& increment) { + return pim.biascorrectedDeltaRij(increment, {}); + }; + Matrix3 expectedH = numericalDerivative11(g, biasOmegaIncr); + EXPECT(assert_equal(expectedH, H)); + + // Let's integrate a second IMU measurement and check the Jacobian update: + pim.integrateGyroMeasurement(measuredOmega, bias, deltaT); + corrected = pim.biascorrectedDeltaRij(biasOmegaIncr, H); + expectedH = numericalDerivative11(g, biasOmegaIncr); + EXPECT(assert_equal(expectedH, H)); +} + +// Create params we have a non-axis-aligned rotation and even an offset. +static std::shared_ptr paramsWithArbitraryTransform() { + auto p = std::make_shared(); + p->setBodyPSensor({Rot3::Expmap({1,2,3}), {4,5,6}}); + return p; +} + +TEST(PreintegratedRotation, integrateGyroMeasurementWithArbitraryTransform) { + // Example with a non-axis-aligned transform and some position. + using namespace biased_x_rotation; + auto p = paramsWithArbitraryTransform(); + + // Check the derivative: + Matrix3 H_bias; + const internal::IncrementalRotation f{measuredOmega, deltaT, p->getBodyPSensor()}; + f(bias, H_bias); + EXPECT(assert_equal(numericalDerivative11(f, bias), H_bias)) + + // Check derivative of deltaRij() after integration. + Matrix3 F; + PreintegratedRotation pim(p); + pim.integrateGyroMeasurement(measuredOmega, bias, deltaT, F); + + // Check that system matrix F is the first derivative of compose: + EXPECT(assert_equal(pim.deltaRij().inverse().AdjointMap(), F)) + + // Make sure delRdelBiasOmega is H_bias after integration. + EXPECT(assert_equal(H_bias, pim.delRdelBiasOmega())) + + // Check the bias correction in same way, but will now yield pitch change. + Matrix3 H; + const double delta = 0.05; + const Vector3 biasOmegaIncr(delta, 0, 0); + Rot3 corrected = pim.biascorrectedDeltaRij(biasOmegaIncr, H); + + // Check the derivative matches the numerical one + auto g = [&](const Vector3& increment) { + return pim.biascorrectedDeltaRij(increment, {}); + }; + Matrix3 expectedH = numericalDerivative11(g, biasOmegaIncr); + EXPECT(assert_equal(expectedH, H)); + + // Let's integrate a second IMU measurement and check the Jacobian update: + pim.integrateGyroMeasurement(measuredOmega, bias, deltaT); + corrected = pim.biascorrectedDeltaRij(biasOmegaIncr, H); + expectedH = numericalDerivative11(g, biasOmegaIncr); + EXPECT(assert_equal(expectedH, H)); } //****************************************************************************** diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 0b5449b58..47857a2a2 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -424,6 +424,11 @@ ISAM2Result ISAM2::update(const NonlinearFactorGraph& newFactors, ISAM2Result result(params_.enableDetailedResults); UpdateImpl update(params_, updateParams); + // Initialize any new variables \Theta_{new} and add + // \Theta:=\Theta\cup\Theta_{new}. + // Needed before delta update if using Dogleg optimizer. + addVariables(newTheta, result.details()); + // Update delta if we need it to check relinearization later if (update.relinarizationNeeded(update_count_)) updateDelta(updateParams.forceFullSolve); @@ -435,9 +440,7 @@ ISAM2Result ISAM2::update(const NonlinearFactorGraph& newFactors, update.computeUnusedKeys(newFactors, variableIndex_, result.keysWithRemovedFactors, &result.unusedKeys); - // 2. Initialize any new variables \Theta_{new} and add - // \Theta:=\Theta\cup\Theta_{new}. - addVariables(newTheta, result.details()); + // 2. Compute new error to check for relinearization if (params_.evaluateNonlinearError) update.error(nonlinearFactors_, calculateEstimate(), &result.errorBefore); @@ -731,6 +734,7 @@ void ISAM2::updateDelta(bool forceFullSolve) const { effectiveWildfireThreshold, &delta_); deltaReplacedMask_.clear(); gttoc(Wildfire_update); + } else if (std::holds_alternative(params_.optimizationParams)) { // If using Dogleg, do a Dogleg step const ISAM2DoglegParams& doglegParams = @@ -769,9 +773,8 @@ void ISAM2::updateDelta(bool forceFullSolve) const { gttic(Copy_dx_d); // Update Delta and linear step doglegDelta_ = doglegResult.delta; - delta_ = - doglegResult - .dx_d; // Copy the VectorValues containing with the linear solution + // Copy the VectorValues containing with the linear solution + delta_ = doglegResult.dx_d; gttoc(Copy_dx_d); } else { throw std::runtime_error("iSAM2: unknown ISAM2Params type"); diff --git a/gtsam/symbolic/SymbolicFactor-inst.h b/gtsam/symbolic/SymbolicFactor-inst.h index db0eb05ca..a5410aad5 100644 --- a/gtsam/symbolic/SymbolicFactor-inst.h +++ b/gtsam/symbolic/SymbolicFactor-inst.h @@ -42,7 +42,9 @@ namespace gtsam // Gather all keys KeySet allKeys; for(const std::shared_ptr& factor: factors) { - allKeys.insert(factor->begin(), factor->end()); + // Non-active factors are nullptr + if (factor) + allKeys.insert(factor->begin(), factor->end()); } // Check keys diff --git a/gtsam_unstable/gtsam_unstable.i b/gtsam_unstable/gtsam_unstable.i index 8566934dd..3beb845fb 100644 --- a/gtsam_unstable/gtsam_unstable.i +++ b/gtsam_unstable/gtsam_unstable.i @@ -671,6 +671,21 @@ class AHRS { //void print(string s) const; }; +#include +template +virtual class PartialPriorFactor : gtsam::NoiseModelFactor { + PartialPriorFactor(gtsam::Key key, size_t idx, double prior, + const gtsam::noiseModel::Base* model); + PartialPriorFactor(gtsam::Key key, const std::vector& indices, + const gtsam::Vector& prior, + const gtsam::noiseModel::Base* model); + + // enabling serialization functionality + void serialize() const; + + const gtsam::Vector& prior(); +}; + // Tectonic SAM Factors #include diff --git a/gtsam_unstable/slam/PartialPriorFactor.h b/gtsam_unstable/slam/PartialPriorFactor.h index b70fe8b19..7722e5d82 100644 --- a/gtsam_unstable/slam/PartialPriorFactor.h +++ b/gtsam_unstable/slam/PartialPriorFactor.h @@ -50,9 +50,6 @@ namespace gtsam { Vector prior_; ///< Measurement on tangent space parameters, in compressed form. std::vector indices_; ///< Indices of the measured tangent space parameters. - /** default constructor - only use for serialization */ - PartialPriorFactor() {} - /** * constructor with just minimum requirements for a factor - allows more * computation in the constructor. This should only be used by subclasses @@ -65,7 +62,8 @@ namespace gtsam { // Provide access to the Matrix& version of evaluateError: using Base::evaluateError; - ~PartialPriorFactor() override {} + /** default constructor - only use for serialization */ + PartialPriorFactor() {} /** Single Element Constructor: Prior on a single parameter at index 'idx' in the tangent vector.*/ PartialPriorFactor(Key key, size_t idx, double prior, const SharedNoiseModel& model) : @@ -85,6 +83,8 @@ namespace gtsam { assert(model->dim() == (size_t)prior.size()); } + ~PartialPriorFactor() override {} + /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { return std::static_pointer_cast( diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index ba55ac2af..43f8d78bd 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -29,11 +29,8 @@ if(POLICY CMP0057) cmake_policy(SET CMP0057 NEW) endif() -# Prefer system pybind11 first, if not found, rely on bundled version: -find_package(pybind11 CONFIG QUIET) -if (NOT pybind11_FOUND) - add_subdirectory(${PROJECT_SOURCE_DIR}/wrap/pybind11 pybind11) -endif() +# Use bundled pybind11 version +add_subdirectory(${PROJECT_SOURCE_DIR}/wrap/pybind11 pybind11) # Set the wrapping script variable set(PYBIND_WRAP_SCRIPT "${PROJECT_SOURCE_DIR}/wrap/scripts/pybind_wrap.py") @@ -189,6 +186,8 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON) gtsam::BinaryMeasurementsPoint3 gtsam::BinaryMeasurementsUnit3 gtsam::BinaryMeasurementsRot3 + gtsam::SimWall2DVector + gtsam::SimPolygon2DVector gtsam::CameraSetCal3_S2 gtsam::CameraSetCal3Bundler gtsam::CameraSetCal3Unified @@ -264,11 +263,25 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON) endif() # Add custom target so we can install with `make python-install` +# Note below we make sure to install with --user iff not in a virtualenv set(GTSAM_PYTHON_INSTALL_TARGET python-install) -add_custom_target(${GTSAM_PYTHON_INSTALL_TARGET} - COMMAND ${PYTHON_EXECUTABLE} -m pip install --user . +#TODO(Varun) Maybe move the long command to script? +# https://stackoverflow.com/questions/49053544/how-do-i-run-a-python-script-every-time-in-a-cmake-build +if (NOT WIN32) # WIN32=1 is target platform is Windows + add_custom_target(${GTSAM_PYTHON_INSTALL_TARGET} + COMMAND stubgen -q -p gtsam && cp -a out/gtsam/ gtsam && ${PYTHON_EXECUTABLE} -c "import sys, subprocess; cmd = [sys.executable, '-m', 'pip', 'install']; has_venv = hasattr(sys, 'real_prefix') or (hasattr(sys, 'base_prefix') and sys.base_prefix != sys.prefix); cmd.append('--user' if not has_venv else ''); cmd.append('.'); subprocess.check_call([c for c in cmd if c])" DEPENDS ${GTSAM_PYTHON_DEPENDENCIES} - WORKING_DIRECTORY ${GTSAM_PYTHON_BUILD_DIRECTORY}) + WORKING_DIRECTORY ${GTSAM_PYTHON_BUILD_DIRECTORY} + VERBATIM) +else() + #TODO(Varun) Find equivalent cp on Windows + add_custom_target(${GTSAM_PYTHON_INSTALL_TARGET} + COMMAND ${PYTHON_EXECUTABLE} -c "import sys, subprocess; cmd = [sys.executable, '-m', 'pip', 'install']; has_venv = hasattr(sys, 'real_prefix') or (hasattr(sys, 'base_prefix') and sys.base_prefix != sys.prefix); cmd.append('--user' if not has_venv else ''); cmd.append('.'); subprocess.check_call([c for c in cmd if c])" + DEPENDS ${GTSAM_PYTHON_DEPENDENCIES} + WORKING_DIRECTORY ${GTSAM_PYTHON_BUILD_DIRECTORY} + VERBATIM) +endif() + # Custom make command to run all GTSAM Python tests add_custom_target( diff --git a/python/dev_requirements.txt b/python/dev_requirements.txt index 6970ee613..6a5e7f924 100644 --- a/python/dev_requirements.txt +++ b/python/dev_requirements.txt @@ -1,2 +1,3 @@ -r requirements.txt -pyparsing>=2.4.2 \ No newline at end of file +pyparsing>=2.4.2 +mypy==1.4.1 #TODO(Varun) A bug in mypy>=1.5.0 causes an unresolved placeholder error when importing numpy>=2.0.0 (https://github.com/python/mypy/issues/17396) \ No newline at end of file diff --git a/python/gtsam/examples/README.md b/python/gtsam/examples/README.md index 2a2c9f2ef..70f6cfcb0 100644 --- a/python/gtsam/examples/README.md +++ b/python/gtsam/examples/README.md @@ -17,7 +17,7 @@ | InverseKinematicsExampleExpressions.cpp | | | ISAM2Example_SmartFactor | | | ISAM2_SmartFactorStereo_IMU | | -| LocalizationExample | impossible? | +| LocalizationExample | :heavy_check_mark: | | METISOrderingExample | | | OdometryExample | :heavy_check_mark: | | PlanarSLAMExample | :heavy_check_mark: | diff --git a/python/gtsam_unstable/examples/LocalizationExample.py b/python/gtsam_unstable/examples/LocalizationExample.py new file mode 100644 index 000000000..ad8266d6d --- /dev/null +++ b/python/gtsam_unstable/examples/LocalizationExample.py @@ -0,0 +1,68 @@ +""" +A simple 2D pose slam example with "GPS" measurements + - The robot moves forward 2 meter each iteration + - The robot initially faces along the X axis (horizontal, to the right in 2D) + - We have full odometry between pose + - We have "GPS-like" measurements implemented with a custom factor +""" +import numpy as np + +import gtsam +from gtsam import BetweenFactorPose2, Pose2, noiseModel +from gtsam_unstable import PartialPriorFactorPose2 + + +def main(): + # 1. Create a factor graph container and add factors to it. + graph = gtsam.NonlinearFactorGraph() + + # 2a. Add odometry factors + # For simplicity, we will use the same noise model for each odometry factor + odometryNoise = noiseModel.Diagonal.Sigmas(np.asarray([0.2, 0.2, 0.1])) + + # Create odometry (Between) factors between consecutive poses + graph.push_back( + BetweenFactorPose2(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise)) + graph.push_back( + BetweenFactorPose2(2, 3, Pose2(2.0, 0.0, 0.0), odometryNoise)) + + # 2b. Add "GPS-like" measurements + # We will use PartialPrior factor for this. + unaryNoise = noiseModel.Diagonal.Sigmas(np.array([0.1, + 0.1])) # 10cm std on x,y + + graph.push_back( + PartialPriorFactorPose2(1, [0, 1], np.asarray([0.0, 0.0]), unaryNoise)) + graph.push_back( + PartialPriorFactorPose2(2, [0, 1], np.asarray([2.0, 0.0]), unaryNoise)) + graph.push_back( + PartialPriorFactorPose2(3, [0, 1], np.asarray([4.0, 0.0]), unaryNoise)) + graph.print("\nFactor Graph:\n") + + # 3. Create the data structure to hold the initialEstimate estimate to the solution + # For illustrative purposes, these have been deliberately set to incorrect values + initialEstimate = gtsam.Values() + initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2)) + initialEstimate.insert(2, Pose2(2.3, 0.1, -0.2)) + initialEstimate.insert(3, Pose2(4.1, 0.1, 0.1)) + initialEstimate.print("\nInitial Estimate:\n") + + # 4. Optimize using Levenberg-Marquardt optimization. The optimizer + # accepts an optional set of configuration parameters, controlling + # things like convergence criteria, the type of linear system solver + # to use, and the amount of information displayed during optimization. + # Here we will use the default set of parameters. See the + # documentation for the full set of parameters. + optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate) + result = optimizer.optimize() + result.print("Final Result:\n") + + # 5. Calculate and print marginal covariances for all variables + marginals = gtsam.Marginals(graph, result) + print("x1 covariance:\n", marginals.marginalCovariance(1)) + print("x2 covariance:\n", marginals.marginalCovariance(2)) + print("x3 covariance:\n", marginals.marginalCovariance(3)) + + +if __name__ == "__main__": + main() diff --git a/python/gtsam_unstable/gtsam_unstable.tpl b/python/gtsam_unstable/gtsam_unstable.tpl index b98872c46..006ca7fc8 100644 --- a/python/gtsam_unstable/gtsam_unstable.tpl +++ b/python/gtsam_unstable/gtsam_unstable.tpl @@ -9,6 +9,7 @@ #include #include +#include #include #include #include diff --git a/python/setup.py.in b/python/setup.py.in index 824a6656e..b9d7392c7 100644 --- a/python/setup.py.in +++ b/python/setup.py.in @@ -13,6 +13,7 @@ package_data = { "./*.so", "./*.dll", "./*.pyd", + "*.pyi", "**/*.pyi", # Add the type hints ] } diff --git a/tests/testDoglegOptimizer.cpp b/tests/testDoglegOptimizer.cpp index e64e5ab7a..6fbc522d3 100644 --- a/tests/testDoglegOptimizer.cpp +++ b/tests/testDoglegOptimizer.cpp @@ -24,10 +24,9 @@ #include #include #include -#include -#include -#include -#include +#include +#include +#include "examples/SFMdata.h" #include @@ -36,7 +35,6 @@ using namespace gtsam; // Convenience for named keys using symbol_shorthand::X; -using symbol_shorthand::L; /* ************************************************************************* */ TEST(DoglegOptimizer, ComputeBlend) { @@ -185,6 +183,128 @@ TEST(DoglegOptimizer, Constraint) { #endif } +/* ************************************************************************* */ +/** + * Test created to fix issue in ISAM2 when using the DogLegOptimizer. + * Originally reported by kvmanohar22 in issue #301 + * https://github.com/borglab/gtsam/issues/301 + * + * This test is based on a script provided by kvmanohar22 + * to help reproduce the issue. + */ +TEST(DogLegOptimizer, VariableUpdate) { + // Make the typename short so it looks much cleaner + typedef SmartProjectionPoseFactor SmartFactor; + + // create a typedef to the camera type + typedef PinholePose Camera; + // Define the camera calibration parameters + Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)); + + // Define the camera observation noise model + noiseModel::Isotropic::shared_ptr measurementNoise = + noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v + + // Create the set of ground-truth landmarks and poses + vector points = createPoints(); + vector poses = createPoses(); + + // Create a factor graph + NonlinearFactorGraph graph; + + ISAM2DoglegParams doglegparams = ISAM2DoglegParams(); + doglegparams.verbose = false; + ISAM2Params isam2_params; + isam2_params.evaluateNonlinearError = true; + isam2_params.relinearizeThreshold = 0.0; + isam2_params.enableRelinearization = true; + isam2_params.optimizationParams = doglegparams; + isam2_params.relinearizeSkip = 1; + ISAM2 isam2(isam2_params); + + // Simulated measurements from each camera pose, adding them to the factor + // graph + unordered_map smart_factors; + for (size_t j = 0; j < points.size(); ++j) { + // every landmark represent a single landmark, we use shared pointer to init + // the factor, and then insert measurements. + SmartFactor::shared_ptr smartfactor(new SmartFactor(measurementNoise, K)); + + for (size_t i = 0; i < poses.size(); ++i) { + // generate the 2D measurement + Camera camera(poses[i], K); + Point2 measurement = camera.project(points[j]); + + // call add() function to add measurement into a single factor, here we + // need to add: + // 1. the 2D measurement + // 2. the corresponding camera's key + // 3. camera noise model + // 4. camera calibration + + // add only first 3 measurements and update the later measurements + // incrementally + if (i < 3) smartfactor->add(measurement, i); + } + + // insert the smart factor in the graph + smart_factors[j] = smartfactor; + graph.push_back(smartfactor); + } + + // Add a prior on pose x0. This indirectly specifies where the origin is. + // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw + noiseModel::Diagonal::shared_ptr noise = noiseModel::Diagonal::Sigmas( + (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished()); + graph.emplace_shared >(0, poses[0], noise); + + // Because the structure-from-motion problem has a scale ambiguity, the + // problem is still under-constrained. Here we add a prior on the second pose + // x1, so this will fix the scale by indicating the distance between x0 and + // x1. Because these two are fixed, the rest of the poses will be also be + // fixed. + graph.emplace_shared >(1, poses[1], + noise); // add directly to graph + + // Create the initial estimate to the solution + // Intentionally initialize the variables off from the ground truth + Values initialEstimate; + Pose3 delta(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)); + for (size_t i = 0; i < 3; ++i) + initialEstimate.insert(i, poses[i].compose(delta)); + // initialEstimate.print("Initial Estimates:\n"); + + // Optimize the graph and print results + isam2.update(graph, initialEstimate); + Values result = isam2.calculateEstimate(); + // result.print("Results:\n"); + + // we add new measurements from this pose + size_t pose_idx = 3; + + // Now update existing smart factors with new observations + for (size_t j = 0; j < points.size(); ++j) { + SmartFactor::shared_ptr smartfactor = smart_factors[j]; + + // add the 4th measurement + Camera camera(poses[pose_idx], K); + Point2 measurement = camera.project(points[j]); + smartfactor->add(measurement, pose_idx); + } + + graph.resize(0); + initialEstimate.clear(); + + // update initial estimate for the new pose + initialEstimate.insert(pose_idx, poses[pose_idx].compose(delta)); + + // this should break the system + isam2.update(graph, initialEstimate); + result = isam2.calculateEstimate(); + EXPECT(std::find(result.keys().begin(), result.keys().end(), pose_idx) != + result.keys().end()); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index 1ea60dac9..721ed51c0 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -994,6 +994,56 @@ TEST(ISAM2, calculate_nnz) EXPECT_LONGS_EQUAL(expected, actual); } +class FixActiveFactor : public NoiseModelFactorN { + using Base = NoiseModelFactorN; + bool is_active_; + +public: + FixActiveFactor(const gtsam::Key& key, const bool active) + : Base(nullptr, key), is_active_(active) {} + + virtual bool active(const gtsam::Values &values) const override { + return is_active_; + } + + virtual Vector + evaluateError(const Vector2& x, + Base::OptionalMatrixTypeT H = nullptr) const override { + if (H) { + *H = Vector2::Identity(); + } + return Vector2::Zero(); + } +}; + +TEST(ActiveFactorTesting, Issue1596) { + // Issue1596: When a derived Nonlinear Factor is not active, the linearization returns a nullptr (NonlinearFactor.cpp:156), which + // causes an error when `EliminateSymbolic` is called (SymbolicFactor-inst.h:45) due to not checking if the factor is nullptr. + const gtsam::Key key{Symbol('x', 0)}; + + ISAM2 isam; + Values values; + NonlinearFactorGraph graph; + + // Insert an active factor + values.insert(key, Vector2::Zero()); + graph.emplace_shared(key, true); + + // No problem here + isam.update(graph, values); + + graph = NonlinearFactorGraph(); + + // Inserting a factor that is never active + graph.emplace_shared(key, false); + + // This call throws the error if the pointer is not validated on (SymbolicFactor-inst.h:45) + isam.update(graph); + + // If the bug is fixed, this line is reached. + EXPECT(isam.getFactorsUnsafe().size() == 2); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */