diff --git a/CMakeLists.txt b/CMakeLists.txt index aa6082cb3..eedc42c9e 100644 --- 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 index cc8af7248..60eff197a 100644 --- 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 index 4f5743aa6..d55a760c6 100644 --- 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 index 111114a7b..4c44d2cb3 100644 --- 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 index 5757f5ee1..c68679667 100644 --- 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 index 0a56e2ef5..ea822b796 100644 --- 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 index ed69981d8..990ffdfe2 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -181,6 +181,18 @@ public: 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::inverse; // version with derivative /** diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 8586f35a0..9639ffbcf 100644 --- 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::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 index 52f5901ee..085d06245 100644 --- 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 class SfmTrack { + Point3 point3() const; size_t number_measurements() const; pair measurement(size_t idx) const; pair siftIndex(size_t idx) const; diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 91ceaa5fd..a8fddcfe4 100644 --- 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 index b1a3a865f..6e2a30d2e 100644 --- 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 `/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 `/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 index eec7c5ebd..bb707a8f5 100644 --- a/python/gtsam/examples/ImuFactorExample.py +++ b/python/gtsam/examples/ImuFactorExample.py @@ -104,7 +104,7 @@ class ImuFactorExample(PreintegrationExample): 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 @@ class ImuFactorExample(PreintegrationExample): 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 index e998e4dcd..e05a0c7e1 100644 --- 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 \ No newline at end of file +- PreintegrationExample +- SFMData