Merge branch 'develop' into python-updates

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)

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.

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()
     

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 <gtsam/geometry/Pose2.h>
+#include <gtsam/nonlinear/GncOptimizer.h>
+#include <gtsam/nonlinear/GncParams.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/nonlinear/LevenbergMarquardtParams.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/slam/BetweenFactor.h>
+
+#include <iostream>
+
+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<BetweenFactor<Pose2>>(1, 2, x1, noiseModel::Isotropic::Sigma(3, 0.2));
+  Pose2 x2(0.0, 1.0, 0.1);
+  graph.emplace_shared<BetweenFactor<Pose2>>(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<LevenbergMarquardtParams> gncParams(lmParams);
+  gncParams.setLossType(GncLossType::TLS);
+
+  // Optimize the graph and print results
+  GncOptimizer<GncParams<LevenbergMarquardtParams>> optimizer(graph, initial, gncParams);
+  Values result = optimizer.optimize();
+  result.print("Final Result:");
+
+  return 0;
+}
+

gtsam/discrete/discrete.i

@@ -280,11 +280,11 @@ class DiscreteLookupDAG {
 };
 
 #include <gtsam/discrete/DiscreteFactorGraph.h>
-std::pair<gtsam::DiscreteConditional*, gtsam::DecisionTreeFactor*>
+pair<gtsam::DiscreteConditional*, gtsam::DecisionTreeFactor*>
 EliminateDiscrete(const gtsam::DiscreteFactorGraph& factors,
                   const gtsam::Ordering& frontalKeys);
 
-std::pair<gtsam::DiscreteConditional*, gtsam::DecisionTreeFactor*>
+pair<gtsam::DiscreteConditional*, gtsam::DecisionTreeFactor*>
 EliminateForMPE(const gtsam::DiscreteFactorGraph& factors,
                 const gtsam::Ordering& frontalKeys);
 

gtsam/geometry/tests/testSphericalCamera.cpp

@@ -24,6 +24,13 @@
 #include <cmath>
 #include <iostream>
 
+#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));
 }
 
 /* ************************************************************************* */

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<Matrix*>(nullptr)
+#define OptionalNone static_cast<gtsam::Matrix*>(nullptr)
 
 /** This typedef will be used everywhere boost::optional<Matrix&> reference was used
  * previously. This is used to indicate that the Jacobian is optional. In the future