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) 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. 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() diff --git a/examples/GNCExample.cpp b/examples/GNCExample.cpp new file mode 100644 index 000000000..8a4f0706f --- /dev/null +++ b/examples/GNCExample.cpp @@ -0,0 +1,75 @@ +/* ---------------------------------------------------------------------------- + + * 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 + */ + +/** + * 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 +#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(3, 0.1); + graph.addPrior(1, Pose2(0.0, 0.0, 0.0), priorNoise); + + // Add additional factors, noise models must be Gaussian + 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, 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; + 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; +} + 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); 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)); } /* ************************************************************************* */ 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