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

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

release/4.3a0
alexma3312 2020-09-26 14:11:53 -04:00
parent 8236d69fa1 79d6d5fa2f
commit 3727cc6053
13 changed files with 107 additions and 61 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

2
README.md
View File

@ -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

12
cmake/FindNumPy.cmake
View File

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

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

8
cmake/GtsamPrinting.cmake
View File

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

20
gtsam/geometry/Pose3.cpp
View File

@ -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;

12
gtsam/geometry/Pose3.h
View File

@ -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<Pose3, 6>::inverse; // version with derivative
/**

11
gtsam/geometry/tests/testPose3.cpp
View File

@ -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;

5
gtsam/gtsam.i
View File

@ -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;

3
gtsam/slam/dataset.h
View File

@ -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

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
- 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`).

4
python/gtsam/examples/ImuFactorExample.py
View File

@ -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:")

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

@ -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
- PreintegrationExample
- SFMData