diff --git a/.cproject b/.cproject index 241afea81..617aa3795 100644 --- a/.cproject +++ b/.cproject @@ -592,6 +592,7 @@ make + tests/testBayesTree.run true false @@ -599,6 +600,7 @@ make + testBinaryBayesNet.run true false @@ -646,6 +648,7 @@ make + testSymbolicBayesNet.run true false @@ -653,6 +656,7 @@ make + tests/testSymbolicFactor.run true false @@ -660,6 +664,7 @@ make + testSymbolicFactorGraph.run true false @@ -675,6 +680,7 @@ make + tests/testBayesTree true false @@ -848,6 +854,14 @@ true true + + make + -j4 + testSmartStereoProjectionPoseFactor.run + true + true + true + make -j5 @@ -1122,6 +1136,7 @@ make + testErrors.run true false @@ -1351,6 +1366,46 @@ true true + + make + -j5 + testBTree.run + true + true + true + + + make + -j5 + testDSF.run + true + true + true + + + make + -j5 + testDSFMap.run + true + true + true + + + make + -j5 + testDSFVector.run + true + true + true + + + make + -j5 + testFixedVector.run + true + true + true + make -j2 @@ -1433,7 +1488,6 @@ make - testSimulated2DOriented.run true false @@ -1473,7 +1527,6 @@ make - testSimulated2D.run true false @@ -1481,7 +1534,6 @@ make - testSimulated3D.run true false @@ -1495,46 +1547,6 @@ true true - - make - -j5 - testBTree.run - true - true - true - - - make - -j5 - testDSF.run - true - true - true - - - make - -j5 - testDSFMap.run - true - true - true - - - make - -j5 - testDSFVector.run - true - true - true - - - make - -j5 - testFixedVector.run - true - true - true - make -j5 @@ -1792,6 +1804,7 @@ cpack + -G DEB true false @@ -1799,6 +1812,7 @@ cpack + -G RPM true false @@ -1806,6 +1820,7 @@ cpack + -G TGZ true false @@ -1813,6 +1828,7 @@ cpack + --config CPackSourceConfig.cmake true false @@ -2425,6 +2441,14 @@ true true + + make + -j4 + testGlobalFunction.run + true + true + true + make -j5 @@ -2659,6 +2683,7 @@ make + testGraph.run true false @@ -2666,6 +2691,7 @@ make + testJunctionTree.run true false @@ -2673,6 +2699,7 @@ make + testSymbolicBayesNetB.run true false @@ -2934,6 +2961,14 @@ true true + + make + -j4 + testRangeFactor.run + true + true + true + make -j2 @@ -3216,7 +3251,6 @@ make - tests/testGaussianISAM2 true false 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 96d51117a..67b3f2888 100644 --- a/gtsam.h +++ b/gtsam.h @@ -758,10 +758,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); @@ -2198,10 +2194,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 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 100% rename from gtsam_unstable/nonlinear/ceres_rotation.h rename to gtsam/3rdparty/ceres/rotation.h 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/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/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 1655057ca..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,19 +89,21 @@ 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; } }; @@ -111,10 +114,9 @@ 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); } //----------------------------------------------------------------------------- @@ -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; }; /** @@ -569,7 +570,7 @@ struct GenerateFunctionalNode: Argument, Base { }; /// 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 @@ -645,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); @@ -670,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; @@ -715,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; @@ -769,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; @@ -787,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 f0c1751cf..980c47cd1 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), @@ -233,9 +233,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_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/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/tests/testBasisDecompositions.cpp b/gtsam_unstable/nonlinear/tests/testBasisDecompositions.cpp deleted file mode 100644 index 5d6ea2168..000000000 --- a/gtsam_unstable/nonlinear/tests/testBasisDecompositions.cpp +++ /dev/null @@ -1,231 +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 - -namespace gtsam { - -/// 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; - } -}; - -} - -#include -#include -#include -#include - -namespace gtsam { - -/// For now, this is our sequence representation -typedef std::map Sequence; - -/** - * Class that does Fourier Decomposition via least squares - */ -class FourierDecomposition { -public: - - typedef Vector3 Coefficients; ///< Fourier coefficients - -private: - Coefficients c_; - -public: - - /// Create nonlinear FG from Sequence - static NonlinearFactorGraph NonlinearGraph(const Sequence& sequence, - const SharedNoiseModel& model) { - NonlinearFactorGraph graph; - Expression c(0); - typedef std::pair Sample; - BOOST_FOREACH(Sample sample, sequence) { - Expression expression(Fourier<3>(sample.first), c); - ExpressionFactor factor(model, sample.second, expression); - graph.add(factor); - } - return graph; - } - - /// Create linear FG from Sequence - static GaussianFactorGraph::shared_ptr LinearGraph(const Sequence& sequence, - const SharedNoiseModel& model) { - NonlinearFactorGraph graph = NonlinearGraph(sequence, model); - Values values; - values.insert(0, Coefficients::Zero()); // does not matter - GaussianFactorGraph::shared_ptr gfg = graph.linearize(values); - return gfg; - } - - /// Constructor - FourierDecomposition(const Sequence& sequence, - const SharedNoiseModel& model) { - GaussianFactorGraph::shared_ptr gfg = LinearGraph(sequence, model); - VectorValues solution = gfg->optimize(); - c_ = solution.at(0); - } - - /// Return Fourier coefficients - Coefficients coefficients() { - return c_; - } -}; - -} - -#include -#include -#include - -using namespace std; -using namespace gtsam; - -noiseModel::Diagonal::shared_ptr model = noiseModel::Unit::Create(1); - -//****************************************************************************** -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, ManualFourier) { - - // Create linear factor graph - GaussianFactorGraph g; - Key key(1); - Expression c(key); - Values values; - values.insert(key, Vector3::Zero()); // does not matter - 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); - g.add(f1); - - // With ExpressionFactor - Expression expression(Fourier<3>(x), c); - EXPECT_CORRECT_EXPRESSION_JACOBIANS(expression, values, 1e-5, 1e-9); - { - ExpressionFactor f2(model, y, expression); - boost::shared_ptr gf = f2.linearize(values); - boost::shared_ptr jf = // - boost::dynamic_pointer_cast(gf); - CHECK(jf); - EXPECT( assert_equal(f1, *jf, 1e-9)); - } - { - ExpressionFactor f2(model, y, expression); - boost::shared_ptr gf = f2.linearize(values); - boost::shared_ptr jf = // - boost::dynamic_pointer_cast(gf); - CHECK(jf); - EXPECT( assert_equal(f1, *jf, 1e-9)); - } - { - ExpressionFactor f2(model, y, expression); - boost::shared_ptr gf = f2.linearize(values); - boost::shared_ptr jf = // - boost::dynamic_pointer_cast(gf); - CHECK(jf); - EXPECT( assert_equal(f1, *jf, 1e-9)); - } - { - ExpressionFactor f2(model, y, expression); - boost::shared_ptr gf = f2.linearize(values); - boost::shared_ptr jf = // - boost::dynamic_pointer_cast(gf); - CHECK(jf); - EXPECT( assert_equal(f1, *jf, 1e-9)); - } - } - - // Solve - VectorValues actual = g.optimize(); - - // Check - Vector3 expected(1.5661, 1.2717, 1.2717); - EXPECT(assert_equal((Vector) expected, actual.at(key),1e-4)); -} - -//****************************************************************************** -TEST(BasisDecompositions, FourierDecomposition) { - - // Create example sequence - Sequence sequence; - for (size_t i = 0; i < 16; i++) { - double x = i * M_PI / 8, y = exp(sin(x) + cos(x)); - sequence[x] = y; - } - - // Do Fourier Decomposition - FourierDecomposition actual(sequence, model); - - // Check - Vector3 expected(1.5661, 1.2717, 1.2717); - EXPECT(assert_equal((Vector) expected, actual.coefficients(),1e-4)); -} - -//****************************************************************************** -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -//****************************************************************************** - diff --git a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp index 09cad558a..5758509eb 100644 --- a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp @@ -131,14 +131,14 @@ TEST(ExpressionFactor, Unary) { // Unary(Leaf)) and Unary(Unary(Leaf))) // wide version (not handled in fixed-size pipeline) typedef Eigen::Matrix Matrix93; -Vector9 wide(const Point3& p, boost::optional H) { +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, boost::optional H) { +Vector9 id9(const Vector9& v, OptionalJacobian<9,9> H) { if (H) *H = Matrix9::Identity(); return v; } @@ -161,7 +161,7 @@ TEST(ExpressionFactor, Wide) { /* ************************************************************************* */ 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); } @@ -427,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 55f9093ce..ed53d7465 100644 --- a/gtsam_unstable/slam/expressions.h +++ b/gtsam_unstable/slam/expressions.h @@ -20,9 +20,8 @@ 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; + Point2 (Pose2::*transform)(const Point2& p, OptionalJacobian<2, 3> H1, + OptionalJacobian<2, 2> H2) const = &Pose2::transform_to; return Point2_(x, transform, p); } @@ -35,9 +34,8 @@ 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; + Point3 (Pose3::*transform)(const Point3& p, OptionalJacobian<3, 6> Dpose, + OptionalJacobian<3, 3> Dpoint) const = &Pose3::transform_to; return Point3_(x, transform, p); } @@ -51,8 +49,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/wrap/Argument.h b/wrap/Argument.h index 3b7a13ee3..fd7e82061 100644 --- a/wrap/Argument.h +++ b/wrap/Argument.h @@ -171,27 +171,28 @@ struct ArgumentListGrammar: public classic::grammar { wrap::ArgumentList& result_; ///< successful parse will be placed in here - const Argument arg0; ///< used to reset arg - mutable Argument arg; ///< temporary argument for use during parsing - ArgumentGrammar argument_g; ///< single Argument parser - /// Construct type grammar and specify where result is placed ArgumentListGrammar(wrap::ArgumentList& result) : - result_(result), argument_g(arg) { + 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) { + definition(ArgumentListGrammar const& self) : + argument_g(arg) { using namespace classic; - argument_p = self.argument_g // - [classic::push_back_a(self.result_, self.arg)] // - [assign_a(self.arg, self.arg0)]; + argument_p = argument_g // + [classic::push_back_a(self.result_, arg)] // + [assign_a(arg, arg0)]; argumentList_p = '(' >> !argument_p >> *(',' >> argument_p) >> ')'; } diff --git a/wrap/Class.h b/wrap/Class.h index 8faf7ab77..f4c687eca 100644 --- a/wrap/Class.h +++ b/wrap/Class.h @@ -19,6 +19,7 @@ #pragma once +#include "spirit.h" #include "Template.h" #include "Constructor.h" #include "Deconstructor.h" @@ -26,6 +27,18 @@ #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 @@ -145,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/GlobalFunction.h b/wrap/GlobalFunction.h index 4805231fb..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 { @@ -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/Module.cpp b/wrap/Module.cpp index 917130f5c..55fd13715 100644 --- a/wrap/Module.cpp +++ b/wrap/Module.cpp @@ -22,19 +22,6 @@ #include "TypeAttributesTable.h" #include "utilities.h" -//#define BOOST_SPIRIT_DEBUG -#include "spirit.h" - -#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 @@ -111,15 +98,24 @@ void Module::parseMarkup(const std::string& data) { typedef rule Rule; BasicRules basic; - // TODO, do we really need cls here? Non-local + vector namespaces; // current namespace tag + + // parse a full class Class cls0(verbose),cls(verbose); - + 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, singleInstantiation0; TypeListGrammar<'<','>'> typelist_g(singleInstantiation.typeList); // typedef gtsam::RangeFactor RangeFactorPosePoint2; - vector namespaces; // current namespace tag TypeGrammar instantiationClass_g(singleInstantiation.class_); Rule templateSingleInstantiation_p = (str_p("typedef") >> instantiationClass_g >> @@ -130,109 +126,8 @@ void Module::parseMarkup(const std::string& data) { [push_back_a(templateInstantiationTypedefs, singleInstantiation)] [assign_a(singleInstantiation, singleInstantiation0)]; - // template - Rule templateList_p = - (str_p("template") >> - '<' >> basic.name_p[push_back_a(cls.templateArgs)] >> *(',' >> basic.name_p[push_back_a(cls.templateArgs)]) >> - '>'); - - // NOTE: allows for pointers to all types - ArgumentList args; - ArgumentListGrammar argumentList_g(args); - - // parse class constructor - Constructor constructor0(verbose), constructor(verbose); - Rule constructor_p = - (basic.className_p >> argumentList_g >> ';' >> !basic.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 = basic.namespace_p[push_back_a(namespaces_return)] >> str_p("::"); - - ReturnValue retVal0, retVal; - ReturnValueGrammar returnValue_g(retVal); - - Rule methodName_p = lexeme_d[(upper_p | lower_p) >> *(alnum_p | '_')]; - - // template - Template methodTemplate; - TemplateGrammar methodTemplate_g(methodTemplate); - - // gtsam::Values retract(const gtsam::VectorValues& delta) const; - string methodName; - bool isConst, isConst0 = false; - Rule method_p = - !methodTemplate_g >> - (returnValue_g >> methodName_p[assign_a(methodName)] >> - argumentList_g >> - !str_p("const")[assign_a(isConst,true)] >> ';' >> *basic.comments_p) - [bl::bind(&Class::addMethod, bl::var(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,isConst0)]; - - Rule staticMethodName_p = lexeme_d[(upper_p | lower_p) >> *(alnum_p | '_')]; - - Rule static_method_p = - (str_p("static") >> returnValue_g >> staticMethodName_p[assign_a(methodName)] >> - argumentList_g >> ';' >> *basic.comments_p) - [bl::bind(&StaticMethod::addOverload, - bl::var(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)]; - - Rule functions_p = constructor_p | method_p | static_method_p; - - // template - Template classTemplate; - TemplateGrammar classTemplate_g(classTemplate); - - // Parent class - Qualified possibleParent; - TypeGrammar classParent_p(possibleParent); - - // parse a full class - Rule class_p = - eps_p[assign_a(cls,cls0)] - >> (!(classTemplate_g - [push_back_a(cls.templateArgs, classTemplate.argName())] - | templateList_p) - >> !(str_p("virtual")[assign_a(cls.isVirtual, true)]) - >> str_p("class") - >> basic.className_p[assign_a(cls.name_)] - >> ((':' >> classParent_p >> '{') - [bl::bind(&Class::assignParent, bl::var(cls), bl::var(possibleParent))] - [clear_a(possibleParent)] | '{') - >> *(functions_p | basic.comments_p) - >> str_p("};")) - [bl::bind(&Constructor::initializeOrCheck, bl::var(constructor), - bl::var(cls.name_), boost::none, 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(classTemplate.argValues()))] - [clear_a(classTemplate)] - [assign_a(constructor, constructor0)] - [assign_a(cls,cls0)]; - - // parse a global function - Qualified globalFunction; - Rule global_function_p = - (returnValue_g >> staticMethodName_p[assign_a(globalFunction.name_)] >> - argumentList_g >> ';' >> *basic.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), boost::none,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('>'); @@ -245,7 +140,7 @@ void Module::parseMarkup(const std::string& data) { (str_p("namespace") >> basic.namespace_p[push_back_a(namespaces)] >> ch_p('{') - >> *(include_p | class_p | templateSingleInstantiation_p | global_function_p | namespace_def_p | basic.comments_p) + >> *(include_p | class_p | templateSingleInstantiation_p | global_function_g | namespace_def_p | basic.comments_p) >> ch_p('}')) [pop_a(namespaces)]; @@ -265,41 +160,16 @@ void Module::parseMarkup(const std::string& data) { Rule module_content_p = basic.comments_p | include_p | class_p | templateSingleInstantiation_p | forward_declaration_p - | global_function_p | namespace_def_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_g); - 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_g); - 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 << "'" << endl; + cout << "Stopped in:\n" + "class '" << cls.name_ << "'" << endl; throw ParseFailed((int)info.length); } diff --git a/wrap/Module.h b/wrap/Module.h index e99e77bc9..e0c1b3f31 100644 --- a/wrap/Module.h +++ b/wrap/Module.h @@ -34,8 +34,6 @@ namespace wrap { */ struct Module { - typedef std::map GlobalFunctions; - // Filled during parsing: std::string name; ///< module name bool verbose; ///< verbose flag diff --git a/wrap/Qualified.h b/wrap/Qualified.h index 000d749ec..bcc4c0829 100644 --- a/wrap/Qualified.h +++ b/wrap/Qualified.h @@ -222,25 +222,26 @@ struct TypeListGrammar: public classic::grammar > { typedef std::vector TypeList; TypeList& result_; ///< successful parse will be placed in here - mutable wrap::Qualified type; // temporary type for use during parsing - TypeGrammar type_g; - /// Construct type grammar and specify where result is placed TypeListGrammar(TypeList& result) : - result_(result), type_g(type) { + 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) { + definition(TypeListGrammar const& self) : + type_g(type) { using namespace classic; - type_p = self.type_g // - [push_back_a(self.result_, self.type)] // - [clear_a(self.type)]; + + type_p = type_g[push_back_a(self.result_, type)][clear_a(type)]; + typeList_p = OPEN >> !type_p >> *(',' >> type_p) >> CLOSE; } diff --git a/wrap/tests/testClass.cpp b/wrap/tests/testClass.cpp index 0ceff500b..756b6668d 100644 --- a/wrap/tests/testClass.cpp +++ b/wrap/tests/testClass.cpp @@ -46,7 +46,7 @@ TEST( Class, OverloadingMethod ) { EXPECT_LONGS_EQUAL(1, cls.method(name).nrOverloads()); // add an overload w different argument list - args.push_back(Argument(Qualified("Vector",Qualified::EIGEN),"v")); + args.push_back(Argument(Qualified("Vector", Qualified::EIGEN), "v")); cls.addMethod(verbose, is_const, name, args, retVal, tmplate); EXPECT_LONGS_EQUAL(1, cls.nrMethods()); EXPECT_LONGS_EQUAL(2, cls.method(name).nrOverloads()); @@ -82,6 +82,84 @@ TEST( Class, TemplatedMethods ) { EXPECT(cls.exists(name+"Point3")); } +//****************************************************************************** +TEST( Class, Grammar ) { + + using classic::space_p; + + // Create type grammar that will place result in cls + Class cls; + Template t; + ClassGrammar g(cls, t); + + EXPECT(parse("class Point2 {\n};", g, space_p).full); + + string markup( + string("class Point2 { \n") + + string(" double x() const; \n") // Method 1 + + string(" Matrix returnMatrix() const; \n") // Method 2 + + string(" Point2 returnPoint2() const; \n") // Method 3 + + string(" static Vector returnVector(); \n") // Static Method 1 + + string("};")); + + EXPECT(parse(markup.c_str(), g, space_p).full); + + // check return types + EXPECT(assert_equal("Point2", cls.name())); + EXPECT(!cls.isVirtual); + EXPECT(cls.namespaces().empty()); + LONGS_EQUAL(3, cls.nrMethods()); + LONGS_EQUAL(1, cls.static_methods.size()); + + // Method 1 + Method m1 = cls.method("x"); + EXPECT(assert_equal("x", m1.name())); + EXPECT(m1.isConst()); + LONGS_EQUAL(1, m1.nrOverloads()); + + ReturnValue rv1 = m1.returnValue(0); + EXPECT(!rv1.isPair); + EXPECT(!rv1.type1.isPtr); + EXPECT(assert_equal("double", rv1.type1.name())); + EXPECT_LONGS_EQUAL(ReturnType::BASIS, rv1.type1.category); + + // Method 2 + Method m2 = cls.method("returnMatrix"); + EXPECT(assert_equal("returnMatrix", m2.name())); + EXPECT(m2.isConst()); + LONGS_EQUAL(1, m2.nrOverloads()); + + ReturnValue rv2 = m2.returnValue(0); + EXPECT(!rv2.isPair); + EXPECT(!rv2.type1.isPtr); + EXPECT(assert_equal("Matrix", rv2.type1.name())); + EXPECT_LONGS_EQUAL(ReturnType::EIGEN, rv2.type1.category); + + // Method 3 + Method m3 = cls.method("returnPoint2"); + EXPECT(assert_equal("returnPoint2", m3.name())); + EXPECT(m3.isConst()); + LONGS_EQUAL(1, m3.nrOverloads()); + + ReturnValue rv3 = m3.returnValue(0); + EXPECT(!rv3.isPair); + EXPECT(!rv3.type1.isPtr); + EXPECT(assert_equal("Point2", rv3.type1.name())); + EXPECT_LONGS_EQUAL(ReturnType::CLASS, rv3.type1.category); + + // Static Method 1 + // static Vector returnVector(); + StaticMethod sm1 = cls.static_methods.at("returnVector"); + EXPECT(assert_equal("returnVector", sm1.name())); + LONGS_EQUAL(1, sm1.nrOverloads()); + + ReturnValue rv4 = sm1.returnValue(0); + EXPECT(!rv4.isPair); + EXPECT(!rv4.type1.isPtr); + EXPECT(assert_equal("Vector", rv4.type1.name())); + EXPECT_LONGS_EQUAL(ReturnType::EIGEN, rv4.type1.category); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/wrap/tests/testGlobalFunction.cpp b/wrap/tests/testGlobalFunction.cpp new file mode 100644 index 000000000..32ab5dafb --- /dev/null +++ b/wrap/tests/testGlobalFunction.cpp @@ -0,0 +1,54 @@ +/* ---------------------------------------------------------------------------- + + * 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 testMethod.cpp + * @brief Unit test for GlobalFunction class + * @author Frank Dellaert + * @date Nov 12, 2014 + **/ + +#include +#include +#include + +using namespace std; +using namespace wrap; + +//****************************************************************************** +// Constructor +TEST( GlobalFunction, Constructor ) { + GlobalFunction f; +} + +//****************************************************************************** +TEST( GlobalFunction, Grammar ) { + + using classic::space_p; + + // Create type grammar that will place result in actual + GlobalFunctions actual; + vector namespaces; + GlobalFunctionGrammar g(actual,namespaces); + + // a class type with namespaces + EXPECT(parse("Vector aGlobalFunction();", g, space_p).full); + EXPECT(parse("Vector overloadedGlobalFunction(int a);", g, space_p).full); + EXPECT(parse("Vector overloadedGlobalFunction(int a, double b);", g, space_p).full); + LONGS_EQUAL(2,actual.size()); +} + +//****************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//****************************************************************************** diff --git a/wrap/tests/testMethod.cpp b/wrap/tests/testMethod.cpp index 5b58fa31e..d823e9099 100644 --- a/wrap/tests/testMethod.cpp +++ b/wrap/tests/testMethod.cpp @@ -42,83 +42,6 @@ TEST( Method, addOverload ) { EXPECT_LONGS_EQUAL(2, method.nrOverloads()); } -//// http://boost-spirit.com/distrib/spirit_1_8_2/libs/spirit/doc/grammar.html -//struct method_grammar: public classic::grammar { -// -// wrap::Method& result_; ///< successful parse will be placed in here -// -// ArgumentList args; -// Argument arg0, arg; -// TypeGrammar argument_type_g; -// -// ReturnType retType0, retType; -// TypeGrammar returntype_g; -// -// ReturnValue retVal0, retVal; -// -// /// Construct type grammar and specify where result is placed -// method_grammar(wrap::Method& result) : -// result_(result), argument_type_g(arg.type), returntype_g(retType) { -// } -// -// /// Definition of type grammar -// template -// struct definition: basic_rules { -// -// Rule templateArgValue_p, templateArgValues_p, argument_p, argumentList_p, -// returnType1_p, returnType2_p, pair_p, returnValue_p, methodName_p, -// method_p; -// -// definition(method_grammar const& self) { -// -// using namespace wrap; -// using namespace classic; -// -//// Rule templateArgValue_p = type_grammar(self.templateArgValue); -//// -//// // template -//// Rule templateArgValues_p = (str_p("template") >> '<' >> name_p >> '=' -//// >> '{' >> !(templateArgValue_p >> *(',' >> templateArgValue_p)) >> '}' -//// >> '>'); -//// -// // Create type grammar that will place result in actual -// ArgumentList actual; -// ArgumentListGrammar g(actual); -// -// EXPECT(parse("(const gtsam::Point2& p4)", g, space_p).full); -// EXPECT_LONGS_EQUAL(1, actual.size()); -// actual.clear(); -// -// returnType1_p = self.returntype_g // -// [assign_a(self.retVal.type1, retType)] // -// [assign_a(self.retType, self.retType0)]; -// -// returnType2_p = self.returntype_g // -// [assign_a(self.retVal.type2, retType)] // -// [assign_a(self.retType, self.retType0)]; -// -// pair_p = (str_p("pair") >> '<' >> returnType1_p >> ',' >> returnType2_p -// >> '>')[assign_a(self.retVal.isPair, true)]; -// -// returnValue_p = pair_p | returnType1_p; -// -// methodName_p = lexeme_d[(upper_p | lower_p) >> *(alnum_p | '_')]; -// -// // gtsam::Values retract(const gtsam::VectorValues& delta) const; -// method_p = -//// !templateArgValues_p >> -// (returnValue_p >> methodName_p >> '(' >> argumentList_p >> ')' -// >> !str_p("const") >> ';' >> *basic_rules::comments_p); -// } -// -// Rule const& start() const { -// return method_p; -// } -// -// }; -//}; -//// method_grammar -// ////****************************************************************************** //TEST( Method, grammar ) { //