12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge branch 'develop' into feature/RobustShonan

release/4.3a0
lcarlone 2020-09-24 14:37:43 -04:00
parent 9350dd71ae c7936cdeb2
commit baf1b9948d
11 changed files with 180 additions and 3326 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3346
.cproject
View File

File diff suppressed because it is too large Load Diff

39
.settings/language.settings.xml Normal file
View File

@ -0,0 +1,39 @@
<?xml version="1.0" encoding="UTF-8" standalone="no"?>
<project>
<configuration id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544" name="MacOSX GCC">
<extension point="org.eclipse.cdt.core.LanguageSettingsProvider">
<provider copy-of="extension" id="org.eclipse.cdt.ui.UserLanguageSettingsProvider"/>
<provider-reference id="org.eclipse.cdt.core.ReferencedProjectsLanguageSettingsProvider" ref="shared-provider"/>
<provider copy-of="extension" id="org.eclipse.cdt.managedbuilder.core.GCCBuildCommandParser"/>
<provider class="org.eclipse.cdt.managedbuilder.language.settings.providers.GCCBuiltinSpecsDetector" console="false" env-hash="-1627467870878423342" id="org.eclipse.cdt.managedbuilder.core.GCCBuiltinSpecsDetector" keep-relative-paths="false" name="CDT GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
<language-scope id="org.eclipse.cdt.core.gcc"/>
<language-scope id="org.eclipse.cdt.core.g++"/>
</provider>
<provider-reference id="org.eclipse.cdt.managedbuilder.core.MBSLanguageSettingsProvider" ref="shared-provider"/>
</extension>
</configuration>
<configuration id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.1441575890" name="Timing">
<extension point="org.eclipse.cdt.core.LanguageSettingsProvider">
<provider copy-of="extension" id="org.eclipse.cdt.ui.UserLanguageSettingsProvider"/>
<provider-reference id="org.eclipse.cdt.core.ReferencedProjectsLanguageSettingsProvider" ref="shared-provider"/>
<provider copy-of="extension" id="org.eclipse.cdt.managedbuilder.core.GCCBuildCommandParser"/>
<provider class="org.eclipse.cdt.managedbuilder.language.settings.providers.GCCBuiltinSpecsDetector" console="false" env-hash="-1627467870878423342" id="org.eclipse.cdt.managedbuilder.core.GCCBuiltinSpecsDetector" keep-relative-paths="false" name="CDT GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
<language-scope id="org.eclipse.cdt.core.gcc"/>
<language-scope id="org.eclipse.cdt.core.g++"/>
</provider>
<provider-reference id="org.eclipse.cdt.managedbuilder.core.MBSLanguageSettingsProvider" ref="shared-provider"/>
</extension>
</configuration>
<configuration id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.127261216" name="fast">
<extension point="org.eclipse.cdt.core.LanguageSettingsProvider">
<provider copy-of="extension" id="org.eclipse.cdt.ui.UserLanguageSettingsProvider"/>
<provider-reference id="org.eclipse.cdt.core.ReferencedProjectsLanguageSettingsProvider" ref="shared-provider"/>
<provider copy-of="extension" id="org.eclipse.cdt.managedbuilder.core.GCCBuildCommandParser"/>
<provider class="org.eclipse.cdt.managedbuilder.language.settings.providers.GCCBuiltinSpecsDetector" console="false" env-hash="-1627467870878423342" id="org.eclipse.cdt.managedbuilder.core.GCCBuiltinSpecsDetector" keep-relative-paths="false" name="CDT GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
<language-scope id="org.eclipse.cdt.core.gcc"/>
<language-scope id="org.eclipse.cdt.core.g++"/>
</provider>
<provider-reference id="org.eclipse.cdt.managedbuilder.core.MBSLanguageSettingsProvider" ref="shared-provider"/>
</extension>
</configuration>
</project>

25
CMakeLists.txt
View File

@ -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}") 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 (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 # Gather information, perform checks, set defaults
@ -113,6 +118,22 @@ if(GTSAM_INSTALL_MATLAB_TOOLBOX AND NOT BUILD_SHARED_LIBS)
endif() endif()
if(GTSAM_BUILD_PYTHON) 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(GTSAM_UNSTABLE_BUILD_PYTHON)
if (NOT GTSAM_BUILD_UNSTABLE) if (NOT GTSAM_BUILD_UNSTABLE)
message(WARNING "GTSAM_UNSTABLE_BUILD_PYTHON requires the unstable module to be enabled.") 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})") print_config("Use System Eigen" "${GTSAM_USE_SYSTEM_EIGEN} (Using version: ${GTSAM_EIGEN_VERSION})")
if(GTSAM_USE_TBB) if(GTSAM_USE_TBB)
print_config("Use Intel TBB" "Yes") print_config("Use Intel TBB" "Yes (Version: ${TBB_VERSION})")
elseif(TBB_FOUND) 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() else()
print_config("Use Intel TBB" "TBB not found") print_config("Use Intel TBB" "TBB not found")
endif() endif()

2
README.md
View File

@ -62,7 +62,7 @@ GTSAM 4.1 added a new pybind wrapper, and **removed** the deprecated functionali
## Wrappers ## 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 ## The Preintegrated IMU Factor

12
cmake/FindNumPy.cmake
View File

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

22
cmake/GtsamMatlabWrap.cmake
View File

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

8
cmake/GtsamPrinting.cmake
View File

@ -46,16 +46,16 @@ endfunction()
# Prints all the relevant CMake build options for a given target: # Prints all the relevant CMake build options for a given target:
function(print_build_options_for_target target_name_) function(print_build_options_for_target target_name_)
print_padded(GTSAM_COMPILE_FEATURES_PUBLIC) 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_OPTIONS_PUBLIC)
print_padded(GTSAM_COMPILE_DEFINITIONS_PRIVATE) # print_padded(GTSAM_COMPILE_DEFINITIONS_PRIVATE)
print_padded(GTSAM_COMPILE_DEFINITIONS_PUBLIC) print_padded(GTSAM_COMPILE_DEFINITIONS_PUBLIC)
foreach(build_type ${GTSAM_CMAKE_CONFIGURATION_TYPES}) foreach(build_type ${GTSAM_CMAKE_CONFIGURATION_TYPES})
string(TOUPPER "${build_type}" build_type_toupper) 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_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}) print_padded(GTSAM_COMPILE_DEFINITIONS_PUBLIC_${build_type_toupper})
endforeach() endforeach()
endfunction() endfunction()

5
gtsam/gtsam.i
View File

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

3
gtsam/slam/dataset.h
View File

@ -233,6 +233,9 @@ struct SfmTrack {
SiftIndex siftIndex(size_t idx) const { SiftIndex siftIndex(size_t idx) const {
return siftIndices[idx]; return siftIndices[idx];
} }
Point3 point3() const {
return p;
}
}; };
/// Define the structure for the camera poses /// Define the structure for the camera poses

6
python/README.md
View File

@ -18,8 +18,10 @@ This is the Python wrapper around the GTSAM C++ library. We use our custom [wrap
## Install ## 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`). - Build GTSAM and the wrapper with `make` (or `ninja` if you use `-GNinja`).
- To install, simply run `make python-install` (`ninja python-install`). - To install, simply run `make python-install` (`ninja python-install`).

38
python/gtsam/examples/README.md
View File

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