From 5cdbacfd4408db583c1726dd301b64fe60a659ca Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 7 Jan 2024 20:23:58 -0500 Subject: [PATCH 01/62] test and fix for issue 301 --- gtsam/nonlinear/ISAM2.cpp | 15 +++-- tests/testDoglegOptimizer.cpp | 122 ++++++++++++++++++++++++++++++++-- 2 files changed, 126 insertions(+), 11 deletions(-) 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/tests/testDoglegOptimizer.cpp b/tests/testDoglegOptimizer.cpp index e64e5ab7a..d7de63d3d 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,120 @@ TEST(DoglegOptimizer, Constraint) { #endif } +/* ************************************************************************* */ +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); } /* ************************************************************************* */ From 4c8e4973bf00d6d01379ab0b01bc481a653ea86a Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 8 Jan 2024 08:52:47 -0500 Subject: [PATCH 02/62] update Rot2::fromCosSin docstring --- gtsam/geometry/Rot2.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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); /** From 6dfd5671b1303785f28717388f76e3578fee45df Mon Sep 17 00:00:00 2001 From: Gary Date: Wed, 7 Aug 2024 15:46:13 -0400 Subject: [PATCH 03/62] Adding test and fix for issue #1596 A Non-active constraint returns a `nullptr`, which needs to be checked for when gathering the keys in `EliminateSymbolic`. --- gtsam/symbolic/SymbolicFactor-inst.h | 4 ++- tests/testGaussianISAM2.cpp | 50 ++++++++++++++++++++++++++++ 2 files changed, 53 insertions(+), 1 deletion(-) 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/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);} /* ************************************************************************* */ From c22b76506c24da51211b0eb473e92d9df2556233 Mon Sep 17 00:00:00 2001 From: AndreMichelin Date: Mon, 12 Aug 2024 19:03:40 -0700 Subject: [PATCH 04/62] Punctuation/scope --- .../tests/testPreintegratedRotation.cpp | 55 ++++++++++--------- 1 file changed, 28 insertions(+), 27 deletions(-) diff --git a/gtsam/navigation/tests/testPreintegratedRotation.cpp b/gtsam/navigation/tests/testPreintegratedRotation.cpp index 1e27ca1e4..bcd291eb2 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,8 +71,8 @@ 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) { @@ -89,39 +82,47 @@ TEST(PreintegratedRotation, integrateGyroMeasurement) { } //****************************************************************************** + +// 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) { From c2145bdffccfba6558dd42807e4d3c36075255b1 Mon Sep 17 00:00:00 2001 From: AndreMichelin Date: Mon, 12 Aug 2024 19:55:06 -0700 Subject: [PATCH 05/62] Simplify code as we know the Jacobian = R --- gtsam/navigation/PreintegratedRotation.cpp | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) 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; } From 37696b274e1ce9be3169ca5959cb5da50139a0d8 Mon Sep 17 00:00:00 2001 From: AndreMichelin Date: Mon, 12 Aug 2024 19:55:20 -0700 Subject: [PATCH 06/62] Adding more tests --- .../tests/testPreintegratedRotation.cpp | 84 ++++++++++++++++--- 1 file changed, 74 insertions(+), 10 deletions(-) diff --git a/gtsam/navigation/tests/testPreintegratedRotation.cpp b/gtsam/navigation/tests/testPreintegratedRotation.cpp index bcd291eb2..d84af031e 100644 --- a/gtsam/navigation/tests/testPreintegratedRotation.cpp +++ b/gtsam/navigation/tests/testPreintegratedRotation.cpp @@ -74,11 +74,18 @@ TEST(PreintegratedRotation, integrateGyroMeasurement) { 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)); + auto g = [&](const Vector3& increment) { + return pim.biascorrectedDeltaRij(increment, {}); + }; + const Matrix3 sane = numericalDerivative11(g, Z_3x1); + EXPECT(assert_equal(sane, pim.delRdelBiasOmega())); + // TODO(frank): the derivative H we compute in biascorrectedDeltaRij is not the *sane* one! + // EXPECT(assert_equal(sane, H)); + + // Let's integrate a second IMU measurement and check the Jacobian update: + pim.integrateGyroMeasurement(measuredOmega, bias, deltaT); + const Matrix3 sane2 = numericalDerivative11(g, Z_3x1); + EXPECT(assert_equal(sane2, pim.delRdelBiasOmega())); } //****************************************************************************** @@ -124,11 +131,68 @@ TEST(PreintegratedRotation, integrateGyroMeasurementWithTransform) { 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)); + auto g = [&](const Vector3& increment) { + return pim.biascorrectedDeltaRij(increment, {}); + }; + const Matrix3 sane = numericalDerivative11(g, Z_3x1); + EXPECT(assert_equal(sane, pim.delRdelBiasOmega())); + // TODO(frank): the derivative H we compute in biascorrectedDeltaRij is not the *sane* one! + // EXPECT(assert_equal(sane, H)); + + // Let's integrate a second IMU measurement and check the Jacobian update: + pim.integrateGyroMeasurement(measuredOmega, bias, deltaT); + const Matrix3 sane2 = numericalDerivative11(g, Z_3x1); + EXPECT(assert_equal(sane2, pim.delRdelBiasOmega())); +} + +// 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); + + auto g = [&](const Vector3& increment) { + return pim.biascorrectedDeltaRij(increment, {}); + }; + const Matrix3 sane = numericalDerivative11(g, Z_3x1); + EXPECT(assert_equal(sane, pim.delRdelBiasOmega())); + // TODO(frank): the derivative H we compute in biascorrectedDeltaRij is not the *sane* one! + // EXPECT(assert_equal(sane, H)); + + // Let's integrate a second IMU measurement and check the Jacobian update: + pim.integrateGyroMeasurement(measuredOmega, bias, deltaT); + const Matrix3 sane2 = numericalDerivative11(g, Z_3x1); + EXPECT(assert_equal(sane2, pim.delRdelBiasOmega())); + } //****************************************************************************** From 1c40b17fac65dc752f31340daf73a434590c11ad Mon Sep 17 00:00:00 2001 From: AndreMichelin Date: Tue, 13 Aug 2024 17:32:14 -0700 Subject: [PATCH 07/62] Some tests on Expmap/expmap chain rule --- gtsam/geometry/tests/testPose3.cpp | 25 +++++++++++++++++++ gtsam/geometry/tests/testRot3.cpp | 40 ++++++++++++++++++++++++++++++ 2 files changed, 65 insertions(+) diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 93cf99972..065d43bf9 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)); // 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..9555a2445 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -956,6 +956,46 @@ TEST(Rot3, determinant) { } } +/* ************************************************************************* */ +TEST(Rot3, ExpmapChainRule) { + // Muliply 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)); // 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)); +} + +/* ************************************************************************* */ +TEST(Rot3, expmapChainRule) { + // Muliply 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)); + + // 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)); +} + /* ************************************************************************* */ int main() { TestResult tr; From ac96b59469a1360ff4c8c06320b8447b94dd6b49 Mon Sep 17 00:00:00 2001 From: AndreMichelin Date: Wed, 14 Aug 2024 10:50:32 -0700 Subject: [PATCH 08/62] Fix jacobian tests --- .../tests/testPreintegratedRotation.cpp | 39 +++++++++---------- 1 file changed, 19 insertions(+), 20 deletions(-) diff --git a/gtsam/navigation/tests/testPreintegratedRotation.cpp b/gtsam/navigation/tests/testPreintegratedRotation.cpp index d84af031e..468701e3c 100644 --- a/gtsam/navigation/tests/testPreintegratedRotation.cpp +++ b/gtsam/navigation/tests/testPreintegratedRotation.cpp @@ -74,18 +74,18 @@ TEST(PreintegratedRotation, integrateGyroMeasurement) { EQUALITY(Vector3(-deltaT * delta, 0, 0), expected.logmap(corrected)) EXPECT(assert_equal(Rot3::Roll((omega - delta) * deltaT), corrected, 1e-9)) + // Check the derivative matches the numerical one auto g = [&](const Vector3& increment) { return pim.biascorrectedDeltaRij(increment, {}); }; - const Matrix3 sane = numericalDerivative11(g, Z_3x1); - EXPECT(assert_equal(sane, pim.delRdelBiasOmega())); - // TODO(frank): the derivative H we compute in biascorrectedDeltaRij is not the *sane* one! - // EXPECT(assert_equal(sane, H)); - + 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); - const Matrix3 sane2 = numericalDerivative11(g, Z_3x1); - EXPECT(assert_equal(sane2, pim.delRdelBiasOmega())); + expectedH = numericalDerivative11(g, biasOmegaIncr); + corrected = pim.biascorrectedDeltaRij(biasOmegaIncr, H); + EXPECT(assert_equal(expectedH, H)); } //****************************************************************************** @@ -131,18 +131,18 @@ TEST(PreintegratedRotation, integrateGyroMeasurementWithTransform) { EQUALITY(Vector3(0, -deltaT * delta, 0), expected.logmap(corrected)) EXPECT(assert_equal(Rot3::Pitch((omega - delta) * deltaT), corrected, 1e-9)) + // Check the derivative matches the *expectedH* one auto g = [&](const Vector3& increment) { return pim.biascorrectedDeltaRij(increment, {}); }; - const Matrix3 sane = numericalDerivative11(g, Z_3x1); - EXPECT(assert_equal(sane, pim.delRdelBiasOmega())); - // TODO(frank): the derivative H we compute in biascorrectedDeltaRij is not the *sane* one! - // EXPECT(assert_equal(sane, H)); + 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); - const Matrix3 sane2 = numericalDerivative11(g, Z_3x1); - EXPECT(assert_equal(sane2, pim.delRdelBiasOmega())); + 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. @@ -180,19 +180,18 @@ TEST(PreintegratedRotation, integrateGyroMeasurementWithArbitraryTransform) { 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, {}); }; - const Matrix3 sane = numericalDerivative11(g, Z_3x1); - EXPECT(assert_equal(sane, pim.delRdelBiasOmega())); - // TODO(frank): the derivative H we compute in biascorrectedDeltaRij is not the *sane* one! - // EXPECT(assert_equal(sane, H)); + 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); - const Matrix3 sane2 = numericalDerivative11(g, Z_3x1); - EXPECT(assert_equal(sane2, pim.delRdelBiasOmega())); - + corrected = pim.biascorrectedDeltaRij(biasOmegaIncr, H); + expectedH = numericalDerivative11(g, biasOmegaIncr); + EXPECT(assert_equal(expectedH, H)); } //****************************************************************************** From 6412517d3fe2df44a37bfae8c939aa05b2b9e24c Mon Sep 17 00:00:00 2001 From: AndreMichelin Date: Wed, 14 Aug 2024 17:53:18 -0700 Subject: [PATCH 09/62] Fix and comment another test --- .../tests/testManifoldPreintegration.cpp | 28 +++++++++++-------- 1 file changed, 16 insertions(+), 12 deletions(-) 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>())); } /* ************************************************************************* */ From a3afa5740a4f29cab512f50e6685c157e2d2d0d6 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 15 Aug 2024 17:13:19 -0700 Subject: [PATCH 10/62] Fix c++ warnings --- gtsam/3rdparty/Eigen/Eigen/src/SparseCore/TriangularSolver.h | 2 -- .../Eigen/Eigen/src/SparseLU/SparseLU_heap_relax_snode.h | 5 ----- gtsam/3rdparty/metis/GKlib/pdb.c | 2 +- 3 files changed, 1 insertion(+), 8 deletions(-) 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 = ' '; From 7765314638432c267eaf1631c61786c3b55f79d6 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 24 Aug 2024 05:23:52 -0400 Subject: [PATCH 11/62] fix return type of EliminateQR --- gtsam/linear/linear.i | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) 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 { From 1aa5883964da8bc036151c563f4f51dc51a26ef0 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 24 Aug 2024 17:18:55 -0700 Subject: [PATCH 12/62] Upgrade action --- .github/workflows/build-linux.yml | 2 +- .github/workflows/build-macos.yml | 2 +- .github/workflows/build-python.yml | 2 +- .github/workflows/build-special.yml | 2 +- .github/workflows/build-windows.yml | 2 +- 5 files changed, 5 insertions(+), 5 deletions(-) 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..d75042ae8 100644 --- a/.github/workflows/build-macos.yml +++ b/.github/workflows/build-macos.yml @@ -38,7 +38,7 @@ jobs: 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..036717624 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -65,7 +65,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-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 From bf70087fff9d5454a311039375b3c8512d2d47d7 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 25 Aug 2024 04:35:02 -0400 Subject: [PATCH 13/62] Make default PartialPriorFactor constructor public --- gtsam_unstable/slam/PartialPriorFactor.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) 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( From 2e39fef721f8e1dd9a824d6c0e3fed55080e7bd8 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 25 Aug 2024 04:35:16 -0400 Subject: [PATCH 14/62] wrap PartialPriorFactor --- gtsam_unstable/gtsam_unstable.i | 15 +++++++++++++++ 1 file changed, 15 insertions(+) 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 From 95da15a61abac9b6bf66e25e0193841964d86870 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 25 Aug 2024 04:35:33 -0400 Subject: [PATCH 15/62] allow stl binding for pybind11 in gtsam_unstable --- python/CMakeLists.txt | 2 ++ python/gtsam_unstable/gtsam_unstable.tpl | 1 + 2 files changed, 3 insertions(+) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index ba55ac2af..0c9dd6967 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -189,6 +189,8 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON) gtsam::BinaryMeasurementsPoint3 gtsam::BinaryMeasurementsUnit3 gtsam::BinaryMeasurementsRot3 + gtsam::SimWall2DVector + gtsam::SimPolygon2DVector gtsam::CameraSetCal3_S2 gtsam::CameraSetCal3Bundler gtsam::CameraSetCal3Unified 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 From b94ab31e1f058d3fac9596922a6bda01fb1423a5 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 25 Aug 2024 04:36:00 -0400 Subject: [PATCH 16/62] localization example script which gives exact same result as C++ version --- .../examples/LocalizationExample.py | 68 +++++++++++++++++++ 1 file changed, 68 insertions(+) create mode 100644 python/gtsam_unstable/examples/LocalizationExample.py 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() From c57d5a0f7052b9a3b4c34b95b1d795bebe312632 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 25 Aug 2024 07:23:31 -0400 Subject: [PATCH 17/62] Updated porting progress --- python/gtsam/examples/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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: | From 353ff644fcdf51ac3c9e94f99b9e2574dd1fe686 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 24 Jul 2022 12:40:11 -0400 Subject: [PATCH 18/62] generate python type hints during pip build --- python/setup.py.in | 1 + 1 file changed, 1 insertion(+) 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 ] } From 51aadbda3411411597d7f0d53bede212e898dc08 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 24 Jul 2022 22:25:56 -0400 Subject: [PATCH 19/62] add type hints and make command to test gtsam_unstable for python --- python/CMakeLists.txt | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index ba55ac2af..2068ee8dc 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -266,7 +266,9 @@ endif() # Add custom target so we can install with `make python-install` set(GTSAM_PYTHON_INSTALL_TARGET python-install) add_custom_target(${GTSAM_PYTHON_INSTALL_TARGET} - COMMAND ${PYTHON_EXECUTABLE} -m pip install --user . + COMMAND stubgen -q -p gtsam && + rsync -a "out/gtsam/" gtsam && + ${PYTHON_EXECUTABLE} -m pip install --user . DEPENDS ${GTSAM_PYTHON_DEPENDENCIES} WORKING_DIRECTORY ${GTSAM_PYTHON_BUILD_DIRECTORY}) @@ -279,3 +281,12 @@ add_custom_target( ${PYTHON_EXECUTABLE} -m unittest discover -v -s . DEPENDS ${GTSAM_PYTHON_DEPENDENCIES} ${GTSAM_PYTHON_TEST_FILES} WORKING_DIRECTORY "${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam/tests") + +add_custom_target( + python-test-unstable + COMMAND + ${CMAKE_COMMAND} -E env # add package to python path so no need to install + "PYTHONPATH=${GTSAM_PYTHON_BUILD_DIRECTORY}/$ENV{PYTHONPATH}" + ${PYTHON_EXECUTABLE} -m unittest discover -v -s . + DEPENDS ${GTSAM_PYTHON_DEPENDENCIES} ${GTSAM_PYTHON_TEST_FILES} + WORKING_DIRECTORY "${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam_unstable/tests") From 981ec728727c9e810b18d27c166763adaacdb7a5 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 25 Aug 2024 11:00:39 -0700 Subject: [PATCH 20/62] Remove unused variable --- gtsam/discrete/tests/testDiscreteConditional.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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"); From 48215b9063045921bba4268db36e3ac8085687e3 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 25 Aug 2024 14:03:43 -0400 Subject: [PATCH 21/62] remove duplicate target --- python/CMakeLists.txt | 9 --------- 1 file changed, 9 deletions(-) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 2068ee8dc..20f2eab4b 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -281,12 +281,3 @@ add_custom_target( ${PYTHON_EXECUTABLE} -m unittest discover -v -s . DEPENDS ${GTSAM_PYTHON_DEPENDENCIES} ${GTSAM_PYTHON_TEST_FILES} WORKING_DIRECTORY "${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam/tests") - -add_custom_target( - python-test-unstable - COMMAND - ${CMAKE_COMMAND} -E env # add package to python path so no need to install - "PYTHONPATH=${GTSAM_PYTHON_BUILD_DIRECTORY}/$ENV{PYTHONPATH}" - ${PYTHON_EXECUTABLE} -m unittest discover -v -s . - DEPENDS ${GTSAM_PYTHON_DEPENDENCIES} ${GTSAM_PYTHON_TEST_FILES} - WORKING_DIRECTORY "${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam_unstable/tests") From e60477f3c7a65f163ce09002e7e4599c34e118dc Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 25 Aug 2024 11:39:04 -0700 Subject: [PATCH 22/62] Add M1 runner --- .github/workflows/build-python.yml | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index 036717624..1012c7582 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,6 +60,11 @@ 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 From f2d69ed697b047e8fa5c89cc4613ffcee89c3c88 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 25 Aug 2024 14:47:14 -0400 Subject: [PATCH 23/62] always use bundled pybind11 inside wrap --- python/CMakeLists.txt | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index ba55ac2af..12d8a9b4d 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 +find_package(pybind11 CONFIG QUIET PATHS "${PROJECT_SOURCE_DIR}/wrap/pybind11") # Set the wrapping script variable set(PYBIND_WRAP_SCRIPT "${PROJECT_SOURCE_DIR}/wrap/scripts/pybind_wrap.py") From 1c8c1f2e799b3365475e18d4ede89806e04373d6 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 25 Aug 2024 11:55:06 -0700 Subject: [PATCH 24/62] Add venv on mac --- .github/workflows/build-python.yml | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index 1012c7582..c5e63de9d 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -110,6 +110,10 @@ jobs: echo "CC=clang" >> $GITHUB_ENV echo "CXX=clang++" >> $GITHUB_ENV + python$PYTHON_VERSION -m venv venv + source venv/bin/activate + python -m pip install --upgrade pip + - name: Setup msbuild (Windows) if: runner.os == 'Windows' uses: ilammy/msvc-dev-cmd@v1 From c3503064c863815a80daf7b320168c4a0fa02949 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 25 Aug 2024 12:07:47 -0700 Subject: [PATCH 25/62] Try activating venv --- .github/workflows/build-python.yml | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index c5e63de9d..54ea4485c 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -174,7 +174,11 @@ jobs: - name: Install Python Dependencies shell: bash - run: python$PYTHON_VERSION -m pip install -r python/dev_requirements.txt + run: | + if [ "${{ runner.os }}" == "macOS" ]; then + source venv/bin/activate + fi + python$PYTHON_VERSION -m pip install -r python/dev_requirements.txt - name: Build shell: bash From 387349839b6537ff0dd814ccf673cb747e219b36 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 25 Aug 2024 12:22:08 -0700 Subject: [PATCH 26/62] Fix venv for all subsequent actions --- .github/workflows/build-python.yml | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index 54ea4485c..9a9d44f5c 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -112,6 +112,7 @@ jobs: 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: Setup msbuild (Windows) @@ -174,11 +175,7 @@ jobs: - name: Install Python Dependencies shell: bash - run: | - if [ "${{ runner.os }}" == "macOS" ]; then - source venv/bin/activate - fi - python$PYTHON_VERSION -m pip install -r python/dev_requirements.txt + run: python$PYTHON_VERSION -m pip install -r python/dev_requirements.txt - name: Build shell: bash From e58a5c4cac810db86c3f1a960e61db81446126b4 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 25 Aug 2024 15:41:55 -0400 Subject: [PATCH 27/62] directly add pybind11 subdirectory so files are generated correctly --- python/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 12d8a9b4d..b8c2c718f 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -30,7 +30,7 @@ if(POLICY CMP0057) endif() # Use bundled pybind11 version -find_package(pybind11 CONFIG QUIET PATHS "${PROJECT_SOURCE_DIR}/wrap/pybind11") +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") From fad8e63fcea9fc074c843e365409e6461fbc29d0 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 25 Aug 2024 13:02:37 -0700 Subject: [PATCH 28/62] try non-venv way --- .github/workflows/build-python.yml | 7 +------ 1 file changed, 1 insertion(+), 6 deletions(-) diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index 9a9d44f5c..ed1b4cbdb 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -110,11 +110,6 @@ jobs: echo "CC=clang" >> $GITHUB_ENV echo "CXX=clang++" >> $GITHUB_ENV - 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: Setup msbuild (Windows) if: runner.os == 'Windows' uses: ilammy/msvc-dev-cmd@v1 @@ -175,7 +170,7 @@ jobs: - name: Install Python Dependencies shell: bash - run: python$PYTHON_VERSION -m pip install -r python/dev_requirements.txt + run: python$PYTHON_VERSION -m pip install --break-system-packages --user -r python/dev_requirements.txt - name: Build shell: bash From 9dbbb328142c9b9bfa6c99255843fcbb1b0686a5 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 25 Aug 2024 13:17:16 -0700 Subject: [PATCH 29/62] Add dangerous flag --- python/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index ba55ac2af..8fd092e8d 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -266,7 +266,7 @@ endif() # Add custom target so we can install with `make python-install` set(GTSAM_PYTHON_INSTALL_TARGET python-install) add_custom_target(${GTSAM_PYTHON_INSTALL_TARGET} - COMMAND ${PYTHON_EXECUTABLE} -m pip install --user . + COMMAND ${PYTHON_EXECUTABLE} -m pip install --break-system-packages --user . DEPENDS ${GTSAM_PYTHON_DEPENDENCIES} WORKING_DIRECTORY ${GTSAM_PYTHON_BUILD_DIRECTORY}) From b9e68ec79b0d0224464b33332efd5b818961018d Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 25 Aug 2024 13:17:28 -0700 Subject: [PATCH 30/62] Add M1 build --- .github/workflows/build-macos.yml | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/.github/workflows/build-macos.yml b/.github/workflows/build-macos.yml index d75042ae8..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,6 +37,11 @@ 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@v4 From bde9285211b1882865a59f870eb3a8f1bb6d2395 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 25 Aug 2024 16:42:23 -0400 Subject: [PATCH 31/62] better test documentation --- tests/testDoglegOptimizer.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/tests/testDoglegOptimizer.cpp b/tests/testDoglegOptimizer.cpp index d7de63d3d..6fbc522d3 100644 --- a/tests/testDoglegOptimizer.cpp +++ b/tests/testDoglegOptimizer.cpp @@ -184,6 +184,14 @@ TEST(DoglegOptimizer, Constraint) { } /* ************************************************************************* */ +/** + * 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; From 5f43b69c4371ca8d784378d11256f25412874a81 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 25 Aug 2024 16:45:23 -0400 Subject: [PATCH 32/62] add mypy to the dev_requirements so we can get stubgen --- python/dev_requirements.txt | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/python/dev_requirements.txt b/python/dev_requirements.txt index 6970ee613..c76d3bf1b 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.11.2 \ No newline at end of file From 9307536827bd44d7318cb42b2e873ce5dcca598d Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 25 Aug 2024 13:49:43 -0700 Subject: [PATCH 33/62] Add venv on Mac and do not use --user flag in that case --- .github/workflows/build-python.yml | 10 +++++++++- python/CMakeLists.txt | 2 +- 2 files changed, 10 insertions(+), 2 deletions(-) diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index ed1b4cbdb..cb465309e 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -168,9 +168,17 @@ 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 --break-system-packages --user -r python/dev_requirements.txt + run: python$PYTHON_VERSION -m pip install -r python/dev_requirements.txt - name: Build shell: bash diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 8fd092e8d..e13103839 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -266,7 +266,7 @@ endif() # Add custom target so we can install with `make python-install` set(GTSAM_PYTHON_INSTALL_TARGET python-install) add_custom_target(${GTSAM_PYTHON_INSTALL_TARGET} - COMMAND ${PYTHON_EXECUTABLE} -m pip install --break-system-packages --user . + COMMAND ${PYTHON_EXECUTABLE} -m pip install $(if [ -z "$VIRTUAL_ENV" ]; then echo "--user"; fi) . DEPENDS ${GTSAM_PYTHON_DEPENDENCIES} WORKING_DIRECTORY ${GTSAM_PYTHON_BUILD_DIRECTORY}) From d1d6942bde767ec7521bccfa7d410f1da3323925 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 25 Aug 2024 14:40:44 -0700 Subject: [PATCH 34/62] Correct cmake line --- python/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index e13103839..e908d4671 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -266,7 +266,7 @@ endif() # Add custom target so we can install with `make python-install` set(GTSAM_PYTHON_INSTALL_TARGET python-install) add_custom_target(${GTSAM_PYTHON_INSTALL_TARGET} - COMMAND ${PYTHON_EXECUTABLE} -m pip install $(if [ -z "$VIRTUAL_ENV" ]; then echo "--user"; fi) . + COMMAND ${PYTHON_EXECUTABLE} -m pip install `if [ -z "$VIRTUAL_ENV" ]; then echo --user; fi` . DEPENDS ${GTSAM_PYTHON_DEPENDENCIES} WORKING_DIRECTORY ${GTSAM_PYTHON_BUILD_DIRECTORY}) From 0ba23ccbaa0777c7f912b5c93ed25463e801eb1a Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 25 Aug 2024 15:31:25 -0700 Subject: [PATCH 35/62] Try w cross-platform install --- python/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index e908d4671..2151557b6 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -266,7 +266,7 @@ endif() # Add custom target so we can install with `make python-install` set(GTSAM_PYTHON_INSTALL_TARGET python-install) add_custom_target(${GTSAM_PYTHON_INSTALL_TARGET} - COMMAND ${PYTHON_EXECUTABLE} -m pip install `if [ -z "$VIRTUAL_ENV" ]; then echo --user; fi` . + COMMAND ${PYTHON_EXECUTABLE} -c "import sys, subprocess; cmd = [sys.executable, '-m', 'pip', 'install']; if not hasattr(sys, 'real_prefix') and not (hasattr(sys, 'base_prefix') and sys.base_prefix != sys.prefix): cmd.append('--user'); cmd.append('.'); subprocess.check_call(cmd)" DEPENDS ${GTSAM_PYTHON_DEPENDENCIES} WORKING_DIRECTORY ${GTSAM_PYTHON_BUILD_DIRECTORY}) From ce74b2b0c195c34dd8a1c6ef7705b0a2b1af3881 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 25 Aug 2024 15:47:57 -0700 Subject: [PATCH 36/62] Elaborate solution that works on windows as well --- python/CMakeLists.txt | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 2151557b6..7e270e4e8 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -264,11 +264,13 @@ 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} -c "import sys, subprocess; cmd = [sys.executable, '-m', 'pip', 'install']; if not hasattr(sys, 'real_prefix') and not (hasattr(sys, 'base_prefix') and sys.base_prefix != sys.prefix): cmd.append('--user'); cmd.append('.'); subprocess.check_call(cmd)" + 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}) + WORKING_DIRECTORY ${GTSAM_PYTHON_BUILD_DIRECTORY} + VERBATIM) # Custom make command to run all GTSAM Python tests add_custom_target( From 0e73367345f50f9c3a0a7f6e3beb7be124d99f23 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 25 Aug 2024 18:57:03 -0400 Subject: [PATCH 37/62] stubgen only for Windows until we can find the rsync equivalent --- python/CMakeLists.txt | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 20f2eab4b..b32d51c68 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -265,12 +265,18 @@ endif() # Add custom target so we can install with `make python-install` set(GTSAM_PYTHON_INSTALL_TARGET python-install) -add_custom_target(${GTSAM_PYTHON_INSTALL_TARGET} - COMMAND stubgen -q -p gtsam && - rsync -a "out/gtsam/" gtsam && - ${PYTHON_EXECUTABLE} -m pip install --user . +if (NOT WIN32) + add_custom_target(${GTSAM_PYTHON_INSTALL_TARGET} + COMMAND stubgen -q -p gtsam && rsync -a \"out/gtsam/\" gtsam && ${PYTHON_EXECUTABLE} -m pip install --user . DEPENDS ${GTSAM_PYTHON_DEPENDENCIES} WORKING_DIRECTORY ${GTSAM_PYTHON_BUILD_DIRECTORY}) +elseif() + #TODO(Varun) Find rsync equivalent on Windows + add_custom_target(${GTSAM_PYTHON_INSTALL_TARGET} + COMMAND ${PYTHON_EXECUTABLE} -m pip install --user . + DEPENDS ${GTSAM_PYTHON_DEPENDENCIES} + WORKING_DIRECTORY ${GTSAM_PYTHON_BUILD_DIRECTORY}) +endif() # Custom make command to run all GTSAM Python tests add_custom_target( From bfcd5eb08a9feea9f84b079878396afae63e5b54 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 26 Aug 2024 05:48:18 -0400 Subject: [PATCH 38/62] use older version of mypy --- python/dev_requirements.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/dev_requirements.txt b/python/dev_requirements.txt index c76d3bf1b..6a5e7f924 100644 --- a/python/dev_requirements.txt +++ b/python/dev_requirements.txt @@ -1,3 +1,3 @@ -r requirements.txt pyparsing>=2.4.2 -mypy>=1.11.2 \ No newline at end of file +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 From da99cf0e192ca4033d34575714b7591f343258ff Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 26 Aug 2024 06:25:17 -0400 Subject: [PATCH 39/62] remove extraneous variable --- python/CMakeLists.txt | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index b32d51c68..677ee72ce 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -264,15 +264,14 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON) endif() # Add custom target so we can install with `make python-install` -set(GTSAM_PYTHON_INSTALL_TARGET python-install) if (NOT WIN32) - add_custom_target(${GTSAM_PYTHON_INSTALL_TARGET} + add_custom_target(python-install COMMAND stubgen -q -p gtsam && rsync -a \"out/gtsam/\" gtsam && ${PYTHON_EXECUTABLE} -m pip install --user . DEPENDS ${GTSAM_PYTHON_DEPENDENCIES} WORKING_DIRECTORY ${GTSAM_PYTHON_BUILD_DIRECTORY}) elseif() #TODO(Varun) Find rsync equivalent on Windows - add_custom_target(${GTSAM_PYTHON_INSTALL_TARGET} + add_custom_target(python-install COMMAND ${PYTHON_EXECUTABLE} -m pip install --user . DEPENDS ${GTSAM_PYTHON_DEPENDENCIES} WORKING_DIRECTORY ${GTSAM_PYTHON_BUILD_DIRECTORY}) From 48f9d0b11664b5e69e4f8cd925851e3206ecb5af Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 26 Aug 2024 09:44:28 -0400 Subject: [PATCH 40/62] fix if-else mistype --- python/CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 677ee72ce..1e4acf142 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -264,12 +264,12 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON) endif() # Add custom target so we can install with `make python-install` -if (NOT WIN32) +if (NOT WIN32) # WIN32=1 is target platform is Windows add_custom_target(python-install COMMAND stubgen -q -p gtsam && rsync -a \"out/gtsam/\" gtsam && ${PYTHON_EXECUTABLE} -m pip install --user . DEPENDS ${GTSAM_PYTHON_DEPENDENCIES} WORKING_DIRECTORY ${GTSAM_PYTHON_BUILD_DIRECTORY}) -elseif() +else() #TODO(Varun) Find rsync equivalent on Windows add_custom_target(python-install COMMAND ${PYTHON_EXECUTABLE} -m pip install --user . From 1744c4aeb3b8875b8c17651905eebb4c7687d4ef Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 26 Aug 2024 10:07:24 -0400 Subject: [PATCH 41/62] save stubs to 'stubs' directory and use cp instead of rsync --- python/CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 1e4acf142..bdab178ba 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -266,11 +266,11 @@ endif() # Add custom target so we can install with `make python-install` if (NOT WIN32) # WIN32=1 is target platform is Windows add_custom_target(python-install - COMMAND stubgen -q -p gtsam && rsync -a \"out/gtsam/\" gtsam && ${PYTHON_EXECUTABLE} -m pip install --user . + COMMAND stubgen -q -p gtsam -o ./stubs && cp -a stubs/gtsam/ gtsam && ${PYTHON_EXECUTABLE} -m pip install --user . DEPENDS ${GTSAM_PYTHON_DEPENDENCIES} WORKING_DIRECTORY ${GTSAM_PYTHON_BUILD_DIRECTORY}) else() - #TODO(Varun) Find rsync equivalent on Windows + #TODO(Varun) Find equivalent cp on Windows add_custom_target(python-install COMMAND ${PYTHON_EXECUTABLE} -m pip install --user . DEPENDS ${GTSAM_PYTHON_DEPENDENCIES} From f4830baa5e9e319259f17d31ae002e15904744b0 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 26 Aug 2024 17:27:44 -0400 Subject: [PATCH 42/62] common logProbability normalization function --- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 43 ++++++++++++++++------ 1 file changed, 31 insertions(+), 12 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index cb8ceed20..bbf739c65 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -233,6 +233,26 @@ continuousElimination(const HybridGaussianFactorGraph &factors, return {std::make_shared(result.first), result.second}; } +/* ************************************************************************ */ +/** + * @brief Exponential log-probabilities after performing + * the necessary normalizations. + * + * @param logProbabilities DecisionTree of log-probabilities. + * @return AlgebraicDecisionTree + */ +static AlgebraicDecisionTree exponentiateLogProbabilities( + const AlgebraicDecisionTree &logProbabilities) { + // 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()); + + return probabilities; +} + /* ************************************************************************ */ static std::pair> discreteElimination(const HybridGaussianFactorGraph &factors, @@ -245,14 +265,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 = + exponentiateLogProbabilities(logProbabilities); + dfg.emplace_shared(gmf->discreteKeys(), + probabilities); } else if (auto orphan = dynamic_pointer_cast(f)) { // Ignore orphaned clique. @@ -315,13 +339,8 @@ static std::shared_ptr createDiscreteFactor( 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 = + exponentiateLogProbabilities(logProbabilities); return std::make_shared(discreteSeparator, probabilities); } From 475b37f7d6aacf86e9624dd8ae97475396f581f0 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 26 Aug 2024 15:08:04 -0700 Subject: [PATCH 43/62] Fix tolerance for ubuntu quaternion cases --- gtsam/geometry/tests/testPose3.cpp | 2 +- gtsam/geometry/tests/testRot3.cpp | 12 ++++++------ 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 065d43bf9..c9851f761 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -1223,7 +1223,7 @@ TEST(Pose3, ExpmapChainRule) { // Test the derivatives at zero const Matrix6 expected = numericalDerivative11(g, Z_6x1); - EXPECT(assert_equal(expected, M)); // Pose3::ExpmapDerivative(Z_6x1) is identity + 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}; diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 9555a2445..ce056edb6 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -958,7 +958,7 @@ TEST(Rot3, determinant) { /* ************************************************************************* */ TEST(Rot3, ExpmapChainRule) { - // Muliply with an arbitrary matrix and exponentiate + // Multiply with an arbitrary matrix and exponentiate Matrix3 M; M << 1, 2, 3, 4, 5, 6, 7, 8, 9; auto g = [&](const Vector3& omega) { @@ -967,17 +967,17 @@ TEST(Rot3, ExpmapChainRule) { // Test the derivatives at zero const Matrix3 expected = numericalDerivative11(g, Z_3x1); - EXPECT(assert_equal(expected, M)); // SO3::ExpmapDerivative(Z_3x1) is identity + 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)); + EXPECT(assert_equal(expected2, SO3::ExpmapDerivative(M*delta) * M, 1e-5)); } /* ************************************************************************* */ TEST(Rot3, expmapChainRule) { - // Muliply an arbitrary rotation with exp(M*x) + // 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; @@ -988,12 +988,12 @@ TEST(Rot3, expmapChainRule) { // Test the derivatives at zero const Matrix3 expected = numericalDerivative11(g, Z_3x1); - EXPECT(assert_equal(expected, M)); + 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)); + EXPECT(assert_equal(expected2, SO3::ExpmapDerivative(M*delta) * M, 1e-5)); } /* ************************************************************************* */ From 8d3bdc0578c1694a6a2fbe4024d1a63d9052fb5d Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 26 Aug 2024 19:47:09 -0400 Subject: [PATCH 44/62] python package generation with stubs working --- python/CMakeLists.txt | 26 ++++++++++++-------------- 1 file changed, 12 insertions(+), 14 deletions(-) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 9bbde7023..43f8d78bd 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -265,24 +265,22 @@ 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} +#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} + 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) - -# if (NOT WIN32) # WIN32=1 is target platform is Windows -# add_custom_target(${GTSAM_PYTHON_INSTALL_TARGET} -# COMMAND stubgen -q -p gtsam -o ./stubs && cp -a stubs/gtsam/ gtsam && ${PYTHON_EXECUTABLE} -m pip install --user . -# DEPENDS ${GTSAM_PYTHON_DEPENDENCIES} -# WORKING_DIRECTORY ${GTSAM_PYTHON_BUILD_DIRECTORY}) -# else() -# #TODO(Varun) Find equivalent cp on Windows -# add_custom_target(${GTSAM_PYTHON_INSTALL_TARGET} -# COMMAND ${PYTHON_EXECUTABLE} -m pip install --user . -# DEPENDS ${GTSAM_PYTHON_DEPENDENCIES} -# WORKING_DIRECTORY ${GTSAM_PYTHON_BUILD_DIRECTORY}) -# endif() +endif() # Custom make command to run all GTSAM Python tests From e81272b078ea36f0e6cca1f77d28fc3102afba2f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 27 Aug 2024 09:55:05 -0400 Subject: [PATCH 45/62] fix comments --- gtsam/hybrid/tests/testGaussianMixtureFactor.cpp | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp index 0910d2f40..4c293c2b9 100644 --- a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp +++ b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp @@ -251,8 +251,8 @@ TEST(GaussianMixtureFactor, GaussianMixtureModel) { * 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. + * which represents a Gaussian-like function + * where m1>m0 close to 3.1333. */ TEST(GaussianMixtureFactor, GaussianMixtureModel2) { double mu0 = 1.0, mu1 = 3.0; @@ -272,17 +272,16 @@ TEST(GaussianMixtureFactor, GaussianMixtureModel2) { hbn.emplace_back(gm); hbn.emplace_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 + // The result should be a bell curve like function + // with m1 > m0 close to 3.1333. VectorValues given; - given.insert(z, Vector1(mu1 - mu0)); + given.insert(z, Vector1(3.133)); HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given); HybridBayesNet::shared_ptr bn = gfg.eliminateSequential(); HybridBayesNet expected; expected.emplace_back( - new DiscreteConditional(m, "0.338561851224/0.661438148776")); + new DiscreteConditional(m, "0.325603277954/0.674396722046")); EXPECT(assert_equal(expected, *bn)); } From 3dab868ef0cf2e8c8093366d1afaf06ea19d8635 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 28 Aug 2024 12:47:24 -0400 Subject: [PATCH 46/62] rename from exponentiateLogProbabilities to probabilitiesFromLogValues --- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 19 +++++++++---------- 1 file changed, 9 insertions(+), 10 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index bbf739c65..2c5d239f5 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -235,19 +235,18 @@ continuousElimination(const HybridGaussianFactorGraph &factors, /* ************************************************************************ */ /** - * @brief Exponential log-probabilities after performing - * the necessary normalizations. + * @brief Exponentiate log-values, not necessarily normalized, normalize, and + * return as AlgebraicDecisionTree. * - * @param logProbabilities DecisionTree of log-probabilities. + * @param logValues DecisionTree of (unnormalized) log values. * @return AlgebraicDecisionTree */ -static AlgebraicDecisionTree exponentiateLogProbabilities( - const AlgebraicDecisionTree &logProbabilities) { +static AlgebraicDecisionTree probabilitiesFromLogValues( + const AlgebraicDecisionTree &logValues) { // Perform normalization - double max_log = logProbabilities.max(); + double max_log = logValues.max(); AlgebraicDecisionTree probabilities = DecisionTree( - logProbabilities, - [&max_log](const double x) { return exp(x - max_log); }); + logValues, [&max_log](const double x) { return exp(x - max_log); }); probabilities = probabilities.normalize(probabilities.sum()); return probabilities; @@ -274,7 +273,7 @@ discreteElimination(const HybridGaussianFactorGraph &factors, DecisionTree(gmf->factors(), logProbability); AlgebraicDecisionTree probabilities = - exponentiateLogProbabilities(logProbabilities); + probabilitiesFromLogValues(logProbabilities); dfg.emplace_shared(gmf->discreteKeys(), probabilities); @@ -340,7 +339,7 @@ static std::shared_ptr createDiscreteFactor( AlgebraicDecisionTree logProbabilities( DecisionTree(eliminationResults, logProbability)); AlgebraicDecisionTree probabilities = - exponentiateLogProbabilities(logProbabilities); + probabilitiesFromLogValues(logProbabilities); return std::make_shared(discreteSeparator, probabilities); } From 088f1f04bb6a0e27c1dfa9feacefd3bda05cc6ba Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 28 Aug 2024 13:18:55 -0400 Subject: [PATCH 47/62] improve the gaussian mixture model tests --- .../tests/testGaussianMixtureFactor.cpp | 113 +++++++++++++----- 1 file changed, 85 insertions(+), 28 deletions(-) diff --git a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp index 4c293c2b9..4d333f2c3 100644 --- a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp +++ b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp @@ -200,6 +200,42 @@ TEST(GaussianMixtureFactor, Error) { 4.0, mixtureFactor.error({continuousValues, discreteValues}), 1e-9); } +namespace test_gmm { + +/** + * Function to compute P(m=1|z). For P(m=0|z), swap `mus and sigmas. + * Follows equation 7.108 since it is more generic. + */ +double sigmoid(double mu0, double mu1, double sigma0, double sigma1, double z) { + double x1 = ((z - mu0) / sigma0), x2 = ((z - mu1) / sigma1); + double d = std::sqrt(sigma0 * sigma0) / std::sqrt(sigma1 * sigma1); + double e = d * std::exp(-0.5 * (x1 * x1 - x2 * x2)); + return 1 / (1 + e); +}; + +HybridBayesNet GetGaussianMixtureModel(double mu0, double mu1, double sigma0, + double sigma1) { + 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); + + auto gm = new GaussianMixture({z}, {}, {m}, {c0, c1}); + auto mixing = new DiscreteConditional(m, "0.5/0.5"); + + HybridBayesNet hbn; + hbn.emplace_back(gm); + hbn.emplace_back(mixing); + + return hbn; +} + +} // namespace test_gmm + /* ************************************************************************* */ /** * Test a simple Gaussian Mixture Model represented as P(m)P(z|m) @@ -211,6 +247,8 @@ TEST(GaussianMixtureFactor, Error) { * which represents a sigmoid function. */ TEST(GaussianMixtureFactor, GaussianMixtureModel) { + using namespace test_gmm; + double mu0 = 1.0, mu1 = 3.0; double sigma = 2.0; auto model = noiseModel::Isotropic::Sigma(1, sigma); @@ -218,28 +256,52 @@ TEST(GaussianMixtureFactor, GaussianMixtureModel) { 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); - - auto gm = new GaussianMixture({z}, {}, {m}, {c0, c1}); - auto mixing = new DiscreteConditional(m, "0.5/0.5"); - - HybridBayesNet hbn; - hbn.emplace_back(gm); - hbn.emplace_back(mixing); + auto hbn = GetGaussianMixtureModel(mu0, mu1, sigma, sigma); // 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)); + // 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(); + HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given); + HybridBayesNet::shared_ptr bn = gfg.eliminateSequential(); - HybridBayesNet expected; - expected.emplace_back(new DiscreteConditional(m, "0.5/0.5")); + EXPECT_DOUBLES_EQUAL( + sigmoid(mu0, mu1, sigma, sigma, midway), + bn->at(0)->asDiscrete()->operator()(DiscreteValues{{M(0), 1}}), 1e-8); - EXPECT(assert_equal(expected, *bn)); + // 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( + sigmoid(mu0, mu1, sigma, sigma, midway - lambda), + bn->at(0)->asDiscrete()->operator()(DiscreteValues{{M(0), 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( + sigmoid(mu0, mu1, sigma, sigma, midway + lambda), + bn->at(0)->asDiscrete()->operator()(DiscreteValues{{M(0), 1}}), 1e-8); + } } /* ************************************************************************* */ @@ -255,30 +317,25 @@ TEST(GaussianMixtureFactor, GaussianMixtureModel) { * where m1>m0 close to 3.1333. */ TEST(GaussianMixtureFactor, GaussianMixtureModel2) { + using namespace test_gmm; + double mu0 = 1.0, mu1 = 3.0; - auto model0 = noiseModel::Isotropic::Sigma(1, 8.0); - auto model1 = noiseModel::Isotropic::Sigma(1, 4.0); + double sigma0 = 8.0, sigma1 = 4.0; DiscreteKey m(M(0), 2); Key z = Z(0); - auto c0 = make_shared(z, Vector1(mu0), I_1x1, model0), - c1 = make_shared(z, Vector1(mu1), I_1x1, model1); - - auto gm = new GaussianMixture({z}, {}, {m}, {c0, c1}); - auto mixing = new DiscreteConditional(m, "0.5/0.5"); - - HybridBayesNet hbn; - hbn.emplace_back(gm); - hbn.emplace_back(mixing); + auto hbn = GetGaussianMixtureModel(mu0, mu1, sigma0, sigma1); // 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(); + // regression HybridBayesNet expected; expected.emplace_back( new DiscreteConditional(m, "0.325603277954/0.674396722046")); From bb77b0cabbbd9ceb433d80c62950d4cf509bc979 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 28 Aug 2024 17:55:49 -0400 Subject: [PATCH 48/62] improved two state model with different means --- .../tests/testGaussianMixtureFactor.cpp | 118 ++++++++++++------ 1 file changed, 83 insertions(+), 35 deletions(-) diff --git a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp index 4d333f2c3..8b7b5c146 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; @@ -270,7 +271,8 @@ TEST(GaussianMixtureFactor, GaussianMixtureModel) { EXPECT_DOUBLES_EQUAL( sigmoid(mu0, mu1, sigma, sigma, midway), - bn->at(0)->asDiscrete()->operator()(DiscreteValues{{M(0), 1}}), 1e-8); + 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; @@ -288,7 +290,8 @@ TEST(GaussianMixtureFactor, GaussianMixtureModel) { EXPECT_DOUBLES_EQUAL( sigmoid(mu0, mu1, sigma, sigma, midway - lambda), - bn->at(0)->asDiscrete()->operator()(DiscreteValues{{M(0), 1}}), 1e-8); + bn->at(0)->asDiscrete()->operator()(DiscreteValues{{m.first, 1}}), + 1e-8); } { // Shift by lambda @@ -300,7 +303,8 @@ TEST(GaussianMixtureFactor, GaussianMixtureModel) { EXPECT_DOUBLES_EQUAL( sigmoid(mu0, mu1, sigma, sigma, midway + lambda), - bn->at(0)->asDiscrete()->operator()(DiscreteValues{{M(0), 1}}), 1e-8); + bn->at(0)->asDiscrete()->operator()(DiscreteValues{{m.first, 1}}), + 1e-8); } } @@ -343,81 +347,124 @@ TEST(GaussianMixtureFactor, GaussianMixtureModel2) { EXPECT(assert_equal(expected, *bn)); } +namespace test_two_state_estimation { + +/// Create Two State Bayes Network with measurements +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), f01 = F(0); + Key x0 = X(0), x1 = X(1); + + HybridBayesNet hbn; + + auto prior_model = noiseModel::Isotropic::Sigma(1, prior_sigma); + auto measurement_model = noiseModel::Isotropic::Sigma(1, measurement_sigma); + + // 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) + 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(f01, Vector1(mu0), I_1x1, x1, + I_1x1, x0, -I_1x1, model0), + c1 = make_shared(f01, Vector1(mu1), I_1x1, x1, + I_1x1, x0, -I_1x1, model1); + + auto motion = new GaussianMixture({f01}, {x0, x1}, {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|m1)p(z1|x1)p(m1). + * Test a model P(x0)P(z0|x0)P(f01|x1,x0,m1)P(z1|x1)P(m1). * - * p(x1|m1) has different means and same covariance. + * 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), f01 = F(0); - 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)); + // The motion model says we didn't move + given.insert(f01, Vector1(0.0)); { - // 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"); - + // regression 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); + // If we see z1=2.2, discrete mode should say m1=1 given.insert(z1, Vector1(2.2)); 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"); - + // 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(f01|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. @@ -425,8 +472,9 @@ TEST(GaussianMixtureFactor, TwoStateModel) { */ TEST(GaussianMixtureFactor, TwoStateModel2) { 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); From 28f30a232d8b0fac02c4185bae2d1ece2600ddd2 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 28 Aug 2024 18:13:54 -0400 Subject: [PATCH 49/62] update up TwoStateModel test and remove DifferentMeans and DifferentCovariances for later --- .../tests/testGaussianMixtureFactor.cpp | 236 ++---------------- 1 file changed, 17 insertions(+), 219 deletions(-) diff --git a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp index 8b7b5c146..aa2fe0a1d 100644 --- a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp +++ b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp @@ -428,7 +428,7 @@ TEST(GaussianMixtureFactor, TwoStateModel) { VectorValues given; given.insert(z0, Vector1(0.5)); - // The motion model says we didn't move + // The motion model measurement says we didn't move given.insert(f01, Vector1(0.0)); { @@ -467,258 +467,56 @@ TEST(GaussianMixtureFactor, TwoStateModel) { * 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; 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), f01 = F(0); - 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)); + // The motion model measurement says we didn't move + given.insert(f01, Vector1(0.0)); { // 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 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); 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.4262682/0.5737318"); 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; From f8a7b804d3167196b7eed0f257a4e75545076ae8 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 29 Aug 2024 10:45:50 -0400 Subject: [PATCH 50/62] address some comments --- gtsam/hybrid/tests/testGaussianMixtureFactor.cpp | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp index aa2fe0a1d..3d26ae573 100644 --- a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp +++ b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp @@ -204,12 +204,12 @@ TEST(GaussianMixtureFactor, Error) { namespace test_gmm { /** - * Function to compute P(m=1|z). For P(m=0|z), swap `mus and sigmas. + * Function to compute P(m=1|z). For P(m=0|z), swap mus and sigmas. * Follows equation 7.108 since it is more generic. */ double sigmoid(double mu0, double mu1, double sigma0, double sigma1, double z) { double x1 = ((z - mu0) / sigma0), x2 = ((z - mu1) / sigma1); - double d = std::sqrt(sigma0 * sigma0) / std::sqrt(sigma1 * sigma1); + double d = sigma0 / sigma1; double e = d * std::exp(-0.5 * (x1 * x1 - x2 * x2)); return 1 / (1 + e); }; @@ -252,7 +252,6 @@ TEST(GaussianMixtureFactor, GaussianMixtureModel) { double mu0 = 1.0, mu1 = 3.0; double sigma = 2.0; - auto model = noiseModel::Isotropic::Sigma(1, sigma); DiscreteKey m(M(0), 2); Key z = Z(0); @@ -382,7 +381,6 @@ HybridBayesNet CreateBayesNet(double mu0, double mu1, double sigma0, I_1x1, x0, -I_1x1, model1); auto motion = new GaussianMixture({f01}, {x0, x1}, {m1}, {c0, c1}); - hbn.emplace_back(motion); if (add_second_measurement) { @@ -487,7 +485,7 @@ TEST(GaussianMixtureFactor, TwoStateModel2) { VectorValues given; given.insert(z0, Vector1(0.5)); // The motion model measurement says we didn't move - given.insert(f01, Vector1(0.0)); + // given.insert(x1, Vector1(0.0)); { // Start with no measurement on x1, only on x0 @@ -512,7 +510,7 @@ TEST(GaussianMixtureFactor, TwoStateModel2) { // Since we have a measurement on z2, we get a definite result DiscreteConditional expected(m1, "0.4262682/0.5737318"); - + // regression EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), 1e-6)); } } From 617a99f6bf1a61cc84e35c8d26770d7dacc65518 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 29 Aug 2024 13:26:16 -0400 Subject: [PATCH 51/62] format HybridConditional.h --- gtsam/hybrid/HybridConditional.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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. From b4637049267cab61110251e70ebfbc0fa4363db1 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 29 Aug 2024 13:37:28 -0400 Subject: [PATCH 52/62] improved HybridGaussianFactorGraph::printErrors --- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 27 ++++++++++------------ gtsam/hybrid/HybridGaussianFactorGraph.h | 8 +++++++ 2 files changed, 20 insertions(+), 15 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 2c5d239f5..7ac6cef98 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 { @@ -550,7 +547,7 @@ 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; 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: ", From 1673c47ea0b232e2c00adf9d60cc9b67c50ead3b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 29 Aug 2024 14:23:18 -0400 Subject: [PATCH 53/62] unpack HybridConditional in errorTree --- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 5 ++ .../tests/testHybridGaussianFactorGraph.cpp | 51 +++++++++++++++++++ 2 files changed, 56 insertions(+) diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 7ac6cef98..6c3442f97 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -551,6 +551,11 @@ AlgebraicDecisionTree HybridGaussianFactorGraph::errorTree( // 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/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. From 1236e058a11c168006287f614a57b171cf432165 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 29 Aug 2024 19:05:38 -0400 Subject: [PATCH 54/62] rename sigmoid to prob_m_z --- .../tests/testGaussianMixtureFactor.cpp | 32 ++++++++++--------- 1 file changed, 17 insertions(+), 15 deletions(-) diff --git a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp index 3d26ae573..187d9cf23 100644 --- a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp +++ b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp @@ -205,17 +205,20 @@ 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 sigmoid(double mu0, double mu1, double sigma0, double sigma1, double z) { +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 = sigma0 / sigma1; double e = d * std::exp(-0.5 * (x1 * x1 - x2 * x2)); return 1 / (1 + e); }; -HybridBayesNet GetGaussianMixtureModel(double mu0, double mu1, double sigma0, - double sigma1) { +static HybridBayesNet GetGaussianMixtureModel(double mu0, double mu1, + double sigma0, double sigma1) { DiscreteKey m(M(0), 2); Key z = Z(0); @@ -269,7 +272,7 @@ TEST(GaussianMixtureFactor, GaussianMixtureModel) { HybridBayesNet::shared_ptr bn = gfg.eliminateSequential(); EXPECT_DOUBLES_EQUAL( - sigmoid(mu0, mu1, sigma, sigma, midway), + prob_m_z(mu0, mu1, sigma, sigma, midway), bn->at(0)->asDiscrete()->operator()(DiscreteValues{{m.first, 1}}), 1e-8); @@ -288,7 +291,7 @@ TEST(GaussianMixtureFactor, GaussianMixtureModel) { HybridBayesNet::shared_ptr bn = gfg.eliminateSequential(); EXPECT_DOUBLES_EQUAL( - sigmoid(mu0, mu1, sigma, sigma, midway - lambda), + prob_m_z(mu0, mu1, sigma, sigma, midway - lambda), bn->at(0)->asDiscrete()->operator()(DiscreteValues{{m.first, 1}}), 1e-8); } @@ -301,7 +304,7 @@ TEST(GaussianMixtureFactor, GaussianMixtureModel) { HybridBayesNet::shared_ptr bn = gfg.eliminateSequential(); EXPECT_DOUBLES_EQUAL( - sigmoid(mu0, mu1, sigma, sigma, midway + lambda), + prob_m_z(mu0, mu1, sigma, sigma, midway + lambda), bn->at(0)->asDiscrete()->operator()(DiscreteValues{{m.first, 1}}), 1e-8); } @@ -349,11 +352,11 @@ TEST(GaussianMixtureFactor, GaussianMixtureModel2) { namespace test_two_state_estimation { /// Create Two State Bayes Network with measurements -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) { +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), f01 = F(0); Key x0 = X(0), x1 = X(1); @@ -401,7 +404,7 @@ HybridBayesNet CreateBayesNet(double mu0, double mu1, double sigma0, /* ************************************************************************* */ /** - * Test a model P(x0)P(z0|x0)P(f01|x1,x0,m1)P(z1|x1)P(m1). + * 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. * @@ -435,7 +438,6 @@ TEST(GaussianMixtureFactor, TwoStateModel) { // Since no measurement on x1, we hedge our bets DiscreteConditional expected(m1, "0.5/0.5"); - // regression EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()))); } @@ -457,7 +459,7 @@ TEST(GaussianMixtureFactor, TwoStateModel) { /* ************************************************************************* */ /** - * Test a model P(x0)P(z0|x0)P(f01|x1,x0,m1)P(z1|x1)P(m1). + * 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 different covariances. * @@ -520,4 +522,4 @@ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */ \ No newline at end of file +/* ************************************************************************* */ From 88605c851baeae92f507f0fbb228bc0fa063f784 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 29 Aug 2024 19:32:00 -0400 Subject: [PATCH 55/62] make GaussianMixture2 test as good as GaussianMixture test --- .../tests/testGaussianMixtureFactor.cpp | 65 +++++++++++++++---- 1 file changed, 51 insertions(+), 14 deletions(-) diff --git a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp index 187d9cf23..8d2fba821 100644 --- a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp +++ b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp @@ -333,20 +333,56 @@ TEST(GaussianMixtureFactor, GaussianMixtureModel2) { auto hbn = GetGaussianMixtureModel(mu0, mu1, sigma0, sigma1); - // 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(); + 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)); - // regression - HybridBayesNet expected; - expected.emplace_back( - new DiscreteConditional(m, "0.325603277954/0.674396722046")); + HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given); + HybridBayesNet::shared_ptr bn = gfg.eliminateSequential(); - EXPECT(assert_equal(expected, *bn)); + 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 { @@ -470,7 +506,8 @@ TEST(GaussianMixtureFactor, TwoStateModel) { * the P(m1) should be 0.5/0.5. * Getting a measurement on z1 gives use more information. */ -TEST(GaussianMixtureFactor, TwoStateModel2) { +// TODO(Varun) Figuring out the correctness of this +TEST_DISABLED(GaussianMixtureFactor, TwoStateModel2) { using namespace test_two_state_estimation; double mu0 = 1.0, mu1 = 3.0; @@ -479,7 +516,7 @@ TEST(GaussianMixtureFactor, TwoStateModel2) { auto model1 = noiseModel::Isotropic::Sigma(1, sigma1); DiscreteKey m1(M(1), 2); - Key z0 = Z(0), z1 = Z(1), f01 = F(0); + Key z0 = Z(0), z1 = Z(1); // Start with no measurement on x1, only on x0 HybridBayesNet hbn = CreateBayesNet(mu0, mu1, sigma0, sigma1, false); From ca073fd9ef32718161a53c903f8d258438009ed8 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 29 Aug 2024 19:32:19 -0400 Subject: [PATCH 56/62] fix bug in prob_m_z --- gtsam/hybrid/tests/testGaussianMixtureFactor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp index 8d2fba821..c473e567c 100644 --- a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp +++ b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp @@ -212,7 +212,7 @@ namespace test_gmm { 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 = sigma0 / sigma1; + double d = sigma1 / sigma0; double e = d * std::exp(-0.5 * (x1 * x1 - x2 * x2)); return 1 / (1 + e); }; From b18dc3c812f167dc97ecd118397678344b9d53f7 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 29 Aug 2024 19:38:00 -0400 Subject: [PATCH 57/62] fix motion model for TwoStateModel test --- .../hybrid/tests/testGaussianMixtureFactor.cpp | 18 +++++++----------- 1 file changed, 7 insertions(+), 11 deletions(-) diff --git a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp index c473e567c..c8ffdc63b 100644 --- a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp +++ b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp @@ -394,7 +394,7 @@ static HybridBayesNet CreateBayesNet(double mu0, double mu1, double sigma0, double prior_sigma = 1e-3, double measurement_sigma = 3.0) { DiscreteKey m1(M(1), 2); - Key z0 = Z(0), z1 = Z(1), f01 = F(0); + Key z0 = Z(0), z1 = Z(1); Key x0 = X(0), x1 = X(1); HybridBayesNet hbn; @@ -414,12 +414,12 @@ static HybridBayesNet CreateBayesNet(double mu0, double mu1, double sigma0, // Add hybrid motion model auto model0 = noiseModel::Isotropic::Sigma(1, sigma0); auto model1 = noiseModel::Isotropic::Sigma(1, sigma1); - auto c0 = make_shared(f01, Vector1(mu0), I_1x1, x1, - I_1x1, x0, -I_1x1, model0), - c1 = make_shared(f01, Vector1(mu1), I_1x1, x1, - I_1x1, x0, -I_1x1, model1); + 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({f01}, {x0, x1}, {m1}, {c0, c1}); + auto motion = new GaussianMixture({x1}, {x0}, {m1}, {c0, c1}); hbn.emplace_back(motion); if (add_second_measurement) { @@ -458,15 +458,13 @@ TEST(GaussianMixtureFactor, TwoStateModel) { double sigma = 2.0; DiscreteKey m1(M(1), 2); - Key z0 = Z(0), z1 = Z(1), f01 = F(0); + Key z0 = Z(0), z1 = Z(1); // 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)); - // The motion model measurement says we didn't move - given.insert(f01, Vector1(0.0)); { HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given); @@ -523,8 +521,6 @@ TEST_DISABLED(GaussianMixtureFactor, TwoStateModel2) { VectorValues given; given.insert(z0, Vector1(0.5)); - // The motion model measurement says we didn't move - // given.insert(x1, Vector1(0.0)); { // Start with no measurement on x1, only on x0 From d34dd659bc65165fe862b3012f126305134164b9 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 3 Sep 2024 15:10:52 -0400 Subject: [PATCH 58/62] remove overwhelming prior and adjust measurements accordingly --- .../hybrid/tests/testGaussianMixtureFactor.cpp | 18 ++++++------------ 1 file changed, 6 insertions(+), 12 deletions(-) diff --git a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp index c8ffdc63b..cfcf33e4d 100644 --- a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp +++ b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp @@ -399,13 +399,7 @@ static HybridBayesNet CreateBayesNet(double mu0, double mu1, double sigma0, HybridBayesNet hbn; - auto prior_model = noiseModel::Isotropic::Sigma(1, prior_sigma); auto measurement_model = noiseModel::Isotropic::Sigma(1, measurement_sigma); - - // 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) auto p_z0 = new GaussianConditional(z0, Vector1(0.0), -I_1x1, x0, I_1x1, measurement_model); @@ -479,13 +473,14 @@ TEST(GaussianMixtureFactor, TwoStateModel) { // Now we add a measurement z1 on x1 hbn = CreateBayesNet(mu0, mu1, sigma, sigma, true); - // If we see z1=2.2, discrete mode should say m1=1 - 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)); } @@ -504,8 +499,7 @@ TEST(GaussianMixtureFactor, TwoStateModel) { * the P(m1) should be 0.5/0.5. * Getting a measurement on z1 gives use more information. */ -// TODO(Varun) Figuring out the correctness of this -TEST_DISABLED(GaussianMixtureFactor, TwoStateModel2) { +TEST(GaussianMixtureFactor, TwoStateModel2) { using namespace test_two_state_estimation; double mu0 = 1.0, mu1 = 3.0; @@ -544,7 +538,7 @@ TEST_DISABLED(GaussianMixtureFactor, TwoStateModel2) { HybridBayesNet::shared_ptr bn = gfg.eliminateSequential(); // Since we have a measurement on z2, we get a definite result - DiscreteConditional expected(m1, "0.4262682/0.5737318"); + DiscreteConditional expected(m1, "0.44744586/0.55255414"); // regression EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), 1e-6)); } From 82fc9b9eeb94427a093a2a86a76923ff5e798de1 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 3 Sep 2024 15:17:18 -0400 Subject: [PATCH 59/62] account for extra error when sigmas are different --- gtsam/hybrid/GaussianMixture.cpp | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/gtsam/hybrid/GaussianMixture.cpp b/gtsam/hybrid/GaussianMixture.cpp index 0a0332af8..228e851bb 100644 --- a/gtsam/hybrid/GaussianMixture.cpp +++ b/gtsam/hybrid/GaussianMixture.cpp @@ -87,7 +87,18 @@ GaussianFactorGraphTree GaussianMixture::add( /* *******************************************************************************/ GaussianFactorGraphTree GaussianMixture::asGaussianFactorGraphTree() const { - auto wrap = [](const GaussianConditional::shared_ptr &gc) { + auto wrap = [this](const GaussianConditional::shared_ptr &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}; From 06ecf00dba3fa77124bce8eb0b47dc710d4c2328 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 3 Sep 2024 15:27:33 -0400 Subject: [PATCH 60/62] improve docs when computing discreteSeparator --- gtsam/hybrid/HybridFactorGraph.h | 2 +- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 7 ++++--- 2 files changed, 5 insertions(+), 4 deletions(-) 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 6c3442f97..c44fc4f1d 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -330,7 +330,7 @@ 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( @@ -523,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)); From 3f782a4ae7cf99bdc3e40205d665721651fa5ee4 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 3 Sep 2024 17:36:45 -0400 Subject: [PATCH 61/62] check for valid GaussianConditional --- gtsam/hybrid/GaussianMixture.cpp | 24 ++++++++++++++---------- 1 file changed, 14 insertions(+), 10 deletions(-) diff --git a/gtsam/hybrid/GaussianMixture.cpp b/gtsam/hybrid/GaussianMixture.cpp index 228e851bb..fa4248921 100644 --- a/gtsam/hybrid/GaussianMixture.cpp +++ b/gtsam/hybrid/GaussianMixture.cpp @@ -88,16 +88,20 @@ GaussianFactorGraphTree GaussianMixture::add( /* *******************************************************************************/ GaussianFactorGraphTree GaussianMixture::asGaussianFactorGraphTree() const { auto wrap = [this](const GaussianConditional::shared_ptr &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}; + // 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}; }; From 997d0b411b2f7a03124cd28a75e12c6729b9a7ac Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 3 Sep 2024 17:39:52 -0400 Subject: [PATCH 62/62] ratio tests for GaussianMixtureFactor --- .../tests/testGaussianMixtureFactor.cpp | 40 +++++++++++++++++++ 1 file changed, 40 insertions(+) diff --git a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp index cfcf33e4d..139b50e2a 100644 --- a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp +++ b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp @@ -519,6 +519,24 @@ 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 a 50/50 probability @@ -535,6 +553,28 @@ TEST(GaussianMixtureFactor, TwoStateModel2) { 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