From cb226633c59236856a50ae962219656c8eb73e8b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 23 Oct 2023 18:24:25 -0400 Subject: [PATCH 01/14] IterativeSolver equals method --- gtsam/linear/IterativeSolver.cpp | 6 ++++++ gtsam/linear/IterativeSolver.h | 16 +++++++++------- 2 files changed, 15 insertions(+), 7 deletions(-) diff --git a/gtsam/linear/IterativeSolver.cpp b/gtsam/linear/IterativeSolver.cpp index c5a25c4b8..e9b0a5f13 100644 --- a/gtsam/linear/IterativeSolver.cpp +++ b/gtsam/linear/IterativeSolver.cpp @@ -46,6 +46,12 @@ void IterativeOptimizationParameters::print(ostream &os) const { << verbosityTranslator(verbosity_) << endl; } +/*****************************************************************************/ +bool IterativeOptimizationParameters::equals( + const IterativeOptimizationParameters &other, double tol) const { + return verbosity_ == other.verbosity(); +} + /*****************************************************************************/ ostream& operator<<(ostream &os, const IterativeOptimizationParameters &p) { p.print(os); diff --git a/gtsam/linear/IterativeSolver.h b/gtsam/linear/IterativeSolver.h index 0441cd9da..1a66708d4 100644 --- a/gtsam/linear/IterativeSolver.h +++ b/gtsam/linear/IterativeSolver.h @@ -41,15 +41,14 @@ class VectorValues; * parameters for iterative linear solvers */ class IterativeOptimizationParameters { - -public: - + public: typedef std::shared_ptr shared_ptr; - enum Verbosity { - SILENT = 0, COMPLEXITY, ERROR - } verbosity_; + enum Verbosity { SILENT = 0, COMPLEXITY, ERROR }; -public: + protected: + Verbosity verbosity_; + + public: IterativeOptimizationParameters(Verbosity v = SILENT) : verbosity_(v) { @@ -71,6 +70,9 @@ public: /* virtual print function */ GTSAM_EXPORT virtual void print(std::ostream &os) const; + GTSAM_EXPORT virtual bool equals(const IterativeOptimizationParameters &other, + double tol = 1e-9) const; + /* for serialization */ GTSAM_EXPORT friend std::ostream &operator<<( std::ostream &os, const IterativeOptimizationParameters &p); From 48b270efc57fdde5d0726a050c0689a151d996eb Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 23 Oct 2023 18:24:41 -0400 Subject: [PATCH 02/14] improved equals method for NonlinearOptimizerParams --- gtsam/nonlinear/NonlinearOptimizerParams.h | 24 ++++++++++++++-------- 1 file changed, 16 insertions(+), 8 deletions(-) diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.h b/gtsam/nonlinear/NonlinearOptimizerParams.h index cafb235bc..92e22e064 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.h +++ b/gtsam/nonlinear/NonlinearOptimizerParams.h @@ -115,14 +115,22 @@ public: virtual void print(const std::string& str = "") const; bool equals(const NonlinearOptimizerParams& other, double tol = 1e-9) const { - return maxIterations == other.getMaxIterations() - && std::abs(relativeErrorTol - other.getRelativeErrorTol()) <= tol - && std::abs(absoluteErrorTol - other.getAbsoluteErrorTol()) <= tol - && std::abs(errorTol - other.getErrorTol()) <= tol - && verbosityTranslator(verbosity) == other.getVerbosity(); - // && orderingType.equals(other.getOrderingType()_; - // && linearSolverType == other.getLinearSolverType(); - // TODO: check ordering, iterativeParams, and iterationsHook + // Check for equality of shared ptrs + bool iterative_params_equal = false; + if (iterativeParams == other.iterativeParams) { + iterative_params_equal = true; + } + if (iterativeParams && other.iterativeParams) { + iterative_params_equal = iterativeParams->equals(*other.iterativeParams); + } + + return maxIterations == other.getMaxIterations() && + std::abs(relativeErrorTol - other.getRelativeErrorTol()) <= tol && + std::abs(absoluteErrorTol - other.getAbsoluteErrorTol()) <= tol && + std::abs(errorTol - other.getErrorTol()) <= tol && + verbosityTranslator(verbosity) == other.getVerbosity() && + orderingType == other.orderingType && ordering == other.ordering && + linearSolverType == other.linearSolverType && iterative_params_equal; } inline bool isMultifrontal() const { From 19542053ccb9047085aa5282108efe8835492828 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 15 Nov 2023 10:43:33 -0500 Subject: [PATCH 03/14] better formatting for LM Optimizer summary --- gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index c49e3a65a..99682fb77 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -236,9 +236,9 @@ bool LevenbergMarquardtOptimizer::tryLambda(const GaussianFactorGraph& linear, if (currentState->iterations == 0) { cout << "iter cost cost_change lambda success iter_time" << endl; } - cout << setw(4) << currentState->iterations << " " << setw(8) << newError << " " << setw(3) << setprecision(2) - << costChange << " " << setw(3) << setprecision(2) << currentState->lambda << " " << setw(4) - << systemSolvedSuccessfully << " " << setw(3) << setprecision(2) << iterationTime << endl; + cout << setw(4) << currentState->iterations << " " << setw(12) << newError << " " << setw(12) << setprecision(2) + << costChange << " " << setw(10) << setprecision(2) << currentState->lambda << " " << setw(6) + << systemSolvedSuccessfully << " " << setw(10) << setprecision(2) << iterationTime << endl; } if (step_is_successful) { // we have successfully decreased the cost and we have good modelFidelity From 5cf1e0d220534ddcf712cc6ca09308e9658ba20f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 24 Aug 2024 06:19:00 -0400 Subject: [PATCH 04/14] move NonlinearOptimizerParams::equals definition to .cpp file and improve slightly --- gtsam/nonlinear/NonlinearOptimizerParams.cpp | 22 ++++++++++++++++++++ gtsam/nonlinear/NonlinearOptimizerParams.h | 19 +---------------- 2 files changed, 23 insertions(+), 18 deletions(-) diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.cpp b/gtsam/nonlinear/NonlinearOptimizerParams.cpp index 671dbe34d..800db02a0 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.cpp +++ b/gtsam/nonlinear/NonlinearOptimizerParams.cpp @@ -123,6 +123,28 @@ void NonlinearOptimizerParams::print(const std::string& str) const { std::cout.flush(); } +/* ************************************************************************* */ +bool NonlinearOptimizerParams::equals(const NonlinearOptimizerParams& other, + double tol) const { + // Check for equality of shared ptrs + bool iterative_params_equal = iterativeParams == other.iterativeParams; + // Check equality of components + if (iterativeParams && other.iterativeParams) { + iterative_params_equal = iterativeParams->equals(*other.iterativeParams); + } else { + // If one or both shared pointers are null, we can't assume they are equal + iterative_params_equal = false; + } + + return maxIterations == other.getMaxIterations() && + std::abs(relativeErrorTol - other.getRelativeErrorTol()) <= tol && + std::abs(absoluteErrorTol - other.getAbsoluteErrorTol()) <= tol && + std::abs(errorTol - other.getErrorTol()) <= tol && + verbosityTranslator(verbosity) == other.getVerbosity() && + orderingType == other.orderingType && ordering == other.ordering && + linearSolverType == other.linearSolverType && iterative_params_equal; +} + /* ************************************************************************* */ std::string NonlinearOptimizerParams::linearSolverTranslator( LinearSolverType linearSolverType) const { diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.h b/gtsam/nonlinear/NonlinearOptimizerParams.h index 92e22e064..0b4d8b2fd 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.h +++ b/gtsam/nonlinear/NonlinearOptimizerParams.h @@ -114,24 +114,7 @@ public: virtual void print(const std::string& str = "") const; - bool equals(const NonlinearOptimizerParams& other, double tol = 1e-9) const { - // Check for equality of shared ptrs - bool iterative_params_equal = false; - if (iterativeParams == other.iterativeParams) { - iterative_params_equal = true; - } - if (iterativeParams && other.iterativeParams) { - iterative_params_equal = iterativeParams->equals(*other.iterativeParams); - } - - return maxIterations == other.getMaxIterations() && - std::abs(relativeErrorTol - other.getRelativeErrorTol()) <= tol && - std::abs(absoluteErrorTol - other.getAbsoluteErrorTol()) <= tol && - std::abs(errorTol - other.getErrorTol()) <= tol && - verbosityTranslator(verbosity) == other.getVerbosity() && - orderingType == other.orderingType && ordering == other.ordering && - linearSolverType == other.linearSolverType && iterative_params_equal; - } + bool equals(const NonlinearOptimizerParams& other, double tol = 1e-9); inline bool isMultifrontal() const { return (linearSolverType == MULTIFRONTAL_CHOLESKY) From fa33bca0c7ffd240014cd878e2175662a22d4dd4 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 24 Aug 2024 06:35:17 -0400 Subject: [PATCH 05/14] make equals const-qualified --- gtsam/nonlinear/NonlinearOptimizerParams.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.h b/gtsam/nonlinear/NonlinearOptimizerParams.h index 0b4d8b2fd..cef7e85e3 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.h +++ b/gtsam/nonlinear/NonlinearOptimizerParams.h @@ -114,7 +114,7 @@ public: virtual void print(const std::string& str = "") const; - bool equals(const NonlinearOptimizerParams& other, double tol = 1e-9); + bool equals(const NonlinearOptimizerParams& other, double tol = 1e-9) const; inline bool isMultifrontal() const { return (linearSolverType == MULTIFRONTAL_CHOLESKY) From c59f458facb10c224383995ebaefeb0fa6883a4a Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 24 Aug 2024 15:32:18 -0400 Subject: [PATCH 06/14] improved check --- gtsam/nonlinear/NonlinearOptimizerParams.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.cpp b/gtsam/nonlinear/NonlinearOptimizerParams.cpp index 800db02a0..55dfd4561 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.cpp +++ b/gtsam/nonlinear/NonlinearOptimizerParams.cpp @@ -132,8 +132,8 @@ bool NonlinearOptimizerParams::equals(const NonlinearOptimizerParams& other, if (iterativeParams && other.iterativeParams) { iterative_params_equal = iterativeParams->equals(*other.iterativeParams); } else { - // If one or both shared pointers are null, we can't assume they are equal - iterative_params_equal = false; + // Check if either is null. If both are null, then true + iterative_params_equal = !iterativeParams && !other.iterativeParams; } return maxIterations == other.getMaxIterations() && From d20993f74015f0697e3a699659db001dcd3d8385 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sun, 25 Aug 2024 13:24:25 -0400 Subject: [PATCH 07/14] fix C++ version issue of CppUnitLite --- CppUnitLite/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/CppUnitLite/CMakeLists.txt b/CppUnitLite/CMakeLists.txt index cbffa79d1..e3b2dfadb 100644 --- a/CppUnitLite/CMakeLists.txt +++ b/CppUnitLite/CMakeLists.txt @@ -6,6 +6,7 @@ file(GLOB cppunitlite_src "*.cpp") add_library(CppUnitLite STATIC ${cppunitlite_src} ${cppunitlite_headers}) list(APPEND GTSAM_EXPORTED_TARGETS CppUnitLite) set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE) +target_compile_features(CppUnitLite PUBLIC ${GTSAM_COMPILE_FEATURES_PUBLIC}) gtsam_assign_source_folders("${cppunitlite_headers};${cppunitlite_src}") # MSVC project structure From 867f63d600f018324530bb9c48e731a55f445f62 Mon Sep 17 00:00:00 2001 From: Joel Truher Date: Thu, 29 Aug 2024 08:42:27 -0700 Subject: [PATCH 08/14] added SelfCalibrationExample.py --- .../gtsam/examples/SelfCalibrationExample.py | 122 ++++++++++++++++++ 1 file changed, 122 insertions(+) create mode 100644 python/gtsam/examples/SelfCalibrationExample.py diff --git a/python/gtsam/examples/SelfCalibrationExample.py b/python/gtsam/examples/SelfCalibrationExample.py new file mode 100644 index 000000000..a1919e14e --- /dev/null +++ b/python/gtsam/examples/SelfCalibrationExample.py @@ -0,0 +1,122 @@ +# pylint: disable=unused-import,consider-using-from-import,invalid-name,no-name-in-module,no-member,missing-function-docstring,too-many-locals +""" +Transcription of SelfCalibrationExample.cpp +""" +import math + +from gtsam import Cal3_S2 +from gtsam.noiseModel import Diagonal, Isotropic + +# SFM-specific factors +from gtsam import GeneralSFMFactor2Cal3_S2 # does calibration ! +from gtsam import PinholeCameraCal3_S2 + +# Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y). +from gtsam import Point2 +from gtsam import Point3, Pose3, Rot3 + +# Inference and optimization +from gtsam import NonlinearFactorGraph, DoglegOptimizer, Values +from gtsam.symbol_shorthand import K, L, X + + +# this is a direct translation of examples/SFMData.h +# which is slightly different from python/gtsam/examples/SFMdata.py. +def createPoints() -> list[Point3]: + """ + Create the set of ground-truth landmarks + """ + return [ + Point3(10.0, 10.0, 10.0), + Point3(-10.0, 10.0, 10.0), + Point3(-10.0, -10.0, 10.0), + Point3(10.0, -10.0, 10.0), + Point3(10.0, 10.0, -10.0), + Point3(-10.0, 10.0, -10.0), + Point3(-10.0, -10.0, -10.0), + Point3(10.0, -10.0, -10.0), + ] + + +def createPoses( + init: Pose3 = Pose3(Rot3.Ypr(math.pi / 2, 0, -math.pi / 2), Point3(30, 0, 0)), + delta: Pose3 = Pose3( + Rot3.Ypr(0, -math.pi / 4, 0), + Point3(math.sin(math.pi / 4) * 30, 0, 30 * (1 - math.sin(math.pi / 4))), + ), + steps: int = 8, +) -> list[Pose3]: + """ + Create the set of ground-truth poses + Default values give a circular trajectory, + radius 30 at pi/4 intervals, always facing the circle center + """ + poses: list[Pose3] = [] + poses.append(init) + for i in range(1, steps): + poses.append(poses[i - 1].compose(delta)) + return poses + + +def main() -> None: + # Create the set of ground-truth + points: list[Point3] = createPoints() + poses: list[Pose3] = createPoses() + + # Create the factor graph + graph = NonlinearFactorGraph() + + # Add a prior on pose x1. + # 30cm std on x,y,z 0.1 rad on roll,pitch,yaw + poseNoise = Diagonal.Sigmas([0.1, 0.1, 0.1, 0.3, 0.3, 0.3]) + graph.addPriorPose3(X(0), poses[0], poseNoise) + + # Simulated measurements from each camera pose, adding them to the factor graph + Kcal = Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0) + measurementNoise = Isotropic.Sigma(2, 1.0) + for i, pose in enumerate(poses): + for j, point in enumerate(points): + camera = PinholeCameraCal3_S2(pose, Kcal) + measurement: Point2 = camera.project(point) + # The only real difference with the Visual SLAM example is that here we + # use a different factor type, that also calculates the Jacobian with + # respect to calibration + graph.add( + GeneralSFMFactor2Cal3_S2( + measurement, + measurementNoise, + X(i), + L(j), + K(0), + ) + ) + + # Add a prior on the position of the first landmark. + pointNoise = Isotropic.Sigma(3, 0.1) + graph.addPriorPoint3(L(0), points[0], pointNoise) # add directly to graph + + # Add a prior on the calibration. + calNoise = Diagonal.Sigmas([500, 500, 0.1, 100, 100]) + graph.addPriorCal3_S2(K(0), Kcal, calNoise) + + # Create the initial estimate to the solution + # now including an estimate on the camera calibration parameters + initialEstimate = Values() + initialEstimate.insert(K(0), Cal3_S2(60.0, 60.0, 0.0, 45.0, 45.0)) + for i, pose in enumerate(poses): + initialEstimate.insert( + X(i), + pose.compose( + Pose3(Rot3.Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)) + ), + ) + for j, point in enumerate(points): + initialEstimate.insert(L(j), point + Point3(-0.25, 0.20, 0.15)) + + # Optimize the graph and print results + result: Values = DoglegOptimizer(graph, initialEstimate).optimize() + result.print("Final results:\n") + + +if __name__ == "__main__": + main() From ae423798673e200f47b0edd4d25fa6c5fbddfdff Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 29 Aug 2024 15:16:04 -0700 Subject: [PATCH 09/14] Move to cmake 3.5 --- CMakeLists.txt | 2 +- cmake/example_cmake_find_gtsam/CMakeLists.txt | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 66b1804af..916f9df32 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.0) +cmake_minimum_required(VERSION 3.5) # Set the version number for the library set (GTSAM_VERSION_MAJOR 4) diff --git a/cmake/example_cmake_find_gtsam/CMakeLists.txt b/cmake/example_cmake_find_gtsam/CMakeLists.txt index d020f7032..05216d3f3 100644 --- a/cmake/example_cmake_find_gtsam/CMakeLists.txt +++ b/cmake/example_cmake_find_gtsam/CMakeLists.txt @@ -1,7 +1,7 @@ # This file shows how to build and link a user project against GTSAM using CMake ################################################################################### # To create your own project, replace "example" with the actual name of your project -cmake_minimum_required(VERSION 3.0) +cmake_minimum_required(VERSION 3.5) project(example CXX) # Find GTSAM, either from a local build, or from a Debian/Ubuntu package. From 7b96d1e32525dbb123653058e1aab4e82270a782 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 29 Aug 2024 15:32:26 -0700 Subject: [PATCH 10/14] Set policies --- CMakeLists.txt | 4 ++++ cmake/HandleBoost.cmake | 2 +- 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 916f9df32..9bd72d2e6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,4 +1,8 @@ cmake_minimum_required(VERSION 3.5) +cmake_policy(SET CMP0082 NEW) # install from sub-directories immediately +cmake_policy(SET CMP0102 NEW) # set policy on advanced variables and cmake cache +cmake_policy(SET CMP0156 NEW) # new linker strategies +cmake_policy(SET CMP0167 OLD) # Don't complain about boost # Set the version number for the library set (GTSAM_VERSION_MAJOR 4) diff --git a/cmake/HandleBoost.cmake b/cmake/HandleBoost.cmake index 6c742cfe5..03251126e 100644 --- a/cmake/HandleBoost.cmake +++ b/cmake/HandleBoost.cmake @@ -25,7 +25,7 @@ endif() set(BOOST_FIND_MINIMUM_VERSION 1.65) set(BOOST_FIND_MINIMUM_COMPONENTS serialization system filesystem thread program_options date_time timer chrono regex) -find_package(Boost ${BOOST_FIND_MINIMUM_VERSION} COMPONENTS ${BOOST_FIND_MINIMUM_COMPONENTS}) +find_package(Boost ${BOOST_FIND_MINIMUM_VERSION} COMPONENTS ${BOOST_FIND_MINIMUM_COMPONENTS} REQUIRED) # Required components if(NOT Boost_SERIALIZATION_LIBRARY OR NOT Boost_SYSTEM_LIBRARY OR NOT Boost_FILESYSTEM_LIBRARY OR From e698073d6fb775089c7823455fb61fd58b6b09e6 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 30 Aug 2024 10:39:52 -0700 Subject: [PATCH 11/14] Check if policy exists --- CMakeLists.txt | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 9bd72d2e6..3306e3470 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,8 +1,16 @@ cmake_minimum_required(VERSION 3.5) -cmake_policy(SET CMP0082 NEW) # install from sub-directories immediately +if (POLICY CMP0082) + cmake_policy(SET CMP0082 NEW) # install from sub-directories immediately +endif() +if (POLICY CMP0102) cmake_policy(SET CMP0102 NEW) # set policy on advanced variables and cmake cache +endif() +if (POLICY CMP0156) cmake_policy(SET CMP0156 NEW) # new linker strategies +endif() +if (POLICY CMP0167) cmake_policy(SET CMP0167 OLD) # Don't complain about boost +endif() # Set the version number for the library set (GTSAM_VERSION_MAJOR 4) From cc0400371652f00dc066c4cc854e03d35b1959d3 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 4 Sep 2024 15:18:27 -0400 Subject: [PATCH 12/14] conditionalError method --- gtsam/hybrid/GaussianMixture.cpp | 53 ++++++++++++-------------------- gtsam/hybrid/GaussianMixture.h | 4 +++ 2 files changed, 23 insertions(+), 34 deletions(-) diff --git a/gtsam/hybrid/GaussianMixture.cpp b/gtsam/hybrid/GaussianMixture.cpp index fa4248921..325c32f95 100644 --- a/gtsam/hybrid/GaussianMixture.cpp +++ b/gtsam/hybrid/GaussianMixture.cpp @@ -330,19 +330,28 @@ AlgebraicDecisionTree GaussianMixture::logProbability( return DecisionTree(conditionals_, probFunc); } +/* ************************************************************************* */ +double GaussianMixture::conditionalError( + const GaussianConditional::shared_ptr &conditional, + const VectorValues &continuousValues) const { + // Check if valid pointer + if (conditional) { + return conditional->error(continuousValues) + // + logConstant_ - conditional->logNormalizationConstant(); + } else { + // If not valid, pointer, it means this conditional was pruned, + // so we return maximum error. + // This way the negative exponential will give + // a probability value close to 0.0. + return std::numeric_limits::max(); + } +} + /* *******************************************************************************/ AlgebraicDecisionTree GaussianMixture::errorTree( const VectorValues &continuousValues) const { auto errorFunc = [&](const GaussianConditional::shared_ptr &conditional) { - // Check if valid pointer - if (conditional) { - return conditional->error(continuousValues) + // - logConstant_ - conditional->logNormalizationConstant(); - } else { - // If not valid, pointer, it means this conditional was pruned, - // so we return maximum error. - return std::numeric_limits::max(); - } + return conditionalError(conditional, continuousValues); }; DecisionTree error_tree(conditionals_, errorFunc); return error_tree; @@ -350,33 +359,9 @@ AlgebraicDecisionTree GaussianMixture::errorTree( /* *******************************************************************************/ double GaussianMixture::error(const HybridValues &values) const { - // Check if discrete keys in discrete assignment are - // present in the GaussianMixture - KeyVector dKeys = this->discreteKeys_.indices(); - bool valid_assignment = false; - for (auto &&kv : values.discrete()) { - if (std::find(dKeys.begin(), dKeys.end(), kv.first) != dKeys.end()) { - valid_assignment = true; - break; - } - } - - // The discrete assignment is not valid so we throw an error. - if (!valid_assignment) { - throw std::runtime_error( - "Invalid discrete values in values. Not all discrete keys specified."); - } - // Directly index to get the conditional, no need to build the whole tree. auto conditional = conditionals_(values.discrete()); - if (conditional) { - return conditional->error(values.continuous()) + // - logConstant_ - conditional->logNormalizationConstant(); - } else { - // If not valid, pointer, it means this conditional was pruned, - // so we return maximum error. - return std::numeric_limits::max(); - } + return conditionalError(conditional, values.continuous()); } /* *******************************************************************************/ diff --git a/gtsam/hybrid/GaussianMixture.h b/gtsam/hybrid/GaussianMixture.h index 41692bb1c..714c00272 100644 --- a/gtsam/hybrid/GaussianMixture.h +++ b/gtsam/hybrid/GaussianMixture.h @@ -256,6 +256,10 @@ class GTSAM_EXPORT GaussianMixture /// Check whether `given` has values for all frontal keys. bool allFrontalsGiven(const VectorValues &given) const; + /// Helper method to compute the error of a conditional. + double conditionalError(const GaussianConditional::shared_ptr &conditional, + const VectorValues &continuousValues) const; + #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; From 8f65f058e04a5b33fe1650cd2ca3b82106d57357 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 4 Sep 2024 15:18:50 -0400 Subject: [PATCH 13/14] clean up code --- gtsam/hybrid/HybridFactorGraph.cpp | 9 --------- gtsam/hybrid/HybridFactorGraph.h | 5 +---- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 20 +++++-------------- .../tests/testGaussianMixtureFactor.cpp | 8 ++++---- 4 files changed, 10 insertions(+), 32 deletions(-) diff --git a/gtsam/hybrid/HybridFactorGraph.cpp b/gtsam/hybrid/HybridFactorGraph.cpp index f7b96f694..f5a7bcdfe 100644 --- a/gtsam/hybrid/HybridFactorGraph.cpp +++ b/gtsam/hybrid/HybridFactorGraph.cpp @@ -49,15 +49,6 @@ KeySet HybridFactorGraph::discreteKeySet() const { return keys; } -/* ************************************************************************* */ -std::unordered_map HybridFactorGraph::discreteKeyMap() const { - std::unordered_map result; - for (const DiscreteKey& k : discreteKeys()) { - result[k.first] = k; - } - return result; -} - /* ************************************************************************* */ const KeySet HybridFactorGraph::continuousKeySet() const { KeySet keys; diff --git a/gtsam/hybrid/HybridFactorGraph.h b/gtsam/hybrid/HybridFactorGraph.h index 1830d44ab..79f2a7af1 100644 --- a/gtsam/hybrid/HybridFactorGraph.h +++ b/gtsam/hybrid/HybridFactorGraph.h @@ -38,7 +38,7 @@ using SharedFactor = std::shared_ptr; class GTSAM_EXPORT HybridFactorGraph : public FactorGraph { public: using Base = FactorGraph; - using This = HybridFactorGraph; ///< this class + using This = HybridFactorGraph; ///< this class using shared_ptr = std::shared_ptr; ///< shared_ptr to This using Values = gtsam::Values; ///< backwards compatibility @@ -69,9 +69,6 @@ class GTSAM_EXPORT HybridFactorGraph : public FactorGraph { /// 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. - std::unordered_map discreteKeyMap() const; - /// Get all the continuous keys in the factor graph. const KeySet continuousKeySet() const; diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index c44fc4f1d..a7a6eee5a 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -312,8 +312,8 @@ using Result = std::pair, GaussianMixtureFactor::sharedFactor>; /** - * Compute the probability q(μ;m) = exp(-error(μ;m)) * sqrt(det(2π Σ_m) - * from the residual error at the mean μ. + * Compute the probability p(μ;m) = exp(-error(μ;m)) * sqrt(det(2π Σ_m) + * from the residual error ||b||^2 at the mean μ. * The residual error contains no keys, and only * depends on the discrete separator if present. */ @@ -523,19 +523,9 @@ EliminateHybrid(const HybridGaussianFactorGraph &factors, std::inserter(continuousSeparator, continuousSeparator.begin())); // Similarly for the discrete separator. - 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)); - } + // Since we eliminate all continuous variables first, + // the discrete separator will be *all* the discrete keys. + std::set discreteSeparator = factors.discreteKeys(); return hybridElimination(factors, frontalKeys, continuousSeparator, discreteSeparator); diff --git a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp index 139b50e2a..b2a4981f3 100644 --- a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp +++ b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp @@ -434,12 +434,12 @@ static HybridBayesNet CreateBayesNet(double mu0, double mu1, double sigma0, /* ************************************************************************* */ /** - * Test a model P(x0)P(z0|x0)P(x1|x0,m1)P(z1|x1)P(m1). + * Test a model 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)ϕ(x1,x0,m1)ϕ(x1)P(m1) + * ϕ(x0)ϕ(x1,x0,m1)ϕ(x1)P(m1) * * If we only have a measurement on z0, then * the probability of m1 should be 0.5/0.5. @@ -488,12 +488,12 @@ TEST(GaussianMixtureFactor, TwoStateModel) { /* ************************************************************************* */ /** - * Test a model P(x0)P(z0|x0)P(x1|x0,m1)P(z1|x1)P(m1). + * Test a model P(z0|x0)P(x1|x0,m1)P(z1|x1)P(m1). * * P(f01|x1,x0,m1) has different means and different covariances. * * Converting to a factor graph gives us - * P(x0)ϕ(x0)ϕ(x1,x0,m1)ϕ(x1)P(m1) + * ϕ(x0)ϕ(x1,x0,m1)ϕ(x1)P(m1) * * If we only have a measurement on z0, then * the P(m1) should be 0.5/0.5. From 8b04d9b2f510bc310b24c212149fd08c468c6fd8 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 4 Sep 2024 19:27:57 -0400 Subject: [PATCH 14/14] remove tests, will add later --- gtsam/hybrid/tests/testMixtureFactor.cpp | 150 ----------------------- 1 file changed, 150 deletions(-) diff --git a/gtsam/hybrid/tests/testMixtureFactor.cpp b/gtsam/hybrid/tests/testMixtureFactor.cpp index 48b193eeb..a58a4767f 100644 --- a/gtsam/hybrid/tests/testMixtureFactor.cpp +++ b/gtsam/hybrid/tests/testMixtureFactor.cpp @@ -118,156 +118,6 @@ TEST(MixtureFactor, Dim) { EXPECT_LONGS_EQUAL(1, mixtureFactor.dim()); } -/* ************************************************************************* */ -// Test components with differing means -TEST(MixtureFactor, DifferentMeans) { - DiscreteKey m1(M(1), 2), m2(M(2), 2); - - Values values; - double x1 = 0.0, x2 = 1.75, x3 = 2.60; - values.insert(X(1), x1); - values.insert(X(2), x2); - values.insert(X(3), x3); - - auto model0 = noiseModel::Isotropic::Sigma(1, 1e-0); - auto model1 = noiseModel::Isotropic::Sigma(1, 1e-0); - auto prior_noise = noiseModel::Isotropic::Sigma(1, 1e-0); - - auto f0 = std::make_shared>(X(1), X(2), 0.0, model0); - auto f1 = std::make_shared>(X(1), X(2), 2.0, model1); - std::vector factors{f0, f1}; - - MixtureFactor mixtureFactor({X(1), X(2)}, {m1}, factors); - HybridNonlinearFactorGraph hnfg; - hnfg.push_back(mixtureFactor); - - f0 = std::make_shared>(X(2), X(3), 0.0, model0); - f1 = std::make_shared>(X(2), X(3), 2.0, model1); - std::vector factors23{f0, f1}; - hnfg.push_back(MixtureFactor({X(2), X(3)}, {m2}, factors23)); - - auto prior = PriorFactor(X(1), x1, prior_noise); - hnfg.push_back(prior); - - hnfg.emplace_shared>(X(2), 2.0, prior_noise); - - auto hgfg = hnfg.linearize(values); - auto bn = hgfg->eliminateSequential(); - HybridValues actual = bn->optimize(); - - HybridValues expected( - VectorValues{ - {X(1), Vector1(0.0)}, {X(2), Vector1(0.25)}, {X(3), Vector1(-0.6)}}, - DiscreteValues{{M(1), 1}, {M(2), 0}}); - - EXPECT(assert_equal(expected, actual)); - - { - DiscreteValues dv{{M(1), 0}, {M(2), 0}}; - VectorValues cont = bn->optimize(dv); - double error = bn->error(HybridValues(cont, dv)); - // regression - EXPECT_DOUBLES_EQUAL(1.77418393408, error, 1e-9); - } - { - DiscreteValues dv{{M(1), 0}, {M(2), 1}}; - VectorValues cont = bn->optimize(dv); - double error = bn->error(HybridValues(cont, dv)); - // regression - EXPECT_DOUBLES_EQUAL(1.77418393408, error, 1e-9); - } - { - DiscreteValues dv{{M(1), 1}, {M(2), 0}}; - VectorValues cont = bn->optimize(dv); - double error = bn->error(HybridValues(cont, dv)); - // regression - EXPECT_DOUBLES_EQUAL(1.10751726741, error, 1e-9); - } - { - DiscreteValues dv{{M(1), 1}, {M(2), 1}}; - VectorValues cont = bn->optimize(dv); - double error = bn->error(HybridValues(cont, dv)); - // regression - EXPECT_DOUBLES_EQUAL(1.10751726741, error, 1e-9); - } -} - -/* ************************************************************************* */ -// Test components with differing covariances -TEST(MixtureFactor, 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); - - double between = 0.0; - - auto model0 = noiseModel::Isotropic::Sigma(1, 1e2); - auto model1 = noiseModel::Isotropic::Sigma(1, 1e-2); - auto prior_noise = noiseModel::Isotropic::Sigma(1, 1e-3); - - auto f0 = - std::make_shared>(X(1), X(2), between, model0); - auto f1 = - std::make_shared>(X(1), X(2), between, model1); - std::vector factors{f0, f1}; - - // Create via toFactorGraph - using symbol_shorthand::Z; - Matrix H0_1, H0_2, H1_1, H1_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*/}}; - 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); - - gtsam::VectorValues measurements; - measurements.insert(Z(1), gtsam::Z_1x1); - // Create FG with single GaussianMixtureFactor - 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); - - auto hbn = mixture_fg.eliminateSequential(); - - VectorValues cv; - cv.insert(X(1), Vector1(0.0)); - cv.insert(X(2), Vector1(0.0)); - // P(m1) = [0.5, 0.5], so we should pick 0 - DiscreteValues dv; - dv.insert({M(1), 0}); - HybridValues expected_values(cv, dv); - - HybridValues actual_values = hbn->optimize(); - EXPECT(assert_equal(expected_values, actual_values)); - - // Check that we get different error values at the MLE point μ. - AlgebraicDecisionTree errorTree = hbn->errorTree(cv); - - HybridValues hv0(cv, DiscreteValues{{M(1), 0}}); - HybridValues hv1(cv, DiscreteValues{{M(1), 1}}); - - AlgebraicDecisionTree expectedErrorTree(m1, 9.90348755254, - 0.69314718056); - EXPECT(assert_equal(expectedErrorTree, errorTree)); -} - /* ************************************************************************* */ int main() { TestResult tr;