Merge remote-tracking branch 'upstream/develop' into sim3

borglab · Sep 26, 2020 · 3727cc6 · 3727cc6
2 parents 8236d69 + 79d6d5f
commit 3727cc6
Show file tree

Hide file tree

Showing 13 changed files with 107 additions and 61 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -14,6 +14,11 @@ set (GTSAM_VERSION_PATCH 0)
 math (EXPR GTSAM_VERSION_NUMERIC "10000 * ${GTSAM_VERSION_MAJOR} + 100 * ${GTSAM_VERSION_MINOR} + ${GTSAM_VERSION_PATCH}")
 set (GTSAM_VERSION_STRING "${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSION_PATCH}")
 
+set (CMAKE_PROJECT_VERSION ${GTSAM_VERSION_STRING})
+set (CMAKE_PROJECT_VERSION_MAJOR ${GTSAM_VERSION_MAJOR})
+set (CMAKE_PROJECT_VERSION_MINOR ${GTSAM_VERSION_MINOR})
+set (CMAKE_PROJECT_VERSION_PATCH ${GTSAM_VERSION_PATCH})
+
 ###############################################################################
 # Gather information, perform checks, set defaults
 
@@ -113,6 +118,22 @@ if(GTSAM_INSTALL_MATLAB_TOOLBOX AND NOT BUILD_SHARED_LIBS)
 endif()
 
 if(GTSAM_BUILD_PYTHON)
+    # Get info about the Python3 interpreter
+    # https://cmake.org/cmake/help/latest/module/FindPython3.html#module:FindPython3
+    find_package(Python3 COMPONENTS Interpreter Development)
+
+    if(NOT ${Python3_FOUND})
+        message(FATAL_ERROR "Cannot find Python3 interpreter. Please install Python >= 3.6.")
+    endif()
+
+    if(${GTSAM_PYTHON_VERSION} STREQUAL "Default")
+        set(GTSAM_PYTHON_VERSION "${Python3_VERSION_MAJOR}.${Python3_VERSION_MINOR}"
+                CACHE
+                STRING
+                "The version of Python to build the wrappers against."
+                FORCE)
+    endif()
+
     if(GTSAM_UNSTABLE_BUILD_PYTHON)
         if (NOT GTSAM_BUILD_UNSTABLE)
             message(WARNING "GTSAM_UNSTABLE_BUILD_PYTHON requires the unstable module to be enabled.")
@@ -529,9 +550,9 @@ print_build_options_for_target(gtsam)
 print_config("Use System Eigen" "${GTSAM_USE_SYSTEM_EIGEN} (Using version: ${GTSAM_EIGEN_VERSION})")
 
 if(GTSAM_USE_TBB)
-    print_config("Use Intel TBB" "Yes")
+    print_config("Use Intel TBB" "Yes (Version: ${TBB_VERSION})")
 elseif(TBB_FOUND)
-    print_config("Use Intel TBB" "TBB found but GTSAM_WITH_TBB is disabled")
+    print_config("Use Intel TBB" "TBB (Version: ${TBB_VERSION}) found but GTSAM_WITH_TBB is disabled")
 else()
     print_config("Use Intel TBB" "TBB not found")
 endif()

diff --git a/README.md b/README.md
@@ -62,7 +62,7 @@ GTSAM 4.1 added a new pybind wrapper, and **removed** the deprecated functionali
 
 ## Wrappers
 
-We provide support for [MATLAB](matlab/README.md) and [Python](cython/README.md) wrappers for GTSAM. Please refer to the linked documents for more details.
+We provide support for [MATLAB](matlab/README.md) and [Python](python/README.md) wrappers for GTSAM. Please refer to the linked documents for more details.
 
 ## The Preintegrated IMU Factor
 

diff --git a/cmake/FindNumPy.cmake b/cmake/FindNumPy.cmake
@@ -40,17 +40,9 @@
 
 # Finding NumPy involves calling the Python interpreter
 if(NumPy_FIND_REQUIRED)
-  if(GTSAM_PYTHON_VERSION STREQUAL "Default")
-    find_package(PythonInterp REQUIRED)
-  else()
-      find_package(PythonInterp ${GTSAM_PYTHON_VERSION} EXACT REQUIRED)
-  endif()
+  find_package(PythonInterp ${GTSAM_PYTHON_VERSION} EXACT REQUIRED)
 else()
-  if(GTSAM_PYTHON_VERSION STREQUAL "Default")
-    find_package(PythonInterp)
-  else()
-    find_package(PythonInterp ${GTSAM_PYTHON_VERSION} EXACT)
-  endif()
+  find_package(PythonInterp ${GTSAM_PYTHON_VERSION} EXACT)
 endif()
 
 if(NOT PYTHONINTERP_FOUND)

diff --git a/cmake/GtsamMatlabWrap.cmake b/cmake/GtsamMatlabWrap.cmake
@@ -215,19 +215,15 @@ function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs ex
 	# Set up generation of module source file
 	file(MAKE_DIRECTORY "${generated_files_path}")
 
-	if(GTSAM_PYTHON_VERSION STREQUAL "Default")
-		find_package(PythonInterp REQUIRED)
-		find_package(PythonLibs REQUIRED)
-	else()
-		find_package(PythonInterp
-				${GTSAM_PYTHON_VERSION}
-				EXACT
-				REQUIRED)
-		find_package(PythonLibs
-				${GTSAM_PYTHON_VERSION}
-				EXACT
-				REQUIRED)
-	endif()
+    find_package(PythonInterp
+            ${GTSAM_PYTHON_VERSION}
+            EXACT
+            REQUIRED)
+    find_package(PythonLibs
+            ${GTSAM_PYTHON_VERSION}
+            EXACT
+            REQUIRED)
+
 
 	set(_ignore gtsam::Point2
 			gtsam::Point3)

diff --git a/cmake/GtsamPrinting.cmake b/cmake/GtsamPrinting.cmake
@@ -46,16 +46,16 @@ endfunction()
 # Prints all the relevant CMake build options for a given target:
 function(print_build_options_for_target target_name_)
   print_padded(GTSAM_COMPILE_FEATURES_PUBLIC)
-  print_padded(GTSAM_COMPILE_OPTIONS_PRIVATE)
+  # print_padded(GTSAM_COMPILE_OPTIONS_PRIVATE)
   print_padded(GTSAM_COMPILE_OPTIONS_PUBLIC)
-  print_padded(GTSAM_COMPILE_DEFINITIONS_PRIVATE)
+  # print_padded(GTSAM_COMPILE_DEFINITIONS_PRIVATE)
   print_padded(GTSAM_COMPILE_DEFINITIONS_PUBLIC)
 
   foreach(build_type ${GTSAM_CMAKE_CONFIGURATION_TYPES})
     string(TOUPPER "${build_type}" build_type_toupper)
-    print_padded(GTSAM_COMPILE_OPTIONS_PRIVATE_${build_type_toupper})
+    # print_padded(GTSAM_COMPILE_OPTIONS_PRIVATE_${build_type_toupper})
     print_padded(GTSAM_COMPILE_OPTIONS_PUBLIC_${build_type_toupper})
-    print_padded(GTSAM_COMPILE_DEFINITIONS_PRIVATE_${build_type_toupper})
+    # print_padded(GTSAM_COMPILE_DEFINITIONS_PRIVATE_${build_type_toupper})
     print_padded(GTSAM_COMPILE_DEFINITIONS_PUBLIC_${build_type_toupper})
   endforeach()
 endfunction()
diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp
@@ -190,15 +190,7 @@ Vector6 Pose3::ChartAtOrigin::Local(const Pose3& pose, ChartJacobian Hpose) {
 }
 
 /* ************************************************************************* */
-/**
- * Compute the 3x3 bottom-left block Q of the SE3 Expmap derivative matrix
- *  J(xi) = [J_(w) Z_3x3;
- *             Q   J_(w)]
- *  where J_(w) is the SO3 Expmap derivative.
- *  (see Chirikjian11book2, pg 44, eq 10.95.
- *  The closed-form formula is similar to formula 102 in Barfoot14tro)
- */
-static Matrix3 computeQforExpmapDerivative(const Vector6& xi) {
+Matrix3 Pose3::ComputeQforExpmapDerivative(const Vector6& xi, double nearZeroThreshold) {
   const auto w = xi.head<3>();
   const auto v = xi.tail<3>();
   const Matrix3 V = skewSymmetric(v);
@@ -220,7 +212,7 @@ static Matrix3 computeQforExpmapDerivative(const Vector6& xi) {
 #else
   // The closed-form formula in Barfoot14tro eq. (102)
   double phi = w.norm();
-  if (std::abs(phi)>1e-5) {
+  if (std::abs(phi)>nearZeroThreshold) {
     const double sinPhi = sin(phi), cosPhi = cos(phi);
     const double phi2 = phi * phi, phi3 = phi2 * phi, phi4 = phi3 * phi, phi5 = phi4 * phi;
     // Invert the sign of odd-order terms to have the right Jacobian
@@ -230,8 +222,8 @@ static Matrix3 computeQforExpmapDerivative(const Vector6& xi) {
   }
   else {
     Q = -0.5*V + 1./6.*(W*V + V*W - W*V*W)
-        + 1./24.*(W*W*V + V*W*W - 3*W*V*W)
-        - 0.5*(1./24. + 3./120.)*(W*V*W*W + W*W*V*W);
+        - 1./24.*(W*W*V + V*W*W - 3*W*V*W)
+        + 1./120.*(W*V*W*W + W*W*V*W);
   }
 #endif
 
@@ -242,7 +234,7 @@ static Matrix3 computeQforExpmapDerivative(const Vector6& xi) {
 Matrix6 Pose3::ExpmapDerivative(const Vector6& xi) {
   const Vector3 w = xi.head<3>();
   const Matrix3 Jw = Rot3::ExpmapDerivative(w);
-  const Matrix3 Q = computeQforExpmapDerivative(xi);
+  const Matrix3 Q = ComputeQforExpmapDerivative(xi);
   Matrix6 J;
   J << Jw, Z_3x3, Q, Jw;
   return J;
@@ -253,7 +245,7 @@ Matrix6 Pose3::LogmapDerivative(const Pose3& pose) {
   const Vector6 xi = Logmap(pose);
   const Vector3 w = xi.head<3>();
   const Matrix3 Jw = Rot3::LogmapDerivative(w);
-  const Matrix3 Q = computeQforExpmapDerivative(xi);
+  const Matrix3 Q = ComputeQforExpmapDerivative(xi);
   const Matrix3 Q2 = -Jw*Q*Jw;
   Matrix6 J;
   J << Jw, Z_3x3, Q2, Jw;

diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h
@@ -181,6 +181,18 @@ class GTSAM_EXPORT Pose3: public LieGroup<Pose3, 6> {
     static Vector6 Local(const Pose3& pose, ChartJacobian Hpose = boost::none);
   };
 
+  /**
+  * Compute the 3x3 bottom-left block Q of SE3 Expmap right derivative matrix
+  *  J_r(xi) = [J_(w) Z_3x3;
+  *             Q_r   J_(w)]
+  *  where J_(w) is the SO3 Expmap right derivative.
+  *  (see Chirikjian11book2, pg 44, eq 10.95.
+  *  The closed-form formula is identical to formula 102 in Barfoot14tro where
+  *  Q_l of the SE3 Expmap left derivative matrix is given.
+  */
+  static Matrix3 ComputeQforExpmapDerivative(
+      const Vector6& xi, double nearZeroThreshold = 1e-5);
+
   using LieGroup<Pose3, 6>::inverse; // version with derivative
 
   /**

diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp
@@ -807,6 +807,17 @@ TEST(Pose3, ExpmapDerivative2) {
   }
 }
 
+TEST( Pose3, ExpmapDerivativeQr) {
+  Vector6 w = Vector6::Random();
+  w.head<3>().normalize();
+  w.head<3>() = w.head<3>() * 0.9e-2;
+  Matrix3 actualQr = Pose3::ComputeQforExpmapDerivative(w, 0.01);
+  Matrix expectedH = numericalDerivative21<Pose3, Vector6,
+      OptionalJacobian<6, 6> >(&Pose3::Expmap, w, boost::none);
+  Matrix3 expectedQr = expectedH.bottomLeftCorner<3, 3>();
+  EXPECT(assert_equal(expectedQr, actualQr, 1e-6));
+}
+
 /* ************************************************************************* */
 TEST( Pose3, LogmapDerivative) {
   Matrix6 actualH;

diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i
@@ -567,6 +567,7 @@ class Rot2 {
   // Lie Group
   static gtsam::Rot2 Expmap(Vector v);
   static Vector Logmap(const gtsam::Rot2& p);
+  Vector logmap(const gtsam::Rot2& p);
 
   // Group Action on Point2
   gtsam::Point2 rotate(const gtsam::Point2& point) const;
@@ -727,6 +728,7 @@ class Rot3 {
   // Standard Interface
   static gtsam::Rot3 Expmap(Vector v);
   static Vector Logmap(const gtsam::Rot3& p);
+  Vector logmap(const gtsam::Rot3& p);
   Matrix matrix() const;
   Matrix transpose() const;
   gtsam::Point3 column(size_t index) const;
@@ -772,6 +774,7 @@ class Pose2 {
   // Lie Group
   static gtsam::Pose2 Expmap(Vector v);
   static Vector Logmap(const gtsam::Pose2& p);
+  Vector logmap(const gtsam::Pose2& p);
   static Matrix ExpmapDerivative(Vector v);
   static Matrix LogmapDerivative(const gtsam::Pose2& v);
   Matrix AdjointMap() const;
@@ -825,6 +828,7 @@ class Pose3 {
   // Lie Group
   static gtsam::Pose3 Expmap(Vector v);
   static Vector Logmap(const gtsam::Pose3& pose);
+  Vector logmap(const gtsam::Pose3& pose);
   Matrix AdjointMap() const;
   Vector Adjoint(Vector xi) const;
   static Matrix adjointMap_(Vector xi);
@@ -2847,6 +2851,7 @@ virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor {
 
 #include <gtsam/slam/dataset.h>
 class SfmTrack {
+  Point3 point3() const;
   size_t number_measurements() const;
   pair<size_t, gtsam::Point2> measurement(size_t idx) const;
   pair<size_t, size_t> siftIndex(size_t idx) const;

diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h
@@ -233,6 +233,9 @@ struct SfmTrack {
   SiftIndex siftIndex(size_t idx) const {
     return siftIndices[idx];
   }
+  Point3 point3() const {
+    return p;
+  }
 };
 
 /// Define the structure for the camera poses

diff --git a/python/README.md b/python/README.md
@@ -18,8 +18,10 @@ This is the Python wrapper around the GTSAM C++ library. We use our custom [wrap
 
 ## Install
 
-- Run cmake with the `GTSAM_BUILD_PYTHON` cmake flag enabled to configure building the wrapper. The wrapped module will be built and copied to the directory `<PROJECT_BINARY_DIR>/python`.
-
+- Run cmake with the `GTSAM_BUILD_PYTHON` cmake flag enabled to configure building the wrapper. The wrapped module will be built and copied to the directory `<PROJECT_BINARY_DIR>/python`. For example, if your local Python version is 3.6.10, then you should run:
+  ```bash
+  cmake .. -DGTSAM_BUILD_PYTHON=1 -DGTSAM_PYTHON_VERSION=3.6.10
+  ```
 - Build GTSAM and the wrapper with `make` (or `ninja` if you use `-GNinja`).
 
 - To install, simply run `make python-install` (`ninja python-install`).

diff --git a/python/gtsam/examples/ImuFactorExample.py b/python/gtsam/examples/ImuFactorExample.py
@@ -104,7 +104,7 @@ def run(self, T=12, compute_covariances=False, verbose=True):
 
                 if verbose:
                     print(factor)
-                    print(pim.predict(actual_state_i, self.actualBias))
+                    print(pim.predict(initial_state_i, self.actualBias))
 
                 pim.resetIntegration()
 
@@ -125,7 +125,7 @@ def run(self, T=12, compute_covariances=False, verbose=True):
                 i += 1
 
         # add priors on end
-        # self.addPrior(num_poses - 1, graph)
+        self.addPrior(num_poses - 1, graph)
 
         initial.print_("Initial values:")
 

diff --git a/python/gtsam/examples/README.md b/python/gtsam/examples/README.md
@@ -3,50 +3,62 @@
 | C++ Example Name                                      | Ported |
 |-------------------------------------------------------|--------|
 | CameraResectioning                                    |        |
+| CombinedImuFactorsExample                             |        |
 | CreateSFMExampleData                                  |        |
+| DiscreteBayesNetExample                               |        |
 | DiscreteBayesNet_FG                                   | none of the required discrete functionality is exposed through Python |
 | easyPoint2KalmanFilter                                | ExtendedKalmanFilter not yet exposed through Python |
 | elaboratePoint2KalmanFilter                           | GaussianSequentialSolver not yet exposed through Python |
-| ImuFactorExample2                                     | X      |
+| FisheyeExample                                        |        |
+| HMMExample                                            |        |
+| ImuFactorsExample2                                    | :heavy_check_mark:      |
 | ImuFactorsExample                                     |        |
+| IMUKittiExampleGPS                                    |        |
+| InverseKinematicsExampleExpressions.cpp               |        |
 | ISAM2Example_SmartFactor                              |        |
 | ISAM2_SmartFactorStereo_IMU                           |        |
 | LocalizationExample                                   | impossible? |
 | METISOrderingExample                                  |        |
-| OdometryExample                                       | X      |
-| PlanarSLAMExample                                     | X      |
-| Pose2SLAMExample                                      | X      |
+| OdometryExample                                       | :heavy_check_mark:      |
+| PlanarSLAMExample                                     | :heavy_check_mark:      |
+| Pose2SLAMExample                                      | :heavy_check_mark:      |
 | Pose2SLAMExampleExpressions                           |        |
-| Pose2SLAMExample_g2o                                  | X      |
+| Pose2SLAMExample_g2o                                  | :heavy_check_mark:      |
 | Pose2SLAMExample_graph                                |        |
 | Pose2SLAMExample_graphviz                             |        |
 | Pose2SLAMExample_lago                                 | lago not yet exposed through Python |
 | Pose2SLAMStressTest                                   |        |
 | Pose2SLAMwSPCG                                        |        |
+| Pose3Localization                                 |        |
 | Pose3SLAMExample_changeKeys                           |        |
 | Pose3SLAMExampleExpressions_BearingRangeWithTransform |        |
-| Pose3SLAMExample_g2o                                  | X      |
-| Pose3SLAMExample_initializePose3Chordal               |        |
+| Pose3SLAMExample_g2o                                  | :heavy_check_mark:      |
+| Pose3SLAMExample_initializePose3Chordal               | :heavy_check_mark:        |
 | Pose3SLAMExample_initializePose3Gradient              |        |
 | RangeISAMExample_plaza2                               |        |
 | SelfCalibrationExample                                |        |
+| SFMdata                                               |        |     
 | SFMExample_bal_COLAMD_METIS                           |        |
-| SFMExample_bal                                        |        |
-| SFMExample                                            | X      |
+| SFMExample_bal                                        |       |
+| SFMExample                                            | :heavy_check_mark:      |
 | SFMExampleExpressions_bal                             |        |
 | SFMExampleExpressions                                 |        |
 | SFMExample_SmartFactor                                |        |
 | SFMExample_SmartFactorPCG                             |        |
-| SimpleRotation                                        | X      |
+| ShonanAveragingCLI                                    | :heavy_check_mark:       |
+| SimpleRotation                                        | :heavy_check_mark:      |
 | SolverComparer                                        |        |
 | StereoVOExample                                       |        |
 | StereoVOExample_large                                 |        |
 | TimeTBB                                               |        |
 | UGM_chain                                             | discrete functionality not yet exposed |
 | UGM_small                                             | discrete functionality not yet exposed |
-| VisualISAM2Example                                    | X      |
-| VisualISAMExample                                     | X      |
+| VisualISAM2Example                                    | :heavy_check_mark:      |
+| VisualISAMExample                                     | :heavy_check_mark:      |
 
 Extra Examples (with no C++ equivalent)
+- DogLegOptimizerExample
+- GPSFactorExample
 - PlanarManipulatorExample
-- SFMData
+- PreintegrationExample
+- SFMData