diff --git a/.gitignore b/.gitignore index 60633adaf..f3f1efd5b 100644 --- a/.gitignore +++ b/.gitignore @@ -5,3 +5,5 @@ /examples/Data/dubrovnik-3-7-pre-rewritten.txt /examples/Data/pose2example-rewritten.txt /examples/Data/pose3example-rewritten.txt +*.txt.user +*.txt.user.6d59f0c diff --git a/README.md b/README.md index 460f51bf3..623b1ff32 100644 --- a/README.md +++ b/README.md @@ -1,47 +1,49 @@ -README - Georgia Tech Smoothing and Mapping library -=================================================== - -What is GTSAM? --------------- - -GTSAM is a library of C++ classes that implement smoothing and -mapping (SAM) in robotics and vision, using factor graphs and Bayes -networks as the underlying computing paradigm rather than sparse -matrices. - -On top of the C++ library, GTSAM includes a MATLAB interface (enable -GTSAM_INSTALL_MATLAB_TOOLBOX in CMake to build it). A Python interface -is under development. - -Quickstart ----------- - -In the root library folder execute: - -``` -#!bash -$ mkdir build -$ cd build -$ cmake .. -$ make check (optional, runs unit tests) -$ make install -``` - -Prerequisites: - -- [Boost](http://www.boost.org/users/download/) >= 1.43 (Ubuntu: `sudo apt-get install libboost-all-dev`) -- [CMake](http://www.cmake.org/cmake/resources/software.html) >= 2.6 (Ubuntu: `sudo apt-get install cmake`) - -Optional prerequisites - used automatically if findable by CMake: - -- [Intel Threaded Building Blocks (TBB)](http://www.threadingbuildingblocks.org/) (Ubuntu: `sudo apt-get install libtbb-dev`) -- [Intel Math Kernel Library (MKL)](http://software.intel.com/en-us/intel-mkl) - -Additional Information ----------------------- - -See the [`INSTALL`](https://bitbucket.org/gtborg/gtsam/src/develop/INSTALL) file for more detailed installation instructions. - -GTSAM is open source under the BSD license, see the [`LICENSE`](https://bitbucket.org/gtborg/gtsam/src/develop/LICENSE) and [`LICENSE.BSD`](https://bitbucket.org/gtborg/gtsam/src/develop/LICENSE.BSD) files. - -Please see the [`examples/`](https://bitbucket.org/gtborg/gtsam/src/develop/examples) directory and the [`USAGE`](https://bitbucket.org/gtborg/gtsam/src/develop/USAGE) file for examples on how to use GTSAM. +README - Georgia Tech Smoothing and Mapping library +=================================================== + +What is GTSAM? +-------------- + +GTSAM is a library of C++ classes that implement smoothing and +mapping (SAM) in robotics and vision, using factor graphs and Bayes +networks as the underlying computing paradigm rather than sparse +matrices. + +On top of the C++ library, GTSAM includes a MATLAB interface (enable +GTSAM_INSTALL_MATLAB_TOOLBOX in CMake to build it). A Python interface +is under development. + +Quickstart +---------- + +In the root library folder execute: + +``` +#!bash +$ mkdir build +$ cd build +$ cmake .. +$ make check (optional, runs unit tests) +$ make install +``` + +Prerequisites: + +- [Boost](http://www.boost.org/users/download/) >= 1.43 (Ubuntu: `sudo apt-get install libboost-all-dev`) +- [CMake](http://www.cmake.org/cmake/resources/software.html) >= 2.6 (Ubuntu: `sudo apt-get install cmake`) + +Optional prerequisites - used automatically if findable by CMake: + +- [Intel Threaded Building Blocks (TBB)](http://www.threadingbuildingblocks.org/) (Ubuntu: `sudo apt-get install libtbb-dev`) +- [Intel Math Kernel Library (MKL)](http://software.intel.com/en-us/intel-mkl) + +Additional Information +---------------------- + +Read about important [`GTSAM-Concepts`] here. + +See the [`INSTALL`] file for more detailed installation instructions. + +GTSAM is open source under the BSD license, see the [`LICENSE`](https://bitbucket.org/gtborg/gtsam/src/develop/LICENSE) and [`LICENSE.BSD`](https://bitbucket.org/gtborg/gtsam/src/develop/LICENSE.BSD) files. + +Please see the [`examples/`](examples) directory and the [`USAGE`] file for examples on how to use GTSAM. \ No newline at end of file diff --git a/cmake/GtsamBuildTypes.cmake b/cmake/GtsamBuildTypes.cmake index 9a0b297ab..2f11df94e 100644 --- a/cmake/GtsamBuildTypes.cmake +++ b/cmake/GtsamBuildTypes.cmake @@ -15,8 +15,8 @@ option(GTSAM_BUILD_TYPE_POSTFIXES "Enable/Disable appending the build typ # Add debugging flags but only on the first pass if(NOT FIRST_PASS_DONE) if(MSVC) - set(CMAKE_C_FLAGS_DEBUG "/D_DEBUG /MDd /Zi /Ob0 /Od /RTC1 /W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during debug builds." FORCE) - set(CMAKE_CXX_FLAGS_DEBUG "/D_DEBUG /MDd /Zi /Ob0 /Od /RTC1 /W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during debug builds." FORCE) + set(CMAKE_C_FLAGS_DEBUG "/D_DEBUG /MDd /Zi /Ob0 /Od /RTC1 /W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN /DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Flags used by the compiler during debug builds." FORCE) + set(CMAKE_CXX_FLAGS_DEBUG "/D_DEBUG /MDd /Zi /Ob0 /Od /RTC1 /W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN /DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Flags used by the compiler during debug builds." FORCE) set(CMAKE_C_FLAGS_RELWITHDEBINFO "/MD /O2 /DNDEBUG /W3 /GR /EHsc /MP /Zi /d2Zi+ /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during relwithdebinfo builds." FORCE) set(CMAKE_CXX_FLAGS_RELWITHDEBINFO "/MD /O2 /DNDEBUG /W3 /GR /EHsc /MP /Zi /d2Zi+ /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during relwithdebinfo builds." FORCE) set(CMAKE_C_FLAGS_RELEASE "/MD /O2 /DNDEBUG /W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during release builds." FORCE) @@ -34,8 +34,8 @@ if(NOT FIRST_PASS_DONE) set(CMAKE_MODULE_LINKER_FLAGS_PROFILING "${CMAKE_MODULE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during profiling builds." FORCE) mark_as_advanced(CMAKE_C_FLAGS_PROFILING CMAKE_CXX_FLAGS_PROFILING CMAKE_EXE_LINKER_FLAGS_PROFILING CMAKE_SHARED_LINKER_FLAGS_PROFILING CMAKE_MODULE_LINKER_FLAGS_PROFILING) else() - set(CMAKE_C_FLAGS_DEBUG "${CMAKE_C_FLAGS_DEBUG} -fno-inline -Wall" CACHE STRING "Flags used by the compiler during debug builds." FORCE) - set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -fno-inline -Wall" CACHE STRING "Flags used by the compiler during debug builds." FORCE) + set(CMAKE_C_FLAGS_DEBUG "${CMAKE_C_FLAGS_DEBUG} -fno-inline -Wall -DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Flags used by the compiler during debug builds." FORCE) + set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -fno-inline -Wall -DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Flags used by the compiler during debug builds." FORCE) set(CMAKE_C_FLAGS_RELWITHDEBINFO "-g -O3 -Wall -DNDEBUG" CACHE STRING "Flags used by the compiler during relwithdebinfo builds." FORCE) set(CMAKE_CXX_FLAGS_RELWITHDEBINFO "-g -O3 -Wall -DNDEBUG" CACHE STRING "Flags used by the compiler during relwithdebinfo builds." FORCE) set(CMAKE_C_FLAGS_RELEASE "-O3 -Wall -DNDEBUG -Wall" CACHE STRING "Flags used by the compiler during release builds." FORCE) diff --git a/gtsam.h b/gtsam.h index d63563028..56bf378b5 100644 --- a/gtsam.h +++ b/gtsam.h @@ -156,12 +156,6 @@ virtual class Value { size_t dim() const; }; -class Vector3 { - Vector3(Vector v); -}; -class Vector6 { - Vector6(Vector v); -}; #include class LieScalar { // Standard constructors @@ -758,10 +752,6 @@ class CalibratedCamera { gtsam::CalibratedCamera retract(const Vector& d) const; Vector localCoordinates(const gtsam::CalibratedCamera& T2) const; - // Group - gtsam::CalibratedCamera compose(const gtsam::CalibratedCamera& c) const; - gtsam::CalibratedCamera inverse() const; - // Action on Point3 gtsam::Point2 project(const gtsam::Point3& point) const; static gtsam::Point2 project_to_camera(const gtsam::Point3& cameraPoint); @@ -1727,6 +1717,7 @@ class Values { // void insert(size_t j, const gtsam::Value& value); // void update(size_t j, const gtsam::Value& val); // gtsam::Value at(size_t j) const; + void insert(size_t j, const gtsam::Point2& t); void insert(size_t j, const gtsam::Point3& t); void insert(size_t j, const gtsam::Rot2& t); @@ -1738,6 +1729,11 @@ class Values { void insert(size_t j, const gtsam::Cal3Bundler& t); void insert(size_t j, const gtsam::EssentialMatrix& t); void insert(size_t j, const gtsam::imuBias::ConstantBias& t); + void insert(size_t j, Vector t); + void insert(size_t j, Matrix t); + + // Fixed size version + void insertFixed(size_t j, Vector t, size_t n); void update(size_t j, const gtsam::Point2& t); void update(size_t j, const gtsam::Point3& t); @@ -1750,9 +1746,10 @@ class Values { void update(size_t j, const gtsam::Cal3Bundler& t); void update(size_t j, const gtsam::EssentialMatrix& t); void update(size_t j, const gtsam::imuBias::ConstantBias& t); + void update(size_t j, Vector t); + void update(size_t j, Matrix t); - template + template T at(size_t j); }; @@ -2150,7 +2147,7 @@ class NonlinearISAM { #include #include -template +template virtual class PriorFactor : gtsam::NoiseModelFactor { PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); T prior() const; @@ -2195,10 +2192,14 @@ typedef gtsam::RangeFactor RangeFactorPosePoint2; typedef gtsam::RangeFactor RangeFactorPosePoint3; typedef gtsam::RangeFactor RangeFactorPose2; typedef gtsam::RangeFactor RangeFactorPose3; -typedef gtsam::RangeFactor RangeFactorCalibratedCameraPoint; -typedef gtsam::RangeFactor RangeFactorSimpleCameraPoint; -typedef gtsam::RangeFactor RangeFactorCalibratedCamera; -typedef gtsam::RangeFactor RangeFactorSimpleCamera; + +// Commented out by Frank Dec 2014: not poses! +// If needed, we need a RangeFactor that takes a camera, extracts the pose +// Should be easy with Expressions +//typedef gtsam::RangeFactor RangeFactorCalibratedCameraPoint; +//typedef gtsam::RangeFactor RangeFactorSimpleCameraPoint; +//typedef gtsam::RangeFactor RangeFactorCalibratedCamera; +//typedef gtsam::RangeFactor RangeFactorSimpleCamera; #include @@ -2391,7 +2392,7 @@ class ConstantBias { #include class PoseVelocity{ - PoseVelocity(const gtsam::Pose3& pose, const gtsam::Vector3 velocity); + PoseVelocity(const gtsam::Pose3& pose, Vector velocity); }; class ImuFactorPreintegratedMeasurements { // Standard Constructor @@ -2430,7 +2431,7 @@ virtual class ImuFactor : gtsam::NonlinearFactor { const gtsam::Pose3& body_P_sensor); // Standard Interface gtsam::ImuFactorPreintegratedMeasurements preintegratedMeasurements() const; - gtsam::PoseVelocity Predict(const gtsam::Pose3& pose_i, const gtsam::Vector3& vel_i, const gtsam::imuBias::ConstantBias& bias, + gtsam::PoseVelocity Predict(const gtsam::Pose3& pose_i, Vector vel_i, const gtsam::imuBias::ConstantBias& bias, const gtsam::ImuFactorPreintegratedMeasurements& preintegratedMeasurements, Vector gravity, Vector omegaCoriolis) const; }; @@ -2476,7 +2477,7 @@ virtual class AHRSFactor : gtsam::NonlinearFactor { #include class PoseVelocityBias{ - PoseVelocityBias(const gtsam::Pose3& pose, const gtsam::Vector3 velocity, const gtsam::imuBias::ConstantBias& bias); + PoseVelocityBias(const gtsam::Pose3& pose, Vector velocity, const gtsam::imuBias::ConstantBias& bias); }; class CombinedImuFactorPreintegratedMeasurements { // Standard Constructor @@ -2527,7 +2528,7 @@ virtual class CombinedImuFactor : gtsam::NonlinearFactor { // Standard Interface gtsam::CombinedImuFactorPreintegratedMeasurements preintegratedMeasurements() const; - gtsam::PoseVelocityBias Predict(const gtsam::Pose3& pose_i, const gtsam::Vector3& vel_i, const gtsam::imuBias::ConstantBias& bias_i, + gtsam::PoseVelocityBias Predict(const gtsam::Pose3& pose_i, Vector vel_i, const gtsam::imuBias::ConstantBias& bias_i, const gtsam::CombinedImuFactorPreintegratedMeasurements& preintegratedMeasurements, Vector gravity, Vector omegaCoriolis); }; diff --git a/gtsam/3rdparty/CMakeLists.txt b/gtsam/3rdparty/CMakeLists.txt index 49d5a2fbb..301548dcf 100644 --- a/gtsam/3rdparty/CMakeLists.txt +++ b/gtsam/3rdparty/CMakeLists.txt @@ -45,6 +45,8 @@ endif() option(GTSAM_BUILD_METIS_EXECUTABLES "Build metis library executables" OFF) add_subdirectory(metis) +add_subdirectory(ceres) + ############ NOTE: When updating GeographicLib be sure to disable building their examples ############ and unit tests by commenting out their lines: # add_subdirectory (examples) diff --git a/gtsam/3rdparty/ceres/CMakeLists.txt b/gtsam/3rdparty/ceres/CMakeLists.txt new file mode 100644 index 000000000..98b2cffce --- /dev/null +++ b/gtsam/3rdparty/ceres/CMakeLists.txt @@ -0,0 +1,2 @@ +file(GLOB ceres_headers "${CMAKE_CURRENT_SOURCE_DIR}/*.h") +install(FILES ${ceres_headers} DESTINATION include/gtsam/3rdparty/ceres) \ No newline at end of file diff --git a/gtsam_unstable/nonlinear/ceres_autodiff.h b/gtsam/3rdparty/ceres/autodiff.h similarity index 98% rename from gtsam_unstable/nonlinear/ceres_autodiff.h rename to gtsam/3rdparty/ceres/autodiff.h index 2a0ac8987..f124425cc 100644 --- a/gtsam_unstable/nonlinear/ceres_autodiff.h +++ b/gtsam/3rdparty/ceres/autodiff.h @@ -142,10 +142,10 @@ #include -#include -#include -#include -#include +#include +#include +#include +#include #define DCHECK assert #define DCHECK_GT(a,b) assert((a)>(b)) diff --git a/gtsam_unstable/nonlinear/ceres_eigen.h b/gtsam/3rdparty/ceres/eigen.h similarity index 100% rename from gtsam_unstable/nonlinear/ceres_eigen.h rename to gtsam/3rdparty/ceres/eigen.h diff --git a/gtsam_unstable/nonlinear/ceres_example.h b/gtsam/3rdparty/ceres/example.h similarity index 98% rename from gtsam_unstable/nonlinear/ceres_example.h rename to gtsam/3rdparty/ceres/example.h index 45ec3428e..6b051fce0 100644 --- a/gtsam_unstable/nonlinear/ceres_example.h +++ b/gtsam/3rdparty/ceres/example.h @@ -33,7 +33,7 @@ #pragma once -#include +#include // Templated pinhole camera model for used with Ceres. The camera is // parameterized using 9 parameters: 3 for rotation, 3 for translation, 1 for diff --git a/gtsam_unstable/nonlinear/ceres_fixed_array.h b/gtsam/3rdparty/ceres/fixed_array.h similarity index 98% rename from gtsam_unstable/nonlinear/ceres_fixed_array.h rename to gtsam/3rdparty/ceres/fixed_array.h index 69a426379..db1591636 100644 --- a/gtsam_unstable/nonlinear/ceres_fixed_array.h +++ b/gtsam/3rdparty/ceres/fixed_array.h @@ -34,8 +34,8 @@ #include #include -#include -#include +#include +#include namespace ceres { namespace internal { diff --git a/gtsam_unstable/nonlinear/ceres_fpclassify.h b/gtsam/3rdparty/ceres/fpclassify.h similarity index 100% rename from gtsam_unstable/nonlinear/ceres_fpclassify.h rename to gtsam/3rdparty/ceres/fpclassify.h diff --git a/gtsam_unstable/nonlinear/ceres_jet.h b/gtsam/3rdparty/ceres/jet.h similarity index 99% rename from gtsam_unstable/nonlinear/ceres_jet.h rename to gtsam/3rdparty/ceres/jet.h index ed4834caf..12d4e8bc9 100644 --- a/gtsam_unstable/nonlinear/ceres_jet.h +++ b/gtsam/3rdparty/ceres/jet.h @@ -163,7 +163,7 @@ #include #include -#include +#include namespace ceres { diff --git a/gtsam_unstable/nonlinear/ceres_macros.h b/gtsam/3rdparty/ceres/macros.h similarity index 100% rename from gtsam_unstable/nonlinear/ceres_macros.h rename to gtsam/3rdparty/ceres/macros.h diff --git a/gtsam_unstable/nonlinear/ceres_manual_constructor.h b/gtsam/3rdparty/ceres/manual_constructor.h similarity index 100% rename from gtsam_unstable/nonlinear/ceres_manual_constructor.h rename to gtsam/3rdparty/ceres/manual_constructor.h diff --git a/gtsam_unstable/nonlinear/ceres_rotation.h b/gtsam/3rdparty/ceres/rotation.h similarity index 99% rename from gtsam_unstable/nonlinear/ceres_rotation.h rename to gtsam/3rdparty/ceres/rotation.h index b02c10211..6ed3b88cb 100644 --- a/gtsam_unstable/nonlinear/ceres_rotation.h +++ b/gtsam/3rdparty/ceres/rotation.h @@ -47,6 +47,7 @@ #include #include +#include #include #define DCHECK assert diff --git a/gtsam_unstable/nonlinear/ceres_variadic_evaluate.h b/gtsam/3rdparty/ceres/variadic_evaluate.h similarity index 97% rename from gtsam_unstable/nonlinear/ceres_variadic_evaluate.h rename to gtsam/3rdparty/ceres/variadic_evaluate.h index 7d22fe22e..86aec4829 100644 --- a/gtsam_unstable/nonlinear/ceres_variadic_evaluate.h +++ b/gtsam/3rdparty/ceres/variadic_evaluate.h @@ -34,9 +34,9 @@ #include -#include -#include -#include +#include +#include +#include namespace ceres { namespace internal { diff --git a/gtsam/base/LieMatrix.h b/gtsam/base/LieMatrix.h index f1fc69e71..7b120df1c 100644 --- a/gtsam/base/LieMatrix.h +++ b/gtsam/base/LieMatrix.h @@ -19,6 +19,12 @@ #include +#ifdef _MSC_VER +#pragma message("LieMatrix.h is deprecated. Please use Eigen::Matrix instead.") +#else +#warning "LieMatrix.h is deprecated. Please use Eigen::Matrix instead." +#endif + #include #include #include @@ -27,7 +33,9 @@ namespace gtsam { /** - * LieVector is a wrapper around vector to allow it to be a Lie type + * @deprecated: LieScalar, LieVector and LieMatrix are obsolete in GTSAM 4.0 as + * we can directly add double, Vector, and Matrix into values now, because of + * gtsam::traits. */ struct LieMatrix : public Matrix { diff --git a/gtsam/base/LieScalar.h b/gtsam/base/LieScalar.h index 5125340be..b208a584a 100644 --- a/gtsam/base/LieScalar.h +++ b/gtsam/base/LieScalar.h @@ -17,6 +17,12 @@ #pragma once +#ifdef _MSC_VER +#pragma message("LieScalar.h is deprecated. Please use double/float instead.") +#else + #warning "LieScalar.h is deprecated. Please use double/float instead." +#endif + #include #include #include @@ -24,7 +30,9 @@ namespace gtsam { /** - * LieScalar is a wrapper around double to allow it to be a Lie type + * @deprecated: LieScalar, LieVector and LieMatrix are obsolete in GTSAM 4.0 as + * we can directly add double, Vector, and Matrix into values now, because of + * gtsam::traits. */ struct GTSAM_EXPORT LieScalar { diff --git a/gtsam/base/LieVector.h b/gtsam/base/LieVector.h index d1b2ad78c..d158548ad 100644 --- a/gtsam/base/LieVector.h +++ b/gtsam/base/LieVector.h @@ -17,6 +17,12 @@ #pragma once +#ifdef _MSC_VER +#pragma message("LieVector.h is deprecated. Please use Eigen::Vector instead.") +#else +#warning "LieVector.h is deprecated. Please use Eigen::Vector instead." +#endif + #include #include #include @@ -24,7 +30,9 @@ namespace gtsam { /** - * LieVector is a wrapper around vector to allow it to be a Lie type + * @deprecated: LieScalar, LieVector and LieMatrix are obsolete in GTSAM 4.0 as + * we can directly add double, Vector, and Matrix into values now, because of + * gtsam::traits. */ struct LieVector : public Vector { diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h index a3a5b029b..1190822ed 100644 --- a/gtsam/base/Manifold.h +++ b/gtsam/base/Manifold.h @@ -73,7 +73,6 @@ template struct dimension: public Dynamic { }; - /** * zero::value is intended to be the origin of a canonical coordinate system * with canonical(t) == DefaultChart::local(zero::value, t) @@ -111,14 +110,16 @@ struct is_group > : publi }; template -struct is_manifold > : public boost::true_type{ +struct is_manifold > : public boost::true_type { }; template -struct dimension > : public boost::integral_constant { - //TODO after switch to c++11 : the above should should be extracted to a constexpr function - // for readability and to reduce code duplication +struct dimension > : public boost::integral_constant< + int, + M == Eigen::Dynamic ? Eigen::Dynamic : + (N == Eigen::Dynamic ? Eigen::Dynamic : M * N)> { + //TODO after switch to c++11 : the above should should be extracted to a constexpr function + // for readability and to reduce code duplication }; template @@ -131,10 +132,10 @@ struct zero > { }; template -struct identity > : public zero > { +struct identity > : public zero< + Eigen::Matrix > { }; - template struct is_chart: public boost::false_type { }; @@ -248,12 +249,16 @@ struct DefaultChart > { * This chart for the vector space of M x N matrices (represented by Eigen matrices) chooses as basis the one with respect to which the coordinates are exactly the matrix entries as laid out in memory (as determined by Options). * Computing coordinates for a matrix is then simply a reshape to the row vector of appropriate size. */ - typedef Eigen::Matrix type; + typedef Eigen::Matrix type; typedef type T; - typedef Eigen::Matrix::value, 1> vector;BOOST_STATIC_ASSERT_MSG((M!=Eigen::Dynamic && N!=Eigen::Dynamic), - "DefaultChart has not been implemented yet for dynamically sized matrices"); + typedef Eigen::Matrix::value, 1> vector; + + BOOST_STATIC_ASSERT_MSG((M!=Eigen::Dynamic && N!=Eigen::Dynamic), + "Internal error: DefaultChart for Dynamic should be handled by template below"); + static vector local(const T& origin, const T& other) { - return reshape(other) - reshape(origin); + return reshape(other) + - reshape(origin); } static T retract(const T& origin, const vector& d) { return origin + reshape(d); @@ -266,20 +271,36 @@ struct DefaultChart > { // Dynamically sized Vector template<> struct DefaultChart { - typedef Vector T; - typedef T type; - typedef T vector; - static vector local(const T& origin, const T& other) { + typedef Vector type; + typedef Vector vector; + static vector local(const Vector& origin, const Vector& other) { return other - origin; } - static T retract(const T& origin, const vector& d) { + static Vector retract(const Vector& origin, const vector& d) { return origin + d; } - static int getDimension(const T& origin) { + static int getDimension(const Vector& origin) { return origin.size(); } }; +// Dynamically sized Matrix +template<> +struct DefaultChart { + typedef Matrix type; + typedef Vector vector; + static vector local(const Matrix& origin, const Matrix& other) { + Matrix d = other - origin; + return Eigen::Map(d.data(),getDimension(d)); + } + static Matrix retract(const Matrix& origin, const vector& d) { + return origin + Eigen::Map(d.data(),origin.rows(),origin.cols()); + } + static int getDimension(const Matrix& m) { + return m.size(); + } +}; + /** * Old Concept check class for Manifold types * Requires a mapping between a linear tangent space and the underlying diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index 132bf79ad..3ecfcf8fa 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -37,27 +37,31 @@ namespace gtsam { typedef Eigen::MatrixXd Matrix; typedef Eigen::Matrix MatrixRowMajor; -typedef Eigen::Matrix2d Matrix2; -typedef Eigen::Matrix3d Matrix3; -typedef Eigen::Matrix4d Matrix4; -typedef Eigen::Matrix Matrix5; -typedef Eigen::Matrix Matrix6; +// Create handy typedefs and constants for square-size matrices +// MatrixMN, MatrixN = MatrixNN, I_NxN, and Z_NxN, for M,N=1..9 +#define GTSAM_MAKE_TYPEDEFS(SIZE, SUFFIX) \ +typedef Eigen::Matrix Matrix##SUFFIX; \ +typedef Eigen::Matrix Matrix1##SUFFIX; \ +typedef Eigen::Matrix Matrix2##SUFFIX; \ +typedef Eigen::Matrix Matrix3##SUFFIX; \ +typedef Eigen::Matrix Matrix4##SUFFIX; \ +typedef Eigen::Matrix Matrix5##SUFFIX; \ +typedef Eigen::Matrix Matrix6##SUFFIX; \ +typedef Eigen::Matrix Matrix7##SUFFIX; \ +typedef Eigen::Matrix Matrix8##SUFFIX; \ +typedef Eigen::Matrix Matrix9##SUFFIX; \ +static const Eigen::MatrixBase::IdentityReturnType I_##SUFFIX##x##SUFFIX = Matrix##SUFFIX::Identity(); \ +static const Eigen::MatrixBase::ConstantReturnType Z_##SUFFIX##x##SUFFIX = Matrix##SUFFIX::Zero(); -typedef Eigen::Matrix Matrix23; -typedef Eigen::Matrix Matrix24; -typedef Eigen::Matrix Matrix25; -typedef Eigen::Matrix Matrix26; -typedef Eigen::Matrix Matrix27; -typedef Eigen::Matrix Matrix28; -typedef Eigen::Matrix Matrix29; - -typedef Eigen::Matrix Matrix32; -typedef Eigen::Matrix Matrix34; -typedef Eigen::Matrix Matrix35; -typedef Eigen::Matrix Matrix36; -typedef Eigen::Matrix Matrix37; -typedef Eigen::Matrix Matrix38; -typedef Eigen::Matrix Matrix39; +GTSAM_MAKE_TYPEDEFS(1,1); +GTSAM_MAKE_TYPEDEFS(2,2); +GTSAM_MAKE_TYPEDEFS(3,3); +GTSAM_MAKE_TYPEDEFS(4,4); +GTSAM_MAKE_TYPEDEFS(5,5); +GTSAM_MAKE_TYPEDEFS(6,6); +GTSAM_MAKE_TYPEDEFS(7,7); +GTSAM_MAKE_TYPEDEFS(8,8); +GTSAM_MAKE_TYPEDEFS(9,9); // Matrix expressions for accessing parts of matrices typedef Eigen::Block SubMatrix; diff --git a/gtsam/base/OptionalJacobian.h b/gtsam/base/OptionalJacobian.h new file mode 100644 index 000000000..5651816ba --- /dev/null +++ b/gtsam/base/OptionalJacobian.h @@ -0,0 +1,115 @@ +/* ---------------------------------------------------------------------------- + + * 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 OptionalJacobian.h + * @brief Special class for optional Matrix arguments + * @author Frank Dellaert + * @author Natesh Srinivasan + * @date Nov 28, 2014 + */ + +#pragma once + +#include + +#ifndef OPTIONALJACOBIAN_NOBOOST +#include +#endif + +namespace gtsam { + +/** + * OptionalJacobian is an Eigen::Ref like class that can take be constructed using + * either a fixed size or dynamic Eigen matrix. In the latter case, the dynamic + * matrix will be resized. Finally, there is a constructor that takes + * boost::none, the default constructor acts like boost::none, and + * boost::optional is also supported for backwards compatibility. + */ +template +class OptionalJacobian { + +public: + + /// Fixed size type + typedef Eigen::Matrix Fixed; + +private: + + Eigen::Map map_; /// View on constructor argument, if given + + // Trick from http://eigen.tuxfamily.org/dox/group__TutorialMapClass.html + // uses "placement new" to make map_ usurp the memory of the fixed size matrix + void usurp(double* data) { + new (&map_) Eigen::Map(data); + } + +public: + + /// Default constructor acts like boost::none + OptionalJacobian() : + map_(NULL) { + } + + /// Constructor that will usurp data of a fixed-size matrix + OptionalJacobian(Fixed& fixed) : + map_(NULL) { + usurp(fixed.data()); + } + + /// Constructor that will usurp data of a fixed-size matrix, pointer version + OptionalJacobian(Fixed* fixedPtr) : + map_(NULL) { + if (fixedPtr) + usurp(fixedPtr->data()); + } + + /// Constructor that will resize a dynamic matrix (unless already correct) + OptionalJacobian(Eigen::MatrixXd& dynamic) : + map_(NULL) { + dynamic.resize(Rows, Cols); // no malloc if correct size + usurp(dynamic.data()); + } + +#ifndef OPTIONALJACOBIAN_NOBOOST + + /// Constructor with boost::none just makes empty + OptionalJacobian(boost::none_t none) : + map_(NULL) { + } + + /// Constructor compatible with old-style derivatives + OptionalJacobian(const boost::optional optional) : + map_(NULL) { + if (optional) { + optional->resize(Rows, Cols); + usurp(optional->data()); + } + } + +#endif + + /// Return true is allocated, false if default constructor was used + operator bool() const { + return map_.data(); + } + + /// De-reference, like boost optional + Eigen::Map& operator*() { + return map_; + } + + /// TODO: operator->() + Eigen::Map* operator->(){ return &map_; } +}; + +} // namespace gtsam + diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index 1be8408d2..74cd248a1 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -34,6 +34,7 @@ namespace gtsam { typedef Eigen::VectorXd Vector; // Commonly used fixed size vectors +typedef Eigen::Matrix Vector1; typedef Eigen::Vector2d Vector2; typedef Eigen::Vector3d Vector3; typedef Eigen::Matrix Vector4; @@ -42,6 +43,7 @@ typedef Eigen::Matrix Vector6; typedef Eigen::Matrix Vector7; typedef Eigen::Matrix Vector8; typedef Eigen::Matrix Vector9; +typedef Eigen::Matrix Vector10; typedef Eigen::VectorBlock SubVector; typedef Eigen::VectorBlock ConstSubVector; diff --git a/gtsam/base/debug.cpp b/gtsam/base/debug.cpp index e50e3af35..6abc98642 100644 --- a/gtsam/base/debug.cpp +++ b/gtsam/base/debug.cpp @@ -17,9 +17,32 @@ */ #include +#ifdef GTSAM_USE_TBB +#include +#endif namespace gtsam { - GTSAM_EXPORT FastMap > debugFlags; +GTSAM_EXPORT FastMap > debugFlags; + +#ifdef GTSAM_USE_TBB +tbb::mutex debugFlagsMutex; +#endif + +/* ************************************************************************* */ +bool guardedIsDebug(const std::string& s) { +#ifdef GTSAM_USE_TBB + tbb::mutex::scoped_lock lock(debugFlagsMutex); +#endif + return gtsam::debugFlags[s]; +} + +/* ************************************************************************* */ +void guardedSetDebug(const std::string& s, const bool v) { +#ifdef GTSAM_USE_TBB + tbb::mutex::scoped_lock lock(debugFlagsMutex); +#endif + gtsam::debugFlags[s] = v; +} } diff --git a/gtsam/base/debug.h b/gtsam/base/debug.h index f416bd826..e21efcc83 100644 --- a/gtsam/base/debug.h +++ b/gtsam/base/debug.h @@ -43,6 +43,10 @@ namespace gtsam { GTSAM_EXTERN_EXPORT FastMap > debugFlags; + + // Non-guarded use led to crashes, and solved in commit cd35db2 + bool GTSAM_EXPORT guardedIsDebug(const std::string& s); + void GTSAM_EXPORT guardedSetDebug(const std::string& s, const bool v); } #undef ISDEBUG @@ -50,8 +54,8 @@ namespace gtsam { #ifdef GTSAM_ENABLE_DEBUG -#define ISDEBUG(S) (gtsam::debugFlags[S]) -#define SETDEBUG(S,V) ((void)(gtsam::debugFlags[S] = (V))) +#define ISDEBUG(S) (gtsam::guardedIsDebug(S)) +#define SETDEBUG(S,V) ((void)(gtsam::guardedSetDebug(S,V))) #else diff --git a/gtsam/base/tests/testOptionalJacobian.cpp b/gtsam/base/tests/testOptionalJacobian.cpp new file mode 100644 index 000000000..6584c82d1 --- /dev/null +++ b/gtsam/base/tests/testOptionalJacobian.cpp @@ -0,0 +1,105 @@ +/* ---------------------------------------------------------------------------- + + * 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 testOptionalJacobian.cpp + * @brief Unit test for OptionalJacobian + * @author Frank Dellaert + * @date Nov 28, 2014 + **/ + +#include +#include +#include + +using namespace std; +using namespace gtsam; + +//****************************************************************************** +TEST( OptionalJacobian, Constructors ) { + Matrix23 fixed; + + OptionalJacobian<2, 3> H1; + EXPECT(!H1); + + OptionalJacobian<2, 3> H2(fixed); + EXPECT(H2); + + OptionalJacobian<2, 3> H3(&fixed); + EXPECT(H3); + + Matrix dynamic; + OptionalJacobian<2, 3> H4(dynamic); + EXPECT(H4); + + OptionalJacobian<2, 3> H5(boost::none); + EXPECT(!H5); + + boost::optional optional(dynamic); + OptionalJacobian<2, 3> H6(optional); + EXPECT(H6); +} + +//****************************************************************************** +void test(OptionalJacobian<2, 3> H = boost::none) { + if (H) + *H = Matrix23::Zero(); +} + +void testPtr(Matrix23* H = NULL) { + if (H) + *H = Matrix23::Zero(); +} + +TEST( OptionalJacobian, Ref2) { + + Matrix expected; + expected = Matrix23::Zero(); + + // Default argument does nothing + test(); + + // Fixed size, no copy + Matrix23 fixed1; + fixed1.setOnes(); + test(fixed1); + EXPECT(assert_equal(expected,fixed1)); + + // Fixed size, no copy, pointer style + Matrix23 fixed2; + fixed2.setOnes(); + test(&fixed2); + EXPECT(assert_equal(expected,fixed2)); + + // Empty is no longer a sign we don't want a matrix, we want it resized + Matrix dynamic0; + test(dynamic0); + EXPECT(assert_equal(expected,dynamic0)); + + // Dynamic wrong size + Matrix dynamic1(3, 5); + dynamic1.setOnes(); + test(dynamic1); + EXPECT(assert_equal(expected,dynamic0)); + + // Dynamic right size + Matrix dynamic2(2, 5); + dynamic2.setOnes(); + test(dynamic2); + EXPECT(assert_equal(dynamic2,dynamic0)); +} + +//****************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//****************************************************************************** diff --git a/gtsam/geometry/Cal3Bundler.cpp b/gtsam/geometry/Cal3Bundler.cpp index 8ff728d26..368ae6c98 100644 --- a/gtsam/geometry/Cal3Bundler.cpp +++ b/gtsam/geometry/Cal3Bundler.cpp @@ -34,20 +34,22 @@ Cal3Bundler::Cal3Bundler(double f, double k1, double k2, double u0, double v0) : } /* ************************************************************************* */ -Matrix Cal3Bundler::K() const { +Matrix3 Cal3Bundler::K() const { Matrix3 K; K << f_, 0, u0_, 0, f_, v0_, 0, 0, 1; return K; } /* ************************************************************************* */ -Vector Cal3Bundler::k() const { - return (Vector(4) << k1_, k2_, 0, 0).finished(); +Vector4 Cal3Bundler::k() const { + Vector4 rvalue_; + rvalue_ << k1_, k2_, 0, 0; + return rvalue_; } /* ************************************************************************* */ Vector3 Cal3Bundler::vector() const { - return (Vector(3) << f_, k1_, k2_).finished(); + return Vector3(f_, k1_, k2_); } /* ************************************************************************* */ @@ -64,21 +66,9 @@ bool Cal3Bundler::equals(const Cal3Bundler& K, double tol) const { return true; } -/* ************************************************************************* */ -Point2 Cal3Bundler::uncalibrate(const Point2& p) const { - // r = x^2 + y^2; - // g = (1 + k(1)*r + k(2)*r^2); - // pi(:,i) = g * pn(:,i) - const double x = p.x(), y = p.y(); - const double r = x * x + y * y; - const double g = 1. + (k1_ + k2_ * r) * r; - const double u = g * x, v = g * y; - return Point2(u0_ + f_ * u, v0_ + f_ * v); -} - /* ************************************************************************* */ Point2 Cal3Bundler::uncalibrate(const Point2& p, // - boost::optional Dcal, boost::optional Dp) const { + OptionalJacobian<2, 3> Dcal, OptionalJacobian<2, 2> Dp) const { // r = x^2 + y^2; // g = (1 + k(1)*r + k(2)*r^2); // pi(:,i) = g * pn(:,i) @@ -103,35 +93,6 @@ Point2 Cal3Bundler::uncalibrate(const Point2& p, // return Point2(u0_ + f_ * u, v0_ + f_ * v); } -/* ************************************************************************* */ -Point2 Cal3Bundler::uncalibrate(const Point2& p, // - boost::optional Dcal, boost::optional Dp) const { - // r = x^2 + y^2; - // g = (1 + k(1)*r + k(2)*r^2); - // pi(:,i) = g * pn(:,i) - const double x = p.x(), y = p.y(); - const double r = x * x + y * y; - const double g = 1. + (k1_ + k2_ * r) * r; - const double u = g * x, v = g * y; - - // Derivatives make use of intermediate variables above - if (Dcal) { - double rx = r * x, ry = r * y; - Dcal->resize(2, 3); - *Dcal << u, f_ * rx, f_ * r * rx, v, f_ * ry, f_ * r * ry; - } - - if (Dp) { - const double a = 2. * (k1_ + 2. * k2_ * r); - const double axx = a * x * x, axy = a * x * y, ayy = a * y * y; - Dp->resize(2,2); - *Dp << g + axx, axy, axy, g + ayy; - *Dp *= f_; - } - - return Point2(u0_ + f_ * u, v0_ + f_ * v); -} - /* ************************************************************************* */ Point2 Cal3Bundler::calibrate(const Point2& pi, const double tol) const { // Copied from Cal3DS2 :-( @@ -161,24 +122,25 @@ Point2 Cal3Bundler::calibrate(const Point2& pi, const double tol) const { } /* ************************************************************************* */ -Matrix Cal3Bundler::D2d_intrinsic(const Point2& p) const { - Matrix Dp; +Matrix2 Cal3Bundler::D2d_intrinsic(const Point2& p) const { + Matrix2 Dp; uncalibrate(p, boost::none, Dp); return Dp; } /* ************************************************************************* */ -Matrix Cal3Bundler::D2d_calibration(const Point2& p) const { - Matrix Dcal; +Matrix23 Cal3Bundler::D2d_calibration(const Point2& p) const { + Matrix23 Dcal; uncalibrate(p, Dcal, boost::none); return Dcal; } /* ************************************************************************* */ -Matrix Cal3Bundler::D2d_intrinsic_calibration(const Point2& p) const { - Matrix Dcal, Dp; +Matrix25 Cal3Bundler::D2d_intrinsic_calibration(const Point2& p) const { + Matrix23 Dcal; + Matrix2 Dp; uncalibrate(p, Dcal, Dp); - Matrix H(2, 5); + Matrix25 H; H << Dp, Dcal; return H; } diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h index 4c77fdf23..fc1d63e10 100644 --- a/gtsam/geometry/Cal3Bundler.h +++ b/gtsam/geometry/Cal3Bundler.h @@ -69,8 +69,8 @@ public: /// @name Standard Interface /// @{ - Matrix K() const; ///< Standard 3*3 calibration matrix - Vector k() const; ///< Radial distortion parameters (4 of them, 2 0) + Matrix3 K() const; ///< Standard 3*3 calibration matrix + Vector4 k() const; ///< Radial distortion parameters (4 of them, 2 0) Vector3 vector() const; @@ -106,43 +106,27 @@ public: /** - * convert intrinsic coordinates xy to image coordinates uv - * @param p point in intrinsic coordinates - * @return point in image coordinates - */ - Point2 uncalibrate(const Point2& p) const; - - /** - * Version of uncalibrate with fixed derivatives + * @brief: convert intrinsic coordinates xy to image coordinates uv + * Version of uncalibrate with derivatives * @param p point in intrinsic coordinates * @param Dcal optional 2*3 Jacobian wrpt CalBundler parameters * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates * @return point in image coordinates */ - Point2 uncalibrate(const Point2& p, boost::optional Dcal, - boost::optional Dp) const; - - /** - * Version of uncalibrate with dynamic derivatives - * @param p point in intrinsic coordinates - * @param Dcal optional 2*3 Jacobian wrpt CalBundler parameters - * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates - * @return point in image coordinates - */ - Point2 uncalibrate(const Point2& p, boost::optional Dcal, - boost::optional Dp) const; + Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 3> Dcal = boost::none, + OptionalJacobian<2, 2> Dp = boost::none) const; /// Conver a pixel coordinate to ideal coordinate Point2 calibrate(const Point2& pi, const double tol = 1e-5) const; /// @deprecated might be removed in next release, use uncalibrate - Matrix D2d_intrinsic(const Point2& p) const; + Matrix2 D2d_intrinsic(const Point2& p) const; /// @deprecated might be removed in next release, use uncalibrate - Matrix D2d_calibration(const Point2& p) const; + Matrix23 D2d_calibration(const Point2& p) const; /// @deprecated might be removed in next release, use uncalibrate - Matrix D2d_intrinsic_calibration(const Point2& p) const; + Matrix25 D2d_intrinsic_calibration(const Point2& p) const; /// @} /// @name Manifold diff --git a/gtsam/geometry/Cal3DS2_Base.cpp b/gtsam/geometry/Cal3DS2_Base.cpp index 1105fecfb..12060c12d 100644 --- a/gtsam/geometry/Cal3DS2_Base.cpp +++ b/gtsam/geometry/Cal3DS2_Base.cpp @@ -28,18 +28,22 @@ Cal3DS2_Base::Cal3DS2_Base(const Vector &v): fx_(v[0]), fy_(v[1]), s_(v[2]), u0_(v[3]), v0_(v[4]), k1_(v[5]), k2_(v[6]), p1_(v[7]), p2_(v[8]){} /* ************************************************************************* */ -Matrix Cal3DS2_Base::K() const { - return (Matrix(3, 3) << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0).finished(); +Matrix3 Cal3DS2_Base::K() const { + Matrix3 K; + K << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0; + return K; } /* ************************************************************************* */ -Vector Cal3DS2_Base::vector() const { - return (Vector(9) << fx_, fy_, s_, u0_, v0_, k1_, k2_, p1_, p2_).finished(); +Vector9 Cal3DS2_Base::vector() const { + Vector9 v; + v << fx_, fy_, s_, u0_, v0_, k1_, k2_, p1_, p2_; + return v; } /* ************************************************************************* */ void Cal3DS2_Base::print(const std::string& s_) const { - gtsam::print(K(), s_ + ".K"); + gtsam::print((Matrix)K(), s_ + ".K"); gtsam::print(Vector(k()), s_ + ".k"); } @@ -91,8 +95,7 @@ static Matrix2 D2dintrinsic(double x, double y, double rr, /* ************************************************************************* */ Point2 Cal3DS2_Base::uncalibrate(const Point2& p, - boost::optional H1, - boost::optional H2) const { + OptionalJacobian<2,9> H1, OptionalJacobian<2,2> H2) const { // rr = x^2 + y^2; // g = (1 + k(1)*rr + k(2)*rr^2); @@ -126,26 +129,6 @@ Point2 Cal3DS2_Base::uncalibrate(const Point2& p, return Point2(fx_ * pnx + s_ * pny + u0_, fy_ * pny + v0_); } -/* ************************************************************************* */ -Point2 Cal3DS2_Base::uncalibrate(const Point2& p, - boost::optional H1, - boost::optional H2) const { - - if (H1 || H2) { - Matrix29 D1; - Matrix2 D2; - Point2 pu = uncalibrate(p, D1, D2); - if (H1) - *H1 = D1; - if (H2) - *H2 = D2; - return pu; - - } else { - return uncalibrate(p); - } -} - /* ************************************************************************* */ Point2 Cal3DS2_Base::calibrate(const Point2& pi, const double tol) const { // Use the following fixed point iteration to invert the radial distortion. @@ -177,7 +160,7 @@ Point2 Cal3DS2_Base::calibrate(const Point2& pi, const double tol) const { } /* ************************************************************************* */ -Matrix Cal3DS2_Base::D2d_intrinsic(const Point2& p) const { +Matrix2 Cal3DS2_Base::D2d_intrinsic(const Point2& p) const { const double x = p.x(), y = p.y(), xx = x * x, yy = y * y; const double rr = xx + yy; const double r4 = rr * rr; @@ -188,7 +171,7 @@ Matrix Cal3DS2_Base::D2d_intrinsic(const Point2& p) const { } /* ************************************************************************* */ -Matrix Cal3DS2_Base::D2d_calibration(const Point2& p) const { +Matrix29 Cal3DS2_Base::D2d_calibration(const Point2& p) const { const double x = p.x(), y = p.y(), xx = x * x, yy = y * y, xy = x * y; const double rr = xx + yy; const double r4 = rr * rr; diff --git a/gtsam/geometry/Cal3DS2_Base.h b/gtsam/geometry/Cal3DS2_Base.h index 61c01e349..37c156743 100644 --- a/gtsam/geometry/Cal3DS2_Base.h +++ b/gtsam/geometry/Cal3DS2_Base.h @@ -45,9 +45,9 @@ protected: double p1_, p2_ ; // tangential distortion public: - Matrix K() const ; - Eigen::Vector4d k() const { return Eigen::Vector4d(k1_, k2_, p1_, p2_); } - Vector vector() const ; + Matrix3 K() const ; + Vector4 k() const { return Vector4(k1_, k2_, p1_, p2_); } + Vector9 vector() const ; /// @name Standard Constructors /// @{ @@ -113,23 +113,18 @@ public: * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates * @return point in (distorted) image coordinates */ - Point2 uncalibrate(const Point2& p, - boost::optional Dcal = boost::none, - boost::optional Dp = boost::none) const ; - - Point2 uncalibrate(const Point2& p, - boost::optional Dcal, - boost::optional Dp) const ; + OptionalJacobian<2,9> Dcal = boost::none, + OptionalJacobian<2,2> Dp = boost::none) const ; /// Convert (distorted) image coordinates uv to intrinsic coordinates xy Point2 calibrate(const Point2& p, const double tol=1e-5) const; /// Derivative of uncalibrate wrpt intrinsic coordinates - Matrix D2d_intrinsic(const Point2& p) const ; + Matrix2 D2d_intrinsic(const Point2& p) const ; /// Derivative of uncalibrate wrpt the calibration parameters - Matrix D2d_calibration(const Point2& p) const ; + Matrix29 D2d_calibration(const Point2& p) const ; private: diff --git a/gtsam/geometry/Cal3Unified.cpp b/gtsam/geometry/Cal3Unified.cpp index 6bc4c4bb3..8b7c1abf4 100644 --- a/gtsam/geometry/Cal3Unified.cpp +++ b/gtsam/geometry/Cal3Unified.cpp @@ -29,8 +29,10 @@ Cal3Unified::Cal3Unified(const Vector &v): Base(v[0], v[1], v[2], v[3], v[4], v[5], v[6], v[7], v[8]), xi_(v[9]) {} /* ************************************************************************* */ -Vector Cal3Unified::vector() const { - return (Vector(10) << Base::vector(), xi_).finished(); +Vector10 Cal3Unified::vector() const { + Vector10 v; + v << Base::vector(), xi_; + return v; } /* ************************************************************************* */ @@ -52,8 +54,8 @@ bool Cal3Unified::equals(const Cal3Unified& K, double tol) const { /* ************************************************************************* */ // todo: make a fixed sized jacobian version of this Point2 Cal3Unified::uncalibrate(const Point2& p, - boost::optional H1, - boost::optional H2) const { + OptionalJacobian<2,10> H1, + OptionalJacobian<2,2> H2) const { // this part of code is modified from Cal3DS2, // since the second part of this model (after project to normalized plane) @@ -81,10 +83,7 @@ Point2 Cal3Unified::uncalibrate(const Point2& p, Vector2 DU; DU << -xs * sqrt_nx * xi_sqrt_nx2, // -ys * sqrt_nx * xi_sqrt_nx2; - - H1->resize(2,10); - H1->block<2,9>(0,0) = H1base; - H1->block<2,1>(0,9) = H2base * DU; + *H1 << H1base, H2base * DU; } // Inlined derivative for points @@ -96,7 +95,7 @@ Point2 Cal3Unified::uncalibrate(const Point2& p, DU << (sqrt_nx + xi*(ys*ys + 1)) * denom, mid, // mid, (sqrt_nx + xi*(xs*xs + 1)) * denom; - *H2 = H2base * DU; + *H2 << H2base * DU; } return puncalib; @@ -136,7 +135,7 @@ Cal3Unified Cal3Unified::retract(const Vector& d) const { } /* ************************************************************************* */ -Vector Cal3Unified::localCoordinates(const Cal3Unified& T2) const { +Vector10 Cal3Unified::localCoordinates(const Cal3Unified& T2) const { return T2.vector() - vector(); } diff --git a/gtsam/geometry/Cal3Unified.h b/gtsam/geometry/Cal3Unified.h index f65b27780..e2aa3fc8b 100644 --- a/gtsam/geometry/Cal3Unified.h +++ b/gtsam/geometry/Cal3Unified.h @@ -51,7 +51,7 @@ private: public: - Vector vector() const ; + Vector10 vector() const ; /// @name Standard Constructors /// @{ @@ -96,8 +96,8 @@ public: * @return point in image coordinates */ Point2 uncalibrate(const Point2& p, - boost::optional Dcal = boost::none, - boost::optional Dp = boost::none) const ; + OptionalJacobian<2,10> Dcal = boost::none, + OptionalJacobian<2,2> Dp = boost::none) const ; /// Conver a pixel coordinate to ideal coordinate Point2 calibrate(const Point2& p, const double tol=1e-5) const; @@ -116,7 +116,7 @@ public: Cal3Unified retract(const Vector& d) const ; /// Given a different calibration, calculate update to obtain it - Vector localCoordinates(const Cal3Unified& T2) const ; + Vector10 localCoordinates(const Cal3Unified& T2) const ; /// Return dimensions of calibration manifold object virtual size_t dim() const { return 10 ; } //TODO: make a final dimension variable (also, usually size_t in other classes e.g. Pose2) diff --git a/gtsam/geometry/Cal3_S2.cpp b/gtsam/geometry/Cal3_S2.cpp index aece1ded1..3ec29bbd2 100644 --- a/gtsam/geometry/Cal3_S2.cpp +++ b/gtsam/geometry/Cal3_S2.cpp @@ -53,7 +53,7 @@ Cal3_S2::Cal3_S2(const std::string &path) : /* ************************************************************************* */ void Cal3_S2::print(const std::string& s) const { - gtsam::print(matrix(), s); + gtsam::print((Matrix)matrix(), s); } /* ************************************************************************* */ @@ -72,32 +72,13 @@ bool Cal3_S2::equals(const Cal3_S2& K, double tol) const { } /* ************************************************************************* */ -Point2 Cal3_S2::uncalibrate(const Point2& p, boost::optional Dcal, - boost::optional Dp) const { +Point2 Cal3_S2::uncalibrate(const Point2& p, OptionalJacobian<2, 5> Dcal, + OptionalJacobian<2, 2> Dp) const { const double x = p.x(), y = p.y(); - if (Dcal) { - Dcal->resize(2,5); + if (Dcal) *Dcal << x, 0.0, y, 1.0, 0.0, 0.0, y, 0.0, 0.0, 1.0; - } - if (Dp) { - Dp->resize(2,2); + if (Dp) *Dp << fx_, s_, 0.0, fy_; - } - return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_); -} - -/* ************************************************************************* */ -Point2 Cal3_S2::uncalibrate(const Point2& p, boost::optional Dcal, - boost::optional Dp) const { - const double x = p.x(), y = p.y(); - if (Dcal) *Dcal << x, 0.0, y, 1.0, 0.0, 0.0, y, 0.0, 0.0, 1.0; - if (Dp) *Dp << fx_, s_, 0.0, fy_; - return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_); -} - -/* ************************************************************************* */ -Point2 Cal3_S2::uncalibrate(const Point2& p) const { - const double x = p.x(), y = p.y(); return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_); } diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index 6317d251d..e6b56eea0 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/gtsam/geometry/Cal3_S2.h @@ -117,37 +117,33 @@ public: } /// vectorized form (column-wise) - Vector vector() const { - double r[] = { fx_, fy_, s_, u0_, v0_ }; - Vector v(5); - std::copy(r, r + 5, v.data()); + Vector5 vector() const { + Vector5 v; + v << fx_, fy_, s_, u0_, v0_; return v; } /// return calibration matrix K - Matrix K() const { - return (Matrix(3, 3) << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0).finished(); + Matrix3 K() const { + Matrix3 K; + K << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0; + return K; } /** @deprecated The following function has been deprecated, use K above */ - Matrix matrix() const { + Matrix3 matrix() const { return K(); } /// return inverted calibration matrix inv(K) - Matrix matrix_inverse() const { + Matrix3 matrix_inverse() const { const double fxy = fx_ * fy_, sv0 = s_ * v0_, fyu0 = fy_ * u0_; - return (Matrix(3, 3) << 1.0 / fx_, -s_ / fxy, (sv0 - fyu0) / fxy, 0.0, - 1.0 / fy_, -v0_ / fy_, 0.0, 0.0, 1.0).finished(); + Matrix3 K_inverse; + K_inverse << 1.0 / fx_, -s_ / fxy, (sv0 - fyu0) / fxy, 0.0, + 1.0 / fy_, -v0_ / fy_, 0.0, 0.0, 1.0; + return K_inverse; } - /** - * convert intrinsic coordinates xy to image coordinates uv - * @param p point in intrinsic coordinates - * @return point in image coordinates - */ - Point2 uncalibrate(const Point2& p) const; - /** * convert intrinsic coordinates xy to image coordinates uv, fixed derivaitves * @param p point in intrinsic coordinates @@ -155,18 +151,8 @@ public: * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates * @return point in image coordinates */ - Point2 uncalibrate(const Point2& p, boost::optional Dcal, - boost::optional Dp) const; - - /** - * convert intrinsic coordinates xy to image coordinates uv, dynamic derivaitves - * @param p point in intrinsic coordinates - * @param Dcal optional 2*5 Jacobian wrpt Cal3_S2 parameters - * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates - * @return point in image coordinates - */ - Point2 uncalibrate(const Point2& p, boost::optional Dcal, - boost::optional Dp) const; + Point2 uncalibrate(const Point2& p, OptionalJacobian<2,5> Dcal = boost::none, + OptionalJacobian<2,2> Dp = boost::none) const; /** * convert image coordinates uv to intrinsic coordinates xy @@ -184,10 +170,10 @@ public: /// "Between", subtracts calibrations. between(p,q) == compose(inverse(p),q) inline Cal3_S2 between(const Cal3_S2& q, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const { - if(H1) *H1 = -eye(5); - if(H2) *H2 = eye(5); + OptionalJacobian<5,5> H1=boost::none, + OptionalJacobian<5,5> H2=boost::none) const { + if(H1) *H1 = -I_5x5; + if(H2) *H2 = I_5x5; return Cal3_S2(q.fx_-fx_, q.fy_-fy_, q.s_-s_, q.u0_-u0_, q.v0_-v0_); } @@ -212,7 +198,7 @@ public: } /// Unretraction for the calibration - Vector localCoordinates(const Cal3_S2& T2) const { + Vector5 localCoordinates(const Cal3_S2& T2) const { return T2.vector() - vector(); } diff --git a/gtsam/geometry/Cal3_S2Stereo.h b/gtsam/geometry/Cal3_S2Stereo.h index b47153547..68732ea8e 100644 --- a/gtsam/geometry/Cal3_S2Stereo.h +++ b/gtsam/geometry/Cal3_S2Stereo.h @@ -103,6 +103,38 @@ namespace gtsam { /// return baseline inline double baseline() const { return b_; } + /// vectorized form (column-wise) + Vector6 vector() const { + Vector6 v; + v << K_.vector(), b_; + return v; + } + + /// @} + /// @name Manifold + /// @{ + + /// return DOF, dimensionality of tangent space + inline size_t dim() const { + return 6; + } + + /// return DOF, dimensionality of tangent space + static size_t Dim() { + return 6; + } + + /// Given 6-dim tangent vector, create new calibration + inline Cal3_S2Stereo retract(const Vector& d) const { + return Cal3_S2Stereo(K_.fx() + d(0), K_.fy() + d(1), K_.skew() + d(2), K_.px() + d(3), K_.py() + d(4), b_ + d(5)); + } + + /// Unretraction for the calibration + Vector6 localCoordinates(const Cal3_S2Stereo& T2) const { + return T2.vector() - vector(); + } + + /// @} /// @name Advanced Interface /// @{ @@ -119,4 +151,23 @@ namespace gtsam { /// @} }; + + // Define GTSAM traits + namespace traits { + + template<> + struct GTSAM_EXPORT is_manifold : public boost::true_type{ + }; + + template<> + struct GTSAM_EXPORT dimension : public boost::integral_constant{ + }; + + template<> + struct GTSAM_EXPORT zero { + static Cal3_S2Stereo value() { return Cal3_S2Stereo();} + }; + + } + } // \ namespace gtsam diff --git a/gtsam/geometry/CalibratedCamera.cpp b/gtsam/geometry/CalibratedCamera.cpp index 392a53858..1f5f1f8a5 100644 --- a/gtsam/geometry/CalibratedCamera.cpp +++ b/gtsam/geometry/CalibratedCamera.cpp @@ -33,10 +33,10 @@ CalibratedCamera::CalibratedCamera(const Vector &v) : /* ************************************************************************* */ Point2 CalibratedCamera::project_to_camera(const Point3& P, - boost::optional H1) { + OptionalJacobian<2,3> H1) { if (H1) { double d = 1.0 / P.z(), d2 = d * d; - *H1 = (Matrix(2, 3) << d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2).finished(); + *H1 << d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2; } return Point2(P.x() / P.z(), P.y() / P.z()); } @@ -59,10 +59,12 @@ CalibratedCamera CalibratedCamera::Level(const Pose2& pose2, double height) { /* ************************************************************************* */ Point2 CalibratedCamera::project(const Point3& point, - boost::optional Dpose, boost::optional Dpoint) const { + OptionalJacobian<2,6> Dpose, OptionalJacobian<2,3> Dpoint) const { #ifdef CALIBRATEDCAMERA_CHAIN_RULE - Point3 q = pose_.transform_to(point, Dpose, Dpoint); + Matrix36 Dpose_; + Matrix3 Dpoint_; + Point3 q = pose_.transform_to(point, Dpose ? Dpose_ : 0, Dpoint ? Dpoint_ : 0); #else Point3 q = pose_.transform_to(point); #endif @@ -75,23 +77,26 @@ Point2 CalibratedCamera::project(const Point3& point, if (Dpose || Dpoint) { #ifdef CALIBRATEDCAMERA_CHAIN_RULE // just implement chain rule - Matrix H; - project_to_camera(q,H); - if (Dpose) *Dpose = H * (*Dpose); - if (Dpoint) *Dpoint = H * (*Dpoint); + if(Dpose && Dpoint) { + Matrix23 H; + project_to_camera(q,H); + if (Dpose) *Dpose = H * (*Dpose_); + if (Dpoint) *Dpoint = H * (*Dpoint_); + } #else // optimized version, see CalibratedCamera.nb const double z = q.z(), d = 1.0 / z; const double u = intrinsic.x(), v = intrinsic.y(), uv = u * v; if (Dpose) - *Dpose = (Matrix(2, 6) << uv, -(1. + u * u), v, -d, 0., d * u, (1. + v * v), - -uv, -u, 0., -d, d * v).finished(); + *Dpose << uv, -(1. + u * u), v, -d, 0., d * u, (1. + v * v), + -uv, -u, 0., -d, d * v; if (Dpoint) { - const Matrix R(pose_.rotation().matrix()); - *Dpoint = d - * (Matrix(2, 3) << R(0, 0) - u * R(0, 2), R(1, 0) - u * R(1, 2), + const Matrix3 R(pose_.rotation().matrix()); + Matrix23 Dpoint_; + Dpoint_ << R(0, 0) - u * R(0, 2), R(1, 0) - u * R(1, 2), R(2, 0) - u * R(2, 2), R(0, 1) - v * R(0, 2), - R(1, 1) - v * R(1, 2), R(2, 1) - v * R(2, 2)).finished(); + R(1, 1) - v * R(1, 2), R(2, 1) - v * R(2, 2); + *Dpoint = d * Dpoint_; } #endif } diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index c66941091..f5a8b4469 100644 --- a/gtsam/geometry/CalibratedCamera.h +++ b/gtsam/geometry/CalibratedCamera.h @@ -18,9 +18,8 @@ #pragma once -#include -#include #include +#include namespace gtsam { @@ -88,26 +87,6 @@ public: return pose_; } - /// compose the two camera poses: TODO Frank says this might not make sense - inline const CalibratedCamera compose(const CalibratedCamera &c, - boost::optional H1 = boost::none, boost::optional H2 = - boost::none) const { - return CalibratedCamera(pose_.compose(c.pose(), H1, H2)); - } - - /// between the two camera poses: TODO Frank says this might not make sense - inline const CalibratedCamera between(const CalibratedCamera& c, - boost::optional H1 = boost::none, boost::optional H2 = - boost::none) const { - return CalibratedCamera(pose_.between(c.pose(), H1, H2)); - } - - /// invert the camera pose: TODO Frank says this might not make sense - inline const CalibratedCamera inverse(boost::optional H1 = - boost::none) const { - return CalibratedCamera(pose_.inverse(H1)); - } - /** * Create a level camera at the given 2D pose and height * @param pose2 specifies the location and viewing direction @@ -152,8 +131,8 @@ public: * @return the intrinsic coordinates of the projected point */ Point2 project(const Point3& point, - boost::optional Dpose = boost::none, - boost::optional Dpoint = boost::none) const; + OptionalJacobian<2, 6> Dpose = boost::none, + OptionalJacobian<2, 3> Dpoint = boost::none) const; /** * projects a 3-dimensional point in camera coordinates into the @@ -161,7 +140,7 @@ public: * With optional 2by3 derivative */ static Point2 project_to_camera(const Point3& cameraPoint, - boost::optional H1 = boost::none); + OptionalJacobian<2, 3> H1 = boost::none); /** * backproject a 2-dimensional point to a 3-dimension point @@ -175,8 +154,8 @@ public: * @param H2 optionally computed Jacobian with respect to the 3D point * @return range (double) */ - double range(const Point3& point, boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const { + double range(const Point3& point, OptionalJacobian<1, 6> H1 = boost::none, + OptionalJacobian<1, 3> H2 = boost::none) const { return pose_.range(point, H1, H2); } @@ -187,8 +166,8 @@ public: * @param H2 optionally computed Jacobian with respect to the 3D point * @return range (double) */ - double range(const Pose3& pose, boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const { + double range(const Pose3& pose, OptionalJacobian<1, 6> H1 = boost::none, + OptionalJacobian<1, 6> H2 = boost::none) const { return pose_.range(pose, H1, H2); } @@ -199,8 +178,8 @@ public: * @param H2 optionally computed Jacobian with respect to the 3D point * @return range (double) */ - double range(const CalibratedCamera& camera, boost::optional H1 = - boost::none, boost::optional H2 = boost::none) const { + double range(const CalibratedCamera& camera, OptionalJacobian<1, 6> H1 = + boost::none, OptionalJacobian<1, 6> H2 = boost::none) const { return pose_.range(camera.pose_, H1, H2); } @@ -224,15 +203,16 @@ private: namespace traits { template<> -struct GTSAM_EXPORT is_group : public boost::true_type{ +struct GTSAM_EXPORT is_group : public boost::true_type { }; template<> -struct GTSAM_EXPORT is_manifold : public boost::true_type{ +struct GTSAM_EXPORT is_manifold : public boost::true_type { }; template<> -struct GTSAM_EXPORT dimension : public boost::integral_constant{ +struct GTSAM_EXPORT dimension : public boost::integral_constant< + int, 6> { }; } diff --git a/gtsam/geometry/EssentialMatrix.cpp b/gtsam/geometry/EssentialMatrix.cpp index e65e5d097..062178fea 100644 --- a/gtsam/geometry/EssentialMatrix.cpp +++ b/gtsam/geometry/EssentialMatrix.cpp @@ -14,7 +14,7 @@ namespace gtsam { /* ************************************************************************* */ EssentialMatrix EssentialMatrix::FromPose3(const Pose3& _1P2_, - boost::optional H) { + OptionalJacobian<5, 6> H) { const Rot3& _1R2_ = _1P2_.rotation(); const Point3& _1T2_ = _1P2_.translation(); if (!H) { @@ -25,13 +25,10 @@ EssentialMatrix EssentialMatrix::FromPose3(const Pose3& _1P2_, // Calculate the 5*6 Jacobian H = D_E_1P2 // D_E_1P2 = [D_E_1R2 D_E_1T2], 5*3 wrpt rotation, 5*3 wrpt translation // First get 2*3 derivative from Unit3::FromPoint3 - Matrix D_direction_1T2; + Matrix23 D_direction_1T2; Unit3 direction = Unit3::FromPoint3(_1T2_, D_direction_1T2); - H->resize(5, 6); - H->block<3, 3>(0, 0) << Matrix::Identity(3, 3); // upper left - H->block<2, 3>(3, 0) << Matrix::Zero(2, 3); // lower left - H->block<3, 3>(0, 3) << Matrix::Zero(3, 3); // upper right - H->block<2, 3>(3, 3) << D_direction_1T2 * _1R2_.matrix(); // lower right + *H << I_3x3, Z_3x3, // + Matrix23::Zero(), D_direction_1T2 * _1R2_.matrix(); return EssentialMatrix(_1R2_, direction); } } @@ -54,23 +51,26 @@ EssentialMatrix EssentialMatrix::retract(const Vector& xi) const { } /* ************************************************************************* */ -Vector EssentialMatrix::localCoordinates(const EssentialMatrix& other) const { - return (Vector(5) << - aRb_.localCoordinates(other.aRb_), aTb_.localCoordinates(other.aTb_)).finished(); +Vector5 EssentialMatrix::localCoordinates(const EssentialMatrix& other) const { + Vector5 v; + v << aRb_.localCoordinates(other.aRb_), + aTb_.localCoordinates(other.aTb_); + return v; } /* ************************************************************************* */ -Point3 EssentialMatrix::transform_to(const Point3& p, - boost::optional DE, boost::optional Dpoint) const { +Point3 EssentialMatrix::transform_to(const Point3& p, OptionalJacobian<3, 5> DE, + OptionalJacobian<3, 3> Dpoint) const { Pose3 pose(aRb_, aTb_.point3()); - Point3 q = pose.transform_to(p, DE, Dpoint); + Matrix36 DE_; + Point3 q = pose.transform_to(p, DE ? &DE_ : 0, Dpoint); if (DE) { // DE returned by pose.transform_to is 3*6, but we need it to be 3*5 // The last 3 columns are derivative with respect to change in translation // The derivative of translation with respect to a 2D sphere delta is 3*2 aTb_.basis() // Duy made an educated guess that this needs to be rotated to the local frame - Matrix H(3, 5); - H << DE->block<3, 3>(0, 0), -aRb_.transpose() * aTb_.basis(); + Matrix35 H; + H << DE_.block < 3, 3 > (0, 0), -aRb_.transpose() * aTb_.basis(); *DE = H; } return q; @@ -78,7 +78,7 @@ Point3 EssentialMatrix::transform_to(const Point3& p, /* ************************************************************************* */ EssentialMatrix EssentialMatrix::rotate(const Rot3& cRb, - boost::optional HE, boost::optional HR) const { + OptionalJacobian<5, 5> HE, OptionalJacobian<5, 3> HR) const { // The rotation must be conjugated to act in the camera frame Rot3 c1Rc2 = aRb_.conjugate(cRb); @@ -89,18 +89,16 @@ EssentialMatrix EssentialMatrix::rotate(const Rot3& cRb, return EssentialMatrix(c1Rc2, c1Tc2); } else { // Calculate derivatives - Matrix D_c1Tc2_cRb, D_c1Tc2_aTb; // 2*3 and 2*2 + Matrix23 D_c1Tc2_cRb; // 2*3 + Matrix2 D_c1Tc2_aTb; // 2*2 Unit3 c1Tc2 = cRb.rotate(aTb_, D_c1Tc2_cRb, D_c1Tc2_aTb); - if (HE) { - *HE = zeros(5, 5); - HE->block<3, 3>(0, 0) << cRb.matrix(); // a change in aRb_ will yield a rotated change in c1Rc2 - HE->block<2, 2>(3, 3) << D_c1Tc2_aTb; // (2*2) - } + if (HE) + *HE << cRb.matrix(), Matrix32::Zero(), // + Matrix23::Zero(), D_c1Tc2_aTb; if (HR) { throw runtime_error( "EssentialMatrix::rotate: derivative HR not implemented yet"); /* - HR->resize(5, 3); HR->block<3, 3>(0, 0) << zeros(3, 3); // a change in the rotation yields ? HR->block<2, 3>(3, 0) << zeros(2, 3); // (2*3) * (3*3) ? */ @@ -110,13 +108,12 @@ EssentialMatrix EssentialMatrix::rotate(const Rot3& cRb, } /* ************************************************************************* */ -double EssentialMatrix::error(const Vector& vA, const Vector& vB, // - boost::optional H) const { +double EssentialMatrix::error(const Vector3& vA, const Vector3& vB, // + OptionalJacobian<1, 5> H) const { if (H) { - H->resize(1, 5); // See math.lyx - Matrix HR = vA.transpose() * E_ * skewSymmetric(-vB); - Matrix HD = vA.transpose() * skewSymmetric(-aRb_.matrix() * vB) + Matrix13 HR = vA.transpose() * E_ * skewSymmetric(-vB); + Matrix12 HD = vA.transpose() * skewSymmetric(-aRb_.matrix() * vB) * aTb_.basis(); *H << HR, HD; } diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h index fd5f45160..546e432b9 100644 --- a/gtsam/geometry/EssentialMatrix.h +++ b/gtsam/geometry/EssentialMatrix.h @@ -31,8 +31,8 @@ private: public: /// Static function to convert Point2 to homogeneous coordinates - static Vector Homogeneous(const Point2& p) { - return (Vector(3) << p.x(), p.y(), 1).finished(); + static Vector3 Homogeneous(const Point2& p) { + return Vector3(p.x(), p.y(), 1); } /// @name Constructors and named constructors @@ -50,7 +50,7 @@ public: /// Named constructor converting a Pose3 with scale to EssentialMatrix (no scale) static EssentialMatrix FromPose3(const Pose3& _1P2_, - boost::optional H = boost::none); + OptionalJacobian<5, 6> H = boost::none); /// Random, using Rot3::Random and Unit3::Random template @@ -84,15 +84,15 @@ public: } /// Return the dimensionality of the tangent space - virtual size_t dim() const { + size_t dim() const { return 5; } /// Retract delta to manifold - virtual EssentialMatrix retract(const Vector& xi) const; + EssentialMatrix retract(const Vector& xi) const; /// Compute the coordinates in the tangent space - virtual Vector localCoordinates(const EssentialMatrix& other) const; + Vector5 localCoordinates(const EssentialMatrix& other) const; /// @} @@ -132,16 +132,16 @@ public: * @return point in pose coordinates */ Point3 transform_to(const Point3& p, - boost::optional DE = boost::none, - boost::optional Dpoint = boost::none) const; + OptionalJacobian<3,5> DE = boost::none, + OptionalJacobian<3,3> Dpoint = boost::none) const; /** * Given essential matrix E in camera frame B, convert to body frame C * @param cRb rotation from body frame to camera frame * @param E essential matrix E in camera frame C */ - EssentialMatrix rotate(const Rot3& cRb, boost::optional HE = - boost::none, boost::optional HR = boost::none) const; + EssentialMatrix rotate(const Rot3& cRb, OptionalJacobian<5, 5> HE = + boost::none, OptionalJacobian<5, 3> HR = boost::none) const; /** * Given essential matrix E in camera frame B, convert to body frame C @@ -153,8 +153,8 @@ public: } /// epipolar error, algebraic - double error(const Vector& vA, const Vector& vB, // - boost::optional H = boost::none) const; + double error(const Vector3& vA, const Vector3& vB, // + OptionalJacobian<1,5> H = boost::none) const; /// @} diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index c5174ae65..9b881dc78 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -18,16 +18,9 @@ #pragma once -#include -#include -#include -#include -#include -#include -#include -#include -#include #include +#include +#include namespace gtsam { @@ -42,7 +35,9 @@ private: Pose3 pose_; Calibration K_; - static const int DimK = traits::dimension::value; + // Get dimensions of calibration type and This at compile time + static const int DimK = traits::dimension::value, // + DimC = 6 + DimK; public: @@ -114,9 +109,9 @@ public: /// @{ explicit PinholeCamera(const Vector &v) { - pose_ = Pose3::Expmap(v.head(Pose3::Dim())); - if (v.size() > Pose3::Dim()) { - K_ = Calibration(v.tail(Calibration::Dim())); + pose_ = Pose3::Expmap(v.head(6)); + if (v.size() > 6) { + K_ = Calibration(v.tail(DimK)); } } @@ -167,82 +162,30 @@ public: return K_; } - /// @} - /// @name Group ?? Frank says this might not make sense - /// @{ - - /// compose two cameras: TODO Frank says this might not make sense - inline const PinholeCamera compose(const PinholeCamera &c, - boost::optional H1 = boost::none, boost::optional H2 = - boost::none) const { - PinholeCamera result(pose_.compose(c.pose(), H1, H2), K_); - if (H1) { - H1->conservativeResize(Dim(), Dim()); - H1->topRightCorner(Pose3::Dim(), Calibration::Dim()) = zeros(Pose3::Dim(), - Calibration::Dim()); - H1->bottomRows(Calibration::Dim()) = zeros(Calibration::Dim(), Dim()); - } - if (H2) { - H2->conservativeResize(Dim(), Dim()); - H2->topRightCorner(Pose3::Dim(), Calibration::Dim()) = zeros(Pose3::Dim(), - Calibration::Dim()); - H2->bottomRows(Calibration::Dim()) = zeros(Calibration::Dim(), Dim()); - } - return result; - } - - /// between two cameras: TODO Frank says this might not make sense - inline const PinholeCamera between(const PinholeCamera& c, - boost::optional H1 = boost::none, boost::optional H2 = - boost::none) const { - PinholeCamera result(pose_.between(c.pose(), H1, H2), K_); - if (H1) { - H1->conservativeResize(Dim(), Dim()); - H1->topRightCorner(Pose3::Dim(), Calibration::Dim()) = zeros(Pose3::Dim(), - Calibration::Dim()); - H1->bottomRows(Calibration::Dim()) = zeros(Calibration::Dim(), Dim()); - } - if (H2) { - H2->conservativeResize(Dim(), Dim()); - H2->topRightCorner(Pose3::Dim(), Calibration::Dim()) = zeros(Pose3::Dim(), - Calibration::Dim()); - H2->bottomRows(Calibration::Dim()) = zeros(Calibration::Dim(), Dim()); - } - return result; - } - - /// inverse camera: TODO Frank says this might not make sense - inline const PinholeCamera inverse( - boost::optional H1 = boost::none) const { - PinholeCamera result(pose_.inverse(H1), K_); - if (H1) { - H1->conservativeResize(Dim(), Dim()); - H1->topRightCorner(Pose3::Dim(), Calibration::Dim()) = zeros(Pose3::Dim(), - Calibration::Dim()); - H1->bottomRows(Calibration::Dim()) = zeros(Calibration::Dim(), Dim()); - } - return result; - } - - /// compose two cameras: TODO Frank says this might not make sense - inline const PinholeCamera compose(const Pose3 &c) const { - return PinholeCamera(pose_.compose(c), K_); - } - /// @} /// @name Manifold /// @{ - /// move a cameras according to d - PinholeCamera retract(const Vector& d) const { - if ((size_t) d.size() == pose_.dim()) - return PinholeCamera(pose().retract(d), calibration()); - else - return PinholeCamera(pose().retract(d.head(pose().dim())), - calibration().retract(d.tail(calibration().dim()))); + /// Manifold dimension + inline size_t dim() const { + return DimC; } - typedef Eigen::Matrix VectorK6; + /// Manifold dimension + inline static size_t Dim() { + return DimC; + } + + typedef Eigen::Matrix VectorK6; + + /// move a cameras according to d + PinholeCamera retract(const Vector& d) const { + if ((size_t) d.size() == 6) + return PinholeCamera(pose().retract(d), calibration()); + else + return PinholeCamera(pose().retract(d.head(6)), + calibration().retract(d.tail(calibration().dim()))); + } /// return canonical coordinate VectorK6 localCoordinates(const PinholeCamera& T2) const { @@ -252,16 +195,6 @@ public: return d; } - /// Manifold dimension - inline size_t dim() const { - return pose_.dim() + K_.dim(); - } - - /// Manifold dimension - inline static size_t Dim() { - return Pose3::Dim() + Calibration::Dim(); - } - /// @} /// @name Transformations and measurement functions /// @{ @@ -272,8 +205,8 @@ public: * @param P A point in camera coordinates * @param Dpoint is the 2*3 Jacobian w.r.t. P */ - inline static Point2 project_to_camera(const Point3& P, - boost::optional Dpoint = boost::none) { + static Point2 project_to_camera(const Point3& P, // + OptionalJacobian<2, 3> Dpoint = boost::none) { #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION if (P.z() <= 0) throw CheiralityException(); @@ -292,21 +225,7 @@ public: return std::make_pair(K_.uncalibrate(pn), pc.z() > 0); } - /** project a point from world coordinate to the image - * @param pw is a point in world coordinates - */ - inline Point2 project(const Point3& pw) const { - - // Transform to camera coordinates and check cheirality - const Point3 pc = pose_.transform_to(pw); - - // Project to normalized image coordinates - const Point2 pn = project_to_camera(pc); - - return K_.uncalibrate(pn); - } - - typedef Eigen::Matrix Matrix2K; + typedef Eigen::Matrix Matrix2K; /** project a point from world coordinate to the image * @param pw is a point in world coordinates @@ -314,11 +233,9 @@ public: * @param Dpoint is the Jacobian w.r.t. point3 * @param Dcal is the Jacobian w.r.t. calibration */ - inline Point2 project( - const Point3& pw, // - boost::optional Dpose, - boost::optional Dpoint, - boost::optional Dcal) const { + inline Point2 project(const Point3& pw, OptionalJacobian<2, 6> Dpose = + boost::none, OptionalJacobian<2, 3> Dpoint = boost::none, + OptionalJacobian<2, DimK> Dcal = boost::none) const { // Transform to camera coordinates and check cheirality const Point3 pc = pose_.transform_to(pw); @@ -340,46 +257,7 @@ public: calculateDpoint(pn, d, pose_.rotation().matrix(), Dpi_pn, *Dpoint); return pi; } else - return K_.uncalibrate(pn, Dcal, boost::none); - } - - /** project a point from world coordinate to the image - * @param pw is a point in world coordinates - * @param Dpose is the Jacobian w.r.t. pose3 - * @param Dpoint is the Jacobian w.r.t. point3 - * @param Dcal is the Jacobian w.r.t. calibration - */ - inline Point2 project( - const Point3& pw, // - boost::optional Dpose, - boost::optional Dpoint, - boost::optional Dcal) const { - - // Transform to camera coordinates and check cheirality - const Point3 pc = pose_.transform_to(pw); - - // Project to normalized image coordinates - const Point2 pn = project_to_camera(pc); - - if (Dpose || Dpoint) { - const double z = pc.z(), d = 1.0 / z; - - // uncalibration - Matrix Dpi_pn(2, 2); - const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn); - - // chain the Jacobian matrices - if (Dpose) { - Dpose->resize(2, 6); - calculateDpose(pn, d, Dpi_pn, *Dpose); - } - if (Dpoint) { - Dpoint->resize(2, 3); - calculateDpoint(pn, d, pose_.rotation().matrix(), Dpi_pn, *Dpoint); - } - return pi; - } else - return K_.uncalibrate(pn, Dcal, boost::none); + return K_.uncalibrate(pn, Dcal); } /** project a point at infinity from world coordinate to the image @@ -388,11 +266,10 @@ public: * @param Dpoint is the Jacobian w.r.t. point3 * @param Dcal is the Jacobian w.r.t. calibration */ - inline Point2 projectPointAtInfinity( - const Point3& pw, // - boost::optional Dpose = boost::none, - boost::optional Dpoint = boost::none, - boost::optional Dcal = boost::none) const { + inline Point2 projectPointAtInfinity(const Point3& pw, + OptionalJacobian<2, 6> Dpose = boost::none, + OptionalJacobian<2, 2> Dpoint = boost::none, + OptionalJacobian<2, DimK> Dcal = boost::none) const { if (!Dpose && !Dpoint && !Dcal) { const Point3 pc = pose_.rotation().unrotate(pw); // get direction in camera frame (translation does not matter) @@ -401,40 +278,30 @@ public: } // world to camera coordinate - Matrix Dpc_rot /* 3*3 */, Dpc_point /* 3*3 */; + Matrix3 Dpc_rot, Dpc_point; const Point3 pc = pose_.rotation().unrotate(pw, Dpc_rot, Dpc_point); - Matrix Dpc_pose = Matrix::Zero(3, 6); - Dpc_pose.block(0, 0, 3, 3) = Dpc_rot; + Matrix36 Dpc_pose; + Dpc_pose.setZero(); + Dpc_pose.leftCols<3>() = Dpc_rot; // camera to normalized image coordinate Matrix23 Dpn_pc; // 2*3 const Point2 pn = project_to_camera(pc, Dpn_pc); // uncalibration - Matrix Dpi_pn; // 2*2 + Matrix2 Dpi_pn; // 2*2 const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn); // chain the Jacobian matrices - const Matrix Dpi_pc = Dpi_pn * Dpn_pc; + const Matrix23 Dpi_pc = Dpi_pn * Dpn_pc; if (Dpose) *Dpose = Dpi_pc * Dpc_pose; if (Dpoint) - *Dpoint = (Dpi_pc * Dpc_point).block(0, 0, 2, 2); // only 2dof are important for the point (direction-only) + *Dpoint = (Dpi_pc * Dpc_point).leftCols<2>(); // only 2dof are important for the point (direction-only) return pi; } - typedef Eigen::Matrix Matrix2K6; - - /** project a point from world coordinate to the image - * @param pw is a point in the world coordinate - */ - Point2 project2(const Point3& pw) const { - const Point3 pc = pose_.transform_to(pw); - const Point2 pn = project_to_camera(pc); - return K_.uncalibrate(pn); - } - /** project a point from world coordinate to the image, fixed Jacobians * @param pw is a point in the world coordinate * @param Dcamera is the Jacobian w.r.t. [pose3 calibration] @@ -442,8 +309,8 @@ public: */ Point2 project2( const Point3& pw, // - boost::optional Dcamera, - boost::optional Dpoint) const { + OptionalJacobian<2, DimC> Dcamera = boost::none, + OptionalJacobian<2, 3> Dpoint = boost::none) const { const Point3 pc = pose_.transform_to(pw); const Point2 pn = project_to_camera(pc); @@ -459,8 +326,8 @@ public: const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn); if (Dcamera) { // TODO why does leftCols<6>() not compile ?? - calculateDpose(pn, d, Dpi_pn, Dcamera->leftCols(6)); - Dcamera->rightCols(K_.dim()) = Dcal; // Jacobian wrt calib + calculateDpose(pn, d, Dpi_pn, (*Dcamera).leftCols(6)); + (*Dcamera).rightCols(DimK) = Dcal; // Jacobian wrt calib } if (Dpoint) { calculateDpoint(pn, d, pose_.rotation().matrix(), Dpi_pn, *Dpoint); @@ -469,40 +336,6 @@ public: } } - /** project a point from world coordinate to the image - * @param pw is a point in the world coordinate - * @param Dcamera is the Jacobian w.r.t. [pose3 calibration] - * @param Dpoint is the Jacobian w.r.t. point3 - */ - Point2 project2(const Point3& pw, // - boost::optional Dcamera, boost::optional Dpoint) const { - - const Point3 pc = pose_.transform_to(pw); - const Point2 pn = project_to_camera(pc); - - if (!Dcamera && !Dpoint) { - return K_.uncalibrate(pn); - } else { - const double z = pc.z(), d = 1.0 / z; - - // uncalibration - Matrix2K Dcal; - Matrix2 Dpi_pn; - const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn); - - if (Dcamera) { - Dcamera->resize(2, this->dim()); - calculateDpose(pn, d, Dpi_pn, Dcamera->leftCols<6>()); - Dcamera->rightCols(K_.dim()) = Dcal; // Jacobian wrt calib - } - if (Dpoint) { - Dpoint->resize(2, 3); - calculateDpoint(pn, d, pose_.rotation().matrix(), Dpi_pn, *Dpoint); - } - return pi; - } - } - /// backproject a 2-dimensional point to a 3-dimensional point at given depth inline Point3 backproject(const Point2& p, double depth) const { const Point2 pn = K_.calibrate(p); @@ -520,71 +353,64 @@ public: /** * Calculate range to a landmark * @param point 3D location of landmark - * @param Dpose the optionally computed Jacobian with respect to pose + * @param Dcamera the optionally computed Jacobian with respect to pose * @param Dpoint the optionally computed Jacobian with respect to the landmark * @return range (double) */ double range( const Point3& point, // - boost::optional Dpose = boost::none, - boost::optional Dpoint = boost::none) const { - double result = pose_.range(point, Dpose, Dpoint); - if (Dpose) { - // Add columns of zeros to Jacobian for calibration - Matrix& H1r(*Dpose); - H1r.conservativeResize(Eigen::NoChange, pose_.dim() + K_.dim()); - H1r.block(0, pose_.dim(), 1, K_.dim()) = Matrix::Zero(1, K_.dim()); - } + OptionalJacobian<1, DimC> Dcamera = boost::none, + OptionalJacobian<1, 3> Dpoint = boost::none) const { + Matrix16 Dpose_; + double result = pose_.range(point, Dcamera ? &Dpose_ : 0, Dpoint); + if (Dcamera) + *Dcamera << Dpose_, Eigen::Matrix::Zero(); return result; } /** * Calculate range to another pose * @param pose Other SO(3) pose - * @param Dpose the optionally computed Jacobian with respect to pose + * @param Dcamera the optionally computed Jacobian with respect to pose * @param Dpose2 the optionally computed Jacobian with respect to the other pose * @return range (double) */ double range( const Pose3& pose, // - boost::optional Dpose = boost::none, - boost::optional Dpose2 = boost::none) const { - double result = pose_.range(pose, Dpose, Dpose2); - if (Dpose) { - // Add columns of zeros to Jacobian for calibration - Matrix& H1r(*Dpose); - H1r.conservativeResize(Eigen::NoChange, pose_.dim() + K_.dim()); - H1r.block(0, pose_.dim(), 1, K_.dim()) = Matrix::Zero(1, K_.dim()); - } + OptionalJacobian<1, DimC> Dcamera = boost::none, + OptionalJacobian<1, 6> Dpose = boost::none) const { + Matrix16 Dpose_; + double result = pose_.range(pose, Dcamera ? &Dpose_ : 0, Dpose); + if (Dcamera) + *Dcamera << Dpose_, Eigen::Matrix::Zero(); return result; } /** * Calculate range to another camera * @param camera Other camera - * @param Dpose the optionally computed Jacobian with respect to pose + * @param Dcamera the optionally computed Jacobian with respect to pose * @param Dother the optionally computed Jacobian with respect to the other camera * @return range (double) */ template double range( const PinholeCamera& camera, // - boost::optional Dpose = boost::none, - boost::optional Dother = boost::none) const { - double result = pose_.range(camera.pose_, Dpose, Dother); - if (Dpose) { - // Add columns of zeros to Jacobian for calibration - Matrix& H1r(*Dpose); - H1r.conservativeResize(Eigen::NoChange, pose_.dim() + K_.dim()); - H1r.block(0, pose_.dim(), 1, K_.dim()) = Matrix::Zero(1, K_.dim()); + OptionalJacobian<1, DimC> Dcamera = boost::none, +// OptionalJacobian<1, 6 + traits::dimension::value> Dother = + boost::optional Dother = + boost::none) const { + Matrix16 Dcamera_, Dother_; + double result = pose_.range(camera.pose(), Dcamera ? &Dcamera_ : 0, + Dother ? &Dother_ : 0); + if (Dcamera) { + Dcamera->resize(1, 6 + DimK); + *Dcamera << Dcamera_, Eigen::Matrix::Zero(); } if (Dother) { - // Add columns of zeros to Jacobian for calibration - Matrix& H2r(*Dother); - H2r.conservativeResize(Eigen::NoChange, - camera.pose().dim() + camera.calibration().dim()); - H2r.block(0, camera.pose().dim(), 1, camera.calibration().dim()) = - Matrix::Zero(1, camera.calibration().dim()); + Dother->resize(1, 6+traits::dimension::value); + Dother->setZero(); + Dother->block(0, 0, 1, 6) = Dother_; } return result; } @@ -592,15 +418,15 @@ public: /** * Calculate range to another camera * @param camera Other camera - * @param Dpose the optionally computed Jacobian with respect to pose + * @param Dcamera the optionally computed Jacobian with respect to pose * @param Dother the optionally computed Jacobian with respect to the other camera * @return range (double) */ double range( const CalibratedCamera& camera, // - boost::optional Dpose = boost::none, - boost::optional Dother = boost::none) const { - return pose_.range(camera.pose_, Dpose, Dother); + OptionalJacobian<1, DimC> Dcamera = boost::none, + OptionalJacobian<1, 6> Dother = boost::none) const { + return range(camera.pose(), Dcamera, Dother); } private: @@ -663,7 +489,7 @@ private: namespace traits { template -struct GTSAM_EXPORT is_manifold > : public boost::true_type{ +struct GTSAM_EXPORT is_manifold > : public boost::true_type { }; template diff --git a/gtsam/geometry/Point2.cpp b/gtsam/geometry/Point2.cpp index 8b90aed63..17e3ef370 100644 --- a/gtsam/geometry/Point2.cpp +++ b/gtsam/geometry/Point2.cpp @@ -38,15 +38,9 @@ bool Point2::equals(const Point2& q, double tol) const { } /* ************************************************************************* */ -double Point2::norm() const { - return sqrt(x_ * x_ + y_ * y_); -} - -/* ************************************************************************* */ -double Point2::norm(boost::optional H) const { - double r = norm(); +double Point2::norm(OptionalJacobian<1,2> H) const { + double r = sqrt(x_ * x_ + y_ * y_); if (H) { - H->resize(1,2); if (fabs(r) > 1e-10) *H << x_ / r, y_ / r; else @@ -56,12 +50,11 @@ double Point2::norm(boost::optional H) const { } /* ************************************************************************* */ -static const Matrix I2 = eye(2); -double Point2::distance(const Point2& point, boost::optional H1, - boost::optional H2) const { +double Point2::distance(const Point2& point, OptionalJacobian<1,2> H1, + OptionalJacobian<1,2> H2) const { Point2 d = point - *this; if (H1 || H2) { - Matrix H; + Matrix12 H; double r = d.norm(H); if (H1) *H1 = -H; if (H2) *H2 = H; diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index a4e0cc296..5dabc4bd6 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -20,7 +20,7 @@ #include #include -#include +#include #include namespace gtsam { @@ -125,10 +125,10 @@ public: /// "Compose", just adds the coordinates of two points. With optional derivatives inline Point2 compose(const Point2& q, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const { - if(H1) *H1 = eye(2); - if(H2) *H2 = eye(2); + OptionalJacobian<2,2> H1=boost::none, + OptionalJacobian<2,2> H2=boost::none) const { + if(H1) *H1 = I_2x2; + if(H2) *H2 = I_2x2; return *this + q; } @@ -137,10 +137,10 @@ public: /// "Between", subtracts point coordinates. between(p,q) == compose(inverse(p),q) inline Point2 between(const Point2& q, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const { - if(H1) *H1 = -eye(2); - if(H2) *H2 = eye(2); + OptionalJacobian<2,2> H1=boost::none, + OptionalJacobian<2,2> H2=boost::none) const { + if(H1) *H1 = -I_2x2; + if(H2) *H2 = I_2x2; return q - (*this); } @@ -171,7 +171,7 @@ public: static inline Point2 Expmap(const Vector& v) { return Point2(v); } /// Log map around identity - just return the Point2 as a vector - static inline Vector Logmap(const Point2& dp) { return (Vector(2) << dp.x(), dp.y()).finished(); } + static inline Vector2 Logmap(const Point2& dp) { return Vector2(dp.x(), dp.y()); } /// @} /// @name Vector Space @@ -180,15 +180,12 @@ public: /** creates a unit vector */ Point2 unit() const { return *this/norm(); } - /** norm of point */ - double norm() const; - /** norm of point, with derivative */ - double norm(boost::optional H) const; + double norm(OptionalJacobian<1,2> H = boost::none) const; /** distance between two points */ - double distance(const Point2& p2, boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const; + double distance(const Point2& p2, OptionalJacobian<1,2> H1 = boost::none, + OptionalJacobian<1,2> H2 = boost::none) const; /** @deprecated The following function has been deprecated, use distance above */ inline double dist(const Point2& p2) const { diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp index ce56c78c1..330fafb97 100644 --- a/gtsam/geometry/Point3.cpp +++ b/gtsam/geometry/Point3.cpp @@ -63,22 +63,18 @@ Point3 Point3::operator/(double s) const { } /* ************************************************************************* */ -Point3 Point3::add(const Point3 &q, boost::optional H1, - boost::optional H2) const { - if (H1) - *H1 = eye(3, 3); - if (H2) - *H2 = eye(3, 3); +Point3 Point3::add(const Point3 &q, OptionalJacobian<3,3> H1, + OptionalJacobian<3,3> H2) const { + if (H1) *H1 = I_3x3; + if (H2) *H2 = I_3x3; return *this + q; } /* ************************************************************************* */ -Point3 Point3::sub(const Point3 &q, boost::optional H1, - boost::optional H2) const { - if (H1) - *H1 = eye(3, 3); - if (H2) - *H2 = -eye(3, 3); +Point3 Point3::sub(const Point3 &q, OptionalJacobian<3,3> H1, + OptionalJacobian<3,3> H2) const { + if (H1) *H1 = I_3x3; + if (H2) *H2 = I_3x3; return *this - q; } @@ -94,26 +90,8 @@ double Point3::dot(const Point3 &q) const { } /* ************************************************************************* */ -double Point3::norm() const { - return sqrt(x_ * x_ + y_ * y_ + z_ * z_); -} - -/* ************************************************************************* */ -double Point3::norm(boost::optional H) const { - double r = norm(); - if (H) { - H->resize(1,3); - if (fabs(r) > 1e-10) - *H << x_ / r, y_ / r, z_ / r; - else - *H << 1, 1, 1; // really infinity, why 1 ? - } - return r; -} - -/* ************************************************************************* */ -double Point3::norm(boost::optional&> H) const { - double r = norm(); +double Point3::norm(OptionalJacobian<1,3> H) const { + double r = sqrt(x_ * x_ + y_ * y_ + z_ * z_); if (H) { if (fabs(r) > 1e-10) *H << x_ / r, y_ / r, z_ / r; @@ -124,13 +102,12 @@ double Point3::norm(boost::optional&> H) const { } /* ************************************************************************* */ -Point3 Point3::normalize(boost::optional H) const { +Point3 Point3::normalize(OptionalJacobian<3,3> H) const { Point3 normalized = *this / norm(); if (H) { // 3*3 Derivative double x2 = x_ * x_, y2 = y_ * y_, z2 = z_ * z_; double xy = x_ * y_, xz = x_ * z_, yz = y_ * z_; - H->resize(3, 3); *H << y2 + z2, -xy, -xz, /**/-xy, x2 + z2, -yz, /**/-xz, -yz, x2 + y2; *H /= pow(x2 + y2 + z2, 1.5); } diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index b7f7f8ffa..56d9b8bef 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -21,9 +21,9 @@ #pragma once -#include #include #include +#include #include @@ -93,10 +93,10 @@ namespace gtsam { /// "Compose" - just adds coordinates of two points inline Point3 compose(const Point3& p2, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const { - if (H1) *H1 = eye(3); - if (H2) *H2 = eye(3); + OptionalJacobian<3,3> H1=boost::none, + OptionalJacobian<3,3> H2=boost::none) const { + if (H1) *H1 << I_3x3; + if (H2) *H2 << I_3x3; return *this + p2; } @@ -105,10 +105,10 @@ namespace gtsam { /** Between using the default implementation */ inline Point3 between(const Point3& p2, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const { - if(H1) *H1 = -eye(3); - if(H2) *H2 = eye(3); + OptionalJacobian<3,3> H1=boost::none, + OptionalJacobian<3,3> H2=boost::none) const { + if(H1) *H1 = -I_3x3; + if(H2) *H2 = I_3x3; return p2 - *this; } @@ -142,13 +142,13 @@ namespace gtsam { static inline Vector3 Logmap(const Point3& dp) { return Vector3(dp.x(), dp.y(), dp.z()); } /// Left-trivialized derivative of the exponential map - static Matrix dexpL(const Vector& v) { - return eye(3); + static Matrix3 dexpL(const Vector& v) { + return I_3x3; } /// Left-trivialized derivative inverse of the exponential map - static Matrix dexpInvL(const Vector& v) { - return eye(3); + static Matrix3 dexpInvL(const Vector& v) { + return I_3x3; } /// @} @@ -163,14 +163,16 @@ namespace gtsam { /** distance between two points */ inline double distance(const Point3& p2, - boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { + OptionalJacobian<1,3> H1 = boost::none, OptionalJacobian<1,3> H2 = boost::none) const { double d = (p2 - *this).norm(); if (H1) { - *H1 = (Matrix(1, 3) << x_-p2.x(), y_-p2.y(), z_-p2.z()).finished()*(1./d); + *H1 << x_-p2.x(), y_-p2.y(), z_-p2.z(); + *H1 = *H1 *(1./d); } if (H2) { - *H2 = (Matrix(1, 3) << -x_+p2.x(), -y_+p2.y(), -z_+p2.z()).finished()*(1./d); + *H2 << -x_+p2.x(), -y_+p2.y(), -z_+p2.z(); + *H2 << *H2 *(1./d); } return d; } @@ -180,17 +182,11 @@ namespace gtsam { return (p2 - *this).norm(); } - /** Distance of the point from the origin */ - double norm() const; - /** Distance of the point from the origin, with Jacobian */ - double norm(boost::optional H) const; - - /** Distance of the point from the origin, with Jacobian */ - double norm(boost::optional&> H) const; + double norm(OptionalJacobian<1,3> H = boost::none) const; /** normalize, with optional Jacobian */ - Point3 normalize(boost::optional H = boost::none) const; + Point3 normalize(OptionalJacobian<3, 3> H = boost::none) const; /** cross product @return this x q */ Point3 cross(const Point3 &q) const; @@ -219,11 +215,11 @@ namespace gtsam { /** add two points, add(this,q) is same as this + q */ Point3 add (const Point3 &q, - boost::optional H1=boost::none, boost::optional H2=boost::none) const; + OptionalJacobian<3, 3> H1=boost::none, OptionalJacobian<3, 3> H2=boost::none) const; /** subtract two points, sub(this,q) is same as this - q */ Point3 sub (const Point3 &q, - boost::optional H1=boost::none, boost::optional H2=boost::none) const; + OptionalJacobian<3,3> H1=boost::none, OptionalJacobian<3,3> H2=boost::none) const; /// @} diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index 7a693ed3d..edc16af03 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -33,15 +33,20 @@ INSTANTIATE_LIE(Pose2); /** instantiate concept checks */ GTSAM_CONCEPT_POSE_INST(Pose2); -static const Matrix I3 = eye(3), Z12 = zeros(1,2); static const Rot2 R_PI_2(Rot2::fromCosSin(0., 1.)); /* ************************************************************************* */ -Matrix Pose2::matrix() const { - Matrix R = r_.matrix(); - R = stack(2, &R, &Z12); - Matrix T = (Matrix(3, 1) << t_.x(), t_.y(), 1.0).finished(); - return collect(2, &R, &T); +Matrix3 Pose2::matrix() const { + Matrix2 R = r_.matrix(); + Matrix32 R0; + R0.block<2,2>(0,0) = R; + R0.block<1,2>(2,0).setZero(); + Matrix31 T; + T << t_.x(), t_.y(), 1.0; + Matrix3 RT_; + RT_.block<3,2>(0,0) = R0; + RT_.block<3,1>(0,2) = T; + return RT_; } /* ************************************************************************* */ @@ -70,18 +75,18 @@ Pose2 Pose2::Expmap(const Vector& xi) { } /* ************************************************************************* */ -Vector Pose2::Logmap(const Pose2& p) { +Vector3 Pose2::Logmap(const Pose2& p) { const Rot2& R = p.r(); const Point2& t = p.t(); double w = R.theta(); if (std::abs(w) < 1e-10) - return (Vector(3) << t.x(), t.y(), w).finished(); + return Vector3(t.x(), t.y(), w); else { double c_1 = R.c()-1.0, s = R.s(); double det = c_1*c_1 + s*s; Point2 p = R_PI_2 * (R.unrotate(t) - t); Point2 v = (w/det) * p; - return (Vector(3) << v.x(), v.y(), w).finished(); + return Vector3(v.x(), v.y(), w); } } @@ -96,117 +101,76 @@ Pose2 Pose2::retract(const Vector& v) const { } /* ************************************************************************* */ -Vector Pose2::localCoordinates(const Pose2& p2) const { +Vector3 Pose2::localCoordinates(const Pose2& p2) const { #ifdef SLOW_BUT_CORRECT_EXPMAP return Logmap(between(p2)); #else Pose2 r = between(p2); - return (Vector(3) << r.x(), r.y(), r.theta()).finished(); + return Vector3(r.x(), r.y(), r.theta()); #endif } /* ************************************************************************* */ // Calculate Adjoint map // Ad_pose is 3*3 matrix that when applied to twist xi, returns Ad_pose(xi) -Matrix Pose2::AdjointMap() const { +Matrix3 Pose2::AdjointMap() const { double c = r_.c(), s = r_.s(), x = t_.x(), y = t_.y(); - return (Matrix(3, 3) << + Matrix3 rvalue; + rvalue << c, -s, y, s, c, -x, - 0.0, 0.0, 1.0 - ).finished(); + 0.0, 0.0, 1.0; + return rvalue; } /* ************************************************************************* */ -Pose2 Pose2::inverse(boost::optional H1) const { +Pose2 Pose2::inverse(OptionalJacobian<3,3> H1) const { if (H1) *H1 = -AdjointMap(); return Pose2(r_.inverse(), r_.unrotate(Point2(-t_.x(), -t_.y()))); } -/* ************************************************************************* */ -// see doc/math.lyx, SE(2) section -Point2 Pose2::transform_to(const Point2& point) const { - Point2 d = point - t_; - return r_.unrotate(d); -} - /* ************************************************************************* */ // see doc/math.lyx, SE(2) section Point2 Pose2::transform_to(const Point2& point, - boost::optional H1, boost::optional H2) const { + OptionalJacobian<2, 3> H1, OptionalJacobian<2, 2> H2) const { Point2 d = point - t_; Point2 q = r_.unrotate(d); if (!H1 && !H2) return q; if (H1) *H1 << -1.0, 0.0, q.y(), 0.0, -1.0, -q.x(); - if (H2) *H2 = r_.transpose(); + if (H2) *H2 << r_.transpose(); return q; } /* ************************************************************************* */ // see doc/math.lyx, SE(2) section -Point2 Pose2::transform_to(const Point2& point, - boost::optional H1, boost::optional H2) const { - Point2 d = point - t_; - Point2 q = r_.unrotate(d); - if (!H1 && !H2) return q; - if (H1) *H1 = (Matrix(2, 3) << - -1.0, 0.0, q.y(), - 0.0, -1.0, -q.x()).finished(); - if (H2) *H2 = r_.transpose(); - return q; -} - -/* ************************************************************************* */ -// see doc/math.lyx, SE(2) section -Pose2 Pose2::compose(const Pose2& p2, boost::optional H1, - boost::optional H2) const { +Pose2 Pose2::compose(const Pose2& p2, OptionalJacobian<3,3> H1, + OptionalJacobian<3,3> H2) const { // TODO: inline and reuse? if(H1) *H1 = p2.inverse().AdjointMap(); - if(H2) *H2 = I3; + if(H2) *H2 = I_3x3; return (*this)*p2; } /* ************************************************************************* */ // see doc/math.lyx, SE(2) section Point2 Pose2::transform_from(const Point2& p, - boost::optional H1, boost::optional H2) const { + OptionalJacobian<2, 3> H1, OptionalJacobian<2, 2> H2) const { const Point2 q = r_ * p; if (H1 || H2) { - const Matrix R = r_.matrix(); - const Matrix Drotate1 = (Matrix(2, 1) << -q.y(), q.x()).finished(); - if (H1) *H1 = collect(2, &R, &Drotate1); // [R R_{pi/2}q] - if (H2) *H2 = R; // R + const Matrix2 R = r_.matrix(); + Matrix21 Drotate1; + Drotate1 << -q.y(), q.x(); + if (H1) *H1 << R, Drotate1; + if (H2) *H2 = R; // R } return q + t_; } /* ************************************************************************* */ -Pose2 Pose2::between(const Pose2& p2) const { - // get cosines and sines from rotation matrices - const Rot2& R1 = r_, R2 = p2.r(); - double c1=R1.c(), s1=R1.s(), c2=R2.c(), s2=R2.s(); - - // Assert that R1 and R2 are normalized - assert(std::abs(c1*c1 + s1*s1 - 1.0) < 1e-5 && std::abs(c2*c2 + s2*s2 - 1.0) < 1e-5); - - // Calculate delta rotation = between(R1,R2) - double c = c1 * c2 + s1 * s2, s = -s1 * c2 + c1 * s2; - Rot2 R(Rot2::atan2(s,c)); // normalizes - - // Calculate delta translation = unrotate(R1, dt); - Point2 dt = p2.t() - t_; - double x = dt.x(), y = dt.y(); - // t = R1' * (t2-t1) - Point2 t(c1 * x + s1 * y, -s1 * x + c1 * y); - - return Pose2(R,t); -} - -/* ************************************************************************* */ -Pose2 Pose2::between(const Pose2& p2, boost::optional H1, - boost::optional H2) const { +Pose2 Pose2::between(const Pose2& p2, OptionalJacobian<3,3> H1, + OptionalJacobian<3,3> H2) const { // get cosines and sines from rotation matrices const Rot2& R1 = r_, R2 = p2.r(); double c1=R1.c(), s1=R1.s(), c2=R2.c(), s2=R2.s(); @@ -233,97 +197,75 @@ Pose2 Pose2::between(const Pose2& p2, boost::optional H1, s, -c, dt2, 0.0, 0.0,-1.0; } - if (H2) *H2 = I3; - - return Pose2(R,t); -} - -/* ************************************************************************* */ -Pose2 Pose2::between(const Pose2& p2, boost::optional H1, - boost::optional H2) const { - // get cosines and sines from rotation matrices - const Rot2& R1 = r_, R2 = p2.r(); - double c1=R1.c(), s1=R1.s(), c2=R2.c(), s2=R2.s(); - - // Assert that R1 and R2 are normalized - assert(std::abs(c1*c1 + s1*s1 - 1.0) < 1e-5 && std::abs(c2*c2 + s2*s2 - 1.0) < 1e-5); - - // Calculate delta rotation = between(R1,R2) - double c = c1 * c2 + s1 * s2, s = -s1 * c2 + c1 * s2; - Rot2 R(Rot2::atan2(s,c)); // normalizes - - // Calculate delta translation = unrotate(R1, dt); - Point2 dt = p2.t() - t_; - double x = dt.x(), y = dt.y(); - // t = R1' * (t2-t1) - Point2 t(c1 * x + s1 * y, -s1 * x + c1 * y); - - // FD: This is just -AdjointMap(between(p2,p1)) inlined and re-using above - if (H1) { - double dt1 = -s2 * x + c2 * y; - double dt2 = -c2 * x - s2 * y; - *H1 = (Matrix(3, 3) << - -c, -s, dt1, - s, -c, dt2, - 0.0, 0.0,-1.0).finished(); - } - if (H2) *H2 = I3; + if (H2) *H2 = I_3x3; return Pose2(R,t); } /* ************************************************************************* */ Rot2 Pose2::bearing(const Point2& point, - boost::optional H1, boost::optional H2) const { - Point2 d = transform_to(point, H1, H2); + OptionalJacobian<1, 3> H1, OptionalJacobian<1, 2> H2) const { + // make temporary matrices + Matrix23 D1; Matrix2 D2; + Point2 d = transform_to(point, H1 ? &D1 : 0, H2 ? &D2 : 0); // uses pointer version if (!H1 && !H2) return Rot2::relativeBearing(d); - Matrix D_result_d; + Matrix12 D_result_d; Rot2 result = Rot2::relativeBearing(d, D_result_d); - if (H1) *H1 = D_result_d * (*H1); - if (H2) *H2 = D_result_d * (*H2); + if (H1) *H1 = D_result_d * (D1); + if (H2) *H2 = D_result_d * (D2); return result; } /* ************************************************************************* */ -Rot2 Pose2::bearing(const Pose2& point, - boost::optional H1, boost::optional H2) const { - Rot2 result = bearing(point.t(), H1, H2); +Rot2 Pose2::bearing(const Pose2& pose, + OptionalJacobian<1, 3> H1, OptionalJacobian<1, 3> H2) const { + Matrix12 D2; + Rot2 result = bearing(pose.t(), H1, H2 ? &D2 : 0); if (H2) { - Matrix H2_ = *H2 * point.r().matrix(); - *H2 = zeros(1, 3); - insertSub(*H2, H2_, 0, 0); + Matrix12 H2_ = D2 * pose.r().matrix(); + *H2 << H2_, Z_1x1; } return result; } - /* ************************************************************************* */ double Pose2::range(const Point2& point, - boost::optional H1, boost::optional H2) const { + OptionalJacobian<1,3> H1, OptionalJacobian<1,2> H2) const { Point2 d = point - t_; if (!H1 && !H2) return d.norm(); - Matrix H; + Matrix12 H; double r = d.norm(H); - if (H1) *H1 = H * (Matrix(2, 3) << - -r_.c(), r_.s(), 0.0, - -r_.s(), -r_.c(), 0.0).finished(); + if (H1) { + Matrix23 H1_; + H1_ << -r_.c(), r_.s(), 0.0, + -r_.s(), -r_.c(), 0.0; + *H1 = H * H1_; + } if (H2) *H2 = H; return r; } /* ************************************************************************* */ -double Pose2::range(const Pose2& pose2, - boost::optional H1, - boost::optional H2) const { - Point2 d = pose2.t() - t_; +double Pose2::range(const Pose2& pose, + OptionalJacobian<1,3> H1, + OptionalJacobian<1,3> H2) const { + Point2 d = pose.t() - t_; if (!H1 && !H2) return d.norm(); - Matrix H; + Matrix12 H; double r = d.norm(H); - if (H1) *H1 = H * (Matrix(2, 3) << + if (H1) { + Matrix23 H1_; + H1_ << -r_.c(), r_.s(), 0.0, - -r_.s(), -r_.c(), 0.0).finished(); - if (H2) *H2 = H * (Matrix(2, 3) << - pose2.r_.c(), -pose2.r_.s(), 0.0, - pose2.r_.s(), pose2.r_.c(), 0.0).finished(); + -r_.s(), -r_.c(), 0.0; + *H1 = H * H1_; + } + if (H2) { + Matrix23 H2_; + H2_ << + pose.r_.c(), -pose.r_.s(), 0.0, + pose.r_.s(), pose.r_.c(), 0.0; + *H2 = H * H2_; + } return r; } diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index d43be6afb..a48be51cf 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -107,12 +107,12 @@ public: inline static Pose2 identity() { return Pose2(); } /// inverse transformation with derivatives - Pose2 inverse(boost::optional H1=boost::none) const; + Pose2 inverse(OptionalJacobian<3, 3> H1=boost::none) const; /// compose this transformation onto another (first *this and then p2) Pose2 compose(const Pose2& p2, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const; + OptionalJacobian<3, 3> H1 = boost::none, + OptionalJacobian<3, 3> H2 = boost::none) const; /// compose syntactic sugar inline Pose2 operator*(const Pose2& p2) const { @@ -122,19 +122,8 @@ public: /** * Return relative pose between p1 and p2, in p1 coordinate frame */ - Pose2 between(const Pose2& p2) const; - - /** - * Return relative pose between p1 and p2, in p1 coordinate frame - */ - Pose2 between(const Pose2& p2, boost::optional H1, - boost::optional H2) const; - - /** - * Return relative pose between p1 and p2, in p1 coordinate frame - */ - Pose2 between(const Pose2& p2, boost::optional H1, - boost::optional H2) const; + Pose2 between(const Pose2& p2, OptionalJacobian<3,3> H1 = boost::none, + OptionalJacobian<3,3> H = boost::none) const; /// @} /// @name Manifold @@ -150,7 +139,7 @@ public: Pose2 retract(const Vector& v) const; /// Local 3D coordinates \f$ [T_x,T_y,\theta] \f$ of Pose2 manifold neighborhood around current pose - Vector localCoordinates(const Pose2& p2) const; + Vector3 localCoordinates(const Pose2& p2) const; /// @} /// @name Lie Group @@ -160,13 +149,13 @@ public: static Pose2 Expmap(const Vector& xi); ///Log map at identity - return the canonical coordinates \f$ [T_x,T_y,\theta] \f$ of this rotation - static Vector Logmap(const Pose2& p); + static Vector3 Logmap(const Pose2& p); /** * Calculate Adjoint map * Ad_pose is 3*3 matrix that when applied to twist xi \f$ [T_x,T_y,\theta] \f$, returns Ad_pose(xi) */ - Matrix AdjointMap() const; + Matrix3 AdjointMap() const; inline Vector Adjoint(const Vector& xi) const { assert(xi.size() == 3); return AdjointMap()*xi; @@ -179,34 +168,27 @@ public: * v (vx,vy) = 2D velocity * @return xihat, 3*3 element of Lie algebra that can be exponentiated */ - static inline Matrix wedge(double vx, double vy, double w) { - return (Matrix(3,3) << - 0.,-w, vx, - w, 0., vy, - 0., 0., 0.).finished(); + static inline Matrix3 wedge(double vx, double vy, double w) { + Matrix3 m; + m << 0.,-w, vx, + w, 0., vy, + 0., 0., 0.; + return m; } /// @} /// @name Group Action on Point2 /// @{ - /** Return point coordinates in pose coordinate frame */ - Point2 transform_to(const Point2& point) const; - /** Return point coordinates in pose coordinate frame */ Point2 transform_to(const Point2& point, - boost::optional H1, - boost::optional H2) const; - - /** Return point coordinates in pose coordinate frame */ - Point2 transform_to(const Point2& point, - boost::optional H1, - boost::optional H2) const; + OptionalJacobian<2, 3> H1 = boost::none, + OptionalJacobian<2, 2> H2 = boost::none) const; /** Return point coordinates in global frame */ Point2 transform_from(const Point2& point, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const; + OptionalJacobian<2, 3> H1 = boost::none, + OptionalJacobian<2, 2> H2 = boost::none) const; /** syntactic sugar for transform_from */ inline Point2 operator*(const Point2& point) const { return transform_from(point);} @@ -237,7 +219,7 @@ public: inline const Rot2& rotation() const { return r_; } //// return transformation matrix - Matrix matrix() const; + Matrix3 matrix() const; /** * Calculate bearing to a landmark @@ -245,17 +227,15 @@ public: * @return 2D rotation \f$ \in SO(2) \f$ */ Rot2 bearing(const Point2& point, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const; + OptionalJacobian<1, 3> H1=boost::none, OptionalJacobian<1, 2> H2=boost::none) const; /** * Calculate bearing to another pose * @param point SO(2) location of other pose * @return 2D rotation \f$ \in SO(2) \f$ */ - Rot2 bearing(const Pose2& point, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const; + Rot2 bearing(const Pose2& pose, + OptionalJacobian<1, 3> H1=boost::none, OptionalJacobian<1, 3> H2=boost::none) const; /** * Calculate range to a landmark @@ -263,8 +243,8 @@ public: * @return range (double) */ double range(const Point2& point, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const; + OptionalJacobian<1, 3> H1=boost::none, + OptionalJacobian<1, 2> H2=boost::none) const; /** * Calculate range to another pose @@ -272,8 +252,8 @@ public: * @return range (double) */ double range(const Pose2& point, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const; + OptionalJacobian<1, 3> H1=boost::none, + OptionalJacobian<1, 3> H2=boost::none) const; /// @} /// @name Advanced Interface diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index b134553d9..0bc07b753 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -32,9 +32,6 @@ INSTANTIATE_LIE(Pose3); /** instantiate concept checks */ GTSAM_CONCEPT_POSE_INST(Pose3); -static const Matrix3 I3 = eye(3), Z3 = zeros(3, 3), _I3 = -I3; -static const Matrix6 I6 = eye(6); - /* ************************************************************************* */ Pose3::Pose3(const Pose2& pose2) : R_(Rot3::rodriguez(0, 0, pose2.theta())), t_( @@ -50,59 +47,62 @@ Matrix6 Pose3::AdjointMap() const { const Vector3 t = t_.vector(); Matrix3 A = skewSymmetric(t) * R; Matrix6 adj; - adj << R, Z3, A, R; + adj << R, Z_3x3, A, R; return adj; } /* ************************************************************************* */ -Matrix6 Pose3::adjointMap(const Vector& xi) { +Matrix6 Pose3::adjointMap(const Vector6& xi) { Matrix3 w_hat = skewSymmetric(xi(0), xi(1), xi(2)); Matrix3 v_hat = skewSymmetric(xi(3), xi(4), xi(5)); Matrix6 adj; - adj << w_hat, Z3, v_hat, w_hat; + adj << w_hat, Z_3x3, v_hat, w_hat; return adj; } /* ************************************************************************* */ -Vector Pose3::adjoint(const Vector& xi, const Vector& y, - boost::optional H) { +Vector6 Pose3::adjoint(const Vector6& xi, const Vector6& y, + OptionalJacobian<6,6> H) { if (H) { - *H = zeros(6, 6); + H->setZero(); for (int i = 0; i < 6; ++i) { - Vector dxi = zero(6); + Vector6 dxi; + dxi.setZero(); dxi(i) = 1.0; - Matrix Gi = adjointMap(dxi); - (*H).col(i) = Gi * y; + Matrix6 Gi = adjointMap(dxi); + H->col(i) = Gi * y; } } return adjointMap(xi) * y; } /* ************************************************************************* */ -Vector Pose3::adjointTranspose(const Vector& xi, const Vector& y, - boost::optional H) { +Vector6 Pose3::adjointTranspose(const Vector6& xi, const Vector6& y, + OptionalJacobian<6,6> H) { if (H) { - *H = zeros(6, 6); + H->setZero(); for (int i = 0; i < 6; ++i) { - Vector dxi = zero(6); + Vector6 dxi; + dxi.setZero(); dxi(i) = 1.0; - Matrix GTi = adjointMap(dxi).transpose(); - (*H).col(i) = GTi * y; + Matrix6 GTi = adjointMap(dxi).transpose(); + H->col(i) = GTi * y; } } - Matrix adjT = adjointMap(xi).transpose(); return adjointMap(xi).transpose() * y; } /* ************************************************************************* */ -Matrix6 Pose3::dExpInv_exp(const Vector& xi) { +Matrix6 Pose3::dExpInv_exp(const Vector6& xi) { // Bernoulli numbers, from Wikipedia static const Vector B = (Vector(9) << 1.0, -1.0 / 2.0, 1. / 6., 0.0, -1.0 / 30.0, 0.0, 1.0 / 42.0, 0.0, -1.0 / 30).finished(); static const int N = 5; // order of approximation - Matrix res = I6; - Matrix6 ad_i = I6; + Matrix6 res; + res = I_6x6; + Matrix6 ad_i; + ad_i = I_6x6; Matrix6 ad_xi = adjointMap(xi); double fac = 1.0; for (int i = 1; i < N; ++i) { @@ -225,7 +225,7 @@ Vector6 Pose3::localCoordinates(const Pose3& T, Matrix4 Pose3::matrix() const { const Matrix3 R = R_.matrix(); const Vector3 T = t_.vector(); - Eigen::Matrix A14; + Matrix14 A14; A14 << 0.0, 0.0, 0.0, 1.0; Matrix4 mat; mat << R, T, A14; @@ -240,12 +240,11 @@ Pose3 Pose3::transform_to(const Pose3& pose) const { } /* ************************************************************************* */ -Point3 Pose3::transform_from(const Point3& p, boost::optional Dpose, - boost::optional Dpoint) const { +Point3 Pose3::transform_from(const Point3& p, OptionalJacobian<3,6> Dpose, + OptionalJacobian<3,3> Dpoint) const { if (Dpose) { const Matrix3 R = R_.matrix(); Matrix3 DR = R * skewSymmetric(-p.x(), -p.y(), -p.z()); - Dpose->resize(3, 6); (*Dpose) << DR, R; } if (Dpoint) @@ -254,13 +253,8 @@ Point3 Pose3::transform_from(const Point3& p, boost::optional Dpose, } /* ************************************************************************* */ -Point3 Pose3::transform_to(const Point3& p) const { - return R_.unrotate(p - t_); -} - -/* ************************************************************************* */ -Point3 Pose3::transform_to(const Point3& p, boost::optional Dpose, - boost::optional Dpoint) const { +Point3 Pose3::transform_to(const Point3& p, OptionalJacobian<3,6> Dpose, + OptionalJacobian<3,3> Dpoint) const { // Only get transpose once, to avoid multiple allocations, // as well as multiple conversions in the Quaternion case const Matrix3 Rt = R_.transpose(); @@ -278,77 +272,57 @@ Point3 Pose3::transform_to(const Point3& p, boost::optional Dpose, } /* ************************************************************************* */ -Point3 Pose3::transform_to(const Point3& p, boost::optional Dpose, - boost::optional Dpoint) const { - const Matrix3 Rt = R_.transpose(); - const Point3 q(Rt*(p - t_).vector()); - if (Dpose) { - const double wx = q.x(), wy = q.y(), wz = q.z(); - Dpose->resize(3, 6); - (*Dpose) << - 0.0, -wz, +wy,-1.0, 0.0, 0.0, - +wz, 0.0, -wx, 0.0,-1.0, 0.0, - -wy, +wx, 0.0, 0.0, 0.0,-1.0; - } - if (Dpoint) - *Dpoint = Rt; - return q; -} - -/* ************************************************************************* */ -Pose3 Pose3::compose(const Pose3& p2, boost::optional H1, - boost::optional H2) const { - if (H1) - *H1 = p2.inverse().AdjointMap(); - if (H2) - *H2 = I6; +Pose3 Pose3::compose(const Pose3& p2, OptionalJacobian<6,6> H1, + OptionalJacobian<6,6> H2) const { + if (H1) *H1 = p2.inverse().AdjointMap(); + if (H2) *H2 = I_6x6; return (*this) * p2; } /* ************************************************************************* */ -Pose3 Pose3::inverse(boost::optional H1) const { - if (H1) - *H1 = -AdjointMap(); +Pose3 Pose3::inverse(OptionalJacobian<6,6> H1) const { + if (H1) *H1 = -AdjointMap(); Rot3 Rt = R_.inverse(); return Pose3(Rt, Rt * (-t_)); } /* ************************************************************************* */ // between = compose(p2,inverse(p1)); -Pose3 Pose3::between(const Pose3& p2, boost::optional H1, - boost::optional H2) const { +Pose3 Pose3::between(const Pose3& p2, OptionalJacobian<6,6> H1, + OptionalJacobian<6,6> H2) const { Pose3 result = inverse() * p2; - if (H1) - *H1 = -result.inverse().AdjointMap(); - if (H2) - *H2 = I6; + if (H1) *H1 = -result.inverse().AdjointMap(); + if (H2) *H2 = I_6x6; return result; } /* ************************************************************************* */ -double Pose3::range(const Point3& point, boost::optional H1, - boost::optional H2) const { +double Pose3::range(const Point3& point, OptionalJacobian<1, 6> H1, + OptionalJacobian<1, 3> H2) const { if (!H1 && !H2) return transform_to(point).norm(); - Point3 d = transform_to(point, H1, H2); - double x = d.x(), y = d.y(), z = d.z(), d2 = x * x + y * y + z * z, n = sqrt( - d2); - Matrix D_result_d = (Matrix(1, 3) << x / n, y / n, z / n).finished(); - if (H1) - *H1 = D_result_d * (*H1); - if (H2) - *H2 = D_result_d * (*H2); - return n; + else { + Matrix36 D1; + Matrix3 D2; + Point3 d = transform_to(point, H1 ? &D1 : 0, H2 ? &D2 : 0); + const double x = d.x(), y = d.y(), z = d.z(), d2 = x * x + y * y + z * z, + n = sqrt(d2); + Matrix13 D_result_d; + D_result_d << x / n, y / n, z / n; + if (H1) *H1 = D_result_d * D1; + if (H2) *H2 = D_result_d * D2; + return n; + } } /* ************************************************************************* */ -double Pose3::range(const Pose3& point, boost::optional H1, - boost::optional H2) const { - double r = range(point.translation(), H1, H2); +double Pose3::range(const Pose3& pose, OptionalJacobian<1,6> H1, + OptionalJacobian<1,6> H2) const { + Matrix13 D2; + double r = range(pose.translation(), H1, H2? &D2 : 0); if (H2) { - Matrix H2_ = *H2 * point.rotation().matrix(); - *H2 = zeros(1, 6); - insertSub(*H2, H2_, 0, 3); + Matrix13 H2_ = D2 * pose.rotation().matrix(); + *H2 << Matrix13::Zero(), H2_; } return r; } @@ -370,7 +344,7 @@ boost::optional align(const vector& pairs) { cq *= f; // Add to form H matrix - Matrix H = zeros(3, 3); + Matrix3 H = Eigen::Matrix3d::Zero(); BOOST_FOREACH(const Point3Pair& pair, pairs){ Vector dp = pair.first.vector() - cp; Vector dq = pair.second.vector() - cq; @@ -378,13 +352,13 @@ boost::optional align(const vector& pairs) { } // Compute SVD - Matrix U, V; + Matrix U,V; Vector S; svd(H, U, S, V); // Recover transform with correction from Eggert97machinevisionandapplications - Matrix UVtranspose = U * V.transpose(); - Matrix detWeighting = eye(3, 3); + Matrix3 UVtranspose = U * V.transpose(); + Matrix3 detWeighting = I_3x3; detWeighting(2, 2) = UVtranspose.determinant(); Rot3 R(Matrix(V * detWeighting * U.transpose())); Point3 t = Point3(cq) - R * Point3(cp); diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 2090558a6..6a7ed8456 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -70,6 +70,12 @@ public: R_(R), t_(t) { } + /** Construct from R,t, where t \in vector3 */ + Pose3(const Rot3& R, const Vector3& t) : + R_(R), t_(t) { + } + + /** Construct from Pose2 */ explicit Pose3(const Pose2& pose2); @@ -99,11 +105,11 @@ public: } /// inverse transformation with derivatives - Pose3 inverse(boost::optional H1 = boost::none) const; + Pose3 inverse(OptionalJacobian<6, 6> H1 = boost::none) const; ///compose this transformation onto another (first *this and then p2) - Pose3 compose(const Pose3& p2, boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const; + Pose3 compose(const Pose3& p2, OptionalJacobian<6, 6> H1 = boost::none, + OptionalJacobian<6, 6> H2 = boost::none) const; /// compose syntactic sugar Pose3 operator*(const Pose3& T) const { @@ -114,8 +120,8 @@ public: * Return relative pose between p1 and p2, in p1 coordinate frame * as well as optionally the derivatives */ - Pose3 between(const Pose3& p2, boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const; + Pose3 between(const Pose3& p2, OptionalJacobian<6, 6> H1 = boost::none, + OptionalJacobian<6, 6> H2 = boost::none) const; /// @} /// @name Manifold @@ -186,17 +192,17 @@ public: * and its inverse transpose in the discrete Euler Poincare' (DEP) operator. * */ - static Matrix6 adjointMap(const Vector& xi); + static Matrix6 adjointMap(const Vector6 &xi); /** * Action of the adjointMap on a Lie-algebra vector y, with optional derivatives */ - static Vector adjoint(const Vector& xi, const Vector& y, boost::optional H = boost::none); + static Vector6 adjoint(const Vector6 &xi, const Vector6 &y, OptionalJacobian<6,6> = boost::none); /** * The dual version of adjoint action, acting on the dual space of the Lie-algebra vector space. */ - static Vector adjointTranspose(const Vector& xi, const Vector& y, boost::optional H = boost::none); + static Vector6 adjointTranspose(const Vector6& xi, const Vector6& y, OptionalJacobian<6, 6> H = boost::none); /** * Compute the inverse right-trivialized tangent (derivative) map of the exponential map, @@ -208,7 +214,7 @@ public: * Ernst Hairer, et al., Geometric Numerical Integration, * Structure-Preserving Algorithms for Ordinary Differential Equations, 2nd edition, Springer-Verlag, 2006. */ - static Matrix6 dExpInv_exp(const Vector& xi); + static Matrix6 dExpInv_exp(const Vector6 &xi); /** * wedge for Pose3: @@ -237,7 +243,7 @@ public: * @return point in world coordinates */ Point3 transform_from(const Point3& p, - boost::optional Dpose=boost::none, boost::optional Dpoint=boost::none) const; + OptionalJacobian<3,6> Dpose=boost::none, OptionalJacobian<3,3> Dpoint=boost::none) const; /** syntactic sugar for transform_from */ inline Point3 operator*(const Point3& p) const { return transform_from(p); } @@ -249,13 +255,9 @@ public: * @param Dpoint optional 3*3 Jacobian wrpt point * @return point in Pose coordinates */ - Point3 transform_to(const Point3& p) const; - Point3 transform_to(const Point3& p, - boost::optional Dpose, boost::optional Dpoint) const; - - Point3 transform_to(const Point3& p, - boost::optional Dpose, boost::optional Dpoint) const; + OptionalJacobian<3,6> Dpose = boost::none, + OptionalJacobian<3,3> Dpoint = boost::none) const; /// @} /// @name Standard Interface @@ -288,8 +290,8 @@ public: * @return range (double) */ double range(const Point3& point, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const; + OptionalJacobian<1,6> H1=boost::none, + OptionalJacobian<1,3> H2=boost::none) const; /** * Calculate range to another pose @@ -297,8 +299,8 @@ public: * @return range (double) */ double range(const Pose3& pose, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const; + OptionalJacobian<1,6> H1=boost::none, + OptionalJacobian<1,6> H2=boost::none) const; /// @} /// @name Advanced Interface diff --git a/gtsam/geometry/Rot2.cpp b/gtsam/geometry/Rot2.cpp index 0133c9440..13c22ddc1 100644 --- a/gtsam/geometry/Rot2.cpp +++ b/gtsam/geometry/Rot2.cpp @@ -64,21 +64,25 @@ Rot2& Rot2::normalize() { } /* ************************************************************************* */ -Matrix Rot2::matrix() const { - return (Matrix(2, 2) << c_, -s_, s_, c_).finished(); +Matrix2 Rot2::matrix() const { + Matrix2 rvalue_; + rvalue_ << c_, -s_, s_, c_; + return rvalue_; } /* ************************************************************************* */ -Matrix Rot2::transpose() const { - return (Matrix(2, 2) << c_, s_, -s_, c_).finished(); +Matrix2 Rot2::transpose() const { + Matrix2 rvalue_; + rvalue_ << c_, s_, -s_, c_; + return rvalue_; } /* ************************************************************************* */ // see doc/math.lyx, SO(2) section -Point2 Rot2::rotate(const Point2& p, boost::optional H1, - boost::optional H2) const { +Point2 Rot2::rotate(const Point2& p, OptionalJacobian<2, 1> H1, + OptionalJacobian<2, 2> H2) const { const Point2 q = Point2(c_ * p.x() + -s_ * p.y(), s_ * p.x() + c_ * p.y()); - if (H1) *H1 = (Matrix(2, 1) << -q.y(), q.x()).finished(); + if (H1) *H1 << -q.y(), q.x(); if (H2) *H2 = matrix(); return q; } @@ -86,21 +90,23 @@ Point2 Rot2::rotate(const Point2& p, boost::optional H1, /* ************************************************************************* */ // see doc/math.lyx, SO(2) section Point2 Rot2::unrotate(const Point2& p, - boost::optional H1, boost::optional H2) const { + OptionalJacobian<2, 1> H1, OptionalJacobian<2, 2> H2) const { const Point2 q = Point2(c_ * p.x() + s_ * p.y(), -s_ * p.x() + c_ * p.y()); - if (H1) *H1 = (Matrix(2, 1) << q.y(), -q.x()).finished(); // R_{pi/2}q + if (H1) *H1 << q.y(), -q.x(); if (H2) *H2 = transpose(); return q; } /* ************************************************************************* */ -Rot2 Rot2::relativeBearing(const Point2& d, boost::optional H) { +Rot2 Rot2::relativeBearing(const Point2& d, OptionalJacobian<1, 2> H) { double x = d.x(), y = d.y(), d2 = x * x + y * y, n = sqrt(d2); if(fabs(n) > 1e-5) { - if (H) *H = (Matrix(1, 2) << -y / d2, x / d2).finished(); + if (H) { + *H << -y / d2, x / d2; + } return Rot2::fromCosSin(x / n, y / n); } else { - if (H) *H = (Matrix(1, 2) << 0.0, 0.0).finished(); + if (H) *H << 0.0, 0.0; return Rot2(); } } diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index af9148fd3..d5ea6afdc 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -87,7 +87,7 @@ namespace gtsam { * @param H optional reference for Jacobian * @return 2D rotation \f$ \in SO(2) \f$ */ - static Rot2 relativeBearing(const Point2& d, boost::optional H = + static Rot2 relativeBearing(const Point2& d, OptionalJacobian<1,2> H = boost::none); /** Named constructor that behaves as atan2, i.e., y,x order (!) and normalizes */ @@ -116,10 +116,10 @@ namespace gtsam { } /** Compose - make a new rotation by adding angles */ - inline Rot2 compose(const Rot2& R, boost::optional H1 = - boost::none, boost::optional H2 = boost::none) const { - if (H1) *H1 = eye(1); - if (H2) *H2 = eye(1); + inline Rot2 compose(const Rot2& R, OptionalJacobian<1,1> H1 = + boost::none, OptionalJacobian<1,1> H2 = boost::none) const { + if (H1) *H1 = I_1x1; // set to 1x1 identity matrix + if (H2) *H2 = I_1x1; // set to 1x1 identity matrix return fromCosSin(c_ * R.c_ - s_ * R.s_, s_ * R.c_ + c_ * R.s_); } @@ -129,10 +129,10 @@ namespace gtsam { } /** Between using the default implementation */ - inline Rot2 between(const Rot2& R, boost::optional H1 = - boost::none, boost::optional H2 = boost::none) const { - if (H1) *H1 = -eye(1); - if (H2) *H2 = eye(1); + inline Rot2 between(const Rot2& R, OptionalJacobian<1,1> H1 = + boost::none, OptionalJacobian<1,1> H2 = boost::none) const { + if (H1) *H1 = -I_1x1; // set to 1x1 identity matrix + if (H2) *H2 = I_1x1; // set to 1x1 identity matrix return fromCosSin(c_ * R.c_ + s_ * R.s_, -s_ * R.c_ + c_ * R.s_); } @@ -154,7 +154,7 @@ namespace gtsam { inline Rot2 retract(const Vector& v) const { return *this * Expmap(v); } /// Returns inverse retraction - inline Vector localCoordinates(const Rot2& t2) const { return Logmap(between(t2)); } + inline Vector1 localCoordinates(const Rot2& t2) const { return Logmap(between(t2)); } /// @} /// @name Lie Group @@ -169,8 +169,10 @@ namespace gtsam { } ///Log map at identity - return the canonical coordinates of this rotation - static inline Vector Logmap(const Rot2& r) { - return (Vector(1) << r.theta()).finished(); + static inline Vector1 Logmap(const Rot2& r) { + Vector1 v; + v << r.theta(); + return v; } /// @} @@ -180,8 +182,8 @@ namespace gtsam { /** * rotate point from rotated coordinate frame to world \f$ p^w = R_c^w p^c \f$ */ - Point2 rotate(const Point2& p, boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const; + Point2 rotate(const Point2& p, OptionalJacobian<2, 1> H1 = boost::none, + OptionalJacobian<2, 2> H2 = boost::none) const; /** syntactic sugar for rotate */ inline Point2 operator*(const Point2& p) const { @@ -191,8 +193,8 @@ namespace gtsam { /** * rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$ */ - Point2 unrotate(const Point2& p, boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const; + Point2 unrotate(const Point2& p, OptionalJacobian<2, 1> H1 = boost::none, + OptionalJacobian<2, 2> H2 = boost::none) const; /// @} /// @name Standard Interface @@ -225,10 +227,10 @@ namespace gtsam { } /** return 2*2 rotation matrix */ - Matrix matrix() const; + Matrix2 matrix() const; /** return 2*2 transpose (inverse) rotation matrix */ - Matrix transpose() const; + Matrix2 transpose() const; private: /** Serialization function */ diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index 846e0e070..0b88a70c7 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -27,8 +27,6 @@ using namespace std; namespace gtsam { -static const Matrix3 I3 = Matrix3::Identity(); - /* ************************************************************************* */ void Rot3::print(const std::string& s) const { gtsam::print((Matrix)matrix(), s); @@ -54,7 +52,7 @@ Rot3 Rot3::Random(boost::mt19937 & rng) { } /* ************************************************************************* */ -Rot3 Rot3::rodriguez(const Vector& w) { +Rot3 Rot3::rodriguez(const Vector3& w) { double t = w.norm(); if (t < 1e-10) return Rot3(); return rodriguez(w/t, t); @@ -72,23 +70,21 @@ Point3 Rot3::operator*(const Point3& p) const { /* ************************************************************************* */ Unit3 Rot3::rotate(const Unit3& p, - boost::optional HR, boost::optional Hp) const { - Unit3 q = Unit3(rotate(p.point3(Hp))); - if (Hp) - (*Hp) = q.basis().transpose() * matrix() * (*Hp); - if (HR) - (*HR) = -q.basis().transpose() * matrix() * p.skew(); + OptionalJacobian<2,3> HR, OptionalJacobian<2,2> Hp) const { + Matrix32 Dp; + Unit3 q = Unit3(rotate(p.point3(Hp ? &Dp : 0))); + if (Hp) *Hp = q.basis().transpose() * matrix() * Dp; + if (HR) *HR = -q.basis().transpose() * matrix() * p.skew(); return q; } /* ************************************************************************* */ Unit3 Rot3::unrotate(const Unit3& p, - boost::optional HR, boost::optional Hp) const { - Unit3 q = Unit3(unrotate(p.point3(Hp))); - if (Hp) - (*Hp) = q.basis().transpose() * matrix().transpose () * (*Hp); - if (HR) - (*HR) = q.basis().transpose() * q.skew(); + OptionalJacobian<2,3> HR, OptionalJacobian<2,2> Hp) const { + Matrix32 Dp; + Unit3 q = Unit3(unrotate(p.point3(Dp))); + if (Hp) *Hp = q.basis().transpose() * matrix().transpose () * Dp; + if (HR) *HR = q.basis().transpose() * q.skew(); return q; } @@ -99,8 +95,8 @@ Unit3 Rot3::operator*(const Unit3& p) const { /* ************************************************************************* */ // see doc/math.lyx, SO(3) section -Point3 Rot3::unrotate(const Point3& p, boost::optional H1, - boost::optional H2) const { +Point3 Rot3::unrotate(const Point3& p, OptionalJacobian<3,3> H1, + OptionalJacobian<3,3> H2) const { const Matrix3& Rt = transpose(); Point3 q(Rt * p.vector()); // q = Rt*p const double wx = q.x(), wy = q.y(), wz = q.z(); @@ -111,32 +107,16 @@ Point3 Rot3::unrotate(const Point3& p, boost::optional H1, return q; } -/* ************************************************************************* */ -// see doc/math.lyx, SO(3) section -Point3 Rot3::unrotate(const Point3& p, - boost::optional H1, boost::optional H2) const { - const Matrix3& Rt = transpose(); - Point3 q(Rt * p.vector()); // q = Rt*p - const double wx = q.x(), wy = q.y(), wz = q.z(); - if (H1) { - H1->resize(3,3); - *H1 << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0; - } - if (H2) - *H2 = Rt; - return q; -} - /* ************************************************************************* */ /// Follow Iserles05an, B10, pg 147, with a sign change in the second term (left version) Matrix3 Rot3::dexpL(const Vector3& v) { if(zero(v)) return eye(3); - Matrix x = skewSymmetric(v); - Matrix x2 = x*x; + Matrix3 x = skewSymmetric(v); + Matrix3 x2 = x*x; double theta = v.norm(), vi = theta/2.0; double s1 = sin(vi)/vi; double s2 = (theta - sin(theta))/(theta*theta*theta); - Matrix res = eye(3) - 0.5*s1*s1*x + s2*x2; + Matrix3 res = I_3x3 - 0.5*s1*s1*x + s2*x2; return res; } @@ -144,11 +124,11 @@ Matrix3 Rot3::dexpL(const Vector3& v) { /// Follow Iserles05an, B11, pg 147, with a sign change in the second term (left version) Matrix3 Rot3::dexpInvL(const Vector3& v) { if(zero(v)) return eye(3); - Matrix x = skewSymmetric(v); - Matrix x2 = x*x; + Matrix3 x = skewSymmetric(v); + Matrix3 x2 = x*x; double theta = v.norm(), vi = theta/2.0; double s2 = (theta*tan(M_PI_2-vi) - 2)/(2*theta*theta); - Matrix res = eye(3) + 0.5*x - s2*x2; + Matrix3 res = I_3x3 + 0.5*x - s2*x2; return res; } @@ -167,7 +147,7 @@ Point3 Rot3::column(int index) const{ /* ************************************************************************* */ Vector3 Rot3::xyz() const { - Matrix I;Vector3 q; + Matrix3 I;Vector3 q; boost::tie(I,q)=RQ(matrix()); return q; } @@ -200,11 +180,11 @@ Matrix3 Rot3::rightJacobianExpMapSO3(const Vector3& x) { double normx = norm_2(x); // rotation angle Matrix3 Jr; if (normx < 10e-8){ - Jr = Matrix3::Identity(); + Jr = I_3x3; } else{ const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^ - Jr = Matrix3::Identity() - ((1-cos(normx))/(normx*normx)) * X + + Jr = I_3x3 - ((1-cos(normx))/(normx*normx)) * X + ((normx-sin(normx))/(normx*normx*normx)) * X * X; // right Jacobian } return Jr; @@ -217,11 +197,11 @@ Matrix3 Rot3::rightJacobianExpMapSO3inverse(const Vector3& x) { Matrix3 Jrinv; if (normx < 10e-8){ - Jrinv = Matrix3::Identity(); + Jrinv = I_3x3; } else{ const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^ - Jrinv = Matrix3::Identity() + + Jrinv = I_3x3 + 0.5 * X + (1/(normx*normx) - (1+cos(normx))/(2*normx * sin(normx)) ) * X * X; } return Jrinv; @@ -255,12 +235,6 @@ ostream &operator<<(ostream &os, const Rot3& R) { return os; } -/* ************************************************************************* */ -Point3 Rot3::unrotate(const Point3& p) const { - // Eigen expression - return Point3(transpose()*p.vector()); // q = Rt*p -} - /* ************************************************************************* */ Rot3 Rot3::slerp(double t, const Rot3& other) const { // amazingly simple in GTSAM :-) diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index ef2d19750..6fe3f92b4 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -183,7 +183,7 @@ namespace gtsam { * @param v a vector of incremental roll,pitch,yaw * @return incremental rotation matrix */ - static Rot3 rodriguez(const Vector& v); + static Rot3 rodriguez(const Vector3& v); /** * Rodriguez' formula to compute an incremental rotation matrix @@ -193,7 +193,7 @@ namespace gtsam { * @return incremental rotation matrix */ static Rot3 rodriguez(double wx, double wy, double wz) - { return rodriguez((Vector(3) << wx, wy, wz).finished());} + { return rodriguez(Vector3(wx, wy, wz));} /// @} /// @name Testable @@ -215,18 +215,11 @@ namespace gtsam { } /// derivative of inverse rotation R^T s.t. inverse(R)*R = identity - Rot3 inverse(boost::optional H1=boost::none) const; + Rot3 inverse(boost::optional H1=boost::none) const; /// Compose two rotations i.e., R= (*this) * R2 - Rot3 compose(const Rot3& R2) const; - - /// Compose two rotations i.e., R= (*this) * R2 - Rot3 compose(const Rot3& R2, boost::optional H1, - boost::optional H2) const; - - /// Compose two rotations i.e., R= (*this) * R2 - Rot3 compose(const Rot3& R2, boost::optional H1, - boost::optional H2) const; + Rot3 compose(const Rot3& R2, OptionalJacobian<3, 3> H1 = boost::none, + OptionalJacobian<3, 3> H2 = boost::none) const; /** compose two rotations */ Rot3 operator*(const Rot3& R2) const; @@ -245,8 +238,8 @@ namespace gtsam { * Return relative rotation D s.t. R2=D*R1, i.e. D=R2*R1' */ Rot3 between(const Rot3& R2, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const; + OptionalJacobian<3,3> H1=boost::none, + OptionalJacobian<3,3> H2=boost::none) const; /// @} /// @name Manifold @@ -328,34 +321,27 @@ namespace gtsam { /** * rotate point from rotated coordinate frame to world \f$ p^w = R_c^w p^c \f$ */ - Point3 rotate(const Point3& p, boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const; + Point3 rotate(const Point3& p, OptionalJacobian<3,3> H1 = boost::none, + OptionalJacobian<3,3> H2 = boost::none) const; /// rotate point from rotated coordinate frame to world = R*p Point3 operator*(const Point3& p) const; /// rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$ - Point3 unrotate(const Point3& p) const; - - /// rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$ - Point3 unrotate(const Point3& p, boost::optional H1, - boost::optional H2) const; - - /// rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$ - Point3 unrotate(const Point3& p, boost::optional H1, - boost::optional H2) const; + Point3 unrotate(const Point3& p, OptionalJacobian<3,3> H1 = boost::none, + OptionalJacobian<3,3> H2=boost::none) const; /// @} /// @name Group Action on Unit3 /// @{ /// rotate 3D direction from rotated coordinate frame to world frame - Unit3 rotate(const Unit3& p, boost::optional HR = boost::none, - boost::optional Hp = boost::none) const; + Unit3 rotate(const Unit3& p, OptionalJacobian<2,3> HR = boost::none, + OptionalJacobian<2,2> Hp = boost::none) const; /// unrotate 3D direction from world frame to rotated coordinate frame - Unit3 unrotate(const Unit3& p, boost::optional HR = boost::none, - boost::optional Hp = boost::none) const; + Unit3 unrotate(const Unit3& p, OptionalJacobian<2,3> HR = boost::none, + OptionalJacobian<2,2> Hp = boost::none) const; /// rotate 3D direction from rotated coordinate frame to world frame Unit3 operator*(const Unit3& p) const; diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index d0c7e95f3..fc3490fb5 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -30,10 +30,8 @@ using namespace std; namespace gtsam { -static const Matrix3 I3 = Matrix3::Identity(); - /* ************************************************************************* */ -Rot3::Rot3() : rot_(Matrix3::Identity()) {} +Rot3::Rot3() : rot_(I_3x3) {} /* ************************************************************************* */ Rot3::Rot3(const Point3& col1, const Point3& col2, const Point3& col3) { @@ -142,23 +140,9 @@ Rot3 Rot3::rodriguez(const Vector& w, double theta) { } /* ************************************************************************* */ -Rot3 Rot3::compose (const Rot3& R2) const { - return *this * R2; -} - -/* ************************************************************************* */ -Rot3 Rot3::compose (const Rot3& R2, - boost::optional H1, boost::optional H2) const { +Rot3 Rot3::compose(const Rot3& R2, OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2) const { if (H1) *H1 = R2.transpose(); - if (H2) *H2 = I3; - return *this * R2; -} - -/* ************************************************************************* */ -Rot3 Rot3::compose (const Rot3& R2, - boost::optional H1, boost::optional H2) const { - if (H1) *H1 = R2.transpose(); - if (H2) *H2 = I3; + if (H2) *H2 = I_3x3; return *this * R2; } @@ -174,23 +158,23 @@ Matrix3 Rot3::transpose() const { } /* ************************************************************************* */ -Rot3 Rot3::inverse(boost::optional H1) const { +Rot3 Rot3::inverse(boost::optional H1) const { if (H1) *H1 = -rot_; return Rot3(Matrix3(transpose())); } /* ************************************************************************* */ Rot3 Rot3::between (const Rot3& R2, - boost::optional H1, boost::optional H2) const { + OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const { if (H1) *H1 = -(R2.transpose()*rot_); - if (H2) *H2 = I3; + if (H2) *H2 = I_3x3; Matrix3 R12 = transpose()*R2.rot_; return Rot3(R12); } /* ************************************************************************* */ Point3 Rot3::rotate(const Point3& p, - boost::optional H1, boost::optional H2) const { + OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const { if (H1 || H2) { if (H1) *H1 = rot_ * skewSymmetric(-p.x(), -p.y(), -p.z()); if (H2) *H2 = rot_; @@ -257,7 +241,7 @@ Rot3 Rot3::retract(const Vector& omega, Rot3::CoordinatesMode mode) const { } else if(mode == Rot3::CAYLEY) { return retractCayley(omega); } else if(mode == Rot3::SLOW_CAYLEY) { - Matrix Omega = skewSymmetric(omega); + Matrix3 Omega = skewSymmetric(omega); return (*this)*CayleyFixed<3>(-Omega/2); } else { assert(false); diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp index 19de92ca2..26ca25bf2 100644 --- a/gtsam/geometry/Rot3Q.cpp +++ b/gtsam/geometry/Rot3Q.cpp @@ -87,22 +87,9 @@ namespace gtsam { Rot3 Rot3::rodriguez(const Vector& w, double theta) { return Quaternion(Eigen::AngleAxisd(theta, w)); } - /* ************************************************************************* */ - Rot3 Rot3::compose(const Rot3& R2) const { - return Rot3(quaternion_ * R2.quaternion_); - } - /* ************************************************************************* */ Rot3 Rot3::compose(const Rot3& R2, - boost::optional H1, boost::optional H2) const { - if (H1) *H1 = R2.transpose(); - if (H2) *H2 = I3; - return Rot3(quaternion_ * R2.quaternion_); - } - - /* ************************************************************************* */ - Rot3 Rot3::compose(const Rot3& R2, - boost::optional H1, boost::optional H2) const { + OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const { if (H1) *H1 = R2.transpose(); if (H2) *H2 = I3; return Rot3(quaternion_ * R2.quaternion_); @@ -114,7 +101,7 @@ namespace gtsam { } /* ************************************************************************* */ - Rot3 Rot3::inverse(boost::optional H1) const { + Rot3 Rot3::inverse(boost::optional H1) const { if (H1) *H1 = -matrix(); return Rot3(quaternion_.inverse()); } @@ -129,7 +116,7 @@ namespace gtsam { /* ************************************************************************* */ Rot3 Rot3::between(const Rot3& R2, - boost::optional H1, boost::optional H2) const { + OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const { if (H1) *H1 = -(R2.transpose()*matrix()); if (H2) *H2 = I3; return between_default(*this, R2); @@ -137,7 +124,7 @@ namespace gtsam { /* ************************************************************************* */ Point3 Rot3::rotate(const Point3& p, - boost::optional H1, boost::optional H2) const { + OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const { Matrix R = matrix(); if (H1) *H1 = R * skewSymmetric(-p.x(), -p.y(), -p.z()); if (H2) *H2 = R; diff --git a/gtsam/geometry/SimpleCamera.cpp b/gtsam/geometry/SimpleCamera.cpp index cab871874..d92c5bdd7 100644 --- a/gtsam/geometry/SimpleCamera.cpp +++ b/gtsam/geometry/SimpleCamera.cpp @@ -21,27 +21,27 @@ namespace gtsam { - SimpleCamera simpleCamera(const Matrix& P) { + SimpleCamera simpleCamera(const Matrix34& P) { // P = [A|a] = s K cRw [I|-T], with s the unknown scale - Matrix A = P.topLeftCorner(3, 3); - Vector a = P.col(3); + Matrix3 A = P.topLeftCorner(3, 3); + Vector3 a = P.col(3); // do RQ decomposition to get s*K and cRw angles - Matrix sK; - Vector xyz; + Matrix3 sK; + Vector3 xyz; boost::tie(sK, xyz) = RQ(A); // Recover scale factor s and K double s = sK(2, 2); - Matrix K = sK / s; + Matrix3 K = sK / s; // Recover cRw itself, and its inverse Rot3 cRw = Rot3::RzRyRx(xyz); Rot3 wRc = cRw.inverse(); // Now, recover T from a = - s K cRw T = - A T - Vector T = -(A.inverse() * a); + Vector3 T = -(A.inverse() * a); return SimpleCamera(Pose3(wRc, T), Cal3_S2(K(0, 0), K(1, 1), K(0, 1), K(0, 2), K(1, 2))); } diff --git a/gtsam/geometry/SimpleCamera.h b/gtsam/geometry/SimpleCamera.h index 557654d2d..c6d33bcb3 100644 --- a/gtsam/geometry/SimpleCamera.h +++ b/gtsam/geometry/SimpleCamera.h @@ -27,5 +27,5 @@ namespace gtsam { typedef PinholeCamera SimpleCamera; /// Recover camera from 3*4 camera matrix - GTSAM_EXPORT SimpleCamera simpleCamera(const Matrix& P); + GTSAM_EXPORT SimpleCamera simpleCamera(const Matrix34& P); } diff --git a/gtsam/geometry/StereoCamera.cpp b/gtsam/geometry/StereoCamera.cpp index b9e03c01d..9170f8282 100644 --- a/gtsam/geometry/StereoCamera.cpp +++ b/gtsam/geometry/StereoCamera.cpp @@ -30,8 +30,8 @@ namespace gtsam { /* ************************************************************************* */ StereoPoint2 StereoCamera::project(const Point3& point, - boost::optional H1, boost::optional H2, - boost::optional H3) const { + OptionalJacobian<3,6> H1, OptionalJacobian<3,3> H2, + OptionalJacobian<3,6> H3) const { #ifdef STEREOCAMERA_CHAIN_RULE const Point3 q = leftCamPose_.transform_to(point, H1, H2); @@ -58,28 +58,28 @@ namespace gtsam { if (H1 || H2) { #ifdef STEREOCAMERA_CHAIN_RULE // just implement chain rule - Matrix D_project_point = Dproject_to_stereo_camera1(q); // 3x3 Jacobian + Matrix3 D_project_point = Dproject_to_stereo_camera1(q); // 3x3 Jacobian if (H1) *H1 = D_project_point*(*H1); if (H2) *H2 = D_project_point*(*H2); #else // optimized version, see StereoCamera.nb if (H1) { const double v1 = v/fy, v2 = fx*v1, dx=d*x; - *H1 = (Matrix(3, 6) << - uL*v1, -fx-dx*uL, v2, -dfx, 0.0, d*uL, + *H1 << uL*v1, -fx-dx*uL, v2, -dfx, 0.0, d*uL, uR*v1, -fx-dx*uR, v2, -dfx, 0.0, d*uR, - fy + v*v1, -dx*v , -x*dfy, 0.0, -dfy, d*v - ).finished(); + fy + v*v1, -dx*v , -x*dfy, 0.0, -dfy, d*v; } if (H2) { - const Matrix R(leftCamPose_.rotation().matrix()); - *H2 = d * (Matrix(3, 3) << - fx*R(0, 0) - R(0, 2)*uL, fx*R(1, 0) - R(1, 2)*uL, fx*R(2, 0) - R(2, 2)*uL, - fx*R(0, 0) - R(0, 2)*uR, fx*R(1, 0) - R(1, 2)*uR, fx*R(2, 0) - R(2, 2)*uR, - fy*R(0, 1) - R(0, 2)*v , fy*R(1, 1) - R(1, 2)*v , fy*R(2, 1) - R(2, 2)*v - ).finished(); + const Matrix3 R(leftCamPose_.rotation().matrix()); + *H2 << fx*R(0, 0) - R(0, 2)*uL, fx*R(1, 0) - R(1, 2)*uL, fx*R(2, 0) - R(2, 2)*uL, + fx*R(0, 0) - R(0, 2)*uR, fx*R(1, 0) - R(1, 2)*uR, fx*R(2, 0) - R(2, 2)*uR, + fy*R(0, 1) - R(0, 2)*v , fy*R(1, 1) - R(1, 2)*v , fy*R(2, 1) - R(2, 2)*v; + *H2 << d * (*H2); } #endif + if (H3) + // TODO, this is set to zero, as Cal3_S2Stereo cannot be optimized yet + H3->setZero(); } // finally translate @@ -87,15 +87,15 @@ namespace gtsam { } /* ************************************************************************* */ - Matrix StereoCamera::Dproject_to_stereo_camera1(const Point3& P) const { + Matrix3 StereoCamera::Dproject_to_stereo_camera1(const Point3& P) const { double d = 1.0 / P.z(), d2 = d*d; const Cal3_S2Stereo& K = *K_; double f_x = K.fx(), f_y = K.fy(), b = K.baseline(); - return (Matrix(3, 3) << - f_x*d, 0.0, -d2*f_x* P.x(), + Matrix3 m; + m << f_x*d, 0.0, -d2*f_x* P.x(), f_x*d, 0.0, -d2*f_x*(P.x() - b), - 0.0, f_y*d, -d2*f_y* P.y() - ).finished(); + 0.0, f_y*d, -d2*f_y* P.y(); + return m; } } diff --git a/gtsam/geometry/StereoCamera.h b/gtsam/geometry/StereoCamera.h index 60ea7693d..7650aa086 100644 --- a/gtsam/geometry/StereoCamera.h +++ b/gtsam/geometry/StereoCamera.h @@ -90,14 +90,8 @@ public: } /// Local coordinates of manifold neighborhood around current value - inline Vector localCoordinates(const StereoCamera& t2) const { - return Vector(leftCamPose_.localCoordinates(t2.leftCamPose_)); - } - - Pose3 between(const StereoCamera &camera, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const { - return leftCamPose_.between(camera.pose(), H1, H2); + inline Vector6 localCoordinates(const StereoCamera& t2) const { + return leftCamPose_.localCoordinates(t2.leftCamPose_); } /// @} @@ -119,9 +113,9 @@ public: * @param H3 IGNORED (for calibration) */ StereoPoint2 project(const Point3& point, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none, - boost::optional H3 = boost::none) const; + OptionalJacobian<3, 6> H1 = boost::none, + OptionalJacobian<3, 3> H2 = boost::none, + OptionalJacobian<3, 6> H3 = boost::none) const; /** * @@ -139,7 +133,7 @@ public: private: /** utility functions */ - Matrix Dproject_to_stereo_camera1(const Point3& P) const; + Matrix3 Dproject_to_stereo_camera1(const Point3& P) const; friend class boost::serialization::access; template diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index 5e13129cc..5b4a8b33e 100644 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -39,14 +39,13 @@ using namespace std; namespace gtsam { /* ************************************************************************* */ -Unit3 Unit3::FromPoint3(const Point3& point, boost::optional H) { +Unit3 Unit3::FromPoint3(const Point3& point, OptionalJacobian<2,3> H) { Unit3 direction(point); if (H) { // 3*3 Derivative of representation with respect to point is 3*3: - Matrix D_p_point; + Matrix3 D_p_point; point.normalize(D_p_point); // TODO, this calculates norm a second time :-( // Calculate the 2*3 Jacobian - H->resize(2, 3); *H << direction.basis().transpose() * D_p_point; } return direction; @@ -67,7 +66,7 @@ Unit3 Unit3::Random(boost::mt19937 & rng) { } /* ************************************************************************* */ -const Unit3::Matrix32& Unit3::basis() const { +const Matrix32& Unit3::basis() const { // Return cached version if exists if (B_) @@ -92,7 +91,7 @@ const Unit3::Matrix32& Unit3::basis() const { b2 = b2 / b2.norm(); // Create the basis matrix - B_.reset(Unit3::Matrix32()); + B_.reset(Matrix32()); (*B_) << b1.x(), b2.x(), b1.y(), b2.y(), b1.z(), b2.z(); return *B_; } @@ -104,38 +103,39 @@ void Unit3::print(const std::string& s) const { } /* ************************************************************************* */ -Matrix Unit3::skew() const { +Matrix3 Unit3::skew() const { return skewSymmetric(p_.x(), p_.y(), p_.z()); } /* ************************************************************************* */ -Vector Unit3::error(const Unit3& q, boost::optional H) const { +Vector2 Unit3::error(const Unit3& q, OptionalJacobian<2,2> H) const { // 2D error is equal to B'*q, as B is 3x2 matrix and q is 3x1 - Matrix Bt = basis().transpose(); - Vector xi = Bt * q.p_.vector(); + Matrix23 Bt = basis().transpose(); + Vector2 xi = Bt * q.p_.vector(); if (H) *H = Bt * q.basis(); return xi; } /* ************************************************************************* */ -double Unit3::distance(const Unit3& q, boost::optional H) const { - Vector xi = error(q, H); +double Unit3::distance(const Unit3& q, OptionalJacobian<1,2> H) const { + Matrix2 H_; + Vector2 xi = error(q, H_); double theta = xi.norm(); if (H) - *H = (xi.transpose() / theta) * (*H); + *H = (xi.transpose() / theta) * H_; return theta; } /* ************************************************************************* */ -Unit3 Unit3::retract(const Vector& v) const { +Unit3 Unit3::retract(const Vector2& v) const { // Get the vector form of the point and the basis matrix - Vector p = Point3::Logmap(p_); - Matrix B = basis(); + Vector3 p = Point3::Logmap(p_); + Matrix32 B = basis(); // Compute the 3D xi_hat vector - Vector xi_hat = v(0) * B.col(0) + v(1) * B.col(1); + Vector3 xi_hat = v(0) * B.col(0) + v(1) * B.col(1); double xi_hat_norm = xi_hat.norm(); @@ -147,28 +147,28 @@ Unit3 Unit3::retract(const Vector& v) const { return Unit3(-point3()); } - Vector exp_p_xi_hat = cos(xi_hat_norm) * p + Vector3 exp_p_xi_hat = cos(xi_hat_norm) * p + sin(xi_hat_norm) * (xi_hat / xi_hat_norm); return Unit3(exp_p_xi_hat); } /* ************************************************************************* */ -Vector Unit3::localCoordinates(const Unit3& y) const { +Vector2 Unit3::localCoordinates(const Unit3& y) const { - Vector p = Point3::Logmap(p_); - Vector q = Point3::Logmap(y.p_); + Vector3 p = Point3::Logmap(p_); + Vector3 q = Point3::Logmap(y.p_); double dot = p.dot(q); // Check for special cases if (std::abs(dot - 1.0) < 1e-16) return Vector2(0, 0); else if (std::abs(dot + 1.0) < 1e-16) - return (Vector(2) << M_PI, 0).finished(); + return Vector2(M_PI, 0); else { // no special case double theta = acos(dot); - Vector result_hat = (theta / sin(theta)) * (q - p * dot); + Vector3 result_hat = (theta / sin(theta)) * (q - p * dot); return basis().transpose() * result_hat; } } diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h index d04e57d4b..f83404e74 100644 --- a/gtsam/geometry/Unit3.h +++ b/gtsam/geometry/Unit3.h @@ -32,8 +32,6 @@ class GTSAM_EXPORT Unit3{ private: - typedef Eigen::Matrix Matrix32; - Point3 p_; ///< The location of the point on the unit sphere mutable boost::optional B_; ///< Cached basis @@ -52,6 +50,11 @@ public: p_(p / p.norm()) { } + /// Construct from a vector3 + explicit Unit3(const Vector3& p) : + p_(p / p.norm()) { + } + /// Construct from x,y,z Unit3(double x, double y, double z) : p_(x, y, z) { @@ -59,7 +62,7 @@ public: } /// Named constructor from Point3 with optional Jacobian - static Unit3 FromPoint3(const Point3& point, boost::optional H = + static Unit3 FromPoint3(const Point3& point, OptionalJacobian<2,3> H = boost::none); /// Random direction, using boost::uniform_on_sphere @@ -90,10 +93,10 @@ public: const Matrix32& basis() const; /// Return skew-symmetric associated with 3D point on unit sphere - Matrix skew() const; + Matrix3 skew() const; /// Return unit-norm Point3 - const Point3& point3(boost::optional H = boost::none) const { + const Point3& point3(OptionalJacobian<3,2> H = boost::none) const { if (H) *H = basis(); return p_; @@ -105,12 +108,12 @@ public: } /// Signed, vector-valued error between two directions - Vector error(const Unit3& q, - boost::optional H = boost::none) const; + Vector2 error(const Unit3& q, + OptionalJacobian<2,2> H = boost::none) const; /// Distance between two directions double distance(const Unit3& q, - boost::optional H = boost::none) const; + OptionalJacobian<1,2> H = boost::none) const; /// @} @@ -133,10 +136,10 @@ public: }; /// The retract function - Unit3 retract(const Vector& v) const; + Unit3 retract(const Vector2& v) const; /// The local coordinates function - Vector localCoordinates(const Unit3& s) const; + Vector2 localCoordinates(const Unit3& s) const; /// @} @@ -144,7 +147,6 @@ private: /// @name Advanced Interface /// @{ - /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/geometry/tests/testCal3_S2.cpp b/gtsam/geometry/tests/testCal3_S2.cpp index 86c0b7e33..aa06a2e29 100644 --- a/gtsam/geometry/tests/testCal3_S2.cpp +++ b/gtsam/geometry/tests/testCal3_S2.cpp @@ -60,7 +60,7 @@ TEST( Cal3_S2, calibrate_homogeneous) { Point2 uncalibrate_(const Cal3_S2& k, const Point2& pt) { return k.uncalibrate(pt); } TEST( Cal3_S2, Duncalibrate1) { - Matrix computed; K.uncalibrate(p, computed, boost::none); + Matrix25 computed; K.uncalibrate(p, computed, boost::none); Matrix numerical = numericalDerivative21(uncalibrate_, K, p); CHECK(assert_equal(numerical,computed,1e-8)); } diff --git a/gtsam/geometry/tests/testCalibratedCamera.cpp b/gtsam/geometry/tests/testCalibratedCamera.cpp index 5aeee03d4..6a990e08e 100644 --- a/gtsam/geometry/tests/testCalibratedCamera.cpp +++ b/gtsam/geometry/tests/testCalibratedCamera.cpp @@ -15,12 +15,14 @@ * @brief test CalibratedCamera class */ -#include - -#include +#include +#include #include #include -#include + +#include + +#include using namespace std; using namespace gtsam; diff --git a/gtsam/geometry/tests/testPinholeCamera.cpp b/gtsam/geometry/tests/testPinholeCamera.cpp index ddeae2b7d..6cb84ec85 100644 --- a/gtsam/geometry/tests/testPinholeCamera.cpp +++ b/gtsam/geometry/tests/testPinholeCamera.cpp @@ -15,29 +15,28 @@ * @brief test PinholeCamera class */ -#include -#include - #include #include #include #include #include +#include + +#include +#include using namespace std; using namespace gtsam; +typedef PinholeCamera Camera; + static const Cal3_S2 K(625, 625, 0, 0, 0); -static const Pose3 pose1((Matrix)(Matrix(3,3) << - 1., 0., 0., - 0.,-1., 0., - 0., 0.,-1. - ).finished(), - Point3(0,0,0.5)); +static const Pose3 pose(Matrix3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5)); +static const Camera camera(pose, K); -typedef PinholeCamera Camera; -static const Camera camera(pose1, K); +static const Pose3 pose1(Rot3(), Point3(0, 1, 0.5)); +static const Camera camera1(pose1, K); static const Point3 point1(-0.08,-0.08, 0.0); static const Point3 point2(-0.08, 0.08, 0.0); @@ -52,8 +51,8 @@ static const Point3 point4_inf( 0.16,-0.16, -1.0); /* ************************************************************************* */ TEST( PinholeCamera, constructor) { - CHECK(assert_equal( camera.calibration(), K)); - CHECK(assert_equal( camera.pose(), pose1)); + EXPECT(assert_equal( camera.calibration(), K)); + EXPECT(assert_equal( camera.pose(), pose)); } /* ************************************************************************* */ @@ -67,7 +66,7 @@ TEST( PinholeCamera, level2) Point3 x(1,0,0),y(0,0,-1),z(0,1,0); Rot3 wRc(x,y,z); Pose3 expected(wRc,Point3(0.4,0.3,0.1)); - CHECK(assert_equal( camera.pose(), expected)); + EXPECT(assert_equal( camera.pose(), expected)); } /* ************************************************************************* */ @@ -80,72 +79,72 @@ TEST( PinholeCamera, lookat) // expected Point3 xc(0,1,0),yc(0,0,-1),zc(-1,0,0); Pose3 expected(Rot3(xc,yc,zc),C); - CHECK(assert_equal( camera.pose(), expected)); + EXPECT(assert_equal( camera.pose(), expected)); Point3 C2(30.0,0.0,10.0); Camera camera2 = Camera::Lookat(C2, Point3(), Point3(0.0,0.0,1.0)); Matrix R = camera2.pose().rotation().matrix(); Matrix I = trans(R)*R; - CHECK(assert_equal(I, eye(3))); + EXPECT(assert_equal(I, eye(3))); } /* ************************************************************************* */ TEST( PinholeCamera, project) { - CHECK(assert_equal( camera.project(point1), Point2(-100, 100) )); - CHECK(assert_equal( camera.project(point2), Point2(-100, -100) )); - CHECK(assert_equal( camera.project(point3), Point2( 100, -100) )); - CHECK(assert_equal( camera.project(point4), Point2( 100, 100) )); + EXPECT(assert_equal( camera.project(point1), Point2(-100, 100) )); + EXPECT(assert_equal( camera.project(point2), Point2(-100, -100) )); + EXPECT(assert_equal( camera.project(point3), Point2( 100, -100) )); + EXPECT(assert_equal( camera.project(point4), Point2( 100, 100) )); } /* ************************************************************************* */ TEST( PinholeCamera, backproject) { - CHECK(assert_equal( camera.backproject(Point2(-100, 100), 0.5), point1)); - CHECK(assert_equal( camera.backproject(Point2(-100, -100), 0.5), point2)); - CHECK(assert_equal( camera.backproject(Point2( 100, -100), 0.5), point3)); - CHECK(assert_equal( camera.backproject(Point2( 100, 100), 0.5), point4)); + EXPECT(assert_equal( camera.backproject(Point2(-100, 100), 0.5), point1)); + EXPECT(assert_equal( camera.backproject(Point2(-100, -100), 0.5), point2)); + EXPECT(assert_equal( camera.backproject(Point2( 100, -100), 0.5), point3)); + EXPECT(assert_equal( camera.backproject(Point2( 100, 100), 0.5), point4)); } /* ************************************************************************* */ TEST( PinholeCamera, backprojectInfinity) { - CHECK(assert_equal( camera.backprojectPointAtInfinity(Point2(-100, 100)), point1_inf)); - CHECK(assert_equal( camera.backprojectPointAtInfinity(Point2(-100, -100)), point2_inf)); - CHECK(assert_equal( camera.backprojectPointAtInfinity(Point2( 100, -100)), point3_inf)); - CHECK(assert_equal( camera.backprojectPointAtInfinity(Point2( 100, 100)), point4_inf)); + EXPECT(assert_equal( camera.backprojectPointAtInfinity(Point2(-100, 100)), point1_inf)); + EXPECT(assert_equal( camera.backprojectPointAtInfinity(Point2(-100, -100)), point2_inf)); + EXPECT(assert_equal( camera.backprojectPointAtInfinity(Point2( 100, -100)), point3_inf)); + EXPECT(assert_equal( camera.backprojectPointAtInfinity(Point2( 100, 100)), point4_inf)); } /* ************************************************************************* */ TEST( PinholeCamera, backproject2) { Point3 origin; - Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera looking down + Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera1 looking down Camera camera(Pose3(rot, origin), K); Point3 actual = camera.backproject(Point2(), 1.); Point3 expected(0., 1., 0.); pair x = camera.projectSafe(expected); - CHECK(assert_equal(expected, actual)); - CHECK(assert_equal(Point2(), x.first)); - CHECK(x.second); + EXPECT(assert_equal(expected, actual)); + EXPECT(assert_equal(Point2(), x.first)); + EXPECT(x.second); } /* ************************************************************************* */ TEST( PinholeCamera, backprojectInfinity2) { Point3 origin; - Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera looking down + Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera1 looking down Camera camera(Pose3(rot, origin), K); Point3 actual = camera.backprojectPointAtInfinity(Point2()); Point3 expected(0., 1., 0.); Point2 x = camera.projectPointAtInfinity(expected); - CHECK(assert_equal(expected, actual)); - CHECK(assert_equal(Point2(), x)); + EXPECT(assert_equal(expected, actual)); + EXPECT(assert_equal(Point2(), x)); } /* ************************************************************************* */ @@ -159,8 +158,8 @@ TEST( PinholeCamera, backprojectInfinity3) Point3 expected(0., 0., 1.); Point2 x = camera.projectPointAtInfinity(expected); - CHECK(assert_equal(expected, actual)); - CHECK(assert_equal(Point2(), x)); + EXPECT(assert_equal(expected, actual)); + EXPECT(assert_equal(Point2(), x)); } /* ************************************************************************* */ @@ -173,13 +172,13 @@ TEST( PinholeCamera, Dproject) { Matrix Dpose, Dpoint, Dcal; Point2 result = camera.project(point1, Dpose, Dpoint, Dcal); - Matrix numerical_pose = numericalDerivative31(project3, pose1, point1, K); - Matrix numerical_point = numericalDerivative32(project3, pose1, point1, K); - Matrix numerical_cal = numericalDerivative33(project3, pose1, point1, K); - CHECK(assert_equal(Point2(-100, 100), result)); - CHECK(assert_equal(numerical_pose, Dpose, 1e-7)); - CHECK(assert_equal(numerical_point, Dpoint, 1e-7)); - CHECK(assert_equal(numerical_cal, Dcal, 1e-7)); + Matrix numerical_pose = numericalDerivative31(project3, pose, point1, K); + Matrix Hexpected2 = numericalDerivative32(project3, pose, point1, K); + Matrix numerical_cal = numericalDerivative33(project3, pose, point1, K); + EXPECT(assert_equal(Point2(-100, 100), result)); + EXPECT(assert_equal(numerical_pose, Dpose, 1e-7)); + EXPECT(assert_equal(Hexpected2, Dpoint, 1e-7)); + EXPECT(assert_equal(numerical_cal, Dcal, 1e-7)); } /* ************************************************************************* */ @@ -190,21 +189,21 @@ static Point2 projectInfinity3(const Pose3& pose, const Point3& point3D, const C TEST( PinholeCamera, Dproject_Infinity) { Matrix Dpose, Dpoint, Dcal; - Point3 point3D(point1.x(), point1.y(), -10.0); // a point in front of the camera + Point3 point3D(point1.x(), point1.y(), -10.0); // a point in front of the camera1 // test Projection Point2 actual = camera.projectPointAtInfinity(point3D, Dpose, Dpoint, Dcal); Point2 expected(-5.0, 5.0); - CHECK(assert_equal(actual, expected, 1e-7)); + EXPECT(assert_equal(actual, expected, 1e-7)); // test Jacobians - Matrix numerical_pose = numericalDerivative31(projectInfinity3, pose1, point3D, K); - Matrix numerical_point = numericalDerivative32(projectInfinity3, pose1, point3D, K); - Matrix numerical_point2x2 = numerical_point.block(0,0,2,2); // only the direction to the point matters - Matrix numerical_cal = numericalDerivative33(projectInfinity3, pose1, point3D, K); - CHECK(assert_equal(numerical_pose, Dpose, 1e-7)); - CHECK(assert_equal(numerical_point2x2, Dpoint, 1e-7)); - CHECK(assert_equal(numerical_cal, Dcal, 1e-7)); + Matrix numerical_pose = numericalDerivative31(projectInfinity3, pose, point3D, K); + Matrix Hexpected2 = numericalDerivative32(projectInfinity3, pose, point3D, K); + Matrix numerical_point2x2 = Hexpected2.block(0,0,2,2); // only the direction to the point matters + Matrix numerical_cal = numericalDerivative33(projectInfinity3, pose, point3D, K); + EXPECT(assert_equal(numerical_pose, Dpose, 1e-7)); + EXPECT(assert_equal(numerical_point2x2, Dpoint, 1e-7)); + EXPECT(assert_equal(numerical_cal, Dcal, 1e-7)); } /* ************************************************************************* */ @@ -217,11 +216,80 @@ TEST( PinholeCamera, Dproject2) { Matrix Dcamera, Dpoint; Point2 result = camera.project2(point1, Dcamera, Dpoint); - Matrix numerical_camera = numericalDerivative21(project4, camera, point1); - Matrix numerical_point = numericalDerivative22(project4, camera, point1); - CHECK(assert_equal(result, Point2(-100, 100) )); - CHECK(assert_equal(numerical_camera, Dcamera, 1e-7)); - CHECK(assert_equal(numerical_point, Dpoint, 1e-7)); + Matrix Hexpected1 = numericalDerivative21(project4, camera, point1); + Matrix Hexpected2 = numericalDerivative22(project4, camera, point1); + EXPECT(assert_equal(result, Point2(-100, 100) )); + EXPECT(assert_equal(Hexpected1, Dcamera, 1e-7)); + EXPECT(assert_equal(Hexpected2, Dpoint, 1e-7)); +} + +/* ************************************************************************* */ +static double range0(const Camera& camera, const Point3& point) { + return camera.range(point); +} + +/* ************************************************************************* */ +TEST( PinholeCamera, range0) { + Matrix D1; Matrix D2; + double result = camera.range(point1, D1, D2); + Matrix Hexpected1 = numericalDerivative21(range0, camera, point1); + Matrix Hexpected2 = numericalDerivative22(range0, camera, point1); + EXPECT_DOUBLES_EQUAL(point1.distance(camera.pose().translation()), result, + 1e-9); + EXPECT(assert_equal(Hexpected1, D1, 1e-7)); + EXPECT(assert_equal(Hexpected2, D2, 1e-7)); +} + +/* ************************************************************************* */ +static double range1(const Camera& camera, const Pose3& pose) { + return camera.range(pose); +} + +/* ************************************************************************* */ +TEST( PinholeCamera, range1) { + Matrix D1; Matrix D2; + double result = camera.range(pose1, D1, D2); + Matrix Hexpected1 = numericalDerivative21(range1, camera, pose1); + Matrix Hexpected2 = numericalDerivative22(range1, camera, pose1); + EXPECT_DOUBLES_EQUAL(1, result, 1e-9); + EXPECT(assert_equal(Hexpected1, D1, 1e-7)); + EXPECT(assert_equal(Hexpected2, D2, 1e-7)); +} + +/* ************************************************************************* */ +typedef PinholeCamera Camera2; +static const Cal3Bundler K2(625, 1e-3, 1e-3); +static const Camera2 camera2(pose1, K2); +static double range2(const Camera& camera, const Camera2& camera2) { + return camera.range(camera2); +} + +/* ************************************************************************* */ +TEST( PinholeCamera, range2) { + Matrix D1; Matrix D2; + double result = camera.range(camera2, D1, D2); + Matrix Hexpected1 = numericalDerivative21(range2, camera, camera2); + Matrix Hexpected2 = numericalDerivative22(range2, camera, camera2); + EXPECT_DOUBLES_EQUAL(1, result, 1e-9); + EXPECT(assert_equal(Hexpected1, D1, 1e-7)); + EXPECT(assert_equal(Hexpected2, D2, 1e-7)); +} + +/* ************************************************************************* */ +static const CalibratedCamera camera3(pose1); +static double range3(const Camera& camera, const CalibratedCamera& camera3) { + return camera.range(camera3); +} + +/* ************************************************************************* */ +TEST( PinholeCamera, range3) { + Matrix D1; Matrix D2; + double result = camera.range(camera3, D1, D2); + Matrix Hexpected1 = numericalDerivative21(range3, camera, camera3); + Matrix Hexpected2 = numericalDerivative22(range3, camera, camera3); + EXPECT_DOUBLES_EQUAL(1, result, 1e-9); + EXPECT(assert_equal(Hexpected1, D1, 1e-7)); + EXPECT(assert_equal(Hexpected2, D2, 1e-7)); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index b4de6eb7c..f12d96899 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -44,7 +44,7 @@ TEST(Pose2, constructors) { Pose2 origin; assert_equal(pose,origin); Pose2 t(M_PI/2.0+0.018, Point2(1.015, 2.01)); - EXPECT(assert_equal(t,Pose2(t.matrix()))); + EXPECT(assert_equal(t,Pose2((Matrix)t.matrix()))); } /* ************************************************************************* */ @@ -519,7 +519,7 @@ TEST( Pose2, bearing ) expectedH1 = numericalDerivative21(bearing_proxy, x2, l3); EXPECT(assert_equal(expectedH1,actualH1)); expectedH2 = numericalDerivative22(bearing_proxy, x2, l3); - EXPECT(assert_equal(expectedH1,actualH1)); + EXPECT(assert_equal(expectedH2,actualH2)); // establish bearing is indeed 45 degrees even if rotated Rot2 actual34 = x3.bearing(l4, actualH1, actualH2); @@ -529,7 +529,7 @@ TEST( Pose2, bearing ) expectedH1 = numericalDerivative21(bearing_proxy, x3, l4); expectedH2 = numericalDerivative22(bearing_proxy, x3, l4); EXPECT(assert_equal(expectedH1,actualH1)); - EXPECT(assert_equal(expectedH1,actualH1)); + EXPECT(assert_equal(expectedH2,actualH2)); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testRot2.cpp b/gtsam/geometry/tests/testRot2.cpp index 8a1f942d2..cfb103c5d 100644 --- a/gtsam/geometry/tests/testRot2.cpp +++ b/gtsam/geometry/tests/testRot2.cpp @@ -49,7 +49,9 @@ TEST( Rot2, unit) /* ************************************************************************* */ TEST( Rot2, transpose) { - CHECK(assert_equal(R.inverse().matrix(),R.transpose())); + Matrix expected = R.inverse().matrix(); + Matrix actual = R.transpose(); + CHECK(assert_equal(expected,actual)); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 88accb90f..c89d2dacb 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -37,7 +37,6 @@ GTSAM_CONCEPT_LIE_INST(Rot3) static Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2); static Point3 P(0.2, 0.7, -2.0); static double error = 1e-9, epsilon = 0.001; -static const Matrix I3 = eye(3); /* ************************************************************************* */ TEST( Rot3, chart) @@ -50,7 +49,7 @@ TEST( Rot3, chart) /* ************************************************************************* */ TEST( Rot3, constructor) { - Rot3 expected(I3); + Rot3 expected((Matrix)I_3x3); Point3 r1(1,0,0), r2(0,1,0), r3(0,0,1); Rot3 actual(r1, r2, r3); CHECK(assert_equal(actual,expected)); @@ -95,7 +94,7 @@ Rot3 slow_but_correct_rodriguez(const Vector& w) { double t = norm_2(w); Matrix J = skewSymmetric(w / t); if (t < 1e-5) return Rot3(); - Matrix R = I3 + sin(t) * J + (1.0 - cos(t)) * (J * J); + Matrix R = I_3x3 + sin(t) * J + (1.0 - cos(t)) * (J * J); return R; } @@ -359,7 +358,7 @@ TEST( Rot3, inverse ) Rot3 R = Rot3::rodriguez(0.1, 0.2, 0.3); Rot3 I; - Matrix actualH; + Matrix3 actualH; Rot3 actual = R.inverse(actualH); CHECK(assert_equal(I,R*actual)); CHECK(assert_equal(I,actual*R)); @@ -482,7 +481,7 @@ TEST( Rot3, RQ) Vector actual; boost::tie(actualK, actual) = RQ(R.matrix()); Vector expected = Vector3(0.14715, 0.385821, 0.231671); - CHECK(assert_equal(I3,actualK)); + CHECK(assert_equal(I_3x3,actualK)); CHECK(assert_equal(expected,actual,1e-6)); // Try using xyz call, asserting that Rot3::RzRyRx(x,y,z).xyz()==[x;y;z] @@ -516,7 +515,7 @@ TEST( Rot3, expmapStability ) { w(2), 0.0, -w(0), -w(1), w(0), 0.0 ).finished(); Matrix W2 = W*W; - Matrix Rmat = I3 + (1.0-theta2/6.0 + theta2*theta2/120.0 + Matrix Rmat = I_3x3 + (1.0-theta2/6.0 + theta2*theta2/120.0 - theta2*theta2*theta2/5040.0)*W + (0.5 - theta2/24.0 + theta2*theta2/720.0)*W2 ; Rot3 expectedR( Rmat ); CHECK(assert_equal(expectedR, actualR, 1e-10)); @@ -578,7 +577,7 @@ TEST(Rot3, quaternion) { TEST( Rot3, Cayley ) { Matrix A = skewSymmetric(1,2,-3); Matrix Q = Cayley(A); - EXPECT(assert_equal(I3, trans(Q)*Q)); + EXPECT(assert_equal((Matrix)I_3x3, trans(Q)*Q)); EXPECT(assert_equal(A, Cayley(Q))); } diff --git a/gtsam/geometry/tests/testRot3M.cpp b/gtsam/geometry/tests/testRot3M.cpp index 1313a3be5..a25db07f6 100644 --- a/gtsam/geometry/tests/testRot3M.cpp +++ b/gtsam/geometry/tests/testRot3M.cpp @@ -37,7 +37,6 @@ GTSAM_CONCEPT_LIE_INST(Rot3) static Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2); static Point3 P(0.2, 0.7, -2.0); -static const Matrix I3 = eye(3); /* ************************************************************************* */ TEST(Rot3, manifold_cayley) diff --git a/gtsam/geometry/tests/testStereoCamera.cpp b/gtsam/geometry/tests/testStereoCamera.cpp index eebe2d60a..c77509b91 100644 --- a/gtsam/geometry/tests/testStereoCamera.cpp +++ b/gtsam/geometry/tests/testStereoCamera.cpp @@ -74,7 +74,7 @@ TEST( StereoCamera, project) /* ************************************************************************* */ -static Pose3 camera1((Matrix)(Matrix(3,3) << +static Pose3 camPose((Matrix)(Matrix(3,3) << 1., 0., 0., 0.,-1., 0., 0., 0.,-1. @@ -82,33 +82,41 @@ static Pose3 camera1((Matrix)(Matrix(3,3) << Point3(0,0,6.25)); static Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5)); -static StereoCamera stereoCam(Pose3(), K); +static StereoCamera stereoCam(camPose, K); // point X Y Z in meters -static Point3 p(0, 0, 5); +static Point3 landmark(0, 0, 5); /* ************************************************************************* */ -static StereoPoint2 project_(const StereoCamera& cam, const Point3& point) { return cam.project(point); } -TEST( StereoCamera, Dproject_stereo_pose) -{ - Matrix expected = numericalDerivative21(project_,stereoCam, p); - Matrix actual; stereoCam.project(p, actual, boost::none); - CHECK(assert_equal(expected,actual,1e-7)); +static StereoPoint2 project3(const Pose3& pose, const Point3& point, const Cal3_S2Stereo& K) { + return StereoCamera(pose, boost::make_shared(K)).project(point); } /* ************************************************************************* */ -TEST( StereoCamera, Dproject_stereo_point) +TEST( StereoCamera, Dproject) { - Matrix expected = numericalDerivative22(project_,stereoCam, p); - Matrix actual; stereoCam.project(p, boost::none, actual); - CHECK(assert_equal(expected,actual,1e-8)); + Matrix expected1 = numericalDerivative31(project3, camPose, landmark, *K); + Matrix actual1; stereoCam.project(landmark, actual1, boost::none, boost::none); + CHECK(assert_equal(expected1,actual1,1e-7)); + + Matrix expected2 = numericalDerivative32(project3,camPose, landmark, *K); + Matrix actual2; stereoCam.project(landmark, boost::none, actual2, boost::none); + CHECK(assert_equal(expected2,actual2,1e-7)); + + Matrix expected3 = numericalDerivative33(project3,camPose, landmark, *K); + Matrix actual3; stereoCam.project(landmark, boost::none, boost::none, actual3); +// CHECK(assert_equal(expected3,actual3,1e-8)); } +/* ************************************************************************* */ TEST( StereoCamera, backproject) { + Cal3_S2Stereo::shared_ptr K2(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5)); + StereoCamera stereoCam2(Pose3(), K2); + Point3 expected(1.2, 2.3, 4.5); - StereoPoint2 stereo_point = stereoCam.project(expected); - Point3 actual = stereoCam.backproject(stereo_point); + StereoPoint2 stereo_point = stereoCam2.project(expected); + Point3 actual = stereoCam2.backproject(stereo_point); CHECK(assert_equal(expected,actual,1e-8)); } diff --git a/gtsam/geometry/tests/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp index 0bd553a40..f986cca1d 100644 --- a/gtsam/geometry/tests/testTriangulation.cpp +++ b/gtsam/geometry/tests/testTriangulation.cpp @@ -262,25 +262,6 @@ TEST( triangulation, twoIdenticalPoses) { } */ -//****************************************************************************** -TEST( triangulation, TriangulationFactor ) { - // Create the factor with a measurement that is 3 pixels off in x - Key pointKey(1); - SharedNoiseModel model; - typedef TriangulationFactor<> Factor; - Factor factor(camera1, z1, model, pointKey); - - // Use the factor to calculate the Jacobians - Matrix HActual; - factor.evaluateError(landmark, HActual); - - Matrix HExpected = numericalDerivative11( - boost::bind(&Factor::evaluateError, &factor, _1, boost::none), landmark); - - // Verify the Jacobians are correct - CHECK(assert_equal(HExpected, HActual, 1e-3)); -} - //****************************************************************************** int main() { TestResult tr; diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp index 0102477b3..072f3b7ad 100644 --- a/gtsam/geometry/tests/testUnit3.cpp +++ b/gtsam/geometry/tests/testUnit3.cpp @@ -108,9 +108,9 @@ TEST(Unit3, unrotate) { TEST(Unit3, error) { Unit3 p(1, 0, 0), q = p.retract(Vector2(0.5, 0)), // r = p.retract(Vector2(0.8, 0)); - EXPECT(assert_equal(Vector2(0, 0), p.error(p), 1e-8)); - EXPECT(assert_equal(Vector2(0.479426, 0), p.error(q), 1e-5)); - EXPECT(assert_equal(Vector2(0.717356, 0), p.error(r), 1e-5)); + EXPECT(assert_equal((Vector)(Vector2(0, 0)), p.error(p), 1e-8)); + EXPECT(assert_equal((Vector)(Vector2(0.479426, 0)), p.error(q), 1e-5)); + EXPECT(assert_equal((Vector)(Vector2(0.717356, 0)), p.error(r), 1e-5)); Matrix actual, expected; // Use numerical derivatives to calculate the expected Jacobian diff --git a/gtsam/geometry/triangulation.cpp b/gtsam/geometry/triangulation.cpp index 9e1575801..474689525 100644 --- a/gtsam/geometry/triangulation.cpp +++ b/gtsam/geometry/triangulation.cpp @@ -30,7 +30,7 @@ namespace gtsam { * @param rank_tol SVD rank tolerance * @return Triangulated Point3 */ -Point3 triangulateDLT(const std::vector& projection_matrices, +Point3 triangulateDLT(const std::vector& projection_matrices, const std::vector& measurements, double rank_tol) { // number of cameras @@ -41,7 +41,7 @@ Point3 triangulateDLT(const std::vector& projection_matrices, for (size_t i = 0; i < m; i++) { size_t row = i * 2; - const Matrix& projection = projection_matrices.at(i); + const Matrix34& projection = projection_matrices.at(i); const Point2& p = measurements.at(i); // build system of equations diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index fabcc4c02..ce83f780b 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -19,12 +19,10 @@ #pragma once -#include +#include +#include #include #include -#include - -#include namespace gtsam { @@ -53,7 +51,7 @@ public: * @return Triangulated Point3 */ GTSAM_EXPORT Point3 triangulateDLT( - const std::vector& projection_matrices, + const std::vector& projection_matrices, const std::vector& measurements, double rank_tol); /// @@ -189,12 +187,11 @@ Point3 triangulatePoint3(const std::vector& poses, throw(TriangulationUnderconstrainedException()); // construct projection matrices from poses & calibration - std::vector projection_matrices; + std::vector projection_matrices; BOOST_FOREACH(const Pose3& pose, poses) { projection_matrices.push_back( - sharedCal->K() * sub(pose.inverse().matrix(), 0, 3, 0, 4)); + sharedCal->K() * (pose.inverse().matrix()).block<3,4>(0,0)); } - // Triangulate linearly Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol); @@ -240,12 +237,11 @@ Point3 triangulatePoint3( // construct projection matrices from poses & calibration typedef PinholeCamera Camera; - std::vector projection_matrices; - BOOST_FOREACH(const Camera& camera, cameras) - projection_matrices.push_back( - camera.calibration().K() - * sub(camera.pose().inverse().matrix(), 0, 3, 0, 4)); - + std::vector projection_matrices; + BOOST_FOREACH(const Camera& camera, cameras) { + Matrix P_ = (camera.pose().inverse().matrix()); + projection_matrices.push_back(camera.calibration().K()* (P_.block<3,4>(0,0)) ); + } Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol); // The n refine using non-linear optimization diff --git a/gtsam/linear/IterativeSolver.h b/gtsam/linear/IterativeSolver.h index 3c397c9e9..d6e1b6e98 100644 --- a/gtsam/linear/IterativeSolver.h +++ b/gtsam/linear/IterativeSolver.h @@ -106,9 +106,9 @@ namespace gtsam { typedef boost::tuple Base; KeyInfoEntry(){} KeyInfoEntry(size_t idx, size_t d, Key start) : Base(idx, d, start) {} - const size_t index() const { return this->get<0>(); } - const size_t dim() const { return this->get<1>(); } - const size_t colstart() const { return this->get<2>(); } + size_t index() const { return this->get<0>(); } + size_t dim() const { return this->get<1>(); } + size_t colstart() const { return this->get<2>(); } }; /************************************************************************************/ diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp new file mode 100644 index 000000000..a2ce631ea --- /dev/null +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -0,0 +1,497 @@ +/* ---------------------------------------------------------------------------- + + * 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 CombinedImuFactor.cpp + * @author Luca Carlone + * @author Stephen Williams + * @author Richard Roberts + * @author Vadim Indelman + * @author David Jensen + * @author Frank Dellaert + **/ + +#include + +/* External or standard includes */ +#include + +namespace gtsam { + +using namespace std; + +//------------------------------------------------------------------------------ +// Inner class CombinedPreintegratedMeasurements +//------------------------------------------------------------------------------ +CombinedImuFactor::CombinedPreintegratedMeasurements::CombinedPreintegratedMeasurements( + const imuBias::ConstantBias& bias, const Matrix3& measuredAccCovariance, + const Matrix3& measuredOmegaCovariance, const Matrix3& integrationErrorCovariance, + const Matrix3& biasAccCovariance, const Matrix3& biasOmegaCovariance, + const Matrix& biasAccOmegaInit, const bool use2ndOrderIntegration) : + biasHat_(bias), deltaPij_(Vector3::Zero()), deltaVij_(Vector3::Zero()), + deltaRij_(Rot3()), deltaTij_(0.0), + delPdelBiasAcc_(Z_3x3), delPdelBiasOmega_(Z_3x3), + delVdelBiasAcc_(Z_3x3), delVdelBiasOmega_(Z_3x3), + delRdelBiasOmega_(Z_3x3), use2ndOrderIntegration_(use2ndOrderIntegration) +{ + measurementCovariance_.setZero(); + measurementCovariance_.block<3,3>(0,0) = integrationErrorCovariance; + measurementCovariance_.block<3,3>(3,3) = measuredAccCovariance; + measurementCovariance_.block<3,3>(6,6) = measuredOmegaCovariance; + measurementCovariance_.block<3,3>(9,9) = biasAccCovariance; + measurementCovariance_.block<3,3>(12,12) = biasOmegaCovariance; + measurementCovariance_.block<6,6>(15,15) = biasAccOmegaInit; + PreintMeasCov_.setZero(); +} + +//------------------------------------------------------------------------------ +void CombinedImuFactor::CombinedPreintegratedMeasurements::print(const string& s) const{ + cout << s << endl; + biasHat_.print(" biasHat"); + cout << " deltaTij " << deltaTij_ << endl; + cout << " deltaPij [ " << deltaPij_.transpose() << " ]" << endl; + cout << " deltaVij [ " << deltaVij_.transpose() << " ]" << endl; + deltaRij_.print(" deltaRij "); + cout << " measurementCovariance [ " << measurementCovariance_ << " ]" << endl; + cout << " PreintMeasCov [ " << PreintMeasCov_ << " ]" << endl; +} + +//------------------------------------------------------------------------------ +bool CombinedImuFactor::CombinedPreintegratedMeasurements::equals(const CombinedPreintegratedMeasurements& expected, double tol) const{ + return biasHat_.equals(expected.biasHat_, tol) + && equal_with_abs_tol(measurementCovariance_, expected.measurementCovariance_, tol) + && equal_with_abs_tol(deltaPij_, expected.deltaPij_, tol) + && equal_with_abs_tol(deltaVij_, expected.deltaVij_, tol) + && deltaRij_.equals(expected.deltaRij_, tol) + && fabs(deltaTij_ - expected.deltaTij_) < tol + && equal_with_abs_tol(delPdelBiasAcc_, expected.delPdelBiasAcc_, tol) + && equal_with_abs_tol(delPdelBiasOmega_, expected.delPdelBiasOmega_, tol) + && equal_with_abs_tol(delVdelBiasAcc_, expected.delVdelBiasAcc_, tol) + && equal_with_abs_tol(delVdelBiasOmega_, expected.delVdelBiasOmega_, tol) + && equal_with_abs_tol(delRdelBiasOmega_, expected.delRdelBiasOmega_, tol); +} + +//------------------------------------------------------------------------------ +void CombinedImuFactor::CombinedPreintegratedMeasurements::resetIntegration(){ + deltaPij_ = Vector3::Zero(); + deltaVij_ = Vector3::Zero(); + deltaRij_ = Rot3(); + deltaTij_ = 0.0; + delPdelBiasAcc_ = Z_3x3; + delPdelBiasOmega_ = Z_3x3; + delVdelBiasAcc_ = Z_3x3; + delVdelBiasOmega_ = Z_3x3; + delRdelBiasOmega_ = Z_3x3; + PreintMeasCov_.setZero(); +} + +//------------------------------------------------------------------------------ +void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement( + const Vector3& measuredAcc, const Vector3& measuredOmega, + double deltaT, boost::optional body_P_sensor) { + + // NOTE: order is important here because each update uses old values, e.g., velocity and position updates are based on previous rotation estimate. + // (i.e., we have to update jacobians and covariances before updating preintegrated measurements). + + // First we compensate the measurements for the bias: since we have only an estimate of the bias, the covariance includes the corresponding uncertainty + Vector3 correctedAcc = biasHat_.correctAccelerometer(measuredAcc); + Vector3 correctedOmega = biasHat_.correctGyroscope(measuredOmega); + + // Then compensate for sensor-body displacement: we express the quantities (originally in the IMU frame) into the body frame + if(body_P_sensor){ + Matrix3 body_R_sensor = body_P_sensor->rotation().matrix(); + correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame + Matrix3 body_omega_body__cross = skewSymmetric(correctedOmega); + correctedAcc = body_R_sensor * correctedAcc - body_omega_body__cross * body_omega_body__cross * body_P_sensor->translation().vector(); + // linear acceleration vector in the body frame + } + + const Vector3 theta_incr = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement + const Rot3 Rincr = Rot3::Expmap(theta_incr); // rotation increment computed from the current rotation rate measurement + const Matrix3 Jr_theta_incr = Rot3::rightJacobianExpMapSO3(theta_incr); // Right jacobian computed at theta_incr + + // Update Jacobians + /* ----------------------------------------------------------------------------------------------------------------------- */ + if(!use2ndOrderIntegration_){ + delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT; + delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT; + }else{ + delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - 0.5 * deltaRij_.matrix() * deltaT*deltaT; + delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT - 0.5 * deltaRij_.matrix() + * skewSymmetric(biasHat_.correctAccelerometer(measuredAcc)) * deltaT*deltaT * delRdelBiasOmega_; + } + + delVdelBiasAcc_ += -deltaRij_.matrix() * deltaT; + delVdelBiasOmega_ += -deltaRij_.matrix() * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega_; + delRdelBiasOmega_ = Rincr.inverse().matrix() * delRdelBiasOmega_ - Jr_theta_incr * deltaT; + + // Update preintegrated measurements covariance: as in [2] we consider a first order propagation that + // can be seen as a prediction phase in an EKF framework. In this implementation, contrarily to [2] we + // consider the uncertainty of the bias selection and we keep correlation between biases and preintegrated measurements + /* ----------------------------------------------------------------------------------------------------------------------- */ + const Vector3 theta_i = Rot3::Logmap(deltaRij_); // parametrization of so(3) + const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3(theta_i); + + Rot3 Rot_j = deltaRij_ * Rincr; + const Vector3 theta_j = Rot3::Logmap(Rot_j); // parametrization of so(3) + const Matrix3 Jrinv_theta_j = Rot3::rightJacobianExpMapSO3inverse(theta_j); + + // Single Jacobians to propagate covariance + Matrix3 H_pos_pos = I_3x3; + Matrix3 H_pos_vel = I_3x3 * deltaT; + Matrix3 H_pos_angles = Z_3x3; + + Matrix3 H_vel_pos = Z_3x3; + Matrix3 H_vel_vel = I_3x3; + Matrix3 H_vel_angles = - deltaRij_.matrix() * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT; + // analytic expression corresponding to the following numerical derivative + // Matrix H_vel_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_vel, correctedOmega, correctedAcc, deltaT, _1, deltaVij), theta_i); + Matrix3 H_vel_biasacc = - deltaRij_.matrix() * deltaT; + + Matrix3 H_angles_pos = Z_3x3; + Matrix3 H_angles_vel = Z_3x3; + Matrix3 H_angles_angles = Jrinv_theta_j * Rincr.inverse().matrix() * Jr_theta_i; + Matrix3 H_angles_biasomega =- Jrinv_theta_j * Jr_theta_incr * deltaT; + // analytic expression corresponding to the following numerical derivative + // Matrix H_angles_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_angles, correctedOmega, deltaT, _1), thetaij); + + // overall Jacobian wrt preintegrated measurements (df/dx) + Matrix F(15,15); + F << H_pos_pos, H_pos_vel, H_pos_angles, Z_3x3, Z_3x3, + H_vel_pos, H_vel_vel, H_vel_angles, H_vel_biasacc, Z_3x3, + H_angles_pos, H_angles_vel, H_angles_angles, Z_3x3, H_angles_biasomega, + Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, + Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3; + + // first order uncertainty propagation + // Optimized matrix multiplication (1/deltaT) * G * measurementCovariance * G.transpose() + + Matrix G_measCov_Gt = Matrix::Zero(15,15); + // BLOCK DIAGONAL TERMS + G_measCov_Gt.block<3,3>(0,0) = deltaT * measurementCovariance_.block<3,3>(0,0); + + G_measCov_Gt.block<3,3>(3,3) = (1/deltaT) * (H_vel_biasacc) * + (measurementCovariance_.block<3,3>(3,3) + measurementCovariance_.block<3,3>(15,15) ) * + (H_vel_biasacc.transpose()); + + G_measCov_Gt.block<3,3>(6,6) = (1/deltaT) * (H_angles_biasomega) * + (measurementCovariance_.block<3,3>(6,6) + measurementCovariance_.block<3,3>(18,18) ) * + (H_angles_biasomega.transpose()); + + G_measCov_Gt.block<3,3>(9,9) = deltaT * measurementCovariance_.block<3,3>(9,9); + + G_measCov_Gt.block<3,3>(12,12) = deltaT * measurementCovariance_.block<3,3>(12,12); + + // NEW OFF BLOCK DIAGONAL TERMS + Matrix3 block23 = H_vel_biasacc * measurementCovariance_.block<3,3>(18,15) * H_angles_biasomega.transpose(); + G_measCov_Gt.block<3,3>(3,6) = block23; + G_measCov_Gt.block<3,3>(6,3) = block23.transpose(); + + PreintMeasCov_ = F * PreintMeasCov_ * F.transpose() + G_measCov_Gt; + + // Update preintegrated measurements + /* ----------------------------------------------------------------------------------------------------------------------- */ + if(!use2ndOrderIntegration_){ + deltaPij_ += deltaVij_ * deltaT; + }else{ + deltaPij_ += deltaVij_ * deltaT + 0.5 * deltaRij_.matrix() * biasHat_.correctAccelerometer(measuredAcc) * deltaT*deltaT; + } + deltaVij_ += deltaRij_.matrix() * correctedAcc * deltaT; + deltaRij_ = deltaRij_ * Rincr; + deltaTij_ += deltaT; +} + +//------------------------------------------------------------------------------ +// CombinedImuFactor methods +//------------------------------------------------------------------------------ +CombinedImuFactor::CombinedImuFactor() : + preintegratedMeasurements_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Matrix::Zero(6,6)) {} + +//------------------------------------------------------------------------------ +CombinedImuFactor::CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j, + const CombinedPreintegratedMeasurements& preintegratedMeasurements, + const Vector3& gravity, const Vector3& omegaCoriolis, + boost::optional body_P_sensor, const bool use2ndOrderCoriolis) : + Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.PreintMeasCov_), pose_i, vel_i, pose_j, vel_j, bias_i, bias_j), + preintegratedMeasurements_(preintegratedMeasurements), + gravity_(gravity), + omegaCoriolis_(omegaCoriolis), + body_P_sensor_(body_P_sensor), + use2ndOrderCoriolis_(use2ndOrderCoriolis){ +} + +//------------------------------------------------------------------------------ +gtsam::NonlinearFactor::shared_ptr CombinedImuFactor::clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); +} + +//------------------------------------------------------------------------------ +void CombinedImuFactor::print(const string& s, const KeyFormatter& keyFormatter) const { + cout << s << "CombinedImuFactor(" + << keyFormatter(this->key1()) << "," + << keyFormatter(this->key2()) << "," + << keyFormatter(this->key3()) << "," + << keyFormatter(this->key4()) << "," + << keyFormatter(this->key5()) << "," + << keyFormatter(this->key6()) << ")\n"; + preintegratedMeasurements_.print(" preintegrated measurements:"); + cout << " gravity: [ " << gravity_.transpose() << " ]" << endl; + cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << endl; + this->noiseModel_->print(" noise model: "); + if(this->body_P_sensor_) + this->body_P_sensor_->print(" sensor pose in body frame: "); +} + +//------------------------------------------------------------------------------ +bool CombinedImuFactor::equals(const NonlinearFactor& expected, double tol) const { + const This *e = dynamic_cast (&expected); + return e != NULL && Base::equals(*e, tol) + && preintegratedMeasurements_.equals(e->preintegratedMeasurements_, tol) + && equal_with_abs_tol(gravity_, e->gravity_, tol) + && equal_with_abs_tol(omegaCoriolis_, e->omegaCoriolis_, tol) + && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_))); +} + +//------------------------------------------------------------------------------ +Vector CombinedImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, + const imuBias::ConstantBias& bias_i, const imuBias::ConstantBias& bias_j, + boost::optional H1, boost::optional H2, + boost::optional H3, boost::optional H4, + boost::optional H5, boost::optional H6) const { + + const double& deltaTij = preintegratedMeasurements_.deltaTij_; + const Vector3 biasAccIncr = bias_i.accelerometer() - preintegratedMeasurements_.biasHat_.accelerometer(); + const Vector3 biasOmegaIncr = bias_i.gyroscope() - preintegratedMeasurements_.biasHat_.gyroscope(); + + // we give some shorter name to rotations and translations + const Rot3 Rot_i = pose_i.rotation(); + const Rot3 Rot_j = pose_j.rotation(); + const Vector3 pos_i = pose_i.translation().vector(); + const Vector3 pos_j = pose_j.translation().vector(); + + // We compute factor's Jacobians, according to [3] + /* ---------------------------------------------------------------------------------------------------- */ + const Rot3 deltaRij_biascorrected = preintegratedMeasurements_.deltaRij_.retract(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP); + // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr) + + Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); + + Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected - + Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij; // Coriolis term + + const Rot3 deltaRij_biascorrected_corioliscorrected = + Rot3::Expmap( theta_biascorrected_corioliscorrected ); + + const Rot3 fRhat = deltaRij_biascorrected_corioliscorrected.between(Rot_i.between(Rot_j)); + + const Matrix3 Jr_theta_bcc = Rot3::rightJacobianExpMapSO3(theta_biascorrected_corioliscorrected); + + const Matrix3 Jtheta = -Jr_theta_bcc * skewSymmetric(Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij); + + const Matrix3 Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat)); + + if(H1) { + H1->resize(15,6); + + Matrix3 dfPdPi; + Matrix3 dfVdPi; + if(use2ndOrderCoriolis_){ + dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij; + dfVdPi = skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij; + } + else{ + dfPdPi = - Rot_i.matrix(); + dfVdPi = Z_3x3; + } + + (*H1) << + // dfP/dRi + Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij_ + + preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr), + // dfP/dPi + dfPdPi, + // dfV/dRi + Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij_ + + preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr), + // dfV/dPi + dfVdPi, + // dfR/dRi + Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta), + // dfR/dPi + Z_3x3, + //dBiasAcc/dPi + Z_3x3, Z_3x3, + //dBiasOmega/dPi + Z_3x3, Z_3x3; + } + + if(H2) { + H2->resize(15,3); + (*H2) << + // dfP/dVi + - I_3x3 * deltaTij + + skewSymmetric(omegaCoriolis_) * deltaTij * deltaTij, // Coriolis term - we got rid of the 2 wrt ins paper + // dfV/dVi + - I_3x3 + + 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term + // dfR/dVi + Z_3x3, + //dBiasAcc/dVi + Z_3x3, + //dBiasOmega/dVi + Z_3x3; + } + + if(H3) { + H3->resize(15,6); + (*H3) << + // dfP/dPosej + Z_3x3, Rot_j.matrix(), + // dfV/dPosej + Matrix::Zero(3,6), + // dfR/dPosej + Jrinv_fRhat * ( I_3x3 ), Z_3x3, + //dBiasAcc/dPosej + Z_3x3, Z_3x3, + //dBiasOmega/dPosej + Z_3x3, Z_3x3; + } + + if(H4) { + H4->resize(15,3); + (*H4) << + // dfP/dVj + Z_3x3, + // dfV/dVj + I_3x3, + // dfR/dVj + Z_3x3, + //dBiasAcc/dVj + Z_3x3, + //dBiasOmega/dVj + Z_3x3; + } + + if(H5) { + const Matrix3 Jrinv_theta_bc = Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected); + const Matrix3 Jr_JbiasOmegaIncr = Rot3::rightJacobianExpMapSO3(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr); + const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega_; + + H5->resize(15,6); + (*H5) << + // dfP/dBias_i + - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasAcc_, + - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasOmega_, + // dfV/dBias_i + - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasAcc_, + - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasOmega_, + // dfR/dBias_i + Matrix::Zero(3,3), + Jrinv_fRhat * ( - fRhat.inverse().matrix() * JbiasOmega), + //dBiasAcc/dBias_i + -I_3x3, Z_3x3, + //dBiasOmega/dBias_i + Z_3x3, -I_3x3; + } + + if(H6) { + H6->resize(15,6); + (*H6) << + // dfP/dBias_j + Z_3x3, Z_3x3, + // dfV/dBias_j + Z_3x3, Z_3x3, + // dfR/dBias_j + Z_3x3, Z_3x3, + //dBiasAcc/dBias_j + I_3x3, Z_3x3, + //dBiasOmega/dBias_j + Z_3x3, I_3x3; + } + + // Evaluate residual error, according to [3] + /* ---------------------------------------------------------------------------------------------------- */ + const Vector3 fp = + pos_j - pos_i + - Rot_i.matrix() * (preintegratedMeasurements_.deltaPij_ + + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr + + preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr) + - vel_i * deltaTij + + skewSymmetric(omegaCoriolis_) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper + - 0.5 * gravity_ * deltaTij*deltaTij; + + const Vector3 fv = + vel_j - vel_i - Rot_i.matrix() * (preintegratedMeasurements_.deltaVij_ + + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr + + preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr) + + 2 * skewSymmetric(omegaCoriolis_) * vel_i * deltaTij // Coriolis term + - gravity_ * deltaTij; + + const Vector3 fR = Rot3::Logmap(fRhat); + + const Vector3 fbiasAcc = bias_j.accelerometer() - bias_i.accelerometer(); + + const Vector3 fbiasOmega = bias_j.gyroscope() - bias_i.gyroscope(); + + Vector r(15); r << fp, fv, fR, fbiasAcc, fbiasOmega; // vector of size 15 + + return r; +} + +//------------------------------------------------------------------------------ +PoseVelocityBias CombinedImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i, + const imuBias::ConstantBias& bias_i, + const CombinedPreintegratedMeasurements& preintegratedMeasurements, + const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis){ + + const double& deltaTij = preintegratedMeasurements.deltaTij_; + const Vector3 biasAccIncr = bias_i.accelerometer() - preintegratedMeasurements.biasHat_.accelerometer(); + const Vector3 biasOmegaIncr = bias_i.gyroscope() - preintegratedMeasurements.biasHat_.gyroscope(); + + const Rot3 Rot_i = pose_i.rotation(); + const Vector3 pos_i = pose_i.translation().vector(); + + // Predict state at time j + /* ---------------------------------------------------------------------------------------------------- */ + Vector3 pos_j = pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij_ + + preintegratedMeasurements.delPdelBiasAcc_ * biasAccIncr + + preintegratedMeasurements.delPdelBiasOmega_ * biasOmegaIncr) + + vel_i * deltaTij + - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper + + 0.5 * gravity * deltaTij*deltaTij; + + Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij_ + + preintegratedMeasurements.delVdelBiasAcc_ * biasAccIncr + + preintegratedMeasurements.delVdelBiasOmega_ * biasOmegaIncr) + - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term + + gravity * deltaTij); + + if(use2ndOrderCoriolis){ + pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij; // 2nd order coriolis term for position + vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity + } + + const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij_.retract(preintegratedMeasurements.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP); + // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr) + Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); + Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected - + Rot_i.inverse().matrix() * omegaCoriolis * deltaTij; // Coriolis term + const Rot3 deltaRij_biascorrected_corioliscorrected = + Rot3::Expmap( theta_biascorrected_corioliscorrected ); + const Rot3 Rot_j = Rot_i.compose( deltaRij_biascorrected_corioliscorrected ); + + Pose3 pose_j = Pose3( Rot_j, Point3(pos_j) ); + + return PoseVelocityBias(pose_j, vel_j, bias_i); +} + +} /// namespace gtsam diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 069723eca..5e2bfeadb 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -11,714 +11,291 @@ /** * @file CombinedImuFactor.h - * @author Luca Carlone, Stephen Williams, Richard Roberts, Vadim Indelman, David Jensen + * @author Luca Carlone + * @author Stephen Williams + * @author Richard Roberts + * @author Vadim Indelman + * @author David Jensen + * @author Frank Dellaert **/ #pragma once /* GTSAM includes */ -#include -#include #include -#include +#include #include -/* External or standard includes */ -#include - - namespace gtsam { - /** - * Struct to hold all state variables of CombinedImuFactor - * returned by predict function - */ - struct PoseVelocityBias { - Pose3 pose; - Vector3 velocity; - imuBias::ConstantBias bias; +/** + * + * @addtogroup SLAM + * + * If you are using the factor, please cite: + * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating conditionally + * independent sets in factor graphs: a unifying perspective based on smart factors, + * Int. Conf. on Robotics and Automation (ICRA), 2014. + * + * REFERENCES: + * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008. + * [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built + * Environments Without Initial Conditions", TRO, 28(1):61-76, 2012. + * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: Computation of the Jacobian Matrices", Tech. Report, 2013. + */ - PoseVelocityBias(const Pose3& _pose, const Vector3& _velocity, - const imuBias::ConstantBias _bias) : +/** + * Struct to hold all state variables of CombinedImuFactor returned by Predict function + */ +struct PoseVelocityBias { + Pose3 pose; + Vector3 velocity; + imuBias::ConstantBias bias; + + PoseVelocityBias(const Pose3& _pose, const Vector3& _velocity, + const imuBias::ConstantBias _bias) : pose(_pose), velocity(_velocity), bias(_bias) { - } - }; + } +}; - /** - * - * @addtogroup SLAM - * - * If you are using the factor, please cite: - * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating conditionally - * independent sets in factor graphs: a unifying perspective based on smart factors, - * Int. Conf. on Robotics and Automation (ICRA), 2014. - * - * REFERENCES: - * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008. - * [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built - * Environments Without Initial Conditions", TRO, 28(1):61-76, 2012. - * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: Computation of the Jacobian Matrices", Tech. Report, 2013. +/** + * CombinedImuFactor is a 6-ways factor involving previous state (pose and velocity of the vehicle, as well as bias + * at previous time step), and current state (pose, velocity, bias at current time step). According to the + * preintegration scheme proposed in [2], the CombinedImuFactor includes many IMU measurements, which are + * "summarized" using the CombinedPreintegratedMeasurements class. There are 3 main differences wrt ImuFactor: + * 1) The factor is 6-ways, meaning that it also involves both biases (previous and current time step). + * Therefore, the factor internally imposes the biases to be slowly varying; in particular, the matrices + * "biasAccCovariance" and "biasOmegaCovariance" described the random walk that models bias evolution. + * 2) The preintegration covariance takes into account the noise in the bias estimate used for integration. + * 3) The covariance matrix of the CombinedPreintegratedMeasurements preserves the correlation between the bias uncertainty + * and the preintegrated measurements uncertainty. + */ +class CombinedImuFactor: public NoiseModelFactor6 { +public: + + /** CombinedPreintegratedMeasurements accumulates (integrates) the IMU measurements (rotation rates and accelerations) + * and the corresponding covariance matrix. The measurements are then used to build the CombinedPreintegrated IMU factor (CombinedImuFactor). + * Integration is done incrementally (ideally, one integrates the measurement as soon as it is received + * from the IMU) so as to avoid costly integration at time of factor construction. */ + class CombinedPreintegratedMeasurements { - class CombinedImuFactor: public NoiseModelFactor6 { + friend class CombinedImuFactor; + + protected: + imuBias::ConstantBias biasHat_; ///< Acceleration and angular rate bias values used during preintegration + Eigen::Matrix measurementCovariance_; ///< (Raw measurements uncertainty) Covariance of the vector + ///< [integrationError measuredAcc measuredOmega biasAccRandomWalk biasOmegaRandomWalk biasAccInit biasOmegaInit] in R^(21 x 21) + + Vector3 deltaPij_; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, in [2]) (in frame i) + Vector3 deltaVij_; ///< Preintegrated relative velocity (in global frame) + Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i) + double deltaTij_; ///< Time interval from i to j + + Matrix3 delPdelBiasAcc_; ///< Jacobian of preintegrated position w.r.t. acceleration bias + Matrix3 delPdelBiasOmega_; ///< Jacobian of preintegrated position w.r.t. angular rate bias + Matrix3 delVdelBiasAcc_; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias + Matrix3 delVdelBiasOmega_; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias + Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias + + Eigen::Matrix PreintMeasCov_; ///< Covariance matrix of the preintegrated measurements + ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION BiasAcc BiasOmega] + ///< (first-order propagation from *measurementCovariance*). CombinedPreintegratedMeasurements also include the biases and keep the correlation + ///< between the preintegrated measurements and the biases + + bool use2ndOrderIntegration_; ///< Controls the order of integration public: - /** Struct to store results of preintegrating IMU measurements. Can be build - * incrementally so as to avoid costly integration at time of factor construction. */ + /** + * Default constructor, initializes the class with no measurements + * @param bias Current estimate of acceleration and rotation rate biases + * @param measuredAccCovariance Covariance matrix of measuredAcc + * @param measuredOmegaCovariance Covariance matrix of measured Angular Rate + * @param integrationErrorCovariance Covariance matrix of integration errors (velocity -> position) + * @param biasAccCovariance Covariance matrix of biasAcc (random walk describing BIAS evolution) + * @param biasOmegaCovariance Covariance matrix of biasOmega (random walk describing BIAS evolution) + * @param biasAccOmegaInit Covariance of biasAcc & biasOmega when preintegrating measurements + * @param use2ndOrderIntegration Controls the order of integration + * (if false: p(t+1) = p(t) + v(t) deltaT ; if true: p(t+1) = p(t) + v(t) deltaT + 0.5 * acc(t) deltaT^2) + */ + CombinedPreintegratedMeasurements(const imuBias::ConstantBias& bias, const Matrix3& measuredAccCovariance, + const Matrix3& measuredOmegaCovariance, const Matrix3& integrationErrorCovariance, + const Matrix3& biasAccCovariance, const Matrix3& biasOmegaCovariance, + const Matrix& biasAccOmegaInit, const bool use2ndOrderIntegration = false); - /** CombinedPreintegratedMeasurements accumulates (integrates) the IMU measurements (rotation rates and accelerations) - * and the corresponding covariance matrix. The measurements are then used to build the Preintegrated IMU factor*/ - class CombinedPreintegratedMeasurements { - friend class CombinedImuFactor; - protected: - imuBias::ConstantBias biasHat_; ///< Acceleration and angular rate bias values used during preintegration - Matrix measurementCovariance_; ///< (Raw measurements uncertainty) Covariance of the vector - ///< [integrationError measuredAcc measuredOmega biasAccRandomWalk biasOmegaRandomWalk biasAccInit biasOmegaInit] in R^(21 x 21) + /// print + void print(const std::string& s = "Preintegrated Measurements:") const; - Vector3 deltaPij_; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, in [2]) (in frame i) - Vector3 deltaVij_; ///< Preintegrated relative velocity (in global frame) - Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i) - double deltaTij_; ///< Time interval from i to j + /// equals + bool equals(const CombinedPreintegratedMeasurements& expected, double tol=1e-9) const; - Matrix3 delPdelBiasAcc_; ///< Jacobian of preintegrated position w.r.t. acceleration bias - Matrix3 delPdelBiasOmega_; ///< Jacobian of preintegrated position w.r.t. angular rate bias - Matrix3 delVdelBiasAcc_; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias - Matrix3 delVdelBiasOmega_; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias - Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias - Matrix PreintMeasCov_; ///< Covariance matrix of the preintegrated measurements (first-order propagation from *measurementCovariance*) - bool use2ndOrderIntegration_; ///< Controls the order of integration -// public: - ///< In the combined factor is also includes the biases and keeps the correlation between the preintegrated measurements and the biases - ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION BiasAcc BiasOmega] - /** Default constructor, initialize with no IMU measurements */ - public: - CombinedPreintegratedMeasurements( - const imuBias::ConstantBias& bias, ///< Current estimate of acceleration and rotation rate biases - const Matrix3& measuredAccCovariance, ///< Covariance matrix of measuredAcc - const Matrix3& measuredOmegaCovariance, ///< Covariance matrix of measuredAcc - const Matrix3& integrationErrorCovariance, ///< Covariance matrix of measuredAcc - const Matrix3& biasAccCovariance, ///< Covariance matrix of biasAcc (random walk describing BIAS evolution) - const Matrix3& biasOmegaCovariance, ///< Covariance matrix of biasOmega (random walk describing BIAS evolution) - const Matrix& biasAccOmegaInit, ///< Covariance of biasAcc & biasOmega when preintegrating measurements - const bool use2ndOrderIntegration = false ///< Controls the order of integration - ///< (this allows to consider the uncertainty of the BIAS choice when integrating the measurements) - ) : biasHat_(bias), measurementCovariance_(21,21), deltaPij_(Vector3::Zero()), deltaVij_(Vector3::Zero()), deltaTij_(0.0), - delPdelBiasAcc_(Matrix3::Zero()), delPdelBiasOmega_(Matrix3::Zero()), - delVdelBiasAcc_(Matrix3::Zero()), delVdelBiasOmega_(Matrix3::Zero()), - delRdelBiasOmega_(Matrix3::Zero()), PreintMeasCov_(Matrix::Zero(15,15)), - use2ndOrderIntegration_(use2ndOrderIntegration) - { - // COVARIANCE OF: [Integration AccMeasurement OmegaMeasurement BiasAccRandomWalk BiasOmegaRandomWalk (BiasAccInit BiasOmegaInit)] SIZE (21x21) - measurementCovariance_ << integrationErrorCovariance , Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), - Matrix3::Zero(), measuredAccCovariance, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), - Matrix3::Zero(), Matrix3::Zero(), measuredOmegaCovariance, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), - Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), biasAccCovariance, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), - Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), biasOmegaCovariance, Matrix3::Zero(), Matrix3::Zero(), - Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), biasAccOmegaInit.block(0,0,3,3), biasAccOmegaInit.block(0,3,3,3), - Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), biasAccOmegaInit.block(3,0,3,3), biasAccOmegaInit.block(3,3,3,3); + /// Re-initialize CombinedPreintegratedMeasurements + void resetIntegration(); - } + /** + * Add a single IMU measurement to the preintegration. + * @param measuredAcc Measured acceleration (in body frame, as given by the sensor) + * @param measuredOmega Measured angular velocity (as given by the sensor) + * @param deltaT Time interval between two consecutive IMU measurements + * @param body_P_sensor Optional sensor frame (pose of the IMU in the body frame) + */ + void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT, + boost::optional body_P_sensor = boost::none); - CombinedPreintegratedMeasurements() : - biasHat_(imuBias::ConstantBias()), measurementCovariance_(21,21), deltaPij_(Vector3::Zero()), deltaVij_(Vector3::Zero()), deltaTij_(0.0), - delPdelBiasAcc_(Matrix3::Zero()), delPdelBiasOmega_(Matrix3::Zero()), - delVdelBiasAcc_(Matrix3::Zero()), delVdelBiasOmega_(Matrix3::Zero()), - delRdelBiasOmega_(Matrix3::Zero()), PreintMeasCov_(Matrix::Zero(15,15)), - use2ndOrderIntegration_(false) ///< Controls the order of integration - { - } + /// methods to access class variables + Matrix measurementCovariance() const {return measurementCovariance_;} + Matrix deltaRij() const {return deltaRij_.matrix();} + double deltaTij() const{return deltaTij_;} + Vector deltaPij() const {return deltaPij_;} + Vector deltaVij() const {return deltaVij_;} + Vector biasHat() const { return biasHat_.vector();} + Matrix delPdelBiasAcc() const { return delPdelBiasAcc_;} + Matrix delPdelBiasOmega() const { return delPdelBiasOmega_;} + Matrix delVdelBiasAcc() const { return delVdelBiasAcc_;} + Matrix delVdelBiasOmega() const { return delVdelBiasOmega_;} + Matrix delRdelBiasOmega() const{ return delRdelBiasOmega_;} + Matrix PreintMeasCov() const { return PreintMeasCov_;} - /** print */ - void print(const std::string& s = "Preintegrated Measurements:") const { - std::cout << s << std::endl; - biasHat_.print(" biasHat"); - std::cout << " deltaTij " << deltaTij_ << std::endl; - std::cout << " deltaPij [ " << deltaPij_.transpose() << " ]" << std::endl; - std::cout << " deltaVij [ " << deltaVij_.transpose() << " ]" << std::endl; - deltaRij_.print(" deltaRij "); - std::cout << " measurementCovariance [ " << measurementCovariance_ << " ]" << std::endl; - std::cout << " PreintMeasCov [ " << PreintMeasCov_ << " ]" << std::endl; - } + /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */ + // This function is only used for test purposes (compare numerical derivatives wrt analytic ones) + static inline Vector PreIntegrateIMUObservations_delta_vel(const Vector& msr_gyro_t, const Vector& msr_acc_t, const double msr_dt, + const Vector3& delta_angles, const Vector& delta_vel_in_t0){ + // Note: all delta terms refer to an IMU\sensor system at t0 + Vector body_t_a_body = msr_acc_t; + Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles); + return delta_vel_in_t0 + R_t_to_t0.matrix() * body_t_a_body * msr_dt; + } - /** equals */ - bool equals(const CombinedPreintegratedMeasurements& expected, double tol=1e-9) const { - return biasHat_.equals(expected.biasHat_, tol) - && equal_with_abs_tol(measurementCovariance_, expected.measurementCovariance_, tol) - && equal_with_abs_tol(deltaPij_, expected.deltaPij_, tol) - && equal_with_abs_tol(deltaVij_, expected.deltaVij_, tol) - && deltaRij_.equals(expected.deltaRij_, tol) - && std::fabs(deltaTij_ - expected.deltaTij_) < tol - && equal_with_abs_tol(delPdelBiasAcc_, expected.delPdelBiasAcc_, tol) - && equal_with_abs_tol(delPdelBiasOmega_, expected.delPdelBiasOmega_, tol) - && equal_with_abs_tol(delVdelBiasAcc_, expected.delVdelBiasAcc_, tol) - && equal_with_abs_tol(delVdelBiasOmega_, expected.delVdelBiasOmega_, tol) - && equal_with_abs_tol(delRdelBiasOmega_, expected.delRdelBiasOmega_, tol); - } - - void resetIntegration(){ - deltaPij_ = Vector3::Zero(); - deltaVij_ = Vector3::Zero(); - deltaRij_ = Rot3(); - deltaTij_ = 0.0; - delPdelBiasAcc_ = Matrix3::Zero(); - delPdelBiasOmega_ = Matrix3::Zero(); - delVdelBiasAcc_ = Matrix3::Zero(); - delVdelBiasOmega_ = Matrix3::Zero(); - delRdelBiasOmega_ = Matrix3::Zero(); - PreintMeasCov_ = Matrix::Zero(15,15); - } - - /** Add a single IMU measurement to the preintegration. */ - void integrateMeasurement( - const Vector3& measuredAcc, ///< Measured linear acceleration (in body frame) - const Vector3& measuredOmega, ///< Measured angular velocity (in body frame) - double deltaT, ///< Time step - boost::optional body_P_sensor = boost::none ///< Sensor frame - ) { - // NOTE: order is important here because each update uses old values, e.g., velocity and position updates are based on previous rotation estimate. - // First we compensate the measurements for the bias: since we have only an estimate of the bias, the covariance includes the corresponding uncertainty - Vector3 correctedAcc = biasHat_.correctAccelerometer(measuredAcc); - Vector3 correctedOmega = biasHat_.correctGyroscope(measuredOmega); - - // Then compensate for sensor-body displacement: we express the quantities (originally in the IMU frame) into the body frame - if(body_P_sensor){ - Matrix3 body_R_sensor = body_P_sensor->rotation().matrix(); - correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame - Matrix3 body_omega_body__cross = skewSymmetric(correctedOmega); - correctedAcc = body_R_sensor * correctedAcc - body_omega_body__cross * body_omega_body__cross * body_P_sensor->translation().vector(); - // linear acceleration vector in the body frame - } - - const Vector3 theta_incr = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement - const Rot3 Rincr = Rot3::Expmap(theta_incr); // rotation increment computed from the current rotation rate measurement - const Matrix3 Jr_theta_incr = Rot3::rightJacobianExpMapSO3(theta_incr); // Right jacobian computed at theta_incr - - // Update Jacobians - /* ----------------------------------------------------------------------------------------------------------------------- */ - if(!use2ndOrderIntegration_){ - delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT; - delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT; - }else{ - delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - 0.5 * deltaRij_.matrix() * deltaT*deltaT; - delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT - 0.5 * deltaRij_.matrix() - * skewSymmetric(biasHat_.correctAccelerometer(measuredAcc)) * deltaT*deltaT * delRdelBiasOmega_; - } - - delVdelBiasAcc_ += -deltaRij_.matrix() * deltaT; - delVdelBiasOmega_ += -deltaRij_.matrix() * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega_; - delRdelBiasOmega_ = Rincr.inverse().matrix() * delRdelBiasOmega_ - Jr_theta_incr * deltaT; - - // Update preintegrated measurements covariance: as in [2] we consider a first order propagation that - // can be seen as a prediction phase in an EKF framework. In this implementation, contrarily to [2] we - // consider the uncertainty of the bias selection and we keep correlation between biases and preintegrated measurements - /* ----------------------------------------------------------------------------------------------------------------------- */ - Matrix3 Z_3x3 = Matrix3::Zero(); - Matrix3 I_3x3 = Matrix3::Identity(); - const Vector3 theta_i = Rot3::Logmap(deltaRij_); // parametrization of so(3) - const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3(theta_i); - - Rot3 Rot_j = deltaRij_ * Rincr; - const Vector3 theta_j = Rot3::Logmap(Rot_j); // parametrization of so(3) - const Matrix3 Jrinv_theta_j = Rot3::rightJacobianExpMapSO3inverse(theta_j); - - // Single Jacobians to propagate covariance - Matrix3 H_pos_pos = I_3x3; - Matrix3 H_pos_vel = I_3x3 * deltaT; - Matrix3 H_pos_angles = Z_3x3; - - Matrix3 H_vel_pos = Z_3x3; - Matrix3 H_vel_vel = I_3x3; - Matrix3 H_vel_angles = - deltaRij_.matrix() * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT; - // analytic expression corresponding to the following numerical derivative - // Matrix H_vel_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_vel, correctedOmega, correctedAcc, deltaT, _1, deltaVij), theta_i); - Matrix3 H_vel_biasacc = - deltaRij_.matrix() * deltaT; - - Matrix3 H_angles_pos = Z_3x3; - Matrix3 H_angles_vel = Z_3x3; - Matrix3 H_angles_angles = Jrinv_theta_j * Rincr.inverse().matrix() * Jr_theta_i; - Matrix3 H_angles_biasomega =- Jrinv_theta_j * Jr_theta_incr * deltaT; - // analytic expression corresponding to the following numerical derivative - // Matrix H_angles_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_angles, correctedOmega, deltaT, _1), thetaij); - - // overall Jacobian wrt preintegrated measurements (df/dx) - Matrix F(15,15); - F << H_pos_pos, H_pos_vel, H_pos_angles, Z_3x3, Z_3x3, - H_vel_pos, H_vel_vel, H_vel_angles, H_vel_biasacc, Z_3x3, - H_angles_pos, H_angles_vel, H_angles_angles, Z_3x3, H_angles_biasomega, - Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, - Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3; - - - // first order uncertainty propagation - // Optimized matrix multiplication (1/deltaT) * G * measurementCovariance * G.transpose() - - Matrix G_measCov_Gt = Matrix::Zero(15,15); - // BLOCK DIAGONAL TERMS - G_measCov_Gt.block(0,0,3,3) = deltaT * measurementCovariance_.block(0,0,3,3); - - G_measCov_Gt.block(3,3,3,3) = (1/deltaT) * (H_vel_biasacc) * - (measurementCovariance_.block(3,3,3,3) + measurementCovariance_.block(15,15,3,3) ) * - (H_vel_biasacc.transpose()); - - G_measCov_Gt.block(6,6,3,3) = (1/deltaT) * (H_angles_biasomega) * - (measurementCovariance_.block(6,6,3,3) + measurementCovariance_.block(18,18,3,3) ) * - (H_angles_biasomega.transpose()); - - G_measCov_Gt.block(9,9,3,3) = deltaT * measurementCovariance_.block(9,9,3,3); - - G_measCov_Gt.block(12,12,3,3) = deltaT * measurementCovariance_.block(12,12,3,3); - - // NEW OFF BLOCK DIAGONAL TERMS - Matrix3 block23 = H_vel_biasacc * measurementCovariance_.block(18,15,3,3) * H_angles_biasomega.transpose(); - G_measCov_Gt.block(3,6,3,3) = block23; - G_measCov_Gt.block(6,3,3,3) = block23.transpose(); - - PreintMeasCov_ = F * PreintMeasCov_ * F.transpose() + G_measCov_Gt; - - // Update preintegrated measurements - /* ----------------------------------------------------------------------------------------------------------------------- */ - if(!use2ndOrderIntegration_){ - deltaPij_ += deltaVij_ * deltaT; - }else{ - deltaPij_ += deltaVij_ * deltaT + 0.5 * deltaRij_.matrix() * biasHat_.correctAccelerometer(measuredAcc) * deltaT*deltaT; - } - deltaVij_ += deltaRij_.matrix() * correctedAcc * deltaT; - deltaRij_ = deltaRij_ * Rincr; - deltaTij_ += deltaT; - } - - /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */ - // This function is only used for test purposes (compare numerical derivatives wrt analytic ones) - static inline Vector PreIntegrateIMUObservations_delta_vel(const Vector& msr_gyro_t, const Vector& msr_acc_t, const double msr_dt, - const Vector3& delta_angles, const Vector& delta_vel_in_t0){ - - // Note: all delta terms refer to an IMU\sensor system at t0 - - Vector body_t_a_body = msr_acc_t; - Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles); - - return delta_vel_in_t0 + R_t_to_t0.matrix() * body_t_a_body * msr_dt; - } - - // This function is only used for test purposes (compare numerical derivatives wrt analytic ones) - static inline Vector PreIntegrateIMUObservations_delta_angles(const Vector& msr_gyro_t, const double msr_dt, - const Vector3& delta_angles){ - - // Note: all delta terms refer to an IMU\sensor system at t0 - - // Calculate the corrected measurements using the Bias object - Vector body_t_omega_body= msr_gyro_t; - - Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles); - - R_t_to_t0 = R_t_to_t0 * Rot3::Expmap( body_t_omega_body*msr_dt ); - return Rot3::Logmap(R_t_to_t0); - } - /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */ - Matrix measurementCovariance() const {return measurementCovariance_;} - Matrix deltaRij() const {return deltaRij_.matrix();} - double deltaTij() const{return deltaTij_;} - Vector deltaPij() const {return deltaPij_;} - Vector deltaVij() const {return deltaVij_;} - Vector biasHat() const { return biasHat_.vector();} - Matrix delPdelBiasAcc() const { return delPdelBiasAcc_;} - Matrix delPdelBiasOmega() const { return delPdelBiasOmega_;} - Matrix delVdelBiasAcc() const { return delVdelBiasAcc_;} - Matrix delVdelBiasOmega() const { return delVdelBiasOmega_;} - Matrix delRdelBiasOmega() const{ return delRdelBiasOmega_;} - Matrix PreintMeasCov() const { return PreintMeasCov_;} - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(biasHat_); - ar & BOOST_SERIALIZATION_NVP(measurementCovariance_); - ar & BOOST_SERIALIZATION_NVP(deltaPij_); - ar & BOOST_SERIALIZATION_NVP(deltaVij_); - ar & BOOST_SERIALIZATION_NVP(deltaRij_); - ar & BOOST_SERIALIZATION_NVP(deltaTij_); - ar & BOOST_SERIALIZATION_NVP(delPdelBiasAcc_); - ar & BOOST_SERIALIZATION_NVP(delPdelBiasOmega_); - ar & BOOST_SERIALIZATION_NVP(delVdelBiasAcc_); - ar & BOOST_SERIALIZATION_NVP(delVdelBiasOmega_); - ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_); - } - }; + // This function is only used for test purposes (compare numerical derivatives wrt analytic ones) + static inline Vector PreIntegrateIMUObservations_delta_angles(const Vector& msr_gyro_t, const double msr_dt, + const Vector3& delta_angles){ + // Note: all delta terms refer to an IMU\sensor system at t0 + // Calculate the corrected measurements using the Bias object + Vector body_t_omega_body= msr_gyro_t; + Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles); + R_t_to_t0 = R_t_to_t0 * Rot3::Expmap( body_t_omega_body*msr_dt ); + return Rot3::Logmap(R_t_to_t0); + } + /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */ private: - - typedef CombinedImuFactor This; - typedef NoiseModelFactor6 Base; - - CombinedPreintegratedMeasurements preintegratedMeasurements_; - Vector3 gravity_; - Vector3 omegaCoriolis_; - boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame - - bool use2ndOrderCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect - - public: - - /** Shorthand for a smart pointer to a factor */ -#if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5 - typedef typename boost::shared_ptr shared_ptr; -#else - typedef boost::shared_ptr shared_ptr; -#endif - - /** Default constructor - only use for serialization */ - CombinedImuFactor() : preintegratedMeasurements_(imuBias::ConstantBias(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix::Zero(6,6)) {} - - /** Constructor */ - CombinedImuFactor( - Key pose_i, ///< previous pose key - Key vel_i, ///< previous velocity key - Key pose_j, ///< current pose key - Key vel_j, ///< current velocity key - Key bias_i, ///< previous bias key - Key bias_j, ///< current bias key - const CombinedPreintegratedMeasurements& preintegratedMeasurements, ///< Preintegrated IMU measurements - const Vector3& gravity, ///< gravity vector - const Vector3& omegaCoriolis, ///< rotation rate of inertial frame - boost::optional body_P_sensor = boost::none, ///< The Pose of the sensor frame in the body frame - const bool use2ndOrderCoriolis = false ///< When true, the second-order term is used in the calculation of the Coriolis Effect - ) : - Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.PreintMeasCov_), pose_i, vel_i, pose_j, vel_j, bias_i, bias_j), - preintegratedMeasurements_(preintegratedMeasurements), - gravity_(gravity), - omegaCoriolis_(omegaCoriolis), - body_P_sensor_(body_P_sensor), - use2ndOrderCoriolis_(use2ndOrderCoriolis){ - } - - virtual ~CombinedImuFactor() {} - - /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { - return boost::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new This(*this))); } - - /** implement functions needed for Testable */ - - /** print */ - virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { - std::cout << s << "CombinedImuFactor(" - << keyFormatter(this->key1()) << "," - << keyFormatter(this->key2()) << "," - << keyFormatter(this->key3()) << "," - << keyFormatter(this->key4()) << "," - << keyFormatter(this->key5()) << "," - << keyFormatter(this->key6()) << ")\n"; - preintegratedMeasurements_.print(" preintegrated measurements:"); - std::cout << " gravity: [ " << gravity_.transpose() << " ]" << std::endl; - std::cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << std::endl; - this->noiseModel_->print(" noise model: "); - if(this->body_P_sensor_) - this->body_P_sensor_->print(" sensor pose in body frame: "); - } - - /** equals */ - virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { - const This *e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) - && preintegratedMeasurements_.equals(e->preintegratedMeasurements_, tol) - && equal_with_abs_tol(gravity_, e->gravity_, tol) - && equal_with_abs_tol(omegaCoriolis_, e->omegaCoriolis_, tol) - && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_))); - } - - /** Access the preintegrated measurements. */ - const CombinedPreintegratedMeasurements& preintegratedMeasurements() const { - return preintegratedMeasurements_; } - - const Vector3& gravity() const { return gravity_; } - - const Vector3& omegaCoriolis() const { return omegaCoriolis_; } - - /** implement functions needed to derive from Factor */ - - /** vector of errors */ - Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, - const imuBias::ConstantBias& bias_i, const imuBias::ConstantBias& bias_j, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none, - boost::optional H3 = boost::none, - boost::optional H4 = boost::none, - boost::optional H5 = boost::none, - boost::optional H6 = boost::none) const - { - - const double& deltaTij = preintegratedMeasurements_.deltaTij_; - const Vector3 biasAccIncr = bias_i.accelerometer() - preintegratedMeasurements_.biasHat_.accelerometer(); - const Vector3 biasOmegaIncr = bias_i.gyroscope() - preintegratedMeasurements_.biasHat_.gyroscope(); - - // we give some shorter name to rotations and translations - const Rot3 Rot_i = pose_i.rotation(); - const Rot3 Rot_j = pose_j.rotation(); - const Vector3 pos_i = pose_i.translation().vector(); - const Vector3 pos_j = pose_j.translation().vector(); - - // We compute factor's Jacobians, according to [3] - /* ---------------------------------------------------------------------------------------------------- */ - const Rot3 deltaRij_biascorrected = preintegratedMeasurements_.deltaRij_.retract(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP); - // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr) - - Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); - - Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected - - Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij; // Coriolis term - - const Rot3 deltaRij_biascorrected_corioliscorrected = - Rot3::Expmap( theta_biascorrected_corioliscorrected ); - - const Rot3 fRhat = deltaRij_biascorrected_corioliscorrected.between(Rot_i.between(Rot_j)); - - const Matrix3 Jr_theta_bcc = Rot3::rightJacobianExpMapSO3(theta_biascorrected_corioliscorrected); - - const Matrix3 Jtheta = -Jr_theta_bcc * skewSymmetric(Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij); - - const Matrix3 Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat)); - - /* - (*H1) << - // dfP/dRi - Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij - + preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr), - // dfP/dPi - - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij, - // dfV/dRi - Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij - + preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr), - // dfV/dPi - skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij, - // dfR/dRi - Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta), - // dfR/dPi - Matrix3::Zero(), - //dBiasAcc/dPi - Matrix3::Zero(), Matrix3::Zero(), - //dBiasOmega/dPi - Matrix3::Zero(), Matrix3::Zero(); - */ - if(H1) { - H1->resize(15,6); - - Matrix3 dfPdPi; - Matrix3 dfVdPi; - if(use2ndOrderCoriolis_){ - dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij; - dfVdPi = skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij; - } - else{ - dfPdPi = - Rot_i.matrix(); - dfVdPi = Matrix3::Zero(); - } - - (*H1) << - // dfP/dRi - Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij_ - + preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr), - // dfP/dPi - dfPdPi, - // dfV/dRi - Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij_ - + preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr), - // dfV/dPi - dfVdPi, - // dfR/dRi - Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta), - // dfR/dPi - Matrix3::Zero(), - //dBiasAcc/dPi - Matrix3::Zero(), Matrix3::Zero(), - //dBiasOmega/dPi - Matrix3::Zero(), Matrix3::Zero(); - } - - if(H2) { - H2->resize(15,3); - (*H2) << - // dfP/dVi - - Matrix3::Identity() * deltaTij - + skewSymmetric(omegaCoriolis_) * deltaTij * deltaTij, // Coriolis term - we got rid of the 2 wrt ins paper - // dfV/dVi - - Matrix3::Identity() - + 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term - // dfR/dVi - Matrix3::Zero(), - //dBiasAcc/dVi - Matrix3::Zero(), - //dBiasOmega/dVi - Matrix3::Zero(); - } - - if(H3) { - - H3->resize(15,6); - (*H3) << - // dfP/dPosej - Matrix3::Zero(), Rot_j.matrix(), - // dfV/dPosej - Matrix::Zero(3,6), - // dfR/dPosej - Jrinv_fRhat * ( Matrix3::Identity() ), Matrix3::Zero(), - //dBiasAcc/dPosej - Matrix3::Zero(), Matrix3::Zero(), - //dBiasOmega/dPosej - Matrix3::Zero(), Matrix3::Zero(); - } - - if(H4) { - H4->resize(15,3); - (*H4) << - // dfP/dVj - Matrix3::Zero(), - // dfV/dVj - Matrix3::Identity(), - // dfR/dVj - Matrix3::Zero(), - //dBiasAcc/dVj - Matrix3::Zero(), - //dBiasOmega/dVj - Matrix3::Zero(); - } - - if(H5) { - const Matrix3 Jrinv_theta_bc = Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected); - const Matrix3 Jr_JbiasOmegaIncr = Rot3::rightJacobianExpMapSO3(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr); - const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega_; - - H5->resize(15,6); - (*H5) << - // dfP/dBias_i - - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasAcc_, - - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasOmega_, - // dfV/dBias_i - - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasAcc_, - - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasOmega_, - // dfR/dBias_i - Matrix::Zero(3,3), - Jrinv_fRhat * ( - fRhat.inverse().matrix() * JbiasOmega), - //dBiasAcc/dBias_i - -Matrix3::Identity(), Matrix3::Zero(), - //dBiasOmega/dBias_i - Matrix3::Zero(), -Matrix3::Identity(); - } - - if(H6) { - - H6->resize(15,6); - (*H6) << - // dfP/dBias_j - Matrix3::Zero(), Matrix3::Zero(), - // dfV/dBias_j - Matrix3::Zero(), Matrix3::Zero(), - // dfR/dBias_j - Matrix3::Zero(), Matrix3::Zero(), - //dBiasAcc/dBias_j - Matrix3::Identity(), Matrix3::Zero(), - //dBiasOmega/dBias_j - Matrix3::Zero(), Matrix3::Identity(); - } - - // Evaluate residual error, according to [3] - /* ---------------------------------------------------------------------------------------------------- */ - const Vector3 fp = - pos_j - pos_i - - Rot_i.matrix() * (preintegratedMeasurements_.deltaPij_ - + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr - + preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr) - - vel_i * deltaTij - + skewSymmetric(omegaCoriolis_) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper - - 0.5 * gravity_ * deltaTij*deltaTij; - - const Vector3 fv = - vel_j - vel_i - Rot_i.matrix() * (preintegratedMeasurements_.deltaVij_ - + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr - + preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr) - + 2 * skewSymmetric(omegaCoriolis_) * vel_i * deltaTij // Coriolis term - - gravity_ * deltaTij; - - const Vector3 fR = Rot3::Logmap(fRhat); - - const Vector3 fbiasAcc = bias_j.accelerometer() - bias_i.accelerometer(); - - const Vector3 fbiasOmega = bias_j.gyroscope() - bias_i.gyroscope(); - - Vector r(15); r << fp, fv, fR, fbiasAcc, fbiasOmega; // vector of size 15 - - return r; - } - - - /** predicted states from IMU */ - static PoseVelocityBias Predict(const Pose3& pose_i, const Vector3& vel_i, - const imuBias::ConstantBias& bias_i, - const CombinedPreintegratedMeasurements& preintegratedMeasurements, - const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional body_P_sensor = boost::none, - const bool use2ndOrderCoriolis = false) - { - - const double& deltaTij = preintegratedMeasurements.deltaTij_; - const Vector3 biasAccIncr = bias_i.accelerometer() - preintegratedMeasurements.biasHat_.accelerometer(); - const Vector3 biasOmegaIncr = bias_i.gyroscope() - preintegratedMeasurements.biasHat_.gyroscope(); - - const Rot3 Rot_i = pose_i.rotation(); - const Vector3 pos_i = pose_i.translation().vector(); - - // Predict state at time j - /* ---------------------------------------------------------------------------------------------------- */ - Vector3 pos_j = pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij_ - + preintegratedMeasurements.delPdelBiasAcc_ * biasAccIncr - + preintegratedMeasurements.delPdelBiasOmega_ * biasOmegaIncr) - + vel_i * deltaTij - - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper - + 0.5 * gravity * deltaTij*deltaTij; - - Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij_ - + preintegratedMeasurements.delVdelBiasAcc_ * biasAccIncr - + preintegratedMeasurements.delVdelBiasOmega_ * biasOmegaIncr) - - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term - + gravity * deltaTij); - - if(use2ndOrderCoriolis){ - pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij; // 2nd order coriolis term for position - vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity - } - - const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij_.retract(preintegratedMeasurements.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP); - // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr) - Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); - Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected - - Rot_i.inverse().matrix() * omegaCoriolis * deltaTij; // Coriolis term - const Rot3 deltaRij_biascorrected_corioliscorrected = - Rot3::Expmap( theta_biascorrected_corioliscorrected ); - const Rot3 Rot_j = Rot_i.compose( deltaRij_biascorrected_corioliscorrected ); - - Pose3 pose_j = Pose3( Rot_j, Point3(pos_j) ); - - return PoseVelocityBias(pose_j, vel_j, bias_i); - } - - - private: - /** Serialization function */ friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("NoiseModelFactor6", - boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(preintegratedMeasurements_); - ar & BOOST_SERIALIZATION_NVP(gravity_); - ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_); - ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); + ar & BOOST_SERIALIZATION_NVP(biasHat_); + ar & BOOST_SERIALIZATION_NVP(measurementCovariance_); + ar & BOOST_SERIALIZATION_NVP(deltaPij_); + ar & BOOST_SERIALIZATION_NVP(deltaVij_); + ar & BOOST_SERIALIZATION_NVP(deltaRij_); + ar & BOOST_SERIALIZATION_NVP(deltaTij_); + ar & BOOST_SERIALIZATION_NVP(delPdelBiasAcc_); + ar & BOOST_SERIALIZATION_NVP(delPdelBiasOmega_); + ar & BOOST_SERIALIZATION_NVP(delVdelBiasAcc_); + ar & BOOST_SERIALIZATION_NVP(delVdelBiasOmega_); + ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_); } - }; // \class CombinedImuFactor + }; - typedef CombinedImuFactor::CombinedPreintegratedMeasurements CombinedImuFactorPreintegratedMeasurements; +private: + + typedef CombinedImuFactor This; + typedef NoiseModelFactor6 Base; + + CombinedPreintegratedMeasurements preintegratedMeasurements_; + Vector3 gravity_; + Vector3 omegaCoriolis_; + boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame + + bool use2ndOrderCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect + +public: + + /** Shorthand for a smart pointer to a factor */ +#if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5 + typedef typename boost::shared_ptr shared_ptr; +#else + typedef boost::shared_ptr shared_ptr; +#endif + + /** Default constructor - only use for serialization */ + CombinedImuFactor(); + + /** + * Constructor + * @param pose_i Previous pose key + * @param vel_i Previous velocity key + * @param pose_j Current pose key + * @param vel_j Current velocity key + * @param bias_i Previous bias key + * @param bias_j Current bias key + * @param CombinedPreintegratedMeasurements CombinedPreintegratedMeasurements IMU measurements + * @param gravity Gravity vector expressed in the global frame + * @param omegaCoriolis Rotation rate of the global frame w.r.t. an inertial frame + * @param body_P_sensor Optional pose of the sensor frame in the body frame + * @param use2ndOrderCoriolis When true, the second-order term is used in the calculation of the Coriolis Effect + */ + CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j, + const CombinedPreintegratedMeasurements& preintegratedMeasurements, + const Vector3& gravity, const Vector3& omegaCoriolis, + boost::optional body_P_sensor = boost::none, const bool use2ndOrderCoriolis = false); + + virtual ~CombinedImuFactor() {} + + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const; + + /** implement functions needed for Testable */ + + /// print + virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + + /// equals + virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const; + + /** Access the preintegrated measurements. */ + + const CombinedPreintegratedMeasurements& preintegratedMeasurements() const { + return preintegratedMeasurements_; } + + const Vector3& gravity() const { return gravity_; } + + const Vector3& omegaCoriolis() const { return omegaCoriolis_; } + + /** implement functions needed to derive from Factor */ + + /// vector of errors + Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, + const imuBias::ConstantBias& bias_i, const imuBias::ConstantBias& bias_j, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none, + boost::optional H3 = boost::none, + boost::optional H4 = boost::none, + boost::optional H5 = boost::none, + boost::optional H6 = boost::none) const; + + /// predicted states from IMU + static PoseVelocityBias Predict(const Pose3& pose_i, const Vector3& vel_i, + const imuBias::ConstantBias& bias_i, + const CombinedPreintegratedMeasurements& preintegratedMeasurements, + const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false); + +private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("NoiseModelFactor6", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(preintegratedMeasurements_); + ar & BOOST_SERIALIZATION_NVP(gravity_); + ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_); + ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); + } +}; // class CombinedImuFactor + +typedef CombinedImuFactor::CombinedPreintegratedMeasurements CombinedImuFactorPreintegratedMeasurements; } /// namespace gtsam diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp new file mode 100644 index 000000000..9a9cc9d62 --- /dev/null +++ b/gtsam/navigation/ImuFactor.cpp @@ -0,0 +1,438 @@ +/* ---------------------------------------------------------------------------- + + * 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 ImuFactor.cpp + * @author Luca Carlone + * @author Stephen Williams + * @author Richard Roberts + * @author Vadim Indelman + * @author David Jensen + * @author Frank Dellaert + **/ + +#include + +/* External or standard includes */ +#include + +namespace gtsam { + +using namespace std; + +//------------------------------------------------------------------------------ +// Inner class PreintegratedMeasurements +//------------------------------------------------------------------------------ +ImuFactor::PreintegratedMeasurements::PreintegratedMeasurements( + const imuBias::ConstantBias& bias, const Matrix3& measuredAccCovariance, + const Matrix3& measuredOmegaCovariance, const Matrix3& integrationErrorCovariance, + const bool use2ndOrderIntegration) : + biasHat_(bias), deltaPij_(Vector3::Zero()), deltaVij_(Vector3::Zero()), + deltaRij_(Rot3()), deltaTij_(0.0), + delPdelBiasAcc_(Z_3x3), delPdelBiasOmega_(Z_3x3), + delVdelBiasAcc_(Z_3x3), delVdelBiasOmega_(Z_3x3), + delRdelBiasOmega_(Z_3x3), use2ndOrderIntegration_(use2ndOrderIntegration) +{ + measurementCovariance_.setZero(); + measurementCovariance_.block<3,3>(0,0) = integrationErrorCovariance; + measurementCovariance_.block<3,3>(3,3) = measuredAccCovariance; + measurementCovariance_.block<3,3>(6,6) = measuredOmegaCovariance; + PreintMeasCov_.setZero(9,9); +} + +//------------------------------------------------------------------------------ +void ImuFactor::PreintegratedMeasurements::print(const string& s) const { + cout << s << endl; + biasHat_.print(" biasHat"); + cout << " deltaTij " << deltaTij_ << endl; + cout << " deltaPij [ " << deltaPij_.transpose() << " ]" << endl; + cout << " deltaVij [ " << deltaVij_.transpose() << " ]" << endl; + deltaRij_.print(" deltaRij "); + cout << " measurementCovariance = \n [ " << measurementCovariance_ << " ]" << endl; + cout << " PreintMeasCov = \n [ " << PreintMeasCov_ << " ]" << endl; +} + +//------------------------------------------------------------------------------ +bool ImuFactor::PreintegratedMeasurements::equals(const PreintegratedMeasurements& expected, double tol) const { + return biasHat_.equals(expected.biasHat_, tol) + && equal_with_abs_tol(measurementCovariance_, expected.measurementCovariance_, tol) + && equal_with_abs_tol(deltaPij_, expected.deltaPij_, tol) + && equal_with_abs_tol(deltaVij_, expected.deltaVij_, tol) + && deltaRij_.equals(expected.deltaRij_, tol) + && fabs(deltaTij_ - expected.deltaTij_) < tol + && equal_with_abs_tol(delPdelBiasAcc_, expected.delPdelBiasAcc_, tol) + && equal_with_abs_tol(delPdelBiasOmega_, expected.delPdelBiasOmega_, tol) + && equal_with_abs_tol(delVdelBiasAcc_, expected.delVdelBiasAcc_, tol) + && equal_with_abs_tol(delVdelBiasOmega_, expected.delVdelBiasOmega_, tol) + && equal_with_abs_tol(delRdelBiasOmega_, expected.delRdelBiasOmega_, tol); +} + +//------------------------------------------------------------------------------ +void ImuFactor::PreintegratedMeasurements::resetIntegration(){ + deltaPij_ = Vector3::Zero(); + deltaVij_ = Vector3::Zero(); + deltaRij_ = Rot3(); + deltaTij_ = 0.0; + delPdelBiasAcc_ = Z_3x3; + delPdelBiasOmega_ = Z_3x3; + delVdelBiasAcc_ = Z_3x3; + delVdelBiasOmega_ = Z_3x3; + delRdelBiasOmega_ = Z_3x3; + PreintMeasCov_.setZero(); +} + +//------------------------------------------------------------------------------ +void ImuFactor::PreintegratedMeasurements::integrateMeasurement( + const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT, + boost::optional body_P_sensor) { + + // NOTE: order is important here because each update uses old values (i.e., we have to update + // jacobians and covariances before updating preintegrated measurements). + + // First we compensate the measurements for the bias + Vector3 correctedAcc = biasHat_.correctAccelerometer(measuredAcc); + Vector3 correctedOmega = biasHat_.correctGyroscope(measuredOmega); + + // Then compensate for sensor-body displacement: we express the quantities (originally in the IMU frame) into the body frame + if(body_P_sensor){ + Matrix3 body_R_sensor = body_P_sensor->rotation().matrix(); + correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame + Matrix3 body_omega_body__cross = skewSymmetric(correctedOmega); + correctedAcc = body_R_sensor * correctedAcc - body_omega_body__cross * body_omega_body__cross * body_P_sensor->translation().vector(); + // linear acceleration vector in the body frame + } + + const Vector3 theta_incr = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement + const Rot3 Rincr = Rot3::Expmap(theta_incr); // rotation increment computed from the current rotation rate measurement + + const Matrix3 Jr_theta_incr = Rot3::rightJacobianExpMapSO3(theta_incr); // Right jacobian computed at theta_incr + + // Update Jacobians + /* ----------------------------------------------------------------------------------------------------------------------- */ + if(!use2ndOrderIntegration_){ + delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT; + delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT; + }else{ + delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - 0.5 * deltaRij_.matrix() * deltaT*deltaT; + delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT - 0.5 * deltaRij_.matrix() + * skewSymmetric(biasHat_.correctAccelerometer(measuredAcc)) * deltaT*deltaT * delRdelBiasOmega_; + } + delVdelBiasAcc_ += -deltaRij_.matrix() * deltaT; + delVdelBiasOmega_ += -deltaRij_.matrix() * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega_; + delRdelBiasOmega_ = Rincr.inverse().matrix() * delRdelBiasOmega_ - Jr_theta_incr * deltaT; + + // Update preintegrated measurements covariance + // as in [2] we consider a first order propagation that can be seen as a prediction phase in an EKF framework + /* ----------------------------------------------------------------------------------------------------------------------- */ + const Vector3 theta_i = Rot3::Logmap(deltaRij_); // parametrization of so(3) + const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3(theta_i); + + Rot3 Rot_j = deltaRij_ * Rincr; + const Vector3 theta_j = Rot3::Logmap(Rot_j); // parametrization of so(3) + const Matrix3 Jrinv_theta_j = Rot3::rightJacobianExpMapSO3inverse(theta_j); + + Matrix H_pos_pos = I_3x3; + Matrix H_pos_vel = I_3x3 * deltaT; + Matrix H_pos_angles = Z_3x3; + + Matrix H_vel_pos = Z_3x3; + Matrix H_vel_vel = I_3x3; + Matrix H_vel_angles = - deltaRij_.matrix() * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT; + // analytic expression corresponding to the following numerical derivative + // Matrix H_vel_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_vel, correctedOmega, correctedAcc, deltaT, _1, deltaVij), theta_i); + + Matrix H_angles_pos = Z_3x3; + Matrix H_angles_vel = Z_3x3; + Matrix H_angles_angles = Jrinv_theta_j * Rincr.inverse().matrix() * Jr_theta_i; + // analytic expression corresponding to the following numerical derivative + // Matrix H_angles_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_angles, correctedOmega, deltaT, _1), thetaij); + + // overall Jacobian wrt preintegrated measurements (df/dx) + Matrix F(9,9); + F << H_pos_pos, H_pos_vel, H_pos_angles, + H_vel_pos, H_vel_vel, H_vel_angles, + H_angles_pos, H_angles_vel, H_angles_angles; + + // first order uncertainty propagation: + // the deltaT allows to pass from continuous time noise to discrete time noise + // measurementCovariance_discrete = measurementCovariance_contTime * (1/deltaT) + // Gt * Qt * G =(approx)= measurementCovariance_discrete * deltaT^2 = measurementCovariance_contTime * deltaT + PreintMeasCov_ = F * PreintMeasCov_ * F.transpose() + measurementCovariance_ * deltaT ; + + // Extended version, without approximation: Gt * Qt * G =(approx)= measurementCovariance_contTime * deltaT + // This in only kept for documentation. + // + // Matrix G(9,9); + // G << I_3x3 * deltaT, Z_3x3, Z_3x3, + // Z_3x3, deltaRij.matrix() * deltaT, Z_3x3, + // Z_3x3, Z_3x3, Jrinv_theta_j * Jr_theta_incr * deltaT; + // + // PreintMeasCov = F * PreintMeasCov * F.transpose() + G * (1/deltaT) * measurementCovariance * G.transpose(); + + // Update preintegrated measurements (this has to be done after the update of covariances and jacobians!) + /* ----------------------------------------------------------------------------------------------------------------------- */ + if(!use2ndOrderIntegration_){ + deltaPij_ += deltaVij_ * deltaT; + }else{ + deltaPij_ += deltaVij_ * deltaT + 0.5 * deltaRij_.matrix() * biasHat_.correctAccelerometer(measuredAcc) * deltaT*deltaT; + } + deltaVij_ += deltaRij_.matrix() * correctedAcc * deltaT; + deltaRij_ = deltaRij_ * Rincr; + deltaTij_ += deltaT; +} + +//------------------------------------------------------------------------------ +// ImuFactor methods +//------------------------------------------------------------------------------ +ImuFactor::ImuFactor() : + preintegratedMeasurements_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3), use2ndOrderCoriolis_(false){} + +//------------------------------------------------------------------------------ +ImuFactor::ImuFactor( + Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias, + const PreintegratedMeasurements& preintegratedMeasurements, + const Vector3& gravity, const Vector3& omegaCoriolis, + boost::optional body_P_sensor, + const bool use2ndOrderCoriolis) : + Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.PreintMeasCov_), pose_i, vel_i, pose_j, vel_j, bias), + preintegratedMeasurements_(preintegratedMeasurements), + gravity_(gravity), + omegaCoriolis_(omegaCoriolis), + body_P_sensor_(body_P_sensor), + use2ndOrderCoriolis_(use2ndOrderCoriolis){ +} + +//------------------------------------------------------------------------------ +gtsam::NonlinearFactor::shared_ptr ImuFactor::clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); +} + +//------------------------------------------------------------------------------ +void ImuFactor::print(const string& s, const KeyFormatter& keyFormatter) const { + cout << s << "ImuFactor(" + << keyFormatter(this->key1()) << "," + << keyFormatter(this->key2()) << "," + << keyFormatter(this->key3()) << "," + << keyFormatter(this->key4()) << "," + << keyFormatter(this->key5()) << ")\n"; + preintegratedMeasurements_.print(" preintegrated measurements:"); + cout << " gravity: [ " << gravity_.transpose() << " ]" << endl; + cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << endl; + this->noiseModel_->print(" noise model: "); + if(this->body_P_sensor_) + this->body_P_sensor_->print(" sensor pose in body frame: "); +} + +//------------------------------------------------------------------------------ +bool ImuFactor::equals(const NonlinearFactor& expected, double tol) const { + const This *e = dynamic_cast (&expected); + return e != NULL && Base::equals(*e, tol) + && preintegratedMeasurements_.equals(e->preintegratedMeasurements_, tol) + && equal_with_abs_tol(gravity_, e->gravity_, tol) + && equal_with_abs_tol(omegaCoriolis_, e->omegaCoriolis_, tol) + && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_))); +} + +//------------------------------------------------------------------------------ +Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, + const imuBias::ConstantBias& bias, + boost::optional H1, boost::optional H2, + boost::optional H3, boost::optional H4, + boost::optional H5) const +{ + + const double& deltaTij = preintegratedMeasurements_.deltaTij_; + const Vector3 biasAccIncr = bias.accelerometer() - preintegratedMeasurements_.biasHat_.accelerometer(); + const Vector3 biasOmegaIncr = bias.gyroscope() - preintegratedMeasurements_.biasHat_.gyroscope(); + + // we give some shorter name to rotations and translations + const Rot3 Rot_i = pose_i.rotation(); + const Rot3 Rot_j = pose_j.rotation(); + const Vector3 pos_i = pose_i.translation().vector(); + const Vector3 pos_j = pose_j.translation().vector(); + + // We compute factor's Jacobians + /* ---------------------------------------------------------------------------------------------------- */ + const Rot3 deltaRij_biascorrected = preintegratedMeasurements_.deltaRij_.retract(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP); + // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr) + + Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); + + Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected - + Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij; // Coriolis term + + const Rot3 deltaRij_biascorrected_corioliscorrected = + Rot3::Expmap( theta_biascorrected_corioliscorrected ); + + const Rot3 fRhat = deltaRij_biascorrected_corioliscorrected.between(Rot_i.between(Rot_j)); + + const Matrix3 Jr_theta_bcc = Rot3::rightJacobianExpMapSO3(theta_biascorrected_corioliscorrected); + + const Matrix3 Jtheta = -Jr_theta_bcc * skewSymmetric(Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij); + + const Matrix3 Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat)); + + if(H1) { + H1->resize(9,6); + + Matrix3 dfPdPi; + Matrix3 dfVdPi; + if(use2ndOrderCoriolis_){ + dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij; + dfVdPi = skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij; + } + else{ + dfPdPi = - Rot_i.matrix(); + dfVdPi = Z_3x3; + } + + (*H1) << + // dfP/dRi + Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij_ + + preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr), + // dfP/dPi + dfPdPi, + // dfV/dRi + Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij_ + + preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr), + // dfV/dPi + dfVdPi, + // dfR/dRi + Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta), + // dfR/dPi + Z_3x3; + } + + if(H2) { + H2->resize(9,3); + (*H2) << + // dfP/dVi + - I_3x3 * deltaTij + + skewSymmetric(omegaCoriolis_) * deltaTij * deltaTij, // Coriolis term - we got rid of the 2 wrt ins paper + // dfV/dVi + - I_3x3 + + 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term + // dfR/dVi + Z_3x3; + } + + if(H3) { + H3->resize(9,6); + (*H3) << + // dfP/dPosej + Z_3x3, Rot_j.matrix(), + // dfV/dPosej + Matrix::Zero(3,6), + // dfR/dPosej + Jrinv_fRhat * ( I_3x3 ), Z_3x3; + } + + if(H4) { + H4->resize(9,3); + (*H4) << + // dfP/dVj + Z_3x3, + // dfV/dVj + I_3x3, + // dfR/dVj + Z_3x3; + } + + if(H5) { + const Matrix3 Jrinv_theta_bc = Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected); + const Matrix3 Jr_JbiasOmegaIncr = Rot3::rightJacobianExpMapSO3(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr); + const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega_; + + H5->resize(9,6); + (*H5) << + // dfP/dBias + - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasAcc_, + - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasOmega_, + // dfV/dBias + - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasAcc_, + - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasOmega_, + // dfR/dBias + Matrix::Zero(3,3), + Jrinv_fRhat * ( - fRhat.inverse().matrix() * JbiasOmega); + } + + // Evaluate residual error, according to [3] + /* ---------------------------------------------------------------------------------------------------- */ + const Vector3 fp = + pos_j - pos_i + - Rot_i.matrix() * (preintegratedMeasurements_.deltaPij_ + + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr + + preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr) + - vel_i * deltaTij + + skewSymmetric(omegaCoriolis_) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper + - 0.5 * gravity_ * deltaTij*deltaTij; + + const Vector3 fv = + vel_j - vel_i - Rot_i.matrix() * (preintegratedMeasurements_.deltaVij_ + + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr + + preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr) + + 2 * skewSymmetric(omegaCoriolis_) * vel_i * deltaTij // Coriolis term + - gravity_ * deltaTij; + + const Vector3 fR = Rot3::Logmap(fRhat); + + Vector r(9); r << fp, fv, fR; + return r; +} + +//------------------------------------------------------------------------------ +PoseVelocity ImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i, + const imuBias::ConstantBias& bias, const PreintegratedMeasurements preintegratedMeasurements, + const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis) +{ + + const double& deltaTij = preintegratedMeasurements.deltaTij_; + const Vector3 biasAccIncr = bias.accelerometer() - preintegratedMeasurements.biasHat_.accelerometer(); + const Vector3 biasOmegaIncr = bias.gyroscope() - preintegratedMeasurements.biasHat_.gyroscope(); + + const Rot3 Rot_i = pose_i.rotation(); + const Vector3 pos_i = pose_i.translation().vector(); + + // Predict state at time j + /* ---------------------------------------------------------------------------------------------------- */ + Vector3 pos_j = pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij_ + + preintegratedMeasurements.delPdelBiasAcc_ * biasAccIncr + + preintegratedMeasurements.delPdelBiasOmega_ * biasOmegaIncr) + + vel_i * deltaTij + - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper + + 0.5 * gravity * deltaTij*deltaTij; + + Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij_ + + preintegratedMeasurements.delVdelBiasAcc_ * biasAccIncr + + preintegratedMeasurements.delVdelBiasOmega_ * biasOmegaIncr) + - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term + + gravity * deltaTij); + + if(use2ndOrderCoriolis){ + pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij; // 2nd order coriolis term for position + vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity + } + + const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij_.retract(preintegratedMeasurements.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP); + // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr) + Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); + Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected - + Rot_i.inverse().matrix() * omegaCoriolis * deltaTij; // Coriolis term + const Rot3 deltaRij_biascorrected_corioliscorrected = + Rot3::Expmap( theta_biascorrected_corioliscorrected ); + const Rot3 Rot_j = Rot_i.compose( deltaRij_biascorrected_corioliscorrected ); + + Pose3 pose_j = Pose3( Rot_j, Point3(pos_j) ); + return PoseVelocity(pose_j, vel_j); +} + +} /// namespace gtsam diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index d1b8a9d80..2af634926 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -11,23 +11,39 @@ /** * @file ImuFactor.h - * @author Luca Carlone, Stephen Williams, Richard Roberts, Vadim Indelman, David Jensen + * @author Luca Carlone + * @author Stephen Williams + * @author Richard Roberts + * @author Vadim Indelman + * @author David Jensen + * @author Frank Dellaert **/ #pragma once /* GTSAM includes */ #include -#include #include -#include #include -/* External or standard includes */ -#include - - namespace gtsam { + +/** + * + * @addtogroup SLAM + * + * If you are using the factor, please cite: + * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating conditionally + * independent sets in factor graphs: a unifying perspective based on smart factors, + * Int. Conf. on Robotics and Automation (ICRA), 2014. + * + ** REFERENCES: + * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008. + * [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built + * Environments Without Initial Conditions", TRO, 28(1):61-76, 2012. + * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: Computation of the Jacobian Matrices", Tech. Report, 2013. + */ + /** * Struct to hold return variables by the Predict Function */ @@ -35,265 +51,121 @@ struct PoseVelocity { Pose3 pose; Vector3 velocity; PoseVelocity(const Pose3& _pose, const Vector3& _velocity) : - pose(_pose), velocity(_velocity) { + pose(_pose), velocity(_velocity) { } }; +/** + * ImuFactor is a 5-ways factor involving previous state (pose and velocity of the vehicle at previous time step), + * current state (pose and velocity at current time step), and the bias estimate. According to the + * preintegration scheme proposed in [2], the ImuFactor includes many IMU measurements, which are + * "summarized" using the PreintegratedMeasurements class. + * Note that this factor does not force "temporal consistency" of the biases (which are usually + * slowly varying quantities), see also CombinedImuFactor for more details. + */ +class ImuFactor: public NoiseModelFactor5 { +public: + /** - * - * @addtogroup SLAM - * - * If you are using the factor, please cite: - * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating conditionally - * independent sets in factor graphs: a unifying perspective based on smart factors, - * Int. Conf. on Robotics and Automation (ICRA), 2014. - * - ** REFERENCES: - * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008. - * [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built - * Environments Without Initial Conditions", TRO, 28(1):61-76, 2012. - * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: Computation of the Jacobian Matrices", Tech. Report, 2013. + * PreintegratedMeasurements accumulates (integrates) the IMU measurements + * (rotation rates and accelerations) and the corresponding covariance matrix. + * The measurements are then used to build the Preintegrated IMU factor (ImuFactor). + * Integration is done incrementally (ideally, one integrates the measurement as soon as it is received + * from the IMU) so as to avoid costly integration at time of factor construction. */ + class PreintegratedMeasurements { - class ImuFactor: public NoiseModelFactor5 { - public: + friend class ImuFactor; - /** Struct to store results of preintegrating IMU measurements. Can be build - * incrementally so as to avoid costly integration at time of factor construction. */ + protected: + imuBias::ConstantBias biasHat_; ///< Acceleration and angular rate bias values used during preintegration + Eigen::Matrix measurementCovariance_; ///< (continuous-time uncertainty) "Covariance" of the vector [integrationError measuredAcc measuredOmega] in R^(9X9) - /** CombinedPreintegratedMeasurements accumulates (integrates) the IMU measurements (rotation rates and accelerations) - * and the corresponding covariance matrix. The measurements are then used to build the Preintegrated IMU factor*/ - class PreintegratedMeasurements { - friend class ImuFactor; - protected: - imuBias::ConstantBias biasHat_; ///< Acceleration and angular rate bias values used during preintegration - Matrix measurementCovariance_; ///< (continuous-time uncertainty) Covariance of the vector [integrationError measuredAcc measuredOmega] in R^(9X9) + Vector3 deltaPij_; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, in [2]) (in frame i) + Vector3 deltaVij_; ///< Preintegrated relative velocity (in global frame) + Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i) + double deltaTij_; ///< Time interval from i to j - Vector3 deltaPij_; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, in [2]) (in frame i) - Vector3 deltaVij_; ///< Preintegrated relative velocity (in global frame) - Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i) - double deltaTij_; ///< Time interval from i to j + Matrix3 delPdelBiasAcc_; ///< Jacobian of preintegrated position w.r.t. acceleration bias + Matrix3 delPdelBiasOmega_; ///< Jacobian of preintegrated position w.r.t. angular rate bias + Matrix3 delVdelBiasAcc_; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias + Matrix3 delVdelBiasOmega_; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias + Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias + + Eigen::Matrix PreintMeasCov_; ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION] + ///< (first-order propagation from *measurementCovariance*). + + bool use2ndOrderIntegration_; ///< Controls the order of integration - Matrix3 delPdelBiasAcc_; ///< Jacobian of preintegrated position w.r.t. acceleration bias - Matrix3 delPdelBiasOmega_; ///< Jacobian of preintegrated position w.r.t. angular rate bias - Matrix3 delVdelBiasAcc_; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias - Matrix3 delVdelBiasOmega_; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias - Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias - Matrix PreintMeasCov_; ///< Covariance matrix of the preintegrated measurements (first-order propagation from *measurementCovariance*) - bool use2ndOrderIntegration_; ///< Controls the order of integration public: - /** Default constructor, initialize with no IMU measurements */ - PreintegratedMeasurements( - const imuBias::ConstantBias& bias, ///< Current estimate of acceleration and rotation rate biases - const Matrix3& measuredAccCovariance, ///< Covariance matrix of measuredAcc - const Matrix3& measuredOmegaCovariance, ///< Covariance matrix of measured Angular Rate - const Matrix3& integrationErrorCovariance, ///< Covariance matrix of integration errors - const bool use2ndOrderIntegration = false ///< Controls the order of integration - ) : biasHat_(bias), measurementCovariance_(9,9), deltaPij_(Vector3::Zero()), deltaVij_(Vector3::Zero()), deltaTij_(0.0), - delPdelBiasAcc_(Matrix3::Zero()), delPdelBiasOmega_(Matrix3::Zero()), - delVdelBiasAcc_(Matrix3::Zero()), delVdelBiasOmega_(Matrix3::Zero()), - delRdelBiasOmega_(Matrix3::Zero()), PreintMeasCov_(9,9), use2ndOrderIntegration_(use2ndOrderIntegration) - { - measurementCovariance_ << integrationErrorCovariance , Matrix3::Zero(), Matrix3::Zero(), - Matrix3::Zero(), measuredAccCovariance, Matrix3::Zero(), - Matrix3::Zero(), Matrix3::Zero(), measuredOmegaCovariance; - PreintMeasCov_ = Matrix::Zero(9,9); - } - PreintegratedMeasurements() : - biasHat_(imuBias::ConstantBias()), measurementCovariance_(9,9), deltaPij_(Vector3::Zero()), deltaVij_(Vector3::Zero()), deltaTij_(0.0), - delPdelBiasAcc_(Matrix3::Zero()), delPdelBiasOmega_(Matrix3::Zero()), - delVdelBiasAcc_(Matrix3::Zero()), delVdelBiasOmega_(Matrix3::Zero()), - delRdelBiasOmega_(Matrix3::Zero()), PreintMeasCov_(9,9), use2ndOrderIntegration_(false) - { - measurementCovariance_ = Matrix::Zero(9,9); - PreintMeasCov_ = Matrix::Zero(9,9); - } + /** + * Default constructor, initializes the class with no measurements + * @param bias Current estimate of acceleration and rotation rate biases + * @param measuredAccCovariance Covariance matrix of measuredAcc + * @param measuredOmegaCovariance Covariance matrix of measured Angular Rate + * @param integrationErrorCovariance Covariance matrix of integration errors (velocity -> position) + * @param use2ndOrderIntegration Controls the order of integration + * (if false: p(t+1) = p(t) + v(t) deltaT ; if true: p(t+1) = p(t) + v(t) deltaT + 0.5 * acc(t) deltaT^2) + */ + PreintegratedMeasurements(const imuBias::ConstantBias& bias, + const Matrix3& measuredAccCovariance, const Matrix3& measuredOmegaCovariance, + const Matrix3& integrationErrorCovariance, const bool use2ndOrderIntegration = false); - /** print */ - void print(const std::string& s = "Preintegrated Measurements:") const { - std::cout << s << std::endl; - biasHat_.print(" biasHat"); - std::cout << " deltaTij " << deltaTij_ << std::endl; - std::cout << " deltaPij [ " << deltaPij_.transpose() << " ]" << std::endl; - std::cout << " deltaVij [ " << deltaVij_.transpose() << " ]" << std::endl; - deltaRij_.print(" deltaRij "); - std::cout << " measurementCovariance [ " << measurementCovariance_ << " ]" << std::endl; - std::cout << " PreintMeasCov [ " << PreintMeasCov_ << " ]" << std::endl; - } + /// print + void print(const std::string& s = "Preintegrated Measurements:") const; - /** equals */ - bool equals(const PreintegratedMeasurements& expected, double tol=1e-9) const { - return biasHat_.equals(expected.biasHat_, tol) - && equal_with_abs_tol(measurementCovariance_, expected.measurementCovariance_, tol) - && equal_with_abs_tol(deltaPij_, expected.deltaPij_, tol) - && equal_with_abs_tol(deltaVij_, expected.deltaVij_, tol) - && deltaRij_.equals(expected.deltaRij_, tol) - && std::fabs(deltaTij_ - expected.deltaTij_) < tol - && equal_with_abs_tol(delPdelBiasAcc_, expected.delPdelBiasAcc_, tol) - && equal_with_abs_tol(delPdelBiasOmega_, expected.delPdelBiasOmega_, tol) - && equal_with_abs_tol(delVdelBiasAcc_, expected.delVdelBiasAcc_, tol) - && equal_with_abs_tol(delVdelBiasOmega_, expected.delVdelBiasOmega_, tol) - && equal_with_abs_tol(delRdelBiasOmega_, expected.delRdelBiasOmega_, tol); - } - Matrix measurementCovariance() const {return measurementCovariance_;} - Matrix deltaRij() const {return deltaRij_.matrix();} - double deltaTij() const{return deltaTij_;} - Vector deltaPij() const {return deltaPij_;} - Vector deltaVij() const {return deltaVij_;} - Vector biasHat() const { return biasHat_.vector();} - Matrix delPdelBiasAcc() const { return delPdelBiasAcc_;} - Matrix delPdelBiasOmega() const { return delPdelBiasOmega_;} - Matrix delVdelBiasAcc() const { return delVdelBiasAcc_;} - Matrix delVdelBiasOmega() const { return delVdelBiasOmega_;} - Matrix delRdelBiasOmega() const{ return delRdelBiasOmega_;} - Matrix preintMeasCov() const { return PreintMeasCov_;} + /// equals + bool equals(const PreintegratedMeasurements& expected, double tol=1e-9) const; + /// Re-initialize PreintegratedMeasurements + void resetIntegration(); - void resetIntegration(){ - deltaPij_ = Vector3::Zero(); - deltaVij_ = Vector3::Zero(); - deltaRij_ = Rot3(); - deltaTij_ = 0.0; - delPdelBiasAcc_ = Matrix3::Zero(); - delPdelBiasOmega_ = Matrix3::Zero(); - delVdelBiasAcc_ = Matrix3::Zero(); - delVdelBiasOmega_ = Matrix3::Zero(); - delRdelBiasOmega_ = Matrix3::Zero(); - PreintMeasCov_ = Matrix::Zero(9,9); - } + /** + * Add a single IMU measurement to the preintegration. + * @param measuredAcc Measured acceleration (in body frame, as given by the sensor) + * @param measuredOmega Measured angular velocity (as given by the sensor) + * @param deltaT Time interval between two consecutive IMU measurements + * @param body_P_sensor Optional sensor frame (pose of the IMU in the body frame) + */ + void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT, + boost::optional body_P_sensor = boost::none); - /** Add a single IMU measurement to the preintegration. */ - void integrateMeasurement( - const Vector3& measuredAcc, ///< Measured linear acceleration (in body frame) - const Vector3& measuredOmega, ///< Measured angular velocity (in body frame) - double deltaT, ///< Time step - boost::optional body_P_sensor = boost::none ///< Sensor frame - ) { + /// methods to access class variables + Matrix measurementCovariance() const {return measurementCovariance_;} + Matrix deltaRij() const {return deltaRij_.matrix();} + double deltaTij() const{return deltaTij_;} + Vector deltaPij() const {return deltaPij_;} + Vector deltaVij() const {return deltaVij_;} + Vector biasHat() const { return biasHat_.vector();} + Matrix delPdelBiasAcc() const { return delPdelBiasAcc_;} + Matrix delPdelBiasOmega() const { return delPdelBiasOmega_;} + Matrix delVdelBiasAcc() const { return delVdelBiasAcc_;} + Matrix delVdelBiasOmega() const { return delVdelBiasOmega_;} + Matrix delRdelBiasOmega() const{ return delRdelBiasOmega_;} + Matrix preintMeasCov() const { return PreintMeasCov_;} - // NOTE: order is important here because each update uses old values. - // First we compensate the measurements for the bias - Vector3 correctedAcc = biasHat_.correctAccelerometer(measuredAcc); - Vector3 correctedOmega = biasHat_.correctGyroscope(measuredOmega); + /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */ + // This function is only used for test purposes (compare numerical derivatives wrt analytic ones) + static inline Vector PreIntegrateIMUObservations_delta_vel(const Vector& msr_gyro_t, const Vector& msr_acc_t, const double msr_dt, + const Vector3& delta_angles, const Vector& delta_vel_in_t0){ + // Note: all delta terms refer to an IMU\sensor system at t0 + Vector body_t_a_body = msr_acc_t; + Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles); + return delta_vel_in_t0 + R_t_to_t0.matrix() * body_t_a_body * msr_dt; + } - // Then compensate for sensor-body displacement: we express the quantities (originally in the IMU frame) into the body frame - if(body_P_sensor){ - Matrix3 body_R_sensor = body_P_sensor->rotation().matrix(); - correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame - Matrix3 body_omega_body__cross = skewSymmetric(correctedOmega); - correctedAcc = body_R_sensor * correctedAcc - body_omega_body__cross * body_omega_body__cross * body_P_sensor->translation().vector(); - // linear acceleration vector in the body frame - } - - const Vector3 theta_incr = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement - const Rot3 Rincr = Rot3::Expmap(theta_incr); // rotation increment computed from the current rotation rate measurement - - const Matrix3 Jr_theta_incr = Rot3::rightJacobianExpMapSO3(theta_incr); // Right jacobian computed at theta_incr - - // Update Jacobians - /* ----------------------------------------------------------------------------------------------------------------------- */ - if(!use2ndOrderIntegration_){ - delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT; - delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT; - }else{ - delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - 0.5 * deltaRij_.matrix() * deltaT*deltaT; - delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT - 0.5 * deltaRij_.matrix() - * skewSymmetric(biasHat_.correctAccelerometer(measuredAcc)) * deltaT*deltaT * delRdelBiasOmega_; - } - delVdelBiasAcc_ += -deltaRij_.matrix() * deltaT; - delVdelBiasOmega_ += -deltaRij_.matrix() * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega_; - delRdelBiasOmega_ = Rincr.inverse().matrix() * delRdelBiasOmega_ - Jr_theta_incr * deltaT; - - // Update preintegrated measurements covariance - /* ----------------------------------------------------------------------------------------------------------------------- */ - Matrix3 Z_3x3 = Matrix3::Zero(); - Matrix3 I_3x3 = Matrix3::Identity(); - const Vector3 theta_i = Rot3::Logmap(deltaRij_); // parametrization of so(3) - const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3(theta_i); - - Rot3 Rot_j = deltaRij_ * Rincr; - const Vector3 theta_j = Rot3::Logmap(Rot_j); // parametrization of so(3) - const Matrix3 Jrinv_theta_j = Rot3::rightJacobianExpMapSO3inverse(theta_j); - - // Update preintegrated measurements covariance: as in [2] we consider a first order propagation that - // can be seen as a prediction phase in an EKF framework - Matrix H_pos_pos = I_3x3; - Matrix H_pos_vel = I_3x3 * deltaT; - Matrix H_pos_angles = Z_3x3; - - Matrix H_vel_pos = Z_3x3; - Matrix H_vel_vel = I_3x3; - Matrix H_vel_angles = - deltaRij_.matrix() * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT; - // analytic expression corresponding to the following numerical derivative - // Matrix H_vel_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_vel, correctedOmega, correctedAcc, deltaT, _1, deltaVij), theta_i); - - Matrix H_angles_pos = Z_3x3; - Matrix H_angles_vel = Z_3x3; - Matrix H_angles_angles = Jrinv_theta_j * Rincr.inverse().matrix() * Jr_theta_i; - // analytic expression corresponding to the following numerical derivative - // Matrix H_angles_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_angles, correctedOmega, deltaT, _1), thetaij); - - // overall Jacobian wrt preintegrated measurements (df/dx) - Matrix F(9,9); - F << H_pos_pos, H_pos_vel, H_pos_angles, - H_vel_pos, H_vel_vel, H_vel_angles, - H_angles_pos, H_angles_vel, H_angles_angles; - - // first order uncertainty propagation: - // the deltaT allows to pass from continuous time noise to discrete time noise - // measurementCovariance_discrete = measurementCovariance_contTime * (1/deltaT) - // Gt * Qt * G =(approx)= measurementCovariance_discrete * deltaT^2 = measurementCovariance_contTime * deltaT - PreintMeasCov_ = F * PreintMeasCov_ * F.transpose() + measurementCovariance_ * deltaT ; - - // Extended version, without approximation: Gt * Qt * G =(approx)= measurementCovariance_contTime * deltaT - // - // Matrix G(9,9); - // G << I_3x3 * deltaT, Z_3x3, Z_3x3, - // Z_3x3, deltaRij.matrix() * deltaT, Z_3x3, - // Z_3x3, Z_3x3, Jrinv_theta_j * Jr_theta_incr * deltaT; - // - // PreintMeasCov = F * PreintMeasCov * F.transpose() + G * (1/deltaT) * measurementCovariance * G.transpose(); - - // Update preintegrated measurements - /* ----------------------------------------------------------------------------------------------------------------------- */ - if(!use2ndOrderIntegration_){ - deltaPij_ += deltaVij_ * deltaT; - }else{ - deltaPij_ += deltaVij_ * deltaT + 0.5 * deltaRij_.matrix() * biasHat_.correctAccelerometer(measuredAcc) * deltaT*deltaT; - } - deltaVij_ += deltaRij_.matrix() * correctedAcc * deltaT; - deltaRij_ = deltaRij_ * Rincr; - deltaTij_ += deltaT; - } - - /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */ - // This function is only used for test purposes (compare numerical derivatives wrt analytic ones) - static inline Vector PreIntegrateIMUObservations_delta_vel(const Vector& msr_gyro_t, const Vector& msr_acc_t, const double msr_dt, - const Vector3& delta_angles, const Vector& delta_vel_in_t0){ - - // Note: all delta terms refer to an IMU\sensor system at t0 - - Vector body_t_a_body = msr_acc_t; - Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles); - - return delta_vel_in_t0 + R_t_to_t0.matrix() * body_t_a_body * msr_dt; - } - - // This function is only used for test purposes (compare numerical derivatives wrt analytic ones) - static inline Vector PreIntegrateIMUObservations_delta_angles(const Vector& msr_gyro_t, const double msr_dt, - const Vector3& delta_angles){ - - // Note: all delta terms refer to an IMU\sensor system at t0 - - // Calculate the corrected measurements using the Bias object - Vector body_t_omega_body= msr_gyro_t; - - Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles); - - R_t_to_t0 = R_t_to_t0 * Rot3::Expmap( body_t_omega_body*msr_dt ); - return Rot3::Logmap(R_t_to_t0); - } - /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */ + // This function is only used for test purposes (compare numerical derivatives wrt analytic ones) + static inline Vector PreIntegrateIMUObservations_delta_angles(const Vector& msr_gyro_t, const double msr_dt, + const Vector3& delta_angles){ + // Note: all delta terms refer to an IMU\sensor system at t0 + // Calculate the corrected measurements using the Bias object + Vector body_t_omega_body= msr_gyro_t; + Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles); + R_t_to_t0 = R_t_to_t0 * Rot3::Expmap( body_t_omega_body*msr_dt ); + return Rot3::Logmap(R_t_to_t0); + } + /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */ private: /** Serialization function */ @@ -336,65 +208,41 @@ struct PoseVelocity { #endif /** Default constructor - only use for serialization */ - ImuFactor() : preintegratedMeasurements_(imuBias::ConstantBias(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()), use2ndOrderCoriolis_(false){} + ImuFactor(); - /** Constructor */ - ImuFactor( - Key pose_i, ///< previous pose key - Key vel_i, ///< previous velocity key - Key pose_j, ///< current pose key - Key vel_j, ///< current velocity key - Key bias, ///< previous bias key - const PreintegratedMeasurements& preintegratedMeasurements, ///< preintegrated IMU measurements - const Vector3& gravity, ///< gravity vector - const Vector3& omegaCoriolis, ///< rotation rate of the inertial frame - boost::optional body_P_sensor = boost::none, ///< The Pose of the sensor frame in the body frame - const bool use2ndOrderCoriolis = false ///< When true, the second-order term is used in the calculation of the Coriolis Effect - ) : - Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.PreintMeasCov_), pose_i, vel_i, pose_j, vel_j, bias), - preintegratedMeasurements_(preintegratedMeasurements), - gravity_(gravity), - omegaCoriolis_(omegaCoriolis), - body_P_sensor_(body_P_sensor), - use2ndOrderCoriolis_(use2ndOrderCoriolis){ - } + /** + * Constructor + * @param pose_i Previous pose key + * @param vel_i Previous velocity key + * @param pose_j Current pose key + * @param vel_j Current velocity key + * @param bias Previous bias key + * @param preintegratedMeasurements Preintegrated IMU measurements + * @param gravity Gravity vector expressed in the global frame + * @param omegaCoriolis Rotation rate of the global frame w.r.t. an inertial frame + * @param body_P_sensor Optional pose of the sensor frame in the body frame + * @param use2ndOrderCoriolis When true, the second-order term is used in the calculation of the Coriolis Effect + */ + ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias, + const PreintegratedMeasurements& preintegratedMeasurements, + const Vector3& gravity, const Vector3& omegaCoriolis, + boost::optional body_P_sensor = boost::none, const bool use2ndOrderCoriolis = false); virtual ~ImuFactor() {} /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { - return boost::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new This(*this))); } + virtual gtsam::NonlinearFactor::shared_ptr clone() const; /** implement functions needed for Testable */ - /** print */ - virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { - std::cout << s << "ImuFactor(" - << keyFormatter(this->key1()) << "," - << keyFormatter(this->key2()) << "," - << keyFormatter(this->key3()) << "," - << keyFormatter(this->key4()) << "," - << keyFormatter(this->key5()) << ")\n"; - preintegratedMeasurements_.print(" preintegrated measurements:"); - std::cout << " gravity: [ " << gravity_.transpose() << " ]" << std::endl; - std::cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << std::endl; - this->noiseModel_->print(" noise model: "); - if(this->body_P_sensor_) - this->body_P_sensor_->print(" sensor pose in body frame: "); - } + /// print + virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; - /** equals */ - virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { - const This *e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) - && preintegratedMeasurements_.equals(e->preintegratedMeasurements_, tol) - && equal_with_abs_tol(gravity_, e->gravity_, tol) - && equal_with_abs_tol(omegaCoriolis_, e->omegaCoriolis_, tol) - && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_))); - } + /// equals + virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const; /** Access the preintegrated measurements. */ + const PreintegratedMeasurements& preintegratedMeasurements() const { return preintegratedMeasurements_; } @@ -404,205 +252,19 @@ struct PoseVelocity { /** implement functions needed to derive from Factor */ - /** vector of errors */ + /// vector of errors Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, const imuBias::ConstantBias& bias, boost::optional H1 = boost::none, boost::optional H2 = boost::none, boost::optional H3 = boost::none, boost::optional H4 = boost::none, - boost::optional H5 = boost::none) const - { + boost::optional H5 = boost::none) const; - const double& deltaTij = preintegratedMeasurements_.deltaTij_; - const Vector3 biasAccIncr = bias.accelerometer() - preintegratedMeasurements_.biasHat_.accelerometer(); - const Vector3 biasOmegaIncr = bias.gyroscope() - preintegratedMeasurements_.biasHat_.gyroscope(); - - // we give some shorter name to rotations and translations - const Rot3 Rot_i = pose_i.rotation(); - const Rot3 Rot_j = pose_j.rotation(); - const Vector3 pos_i = pose_i.translation().vector(); - const Vector3 pos_j = pose_j.translation().vector(); - - // We compute factor's Jacobians - /* ---------------------------------------------------------------------------------------------------- */ - const Rot3 deltaRij_biascorrected = preintegratedMeasurements_.deltaRij_.retract(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP); - // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr) - - Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); - - Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected - - Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij; // Coriolis term - - const Rot3 deltaRij_biascorrected_corioliscorrected = - Rot3::Expmap( theta_biascorrected_corioliscorrected ); - - const Rot3 fRhat = deltaRij_biascorrected_corioliscorrected.between(Rot_i.between(Rot_j)); - - const Matrix3 Jr_theta_bcc = Rot3::rightJacobianExpMapSO3(theta_biascorrected_corioliscorrected); - - const Matrix3 Jtheta = -Jr_theta_bcc * skewSymmetric(Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij); - - const Matrix3 Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat)); - - if(H1) { - H1->resize(9,6); - - Matrix3 dfPdPi; - Matrix3 dfVdPi; - if(use2ndOrderCoriolis_){ - dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij; - dfVdPi = skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij; - } - else{ - dfPdPi = - Rot_i.matrix(); - dfVdPi = Matrix3::Zero(); - } - - (*H1) << - // dfP/dRi - Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij_ - + preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr), - // dfP/dPi - dfPdPi, - // dfV/dRi - Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij_ - + preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr), - // dfV/dPi - dfVdPi, - // dfR/dRi - Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta), - // dfR/dPi - Matrix3::Zero(); - } - - if(H2) { - H2->resize(9,3); - (*H2) << - // dfP/dVi - - Matrix3::Identity() * deltaTij - + skewSymmetric(omegaCoriolis_) * deltaTij * deltaTij, // Coriolis term - we got rid of the 2 wrt ins paper - // dfV/dVi - - Matrix3::Identity() - + 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term - // dfR/dVi - Matrix3::Zero(); - } - - if(H3) { - - H3->resize(9,6); - (*H3) << - // dfP/dPosej - Matrix3::Zero(), Rot_j.matrix(), - // dfV/dPosej - Matrix::Zero(3,6), - // dfR/dPosej - Jrinv_fRhat * ( Matrix3::Identity() ), Matrix3::Zero(); - } - - if(H4) { - H4->resize(9,3); - (*H4) << - // dfP/dVj - Matrix3::Zero(), - // dfV/dVj - Matrix3::Identity(), - // dfR/dVj - Matrix3::Zero(); - } - - if(H5) { - - const Matrix3 Jrinv_theta_bc = Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected); - const Matrix3 Jr_JbiasOmegaIncr = Rot3::rightJacobianExpMapSO3(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr); - const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega_; - - H5->resize(9,6); - (*H5) << - // dfP/dBias - - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasAcc_, - - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasOmega_, - // dfV/dBias - - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasAcc_, - - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasOmega_, - // dfR/dBias - Matrix::Zero(3,3), - Jrinv_fRhat * ( - fRhat.inverse().matrix() * JbiasOmega); - } - - // Evaluate residual error, according to [3] - /* ---------------------------------------------------------------------------------------------------- */ - const Vector3 fp = - pos_j - pos_i - - Rot_i.matrix() * (preintegratedMeasurements_.deltaPij_ - + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr - + preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr) - - vel_i * deltaTij - + skewSymmetric(omegaCoriolis_) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper - - 0.5 * gravity_ * deltaTij*deltaTij; - - const Vector3 fv = - vel_j - vel_i - Rot_i.matrix() * (preintegratedMeasurements_.deltaVij_ - + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr - + preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr) - + 2 * skewSymmetric(omegaCoriolis_) * vel_i * deltaTij // Coriolis term - - gravity_ * deltaTij; - - const Vector3 fR = Rot3::Logmap(fRhat); - - Vector r(9); r << fp, fv, fR; - return r; - } - - - /** predicted states from IMU */ + /// predicted states from IMU static PoseVelocity Predict(const Pose3& pose_i, const Vector3& vel_i, const imuBias::ConstantBias& bias, const PreintegratedMeasurements preintegratedMeasurements, - const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional body_P_sensor = boost::none, - const bool use2ndOrderCoriolis = false) - { - - const double& deltaTij = preintegratedMeasurements.deltaTij_; - const Vector3 biasAccIncr = bias.accelerometer() - preintegratedMeasurements.biasHat_.accelerometer(); - const Vector3 biasOmegaIncr = bias.gyroscope() - preintegratedMeasurements.biasHat_.gyroscope(); - - const Rot3 Rot_i = pose_i.rotation(); - const Vector3 pos_i = pose_i.translation().vector(); - - // Predict state at time j - /* ---------------------------------------------------------------------------------------------------- */ - Vector3 pos_j = pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij_ - + preintegratedMeasurements.delPdelBiasAcc_ * biasAccIncr - + preintegratedMeasurements.delPdelBiasOmega_ * biasOmegaIncr) - + vel_i * deltaTij - - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper - + 0.5 * gravity * deltaTij*deltaTij; - - Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij_ - + preintegratedMeasurements.delVdelBiasAcc_ * biasAccIncr - + preintegratedMeasurements.delVdelBiasOmega_ * biasOmegaIncr) - - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term - + gravity * deltaTij); - - if(use2ndOrderCoriolis){ - pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij; // 2nd order coriolis term for position - vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity - } - - const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij_.retract(preintegratedMeasurements.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP); - // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr) - Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); - Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected - - Rot_i.inverse().matrix() * omegaCoriolis * deltaTij; // Coriolis term - const Rot3 deltaRij_biascorrected_corioliscorrected = - Rot3::Expmap( theta_biascorrected_corioliscorrected ); - const Rot3 Rot_j = Rot_i.compose( deltaRij_biascorrected_corioliscorrected ); - - Pose3 pose_j = Pose3( Rot_j, Point3(pos_j) ); - return PoseVelocity(pose_j, vel_j); - } - + const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false); private: @@ -617,7 +279,7 @@ struct PoseVelocity { ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_); ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); } - }; // \class ImuFactor + }; // class ImuFactor typedef ImuFactor::PreintegratedMeasurements ImuFactorPreintegratedMeasurements; diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index fda82eca9..aab38f258 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -10,19 +10,22 @@ * -------------------------------------------------------------------------- */ /** - * @file testImuFactor.cpp - * @brief Unit test for ImuFactor - * @author Luca Carlone, Stephen Williams, Richard Roberts + * @file testCombinedImuFactor.cpp + * @brief Unit test for Lupton-style combined IMU factor + * @author Luca Carlone + * @author Stephen Williams + * @author Richard Roberts */ -#include -#include #include #include #include #include +#include +#include #include #include + #include #include diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index eac021f4e..cf81c32ae 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -39,15 +39,13 @@ using symbol_shorthand::B; namespace { Vector callEvaluateError(const ImuFactor& factor, const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, - const imuBias::ConstantBias& bias) -{ + const imuBias::ConstantBias& bias){ return factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias); } Rot3 evaluateRotationError(const ImuFactor& factor, const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, - const imuBias::ConstantBias& bias) -{ + const imuBias::ConstantBias& bias){ return Rot3::Expmap(factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias).tail(3) ) ; } @@ -56,9 +54,7 @@ ImuFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements( const list& measuredAccs, const list& measuredOmegas, const list& deltaTs, - const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) - ) -{ + const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) ){ ImuFactor::PreintegratedMeasurements result(bias, Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity()); @@ -68,7 +64,6 @@ ImuFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements( for( ; itAcc != measuredAccs.end(); ++itAcc, ++itOmega, ++itDeltaT) { result.integrateMeasurement(*itAcc, *itOmega, *itDeltaT); } - return result; } @@ -77,8 +72,7 @@ Vector3 evaluatePreintegratedMeasurementsPosition( const list& measuredAccs, const list& measuredOmegas, const list& deltaTs, - const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) ) -{ + const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) ){ return evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs).deltaPij(); } @@ -99,20 +93,16 @@ Rot3 evaluatePreintegratedMeasurementsRotation( const list& measuredAccs, const list& measuredOmegas, const list& deltaTs, - const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) ) -{ + const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) ){ return Rot3(evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs, initialRotationRate).deltaRij()); } -Rot3 evaluateRotation(const Vector3 measuredOmega, const Vector3 biasOmega, const double deltaT) -{ +Rot3 evaluateRotation(const Vector3 measuredOmega, const Vector3 biasOmega, const double deltaT){ return Rot3::Expmap((measuredOmega - biasOmega) * deltaT); } - -Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta) -{ +Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta){ return Rot3::Logmap( Rot3::Expmap(thetahat).compose( Rot3::Expmap(deltatheta) ) ); } @@ -212,7 +202,6 @@ TEST( ImuFactor, Error ) Matrix H1a, H2a, H3a, H4a, H5a; (void) factor.evaluateError(x1, v1, x2, v2, bias, H1a, H2a, H3a, H4a, H5a); - // positions and velocities Matrix H1etop6 = H1e.topRows(6); Matrix H1atop6 = H1a.topRows(6); @@ -230,7 +219,7 @@ TEST( ImuFactor, Error ) EXPECT(assert_equal(RH3e, H3a.bottomRows(3), 1e-5)); // 1e-5 needs to be added only when using quaternions for rotations EXPECT(assert_equal(H4e, H4a)); -// EXPECT(assert_equal(H5e, H5a)); + // EXPECT(assert_equal(H5e, H5a)); } /* ************************************************************************* */ @@ -243,7 +232,6 @@ TEST( ImuFactor, ErrorWithBiases ) // Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/10.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0)); // Vector3 v2(Vector3(0.5, 0.0, 0.0)); - imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot) Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0)); Vector3 v1(Vector3(0.5, 0.0, 0.0)); @@ -260,8 +248,8 @@ TEST( ImuFactor, ErrorWithBiases ) ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()); pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); -// ImuFactor::PreintegratedMeasurements pre_int_data(bias.head(3), bias.tail(3)); -// pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + // ImuFactor::PreintegratedMeasurements pre_int_data(bias.head(3), bias.tail(3)); + // pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); // Create factor ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis); @@ -272,7 +260,7 @@ TEST( ImuFactor, ErrorWithBiases ) // Expected error Vector errorExpected(9); errorExpected << 0, 0, 0, 0, 0, 0, 0, 0, 0; -// EXPECT(assert_equal(errorExpected, errorActual, 1e-6)); + // EXPECT(assert_equal(errorExpected, errorActual, 1e-6)); // Expected Jacobians Matrix H1e = numericalDerivative11( @@ -315,7 +303,6 @@ TEST( ImuFactor, PartialDerivativeExpmap ) Vector3 measuredOmega; measuredOmega << 0.1, 0, 0; double deltaT = 0.5; - // Compute numerical derivatives Matrix expectedDelRdelBiasOmega = numericalDerivative11(boost::bind( &evaluateRotation, measuredOmega, _1, deltaT), Vector3(biasOmega)); @@ -326,7 +313,6 @@ TEST( ImuFactor, PartialDerivativeExpmap ) // Compare Jacobians EXPECT(assert_equal(expectedDelRdelBiasOmega, actualdelRdelBiasOmega, 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations - } /* ************************************************************************* */ @@ -349,9 +335,6 @@ TEST( ImuFactor, PartialDerivativeLogmap ) const Matrix3 actualDelFdeltheta = Matrix3::Identity() + 0.5 * X + (1/(normx*normx) - (1+cos(normx))/(2*normx * sin(normx)) ) * X * X; -// std::cout << "actualDelFdeltheta" << actualDelFdeltheta << std::endl; -// std::cout << "expectedDelFdeltheta" << expectedDelFdeltheta << std::endl; - // Compare Jacobians EXPECT(assert_equal(expectedDelFdeltheta, actualDelFdeltheta)); @@ -361,30 +344,30 @@ TEST( ImuFactor, PartialDerivativeLogmap ) TEST( ImuFactor, fistOrderExponential ) { // Linearization point - Vector3 biasOmega; biasOmega << 0,0,0; ///< Current estimate of rotation rate bias + Vector3 biasOmega; biasOmega << 0,0,0; ///< Current estimate of rotation rate bias - // Measurements - Vector3 measuredOmega; measuredOmega << 0.1, 0, 0; - double deltaT = 1.0; + // Measurements + Vector3 measuredOmega; measuredOmega << 0.1, 0, 0; + double deltaT = 1.0; - // change w.r.t. linearization point - double alpha = 0.0; - Vector3 deltabiasOmega; deltabiasOmega << alpha,alpha,alpha; + // change w.r.t. linearization point + double alpha = 0.0; + Vector3 deltabiasOmega; deltabiasOmega << alpha,alpha,alpha; - const Matrix3 Jr = Rot3::rightJacobianExpMapSO3((measuredOmega - biasOmega) * deltaT); + const Matrix3 Jr = Rot3::rightJacobianExpMapSO3((measuredOmega - biasOmega) * deltaT); - Matrix3 delRdelBiasOmega = - Jr * deltaT; // the delta bias appears with the minus sign + Matrix3 delRdelBiasOmega = - Jr * deltaT; // the delta bias appears with the minus sign - const Matrix expectedRot = Rot3::Expmap((measuredOmega - biasOmega - deltabiasOmega) * deltaT).matrix(); + const Matrix expectedRot = Rot3::Expmap((measuredOmega - biasOmega - deltabiasOmega) * deltaT).matrix(); - const Matrix3 hatRot = Rot3::Expmap((measuredOmega - biasOmega) * deltaT).matrix(); - const Matrix3 actualRot = - hatRot * Rot3::Expmap(delRdelBiasOmega * deltabiasOmega).matrix(); - //hatRot * (Matrix3::Identity() + skewSymmetric(delRdelBiasOmega * deltabiasOmega)); + const Matrix3 hatRot = Rot3::Expmap((measuredOmega - biasOmega) * deltaT).matrix(); + const Matrix3 actualRot = + hatRot * Rot3::Expmap(delRdelBiasOmega * deltabiasOmega).matrix(); + //hatRot * (Matrix3::Identity() + skewSymmetric(delRdelBiasOmega * deltabiasOmega)); - // Compare Jacobians - EXPECT(assert_equal(expectedRot, actualRot)); + // Compare Jacobians + EXPECT(assert_equal(expectedRot, actualRot)); } /* ************************************************************************* */ @@ -495,7 +478,6 @@ TEST( ImuFactor, FirstOrderPreIntegratedMeasurements ) // tictoc_print_(); //} - /* ************************************************************************* */ TEST( ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement ) { @@ -515,15 +497,9 @@ TEST( ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement ) const Pose3 body_P_sensor(Rot3::Expmap(Vector3(0,0.10,0.10)), Point3(1,0,0)); -// ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), -// Vector3(0.0, 0.0, 0.0)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), measuredOmega); - - ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()); - - pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); // Create factor @@ -560,6 +536,7 @@ TEST( ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement ) EXPECT(assert_equal(H5e, H5a)); } +/* ************************************************************************* */ TEST(ImuFactor, PredictPositionAndVelocity){ imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot) @@ -593,6 +570,7 @@ TEST(ImuFactor, PredictPositionAndVelocity){ } +/* ************************************************************************* */ TEST(ImuFactor, PredictRotation) { imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot) diff --git a/gtsam_unstable/nonlinear/AdaptAutoDiff.h b/gtsam/nonlinear/AdaptAutoDiff.h similarity index 88% rename from gtsam_unstable/nonlinear/AdaptAutoDiff.h rename to gtsam/nonlinear/AdaptAutoDiff.h index 96978d9cf..5118426b4 100644 --- a/gtsam_unstable/nonlinear/AdaptAutoDiff.h +++ b/gtsam/nonlinear/AdaptAutoDiff.h @@ -18,8 +18,9 @@ #pragma once -#include +#include #include +#include namespace gtsam { @@ -50,11 +51,8 @@ class AdaptAutoDiff { public: - typedef Eigen::Matrix JacobianTA1; - typedef Eigen::Matrix JacobianTA2; - - T operator()(const A1& a1, const A2& a2, boost::optional H1 = - boost::none, boost::optional H2 = boost::none) { + T operator()(const A1& a1, const A2& a2, OptionalJacobian H1 = boost::none, + OptionalJacobian H2 = boost::none) { using ceres::internal::AutoDiff; diff --git a/gtsam/nonlinear/Expression-inl.h b/gtsam/nonlinear/Expression-inl.h index 08dd18ee3..48445bba5 100644 --- a/gtsam/nonlinear/Expression-inl.h +++ b/gtsam/nonlinear/Expression-inl.h @@ -23,6 +23,7 @@ #include #include #include +#include #include #include @@ -42,21 +43,21 @@ namespace gtsam { const unsigned TraceAlignment = 16; -template -T & upAlign(T & value, unsigned requiredAlignment = TraceAlignment){ +template +T & upAlign(T & value, unsigned requiredAlignment = TraceAlignment) { // right now only word sized types are supported. // Easy to extend if needed, // by somehow inferring the unsigned integer of same size BOOST_STATIC_ASSERT(sizeof(T) == sizeof(size_t)); size_t & uiValue = reinterpret_cast(value); size_t misAlignment = uiValue % requiredAlignment; - if(misAlignment) { + if (misAlignment) { uiValue += requiredAlignment - misAlignment; } return value; } -template -T upAligned(T value, unsigned requiredAlignment = TraceAlignment){ +template +T upAligned(T value, unsigned requiredAlignment = TraceAlignment) { return upAlign(value, requiredAlignment); } @@ -88,33 +89,34 @@ public: namespace internal { -template +template struct UseBlockIf { static void addToJacobian(const Eigen::MatrixBase& dTdA, - JacobianMap& jacobians, Key key){ + JacobianMap& jacobians, Key key) { // block makes HUGE difference - jacobians(key).block(0, 0) += dTdA; - }; + jacobians(key).block( + 0, 0) += dTdA; + } + ; }; /// Handle Leaf Case for Dynamic Matrix type (slower) -template +template struct UseBlockIf { static void addToJacobian(const Eigen::MatrixBase& dTdA, - JacobianMap& jacobians, Key key) { + JacobianMap& jacobians, Key key) { jacobians(key) += dTdA; } }; } -/// Handle Leaf Case: reverseAD ends here, by writing a matrix into Jacobians +/// Handle Leaf Case: reverse AD ends here, by writing a matrix into Jacobians template void handleLeafCase(const Eigen::MatrixBase& dTdA, JacobianMap& jacobians, Key key) { internal::UseBlockIf< - Derived::RowsAtCompileTime != Eigen::Dynamic && - Derived::ColsAtCompileTime != Eigen::Dynamic, - Derived> - ::addToJacobian(dTdA, jacobians, key); + Derived::RowsAtCompileTime != Eigen::Dynamic + && Derived::ColsAtCompileTime != Eigen::Dynamic, Derived>::addToJacobian( + dTdA, jacobians, key); } //----------------------------------------------------------------------------- @@ -186,28 +188,28 @@ public: } } /** - * *** This is the main entry point for reverseAD, called from Expression *** + * *** This is the main entry point for reverse AD, called from Expression *** * Called only once, either inserts I into Jacobians (Leaf) or starts AD (Function) */ typedef Eigen::Matrix JacobianTT; - void startReverseAD(JacobianMap& jacobians) const { + void startReverseAD1(JacobianMap& jacobians) const { if (kind == Leaf) { // This branch will only be called on trivial Leaf expressions, i.e. Priors static const JacobianTT I = JacobianTT::Identity(); handleLeafCase(I, jacobians, content.key); } else if (kind == Function) // This is the more typical entry point, starting the AD pipeline - // Inside the startReverseAD that the correctly dimensioned pipeline is chosen. - content.ptr->startReverseAD(jacobians); + // Inside startReverseAD2 the correctly dimensioned pipeline is chosen. + content.ptr->startReverseAD2(jacobians); } // Either add to Jacobians (Leaf) or propagate (Function) template - void reverseAD(const Eigen::MatrixBase & dTdA, + void reverseAD1(const Eigen::MatrixBase & dTdA, JacobianMap& jacobians) const { if (kind == Leaf) handleLeafCase(dTdA, jacobians, content.key); else if (kind == Function) - content.ptr->reverseAD(dTdA, jacobians); + content.ptr->reverseAD2(dTdA, jacobians); } /// Define type so we can apply it as a meta-function @@ -265,7 +267,7 @@ public: /// Construct an execution trace for reverse AD virtual T traceExecution(const Values& values, ExecutionTrace& trace, - ExecutionTraceStorage* traceStorage) const = 0; + ExecutionTraceStorage* traceStorage) const = 0; }; //----------------------------------------------------------------------------- @@ -336,7 +338,7 @@ public: /// Construct an execution trace for reverse AD virtual const value_type& traceExecution(const Values& values, - ExecutionTrace& trace, + ExecutionTrace& trace, ExecutionTraceStorage* traceStorage) const { trace.setLeaf(key_); return dynamic_cast(values.at(key_)); @@ -454,10 +456,9 @@ struct Jacobian { /// meta-function to generate JacobianTA optional reference template -struct OptionalJacobian { - typedef Eigen::Matrix::value, - traits::dimension::value> Jacobian; - typedef boost::optional type; +struct MakeOptionalJacobian { + typedef OptionalJacobian::value, + traits::dimension::value> type; }; /** @@ -470,10 +471,10 @@ struct FunctionalBase: ExpressionNode { struct Record { void print(const std::string& indent) const { } - void startReverseAD(JacobianMap& jacobians) const { + void startReverseAD4(JacobianMap& jacobians) const { } template - void reverseAD(const SomeMatrix & dFdT, JacobianMap& jacobians) const { + void reverseAD4(const SomeMatrix & dFdT, JacobianMap& jacobians) const { } }; /// Construct an execution trace for reverse AD @@ -505,9 +506,9 @@ struct JacobianTrace { typename Jacobian::type dTdA; }; -/** - * Recursive Definition of Functional ExpressionNode - */ +// Recursive Definition of Functional ExpressionNode +// The reason we inherit from Argument is because we can then +// case to this unique signature to retrieve the expression at any level template struct GenerateFunctionalNode: Argument, Base { @@ -528,7 +529,9 @@ struct GenerateFunctionalNode: Argument, Base { This::expression->dims(map); } - /// Recursive Record Class for Functional Expressions + // Recursive Record Class for Functional Expressions + // The reason we inherit from JacobianTrace is because we can then + // case to this unique signature to retrieve the value/trace at any level struct Record: JacobianTrace, Base::Record { typedef T return_type; @@ -543,22 +546,31 @@ struct GenerateFunctionalNode: Argument, Base { } /// Start the reverse AD process - void startReverseAD(JacobianMap& jacobians) const { - Base::Record::startReverseAD(jacobians); - This::trace.reverseAD(This::dTdA, jacobians); + void startReverseAD4(JacobianMap& jacobians) const { + Base::Record::startReverseAD4(jacobians); + // This is the crucial point where the size of the AD pipeline is selected. + // One pipeline is started for each argument, but the number of rows in each + // pipeline is the same, namely the dimension of the output argument T. + // For example, if the entire expression is rooted by a binary function + // yielding a 2D result, then the matrix dTdA will have 2 rows. + // ExecutionTrace::reverseAD1 just passes this on to CallRecord::reverseAD2 + // which calls the correctly sized CallRecord::reverseAD3, which in turn + // calls reverseAD4 below. + This::trace.reverseAD1(This::dTdA, jacobians); } /// Given df/dT, multiply in dT/dA and continue reverse AD process - template - void reverseAD(const Eigen::Matrix & dFdT, + // Cols is always known at compile time + template + void reverseAD4(const SomeMatrix & dFdT, JacobianMap& jacobians) const { - Base::Record::reverseAD(dFdT, jacobians); - This::trace.reverseAD(dFdT * This::dTdA, jacobians); + Base::Record::reverseAD4(dFdT, jacobians); + This::trace.reverseAD1(dFdT * This::dTdA, jacobians); } }; /// Construct an execution trace for reverse AD - void trace(const Values& values, Record* record, + void trace(const Values& values, Record* record, ExecutionTraceStorage*& traceStorage) const { Base::trace(values, record, traceStorage); // recurse // Write an Expression execution trace in record->trace @@ -614,8 +626,8 @@ struct FunctionalNode { struct Record: public internal::CallRecordImplementor::value>, public Base::Record { using Base::Record::print; - using Base::Record::startReverseAD; - using Base::Record::reverseAD; + using Base::Record::startReverseAD4; + using Base::Record::reverseAD4; virtual ~Record() { } @@ -634,7 +646,7 @@ struct FunctionalNode { }; /// Construct an execution trace for reverse AD - Record* trace(const Values& values, + Record* trace(const Values& values, ExecutionTraceStorage* traceStorage) const { assert(reinterpret_cast(traceStorage) % TraceAlignment == 0); @@ -659,7 +671,8 @@ class UnaryExpression: public FunctionalNode >::type { public: - typedef boost::function::type)> Function; + typedef boost::function< + T(const A1&, typename MakeOptionalJacobian::type)> Function; typedef typename FunctionalNode >::type Base; typedef typename Base::Record Record; @@ -704,8 +717,8 @@ class BinaryExpression: public FunctionalNode >::t public: typedef boost::function< - T(const A1&, const A2&, typename OptionalJacobian::type, - typename OptionalJacobian::type)> Function; + T(const A1&, const A2&, typename MakeOptionalJacobian::type, + typename MakeOptionalJacobian::type)> Function; typedef typename FunctionalNode >::type Base; typedef typename Base::Record Record; @@ -758,9 +771,10 @@ class TernaryExpression: public FunctionalNode public: typedef boost::function< - T(const A1&, const A2&, const A3&, typename OptionalJacobian::type, - typename OptionalJacobian::type, - typename OptionalJacobian::type)> Function; + T(const A1&, const A2&, const A3&, + typename MakeOptionalJacobian::type, + typename MakeOptionalJacobian::type, + typename MakeOptionalJacobian::type)> Function; typedef typename FunctionalNode >::type Base; typedef typename Base::Record Record; @@ -776,7 +790,8 @@ private: this->template reset(e2.root()); this->template reset(e3.root()); ExpressionNode::traceSize_ = // - upAligned(sizeof(Record)) + e1.traceSize() + e2.traceSize() + e3.traceSize(); + upAligned(sizeof(Record)) + e1.traceSize() + e2.traceSize() + + e3.traceSize(); } friend class Expression ; diff --git a/gtsam/nonlinear/Expression.h b/gtsam/nonlinear/Expression.h index d44d21cd7..e63fbc686 100644 --- a/gtsam/nonlinear/Expression.h +++ b/gtsam/nonlinear/Expression.h @@ -20,8 +20,8 @@ #pragma once #include -#include #include +#include #include #include @@ -75,7 +75,7 @@ public: /// Construct a nullary method expression template Expression(const Expression& expression, - T (A::*method)(typename OptionalJacobian::type) const) : + T (A::*method)(typename MakeOptionalJacobian::type) const) : root_(new UnaryExpression(boost::bind(method, _1, _2), expression)) { } @@ -89,8 +89,8 @@ public: /// Construct a unary method expression template Expression(const Expression& expression1, - T (A1::*method)(const A2&, typename OptionalJacobian::type, - typename OptionalJacobian::type) const, + T (A1::*method)(const A2&, typename MakeOptionalJacobian::type, + typename MakeOptionalJacobian::type) const, const Expression& expression2) : root_( new BinaryExpression(boost::bind(method, _1, _2, _3, _4), @@ -206,10 +206,24 @@ private: // with an execution trace, made up entirely of "Record" structs, see // the FunctionalNode class in expression-inl.h size_t size = traceSize(); + + // Windows does not support variable length arrays, so memory must be dynamically + // allocated on Visual Studio. For more information see the issue below + // https://bitbucket.org/gtborg/gtsam/issue/178/vlas-unsupported-in-visual-studio +#ifdef _MSC_VER + ExecutionTraceStorage* traceStorage = new ExecutionTraceStorage[size]; +#else ExecutionTraceStorage traceStorage[size]; +#endif + ExecutionTrace trace; T value(traceExecution(values, trace, traceStorage)); - trace.startReverseAD(jacobians); + trace.startReverseAD1(jacobians); + +#ifdef _MSC_VER + delete[] traceStorage; +#endif + return value; } @@ -224,9 +238,8 @@ template struct apply_compose { typedef T result_type; static const int Dim = traits::dimension::value; - typedef Eigen::Matrix Jacobian; - T operator()(const T& x, const T& y, boost::optional H1, - boost::optional H2) const { + T operator()(const T& x, const T& y, OptionalJacobian H1 = + boost::none, OptionalJacobian H2 = boost::none) const { return x.compose(y, H1, H2); } }; diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 9fd4f8383..d7713d0d5 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -20,14 +20,14 @@ #pragma once -#include -#include - #include #include #include #include +#include +#include +#include /** * Macro to add a standard clone function to a derived factor diff --git a/gtsam/nonlinear/Values.cpp b/gtsam/nonlinear/Values.cpp index b92e54143..3cad10f31 100644 --- a/gtsam/nonlinear/Values.cpp +++ b/gtsam/nonlinear/Values.cpp @@ -130,6 +130,24 @@ namespace gtsam { throw ValuesKeyAlreadyExists(j); } + /* ************************************************************************* */ + void Values::insertFixed(Key j, const Vector& v, size_t n) { + switch (n) { + case 1: insert(j,v); break; + case 2: insert(j,v); break; + case 3: insert(j,v); break; + case 4: insert(j,v); break; + case 5: insert(j,v); break; + case 6: insert(j,v); break; + case 7: insert(j,v); break; + case 8: insert(j,v); break; + case 9: insert(j,v); break; + default: + throw runtime_error( + "Values::insert fixed size can only handle n in 1..9"); + } + } + /* ************************************************************************* */ void Values::insert(const Values& values) { for(const_iterator key_value = values.begin(); key_value != values.end(); ++key_value) { diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index d00baa0d9..9a4c7e798 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -258,6 +258,10 @@ namespace gtsam { template void insert(Key j, const ValueType& val); + /// Special version for small fixed size vectors, for matlab/python + /// throws truntime error if n<1 || n>9 + void insertFixed(Key j, const Vector& v, size_t n); + /// overloaded insert version that also specifies a chart template void insert(Key j, const ValueType& val); diff --git a/gtsam_unstable/nonlinear/tests/testAdaptAutoDiff.cpp b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp similarity index 98% rename from gtsam_unstable/nonlinear/tests/testAdaptAutoDiff.cpp rename to gtsam/nonlinear/tests/testAdaptAutoDiff.cpp index a8151ec11..7d31616c5 100644 --- a/gtsam_unstable/nonlinear/tests/testAdaptAutoDiff.cpp +++ b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp @@ -17,8 +17,8 @@ * @brief unit tests for Block Automatic Differentiation */ -#include -#include +#include +#include #include #include #include diff --git a/gtsam/nonlinear/tests/testExpression.cpp b/gtsam/nonlinear/tests/testExpression.cpp index dfa60e54e..1ea6236e8 100644 --- a/gtsam/nonlinear/tests/testExpression.cpp +++ b/gtsam/nonlinear/tests/testExpression.cpp @@ -34,8 +34,8 @@ using namespace gtsam; /* ************************************************************************* */ template -Point2 uncalibrate(const CAL& K, const Point2& p, - boost::optional Dcal, boost::optional Dp) { +Point2 uncalibrate(const CAL& K, const Point2& p, OptionalJacobian<2, 5> Dcal, + OptionalJacobian<2, 2> Dp) { return K.uncalibrate(p, Dcal, Dp); } @@ -75,13 +75,13 @@ TEST(Expression, Leaves) { /* ************************************************************************* */ // Unary(Leaf) namespace unary { -Point2 f0(const Point3& p, boost::optional H) { +Point2 f0(const Point3& p, OptionalJacobian<2,3> H) { return Point2(); } -LieScalar f1(const Point3& p, boost::optional&> H) { +LieScalar f1(const Point3& p, OptionalJacobian<1, 3> H) { return LieScalar(0.0); } -double f2(const Point3& p, boost::optional&> H) { +double f2(const Point3& p, OptionalJacobian<1, 3> H) { return 0.0; } Expression p(1); @@ -117,7 +117,7 @@ TEST(Expression, NullaryMethod) { // Check dims as map std::map map; norm.dims(map); - LONGS_EQUAL(1,map.size()); + LONGS_EQUAL(1, map.size()); // Get value and Jacobians std::vector H(1); @@ -133,9 +133,8 @@ TEST(Expression, NullaryMethod) { // Binary(Leaf,Leaf) namespace binary { // Create leaves -double doubleF(const Pose3& pose, const Point3& point, - boost::optional&> H1, - boost::optional&> H2) { +double doubleF(const Pose3& pose, // + const Point3& point, OptionalJacobian<1, 6> H1, OptionalJacobian<1, 3> H2) { return 0.0; } Expression x(1); @@ -244,8 +243,7 @@ TEST(Expression, compose3) { /* ************************************************************************* */ // Test with ternary function Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3, - boost::optional H1, boost::optional H2, - boost::optional H3) { + OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2, OptionalJacobian<3, 3> H3) { // return dummy derivatives (not correct, but that's ok for testing here) if (H1) *H1 = eye(3); diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index 09fe0f253..1b1d76d8a 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -12,6 +12,8 @@ /** * @file testValues.cpp * @author Richard Roberts + * @author Frank Dellaert + * @author Mike Bosse */ #include @@ -168,9 +170,9 @@ TEST(Values, basic_functions) Values values; const Values& values_c = values; values.insert(2, Vector3()); - values.insert(4, Vector3()); - values.insert(6, Vector3()); - values.insert(8, Vector3()); + values.insert(4, Vector(3)); + values.insert(6, Matrix23()); + values.insert(8, Matrix(2,3)); // find EXPECT_LONGS_EQUAL(4, values.find(4)->key); @@ -468,6 +470,15 @@ TEST(Values, Destructors) { LONGS_EQUAL(4+2, (long)TestValueData::DestructorCount); } +/* ************************************************************************* */ +TEST(Values, FixedSize) { + Values values; + Vector v(3); v << 5.0, 6.0, 7.0; + values.insertFixed(key1, v, 3); + Vector3 expected(5.0, 6.0, 7.0); + CHECK(assert_equal((Vector)expected, values.at(key1))); + CHECK_EXCEPTION(values.insertFixed(key1, v, 12),runtime_error); +} /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h index 1b630374c..9c5df7ea0 100644 --- a/gtsam/slam/BetweenFactor.h +++ b/gtsam/slam/BetweenFactor.h @@ -74,25 +74,26 @@ namespace gtsam { std::cout << s << "BetweenFactor(" << keyFormatter(this->key1()) << "," << keyFormatter(this->key2()) << ")\n"; - measured_.print(" measured: "); + traits::print()(measured_, " measured: "); this->noiseModel_->print(" noise model: "); } /** equals */ virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This *e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) && this->measured_.equals(e->measured_, tol); + return e != NULL && Base::equals(*e, tol) && traits::equals()(this->measured_, e->measured_, tol); } /** implement functions needed to derive from Factor */ /** vector of errors */ Vector evaluateError(const T& p1, const T& p2, - boost::optional H1 = boost::none, boost::optional H2 = + boost::optional H1 = boost::none,boost::optional H2 = boost::none) const { T hx = p1.between(p2, H1, H2); // h(x) + DefaultChart chart; // manifold equivalent of h(x)-z -> log(z,h(x)) - return measured_.localCoordinates(hx); + return chart.local(measured_, hx); } /** return the measured */ @@ -129,7 +130,9 @@ namespace gtsam { /** Syntactic sugar for constrained version */ BetweenConstraint(const VALUE& measured, Key key1, Key key2, double mu = 1000.0) : - BetweenFactor(key1, key2, measured, noiseModel::Constrained::All(VALUE::Dim(), fabs(mu))) {} + BetweenFactor(key1, key2, measured, + noiseModel::Constrained::All(DefaultChart::getDimension(measured), fabs(mu))) + {} private: diff --git a/gtsam/slam/PriorFactor.h b/gtsam/slam/PriorFactor.h index 888dcb76b..8fbbd4d88 100644 --- a/gtsam/slam/PriorFactor.h +++ b/gtsam/slam/PriorFactor.h @@ -67,23 +67,24 @@ namespace gtsam { /** print */ virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { std::cout << s << "PriorFactor on " << keyFormatter(this->key()) << "\n"; - prior_.print(" prior mean: "); + traits::print()(prior_, " prior mean: "); this->noiseModel_->print(" noise model: "); } /** equals */ virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This* e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) && this->prior_.equals(e->prior_, tol); + return e != NULL && Base::equals(*e, tol) && traits::equals()(prior_, e->prior_, tol); } /** implement functions needed to derive from Factor */ /** vector of errors */ Vector evaluateError(const T& p, boost::optional H = boost::none) const { - if (H) (*H) = eye(p.dim()); + DefaultChart chart; + if (H) (*H) = eye(chart.getDimension(p)); // manifold equivalent of h(x)-z -> log(z,h(x)) - return prior_.localCoordinates(p); + return chart.local(prior_,p); } const VALUE & prior() const { return prior_; } diff --git a/gtsam/slam/RangeFactor.h b/gtsam/slam/RangeFactor.h index af1c1a1bd..88c122f6e 100644 --- a/gtsam/slam/RangeFactor.h +++ b/gtsam/slam/RangeFactor.h @@ -16,99 +16,176 @@ #pragma once -#include -#include #include +#include +#include namespace gtsam { - /** - * Binary factor for a range measurement - * @addtogroup SLAM - */ - template - class RangeFactor: public NoiseModelFactor2 { - private: +/** + * Binary factor for a range measurement + * @addtogroup SLAM + */ +template +class RangeFactor: public NoiseModelFactor2 { +private: - double measured_; /** measurement */ - boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame + double measured_; /** measurement */ - typedef RangeFactor This; - typedef NoiseModelFactor2 Base; + typedef RangeFactor This; + typedef NoiseModelFactor2 Base; - typedef POSE Pose; - typedef POINT Point; + // Concept requirements for this factor + GTSAM_CONCEPT_RANGE_MEASUREMENT_TYPE(T1, T2) - // Concept requirements for this factor - GTSAM_CONCEPT_RANGE_MEASUREMENT_TYPE(POSE, POINT) +public: - public: + RangeFactor() { + } /* Default constructor */ - RangeFactor() {} /* Default constructor */ + RangeFactor(Key key1, Key key2, double measured, + const SharedNoiseModel& model) : + Base(model, key1, key2), measured_(measured) { + } - RangeFactor(Key poseKey, Key pointKey, double measured, - const SharedNoiseModel& model, boost::optional body_P_sensor = boost::none) : - Base(model, poseKey, pointKey), measured_(measured), body_P_sensor_(body_P_sensor) { + virtual ~RangeFactor() { + } + + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } + + /** h(x)-z */ + Vector evaluateError(const T1& t1, const T2& t2, boost::optional H1 = + boost::none, boost::optional H2 = boost::none) const { + double hx; + hx = t1.range(t2, H1, H2); + return (Vector(1) << hx - measured_).finished(); + } + + /** return the measured */ + double measured() const { + return measured_; + } + + /** equals specialized to this factor */ + virtual bool equals(const NonlinearFactor& expected, + double tol = 1e-9) const { + const This *e = dynamic_cast(&expected); + return e != NULL && Base::equals(*e, tol) + && fabs(this->measured_ - e->measured_) < tol; + } + + /** print contents */ + void print(const std::string& s = "", const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const { + std::cout << s << "RangeFactor, range = " << measured_ << std::endl; + Base::print("", keyFormatter); + } + +private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar + & boost::serialization::make_nvp("NoiseModelFactor2", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(measured_); + } +}; +// RangeFactor + +/** + * Binary factor for a range measurement, with a transform applied + * @addtogroup SLAM + */ +template +class RangeFactorWithTransform: public NoiseModelFactor2 { +private: + + double measured_; /** measurement */ + POSE body_P_sensor_; ///< The pose of the sensor in the body frame + + typedef RangeFactorWithTransform This; + typedef NoiseModelFactor2 Base; + + // Concept requirements for this factor + GTSAM_CONCEPT_RANGE_MEASUREMENT_TYPE(POSE, T2) + +public: + + RangeFactorWithTransform() { + } /* Default constructor */ + + RangeFactorWithTransform(Key key1, Key key2, double measured, + const SharedNoiseModel& model, const POSE& body_P_sensor) : + Base(model, key1, key2), measured_(measured), body_P_sensor_( + body_P_sensor) { + } + + virtual ~RangeFactorWithTransform() { + } + + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } + + /** h(x)-z */ + Vector evaluateError(const POSE& t1, const T2& t2, + boost::optional H1 = boost::none, boost::optional H2 = + boost::none) const { + double hx; + if (H1) { + Matrix H0; + hx = t1.compose(body_P_sensor_, H0).range(t2, H1, H2); + *H1 = *H1 * H0; + } else { + hx = t1.compose(body_P_sensor_).range(t2, H1, H2); } + return (Vector(1) << hx - measured_).finished(); + } - virtual ~RangeFactor() {} + /** return the measured */ + double measured() const { + return measured_; + } - /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { - return boost::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new This(*this))); } + /** equals specialized to this factor */ + virtual bool equals(const NonlinearFactor& expected, + double tol = 1e-9) const { + const This *e = dynamic_cast(&expected); + return e != NULL && Base::equals(*e, tol) + && fabs(this->measured_ - e->measured_) < tol + && body_P_sensor_.equals(e->body_P_sensor_); + } - /** h(x)-z */ - Vector evaluateError(const POSE& pose, const POINT& point, - boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { - double hx; - if(body_P_sensor_) { - if(H1) { - Matrix H0; - hx = pose.compose(*body_P_sensor_, H0).range(point, H1, H2); - *H1 = *H1 * H0; - } else { - hx = pose.compose(*body_P_sensor_).range(point, H1, H2); - } - } else { - hx = pose.range(point, H1, H2); - } - return (Vector(1) << hx - measured_).finished(); - } + /** print contents */ + void print(const std::string& s = "", const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const { + std::cout << s << "RangeFactor, range = " << measured_ << std::endl; + this->body_P_sensor_.print(" sensor pose in body frame: "); + Base::print("", keyFormatter); + } - /** return the measured */ - double measured() const { - return measured_; - } +private: - /** equals specialized to this factor */ - virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { - const This *e = dynamic_cast (&expected); - return e != NULL - && Base::equals(*e, tol) - && fabs(this->measured_ - e->measured_) < tol - && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_))); - } + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar + & boost::serialization::make_nvp("NoiseModelFactor2", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(measured_); + ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); + } +}; +// RangeFactor - /** print contents */ - void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { - std::cout << s << "RangeFactor, range = " << measured_ << std::endl; - if(this->body_P_sensor_) - this->body_P_sensor_->print(" sensor pose in body frame: "); - Base::print("", keyFormatter); - } - - private: - - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("NoiseModelFactor2", - boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(measured_); - ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); - } - }; // RangeFactor - -} // namespace gtsam +}// namespace gtsam diff --git a/gtsam/geometry/TriangulationFactor.h b/gtsam/slam/TriangulationFactor.h similarity index 98% rename from gtsam/geometry/TriangulationFactor.h rename to gtsam/slam/TriangulationFactor.h index fc8d546d3..b7f6a20dc 100644 --- a/gtsam/geometry/TriangulationFactor.h +++ b/gtsam/slam/TriangulationFactor.h @@ -10,14 +10,13 @@ * -------------------------------------------------------------------------- */ /** - * testTriangulationFactor.h + * triangulationFactor.h * @date March 2, 2014 * @author Frank Dellaert */ #include #include -#include #include #include diff --git a/gtsam/slam/tests/testBetweenFactor.cpp b/gtsam/slam/tests/testBetweenFactor.cpp index d0dbe7908..b22763099 100644 --- a/gtsam/slam/tests/testBetweenFactor.cpp +++ b/gtsam/slam/tests/testBetweenFactor.cpp @@ -44,6 +44,30 @@ TEST(BetweenFactor, Rot3) { EXPECT(assert_equal(numericalH2,actualH2, 1E-5)); } +/* ************************************************************************* */ +/* +// Constructor scalar +TEST(BetweenFactor, ConstructorScalar) { + SharedNoiseModel model; + double measured_value = 0.0; + BetweenFactor factor(1, 2, measured_value, model); +} + +// Constructor vector3 +TEST(BetweenFactor, ConstructorVector3) { + SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 1.0); + Vector3 measured_value(1, 2, 3); + BetweenFactor factor(1, 2, measured_value, model); +} + +// Constructor dynamic sized vector +TEST(BetweenFactor, ConstructorDynamicSizeVector) { + SharedNoiseModel model = noiseModel::Isotropic::Sigma(5, 1.0); + Vector measured_value(5); measured_value << 1, 2, 3, 4, 5; + BetweenFactor factor(1, 2, measured_value, model); +} +*/ + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/slam/tests/testPriorFactor.cpp b/gtsam/slam/tests/testPriorFactor.cpp index b3e54bedb..b26d633f5 100644 --- a/gtsam/slam/tests/testPriorFactor.cpp +++ b/gtsam/slam/tests/testPriorFactor.cpp @@ -5,8 +5,8 @@ * @date Nov 4, 2014 */ +#include #include -#include #include using namespace std; @@ -14,10 +14,23 @@ using namespace gtsam; /* ************************************************************************* */ -// Constructor -TEST(PriorFactor, Constructor) { +// Constructor scalar +TEST(PriorFactor, ConstructorScalar) { SharedNoiseModel model; - PriorFactor factor(1, LieScalar(1.0), model); + PriorFactor factor(1, 1.0, model); +} + +// Constructor vector3 +TEST(PriorFactor, ConstructorVector3) { + SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 1.0); + PriorFactor factor(1, Vector3(1,2,3), model); +} + +// Constructor dynamic sized vector +TEST(PriorFactor, ConstructorDynamicSizeVector) { + Vector v(5); v << 1, 2, 3, 4, 5; + SharedNoiseModel model = noiseModel::Isotropic::Sigma(5, 1.0); + PriorFactor factor(1, v, model); } /* ************************************************************************* */ diff --git a/gtsam/slam/tests/testRangeFactor.cpp b/gtsam/slam/tests/testRangeFactor.cpp index 6bffa3ce9..a8455d685 100644 --- a/gtsam/slam/tests/testRangeFactor.cpp +++ b/gtsam/slam/tests/testRangeFactor.cpp @@ -34,14 +34,30 @@ static SharedNoiseModel model(noiseModel::Unit::Create(1)); typedef RangeFactor RangeFactor2D; typedef RangeFactor RangeFactor3D; +typedef RangeFactorWithTransform RangeFactorWithTransform2D; +typedef RangeFactorWithTransform RangeFactorWithTransform3D; /* ************************************************************************* */ -Vector factorError2D(const Pose2& pose, const Point2& point, const RangeFactor2D& factor) { +Vector factorError2D(const Pose2& pose, const Point2& point, + const RangeFactor2D& factor) { return factor.evaluateError(pose, point); } /* ************************************************************************* */ -Vector factorError3D(const Pose3& pose, const Point3& point, const RangeFactor3D& factor) { +Vector factorError3D(const Pose3& pose, const Point3& point, + const RangeFactor3D& factor) { + return factor.evaluateError(pose, point); +} + +/* ************************************************************************* */ +Vector factorErrorWithTransform2D(const Pose2& pose, const Point2& point, + const RangeFactorWithTransform2D& factor) { + return factor.evaluateError(pose, point); +} + +/* ************************************************************************* */ +Vector factorErrorWithTransform3D(const Pose3& pose, const Point3& point, + const RangeFactorWithTransform3D& factor) { return factor.evaluateError(pose, point); } @@ -61,10 +77,13 @@ TEST( RangeFactor, ConstructorWithTransform) { Key pointKey(2); double measurement(10.0); Pose2 body_P_sensor_2D(0.25, -0.10, -M_PI_2); - Pose3 body_P_sensor_3D(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); + Pose3 body_P_sensor_3D(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), + Point3(0.25, -0.10, 1.0)); - RangeFactor2D factor2D(poseKey, pointKey, measurement, model, body_P_sensor_2D); - RangeFactor3D factor3D(poseKey, pointKey, measurement, model, body_P_sensor_3D); + RangeFactorWithTransform2D factor2D(poseKey, pointKey, measurement, model, + body_P_sensor_2D); + RangeFactorWithTransform3D factor3D(poseKey, pointKey, measurement, model, + body_P_sensor_3D); } /* ************************************************************************* */ @@ -90,14 +109,19 @@ TEST( RangeFactor, EqualsWithTransform ) { Key pointKey(2); double measurement(10.0); Pose2 body_P_sensor_2D(0.25, -0.10, -M_PI_2); - Pose3 body_P_sensor_3D(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); + Pose3 body_P_sensor_3D(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), + Point3(0.25, -0.10, 1.0)); - RangeFactor2D factor2D_1(poseKey, pointKey, measurement, model, body_P_sensor_2D); - RangeFactor2D factor2D_2(poseKey, pointKey, measurement, model, body_P_sensor_2D); + RangeFactorWithTransform2D factor2D_1(poseKey, pointKey, measurement, model, + body_P_sensor_2D); + RangeFactorWithTransform2D factor2D_2(poseKey, pointKey, measurement, model, + body_P_sensor_2D); CHECK(assert_equal(factor2D_1, factor2D_2)); - RangeFactor3D factor3D_1(poseKey, pointKey, measurement, model, body_P_sensor_3D); - RangeFactor3D factor3D_2(poseKey, pointKey, measurement, model, body_P_sensor_3D); + RangeFactorWithTransform3D factor3D_1(poseKey, pointKey, measurement, model, + body_P_sensor_3D); + RangeFactorWithTransform3D factor3D_2(poseKey, pointKey, measurement, model, + body_P_sensor_3D); CHECK(assert_equal(factor3D_1, factor3D_2)); } @@ -130,7 +154,8 @@ TEST( RangeFactor, Error2DWithTransform ) { Key pointKey(2); double measurement(10.0); Pose2 body_P_sensor(0.25, -0.10, -M_PI_2); - RangeFactor2D factor(poseKey, pointKey, measurement, model, body_P_sensor); + RangeFactorWithTransform2D factor(poseKey, pointKey, measurement, model, + body_P_sensor); // Set the linearization point Rot2 R(0.57); @@ -176,8 +201,10 @@ TEST( RangeFactor, Error3DWithTransform ) { Key poseKey(1); Key pointKey(2); double measurement(10.0); - Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); - RangeFactor3D factor(poseKey, pointKey, measurement, model, body_P_sensor); + Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), + Point3(0.25, -0.10, 1.0)); + RangeFactorWithTransform3D factor(poseKey, pointKey, measurement, model, + body_P_sensor); // Set the linearization point Rot3 R = Rot3::RzRyRx(0.2, -0.3, 1.75); @@ -213,8 +240,10 @@ TEST( RangeFactor, Jacobian2D ) { // Use numerical derivatives to calculate the Jacobians Matrix H1Expected, H2Expected; - H1Expected = numericalDerivative11(boost::bind(&factorError2D, _1, point, factor), pose); - H2Expected = numericalDerivative11(boost::bind(&factorError2D, pose, _1, factor), point); + H1Expected = numericalDerivative11( + boost::bind(&factorError2D, _1, point, factor), pose); + H2Expected = numericalDerivative11( + boost::bind(&factorError2D, pose, _1, factor), point); // Verify the Jacobians are correct CHECK(assert_equal(H1Expected, H1Actual, 1e-9)); @@ -228,7 +257,8 @@ TEST( RangeFactor, Jacobian2DWithTransform ) { Key pointKey(2); double measurement(10.0); Pose2 body_P_sensor(0.25, -0.10, -M_PI_2); - RangeFactor2D factor(poseKey, pointKey, measurement, model, body_P_sensor); + RangeFactorWithTransform2D factor(poseKey, pointKey, measurement, model, + body_P_sensor); // Set the linearization point Rot2 R(0.57); @@ -242,8 +272,10 @@ TEST( RangeFactor, Jacobian2DWithTransform ) { // Use numerical derivatives to calculate the Jacobians Matrix H1Expected, H2Expected; - H1Expected = numericalDerivative11(boost::bind(&factorError2D, _1, point, factor), pose); - H2Expected = numericalDerivative11(boost::bind(&factorError2D, pose, _1, factor), point); + H1Expected = numericalDerivative11( + boost::bind(&factorErrorWithTransform2D, _1, point, factor), pose); + H2Expected = numericalDerivative11( + boost::bind(&factorErrorWithTransform2D, pose, _1, factor), point); // Verify the Jacobians are correct CHECK(assert_equal(H1Expected, H1Actual, 1e-9)); @@ -268,8 +300,10 @@ TEST( RangeFactor, Jacobian3D ) { // Use numerical derivatives to calculate the Jacobians Matrix H1Expected, H2Expected; - H1Expected = numericalDerivative11(boost::bind(&factorError3D, _1, point, factor), pose); - H2Expected = numericalDerivative11(boost::bind(&factorError3D, pose, _1, factor), point); + H1Expected = numericalDerivative11( + boost::bind(&factorError3D, _1, point, factor), pose); + H2Expected = numericalDerivative11( + boost::bind(&factorError3D, pose, _1, factor), point); // Verify the Jacobians are correct CHECK(assert_equal(H1Expected, H1Actual, 1e-9)); @@ -282,8 +316,10 @@ TEST( RangeFactor, Jacobian3DWithTransform ) { Key poseKey(1); Key pointKey(2); double measurement(10.0); - Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); - RangeFactor3D factor(poseKey, pointKey, measurement, model, body_P_sensor); + Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), + Point3(0.25, -0.10, 1.0)); + RangeFactorWithTransform3D factor(poseKey, pointKey, measurement, model, + body_P_sensor); // Set the linearization point Rot3 R = Rot3::RzRyRx(0.2, -0.3, 1.75); @@ -297,8 +333,10 @@ TEST( RangeFactor, Jacobian3DWithTransform ) { // Use numerical derivatives to calculate the Jacobians Matrix H1Expected, H2Expected; - H1Expected = numericalDerivative11(boost::bind(&factorError3D, _1, point, factor), pose); - H2Expected = numericalDerivative11(boost::bind(&factorError3D, pose, _1, factor), point); + H1Expected = numericalDerivative11( + boost::bind(&factorErrorWithTransform3D, _1, point, factor), pose); + H2Expected = numericalDerivative11( + boost::bind(&factorErrorWithTransform3D, pose, _1, factor), point); // Verify the Jacobians are correct CHECK(assert_equal(H1Expected, H1Actual, 1e-9)); @@ -306,6 +344,9 @@ TEST( RangeFactor, Jacobian3DWithTransform ) { } /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ diff --git a/gtsam/slam/tests/testTriangulationFactor.cpp b/gtsam/slam/tests/testTriangulationFactor.cpp new file mode 100644 index 000000000..6b79171df --- /dev/null +++ b/gtsam/slam/tests/testTriangulationFactor.cpp @@ -0,0 +1,70 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/** + * testTriangulationFactor.cpp + * + * Created on: July 30th, 2013 + * Author: cbeall3 + */ + +#include +#include +#include +#include + +#include +#include + +using namespace std; +using namespace gtsam; +using namespace boost::assign; + +// Some common constants +static const boost::shared_ptr sharedCal = // + boost::make_shared(1500, 1200, 0, 640, 480); + +// Looking along X-axis, 1 meter above ground plane (x-y) +static const Rot3 upright = Rot3::ypr(-M_PI / 2, 0., -M_PI / 2); +static const Pose3 pose1 = Pose3(upright, gtsam::Point3(0, 0, 1)); +PinholeCamera camera1(pose1, *sharedCal); + +// landmark ~5 meters infront of camera +static const Point3 landmark(5, 0.5, 1.2); + +// 1. Project two landmarks into two cameras and triangulate +Point2 z1 = camera1.project(landmark); + +//****************************************************************************** +TEST( triangulation, TriangulationFactor ) { + // Create the factor with a measurement that is 3 pixels off in x + Key pointKey(1); + SharedNoiseModel model; + typedef TriangulationFactor<> Factor; + Factor factor(camera1, z1, model, pointKey); + + // Use the factor to calculate the Jacobians + Matrix HActual; + factor.evaluateError(landmark, HActual); + + Matrix HExpected = numericalDerivative11( + boost::bind(&Factor::evaluateError, &factor, _1, boost::none), landmark); + + // Verify the Jacobians are correct + CHECK(assert_equal(HExpected, HActual, 1e-3)); +} + +//****************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//****************************************************************************** diff --git a/gtsam_unstable/nonlinear/CallRecord.h b/gtsam_unstable/nonlinear/CallRecord.h index f1ac0b044..159a841e5 100644 --- a/gtsam_unstable/nonlinear/CallRecord.h +++ b/gtsam_unstable/nonlinear/CallRecord.h @@ -32,12 +32,6 @@ class JacobianMap; // forward declaration //----------------------------------------------------------------------------- -/** - * MaxVirtualStaticRows defines how many separate virtual reverseAD with specific - * static rows (1..MaxVirtualStaticRows) methods will be part of the CallRecord interface. - */ -const int MaxVirtualStaticRows = 4; - namespace internal { /** @@ -57,7 +51,8 @@ struct ConvertToVirtualFunctionSupportedMatrixType { template<> struct ConvertToVirtualFunctionSupportedMatrixType { template - static const Eigen::Matrix convert( + static const Eigen::Matrix convert( const Eigen::MatrixBase & x) { return x; } @@ -69,126 +64,132 @@ struct ConvertToVirtualFunctionSupportedMatrixType { } }; -/** - * Recursive definition of an interface having several purely - * virtual _reverseAD(const Eigen::Matrix &, JacobianMap&) - * with Rows in 1..MaxSupportedStaticRows - */ -template -struct ReverseADInterface: ReverseADInterface { - using ReverseADInterface::_reverseAD; - virtual void _reverseAD( - const Eigen::Matrix & dFdT, - JacobianMap& jacobians) const = 0; -}; - -template -struct ReverseADInterface<0, Cols> { - virtual void _reverseAD( - const Eigen::Matrix & dFdT, - JacobianMap& jacobians) const = 0; - virtual void _reverseAD(const Matrix & dFdT, - JacobianMap& jacobians) const = 0; -}; - -/** - * ReverseADImplementor is a utility class used by CallRecordImplementor to - * implementing the recursive ReverseADInterface interface. - */ -template -struct ReverseADImplementor: ReverseADImplementor { -private: - using ReverseADImplementor::_reverseAD; - virtual void _reverseAD( - const Eigen::Matrix & dFdT, - JacobianMap& jacobians) const { - static_cast(this)->reverseAD(dFdT, jacobians); - } - friend struct internal::ReverseADImplementor; -}; - -template -struct ReverseADImplementor : virtual internal::ReverseADInterface< - MaxVirtualStaticRows, Cols> { -private: - using internal::ReverseADInterface::_reverseAD; - const Derived & derived() const { - return static_cast(*this); - } - virtual void _reverseAD( - const Eigen::Matrix & dFdT, - JacobianMap& jacobians) const { - derived().reverseAD(dFdT, jacobians); - } - virtual void _reverseAD(const Matrix & dFdT, JacobianMap& jacobians) const { - derived().reverseAD(dFdT, jacobians); - } - friend struct internal::ReverseADImplementor; -}; - } // namespace internal /** - * The CallRecord class stores the Jacobians of applying a function - * with respect to each of its arguments. It also stores an execution trace - * (defined below) for each of its arguments. - * - * It is implemented in the function-style ExpressionNode's nested Record class below. + * The CallRecord is an abstract base class for the any class that stores + * the Jacobians of applying a function with respect to each of its arguments, + * as well as an execution trace for each of its arguments. */ template -struct CallRecord: virtual private internal::ReverseADInterface< - MaxVirtualStaticRows, Cols> { +struct CallRecord { + // Print entire record, recursively inline void print(const std::string& indent) const { _print(indent); } - inline void startReverseAD(JacobianMap& jacobians) const { - _startReverseAD(jacobians); + // Main entry point for the reverse AD process of a functional expression. + // Called *once* by the main AD entry point, ExecutionTrace::startReverseAD1 + // This function then calls ExecutionTrace::reverseAD for every argument + // which will in turn call the reverseAD method below. + // This non-virtual function _startReverseAD3, implemented in derived + inline void startReverseAD2(JacobianMap& jacobians) const { + _startReverseAD3(jacobians); } + // Dispatch the reverseAD2 calls issued by ExecutionTrace::reverseAD1 + // Here we convert to dynamic if the template - inline void reverseAD(const Eigen::MatrixBase & dFdT, + inline void reverseAD2(const Eigen::MatrixBase & dFdT, JacobianMap& jacobians) const { - _reverseAD( - internal::ConvertToVirtualFunctionSupportedMatrixType<(Derived::RowsAtCompileTime > MaxVirtualStaticRows)>::convert( - dFdT), jacobians); + _reverseAD3( + internal::ConvertToVirtualFunctionSupportedMatrixType< + (Derived::RowsAtCompileTime > 5)>::convert(dFdT), + jacobians); } - inline void reverseAD(const Matrix & dFdT, JacobianMap& jacobians) const { - _reverseAD(dFdT, jacobians); + // This overload supports matrices with both rows and columns dynamically sized. + // The template version above would be slower by introducing an extra conversion + // to statically sized columns. + inline void reverseAD2(const Matrix & dFdT, JacobianMap& jacobians) const { + _reverseAD3(dFdT, jacobians); } virtual ~CallRecord() { } private: + virtual void _print(const std::string& indent) const = 0; - virtual void _startReverseAD(JacobianMap& jacobians) const = 0; - using internal::ReverseADInterface::_reverseAD; + virtual void _startReverseAD3(JacobianMap& jacobians) const = 0; + + virtual void _reverseAD3(const Matrix & dFdT, + JacobianMap& jacobians) const = 0; + + virtual void _reverseAD3( + const Eigen::Matrix & dFdT, + JacobianMap& jacobians) const = 0; + + virtual void _reverseAD3(const Eigen::Matrix & dFdT, + JacobianMap& jacobians) const = 0; + virtual void _reverseAD3(const Eigen::Matrix & dFdT, + JacobianMap& jacobians) const = 0; + virtual void _reverseAD3(const Eigen::Matrix & dFdT, + JacobianMap& jacobians) const = 0; + virtual void _reverseAD3(const Eigen::Matrix & dFdT, + JacobianMap& jacobians) const = 0; + virtual void _reverseAD3(const Eigen::Matrix & dFdT, + JacobianMap& jacobians) const = 0; }; +/** + * CallRecordMaxVirtualStaticRows tells which separate virtual reverseAD with specific + * static rows (1..CallRecordMaxVirtualStaticRows) methods are part of the CallRecord + * interface. It is used to keep the testCallRecord unit test in sync. + */ +const int CallRecordMaxVirtualStaticRows = 5; + namespace internal { /** * The CallRecordImplementor implements the CallRecord interface for a Derived class by * delegating to its corresponding (templated) non-virtual methods. */ template -struct CallRecordImplementor: public CallRecord, - private ReverseADImplementor { +struct CallRecordImplementor: public CallRecord { private: + const Derived & derived() const { return static_cast(*this); } + virtual void _print(const std::string& indent) const { derived().print(indent); } - virtual void _startReverseAD(JacobianMap& jacobians) const { - derived().startReverseAD(jacobians); + + virtual void _startReverseAD3(JacobianMap& jacobians) const { + derived().startReverseAD4(jacobians); + } + + virtual void _reverseAD3(const Matrix & dFdT, JacobianMap& jacobians) const { + derived().reverseAD4(dFdT, jacobians); + } + + virtual void _reverseAD3( + const Eigen::Matrix & dFdT, + JacobianMap& jacobians) const { + derived().reverseAD4(dFdT, jacobians); + } + virtual void _reverseAD3(const Eigen::Matrix & dFdT, + JacobianMap& jacobians) const { + derived().reverseAD4(dFdT, jacobians); + } + virtual void _reverseAD3(const Eigen::Matrix & dFdT, + JacobianMap& jacobians) const { + derived().reverseAD4(dFdT, jacobians); + } + virtual void _reverseAD3(const Eigen::Matrix & dFdT, + JacobianMap& jacobians) const { + derived().reverseAD4(dFdT, jacobians); + } + virtual void _reverseAD3(const Eigen::Matrix & dFdT, + JacobianMap& jacobians) const { + derived().reverseAD4(dFdT, jacobians); + } + virtual void _reverseAD3(const Eigen::Matrix & dFdT, + JacobianMap& jacobians) const { + derived().reverseAD4(dFdT, jacobians); } - template friend struct ReverseADImplementor; }; } // namespace internal diff --git a/gtsam_unstable/nonlinear/ExpressionFactor.h b/gtsam_unstable/nonlinear/ExpressionFactor.h index 95c4d71a8..56e72a807 100644 --- a/gtsam_unstable/nonlinear/ExpressionFactor.h +++ b/gtsam_unstable/nonlinear/ExpressionFactor.h @@ -32,10 +32,11 @@ namespace gtsam { template class ExpressionFactor: public NoiseModelFactor { +protected: + T measurement_; ///< the measurement to be compared with the expression Expression expression_; ///< the expression that is AD enabled FastVector dims_; ///< dimensions of the Jacobian matrices - size_t augmentedCols_; ///< total number of columns + 1 (for RHS) static const int Dim = traits::dimension::value; @@ -55,15 +56,6 @@ public: // Get keys and dimensions for Jacobian matrices // An Expression is assumed unmutable, so we do this now boost::tie(keys_, dims_) = expression_.keysAndDims(); - - // Add sizes to know how much memory to allocate on stack in linearize - augmentedCols_ = std::accumulate(dims_.begin(), dims_.end(), 1); - -#ifdef DEBUG_ExpressionFactor - BOOST_FOREACH(size_t d, dims_) - std::cout << d << " "; - std::cout << " -> " << Dim << "x" << augmentedCols_ << std::endl; -#endif } /** diff --git a/gtsam_unstable/nonlinear/expressions.h b/gtsam_unstable/nonlinear/expressions.h index 2490100d6..18d0d5d8f 100644 --- a/gtsam_unstable/nonlinear/expressions.h +++ b/gtsam_unstable/nonlinear/expressions.h @@ -13,8 +13,7 @@ namespace gtsam { // Generics - -template +template Expression between(const Expression& t1, const Expression& t2) { return Expression(t1, &T::between, t2); } diff --git a/gtsam_unstable/nonlinear/tests/testBasisDecompositions.cpp b/gtsam_unstable/nonlinear/tests/testBasisDecompositions.cpp deleted file mode 100644 index f113a4f64..000000000 --- a/gtsam_unstable/nonlinear/tests/testBasisDecompositions.cpp +++ /dev/null @@ -1,119 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 testBasisDecompositions.cpp - * @date November 23, 2014 - * @author Frank Dellaert - * @brief unit tests for Basis Decompositions w Expressions - */ - -#include -#include -#include -#include -#include -#include - -#include - -#include -using boost::assign::list_of; - -using namespace std; -using namespace gtsam; - -noiseModel::Diagonal::shared_ptr model = noiseModel::Unit::Create(1); - -/// Fourier -template -class Fourier { - -public: - - typedef Eigen::Matrix Coefficients; - typedef Eigen::Matrix Jacobian; - -private: - - double x_; - Jacobian H_; - -public: - - /// Constructor - Fourier(double x) : - x_(x) { - H_(0, 0) = 1; - for (size_t i = 1; i < N; i += 2) { - H_(0, i) = cos(i * x); - H_(0, i + 1) = sin(i * x); - } - } - - /// Given coefficients c, predict value for x - double operator()(const Coefficients& c, boost::optional H) { - if (H) - (*H) = H_; - return H_ * c; - } -}; - -//****************************************************************************** -TEST(BasisDecompositions, Fourier) { - Fourier<3> fx(0); - Eigen::Matrix expectedH, actualH; - Vector3 c(1.5661, 1.2717, 1.2717); - expectedH = numericalDerivative11( - boost::bind(&Fourier<3>::operator(), fx, _1, boost::none), c); - EXPECT_DOUBLES_EQUAL(c[0]+c[1], fx(c,actualH), 1e-9); - EXPECT(assert_equal((Matrix)expectedH, actualH)); -} - -//****************************************************************************** -TEST(BasisDecompositions, FourierExpression) { - - // Create linear factor graph - GaussianFactorGraph g; - Key key(1); - Vector3_ c(key); - for (size_t i = 0; i < 16; i++) { - double x = i * M_PI / 8, y = exp(sin(x) + cos(x)); - - // Manual JacobianFactor - Matrix A(1, 3); - A << 1, cos(x), sin(x); - Vector b(1); - b << y; - JacobianFactor f1(key, A, b, model); - - // With ExpressionFactor - Expression expression(Fourier<3>(x), c); - ExpressionFactor f2(model, y, expression); - - g.add(f1); - } - - // Solve - VectorValues actual = g.optimize(); - - // Check - Vector3 expected(1.5661, 1.2717, 1.2717); - EXPECT(assert_equal((Vector) expected, actual.at(key),1e-4)); -} - -//****************************************************************************** -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -//****************************************************************************** - diff --git a/gtsam_unstable/nonlinear/tests/testCallRecord.cpp b/gtsam_unstable/nonlinear/tests/testCallRecord.cpp index a4561b349..1cc674901 100644 --- a/gtsam_unstable/nonlinear/tests/testCallRecord.cpp +++ b/gtsam_unstable/nonlinear/tests/testCallRecord.cpp @@ -33,7 +33,7 @@ static const int Cols = 3; int dynamicIfAboveMax(int i){ - if(i > MaxVirtualStaticRows){ + if(i > CallRecordMaxVirtualStaticRows){ return Eigen::Dynamic; } else return i; @@ -43,7 +43,6 @@ struct CallConfig { int compTimeCols; int runTimeRows; int runTimeCols; - CallConfig() {} CallConfig(int rows, int cols): compTimeRows(dynamicIfAboveMax(rows)), compTimeCols(cols), @@ -72,25 +71,26 @@ struct CallConfig { }; struct Record: public internal::CallRecordImplementor { + Record() : cc(0, 0) {} virtual ~Record() { } void print(const std::string& indent) const { } - void startReverseAD(JacobianMap& jacobians) const { + void startReverseAD4(JacobianMap& jacobians) const { } mutable CallConfig cc; private: template - void reverseAD(const SomeMatrix & dFdT, JacobianMap& jacobians) const { + void reverseAD4(const SomeMatrix & dFdT, JacobianMap& jacobians) const { cc.compTimeRows = SomeMatrix::RowsAtCompileTime; cc.compTimeCols = SomeMatrix::ColsAtCompileTime; cc.runTimeRows = dFdT.rows(); cc.runTimeCols = dFdT.cols(); } - template - friend struct internal::ReverseADImplementor; + template + friend struct internal::CallRecordImplementor; }; JacobianMap & NJM= *static_cast(NULL); @@ -102,56 +102,56 @@ TEST(CallRecord, virtualReverseAdDispatching) { Record record; { const int Rows = 1; - record.CallRecord::reverseAD(Eigen::Matrix(), NJM); + record.CallRecord::reverseAD2(Eigen::Matrix(), NJM); EXPECT((assert_equal(record.cc, CallConfig(Rows, Cols)))); - record.CallRecord::reverseAD(DynRowMat(Rows, Cols), NJM); + record.CallRecord::reverseAD2(DynRowMat(Rows, Cols), NJM); EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Cols, Rows, Cols)))); - record.CallRecord::reverseAD(Eigen::MatrixXd(Rows, Cols), NJM); + record.CallRecord::reverseAD2(Eigen::MatrixXd(Rows, Cols), NJM); EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Eigen::Dynamic, Rows, Cols)))); } { const int Rows = 2; - record.CallRecord::reverseAD(Eigen::Matrix(), NJM); + record.CallRecord::reverseAD2(Eigen::Matrix(), NJM); EXPECT((assert_equal(record.cc, CallConfig(Rows, Cols)))); - record.CallRecord::reverseAD(DynRowMat(Rows, Cols), NJM); + record.CallRecord::reverseAD2(DynRowMat(Rows, Cols), NJM); EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Cols, Rows, Cols)))); - record.CallRecord::reverseAD(Eigen::MatrixXd(Rows, Cols), NJM); + record.CallRecord::reverseAD2(Eigen::MatrixXd(Rows, Cols), NJM); EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Eigen::Dynamic, Rows, Cols)))); } { const int Rows = 3; - record.CallRecord::reverseAD(Eigen::Matrix(), NJM); + record.CallRecord::reverseAD2(Eigen::Matrix(), NJM); EXPECT((assert_equal(record.cc, CallConfig(Rows, Cols)))); - record.CallRecord::reverseAD(DynRowMat(Rows, Cols), NJM); + record.CallRecord::reverseAD2(DynRowMat(Rows, Cols), NJM); EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Cols, Rows, Cols)))); - record.CallRecord::reverseAD(Eigen::MatrixXd(Rows, Cols), NJM); + record.CallRecord::reverseAD2(Eigen::MatrixXd(Rows, Cols), NJM); EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Eigen::Dynamic, Rows, Cols)))); } { - const int Rows = MaxVirtualStaticRows; - record.CallRecord::reverseAD(Eigen::Matrix(), NJM); + const int Rows = 4; + record.CallRecord::reverseAD2(Eigen::Matrix(), NJM); EXPECT((assert_equal(record.cc, CallConfig(Rows, Cols)))); - record.CallRecord::reverseAD(DynRowMat(Rows, Cols), NJM); + record.CallRecord::reverseAD2(DynRowMat(Rows, Cols), NJM); EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Cols, Rows, Cols)))); - record.CallRecord::reverseAD(Eigen::MatrixXd(Rows, Cols), NJM); + record.CallRecord::reverseAD2(Eigen::MatrixXd(Rows, Cols), NJM); EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Eigen::Dynamic, Rows, Cols)))); } { - const int Rows = MaxVirtualStaticRows + 1; - record.CallRecord::reverseAD(Eigen::Matrix(), NJM); + const int Rows = 5; + record.CallRecord::reverseAD2(Eigen::Matrix(), NJM); EXPECT((assert_equal(record.cc, CallConfig(Rows, Cols)))); - record.CallRecord::reverseAD(DynRowMat(Rows, Cols), NJM); + record.CallRecord::reverseAD2(DynRowMat(Rows, Cols), NJM); EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Cols, Rows, Cols)))); - record.CallRecord::reverseAD(Eigen::MatrixXd(Rows, Cols), NJM); + record.CallRecord::reverseAD2(Eigen::MatrixXd(Rows, Cols), NJM); EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Eigen::Dynamic, Rows, Cols)))); } { - const int Rows = MaxVirtualStaticRows + 2; - record.CallRecord::reverseAD(Eigen::Matrix(), NJM); + const int Rows = 6; + record.CallRecord::reverseAD2(Eigen::Matrix(), NJM); EXPECT((assert_equal(record.cc, CallConfig(Rows, Cols)))); - record.CallRecord::reverseAD(DynRowMat(Rows, Cols), NJM); + record.CallRecord::reverseAD2(DynRowMat(Rows, Cols), NJM); EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Cols, Rows, Cols)))); - record.CallRecord::reverseAD(Eigen::MatrixXd(Rows, Cols), NJM); + record.CallRecord::reverseAD2(Eigen::MatrixXd(Rows, Cols), NJM); EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Eigen::Dynamic, Rows, Cols)))); } } diff --git a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp index d146ea945..5758509eb 100644 --- a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp @@ -76,10 +76,13 @@ TEST(ExpressionFactor, Model) { // Concise version ExpressionFactor f(model, Point2(0, 0), p); + + // Check values and derivatives EXPECT_DOUBLES_EQUAL(old.error(values), f.error(values), 1e-9); EXPECT_LONGS_EQUAL(2, f.dim()); boost::shared_ptr gf2 = f.linearize(values); EXPECT( assert_equal(*old.linearize(values), *gf2, 1e-9)); + EXPECT_CORRECT_FACTOR_JACOBIANS(f, values, 1e-5, 1e-5); // another way } /* ************************************************************************* */ @@ -124,9 +127,41 @@ TEST(ExpressionFactor, Unary) { EXPECT( assert_equal(expected, *jf, 1e-9)); } +/* ************************************************************************* */ +// Unary(Leaf)) and Unary(Unary(Leaf))) +// wide version (not handled in fixed-size pipeline) +typedef Eigen::Matrix Matrix93; +Vector9 wide(const Point3& p, OptionalJacobian<9,3> H) { + Vector9 v; + v << p.vector(), p.vector(), p.vector(); + if (H) *H << eye(3), eye(3), eye(3); + return v; +} +typedef Eigen::Matrix Matrix9; +Vector9 id9(const Vector9& v, OptionalJacobian<9,9> H) { + if (H) *H = Matrix9::Identity(); + return v; +} +TEST(ExpressionFactor, Wide) { + // Create some values + Values values; + values.insert(2, Point3(0, 0, 1)); + Point3_ point(2); + Vector9 measured; + Expression expression(wide,point); + SharedNoiseModel model = noiseModel::Unit::Create(9); + + ExpressionFactor f1(model, measured, expression); + EXPECT_CORRECT_FACTOR_JACOBIANS(f1, values, 1e-5, 1e-9); + + Expression expression2(id9,expression); + ExpressionFactor f2(model, measured, expression2); + EXPECT_CORRECT_FACTOR_JACOBIANS(f2, values, 1e-5, 1e-9); +} + /* ************************************************************************* */ static Point2 myUncal(const Cal3_S2& K, const Point2& p, - boost::optional Dcal, boost::optional Dp) { + OptionalJacobian<2,5> Dcal, OptionalJacobian<2,2> Dp) { return K.uncalibrate(p, Dcal, Dp); } @@ -392,8 +427,7 @@ TEST(ExpressionFactor, compose3) { /* ************************************************************************* */ // Test compose with three arguments Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3, - boost::optional H1, boost::optional H2, - boost::optional H3) { + OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2, OptionalJacobian<3, 3> H3) { // return dummy derivatives (not correct, but that's ok for testing here) if (H1) *H1 = eye(3); diff --git a/gtsam_unstable/nonlinear/tests/testExpressionMeta.cpp b/gtsam_unstable/nonlinear/tests/testExpressionMeta.cpp index d10e31002..45e294c3d 100644 --- a/gtsam_unstable/nonlinear/tests/testExpressionMeta.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpressionMeta.cpp @@ -179,29 +179,29 @@ TEST(ExpressionFactor, InvokeDerivatives) { // Let's assign it it to a boost function object // cast is needed because Pose3::transform_to is overloaded - typedef boost::function F; - F f = static_cast(&Pose3::transform_to); - - // Create arguments -Pose3 pose; - Point3 point; - typedef boost::fusion::vector Arguments; - Arguments args = boost::fusion::make_vector(pose, point); - - // Create fused function (takes fusion vector) and call it - boost::fusion::fused g(f); - Point3 actual = g(args); - CHECK(assert_equal(point,actual)); - - // We can *immediately* do this using invoke - Point3 actual2 = boost::fusion::invoke(f, args); - CHECK(assert_equal(point,actual2)); +// typedef boost::function F; +// F f = static_cast(&Pose3::transform_to); +// +// // Create arguments +// Pose3 pose; +// Point3 point; +// typedef boost::fusion::vector Arguments; +// Arguments args = boost::fusion::make_vector(pose, point); +// +// // Create fused function (takes fusion vector) and call it +// boost::fusion::fused g(f); +// Point3 actual = g(args); +// CHECK(assert_equal(point,actual)); +// +// // We can *immediately* do this using invoke +// Point3 actual2 = boost::fusion::invoke(f, args); +// CHECK(assert_equal(point,actual2)); // Now, let's create the optional Jacobian arguments typedef Point3 T; typedef boost::mpl::vector TYPES; - typedef boost::mpl::transform >::type Optionals; + typedef boost::mpl::transform >::type Optionals; // Unfortunately this is moot: we need a pointer to a function with the // optional derivatives; I don't see a way of calling a function that we @@ -215,8 +215,8 @@ struct proxy { return pose.transform_to(point); } Point3 operator()(const Pose3& pose, const Point3& point, - boost::optional Dpose, - boost::optional Dpoint) const { + OptionalJacobian<3,6> Dpose, + OptionalJacobian<3,3> Dpoint) const { return pose.transform_to(point, Dpose, Dpoint); } }; diff --git a/gtsam_unstable/slam/AHRS.cpp b/gtsam_unstable/slam/AHRS.cpp index 2a756b5af..2a27730f4 100644 --- a/gtsam_unstable/slam/AHRS.cpp +++ b/gtsam_unstable/slam/AHRS.cpp @@ -12,17 +12,14 @@ using namespace std; namespace gtsam { -Matrix cov(const Matrix& m) { +/* ************************************************************************* */ +Matrix3 AHRS::Cov(const Vector3s& m) { const double num_observations = m.cols(); - const Vector mean = m.rowwise().sum() / num_observations; - Matrix D = m.colwise() - mean; - Matrix DDt = D * trans(D); - return DDt / (num_observations - 1); + const Vector3 mean = m.rowwise().sum() / num_observations; + Vector3s D = m.colwise() - mean; + return D * trans(D) / (num_observations - 1); } -Matrix I3 = eye(3); -Matrix Z3 = zeros(3, 3); - /* ************************************************************************* */ AHRS::AHRS(const Matrix& stationaryU, const Matrix& stationaryF, double g_e, bool flat) : @@ -34,11 +31,11 @@ AHRS::AHRS(const Matrix& stationaryU, const Matrix& stationaryF, double g_e, size_t T = stationaryU.cols(); // estimate standard deviation on gyroscope readings - Pg_ = cov(stationaryU); - Vector sigmas_v_g = esqrt(Pg_.diagonal() * T); + Pg_ = Cov(stationaryU); + Vector3 sigmas_v_g = esqrt(Pg_.diagonal() * T); // estimate standard deviation on accelerometer readings - Pa_ = cov(stationaryF); + Pa_ = Cov(stationaryF); // Quantities needed for integration @@ -46,15 +43,13 @@ AHRS::AHRS(const Matrix& stationaryU, const Matrix& stationaryF, double g_e, double tau_g = 730; // correlation time for gyroscope double tau_a = 730; // correlation time for accelerometer - F_g_ = -I3 / tau_g; - F_a_ = -I3 / tau_a; + F_g_ = -I_3x3 / tau_g; + F_a_ = -I_3x3 / tau_a; Vector3 var_omega_w = 0 * ones(3); // TODO Vector3 var_omega_g = (0.0034 * 0.0034) * ones(3); Vector3 var_omega_a = (0.034 * 0.034) * ones(3); - Vector3 sigmas_v_g_sq = emul(sigmas_v_g, sigmas_v_g); - Vector vars(12); - vars << var_omega_w, var_omega_g, sigmas_v_g_sq, var_omega_a; - var_w_ = diag(vars); + Vector3 sigmas_v_g_sq = sigmas_v_g.cwiseProduct(sigmas_v_g); + var_w_ << var_omega_w, var_omega_g, sigmas_v_g_sq, var_omega_a; // Quantities needed for aiding sigmas_v_a_ = esqrt(T * Pa_.diagonal()); @@ -95,15 +90,15 @@ std::pair AHRS::initialize(double g_e) Matrix P_plus_k2 = Matrix(9, 9); P_plus_k2.block<3,3>(0, 0) = P11; - P_plus_k2.block<3,3>(0, 3) = Z3; + P_plus_k2.block<3,3>(0, 3) = Z_3x3; P_plus_k2.block<3,3>(0, 6) = P12; - P_plus_k2.block<3,3>(3, 0) = Z3; + P_plus_k2.block<3,3>(3, 0) = Z_3x3; P_plus_k2.block<3,3>(3, 3) = Pg_; - P_plus_k2.block<3,3>(3, 6) = Z3; + P_plus_k2.block<3,3>(3, 6) = Z_3x3; P_plus_k2.block<3,3>(6, 0) = trans(P12); - P_plus_k2.block<3,3>(6, 3) = Z3; + P_plus_k2.block<3,3>(6, 3) = Z_3x3; P_plus_k2.block<3,3>(6, 6) = Pa; Vector dx = zero(9); @@ -123,26 +118,26 @@ std::pair AHRS::integrate( // FIXME: //if nargout>1 Matrix bRn = mech.bRn().matrix(); - Matrix I3 = eye(3); - Matrix Z3 = zeros(3, 3); - Matrix F_k = zeros(9, 9); + Matrix9 F_k; F_k.setZero(); F_k.block<3,3>(0, 3) = -bRn; F_k.block<3,3>(3, 3) = F_g_; F_k.block<3,3>(6, 6) = F_a_; - Matrix G_k = zeros(9, 12); + typedef Eigen::Matrix Matrix9_12; + Matrix9_12 G_k; G_k.setZero(); G_k.block<3,3>(0, 0) = -bRn; G_k.block<3,3>(0, 6) = -bRn; - G_k.block<3,3>(3, 3) = I3; - G_k.block<3,3>(6, 9) = I3; + G_k.block<3,3>(3, 3) = I_3x3; + G_k.block<3,3>(6, 9) = I_3x3; - Matrix Q_k = G_k * var_w_ * trans(G_k); - Matrix Psi_k = eye(9) + dt * F_k; // +dt*dt*F_k*F_k/2; // transition matrix + Matrix9 Q_k = G_k * var_w_.asDiagonal() * G_k.transpose(); + Matrix9 Psi_k = I_9x9 + dt * F_k; // +dt*dt*F_k*F_k/2; // transition matrix // This implements update in section 10.6 - Matrix B = zeros(9, 9); - Vector u2 = zero(9); + Matrix9 B; B.setZero(); + Vector9 u2; u2.setZero(); + // TODO predictQ should be templated to also take fixed size matrices. KalmanFilter::State newState = KF_.predictQ(state, Psi_k,B,u2,dt*Q_k); return make_pair(newMech, newState); } @@ -175,7 +170,7 @@ std::pair AHRS::aid( if (Farrell) { // calculate residual gravity measurement z = n_g_ - trans(bRn) * measured_b_g; - H = collect(3, &n_g_cross_, &Z3, &bRn); + H = collect(3, &n_g_cross_, &Z_3x3, &bRn); R = trans(bRn) * diag(emul(sigmas_v_a_, sigmas_v_a_)) * bRn; } else { // my measurement prediction (in body frame): @@ -189,7 +184,7 @@ std::pair AHRS::aid( z = bRn * n_g_ - measured_b_g; // Now the Jacobian H Matrix b_g = bRn * n_g_cross_; - H = collect(3, &b_g, &Z3, &I3); + H = collect(3, &b_g, &Z_3x3, &I_3x3); // And the measurement noise, TODO: should be created once where sigmas_v_a is given R = diag(emul(sigmas_v_a_, sigmas_v_a_)); } @@ -219,7 +214,7 @@ std::pair AHRS::aidGeneral( Vector z = f - increment * f_previous; //Vector z = increment * f_previous - f; Matrix b_g = skewSymmetric(increment* f_previous); - Matrix H = collect(3, &b_g, &I3, &Z3); + Matrix H = collect(3, &b_g, &I_3x3, &Z_3x3); // Matrix R = diag(emul(sigmas_v_a_, sigmas_v_a_)); // Matrix R = diag(Vector3(1.0, 0.2, 1.0)); // good for L_twice Matrix R = diag(Vector3(0.01, 0.0001, 0.01)); @@ -240,16 +235,16 @@ std::pair AHRS::aidGeneral( /* ************************************************************************* */ void AHRS::print(const std::string& s) const { mech0_.print(s + ".mech0_"); - gtsam::print(F_g_, s + ".F_g"); - gtsam::print(F_a_, s + ".F_a"); - gtsam::print(var_w_, s + ".var_w"); + gtsam::print((Matrix)F_g_, s + ".F_g"); + gtsam::print((Matrix)F_a_, s + ".F_a"); + gtsam::print((Vector)var_w_, s + ".var_w"); - gtsam::print(sigmas_v_a_, s + ".sigmas_v_a"); - gtsam::print(n_g_, s + ".n_g"); - gtsam::print(n_g_cross_, s + ".n_g_cross"); + gtsam::print((Vector)sigmas_v_a_, s + ".sigmas_v_a"); + gtsam::print((Vector)n_g_, s + ".n_g"); + gtsam::print((Matrix)n_g_cross_, s + ".n_g_cross"); - gtsam::print(Pg_, s + ".P_g"); - gtsam::print(Pa_, s + ".P_a"); + gtsam::print((Matrix)Pg_, s + ".P_g"); + gtsam::print((Matrix)Pa_, s + ".P_a"); } diff --git a/gtsam_unstable/slam/AHRS.h b/gtsam_unstable/slam/AHRS.h index 81d74a9f5..e15e6e0f7 100644 --- a/gtsam_unstable/slam/AHRS.h +++ b/gtsam_unstable/slam/AHRS.h @@ -14,8 +14,6 @@ namespace gtsam { -GTSAM_UNSTABLE_EXPORT Matrix cov(const Matrix& m); - class GTSAM_UNSTABLE_EXPORT AHRS { private: @@ -25,18 +23,24 @@ private: KalmanFilter KF_; ///< initial Kalman filter // Quantities needed for integration - Matrix F_g_; ///< gyro bias dynamics - Matrix F_a_; ///< acc bias dynamics - Matrix var_w_; ///< dynamic noise variances + Matrix3 F_g_; ///< gyro bias dynamics + Matrix3 F_a_; ///< acc bias dynamics + + typedef Eigen::Matrix Variances; + Variances var_w_; ///< dynamic noise variances // Quantities needed for aiding - Vector sigmas_v_a_; ///< measurement sigma - Vector n_g_; ///< gravity in nav frame - Matrix n_g_cross_; ///< and its skew-symmetric matrix + Vector3 sigmas_v_a_; ///< measurement sigma + Vector3 n_g_; ///< gravity in nav frame + Matrix3 n_g_cross_; ///< and its skew-symmetric matrix - Matrix Pg_, Pa_; + Matrix3 Pg_, Pa_; public: + + typedef Eigen::Matrix Vector3s; + static Matrix3 Cov(const Vector3s& m); + /** * AHRS constructor * @param stationaryU initial interval of gyro measurements, 3xn matrix diff --git a/gtsam_unstable/slam/expressions.h b/gtsam_unstable/slam/expressions.h index 009be46a1..7badc9dd7 100644 --- a/gtsam_unstable/slam/expressions.h +++ b/gtsam_unstable/slam/expressions.h @@ -20,11 +20,7 @@ typedef Expression Rot2_; typedef Expression Pose2_; Point2_ transform_to(const Pose2_& x, const Point2_& p) { - Point2(Pose2::*transform)(const Point2& p, - boost::optional H1, - boost::optional H2) const = &Pose2::transform_to; - - return Point2_(x, transform, p); + return Point2_(x, &Pose2::transform_to, p); } // 3D Geometry @@ -34,12 +30,7 @@ typedef Expression Rot3_; typedef Expression Pose3_; Point3_ transform_to(const Pose3_& x, const Point3_& p) { - - Point3(Pose3::*transform)(const Point3& p, - boost::optional Dpose, - boost::optional Dpoint) const = &Pose3::transform_to; - - return Point3_(x, transform, p); + return Point3_(x, &Pose3::transform_to, p); } // Projection @@ -51,8 +42,7 @@ Point2_ project(const Point3_& p_cam) { } Point2 project6(const Pose3& x, const Point3& p, const Cal3_S2& K, - boost::optional Dpose, boost::optional Dpoint, - boost::optional Dcal) { + OptionalJacobian<2, 6> Dpose, OptionalJacobian<2, 3> Dpoint, OptionalJacobian<2, 5> Dcal) { return PinholeCamera(x, K).project(p, Dpose, Dpoint, Dcal); } diff --git a/gtsam_unstable/slam/tests/testAHRS.cpp b/gtsam_unstable/slam/tests/testAHRS.cpp index 44f516ae9..d0980f452 100644 --- a/gtsam_unstable/slam/tests/testAHRS.cpp +++ b/gtsam_unstable/slam/tests/testAHRS.cpp @@ -30,7 +30,7 @@ TEST (AHRS, cov) { 9.0, 4.0, 7.0, 6.0, 3.0, 2.0).finished(); - Matrix actual = cov(trans(A)); + Matrix actual = AHRS::Cov(trans(A)); Matrix expected = (Matrix(3, 3) << 10.9167, 2.3333, 5.0000, 2.3333, 4.6667, -2.6667, @@ -42,7 +42,7 @@ TEST (AHRS, cov) { /* ************************************************************************* */ TEST (AHRS, covU) { - Matrix actual = cov(10000*stationaryU); + Matrix actual = AHRS::Cov(10000*stationaryU); Matrix expected = (Matrix(3, 3) << 33.3333333, -1.66666667, 53.3333333, -1.66666667, 0.333333333, -5.16666667, @@ -54,7 +54,7 @@ TEST (AHRS, covU) { /* ************************************************************************* */ TEST (AHRS, covF) { - Matrix actual = cov(100*stationaryF); + Matrix actual = AHRS::Cov(100*stationaryF); Matrix expected = (Matrix(3, 3) << 63.3808333, -0.432166667, -48.1706667, -0.432166667, 8.31053333, -16.6792667, diff --git a/gtsam_unstable/timing/timeAdaptAutoDiff.cpp b/gtsam_unstable/timing/timeAdaptAutoDiff.cpp index 32bce9ba5..437067697 100644 --- a/gtsam_unstable/timing/timeAdaptAutoDiff.cpp +++ b/gtsam_unstable/timing/timeAdaptAutoDiff.cpp @@ -17,8 +17,8 @@ */ #include "timeLinearize.h" -#include -#include +#include +#include #include #include #include diff --git a/matlab/gtsam_tests/.gitignore b/matlab/gtsam_tests/.gitignore new file mode 100644 index 000000000..6d725d9bc --- /dev/null +++ b/matlab/gtsam_tests/.gitignore @@ -0,0 +1 @@ +*.m~ diff --git a/matlab/gtsam_tests/testValues.m b/matlab/gtsam_tests/testValues.m new file mode 100644 index 000000000..fe2cd30fe --- /dev/null +++ b/matlab/gtsam_tests/testValues.m @@ -0,0 +1,40 @@ +% test wrapping of Values +import gtsam.*; + +values = Values; +E = EssentialMatrix(Rot3,Unit3); +tol = 1e-9; + +values.insert(0, Point2); +values.insert(1, Point3); +values.insert(2, Rot2); +values.insert(3, Pose2); +values.insert(4, Rot3); +values.insert(5, Pose3); +values.insert(6, Cal3_S2); +values.insert(7, Cal3DS2); +values.insert(8, Cal3Bundler); +values.insert(9, E); +values.insert(10, imuBias.ConstantBias); + +% special cases for Vector and Matrix: +values.insert(11, [1;2;3]); +values.insert(12, [1 2;3 4]); + +EXPECT('at',values.atPoint2(0).equals(Point2,tol)); +EXPECT('at',values.atPoint3(1).equals(Point3,tol)); +EXPECT('at',values.atRot2(2).equals(Rot2,tol)); +EXPECT('at',values.atPose2(3).equals(Pose2,tol)); +EXPECT('at',values.atRot3(4).equals(Rot3,tol)); +EXPECT('at',values.atPose3(5).equals(Pose3,tol)); +EXPECT('at',values.atCal3_S2(6).equals(Cal3_S2,tol)); +EXPECT('at',values.atCal3DS2(7).equals(Cal3DS2,tol)); +EXPECT('at',values.atCal3Bundler(8).equals(Cal3Bundler,tol)); +EXPECT('at',values.atEssentialMatrix(9).equals(E,tol)); +EXPECT('at',values.atConstantBias(10).equals(imuBias.ConstantBias,tol)); + +% special cases for Vector and Matrix: +actualVector = values.atVector(11); +EQUALITY('at',[1;2;3],actualVector,tol); +actualMatrix = values.atMatrix(12); +EQUALITY('at',[1 2;3 4],actualMatrix,tol); diff --git a/matlab/gtsam_tests/test_gtsam.m b/matlab/gtsam_tests/test_gtsam.m index e3705948d..e08019610 100644 --- a/matlab/gtsam_tests/test_gtsam.m +++ b/matlab/gtsam_tests/test_gtsam.m @@ -1,5 +1,8 @@ % Test runner script - runs each test +display 'Starting: testValues' +testValues + display 'Starting: testJacobianFactor' testJacobianFactor diff --git a/wrap/Argument.cpp b/wrap/Argument.cpp index 4989afb0d..1f57917d8 100644 --- a/wrap/Argument.cpp +++ b/wrap/Argument.cpp @@ -20,6 +20,7 @@ #include #include +#include #include #include @@ -31,12 +32,13 @@ using namespace wrap; /* ************************************************************************* */ Argument Argument::expandTemplate(const TemplateSubstitution& ts) const { Argument instArg = *this; - instArg.type = ts(type); + instArg.type = ts.tryToSubstitite(type); return instArg; } /* ************************************************************************* */ -ArgumentList ArgumentList::expandTemplate(const TemplateSubstitution& ts) const { +ArgumentList ArgumentList::expandTemplate( + const TemplateSubstitution& ts) const { ArgumentList instArgList; BOOST_FOREACH(const Argument& arg, *this) { Argument instArg = arg.expandTemplate(ts); @@ -48,25 +50,25 @@ ArgumentList ArgumentList::expandTemplate(const TemplateSubstitution& ts) const /* ************************************************************************* */ string Argument::matlabClass(const string& delim) const { string result; - BOOST_FOREACH(const string& ns, type.namespaces) + BOOST_FOREACH(const string& ns, type.namespaces()) result += ns + delim; - if (type.name == "string" || type.name == "unsigned char" - || type.name == "char") + if (type.name() == "string" || type.name() == "unsigned char" + || type.name() == "char") return result + "char"; - if (type.name == "Vector" || type.name == "Matrix") + if (type.name() == "Vector" || type.name() == "Matrix") return result + "double"; - if (type.name == "int" || type.name == "size_t") + if (type.name() == "int" || type.name() == "size_t") return result + "numeric"; - if (type.name == "bool") + if (type.name() == "bool") return result + "logical"; - return result + type.name; + return result + type.name(); } /* ************************************************************************* */ bool Argument::isScalar() const { - return (type.name == "bool" || type.name == "char" - || type.name == "unsigned char" || type.name == "int" - || type.name == "size_t" || type.name == "double"); + return (type.name() == "bool" || type.name() == "char" + || type.name() == "unsigned char" || type.name() == "int" + || type.name() == "size_t" || type.name() == "double"); } /* ************************************************************************* */ @@ -97,6 +99,13 @@ void Argument::matlab_unwrap(FileWriter& file, const string& matlabName) const { file.oss << ");" << endl; } +/* ************************************************************************* */ +void Argument::proxy_check(FileWriter& proxyFile, const string& s) const { + proxyFile.oss << "isa(" << s << ",'" << matlabClass(".") << "')"; + if (type.name() == "Vector") + proxyFile.oss << " && size(" << s << ",2)==1"; +} + /* ************************************************************************* */ string ArgumentList::types() const { string str; @@ -104,7 +113,7 @@ string ArgumentList::types() const { BOOST_FOREACH(Argument arg, *this) { if (!first) str += ","; - str += arg.type.name; + str += arg.type.name(); first = false; } return str; @@ -116,14 +125,14 @@ string ArgumentList::signature() const { bool cap = false; BOOST_FOREACH(Argument arg, *this) { - BOOST_FOREACH(char ch, arg.type.name) + BOOST_FOREACH(char ch, arg.type.name()) if (isupper(ch)) { sig += ch; //If there is a capital letter, we don't want to read it below cap = true; } if (!cap) - sig += arg.type.name[0]; + sig += arg.type.name()[0]; //Reset to default cap = false; } @@ -170,25 +179,14 @@ void ArgumentList::emit_prototype(FileWriter& file, const string& name) const { BOOST_FOREACH(Argument arg, *this) { if (!first) file.oss << ", "; - file.oss << arg.type.name << " " << arg.name; + file.oss << arg.type.name() << " " << arg.name; first = false; } file.oss << ")"; } + /* ************************************************************************* */ -void ArgumentList::emit_call(FileWriter& proxyFile, - const ReturnValue& returnVal, const string& wrapperName, int id, - bool staticMethod) const { - returnVal.emit_matlab(proxyFile); - proxyFile.oss << wrapperName << "(" << id; - if (!staticMethod) - proxyFile.oss << ", this"; - proxyFile.oss << ", varargin{:});\n"; -} -/* ************************************************************************* */ -void ArgumentList::emit_conditional_call(FileWriter& proxyFile, - const ReturnValue& returnVal, const string& wrapperName, int id, - bool staticMethod) const { +void ArgumentList::proxy_check(FileWriter& proxyFile) const { // Check nr of arguments proxyFile.oss << "if length(varargin) == " << size(); if (size() > 0) @@ -198,15 +196,12 @@ void ArgumentList::emit_conditional_call(FileWriter& proxyFile, for (size_t i = 0; i < size(); i++) { if (!first) proxyFile.oss << " && "; - proxyFile.oss << "isa(varargin{" << i + 1 << "},'" - << (*this)[i].matlabClass(".") << "')"; + string s = "varargin{" + boost::lexical_cast(i + 1) + "}"; + (*this)[i].proxy_check(proxyFile, s); first = false; } proxyFile.oss << "\n"; - - // output call to C++ wrapper - proxyFile.oss << " "; - emit_call(proxyFile, returnVal, wrapperName, id, staticMethod); } + /* ************************************************************************* */ diff --git a/wrap/Argument.h b/wrap/Argument.h index 3d8d7288f..fd7e82061 100644 --- a/wrap/Argument.h +++ b/wrap/Argument.h @@ -28,13 +28,23 @@ namespace wrap { /// Argument class struct Argument { Qualified type; - bool is_const, is_ref, is_ptr; std::string name; + bool is_const, is_ref, is_ptr; Argument() : is_const(false), is_ref(false), is_ptr(false) { } + Argument(const Qualified& t, const std::string& n) : + type(t), name(n), is_const(false), is_ref(false), is_ptr(false) { + } + + bool operator==(const Argument& other) const { + return type == other.type && name == other.name + && is_const == other.is_const && is_ref == other.is_ref + && is_ptr == other.is_ptr; + } + Argument expandTemplate(const TemplateSubstitution& ts) const; /// return MATLAB class for use in isa(x,class) @@ -46,6 +56,12 @@ struct Argument { /// MATLAB code generation, MATLAB to C++ void matlab_unwrap(FileWriter& file, const std::string& matlabName) const; + /** + * emit checking argument to MATLAB proxy + * @param proxyFile output stream + */ + void proxy_check(FileWriter& proxyFile, const std::string& s) const; + friend std::ostream& operator<<(std::ostream& os, const Argument& arg) { os << (arg.is_const ? "const " : "") << arg.type << (arg.is_ptr ? "*" : "") << (arg.is_ref ? "&" : ""); @@ -88,26 +104,12 @@ struct ArgumentList: public std::vector { void emit_prototype(FileWriter& file, const std::string& name) const; /** - * emit emit MATLAB call to proxy + * emit checking arguments to MATLAB proxy * @param proxyFile output stream - * @param returnVal the return value - * @param wrapperName of method or function - * @param staticMethod flag to emit "this" in call */ - void emit_call(FileWriter& proxyFile, const ReturnValue& returnVal, - const std::string& wrapperName, int id, bool staticMethod = false) const; - - /** - * emit conditional MATLAB call to proxy (checking arguments first) - * @param proxyFile output stream - * @param returnVal the return value - * @param wrapperName of method or function - * @param staticMethod flag to emit "this" in call - */ - void emit_conditional_call(FileWriter& proxyFile, - const ReturnValue& returnVal, const std::string& wrapperName, int id, - bool staticMethod = false) const; + void proxy_check(FileWriter& proxyFile) const; + /// Output stream operator friend std::ostream& operator<<(std::ostream& os, const ArgumentList& argList) { os << "("; @@ -122,5 +124,88 @@ struct ArgumentList: public std::vector { }; -} // \namespace wrap +/* ************************************************************************* */ +// http://boost-spirit.com/distrib/spirit_1_8_2/libs/spirit/doc/grammar.html +struct ArgumentGrammar: public classic::grammar { + + wrap::Argument& result_; ///< successful parse will be placed in here + TypeGrammar argument_type_g; ///< Type parser for Argument::type + + /// Construct type grammar and specify where result is placed + ArgumentGrammar(wrap::Argument& result) : + result_(result), argument_type_g(result.type) { + } + + /// Definition of type grammar + template + struct definition: BasicRules { + + typedef classic::rule Rule; + + Rule argument_p; + + definition(ArgumentGrammar const& self) { + using namespace classic; + + // NOTE: allows for pointers to all types + // Slightly more permissive than before on basis/eigen type qualification + // Also, currently parses Point2*&, can't make it work otherwise :-( + argument_p = !str_p("const")[assign_a(self.result_.is_const, T)] // + >> self.argument_type_g // + >> !ch_p('*')[assign_a(self.result_.is_ptr, T)] + >> !ch_p('&')[assign_a(self.result_.is_ref, T)] + >> BasicRules::name_p[assign_a(self.result_.name)]; + } + + Rule const& start() const { + return argument_p; + } + + }; +}; +// ArgumentGrammar + +/* ************************************************************************* */ +// http://boost-spirit.com/distrib/spirit_1_8_2/libs/spirit/doc/grammar.html +struct ArgumentListGrammar: public classic::grammar { + + wrap::ArgumentList& result_; ///< successful parse will be placed in here + + /// Construct type grammar and specify where result is placed + ArgumentListGrammar(wrap::ArgumentList& result) : + result_(result) { + } + + /// Definition of type grammar + template + struct definition { + + const Argument arg0; ///< used to reset arg + Argument arg; ///< temporary argument for use during parsing + ArgumentGrammar argument_g; ///< single Argument parser + + classic::rule argument_p, argumentList_p; + + definition(ArgumentListGrammar const& self) : + argument_g(arg) { + using namespace classic; + + argument_p = argument_g // + [classic::push_back_a(self.result_, arg)] // + [assign_a(arg, arg0)]; + + argumentList_p = '(' >> !argument_p >> *(',' >> argument_p) >> ')'; + } + + classic::rule const& start() const { + return argumentList_p; + } + + }; +}; +// ArgumentListGrammar + +/* ************************************************************************* */ + +}// \namespace wrap diff --git a/wrap/Class.cpp b/wrap/Class.cpp index 0e480f0fd..6e415065d 100644 --- a/wrap/Class.cpp +++ b/wrap/Class.cpp @@ -22,23 +22,68 @@ #include #include +#include +#include #include #include #include +#include // std::ostream_iterator //#include // on Linux GCC: fails with error regarding needing C++0x std flags //#include // same failure as above #include // works on Linux GCC using namespace std; using namespace wrap; +/* ************************************************************************* */ +void Class::assignParent(const Qualified& parent) { + parentClass.reset(parent); +} + +/* ************************************************************************* */ +boost::optional Class::qualifiedParent() const { + boost::optional result = boost::none; + if (parentClass) + result = parentClass->qualifiedName("::"); + return result; +} + +/* ************************************************************************* */ +static void handleException(const out_of_range& oor, + const Class::Methods& methods) { + cerr << "Class::method: key not found: " << oor.what() << ", methods are:\n"; + using boost::adaptors::map_keys; + ostream_iterator out_it(cerr, "\n"); + boost::copy(methods | map_keys, out_it); +} + +/* ************************************************************************* */ +Method& Class::mutableMethod(Str key) { + try { + return methods_.at(key); + } catch (const out_of_range& oor) { + handleException(oor, methods_); + throw runtime_error("Internal error in wrap"); + } +} + +/* ************************************************************************* */ +const Method& Class::method(Str key) const { + try { + return methods_.at(key); + } catch (const out_of_range& oor) { + handleException(oor, methods_); + throw runtime_error("Internal error in wrap"); + } +} + /* ************************************************************************* */ void Class::matlab_proxy(Str toolboxPath, Str wrapperName, const TypeAttributesTable& typeAttributes, FileWriter& wrapperFile, vector& functionNames) const { // Create namespace folders - createNamespaceStructure(namespaces, toolboxPath); + createNamespaceStructure(namespaces(), toolboxPath); // open destination classFile string classFile = matlabName(toolboxPath); @@ -48,21 +93,20 @@ void Class::matlab_proxy(Str toolboxPath, Str wrapperName, const string matlabQualName = qualifiedName("."); const string matlabUniqueName = qualifiedName(); const string cppName = qualifiedName("::"); - const string matlabBaseName = qualifiedParent.qualifiedName("."); - const string cppBaseName = qualifiedParent.qualifiedName("::"); // emit class proxy code // we want our class to inherit the handle class for memory purposes - const string parent = qualifiedParent.empty() ? "handle" : matlabBaseName; + const string parent = + parentClass ? parentClass->qualifiedName(".") : "handle"; comment_fragment(proxyFile); - proxyFile.oss << "classdef " << name << " < " << parent << endl; + proxyFile.oss << "classdef " << name() << " < " << parent << endl; proxyFile.oss << " properties\n"; proxyFile.oss << " ptr_" << matlabUniqueName << " = 0\n"; proxyFile.oss << " end\n"; proxyFile.oss << " methods\n"; // Constructor - proxyFile.oss << " function obj = " << name << "(varargin)\n"; + proxyFile.oss << " function obj = " << name() << "(varargin)\n"; // Special pointer constructors - one in MATLAB to create an object and // assign a pointer returned from a C++ function. In turn this MATLAB // constructor calls a special C++ function that just adds the object to @@ -75,11 +119,12 @@ void Class::matlab_proxy(Str toolboxPath, Str wrapperName, wrapperFile.oss << "\n"; // Regular constructors + boost::optional cppBaseName = qualifiedParent(); for (size_t i = 0; i < constructor.nrOverloads(); i++) { ArgumentList args = constructor.argumentList(i); const int id = (int) functionNames.size(); - constructor.proxy_fragment(proxyFile, wrapperName, !qualifiedParent.empty(), - id, args); + constructor.proxy_fragment(proxyFile, wrapperName, (bool) parentClass, id, + args); const string wrapFunctionName = constructor.wrapper_fragment(wrapperFile, cppName, matlabUniqueName, cppBaseName, id, args); wrapperFile.oss << "\n"; @@ -89,9 +134,9 @@ void Class::matlab_proxy(Str toolboxPath, Str wrapperName, proxyFile.oss << " error('Arguments do not match any overload of " << matlabQualName << " constructor');\n"; proxyFile.oss << " end\n"; - if (!qualifiedParent.empty()) - proxyFile.oss << " obj = obj@" << matlabBaseName << "(uint64(" - << ptr_constructor_key << "), base_ptr);\n"; + if (parentClass) + proxyFile.oss << " obj = obj@" << parentClass->qualifiedName(".") + << "(uint64(" << ptr_constructor_key << "), base_ptr);\n"; proxyFile.oss << " obj.ptr_" << matlabUniqueName << " = my_ptr;\n"; proxyFile.oss << " end\n\n"; @@ -111,7 +156,7 @@ void Class::matlab_proxy(Str toolboxPath, Str wrapperName, << " function disp(obj), obj.display; end\n %DISP Calls print on the object\n"; // Methods - BOOST_FOREACH(const Methods::value_type& name_m, methods) { + BOOST_FOREACH(const Methods::value_type& name_m, methods_) { const Method& m = name_m.second; m.proxy_wrapper_fragments(proxyFile, wrapperFile, cppName, matlabQualName, matlabUniqueName, wrapperName, typeAttributes, functionNames); @@ -151,7 +196,6 @@ void Class::pointer_constructor_fragments(FileWriter& proxyFile, const string matlabUniqueName = qualifiedName(); const string cppName = qualifiedName("::"); - const string baseCppName = qualifiedParent.qualifiedName("::"); const int collectorInsertId = (int) functionNames.size(); const string collectorInsertFunctionName = matlabUniqueName @@ -188,7 +232,7 @@ void Class::pointer_constructor_fragments(FileWriter& proxyFile, } else { proxyFile.oss << " my_ptr = varargin{2};\n"; } - if (qualifiedParent.empty()) // If this class has a base class, we'll get a base class pointer back + if (!parentClass) // If this class has a base class, we'll get a base class pointer back proxyFile.oss << " "; else proxyFile.oss << " base_ptr = "; @@ -211,9 +255,10 @@ void Class::pointer_constructor_fragments(FileWriter& proxyFile, // Add to collector wrapperFile.oss << " collector_" << matlabUniqueName << ".insert(self);\n"; // If we have a base class, return the base class pointer (MATLAB will call the base class collectorInsertAndMakeBase to add this to the collector and recurse the heirarchy) - if (!qualifiedParent.empty()) { + boost::optional cppBaseName = qualifiedParent(); + if (cppBaseName) { wrapperFile.oss << "\n"; - wrapperFile.oss << " typedef boost::shared_ptr<" << baseCppName + wrapperFile.oss << " typedef boost::shared_ptr<" << *cppBaseName << "> SharedBase;\n"; wrapperFile.oss << " out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);\n"; @@ -244,10 +289,10 @@ void Class::pointer_constructor_fragments(FileWriter& proxyFile, /* ************************************************************************* */ Class Class::expandTemplate(const TemplateSubstitution& ts) const { Class inst = *this; - inst.methods = expandMethodTemplate(methods, ts); + inst.methods_ = expandMethodTemplate(methods_, ts); inst.static_methods = expandMethodTemplate(static_methods, ts); inst.constructor = constructor.expandTemplate(ts); - inst.deconstructor.name = inst.name; + inst.deconstructor.name = inst.name(); return inst; } @@ -257,10 +302,10 @@ vector Class::expandTemplate(Str templateArg, vector result; BOOST_FOREACH(const Qualified& instName, instantiations) { Qualified expandedClass = (Qualified) (*this); - expandedClass.name += instName.name; + expandedClass.expand(instName.name()); const TemplateSubstitution ts(templateArg, instName, expandedClass); Class inst = expandTemplate(ts); - inst.name = expandedClass.name; + inst.name_ = expandedClass.name(); inst.templateArgs.clear(); inst.typedefName = qualifiedName("::") + "<" + instName.qualifiedName("::") + ">"; @@ -272,50 +317,50 @@ vector Class::expandTemplate(Str templateArg, /* ************************************************************************* */ void Class::addMethod(bool verbose, bool is_const, Str methodName, const ArgumentList& argumentList, const ReturnValue& returnValue, - Str templateArgName, const vector& templateArgValues) { + const Template& tmplate) { // Check if templated - if (!templateArgName.empty() && templateArgValues.size() > 0) { + if (tmplate.valid()) { // Create method to expand // For all values of the template argument, create a new method - BOOST_FOREACH(const Qualified& instName, templateArgValues) { - const TemplateSubstitution ts(templateArgName, instName, this->name); + BOOST_FOREACH(const Qualified& instName, tmplate.argValues()) { + const TemplateSubstitution ts(tmplate.argName(), instName, *this); // substitute template in arguments ArgumentList expandedArgs = argumentList.expandTemplate(ts); // do the same for return type ReturnValue expandedRetVal = returnValue.expandTemplate(ts); // Now stick in new overload stack with expandedMethodName key // but note we use the same, unexpanded methodName in overload - string expandedMethodName = methodName + instName.name; - methods[expandedMethodName].addOverload(methodName, expandedArgs, + string expandedMethodName = methodName + instName.name(); + methods_[expandedMethodName].addOverload(methodName, expandedArgs, expandedRetVal, is_const, instName, verbose); } } else // just add overload - methods[methodName].addOverload(methodName, argumentList, returnValue, - is_const, Qualified(), verbose); + methods_[methodName].addOverload(methodName, argumentList, returnValue, + is_const, boost::none, verbose); } /* ************************************************************************* */ void Class::erase_serialization() { - Methods::iterator it = methods.find("serializable"); - if (it != methods.end()) { + Methods::iterator it = methods_.find("serializable"); + if (it != methods_.end()) { #ifndef WRAP_DISABLE_SERIALIZE isSerializable = true; #else // cout << "Ignoring serializable() flag in class " << name << endl; #endif - methods.erase(it); + methods_.erase(it); } - it = methods.find("serialize"); - if (it != methods.end()) { + it = methods_.find("serialize"); + if (it != methods_.end()) { #ifndef WRAP_DISABLE_SERIALIZE isSerializable = true; hasSerialization = true; #else // cout << "Ignoring serialize() flag in class " << name << endl; #endif - methods.erase(it); + methods_.erase(it); } } @@ -327,31 +372,31 @@ void Class::verifyAll(vector& validTypes, bool& hasSerialiable) const { // verify all of the function arguments //TODO:verifyArguments(validTypes, constructor.args_list); verifyArguments(validTypes, static_methods); - verifyArguments(validTypes, methods); + verifyArguments(validTypes, methods_); // verify function return types verifyReturnTypes(validTypes, static_methods); - verifyReturnTypes(validTypes, methods); + verifyReturnTypes(validTypes, methods_); // verify parents - if (!qualifiedParent.empty() - && find(validTypes.begin(), validTypes.end(), - qualifiedParent.qualifiedName("::")) == validTypes.end()) - throw DependencyMissing(qualifiedParent.qualifiedName("::"), - qualifiedName("::")); + boost::optional parent = qualifiedParent(); + if (parent + && find(validTypes.begin(), validTypes.end(), *parent) + == validTypes.end()) + throw DependencyMissing(*parent, qualifiedName("::")); } /* ************************************************************************* */ void Class::appendInheritedMethods(const Class& cls, const vector& classes) { - if (!cls.qualifiedParent.empty()) { + if (cls.parentClass) { // Find parent BOOST_FOREACH(const Class& parent, classes) { // We found a parent class for our parent, TODO improve ! - if (parent.name == cls.qualifiedParent.name) { - methods.insert(parent.methods.begin(), parent.methods.end()); + if (parent.name() == cls.parentClass->name()) { + methods_.insert(parent.methods_.begin(), parent.methods_.end()); appendInheritedMethods(parent, classes); } } @@ -361,11 +406,11 @@ void Class::appendInheritedMethods(const Class& cls, /* ************************************************************************* */ string Class::getTypedef() const { string result; - BOOST_FOREACH(Str namesp, namespaces) { + BOOST_FOREACH(Str namesp, namespaces()) { result += ("namespace " + namesp + " { "); } - result += ("typedef " + typedefName + " " + name + ";"); - for (size_t i = 0; i < namespaces.size(); ++i) { + result += ("typedef " + typedefName + " " + name() + ";"); + for (size_t i = 0; i < namespaces().size(); ++i) { result += " }"; } return result; @@ -373,15 +418,15 @@ string Class::getTypedef() const { /* ************************************************************************* */ void Class::comment_fragment(FileWriter& proxyFile) const { - proxyFile.oss << "%class " << name << ", see Doxygen page for details\n"; + proxyFile.oss << "%class " << name() << ", see Doxygen page for details\n"; proxyFile.oss << "%at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html\n"; constructor.comment_fragment(proxyFile); - if (!methods.empty()) + if (!methods_.empty()) proxyFile.oss << "%\n%-------Methods-------\n"; - BOOST_FOREACH(const Methods::value_type& name_m, methods) + BOOST_FOREACH(const Methods::value_type& name_m, methods_) name_m.second.comment_fragment(proxyFile); if (!static_methods.empty()) @@ -393,7 +438,7 @@ void Class::comment_fragment(FileWriter& proxyFile) const { proxyFile.oss << "%\n%-------Serialization Interface-------\n"; proxyFile.oss << "%string_serialize() : returns string\n"; proxyFile.oss << "%string_deserialize(string serialized) : returns " - << this->name << "\n"; + << name() << "\n"; } proxyFile.oss << "%\n"; @@ -586,12 +631,12 @@ string Class::getSerializationExport() const { /* ************************************************************************* */ void Class::python_wrapper(FileWriter& wrapperFile) const { - wrapperFile.oss << "class_<" << name << ">(\"" << name << "\")\n"; - constructor.python_wrapper(wrapperFile, name); + wrapperFile.oss << "class_<" << name() << ">(\"" << name() << "\")\n"; + constructor.python_wrapper(wrapperFile, name()); BOOST_FOREACH(const StaticMethod& m, static_methods | boost::adaptors::map_values) - m.python_wrapper(wrapperFile, name); - BOOST_FOREACH(const Method& m, methods | boost::adaptors::map_values) - m.python_wrapper(wrapperFile, name); + m.python_wrapper(wrapperFile, name()); + BOOST_FOREACH(const Method& m, methods_ | boost::adaptors::map_values) + m.python_wrapper(wrapperFile, name()); wrapperFile.oss << ";\n\n"; } diff --git a/wrap/Class.h b/wrap/Class.h index 34323b797..f4c687eca 100644 --- a/wrap/Class.h +++ b/wrap/Class.h @@ -19,14 +19,29 @@ #pragma once +#include "spirit.h" +#include "Template.h" #include "Constructor.h" #include "Deconstructor.h" #include "Method.h" #include "StaticMethod.h" #include "TypeAttributesTable.h" +#ifdef __GNUC__ +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wunused-variable" +#endif +#include +#include +#ifdef __GNUC__ +#pragma GCC diagnostic pop +#endif + +namespace bl = boost::lambda; + #include #include +#include #include #include @@ -36,13 +51,20 @@ namespace wrap { /// Class has name, constructors, methods class Class: public Qualified { +public: + typedef const std::string& Str; typedef std::map Methods; - Methods methods; ///< Class methods + typedef std::map StaticMethods; + +private: + + boost::optional parentClass; ///< The *single* parent + Methods methods_; ///< Class methods + Method& mutableMethod(Str key); public: - typedef const std::string& Str; - typedef std::map StaticMethods; + StaticMethods static_methods; ///< Static methods // Then the instance variables are set directly by the Module constructor std::vector templateArgs; ///< Template arguments @@ -50,26 +72,28 @@ public: bool isVirtual; ///< Whether the class is part of a virtual inheritance chain bool isSerializable; ///< Whether we can use boost.serialization to serialize the class - creates exports bool hasSerialization; ///< Whether we should create the serialization functions - Qualified qualifiedParent; ///< The *single* parent - StaticMethods static_methods; ///< Static methods Constructor constructor; ///< Class constructors Deconstructor deconstructor; ///< Deconstructor to deallocate C++ object bool verbose_; ///< verbose flag /// Constructor creates an empty class Class(bool verbose = true) : - isVirtual(false), isSerializable(false), hasSerialization(false), deconstructor( - verbose), verbose_(verbose) { + parentClass(boost::none), isVirtual(false), isSerializable(false), hasSerialization( + false), deconstructor(verbose), verbose_(verbose) { } + void assignParent(const Qualified& parent); + + boost::optional qualifiedParent() const; + size_t nrMethods() const { - return methods.size(); - } - Method& method(Str name) { - return methods.at(name); + return methods_.size(); } + + const Method& method(Str key) const; + bool exists(Str name) const { - return methods.find(name) != methods.end(); + return methods_.find(name) != methods_.end(); } // And finally MATLAB code is emitted, methods below called by Module::matlab_code @@ -85,7 +109,7 @@ public: /// Add potentially overloaded, potentially templated method void addMethod(bool verbose, bool is_const, Str methodName, const ArgumentList& argumentList, const ReturnValue& returnValue, - Str templateArgName, const std::vector& templateArgValues); + const Template& tmplate); /// Post-process classes for serialization markers void erase_serialization(); // non-const ! @@ -115,11 +139,11 @@ public: void python_wrapper(FileWriter& wrapperFile) const; friend std::ostream& operator<<(std::ostream& os, const Class& cls) { - os << "class " << cls.name << "{\n"; + os << "class " << cls.name() << "{\n"; os << cls.constructor << ";\n"; BOOST_FOREACH(const StaticMethod& m, cls.static_methods | boost::adaptors::map_values) os << m << ";\n"; - BOOST_FOREACH(const Method& m, cls.methods | boost::adaptors::map_values) + BOOST_FOREACH(const Method& m, cls.methods_ | boost::adaptors::map_values) os << m << ";\n"; os << "};" << std::endl; return os; @@ -134,5 +158,122 @@ private: void comment_fragment(FileWriter& proxyFile) const; }; -} // \namespace wrap +/* ************************************************************************* */ +// http://boost-spirit.com/distrib/spirit_1_8_2/libs/spirit/doc/grammar.html +struct ClassGrammar: public classic::grammar { + + Class& cls_; ///< successful parse will be placed in here + Template& template_; ///< result needs to be visible outside + + /// Construct type grammar and specify where result is placed + ClassGrammar(Class& cls, Template& t) : + cls_(cls), template_(t) { + } + + /// Definition of type grammar + template + struct definition: BasicRules { + + using BasicRules::name_p; + using BasicRules::className_p; + using BasicRules::comments_p; + + // NOTE: allows for pointers to all types + ArgumentList args; + ArgumentListGrammar argumentList_g; + + Constructor constructor0, constructor; + + ReturnValue retVal0, retVal; + ReturnValueGrammar returnValue_g; + + Template methodTemplate; + TemplateGrammar methodTemplate_g, classTemplate_g; + + std::string methodName; + bool isConst, T, F; + + // Parent class + Qualified possibleParent; + TypeGrammar classParent_g; + + classic::rule constructor_p, methodName_p, method_p, + staticMethodName_p, static_method_p, templateList_p, classParent_p, + functions_p, class_p; + + definition(ClassGrammar const& self) : + argumentList_g(args), returnValue_g(retVal), // + methodTemplate_g(methodTemplate), classTemplate_g(self.template_), // + T(true), F(false), classParent_g(possibleParent) { + + using namespace classic; + bool verbose = false; // TODO + + // ConstructorGrammar + constructor_p = (className_p >> argumentList_g >> ';' >> !comments_p) // + [bl::bind(&Constructor::push_back, bl::var(constructor), + bl::var(args))] // + [clear_a(args)]; + + // MethodGrammar + methodName_p = lexeme_d[(upper_p | lower_p) >> *(alnum_p | '_')]; + + // gtsam::Values retract(const gtsam::VectorValues& delta) const; + method_p = !methodTemplate_g + >> (returnValue_g >> methodName_p[assign_a(methodName)] + >> argumentList_g >> !str_p("const")[assign_a(isConst, T)] >> ';' + >> *comments_p) // + [bl::bind(&Class::addMethod, bl::var(self.cls_), verbose, + bl::var(isConst), bl::var(methodName), bl::var(args), + bl::var(retVal), bl::var(methodTemplate))] // + [assign_a(retVal, retVal0)][clear_a(args)] // + [clear_a(methodTemplate)][assign_a(isConst, F)]; + + // StaticMethodGrammar + staticMethodName_p = lexeme_d[(upper_p | lower_p) >> *(alnum_p | '_')]; + + static_method_p = (str_p("static") >> returnValue_g + >> staticMethodName_p[assign_a(methodName)] >> argumentList_g >> ';' + >> *comments_p) // + [bl::bind(&StaticMethod::addOverload, + bl::var(self.cls_.static_methods)[bl::var(methodName)], + bl::var(methodName), bl::var(args), bl::var(retVal), boost::none, + verbose)] // + [assign_a(retVal, retVal0)][clear_a(args)]; + + // template + templateList_p = (str_p("template") >> '<' + >> name_p[push_back_a(self.cls_.templateArgs)] + >> *(',' >> name_p[push_back_a(self.cls_.templateArgs)]) >> '>'); + + // parse a full class + classParent_p = (':' >> classParent_g >> '{') // + [bl::bind(&Class::assignParent, bl::var(self.cls_), + bl::var(possibleParent))][clear_a(possibleParent)]; + + functions_p = constructor_p | method_p | static_method_p; + + // parse a full class + class_p = (!(classTemplate_g[push_back_a(self.cls_.templateArgs, + self.template_.argName())] | templateList_p) + >> !(str_p("virtual")[assign_a(self.cls_.isVirtual, T)]) + >> str_p("class") >> className_p[assign_a(self.cls_.name_)] + >> (classParent_p | '{') >> // + *(functions_p | comments_p) >> str_p("};")) // + [bl::bind(&Constructor::initializeOrCheck, bl::var(constructor), + bl::var(self.cls_.name_), boost::none, verbose)][assign_a( + self.cls_.constructor, constructor)] // + [assign_a(self.cls_.deconstructor.name, self.cls_.name_)] // + [assign_a(constructor, constructor0)]; + } + + classic::rule const& start() const { + return class_p; + } + + }; +}; +// ClassGrammar + +}// \namespace wrap diff --git a/wrap/Constructor.cpp b/wrap/Constructor.cpp index 782c0ca80..35cc0fa53 100644 --- a/wrap/Constructor.cpp +++ b/wrap/Constructor.cpp @@ -69,7 +69,7 @@ void Constructor::proxy_fragment(FileWriter& file, /* ************************************************************************* */ string Constructor::wrapper_fragment(FileWriter& file, Str cppClassName, - Str matlabUniqueName, Str cppBaseClassName, int id, + Str matlabUniqueName, boost::optional cppBaseClassName, int id, const ArgumentList& al) const { const string wrapFunctionName = matlabUniqueName + "_constructor_" @@ -100,9 +100,9 @@ string Constructor::wrapper_fragment(FileWriter& file, Str cppClassName, << endl; // If we have a base class, return the base class pointer (MATLAB will call the base class collectorInsertAndMakeBase to add this to the collector and recurse the heirarchy) - if (!cppBaseClassName.empty()) { + if (cppBaseClassName) { file.oss << "\n"; - file.oss << " typedef boost::shared_ptr<" << cppBaseClassName + file.oss << " typedef boost::shared_ptr<" << *cppBaseClassName << "> SharedBase;\n"; file.oss << " out[1] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);\n"; diff --git a/wrap/Constructor.h b/wrap/Constructor.h index a026bf423..1e061d17c 100644 --- a/wrap/Constructor.h +++ b/wrap/Constructor.h @@ -19,7 +19,7 @@ #pragma once #include "OverloadedFunction.h" - +#include #include #include @@ -68,7 +68,7 @@ struct Constructor: public OverloadedFunction { /// cpp wrapper std::string wrapper_fragment(FileWriter& file, Str cppClassName, - Str matlabUniqueName, Str cppBaseClassName, int id, + Str matlabUniqueName, boost::optional cppBaseClassName, int id, const ArgumentList& al) const; /// constructor function diff --git a/wrap/ForwardDeclaration.h b/wrap/ForwardDeclaration.h index 5506dd778..5ec022ca4 100644 --- a/wrap/ForwardDeclaration.h +++ b/wrap/ForwardDeclaration.h @@ -27,6 +27,7 @@ namespace wrap { std::string name; bool isVirtual; ForwardDeclaration() : isVirtual(false) {} + ForwardDeclaration(const std::string& s) : name(s), isVirtual(false) {} }; -} \ No newline at end of file +} diff --git a/wrap/FullyOverloadedFunction.h b/wrap/FullyOverloadedFunction.h index ac22ec3a8..30e27764b 100644 --- a/wrap/FullyOverloadedFunction.h +++ b/wrap/FullyOverloadedFunction.h @@ -109,8 +109,8 @@ class FullyOverloadedFunction: public Function, public SignatureOverloads { public: bool addOverload(const std::string& name, const ArgumentList& args, - const ReturnValue& retVal, const Qualified& instName = Qualified(), - bool verbose = false) { + const ReturnValue& retVal, boost::optional instName = + boost::none, bool verbose = false) { bool first = initializeOrCheck(name, instName, verbose); SignatureOverloads::push_back(args, retVal); return first; diff --git a/wrap/Function.cpp b/wrap/Function.cpp index 6faa70fb9..7e4ded040 100644 --- a/wrap/Function.cpp +++ b/wrap/Function.cpp @@ -29,12 +29,11 @@ using namespace std; using namespace wrap; /* ************************************************************************* */ -bool Function::initializeOrCheck(const std::string& name, - const Qualified& instName, bool verbose) { +bool Function::initializeOrCheck(const string& name, + boost::optional instName, bool verbose) { if (name.empty()) - throw std::runtime_error( - "Function::initializeOrCheck called with empty name"); + throw runtime_error("Function::initializeOrCheck called with empty name"); // Check if this overload is give to the correct method if (name_.empty()) { @@ -43,15 +42,38 @@ bool Function::initializeOrCheck(const std::string& name, verbose_ = verbose; return true; } else { - if (name_ != name || templateArgValue_ != instName || verbose_ != verbose) - throw std::runtime_error( - "Function::initializeOrCheck called with different arguments: with name " - + name + " instead of expected " + name_ - + ", or with template argument " + instName.qualifiedName(":") - + " instead of expected " + templateArgValue_.qualifiedName(":")); + if (name_ != name || verbose_ != verbose + || ((bool) templateArgValue_ != (bool) instName) + || ((bool) templateArgValue_ && (bool) instName + && !(*templateArgValue_ == *instName))) + throw runtime_error( + "Function::initializeOrCheck called with different arguments"); return false; } } /* ************************************************************************* */ +void Function::emit_call(FileWriter& proxyFile, const ReturnValue& returnVal, + const string& wrapperName, int id) const { + returnVal.emit_matlab(proxyFile); + proxyFile.oss << wrapperName << "(" << id; + if (!isStatic()) + proxyFile.oss << ", this"; + proxyFile.oss << ", varargin{:});\n"; +} + +/* ************************************************************************* */ +void Function::emit_conditional_call(FileWriter& proxyFile, + const ReturnValue& returnVal, const ArgumentList& args, + const string& wrapperName, int id) const { + + // Check all arguments + args.proxy_check(proxyFile); + + // output call to C++ wrapper + proxyFile.oss << " "; + emit_call(proxyFile, returnVal, wrapperName, id); +} + +/* ************************************************************************* */ diff --git a/wrap/Function.h b/wrap/Function.h index 49a26bd8d..ad57a28c8 100644 --- a/wrap/Function.h +++ b/wrap/Function.h @@ -19,6 +19,7 @@ #pragma once #include "Argument.h" +#include namespace wrap { @@ -28,7 +29,7 @@ class Function { protected: std::string name_; ///< name of method - Qualified templateArgValue_; ///< value of template argument if applicable + boost::optional templateArgValue_; ///< value of template argument if applicable bool verbose_; public: @@ -37,19 +38,35 @@ public: * @brief first time, fill in instance variables, otherwise check if same * @return true if first time, false thereafter */ - bool initializeOrCheck(const std::string& name, const Qualified& instName = - Qualified(), bool verbose = false); + bool initializeOrCheck(const std::string& name, + boost::optional instName = boost::none, bool verbose = + false); std::string name() const { return name_; } - std::string matlabName() const { - if (templateArgValue_.empty()) - return name_; - else - return name_ + templateArgValue_.name; + /// Only Methods are non-static + virtual bool isStatic() const { + return true; } + + std::string matlabName() const { + if (templateArgValue_) + return name_ + templateArgValue_->name(); + else + return name_; + } + + /// Emit function call to MATLAB (no argument checking) + void emit_call(FileWriter& proxyFile, const ReturnValue& returnVal, + const std::string& wrapperName, int id) const; + + /// Emit checking arguments and function call to MATLAB + void emit_conditional_call(FileWriter& proxyFile, + const ReturnValue& returnVal, const ArgumentList& args, + const std::string& wrapperName, int id) const; + }; } // \namespace wrap diff --git a/wrap/GlobalFunction.cpp b/wrap/GlobalFunction.cpp index e843481a1..013ef6d28 100644 --- a/wrap/GlobalFunction.cpp +++ b/wrap/GlobalFunction.cpp @@ -18,9 +18,9 @@ using namespace std; /* ************************************************************************* */ void GlobalFunction::addOverload(const Qualified& overload, const ArgumentList& args, const ReturnValue& retVal, - const Qualified& instName, bool verbose) { - string name(overload.name); - FullyOverloadedFunction::addOverload(name, args, retVal, instName, verbose); + boost::optional instName, bool verbose) { + FullyOverloadedFunction::addOverload(overload.name(), args, retVal, instName, + verbose); overloads.push_back(overload); } @@ -37,7 +37,7 @@ void GlobalFunction::matlab_proxy(const string& toolboxPath, for (size_t i = 0; i < overloads.size(); ++i) { Qualified overload = overloads.at(i); // use concatenated namespaces as key - string str_ns = qualifiedName("", overload.namespaces); + string str_ns = qualifiedName("", overload.namespaces()); const ReturnValue& ret = returnValue(i); const ArgumentList& args = argumentList(i); grouped_functions[str_ns].addOverload(overload, args, ret); @@ -59,7 +59,7 @@ void GlobalFunction::generateSingleFunction(const string& toolboxPath, // create the folder for the namespace const Qualified& overload1 = overloads.front(); - createNamespaceStructure(overload1.namespaces, toolboxPath); + createNamespaceStructure(overload1.namespaces(), toolboxPath); // open destination mfunctionFileName string mfunctionFileName = overload1.matlabName(toolboxPath); @@ -80,7 +80,7 @@ void GlobalFunction::generateSingleFunction(const string& toolboxPath, // Output proxy matlab code mfunctionFile.oss << " " << (i == 0 ? "" : "else"); - args.emit_conditional_call(mfunctionFile, returnVal, wrapperName, id, true); // true omits "this" + emit_conditional_call(mfunctionFile, returnVal, args, wrapperName, id); // Output C++ wrapper code @@ -104,7 +104,7 @@ void GlobalFunction::generateSingleFunction(const string& toolboxPath, args.matlab_unwrap(file, 0); // We start at 0 because there is no self object // call method with default type and wrap result - if (returnVal.type1.name != "void") + if (returnVal.type1.name() != "void") returnVal.wrap_result(cppName + "(" + args.names() + ")", file, typeAttributes); else diff --git a/wrap/GlobalFunction.h b/wrap/GlobalFunction.h index 6a49fe5ce..b2a582654 100644 --- a/wrap/GlobalFunction.h +++ b/wrap/GlobalFunction.h @@ -11,6 +11,18 @@ #include "FullyOverloadedFunction.h" +#ifdef __GNUC__ +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wunused-variable" +#endif +#include +#include +#ifdef __GNUC__ +#pragma GCC diagnostic pop +#endif + +namespace bl = boost::lambda; + namespace wrap { struct GlobalFunction: public FullyOverloadedFunction { @@ -19,8 +31,8 @@ struct GlobalFunction: public FullyOverloadedFunction { // adds an overloaded version of this function, void addOverload(const Qualified& overload, const ArgumentList& args, - const ReturnValue& retVal, const Qualified& instName = Qualified(), - bool verbose = false); + const ReturnValue& retVal, boost::optional instName = + boost::none, bool verbose = false); void verifyArguments(const std::vector& validArgs) const { SignatureOverloads::verifyArguments(validArgs, name_); @@ -47,5 +59,67 @@ private: }; -} // \namespace wrap +typedef std::map GlobalFunctions; + +/* ************************************************************************* */ +// http://boost-spirit.com/distrib/spirit_1_8_2/libs/spirit/doc/grammar.html +struct GlobalFunctionGrammar: public classic::grammar { + + GlobalFunctions& global_functions_; ///< successful parse will be placed in here + std::vector& namespaces_; + + /// Construct type grammar and specify where result is placed + GlobalFunctionGrammar(GlobalFunctions& global_functions, + std::vector& namespaces) : + global_functions_(global_functions), namespaces_(namespaces) { + } + + /// Definition of type grammar + template + struct definition: BasicRules { + +// using BasicRules::name_p; +// using BasicRules::className_p; + using BasicRules::comments_p; + + ArgumentList args; + ArgumentListGrammar argumentList_g; + + ReturnValue retVal0, retVal; + ReturnValueGrammar returnValue_g; + + Qualified globalFunction; + + classic::rule globalFunctionName_p, global_function_p; + + definition(GlobalFunctionGrammar const& self) : + argumentList_g(args), returnValue_g(retVal) { + + using namespace classic; + bool verbose = false; // TODO + + globalFunctionName_p = lexeme_d[(upper_p | lower_p) >> *(alnum_p | '_')]; + + // parse a global function + global_function_p = (returnValue_g + >> globalFunctionName_p[assign_a(globalFunction.name_)] + >> argumentList_g >> ';' >> *comments_p) // + [assign_a(globalFunction.namespaces_, self.namespaces_)][bl::bind( + &GlobalFunction::addOverload, + bl::var(self.global_functions_)[bl::var(globalFunction.name_)], + bl::var(globalFunction), bl::var(args), bl::var(retVal), + boost::none, verbose)] // + [assign_a(retVal, retVal0)][clear_a(globalFunction)][clear_a(args)]; + + } + + classic::rule const& start() const { + return global_function_p; + } + + }; +}; +// GlobalFunctionGrammar + +}// \namespace wrap diff --git a/wrap/Method.cpp b/wrap/Method.cpp index 49d90378d..2ad6e8259 100644 --- a/wrap/Method.cpp +++ b/wrap/Method.cpp @@ -30,9 +30,9 @@ using namespace wrap; /* ************************************************************************* */ bool Method::addOverload(Str name, const ArgumentList& args, - const ReturnValue& retVal, bool is_const, const Qualified& instName, - bool verbose) { - bool first = StaticMethod::addOverload(name, args, retVal, instName, verbose); + const ReturnValue& retVal, bool is_const, + boost::optional instName, bool verbose) { + bool first = MethodBase::addOverload(name, args, retVal, instName, verbose); if (first) is_const_ = is_const; else if (is_const && !is_const_) @@ -52,14 +52,12 @@ void Method::proxy_header(FileWriter& proxyFile) const { /* ************************************************************************* */ string Method::wrapper_call(FileWriter& wrapperFile, Str cppClassName, - Str matlabUniqueName, const ArgumentList& args, - const ReturnValue& returnVal, const TypeAttributesTable& typeAttributes, - const Qualified& instName) const { + Str matlabUniqueName, const ArgumentList& args) const { // check arguments // extra argument obj -> nargin-1 is passed ! // example: checkArguments("equals",nargout,nargin-1,2); - wrapperFile.oss << " checkArguments(\"" << name_ << "\",nargout,nargin-1," - << args.size() << ");\n"; + wrapperFile.oss << " checkArguments(\"" << matlabName() + << "\",nargout,nargin-1," << args.size() << ");\n"; // get class pointer // example: shared_ptr = unwrap_shared_ptr< Test >(in[0], "Test"); @@ -72,8 +70,8 @@ string Method::wrapper_call(FileWriter& wrapperFile, Str cppClassName, // call method and wrap result // example: out[0]=wrap(obj->return_field(t)); string expanded = "obj->" + name_; - if (!instName.empty()) - expanded += ("<" + instName.qualifiedName("::") + ">"); + if (templateArgValue_) + expanded += ("<" + templateArgValue_->qualifiedName("::") + ">"); return expanded; } diff --git a/wrap/Method.h b/wrap/Method.h index db9e6bb9f..33ff7072e 100644 --- a/wrap/Method.h +++ b/wrap/Method.h @@ -18,12 +18,12 @@ #pragma once -#include "StaticMethod.h" +#include "MethodBase.h" namespace wrap { /// Method class -class Method: public StaticMethod { +class Method: public MethodBase { bool is_const_; @@ -32,8 +32,9 @@ public: typedef const std::string& Str; bool addOverload(Str name, const ArgumentList& args, - const ReturnValue& retVal, bool is_const, const Qualified& instName = - Qualified(), bool verbose = false); + const ReturnValue& retVal, bool is_const, + boost::optional instName = boost::none, bool verbose = + false); virtual bool isStatic() const { return false; @@ -55,9 +56,7 @@ private: void proxy_header(FileWriter& proxyFile) const; virtual std::string wrapper_call(FileWriter& wrapperFile, Str cppClassName, - Str matlabUniqueName, const ArgumentList& args, - const ReturnValue& returnVal, const TypeAttributesTable& typeAttributes, - const Qualified& instName) const; + Str matlabUniqueName, const ArgumentList& args) const; }; } // \namespace wrap diff --git a/wrap/MethodBase.cpp b/wrap/MethodBase.cpp new file mode 100644 index 000000000..9b5f7135f --- /dev/null +++ b/wrap/MethodBase.cpp @@ -0,0 +1,141 @@ +/* ---------------------------------------------------------------------------- + + * 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 MethodBase.ccp + * @author Frank Dellaert + * @author Andrew Melim + * @author Richard Roberts + **/ + +#include "Method.h" +#include "utilities.h" + +#include +#include +#include + +#include +#include + +using namespace std; +using namespace wrap; + +/* ************************************************************************* */ +void MethodBase::proxy_wrapper_fragments(FileWriter& proxyFile, + FileWriter& wrapperFile, Str cppClassName, Str matlabQualName, + Str matlabUniqueName, Str wrapperName, + const TypeAttributesTable& typeAttributes, + vector& functionNames) const { + + // emit header, e.g., function varargout = templatedMethod(this, varargin) + proxy_header(proxyFile); + + // Emit comments for documentation + string up_name = boost::to_upper_copy(matlabName()); + proxyFile.oss << " % " << up_name << " usage: "; + usage_fragment(proxyFile, matlabName()); + + // Emit URL to Doxygen page + proxyFile.oss << " % " + << "Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html" + << endl; + + // Handle special case of single overload with all numeric arguments + if (nrOverloads() == 1 && argumentList(0).allScalar()) { + // Output proxy matlab code + // TODO: document why is it OK to not check arguments in this case + proxyFile.oss << " "; + const int id = (int) functionNames.size(); + emit_call(proxyFile, returnValue(0), wrapperName, id); + + // Output C++ wrapper code + const string wrapFunctionName = wrapper_fragment(wrapperFile, cppClassName, + matlabUniqueName, 0, id, typeAttributes); + + // Add to function list + functionNames.push_back(wrapFunctionName); + } else { + // Check arguments for all overloads + for (size_t i = 0; i < nrOverloads(); ++i) { + + // Output proxy matlab code + proxyFile.oss << " " << (i == 0 ? "" : "else"); + const int id = (int) functionNames.size(); + emit_conditional_call(proxyFile, returnValue(i), argumentList(i), + wrapperName, id); + + // Output C++ wrapper code + const string wrapFunctionName = wrapper_fragment(wrapperFile, + cppClassName, matlabUniqueName, i, id, typeAttributes); + + // Add to function list + functionNames.push_back(wrapFunctionName); + } + proxyFile.oss << " else\n"; + proxyFile.oss + << " error('Arguments do not match any overload of function " + << matlabQualName << "." << name_ << "');" << endl; + proxyFile.oss << " end\n"; + } + + proxyFile.oss << " end\n"; +} + +/* ************************************************************************* */ +string MethodBase::wrapper_fragment(FileWriter& wrapperFile, Str cppClassName, + Str matlabUniqueName, int overload, int id, + const TypeAttributesTable& typeAttributes) const { + + // generate code + + const string wrapFunctionName = matlabUniqueName + "_" + name_ + "_" + + boost::lexical_cast(id); + + const ArgumentList& args = argumentList(overload); + const ReturnValue& returnVal = returnValue(overload); + + // call + wrapperFile.oss << "void " << wrapFunctionName + << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; + // start + wrapperFile.oss << "{\n"; + + returnVal.wrapTypeUnwrap(wrapperFile); + + wrapperFile.oss << " typedef boost::shared_ptr<" << cppClassName + << "> Shared;" << endl; + + // get call + // for static methods: cppClassName::staticMethod + // for instance methods: obj->instanceMethod + string expanded = wrapper_call(wrapperFile, cppClassName, matlabUniqueName, + args); + + expanded += ("(" + args.names() + ")"); + if (returnVal.type1.name() != "void") + returnVal.wrap_result(expanded, wrapperFile, typeAttributes); + else + wrapperFile.oss << " " + expanded + ";\n"; + + // finish + wrapperFile.oss << "}\n"; + + return wrapFunctionName; +} + +/* ************************************************************************* */ +void MethodBase::python_wrapper(FileWriter& wrapperFile, Str className) const { + wrapperFile.oss << " .def(\"" << name_ << "\", &" << className << "::" + << name_ << ");\n"; +} + +/* ************************************************************************* */ diff --git a/wrap/MethodBase.h b/wrap/MethodBase.h new file mode 100644 index 000000000..903b89569 --- /dev/null +++ b/wrap/MethodBase.h @@ -0,0 +1,67 @@ +/* ---------------------------------------------------------------------------- + + * 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 MethodBase.h + * @brief describes and generates code for static methods + * @author Frank Dellaert + * @author Alex Cunningham + * @author Richard Roberts + **/ + +#pragma once + +#include "FullyOverloadedFunction.h" + +namespace wrap { + +/// MethodBase class +struct MethodBase: public FullyOverloadedFunction { + + typedef const std::string& Str; + + // emit a list of comments, one for each overload + void comment_fragment(FileWriter& proxyFile) const { + SignatureOverloads::comment_fragment(proxyFile, matlabName()); + } + + void verifyArguments(const std::vector& validArgs) const { + SignatureOverloads::verifyArguments(validArgs, name_); + } + + void verifyReturnTypes(const std::vector& validtypes) const { + SignatureOverloads::verifyReturnTypes(validtypes, name_); + } + + // MATLAB code generation + // classPath is class directory, e.g., ../matlab/@Point2 + void proxy_wrapper_fragments(FileWriter& proxyFile, FileWriter& wrapperFile, + Str cppClassName, Str matlabQualName, Str matlabUniqueName, + Str wrapperName, const TypeAttributesTable& typeAttributes, + std::vector& functionNames) const; + + // emit python wrapper + void python_wrapper(FileWriter& wrapperFile, Str className) const; + +protected: + + virtual void proxy_header(FileWriter& proxyFile) const = 0; + + std::string wrapper_fragment(FileWriter& wrapperFile, Str cppClassName, + Str matlabUniqueName, int overload, int id, + const TypeAttributesTable& typeAttributes) const; ///< cpp wrapper + + virtual std::string wrapper_call(FileWriter& wrapperFile, Str cppClassName, + Str matlabUniqueName, const ArgumentList& args) const = 0; +}; + +} // \namespace wrap + diff --git a/wrap/Module.cpp b/wrap/Module.cpp index ac0e0a579..55fd13715 100644 --- a/wrap/Module.cpp +++ b/wrap/Module.cpp @@ -22,22 +22,6 @@ #include "TypeAttributesTable.h" #include "utilities.h" -//#define BOOST_SPIRIT_DEBUG -#include "spirit_actors.h" - -#include -#include -#include -#ifdef __GNUC__ -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wunused-variable" -#endif -#include -#include -#ifdef __GNUC__ -#pragma GCC diagnostic pop -#endif -#include #include #include #include @@ -51,8 +35,6 @@ using namespace BOOST_SPIRIT_CLASSIC_NS; namespace bl = boost::lambda; namespace fs = boost::filesystem; -typedef rule Rule; - /* ************************************************************************* */ // We parse an interface file into a Module object. // The grammar is defined using the boost/spirit combinatorial parser. @@ -102,7 +84,6 @@ Module::Module(const string& interfacePath, void Module::parseMarkup(const std::string& data) { // The parse imperatively :-( updates variables gradually during parse // The one with postfix 0 are used to reset the variables after parse. - vector namespaces; // current namespace tag //---------------------------------------------------------------------------- // Grammar with actions that build the Class object. Actions are @@ -113,211 +94,40 @@ void Module::parseMarkup(const std::string& data) { // http://www.boost.org/doc/libs/1_37_0/libs/spirit/classic/doc/directives.html // ---------------------------------------------------------------------------- - Rule comments_p = comment_p("/*", "*/") | comment_p("//", eol_p); - - Rule basisType_p = - (str_p("string") | "bool" | "size_t" | "int" | "double" | "char" | "unsigned char"); - - Rule keywords_p = - (str_p("const") | "static" | "namespace" | "void" | basisType_p); - - Rule eigenType_p = - (str_p("Vector") | "Matrix"); + // Define Rule and instantiate basic rules + typedef rule Rule; + BasicRules basic; - //Rule for STL Containers (class names are lowercase) - Rule stlType_p = (str_p("vector") | "list"); - - Rule className_p = (lexeme_d[upper_p >> *(alnum_p | '_')] - eigenType_p - keywords_p) | stlType_p; - - Rule namespace_name_p = lexeme_d[lower_p >> *(alnum_p | '_')] - keywords_p; - - Argument arg0, arg; - Rule namespace_arg_p = namespace_name_p - [push_back_a(arg.type.namespaces)] >> str_p("::"); - - Rule argEigenType_p = - eigenType_p[assign_a(arg.type.name)]; - - Rule eigenRef_p = - !str_p("const") [assign_a(arg.is_const,true)] >> - eigenType_p [assign_a(arg.type.name)] >> - ch_p('&') [assign_a(arg.is_ref,true)]; - - Rule classArg_p = - !str_p("const") [assign_a(arg.is_const,true)] >> - *namespace_arg_p >> - className_p[assign_a(arg.type.name)] >> - !ch_p('&')[assign_a(arg.is_ref,true)]; - - Rule name_p = lexeme_d[alpha_p >> *(alnum_p | '_')]; + vector namespaces; // current namespace tag - // TODO, do we really need cls here? Non-local + // parse a full class Class cls0(verbose),cls(verbose); - Rule classParent_p = - *(namespace_name_p[push_back_a(cls.qualifiedParent.namespaces)] >> str_p("::")) >> - className_p[assign_a(cls.qualifiedParent.name)]; - - // parse "gtsam::Pose2" and add to templateArgValues - Qualified templateArgValue; - vector templateArgValues; - Rule templateArgValue_p = - (*(namespace_name_p[push_back_a(templateArgValue.namespaces)] >> str_p("::")) >> - className_p[assign_a(templateArgValue.name)]) - [push_back_a(templateArgValues, templateArgValue)] - [clear_a(templateArgValue)]; - - // template - string templateArgName; - Rule templateArgValues_p = - (str_p("template") >> - '<' >> name_p[assign_a(templateArgName)] >> '=' >> - '{' >> !(templateArgValue_p >> *(',' >> templateArgValue_p)) >> '}' >> - '>'); - + Template classTemplate; + ClassGrammar class_g(cls,classTemplate); + Rule class_p = class_g // + [assign_a(cls.namespaces_, namespaces)] + [bl::bind(&handle_possible_template, bl::var(classes), bl::var(cls), + bl::var(classTemplate.argValues()))] + [clear_a(classTemplate)] // + [assign_a(cls,cls0)]; + // parse "gtsam::Pose2" and add to singleInstantiation.typeList - TemplateInstantiationTypedef singleInstantiation; - Rule templateSingleInstantiationArg_p = - (*(namespace_name_p[push_back_a(templateArgValue.namespaces)] >> str_p("::")) >> - className_p[assign_a(templateArgValue.name)]) - [push_back_a(singleInstantiation.typeList, templateArgValue)] - [clear_a(templateArgValue)]; + TemplateInstantiationTypedef singleInstantiation, singleInstantiation0; + TypeListGrammar<'<','>'> typelist_g(singleInstantiation.typeList); // typedef gtsam::RangeFactor RangeFactorPosePoint2; - TemplateInstantiationTypedef singleInstantiation0; + TypeGrammar instantiationClass_g(singleInstantiation.class_); Rule templateSingleInstantiation_p = - (str_p("typedef") >> - *(namespace_name_p[push_back_a(singleInstantiation.class_.namespaces)] >> str_p("::")) >> - className_p[assign_a(singleInstantiation.class_.name)] >> - '<' >> templateSingleInstantiationArg_p >> *(',' >> templateSingleInstantiationArg_p) >> - '>' >> - className_p[assign_a(singleInstantiation.name)] >> + (str_p("typedef") >> instantiationClass_g >> + typelist_g >> + basic.className_p[assign_a(singleInstantiation.name_)] >> ';') - [assign_a(singleInstantiation.namespaces, namespaces)] + [assign_a(singleInstantiation.namespaces_, namespaces)] [push_back_a(templateInstantiationTypedefs, singleInstantiation)] [assign_a(singleInstantiation, singleInstantiation0)]; - // template - Rule templateList_p = - (str_p("template") >> - '<' >> name_p[push_back_a(cls.templateArgs)] >> *(',' >> name_p[push_back_a(cls.templateArgs)]) >> - '>'); - - // NOTE: allows for pointers to all types - ArgumentList args; - Rule argument_p = - ((basisType_p[assign_a(arg.type.name)] | argEigenType_p | eigenRef_p | classArg_p) - >> !ch_p('*')[assign_a(arg.is_ptr,true)] - >> name_p[assign_a(arg.name)]) - [push_back_a(args, arg)] - [assign_a(arg,arg0)]; - - Rule argumentList_p = !argument_p >> * (',' >> argument_p); - - // parse class constructor - Constructor constructor0(verbose), constructor(verbose); - Rule constructor_p = - (className_p >> '(' >> argumentList_p >> ')' >> ';' >> !comments_p) - [bl::bind(&Constructor::push_back, bl::var(constructor), bl::var(args))] - [clear_a(args)]; - - vector namespaces_return; /// namespace for current return type - Rule namespace_ret_p = namespace_name_p[push_back_a(namespaces_return)] >> str_p("::"); - - // HACK: use const values instead of using enums themselves - somehow this doesn't result in values getting assigned to gibberish - static const ReturnType::return_category RETURN_EIGEN = ReturnType::EIGEN; - static const ReturnType::return_category RETURN_BASIS = ReturnType::BASIS; - static const ReturnType::return_category RETURN_CLASS = ReturnType::CLASS; - static const ReturnType::return_category RETURN_VOID = ReturnType::VOID; - - ReturnType retType0, retType; - Rule returnType_p = - (basisType_p[assign_a(retType.name)][assign_a(retType.category, RETURN_BASIS)]) | - ((*namespace_ret_p)[assign_a(retType.namespaces, namespaces_return)][clear_a(namespaces_return)] - >> (className_p[assign_a(retType.name)][assign_a(retType.category, RETURN_CLASS)]) >> - !ch_p('*')[assign_a(retType.isPtr,true)]) | - (eigenType_p[assign_a(retType.name)][assign_a(retType.category, RETURN_EIGEN)]); - - ReturnValue retVal0, retVal; - Rule returnType1_p = returnType_p[assign_a(retVal.type1,retType)][assign_a(retType,retType0)]; - Rule returnType2_p = returnType_p[assign_a(retVal.type2,retType)][assign_a(retType,retType0)]; - - Rule pair_p = - (str_p("pair") >> '<' >> returnType1_p >> ',' >> returnType2_p >> '>') - [assign_a(retVal.isPair,true)]; - - Rule void_p = str_p("void")[assign_a(retVal.type1.name)][assign_a(retVal.type1.category, RETURN_VOID)]; - - Rule returnValue_p = void_p | pair_p | returnType1_p; - - Rule methodName_p = lexeme_d[(upper_p | lower_p) >> *(alnum_p | '_')]; - - // gtsam::Values retract(const gtsam::VectorValues& delta) const; - string methodName; - bool isConst, isConst0 = false; - Rule method_p = - !templateArgValues_p >> - (returnValue_p >> methodName_p[assign_a(methodName)] >> - '(' >> argumentList_p >> ')' >> - !str_p("const")[assign_a(isConst,true)] >> ';' >> *comments_p) - [bl::bind(&Class::addMethod, bl::var(cls), verbose, bl::var(isConst), - bl::var(methodName), bl::var(args), bl::var(retVal), - bl::var(templateArgName), bl::var(templateArgValues))] - [assign_a(retVal,retVal0)] - [clear_a(args)] - [clear_a(templateArgValues)] - [assign_a(isConst,isConst0)]; - - Rule staticMethodName_p = lexeme_d[(upper_p | lower_p) >> *(alnum_p | '_')]; - - Rule static_method_p = - (str_p("static") >> returnValue_p >> staticMethodName_p[assign_a(methodName)] >> - '(' >> argumentList_p >> ')' >> ';' >> *comments_p) - [bl::bind(&StaticMethod::addOverload, - bl::var(cls.static_methods)[bl::var(methodName)], - bl::var(methodName), bl::var(args), bl::var(retVal), Qualified(),verbose)] - [assign_a(retVal,retVal0)] - [clear_a(args)]; - - Rule functions_p = constructor_p | method_p | static_method_p; - - // parse a full class - vector templateInstantiations; - Rule class_p = - eps_p[assign_a(cls,cls0)] - >> (!(templateArgValues_p - [push_back_a(cls.templateArgs, templateArgName)] - [assign_a(templateInstantiations,templateArgValues)] - [clear_a(templateArgValues)] - | templateList_p) - >> !(str_p("virtual")[assign_a(cls.isVirtual, true)]) - >> str_p("class") - >> className_p[assign_a(cls.name)] - >> ((':' >> classParent_p >> '{') | '{') - >> *(functions_p | comments_p) - >> str_p("};")) - [bl::bind(&Constructor::initializeOrCheck, bl::var(constructor), - bl::var(cls.name), Qualified(), verbose)] - [assign_a(cls.constructor, constructor)] - [assign_a(cls.namespaces, namespaces)] - [assign_a(cls.deconstructor.name,cls.name)] - [bl::bind(&handle_possible_template, bl::var(classes), bl::var(cls), - bl::var(templateInstantiations))] - [clear_a(templateInstantiations)] - [assign_a(constructor, constructor0)] - [assign_a(cls,cls0)]; - - // parse a global function - Qualified globalFunction; - Rule global_function_p = - (returnValue_p >> staticMethodName_p[assign_a(globalFunction.name)] >> - '(' >> argumentList_p >> ')' >> ';' >> *comments_p) - [assign_a(globalFunction.namespaces,namespaces)] - [bl::bind(&GlobalFunction::addOverload, - bl::var(global_functions)[bl::var(globalFunction.name)], - bl::var(globalFunction), bl::var(args), bl::var(retVal), Qualified(),verbose)] - [assign_a(retVal,retVal0)] - [clear_a(globalFunction)] - [clear_a(args)]; + // Create grammar for global functions + GlobalFunctionGrammar global_function_g(global_functions,namespaces); Rule include_p = str_p("#include") >> ch_p('<') >> (*(anychar_p - '>'))[push_back_a(includes)] >> ch_p('>'); @@ -328,9 +138,9 @@ void Module::parseMarkup(const std::string& data) { Rule namespace_def_p = (str_p("namespace") - >> namespace_name_p[push_back_a(namespaces)] + >> basic.namespace_p[push_back_a(namespaces)] >> ch_p('{') - >> *(include_p | class_p | templateSingleInstantiation_p | global_function_p | namespace_def_p | comments_p) + >> *(include_p | class_p | templateSingleInstantiation_p | global_function_g | namespace_def_p | basic.comments_p) >> ch_p('}')) [pop_a(namespaces)]; @@ -343,47 +153,23 @@ void Module::parseMarkup(const std::string& data) { Rule forward_declaration_p = !(str_p("virtual")[assign_a(fwDec.isVirtual, true)]) >> str_p("class") - >> (*(namespace_name_p >> str_p("::")) >> className_p)[assign_a(fwDec.name)] + >> (*(basic.namespace_p >> str_p("::")) >> basic.className_p)[assign_a(fwDec.name)] >> ch_p(';') [push_back_a(forward_declarations, fwDec)] [assign_a(fwDec, fwDec0)]; - Rule module_content_p = comments_p | include_p | class_p | templateSingleInstantiation_p | forward_declaration_p | global_function_p | namespace_def_p; + Rule module_content_p = basic.comments_p | include_p | class_p + | templateSingleInstantiation_p | forward_declaration_p + | global_function_g | namespace_def_p; Rule module_p = *module_content_p >> !end_p; - //---------------------------------------------------------------------------- - // for debugging, define BOOST_SPIRIT_DEBUG -# ifdef BOOST_SPIRIT_DEBUG - BOOST_SPIRIT_DEBUG_NODE(className_p); - BOOST_SPIRIT_DEBUG_NODE(classPtr_p); - BOOST_SPIRIT_DEBUG_NODE(classRef_p); - BOOST_SPIRIT_DEBUG_NODE(basisType_p); - BOOST_SPIRIT_DEBUG_NODE(name_p); - BOOST_SPIRIT_DEBUG_NODE(argument_p); - BOOST_SPIRIT_DEBUG_NODE(argumentList_p); - BOOST_SPIRIT_DEBUG_NODE(constructor_p); - BOOST_SPIRIT_DEBUG_NODE(returnType1_p); - BOOST_SPIRIT_DEBUG_NODE(returnType2_p); - BOOST_SPIRIT_DEBUG_NODE(pair_p); - BOOST_SPIRIT_DEBUG_NODE(void_p); - BOOST_SPIRIT_DEBUG_NODE(returnValue_p); - BOOST_SPIRIT_DEBUG_NODE(methodName_p); - BOOST_SPIRIT_DEBUG_NODE(method_p); - BOOST_SPIRIT_DEBUG_NODE(class_p); - BOOST_SPIRIT_DEBUG_NODE(namespace_def_p); - BOOST_SPIRIT_DEBUG_NODE(module_p); -# endif - //---------------------------------------------------------------------------- - // and parse contents parse_info info = parse(data.c_str(), module_p, space_p); if(!info.full) { printf("parsing stopped at \n%.20s\n",info.stop); - cout << "Stopped near:\n" - "class '" << cls.name << "'\n" - "method '" << methodName << "'\n" - "argument '" << arg.name << "'" << endl; + cout << "Stopped in:\n" + "class '" << cls.name_ << "'" << endl; throw ParseFailed((int)info.length); } @@ -416,6 +202,11 @@ void Module::parseMarkup(const std::string& data) { // Create type attributes table and check validity typeAttributes.addClasses(expandedClasses); typeAttributes.addForwardDeclarations(forward_declarations); + // add Eigen types as template arguments are also checked ? + vector eigen; + eigen.push_back(ForwardDeclaration("Vector")); + eigen.push_back(ForwardDeclaration("Matrix")); + typeAttributes.addForwardDeclarations(eigen); typeAttributes.checkValidity(expandedClasses); } @@ -546,7 +337,7 @@ vector Module::ExpandTypedefInstantiations(const vector& classes, vector Module::GenerateValidTypes(const vector& classes, const vector forwardDeclarations) { vector validTypes; BOOST_FOREACH(const ForwardDeclaration& fwDec, forwardDeclarations) { - validTypes.push_back(fwDec.name); + validTypes.push_back(fwDec.name); } validTypes.push_back("void"); validTypes.push_back("string"); diff --git a/wrap/Module.h b/wrap/Module.h index a4659dc3a..e0c1b3f31 100644 --- a/wrap/Module.h +++ b/wrap/Module.h @@ -18,15 +18,15 @@ #pragma once -#include -#include -#include - #include "Class.h" #include "GlobalFunction.h" #include "TemplateInstantiationTypedef.h" #include "ForwardDeclaration.h" +#include +#include +#include + namespace wrap { /** @@ -34,9 +34,6 @@ namespace wrap { */ struct Module { - typedef std::map GlobalFunctions; - typedef std::map Methods; - // Filled during parsing: std::string name; ///< module name bool verbose; ///< verbose flag diff --git a/wrap/OverloadedFunction.h b/wrap/OverloadedFunction.h index 47c418748..7718bc139 100644 --- a/wrap/OverloadedFunction.h +++ b/wrap/OverloadedFunction.h @@ -87,7 +87,8 @@ class OverloadedFunction: public Function, public ArgumentOverloads { public: bool addOverload(const std::string& name, const ArgumentList& args, - const Qualified& instName = Qualified(), bool verbose = false) { + boost::optional instName = boost::none, bool verbose = + false) { bool first = initializeOrCheck(name, instName, verbose); ArgumentOverloads::push_back(args); return first; diff --git a/wrap/Qualified.h b/wrap/Qualified.h index b23e9020d..bcc4c0829 100644 --- a/wrap/Qualified.h +++ b/wrap/Qualified.h @@ -18,6 +18,7 @@ #pragma once +#include #include #include @@ -26,43 +27,122 @@ namespace wrap { /** * Class to encapuslate a qualified name, i.e., with (nested) namespaces */ -struct Qualified { +class Qualified { - std::vector namespaces; ///< Stack of namespaces - std::string name; ///< type name +//protected: +public: - Qualified(const std::string& name_ = "") : - name(name_) { + std::vector namespaces_; ///< Stack of namespaces + std::string name_; ///< type name + + friend struct TypeGrammar; + friend class TemplateSubstitution; + +public: + + /// the different categories + typedef enum { + CLASS = 1, EIGEN = 2, BASIS = 3, VOID = 4 + } Category; + Category category; + + Qualified() : + category(VOID) { + } + + Qualified(const std::string& n, Category c = CLASS) : + name_(n), category(c) { + } + + Qualified(const std::string& ns1, const std::string& ns2, + const std::string& n, Category c = CLASS) : + name_(n), category(c) { + namespaces_.push_back(ns1); + namespaces_.push_back(ns2); + } + + Qualified(const std::string& ns1, const std::string& n, Category c = CLASS) : + name_(n), category(c) { + namespaces_.push_back(ns1); + } + + Qualified(std::vector ns, const std::string& name) : + namespaces_(ns), name_(name), category(CLASS) { + } + + std::string name() const { + return name_; + } + + std::vector namespaces() const { + return namespaces_; + } + + // Qualified is 'abused' as template argument name as well + // this function checks whether *this matches with templateArg + bool match(const std::string& templateArg) const { + return (name_ == templateArg && namespaces_.empty()); //TODO && category == CLASS); + } + + void rename(const Qualified& q) { + namespaces_ = q.namespaces_; + name_ = q.name_; + category = q.category; + } + + void expand(const std::string& expansion) { + name_ += expansion; + } + + bool operator==(const Qualified& other) const { + return namespaces_ == other.namespaces_ && name_ == other.name_ + && category == other.category; } bool empty() const { - return namespaces.empty() && name.empty(); + return namespaces_.empty() && name_.empty(); } - void clear() { - namespaces.clear(); - name.clear(); + virtual void clear() { + namespaces_.clear(); + name_.clear(); + category = VOID; } - bool operator!=(const Qualified& other) const { - return other.name != name || other.namespaces != namespaces; +public: + + static Qualified MakeClass(std::vector namespaces, + const std::string& name) { + return Qualified(namespaces, name); + } + + static Qualified MakeEigen(const std::string& name) { + return Qualified(name, EIGEN); + } + + static Qualified MakeBasis(const std::string& name) { + return Qualified(name, BASIS); + } + + static Qualified MakeVoid() { + return Qualified("void", VOID); } /// Return a qualified string using given delimiter std::string qualifiedName(const std::string& delimiter = "") const { std::string result; - for (std::size_t i = 0; i < namespaces.size(); ++i) - result += (namespaces[i] + delimiter); - result += name; + for (std::size_t i = 0; i < namespaces_.size(); ++i) + result += (namespaces_[i] + delimiter); + result += name_; return result; } /// Return a matlab file name, i.e. "toolboxPath/+ns1/+ns2/name.m" std::string matlabName(const std::string& toolboxPath) const { std::string result = toolboxPath; - for (std::size_t i = 0; i < namespaces.size(); ++i) - result += ("/+" + namespaces[i]); - result += "/" + name + ".m"; + for (std::size_t i = 0; i < namespaces_.size(); ++i) + result += ("/+" + namespaces_[i]); + result += "/" + name_ + ".m"; return result; } @@ -73,5 +153,109 @@ struct Qualified { }; +/* ************************************************************************* */ +// http://boost-spirit.com/distrib/spirit_1_8_2/libs/spirit/doc/grammar.html +struct TypeGrammar: classic::grammar { + + wrap::Qualified& result_; ///< successful parse will be placed in here + + /// Construct type grammar and specify where result is placed + TypeGrammar(wrap::Qualified& result) : + result_(result) { + } + + /// Definition of type grammar + template + struct definition: BasicRules { + + typedef classic::rule Rule; + + Rule void_p, basisType_p, eigenType_p, namespace_del_p, class_p, type_p; + + definition(TypeGrammar const& self) { + + using namespace wrap; + using namespace classic; + typedef BasicRules Basic; + + // HACK: use const values instead of using enums themselves - somehow this doesn't result in values getting assigned to gibberish + static const Qualified::Category EIGEN = Qualified::EIGEN; + static const Qualified::Category BASIS = Qualified::BASIS; + static const Qualified::Category CLASS = Qualified::CLASS; + static const Qualified::Category VOID = Qualified::VOID; + + void_p = str_p("void") // + [assign_a(self.result_.name_)] // + [assign_a(self.result_.category, VOID)]; + + basisType_p = Basic::basisType_p // + [assign_a(self.result_.name_)] // + [assign_a(self.result_.category, BASIS)]; + + eigenType_p = Basic::eigenType_p // + [assign_a(self.result_.name_)] // + [assign_a(self.result_.category, EIGEN)]; + + namespace_del_p = Basic::namespace_p // + [push_back_a(self.result_.namespaces_)] >> str_p("::"); + + class_p = *namespace_del_p >> Basic::className_p // + [assign_a(self.result_.name_)] // + [assign_a(self.result_.category, CLASS)]; + + type_p = void_p | basisType_p | class_p | eigenType_p; + } + + Rule const& start() const { + return type_p; + } + + }; +}; +// type_grammar + +/* ************************************************************************* */ +// http://boost-spirit.com/distrib/spirit_1_8_2/libs/spirit/doc/grammar.html +template +struct TypeListGrammar: public classic::grammar > { + + typedef std::vector TypeList; + TypeList& result_; ///< successful parse will be placed in here + + /// Construct type grammar and specify where result is placed + TypeListGrammar(TypeList& result) : + result_(result) { + } + + /// Definition of type grammar + template + struct definition { + + wrap::Qualified type; ///< temporary for use during parsing + TypeGrammar type_g; ///< Individual Type grammars + + classic::rule type_p, typeList_p; + + definition(TypeListGrammar const& self) : + type_g(type) { + using namespace classic; + + type_p = type_g[push_back_a(self.result_, type)][clear_a(type)]; + + typeList_p = OPEN >> !type_p >> *(',' >> type_p) >> CLOSE; + } + + classic::rule const& start() const { + return typeList_p; + } + + }; +}; +// TypeListGrammar + +/* ************************************************************************* */ +// Needed for other parsers in Argument.h and ReturnType.h +static const bool T = true; + } // \namespace wrap diff --git a/wrap/ReturnType.cpp b/wrap/ReturnType.cpp index a317a5420..41fd51680 100644 --- a/wrap/ReturnType.cpp +++ b/wrap/ReturnType.cpp @@ -14,7 +14,7 @@ using namespace wrap; /* ************************************************************************* */ string ReturnType::str(bool add_ptr) const { - return maybe_shared_ptr(add_ptr && isPtr, qualifiedName("::"), name); + return maybe_shared_ptr(add_ptr && isPtr, qualifiedName("::"), name()); } /* ************************************************************************* */ @@ -24,37 +24,46 @@ void ReturnType::wrap_result(const string& out, const string& result, string cppType = qualifiedName("::"), matlabType = qualifiedName("."); if (category == CLASS) { + + // Handle Classes string objCopy, ptrType; - ptrType = "Shared" + name; - const bool isVirtual = typeAttributes.at(cppType).isVirtual; - if (isVirtual) { - if (isPtr) - objCopy = result; - else + const bool isVirtual = typeAttributes.attributes(cppType).isVirtual; + if (isPtr) + objCopy = result; // a shared pointer can always be passed as is + else { + // but if we want an actual new object, things get more complex + if (isVirtual) + // A virtual class needs to be cloned, so the whole hierarchy is returned objCopy = result + ".clone()"; - } else { - if (isPtr) - objCopy = result; else - objCopy = ptrType + "(new " + cppType + "(" + result + "))"; + // ...but a non-virtual class can just be copied + objCopy = "Shared" + name() + "(new " + cppType + "(" + result + "))"; } + // e.g. out[1] = wrap_shared_ptr(pairResult.second,"gtsam.Point3", false); wrapperFile.oss << out << " = wrap_shared_ptr(" << objCopy << ",\"" << matlabType << "\", " << (isVirtual ? "true" : "false") << ");\n"; + } else if (isPtr) { - wrapperFile.oss << " Shared" << name << "* ret = new Shared" << name << "(" - << result << ");" << endl; + + // Handle shared pointer case for BASIS/EIGEN/VOID + wrapperFile.oss << " {\n Shared" << name() << "* ret = new Shared" << name() + << "(" << result << ");" << endl; wrapperFile.oss << out << " = wrap_shared_ptr(ret,\"" << matlabType - << "\");\n"; + << "\");\n }\n"; + } else if (matlabType != "void") + + // Handle normal case case for BASIS/EIGEN wrapperFile.oss << out << " = wrap< " << str(false) << " >(" << result << ");\n"; + } /* ************************************************************************* */ void ReturnType::wrapTypeUnwrap(FileWriter& wrapperFile) const { if (category == CLASS) wrapperFile.oss << " typedef boost::shared_ptr<" << qualifiedName("::") - << "> Shared" << name << ";" << endl; + << "> Shared" << name() << ";" << endl; } /* ************************************************************************* */ diff --git a/wrap/ReturnType.h b/wrap/ReturnType.h index 1829fbc81..1c67a1d9a 100644 --- a/wrap/ReturnType.h +++ b/wrap/ReturnType.h @@ -9,6 +9,7 @@ #include "FileWriter.h" #include "TypeAttributesTable.h" #include "utilities.h" +#include #pragma once @@ -17,28 +18,25 @@ namespace wrap { /** * Encapsulates return value of a method or function */ -struct ReturnType: Qualified { - - /// the different supported return value categories - typedef enum { - CLASS = 1, EIGEN = 2, BASIS = 3, VOID = 4 - } return_category; +struct ReturnType: public Qualified { bool isPtr; - return_category category; + friend struct ReturnValueGrammar; + + /// Makes a void type ReturnType() : - isPtr(false), category(CLASS) { + isPtr(false) { } - ReturnType(const std::string& name) : - isPtr(false), category(CLASS) { - Qualified::name = name; + /// Constructor, no namespaces + ReturnType(const std::string& name, Category c = CLASS, bool ptr = false) : + Qualified(name, c), isPtr(ptr) { } - void rename(const Qualified& q) { - name = q.name; - namespaces = q.namespaces; + virtual void clear() { + Qualified::clear(); + isPtr = false; } /// Check if this type is in a set of valid types @@ -64,4 +62,36 @@ private: }; +//****************************************************************************** +// http://boost-spirit.com/distrib/spirit_1_8_2/libs/spirit/doc/grammar.html +struct ReturnTypeGrammar: public classic::grammar { + + wrap::ReturnType& result_; ///< successful parse will be placed in here + + TypeGrammar type_g; + + /// Construct ReturnType grammar and specify where result is placed + ReturnTypeGrammar(wrap::ReturnType& result) : + result_(result), type_g(result_) { + } + + /// Definition of type grammar + template + struct definition { + + classic::rule type_p; + + definition(ReturnTypeGrammar const& self) { + using namespace classic; + type_p = self.type_g >> !ch_p('*')[assign_a(self.result_.isPtr, T)]; + } + + classic::rule const& start() const { + return type_p; + } + + }; +}; +// ReturnTypeGrammar + } // \namespace wrap diff --git a/wrap/ReturnValue.cpp b/wrap/ReturnValue.cpp index 54e585cad..596acb2c3 100644 --- a/wrap/ReturnValue.cpp +++ b/wrap/ReturnValue.cpp @@ -17,9 +17,9 @@ using namespace wrap; /* ************************************************************************* */ ReturnValue ReturnValue::expandTemplate(const TemplateSubstitution& ts) const { ReturnValue instRetVal = *this; - instRetVal.type1 = ts(type1); + instRetVal.type1 = ts.tryToSubstitite(type1); if (isPair) - instRetVal.type2 = ts(type2); + instRetVal.type2 = ts.tryToSubstitite(type2); return instRetVal; } diff --git a/wrap/ReturnValue.h b/wrap/ReturnValue.h index abfcec2b0..629684a34 100644 --- a/wrap/ReturnValue.h +++ b/wrap/ReturnValue.h @@ -25,6 +25,8 @@ struct ReturnValue { bool isPair; ReturnType type1, type2; + friend struct ReturnValueGrammar; + /// Constructor ReturnValue() : isPair(false) { @@ -35,6 +37,22 @@ struct ReturnValue { isPair(false), type1(type) { } + /// Constructor + ReturnValue(const ReturnType& t1, const ReturnType& t2) : + isPair(true), type1(t1), type2(t2) { + } + + virtual void clear() { + type1.clear(); + type2.clear(); + isPair = false; + } + + bool operator==(const ReturnValue& other) const { + return isPair == other.isPair && type1 == other.type1 + && type2 == other.type2; + } + /// Substitute template argument ReturnValue expandTemplate(const TemplateSubstitution& ts) const; @@ -59,4 +77,39 @@ struct ReturnValue { }; -} // \namespace wrap +//****************************************************************************** +// http://boost-spirit.com/distrib/spirit_1_8_2/libs/spirit/doc/grammar.html +struct ReturnValueGrammar: public classic::grammar { + + wrap::ReturnValue& result_; ///< successful parse will be placed in here + ReturnTypeGrammar returnType1_g, returnType2_g; ///< Type parsers + + /// Construct type grammar and specify where result is placed + ReturnValueGrammar(wrap::ReturnValue& result) : + result_(result), returnType1_g(result.type1), returnType2_g(result.type2) { + } + + /// Definition of type grammar + template + struct definition { + + classic::rule pair_p, returnValue_p; + + definition(ReturnValueGrammar const& self) { + using namespace classic; + + pair_p = (str_p("pair") >> '<' >> self.returnType1_g >> ',' + >> self.returnType2_g >> '>')[assign_a(self.result_.isPair, T)]; + + returnValue_p = pair_p | self.returnType1_g; + } + + classic::rule const& start() const { + return returnValue_p; + } + + }; +}; +// ReturnValueGrammar + +}// \namespace wrap diff --git a/wrap/StaticMethod.cpp b/wrap/StaticMethod.cpp index 5f91a15b4..e4e7c89ae 100644 --- a/wrap/StaticMethod.cpp +++ b/wrap/StaticMethod.cpp @@ -16,7 +16,7 @@ * @author Richard Roberts **/ -#include "Method.h" +#include "StaticMethod.h" #include "utilities.h" #include @@ -36,117 +36,9 @@ void StaticMethod::proxy_header(FileWriter& proxyFile) const { proxyFile.oss << " function varargout = " << upperName << "(varargin)\n"; } -/* ************************************************************************* */ -void StaticMethod::proxy_wrapper_fragments(FileWriter& proxyFile, - FileWriter& wrapperFile, Str cppClassName, Str matlabQualName, - Str matlabUniqueName, Str wrapperName, - const TypeAttributesTable& typeAttributes, - vector& functionNames) const { - - // emit header, e.g., function varargout = templatedMethod(this, varargin) - proxy_header(proxyFile); - - // Emit comments for documentation - string up_name = boost::to_upper_copy(matlabName()); - proxyFile.oss << " % " << up_name << " usage: "; - usage_fragment(proxyFile, matlabName()); - - // Emit URL to Doxygen page - proxyFile.oss << " % " - << "Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html" - << endl; - - // Handle special case of single overload with all numeric arguments - if (nrOverloads() == 1 && argumentList(0).allScalar()) { - // Output proxy matlab code - // TODO: document why is it OK to not check arguments in this case - proxyFile.oss << " "; - const int id = (int) functionNames.size(); - argumentList(0).emit_call(proxyFile, returnValue(0), wrapperName, id, - isStatic()); - - // Output C++ wrapper code - const string wrapFunctionName = wrapper_fragment(wrapperFile, cppClassName, - matlabUniqueName, 0, id, typeAttributes, templateArgValue_); - - // Add to function list - functionNames.push_back(wrapFunctionName); - } else { - // Check arguments for all overloads - for (size_t i = 0; i < nrOverloads(); ++i) { - - // Output proxy matlab code - proxyFile.oss << " " << (i == 0 ? "" : "else"); - const int id = (int) functionNames.size(); - argumentList(i).emit_conditional_call(proxyFile, returnValue(i), - wrapperName, id, isStatic()); - - // Output C++ wrapper code - const string wrapFunctionName = wrapper_fragment(wrapperFile, - cppClassName, matlabUniqueName, i, id, typeAttributes, - templateArgValue_); - - // Add to function list - functionNames.push_back(wrapFunctionName); - } - proxyFile.oss << " else\n"; - proxyFile.oss - << " error('Arguments do not match any overload of function " - << matlabQualName << "." << name_ << "');" << endl; - proxyFile.oss << " end\n"; - } - - proxyFile.oss << " end\n"; -} - -/* ************************************************************************* */ -string StaticMethod::wrapper_fragment(FileWriter& wrapperFile, Str cppClassName, - Str matlabUniqueName, int overload, int id, - const TypeAttributesTable& typeAttributes, - const Qualified& instName) const { - - // generate code - - const string wrapFunctionName = matlabUniqueName + "_" + name_ + "_" - + boost::lexical_cast(id); - - const ArgumentList& args = argumentList(overload); - const ReturnValue& returnVal = returnValue(overload); - - // call - wrapperFile.oss << "void " << wrapFunctionName - << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n"; - // start - wrapperFile.oss << "{\n"; - - returnVal.wrapTypeUnwrap(wrapperFile); - - wrapperFile.oss << " typedef boost::shared_ptr<" << cppClassName - << "> Shared;" << endl; - - // get call - // for static methods: cppClassName::staticMethod - // for instance methods: obj->instanceMethod - string expanded = wrapper_call(wrapperFile, cppClassName, matlabUniqueName, - args, returnVal, typeAttributes, instName); - - expanded += ("(" + args.names() + ")"); - if (returnVal.type1.name != "void") - returnVal.wrap_result(expanded, wrapperFile, typeAttributes); - else - wrapperFile.oss << " " + expanded + ";\n"; - - // finish - wrapperFile.oss << "}\n"; - - return wrapFunctionName; -} - /* ************************************************************************* */ string StaticMethod::wrapper_call(FileWriter& wrapperFile, Str cppClassName, - Str matlabUniqueName, const ArgumentList& args, - const ReturnValue& returnVal, const TypeAttributesTable& typeAttributes, - const Qualified& instName) const { + Str matlabUniqueName, const ArgumentList& args) const { // check arguments // NOTE: for static functions, there is no object passed wrapperFile.oss << " checkArguments(\"" << matlabUniqueName << "." << name_ @@ -158,17 +50,10 @@ string StaticMethod::wrapper_call(FileWriter& wrapperFile, Str cppClassName, // call method and wrap result // example: out[0]=wrap(staticMethod(t)); string expanded = cppClassName + "::" + name_; - if (!instName.empty()) - expanded += ("<" + instName.qualifiedName("::") + ">"); + if (templateArgValue_) + expanded += ("<" + templateArgValue_->qualifiedName("::") + ">"); return expanded; } /* ************************************************************************* */ -void StaticMethod::python_wrapper(FileWriter& wrapperFile, - Str className) const { - wrapperFile.oss << " .def(\"" << name_ << "\", &" << className << "::" << name_ - << ");\n"; -} - -/* ************************************************************************* */ diff --git a/wrap/StaticMethod.h b/wrap/StaticMethod.h index de8e4a94e..a01eeff62 100644 --- a/wrap/StaticMethod.h +++ b/wrap/StaticMethod.h @@ -19,42 +19,15 @@ #pragma once -#include "FullyOverloadedFunction.h" +#include "MethodBase.h" namespace wrap { /// StaticMethod class -struct StaticMethod: public FullyOverloadedFunction { +struct StaticMethod: public MethodBase { typedef const std::string& Str; - virtual bool isStatic() const { - return true; - } - - // emit a list of comments, one for each overload - void comment_fragment(FileWriter& proxyFile) const { - SignatureOverloads::comment_fragment(proxyFile, matlabName()); - } - - void verifyArguments(const std::vector& validArgs) const { - SignatureOverloads::verifyArguments(validArgs, name_); - } - - void verifyReturnTypes(const std::vector& validtypes) const { - SignatureOverloads::verifyReturnTypes(validtypes, name_); - } - - // MATLAB code generation - // classPath is class directory, e.g., ../matlab/@Point2 - void proxy_wrapper_fragments(FileWriter& proxyFile, FileWriter& wrapperFile, - Str cppClassName, Str matlabQualName, Str matlabUniqueName, - Str wrapperName, const TypeAttributesTable& typeAttributes, - std::vector& functionNames) const; - - // emit python wrapper - void python_wrapper(FileWriter& wrapperFile, Str className) const; - friend std::ostream& operator<<(std::ostream& os, const StaticMethod& m) { for (size_t i = 0; i < m.nrOverloads(); i++) os << "static " << m.returnVals_[i] << " " << m.name_ << m.argLists_[i]; @@ -65,15 +38,8 @@ protected: virtual void proxy_header(FileWriter& proxyFile) const; - std::string wrapper_fragment(FileWriter& wrapperFile, Str cppClassName, - Str matlabUniqueName, int overload, int id, - const TypeAttributesTable& typeAttributes, const Qualified& instName = - Qualified()) const; ///< cpp wrapper - virtual std::string wrapper_call(FileWriter& wrapperFile, Str cppClassName, - Str matlabUniqueName, const ArgumentList& args, - const ReturnValue& returnVal, const TypeAttributesTable& typeAttributes, - const Qualified& instName) const; + Str matlabUniqueName, const ArgumentList& args) const; }; } // \namespace wrap diff --git a/wrap/Template.h b/wrap/Template.h new file mode 100644 index 000000000..5a64412ed --- /dev/null +++ b/wrap/Template.h @@ -0,0 +1,102 @@ +/* ---------------------------------------------------------------------------- + + * 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 Template.h + * @brief Template name + * @author Frank Dellaert + * @date Nov 11, 2014 + **/ + +#pragma once + +#include + +namespace wrap { + +/// The template specification that goes before a method or a class +class Template { + std::string argName_; + std::vector argValues_; + friend struct TemplateGrammar; +public: + /// The only way to get values into a Template is via our friendly Grammar + Template() { + } + void clear() { + argName_.clear(); + argValues_.clear(); + } + const std::string& argName() const { + return argName_; + } + const std::vector& argValues() const { + return argValues_; + } + size_t nrValues() const { + return argValues_.size(); + } + const Qualified& operator[](size_t i) const { + return argValues_[i]; + } + bool valid() const { + return !argName_.empty() && argValues_.size() > 0; + } + +}; + +/* ************************************************************************* */ +// http://boost-spirit.com/distrib/spirit_1_8_2/libs/spirit/doc/grammar.html +struct TemplateGrammar: public classic::grammar { + + Template& result_; ///< successful parse will be placed in here + TypeListGrammar<'{', '}'> argValues_g; ///< TypeList parser + + /// Construct type grammar and specify where result is placed + TemplateGrammar(Template& result) : + result_(result), argValues_g(result.argValues_) { + } + + /// Definition of type grammar + template + struct definition: BasicRules { + + classic::rule templateArgValues_p; + + definition(TemplateGrammar const& self) { + using classic::str_p; + using classic::assign_a; + templateArgValues_p = (str_p("template") >> '<' + >> (BasicRules::name_p)[assign_a(self.result_.argName_)] + >> '=' >> self.argValues_g >> '>'); + } + + classic::rule const& start() const { + return templateArgValues_p; + } + + }; +}; +// TemplateGrammar + +/// Cool initializer for tests +static inline boost::optional