From 651159f939ad85090902fd89264cc7a0b01b937d Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 7 Sep 2023 16:29:37 -0400 Subject: [PATCH 1/8] remove std:: from return type pair --- gtsam/discrete/discrete.i | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/discrete/discrete.i b/gtsam/discrete/discrete.i index fe8cbc7f3..a1731f8e5 100644 --- a/gtsam/discrete/discrete.i +++ b/gtsam/discrete/discrete.i @@ -280,11 +280,11 @@ class DiscreteLookupDAG { }; #include -std::pair +pair EliminateDiscrete(const gtsam::DiscreteFactorGraph& factors, const gtsam::Ordering& frontalKeys); -std::pair +pair EliminateForMPE(const gtsam::DiscreteFactorGraph& factors, const gtsam::Ordering& frontalKeys); From d2e4dff214d5c0fb7b8fd7b93ef2f3d936f280ac Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Tue, 5 Sep 2023 11:10:01 +0200 Subject: [PATCH 2/8] Relax unit test thresholds to fix 32bit issues Use a conditional threshold depending on architecture --- gtsam/geometry/tests/testSphericalCamera.cpp | 25 +++++++++++++------- 1 file changed, 16 insertions(+), 9 deletions(-) diff --git a/gtsam/geometry/tests/testSphericalCamera.cpp b/gtsam/geometry/tests/testSphericalCamera.cpp index 4bc851f35..cf8970dc4 100644 --- a/gtsam/geometry/tests/testSphericalCamera.cpp +++ b/gtsam/geometry/tests/testSphericalCamera.cpp @@ -24,6 +24,13 @@ #include #include +#if defined(__i686__) || defined(__i386__) +// See issue discussion: https://github.com/borglab/gtsam/issues/1605 +constexpr double TEST_THRESHOLD = 1e-5; +#else +constexpr double TEST_THRESHOLD = 1e-7; +#endif + using namespace std::placeholders; using namespace std; using namespace gtsam; @@ -104,8 +111,8 @@ TEST(SphericalCamera, Dproject) { Matrix numerical_pose = numericalDerivative21(project3, pose, point1); Matrix numerical_point = numericalDerivative22(project3, pose, point1); EXPECT(assert_equal(bearing1, result)); - EXPECT(assert_equal(numerical_pose, Dpose, 1e-7)); - EXPECT(assert_equal(numerical_point, Dpoint, 1e-7)); + EXPECT(assert_equal(numerical_pose, Dpose, TEST_THRESHOLD)); + EXPECT(assert_equal(numerical_point, Dpoint, TEST_THRESHOLD)); } /* ************************************************************************* */ @@ -123,8 +130,8 @@ TEST(SphericalCamera, reprojectionError) { Matrix numerical_point = numericalDerivative32(reprojectionError2, pose, point1, bearing1); EXPECT(assert_equal(Vector2(0.0, 0.0), result)); - EXPECT(assert_equal(numerical_pose, Dpose, 1e-7)); - EXPECT(assert_equal(numerical_point, Dpoint, 1e-7)); + EXPECT(assert_equal(numerical_pose, Dpose, TEST_THRESHOLD)); + EXPECT(assert_equal(numerical_point, Dpoint, TEST_THRESHOLD)); } /* ************************************************************************* */ @@ -137,9 +144,9 @@ TEST(SphericalCamera, reprojectionError_noisy) { numericalDerivative31(reprojectionError2, pose, point1, bearing_noisy); Matrix numerical_point = numericalDerivative32(reprojectionError2, pose, point1, bearing_noisy); - EXPECT(assert_equal(Vector2(-0.050282, 0.00833482), result, 1e-5)); - EXPECT(assert_equal(numerical_pose, Dpose, 1e-7)); - EXPECT(assert_equal(numerical_point, Dpoint, 1e-7)); + EXPECT(assert_equal(Vector2(-0.050282, 0.00833482), result, 1e2*TEST_THRESHOLD)); + EXPECT(assert_equal(numerical_pose, Dpose, TEST_THRESHOLD)); + EXPECT(assert_equal(numerical_point, Dpoint, TEST_THRESHOLD)); } /* ************************************************************************* */ @@ -151,8 +158,8 @@ TEST(SphericalCamera, Dproject2) { camera.project2(point1, Dpose, Dpoint); Matrix numerical_pose = numericalDerivative21(project3, pose1, point1); Matrix numerical_point = numericalDerivative22(project3, pose1, point1); - CHECK(assert_equal(numerical_pose, Dpose, 1e-7)); - CHECK(assert_equal(numerical_point, Dpoint, 1e-7)); + CHECK(assert_equal(numerical_pose, Dpose, TEST_THRESHOLD)); + CHECK(assert_equal(numerical_point, Dpoint, TEST_THRESHOLD)); } /* ************************************************************************* */ From 87e7a03108dc27dd34a2a0eb561208474b435d34 Mon Sep 17 00:00:00 2001 From: Silvio Traversaro Date: Sat, 9 Sep 2023 16:00:49 +0200 Subject: [PATCH 3/8] Remove setting CMAKE_MACOSX_RPATH to 0 by default --- CMakeLists.txt | 6 ------ 1 file changed, 6 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 309cc7c4e..63c7f9e54 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,11 +1,5 @@ cmake_minimum_required(VERSION 3.0) -# new feature to Cmake Version > 2.8.12 -# Mac ONLY. Define Relative Path on Mac OS -if(NOT DEFINED CMAKE_MACOSX_RPATH) - set(CMAKE_MACOSX_RPATH 0) -endif() - # Set the version number for the library set (GTSAM_VERSION_MAJOR 4) set (GTSAM_VERSION_MINOR 3) From d6e9889f45c6df3e9410db8e3eadfbe61e75c232 Mon Sep 17 00:00:00 2001 From: achintyamohan Date: Sun, 10 Sep 2023 20:07:12 -0400 Subject: [PATCH 4/8] Add cpp example for GNC --- examples/GNCExample.cpp | 67 +++++++++++++++++++++++++++++++++++++++++ 1 file changed, 67 insertions(+) create mode 100644 examples/GNCExample.cpp diff --git a/examples/GNCExample.cpp b/examples/GNCExample.cpp new file mode 100644 index 000000000..651e470c8 --- /dev/null +++ b/examples/GNCExample.cpp @@ -0,0 +1,67 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file GNCExample.cpp + * @brief Simple example showcasing a Graduated Non-Convexity based solver + * @author Achintya Mohan + */ + +#include +#include +#include +#include +#include +#include +#include + +#include + +using namespace std; +using namespace gtsam; + +int main() { + cout << "Graduated Non-Convexity Example\n"; + + NonlinearFactorGraph graph; + + // Add a prior to the first point, set to the origin + auto priorNoise = noiseModel::Isotropic::Sigma(2, 0.1); + graph.addPrior(1, Point2(0.0, 0.0), priorNoise); + + // Add additional factors, noise models must be Gaussian + Point2 x1(1.0, 0.0); + graph.emplace_shared>(1, 2, x1, noiseModel::Isotropic::Sigma(2, 0.2)); + Point2 x2(1.1, 0.1); + graph.emplace_shared>(2, 3, x2, noiseModel::Isotropic::Sigma(2, 0.4)); + + // Initial estimates + Values initial; + initial.insert(1, Point2(0.5, -0.1)); + initial.insert(2, Point2(1.3, 0.1)); + initial.insert(3, Point2(0.8, 0.2)); + + // Set options for the non-minimal solver + LevenbergMarquardtParams lmParams; + lmParams.setMaxIterations(1000); + lmParams.setRelativeErrorTol(1e-5); + + // Set GNC-specific options + GncParams gncParams(lmParams); + gncParams.setLossType(GncLossType::TLS); + + // Optimize the graph and print results + GncOptimizer> optimizer(graph, initial, gncParams); + Values result = optimizer.optimize(); + result.print("Final Result:"); + + return 0; +} \ No newline at end of file From 5cb98a6dcdf7cf4b0624f97e526cff99b4a6b85a Mon Sep 17 00:00:00 2001 From: achintyamohan Date: Mon, 11 Sep 2023 16:54:31 -0400 Subject: [PATCH 5/8] Modify GNC example, add comments to explain example purpose --- examples/GNCExample.cpp | 30 +++++++++++++++++++----------- 1 file changed, 19 insertions(+), 11 deletions(-) diff --git a/examples/GNCExample.cpp b/examples/GNCExample.cpp index 651e470c8..8a4f0706f 100644 --- a/examples/GNCExample.cpp +++ b/examples/GNCExample.cpp @@ -15,7 +15,14 @@ * @author Achintya Mohan */ -#include +/** + * A simple 2D pose graph optimization example + * - The robot is initially at origin (0.0, 0.0, 0.0) + * - We have full odometry measurements for 2 motions + * - The robot first moves to (1.0, 0.0, 0.1) and then to (1.0, 1.0, 0.2) + */ + +#include #include #include #include @@ -34,20 +41,20 @@ int main() { NonlinearFactorGraph graph; // Add a prior to the first point, set to the origin - auto priorNoise = noiseModel::Isotropic::Sigma(2, 0.1); - graph.addPrior(1, Point2(0.0, 0.0), priorNoise); + auto priorNoise = noiseModel::Isotropic::Sigma(3, 0.1); + graph.addPrior(1, Pose2(0.0, 0.0, 0.0), priorNoise); // Add additional factors, noise models must be Gaussian - Point2 x1(1.0, 0.0); - graph.emplace_shared>(1, 2, x1, noiseModel::Isotropic::Sigma(2, 0.2)); - Point2 x2(1.1, 0.1); - graph.emplace_shared>(2, 3, x2, noiseModel::Isotropic::Sigma(2, 0.4)); + Pose2 x1(1.0, 0.0, 0.1); + graph.emplace_shared>(1, 2, x1, noiseModel::Isotropic::Sigma(3, 0.2)); + Pose2 x2(0.0, 1.0, 0.1); + graph.emplace_shared>(2, 3, x2, noiseModel::Isotropic::Sigma(3, 0.4)); // Initial estimates Values initial; - initial.insert(1, Point2(0.5, -0.1)); - initial.insert(2, Point2(1.3, 0.1)); - initial.insert(3, Point2(0.8, 0.2)); + initial.insert(1, Pose2(0.2, 0.5, -0.1)); + initial.insert(2, Pose2(0.8, 0.3, 0.1)); + initial.insert(3, Pose2(0.8, 0.2, 0.3)); // Set options for the non-minimal solver LevenbergMarquardtParams lmParams; @@ -64,4 +71,5 @@ int main() { result.print("Final Result:"); return 0; -} \ No newline at end of file +} + From e859e9932b3c417f45e2c8fedfe72c34ddb069c4 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 15 Sep 2023 16:27:48 -0400 Subject: [PATCH 6/8] fix CMake variable in doc folder --- doc/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/CMakeLists.txt b/doc/CMakeLists.txt index f0975821f..05e0a13ef 100644 --- a/doc/CMakeLists.txt +++ b/doc/CMakeLists.txt @@ -56,7 +56,7 @@ if (GTSAM_BUILD_DOCS) if (GTSAM_BUILD_UNSTABLE) list(APPEND doc_subdirs ${gtsam_unstable_doc_subdirs}) endif() - if (GTSAM_BUILD_EXAMPLES) + if (GTSAM_BUILD_EXAMPLES_ALWAYS) list(APPEND doc_subdirs examples) endif() From fbdd602532842c48110cbfe3a612505d127737b3 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Thu, 28 Sep 2023 16:58:00 +0200 Subject: [PATCH 7/8] Fix usage of OptionalNone in a different namespace --- gtsam/nonlinear/NonlinearFactor.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 57c4a00eb..725117748 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -46,7 +46,7 @@ namespace gtsam { * Had to use the static_cast of a nullptr, because the compiler is not able to * deduce the type of the nullptr when expanding the evaluateError templates. */ -#define OptionalNone static_cast(nullptr) +#define OptionalNone static_cast(nullptr) /** This typedef will be used everywhere boost::optional reference was used * previously. This is used to indicate that the Jacobian is optional. In the future From 87158154ada5e094f8aa65773522723134348b0e Mon Sep 17 00:00:00 2001 From: Steve Oswald <82703776+stepeos@users.noreply.github.com> Date: Sun, 1 Oct 2023 22:32:26 +0200 Subject: [PATCH 8/8] Update INSTALL.md correct usage of march native compile option --- INSTALL.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/INSTALL.md b/INSTALL.md index f148e3718..10bee196c 100644 --- a/INSTALL.md +++ b/INSTALL.md @@ -182,7 +182,7 @@ Here are some tips to get the best possible performance out of GTSAM. optimization by 30-50%. Please note that this may not be true for very small problems where the overhead of dispatching work to multiple threads outweighs the benefit. We recommend that you benchmark your problem with/without TBB. -3. Add `-march=native` to `GTSAM_CMAKE_CXX_FLAGS`. A performance gain of +3. Use `GTSAM_BUILD_WITH_MARCH_NATIVE`. A performance gain of 25-30% can be expected on modern processors. Note that this affects the portability of your executable. It may not run when copied to another system with older/different processor architecture.