diff --git a/.gitignore b/.gitignore index 4d0aedb3c..b3ea282dd 100644 --- a/.gitignore +++ b/.gitignore @@ -7,3 +7,5 @@ /examples/Data/pose3example-rewritten.txt *.txt.user *.txt.user.6d59f0c +/python-build/ +*.pydevproject diff --git a/CMakeLists.txt b/CMakeLists.txt index 0380b8a2f..9bfa9a758 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -64,6 +64,8 @@ option(GTSAM_WITH_TBB "Use Intel Threaded Building Blocks (TB option(GTSAM_WITH_EIGEN_MKL "Eigen will use Intel MKL if available" ON) option(GTSAM_WITH_EIGEN_MKL_OPENMP "Eigen, when using Intel MKL, will also use OpenMP for multithreading if available" ON) option(GTSAM_THROW_CHEIRALITY_EXCEPTION "Throw exception when a triangulated point is behind a camera" ON) +option(GTSAM_BUILD_PYTHON "Enable/Disable building & installation of Python module" ON) +option(GTSAM_ALLOW_DEPRECATED_SINCE_V4 "Allow use of methods/functions deprecated in GTSAM 4" ON) # Options relating to MATLAB wrapper # TODO: Check for matlab mex binary before handling building of binaries @@ -316,6 +318,10 @@ if(GTSAM_ENABLE_CONSISTENCY_CHECKS) add_definitions(-DGTSAM_EXTRA_CONSISTENCY_CHECKS) endif() +if(GTSAM_ALLOW_DEPRECATED_SINCE_V4) + add_definitions(-DGTSAM_ALLOW_DEPRECATED_SINCE_V4) +endif() + ############################################################################### # Add components @@ -344,6 +350,20 @@ if (GTSAM_INSTALL_MATLAB_TOOLBOX) add_subdirectory(matlab) endif() +# Python wrap +if (GTSAM_BUILD_PYTHON) + include(GtsamPythonWrap) + + # NOTE: The automatic generation of python wrapper from the gtsampy.h interface is + # not working yet, so we're using a handwritten wrapper files on python/handwritten. + # Once the python wrapping from the interface file is working, you can _swap_ the + # comments on the next lines + + # wrap_and_install_python(gtsampy.h "${GTSAM_ADDITIONAL_LIBRARIES}" "") + add_subdirectory(python) + +endif() + # Build gtsam_unstable if (GTSAM_BUILD_UNSTABLE) add_subdirectory(gtsam_unstable) @@ -456,10 +476,22 @@ print_config_flag(${GTSAM_USE_QUATERNIONS} "Quaternions as default R print_config_flag(${GTSAM_ENABLE_CONSISTENCY_CHECKS} "Runtime consistency checking ") print_config_flag(${GTSAM_ROT3_EXPMAP} "Rot3 retract is full ExpMap ") print_config_flag(${GTSAM_POSE3_EXPMAP} "Pose3 retract is full ExpMap ") +print_config_flag(${GTSAM_ALLOW_DEPRECATED_SINCE_V4} "Deprecated in GTSAM 4 allowed ") message(STATUS "MATLAB toolbox flags ") print_config_flag(${GTSAM_INSTALL_MATLAB_TOOLBOX} "Install matlab toolbox ") print_config_flag(${GTSAM_BUILD_WRAP} "Build Wrap ") + +message(STATUS "Python module flags ") + +if(GTSAM_PYTHON_WARNINGS) + message(STATUS " Build python module : No - dependencies missing") +else() + print_config_flag(${GTSAM_BUILD_PYTHON} "Build python module ") +endif() +if(GTSAM_BUILD_PYTHON) + message(STATUS " Python version : ${GTSAM_PYTHON_VERSION}") +endif() message(STATUS "===============================================================") # Print warnings at the end @@ -472,6 +504,9 @@ endif() if(GTSAM_WITH_EIGEN_MKL_OPENMP AND NOT OPENMP_FOUND AND MKL_FOUND) message(WARNING "Your compiler does not support OpenMP - this is ok, but performance may be improved with OpenMP. Set GTSAM_WITH_EIGEN_MKL_OPENMP to 'Off' to avoid this warning.") endif() +if(GTSAM_BUILD_PYTHON AND GTSAM_PYTHON_WARNINGS) + message(WARNING "${GTSAM_PYTHON_WARNINGS}") +endif() # Include CPack *after* all flags include(CPack) diff --git a/THANKS b/THANKS index 9c06a5d28..f84cfa185 100644 --- a/THANKS +++ b/THANKS @@ -38,6 +38,10 @@ at Uni Zurich: * Christian Forster +at LAAS-CNRS + +* Ellon Paiva + Many thanks for your hard work!!!! Frank Dellaert diff --git a/cmake/FindNumPy.cmake b/cmake/FindNumPy.cmake new file mode 100644 index 000000000..eafed165e --- /dev/null +++ b/cmake/FindNumPy.cmake @@ -0,0 +1,102 @@ +# - Find the NumPy libraries +# This module finds if NumPy is installed, and sets the following variables +# indicating where it is. +# +# TODO: Update to provide the libraries and paths for linking npymath lib. +# +# NUMPY_FOUND - was NumPy found +# NUMPY_VERSION - the version of NumPy found as a string +# NUMPY_VERSION_MAJOR - the major version number of NumPy +# NUMPY_VERSION_MINOR - the minor version number of NumPy +# NUMPY_VERSION_PATCH - the patch version number of NumPy +# NUMPY_VERSION_DECIMAL - e.g. version 1.6.1 is 10601 +# NUMPY_INCLUDE_DIRS - path to the NumPy include files + +#============================================================================ +# Copyright 2012 Continuum Analytics, Inc. +# +# MIT License +# +# Permission is hereby granted, free of charge, to any person obtaining +# a copy of this software and associated documentation files +# (the "Software"), to deal in the Software without restriction, including +# without limitation the rights to use, copy, modify, merge, publish, +# distribute, sublicense, and/or sell copies of the Software, and to permit +# persons to whom the Software is furnished to do so, subject to +# the following conditions: +# +# The above copyright notice and this permission notice shall be included +# in all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS +# OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR +# OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, +# ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR +# OTHER DEALINGS IN THE SOFTWARE. +# +#============================================================================ + +# Finding NumPy involves calling the Python interpreter +if(NumPy_FIND_REQUIRED) + find_package(PythonInterp REQUIRED) +else() + find_package(PythonInterp) +endif() + +if(NOT PYTHONINTERP_FOUND) + set(NUMPY_FOUND FALSE) + return() +endif() + +execute_process(COMMAND "${PYTHON_EXECUTABLE}" "-c" + "import numpy as n; print(n.__version__); print(n.get_include());" + RESULT_VARIABLE _NUMPY_SEARCH_SUCCESS + OUTPUT_VARIABLE _NUMPY_VALUES_OUTPUT + ERROR_VARIABLE _NUMPY_ERROR_VALUE + OUTPUT_STRIP_TRAILING_WHITESPACE) + +if(NOT _NUMPY_SEARCH_SUCCESS MATCHES 0) + if(NumPy_FIND_REQUIRED) + message(FATAL_ERROR + "NumPy import failure:\n${_NUMPY_ERROR_VALUE}") + endif() + set(NUMPY_FOUND FALSE) + return() +endif() + +# Convert the process output into a list +string(REGEX REPLACE ";" "\\\\;" _NUMPY_VALUES ${_NUMPY_VALUES_OUTPUT}) +string(REGEX REPLACE "\n" ";" _NUMPY_VALUES ${_NUMPY_VALUES}) +# Just in case there is unexpected output from the Python command. +list(GET _NUMPY_VALUES -2 NUMPY_VERSION) +list(GET _NUMPY_VALUES -1 NUMPY_INCLUDE_DIRS) + +string(REGEX MATCH "^[0-9]+\\.[0-9]+\\.[0-9]+" _VER_CHECK "${NUMPY_VERSION}") +if("${_VER_CHECK}" STREQUAL "") + # The output from Python was unexpected. Raise an error always + # here, because we found NumPy, but it appears to be corrupted somehow. + message(FATAL_ERROR + "Requested version and include path from NumPy, got instead:\n${_NUMPY_VALUES_OUTPUT}\n") + return() +endif() + +# Make sure all directory separators are '/' +string(REGEX REPLACE "\\\\" "/" NUMPY_INCLUDE_DIRS ${NUMPY_INCLUDE_DIRS}) + +# Get the major and minor version numbers +string(REGEX REPLACE "\\." ";" _NUMPY_VERSION_LIST ${NUMPY_VERSION}) +list(GET _NUMPY_VERSION_LIST 0 NUMPY_VERSION_MAJOR) +list(GET _NUMPY_VERSION_LIST 1 NUMPY_VERSION_MINOR) +list(GET _NUMPY_VERSION_LIST 2 NUMPY_VERSION_PATCH) +string(REGEX MATCH "[0-9]*" NUMPY_VERSION_PATCH ${NUMPY_VERSION_PATCH}) +math(EXPR NUMPY_VERSION_DECIMAL + "(${NUMPY_VERSION_MAJOR} * 10000) + (${NUMPY_VERSION_MINOR} * 100) + ${NUMPY_VERSION_PATCH}") + +find_package_message(NUMPY + "Found NumPy: version \"${NUMPY_VERSION}\" ${NUMPY_INCLUDE_DIRS}" + "${NUMPY_INCLUDE_DIRS}${NUMPY_VERSION}") + +set(NUMPY_FOUND TRUE) + diff --git a/cmake/GtsamPythonWrap.cmake b/cmake/GtsamPythonWrap.cmake index d065a7828..714e37488 100644 --- a/cmake/GtsamPythonWrap.cmake +++ b/cmake/GtsamPythonWrap.cmake @@ -1,5 +1,5 @@ #Setup cache options -option(GTSAM_BUILD_PYTHON "Build Python wrapper statically (increases build time)" OFF) +set(GTSAM_PYTHON_VERSION "Default" CACHE STRING "Target python version for GTSAM python module. Use 'Default' to chose the default version") set(GTSAM_BUILD_PYTHON_FLAGS "" CACHE STRING "Extra flags for running Matlab PYTHON compilation") set(GTSAM_PYTHON_INSTALL_PATH "" CACHE PATH "Python toolbox destination, blank defaults to CMAKE_INSTALL_PREFIX/borg/python") if(NOT GTSAM_PYTHON_INSTALL_PATH) @@ -8,13 +8,13 @@ endif() #Author: Paul Furgale Modified by Andrew Melim function(wrap_python TARGET_NAME PYTHON_MODULE_DIRECTORY) - # Boost - find_package(Boost COMPONENTS python filesystem system REQUIRED) - include_directories(${Boost_INCLUDE_DIRS}) + # # Boost + # find_package(Boost COMPONENTS python filesystem system REQUIRED) + # include_directories(${Boost_INCLUDE_DIRS}) - # Find Python - FIND_PACKAGE(PythonLibs 2.7 REQUIRED) - INCLUDE_DIRECTORIES(${PYTHON_INCLUDE_DIRS}) + # # Find Python + # FIND_PACKAGE(PythonLibs 2.7 REQUIRED) + # INCLUDE_DIRECTORIES(${PYTHON_INCLUDE_DIRS}) IF(APPLE) # The apple framework headers don't include the numpy headers for some reason. @@ -36,23 +36,46 @@ function(wrap_python TARGET_NAME PYTHON_MODULE_DIRECTORY) ENDIF() ENDIF(APPLE) + if(MSVC) + add_library(${moduleName}_python MODULE ${ARGN}) + set_target_properties(${moduleName}_python PROPERTIES + OUTPUT_NAME ${moduleName}_python + CLEAN_DIRECT_OUTPUT 1 + VERSION 1 + SOVERSION 0 + SUFFIX ".pyd") + target_link_libraries(${moduleName}_python ${Boost_PYTHON_LIBRARY} ${PYTHON_LIBRARY} ${gtsamLib}) #temp - # Create a static library version - add_library(${TARGET_NAME} SHARED ${ARGN}) + set(PYLIB_OUTPUT_FILE $) + message(${PYLIB_OUTPUT_FILE}) + get_filename_component(PYLIB_OUTPUT_NAME ${PYLIB_OUTPUT_FILE} NAME_WE) + set(PYLIB_SO_NAME ${PYLIB_OUTPUT_NAME}.pyd) - target_link_libraries(${TARGET_NAME} ${Boost_PYTHON_LIBRARY} ${PYTHON_LIBRARY} gtsam-shared) - set_target_properties(${TARGET_NAME} PROPERTIES - OUTPUT_NAME ${TARGET_NAME} - CLEAN_DIRECT_OUTPUT 1 - VERSION 1 - SOVERSION 0) + ELSE() + # Create a shared library + add_library(${moduleName}_python SHARED ${generated_cpp_file}) + set_target_properties(${moduleName}_python PROPERTIES + OUTPUT_NAME ${moduleName}_python + CLEAN_DIRECT_OUTPUT 1) + target_link_libraries(${moduleName}_python ${Boost_PYTHON_LIBRARY} ${PYTHON_LIBRARY} ${gtsamLib}) #temp + # On OSX and Linux, the python library must end in the extension .so. Build this + # filename here. + get_property(PYLIB_OUTPUT_FILE TARGET ${moduleName}_python PROPERTY LOCATION) + set(PYLIB_OUTPUT_FILE $) + message(${PYLIB_OUTPUT_FILE}) + get_filename_component(PYLIB_OUTPUT_NAME ${PYLIB_OUTPUT_FILE} NAME_WE) + set(PYLIB_SO_NAME lib${moduleName}_python.so) + ENDIF(MSVC) - # On OSX and Linux, the python library must end in the extension .so. Build this - # filename here. - get_property(PYLIB_OUTPUT_FILE TARGET ${TARGET_NAME} PROPERTY LOCATION) - get_filename_component(PYLIB_OUTPUT_NAME ${PYLIB_OUTPUT_FILE} NAME_WE) - set(PYLIB_SO_NAME ${PYLIB_OUTPUT_NAME}.so) + # Installs the library in the gtsam folder, which is used by setup.py to create the gtsam package + set(PYTHON_MODULE_DIRECTORY ${CMAKE_SOURCE_DIR}/python/gtsam) + # Cause the library to be output in the correct directory. + add_custom_command(TARGET ${moduleName}_python + POST_BUILD + COMMAND cp -v ${PYLIB_OUTPUT_FILE} ${PYTHON_MODULE_DIRECTORY}/${PYLIB_SO_NAME} + WORKING_DIRECTORY ${PROJECT_SOURCE_DIR} + COMMENT "Copying library files to python directory" ) # Cause the library to be output in the correct directory. add_custom_command(TARGET ${TARGET_NAME} @@ -64,4 +87,16 @@ function(wrap_python TARGET_NAME PYTHON_MODULE_DIRECTORY) get_directory_property(AMCF ADDITIONAL_MAKE_CLEAN_FILES) list(APPEND AMCF ${PYTHON_MODULE_DIRECTORY}/${PYLIB_SO_NAME}) set_directory_properties(PROPERTIES ADDITIONAL_MAKE_CLEAN_FILES "${AMCF}") -endfunction(wrap_python) \ No newline at end of file +endfunction(wrap_python) + +# Macro to get list of subdirectories +macro(SUBDIRLIST result curdir) + file(GLOB children RELATIVE ${curdir} ${curdir}/*) + set(dirlist "") + foreach(child ${children}) + if(IS_DIRECTORY ${curdir}/${child}) + list(APPEND dirlist ${child}) + endif() + endforeach() + set(${result} ${dirlist}) +endmacro() diff --git a/examples/CreateSFMExampleData.cpp b/examples/CreateSFMExampleData.cpp index bcc0b6320..010f474bf 100644 --- a/examples/CreateSFMExampleData.cpp +++ b/examples/CreateSFMExampleData.cpp @@ -35,7 +35,7 @@ void createExampleBALFile(const string& filename, const vector& P, SfM_data data; // Create two cameras - Rot3 aRb = Rot3::yaw(M_PI_2); + Rot3 aRb = Rot3::Yaw(M_PI_2); Point3 aTb(0.1, 0, 0); Pose3 identity, aPb(aRb, aTb); data.cameras.push_back(SfM_Camera(pose1, K)); @@ -66,7 +66,7 @@ void createExampleBALFile(const string& filename, const vector& P, void create5PointExample1() { // Create two cameras poses - Rot3 aRb = Rot3::yaw(M_PI_2); + Rot3 aRb = Rot3::Yaw(M_PI_2); Point3 aTb(0.1, 0, 0); Pose3 pose1, pose2(aRb, aTb); @@ -85,7 +85,7 @@ void create5PointExample1() { void create5PointExample2() { // Create two cameras poses - Rot3 aRb = Rot3::yaw(M_PI_2); + Rot3 aRb = Rot3::Yaw(M_PI_2); Point3 aTb(10, 0, 0); Pose3 pose1, pose2(aRb, aTb); diff --git a/gtsam.h b/gtsam.h index ca014ad5d..57a1e09be 100644 --- a/gtsam.h +++ b/gtsam.h @@ -434,11 +434,11 @@ class Rot3 { static gtsam::Rot3 Rz(double t); static gtsam::Rot3 RzRyRx(double x, double y, double z); static gtsam::Rot3 RzRyRx(Vector xyz); - static gtsam::Rot3 yaw(double t); // positive yaw is to right (as in aircraft heading) - static gtsam::Rot3 pitch(double t); // positive pitch is up (increasing aircraft altitude) - static gtsam::Rot3 roll(double t); // positive roll is to right (increasing yaw in aircraft) - static gtsam::Rot3 ypr(double y, double p, double r); - static gtsam::Rot3 quaternion(double w, double x, double y, double z); + static gtsam::Rot3 Yaw(double t); // positive yaw is to right (as in aircraft heading) + static gtsam::Rot3 Pitch(double t); // positive pitch is up (increasing aircraft altitude) + static gtsam::Rot3 Roll(double t); // positive roll is to right (increasing yaw in aircraft) + static gtsam::Rot3 Ypr(double y, double p, double r); + static gtsam::Rot3 Quaternion(double w, double x, double y, double z); static gtsam::Rot3 Rodrigues(Vector v); // Testable diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index f373e5ad4..9954240fd 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -120,9 +120,9 @@ Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> H) { Rot3 R = Rot3::Expmap(omega.vector()); double theta2 = omega.dot(omega); if (theta2 > std::numeric_limits::epsilon()) { - double omega_v = omega.dot(v); // translation parallel to axis - Point3 omega_cross_v = omega.cross(v); // points towards axis - Point3 t = (omega_cross_v - R * omega_cross_v + omega_v * omega) / theta2; + Point3 t_parallel = omega * omega.dot(v); // translation parallel to axis + Point3 omega_cross_v = omega.cross(v); // points towards axis + Point3 t = (omega_cross_v - R * omega_cross_v + t_parallel) / theta2; return Pose3(R, t); } else { return Pose3(R, v); diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index 6b28f5125..ef384c3ef 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -123,7 +123,7 @@ Vector3 Rot3::rpy() const { /* ************************************************************************* */ Vector Rot3::quaternion() const { - Quaternion q = toQuaternion(); + gtsam::Quaternion q = toQuaternion(); Vector v(4); v(0) = q.w(); v(1) = q.x(); diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index d8b8a4682..264be1537 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -59,7 +59,7 @@ namespace gtsam { #ifdef GTSAM_USE_QUATERNIONS /** Internal Eigen Quaternion */ - Quaternion quaternion_; + gtsam::Quaternion quaternion_; #else Matrix3 rot_; #endif @@ -146,13 +146,13 @@ namespace gtsam { } /// Positive yaw is to right (as in aircraft heading). See ypr - static Rot3 yaw (double t) { return Rz(t); } + static Rot3 Yaw (double t) { return Rz(t); } /// Positive pitch is up (increasing aircraft altitude).See ypr - static Rot3 pitch(double t) { return Ry(t); } + static Rot3 Pitch(double t) { return Ry(t); } //// Positive roll is to right (increasing yaw in aircraft). - static Rot3 roll (double t) { return Rx(t); } + static Rot3 Roll (double t) { return Rx(t); } /** * Returns rotation nRb from body to nav frame. @@ -163,11 +163,11 @@ namespace gtsam { * as described in http://www.sedris.org/wg8home/Documents/WG80462.pdf. * Assumes vehicle coordinate frame X forward, Y right, Z down. */ - static Rot3 ypr(double y, double p, double r) { return RzRyRx(r,p,y);} + static Rot3 Ypr(double y, double p, double r) { return RzRyRx(r,p,y);} /** Create from Quaternion coefficients */ - static Rot3 quaternion(double w, double x, double y, double z) { - Quaternion q(w, x, y, z); + static Rot3 Quaternion(double w, double x, double y, double z) { + gtsam::Quaternion q(w, x, y, z); return Rot3(q); } @@ -179,7 +179,7 @@ namespace gtsam { */ static Rot3 AxisAngle(const Vector3& axis, double angle) { #ifdef GTSAM_USE_QUATERNIONS - return Quaternion(Eigen::AngleAxis(angle, axis)); + return gtsam::Quaternion(Eigen::AngleAxis(angle, axis)); #else return SO3::AxisAngle(axis,angle); #endif @@ -313,7 +313,7 @@ namespace gtsam { static Rot3 Expmap(const Vector3& v, OptionalJacobian<3,3> H = boost::none) { if(H) *H = Rot3::ExpmapDerivative(v); #ifdef GTSAM_USE_QUATERNIONS - return traits::Expmap(v); + return traits::Expmap(v); #else return traits::Expmap(v); #endif @@ -419,13 +419,13 @@ namespace gtsam { /** * Use RQ to calculate yaw-pitch-roll angle representation - * @return a vector containing ypr s.t. R = Rot3::ypr(y,p,r) + * @return a vector containing ypr s.t. R = Rot3::Ypr(y,p,r) */ Vector3 ypr() const; /** * Use RQ to calculate roll-pitch-yaw angle representation - * @return a vector containing ypr s.t. R = Rot3::ypr(y,p,r) + * @return a vector containing ypr s.t. R = Rot3::Ypr(y,p,r) */ Vector3 rpy() const; @@ -460,7 +460,7 @@ namespace gtsam { /** Compute the quaternion representation of this rotation. * @return The quaternion */ - Quaternion toQuaternion() const; + gtsam::Quaternion toQuaternion() const; /** * Converts to a generic matrix to allow for use with matlab @@ -479,6 +479,8 @@ namespace gtsam { GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Rot3& p); /// @} + +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 /// @name Deprecated /// @{ static Rot3 rodriguez(const Vector3& axis, double angle) { return AxisAngle(axis, angle); } @@ -486,7 +488,15 @@ namespace gtsam { static Rot3 rodriguez(const Unit3& axis, double angle) { return AxisAngle(axis, angle); } static Rot3 rodriguez(const Vector3& w) { return Rodrigues(w); } static Rot3 rodriguez(double wx, double wy, double wz) { return Rodrigues(wx, wy, wz); } - /// @} + static Rot3 yaw (double t) { return Yaw(t); } + static Rot3 pitch(double t) { return Pitch(t); } + static Rot3 roll (double t) { return Roll(t); } + static Rot3 ypr(double y, double p, double r) { return Ypr(r,p,y);} + static Rot3 quaternion(double w, double x, double y, double z) { + return Rot3::Quaternion(w, x, y, z); + } + /// @} +#endif private: diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index a7b654070..c3636000b 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -51,7 +51,7 @@ Rot3::Rot3(double R11, double R12, double R13, } /* ************************************************************************* */ -Rot3::Rot3(const Quaternion& q) : rot_(q.toRotationMatrix()) { +Rot3::Rot3(const gtsam::Quaternion& q) : rot_(q.toRotationMatrix()) { } /* ************************************************************************* */ @@ -191,8 +191,8 @@ Point3 Rot3::r2() const { return Point3(rot_.col(1)); } Point3 Rot3::r3() const { return Point3(rot_.col(2)); } /* ************************************************************************* */ -Quaternion Rot3::toQuaternion() const { - return Quaternion(rot_); +gtsam::Quaternion Rot3::toQuaternion() const { + return gtsam::Quaternion(rot_); } /* ************************************************************************* */ diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp index 2497f6806..f8a01141b 100644 --- a/gtsam/geometry/Rot3Q.cpp +++ b/gtsam/geometry/Rot3Q.cpp @@ -47,30 +47,30 @@ namespace gtsam { R31, R32, R33).finished()) {} /* ************************************************************************* */ - Rot3::Rot3(const Quaternion& q) : + Rot3::Rot3(const gtsam::Quaternion& q) : quaternion_(q) { } /* ************************************************************************* */ Rot3 Rot3::Rx(double t) { - return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitX())); + return gtsam::Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitX())); } /* ************************************************************************* */ Rot3 Rot3::Ry(double t) { - return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitY())); + return gtsam::Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitY())); } /* ************************************************************************* */ Rot3 Rot3::Rz(double t) { - return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitZ())); + return gtsam::Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitZ())); } /* ************************************************************************* */ Rot3 Rot3::RzRyRx(double x, double y, double z) { return Rot3( - Quaternion(Eigen::AngleAxisd(z, Eigen::Vector3d::UnitZ())) * - Quaternion(Eigen::AngleAxisd(y, Eigen::Vector3d::UnitY())) * - Quaternion(Eigen::AngleAxisd(x, Eigen::Vector3d::UnitX()))); + gtsam::Quaternion(Eigen::AngleAxisd(z, Eigen::Vector3d::UnitZ())) * + gtsam::Quaternion(Eigen::AngleAxisd(y, Eigen::Vector3d::UnitY())) * + gtsam::Quaternion(Eigen::AngleAxisd(x, Eigen::Vector3d::UnitX()))); } /* ************************************************************************* */ @@ -98,7 +98,7 @@ namespace gtsam { /* ************************************************************************* */ Vector3 Rot3::Logmap(const Rot3& R, OptionalJacobian<3, 3> H) { - return traits::Logmap(R.quaternion_, H); + return traits::Logmap(R.quaternion_, H); } /* ************************************************************************* */ @@ -128,7 +128,7 @@ namespace gtsam { Point3 Rot3::r3() const { return Point3(quaternion_.toRotationMatrix().col(2)); } /* ************************************************************************* */ - Quaternion Rot3::toQuaternion() const { return quaternion_; } + gtsam::Quaternion Rot3::toQuaternion() const { return quaternion_; } /* ************************************************************************* */ diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index a417164e4..ca80167f4 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -47,7 +47,7 @@ SO3 SO3::AxisAngle(const Vector3& axis, double theta) { // get components of axis \omega, where is a unit vector const double& wx = axis.x(), wy = axis.y(), wz = axis.z(); - const double costheta = cos(theta), sintheta = sin(theta), c_1 = 1 - costheta; + const double costheta = cos(theta), sintheta = sin(theta), s2 = sin(theta/2.0), c_1 = 2.0*s2*s2; const double wx_sintheta = wx * sintheta, wy_sintheta = wy * sintheta, wz_sintheta = wz * sintheta; @@ -130,7 +130,6 @@ Vector3 SO3::Logmap(const SO3& R, ChartJacobian H) { /* ************************************************************************* */ Matrix3 SO3::ExpmapDerivative(const Vector3& omega) { - using std::cos; using std::sin; const double theta2 = omega.dot(omega); diff --git a/gtsam/geometry/tests/testCalibratedCamera.cpp b/gtsam/geometry/tests/testCalibratedCamera.cpp index 3b0906b44..5bc645a58 100644 --- a/gtsam/geometry/tests/testCalibratedCamera.cpp +++ b/gtsam/geometry/tests/testCalibratedCamera.cpp @@ -135,7 +135,7 @@ TEST( CalibratedCamera, Dproject_point_pose) // Add a test with more arbitrary rotation TEST( CalibratedCamera, Dproject_point_pose2) { - static const Pose3 pose1(Rot3::ypr(0.1, -0.1, 0.4), Point3(0, 0, -10)); + static const Pose3 pose1(Rot3::Ypr(0.1, -0.1, 0.4), Point3(0, 0, -10)); static const CalibratedCamera camera(pose1); Matrix Dpose, Dpoint; camera.project(point1, Dpose, Dpoint); @@ -165,7 +165,7 @@ TEST( CalibratedCamera, Dproject_point_pose_infinity) // Add a test with more arbitrary rotation TEST( CalibratedCamera, Dproject_point_pose2_infinity) { - static const Pose3 pose1(Rot3::ypr(0.1, -0.1, 0.4), Point3(0, 0, -10)); + static const Pose3 pose1(Rot3::Ypr(0.1, -0.1, 0.4), Point3(0, 0, -10)); static const CalibratedCamera camera(pose1); Matrix Dpose, Dpoint; camera.project2(pointAtInfinity, Dpose, Dpoint); diff --git a/gtsam/geometry/tests/testEssentialMatrix.cpp b/gtsam/geometry/tests/testEssentialMatrix.cpp index fe27b2911..bfff0a182 100644 --- a/gtsam/geometry/tests/testEssentialMatrix.cpp +++ b/gtsam/geometry/tests/testEssentialMatrix.cpp @@ -20,7 +20,7 @@ GTSAM_CONCEPT_MANIFOLD_INST(EssentialMatrix) //************************************************************************* // Create two cameras and corresponding essential matrix E -Rot3 c1Rc2 = Rot3::yaw(M_PI_2); +Rot3 c1Rc2 = Rot3::Yaw(M_PI_2); Point3 c1Tc2(0.1, 0, 0); EssentialMatrix trueE(c1Rc2, Unit3(c1Tc2)); @@ -98,8 +98,8 @@ Point3 transform_to_(const EssentialMatrix& E, const Point3& point) { } TEST (EssentialMatrix, transform_to) { // test with a more complicated EssentialMatrix - Rot3 aRb2 = Rot3::yaw(M_PI / 3.0) * Rot3::pitch(M_PI_4) - * Rot3::roll(M_PI / 6.0); + Rot3 aRb2 = Rot3::Yaw(M_PI / 3.0) * Rot3::Pitch(M_PI_4) + * Rot3::Roll(M_PI / 6.0); Point3 aTb2(19.2, 3.7, 5.9); EssentialMatrix E(aRb2, Unit3(aTb2)); //EssentialMatrix E(aRb, Unit3(aTb).retract(Vector2(0.1, 0))); @@ -159,7 +159,7 @@ TEST (EssentialMatrix, FromPose3_a) { //************************************************************************* TEST (EssentialMatrix, FromPose3_b) { Matrix actualH; - Rot3 c1Rc2 = Rot3::ypr(0.1, -0.2, 0.3); + Rot3 c1Rc2 = Rot3::Ypr(0.1, -0.2, 0.3); Point3 c1Tc2(0.4, 0.5, 0.6); EssentialMatrix E(c1Rc2, Unit3(c1Tc2)); Pose3 pose(c1Rc2, c1Tc2); // Pose between two cameras @@ -181,7 +181,7 @@ TEST (EssentialMatrix, streaming) { //************************************************************************* TEST (EssentialMatrix, epipoles) { // Create an E - Rot3 c1Rc2 = Rot3::ypr(0.1, -0.2, 0.3); + Rot3 c1Rc2 = Rot3::Ypr(0.1, -0.2, 0.3); Point3 c1Tc2(0.4, 0.5, 0.6); EssentialMatrix E(c1Rc2, Unit3(c1Tc2)); diff --git a/gtsam/geometry/tests/testOrientedPlane3.cpp b/gtsam/geometry/tests/testOrientedPlane3.cpp index 11931449f..180abb0d6 100644 --- a/gtsam/geometry/tests/testOrientedPlane3.cpp +++ b/gtsam/geometry/tests/testOrientedPlane3.cpp @@ -59,7 +59,7 @@ OrientedPlane3 transform_(const OrientedPlane3& plane, const Pose3& xr) { } TEST (OrientedPlane3, transform) { - gtsam::Pose3 pose(gtsam::Rot3::ypr(-M_PI / 4.0, 0.0, 0.0), + gtsam::Pose3 pose(gtsam::Rot3::Ypr(-M_PI / 4.0, 0.0, 0.0), gtsam::Point3(2.0, 3.0, 4.0)); OrientedPlane3 plane(-1, 0, 0, 5); OrientedPlane3 expectedPlane(-sqrt(2.0) / 2.0, -sqrt(2.0) / 2.0, 0.0, 3); diff --git a/gtsam/geometry/tests/testPinholeCamera.cpp b/gtsam/geometry/tests/testPinholeCamera.cpp index 74bc4ca2a..7293d4235 100644 --- a/gtsam/geometry/tests/testPinholeCamera.cpp +++ b/gtsam/geometry/tests/testPinholeCamera.cpp @@ -242,7 +242,7 @@ TEST( PinholeCamera, Dproject2) // Add a test with more arbitrary rotation TEST( PinholeCamera, Dproject3) { - static const Pose3 pose1(Rot3::ypr(0.1, -0.1, 0.4), Point3(0, 0, -10)); + static const Pose3 pose1(Rot3::Ypr(0.1, -0.1, 0.4), Point3(0, 0, -10)); static const Camera camera(pose1); Matrix Dpose, Dpoint; camera.project2(point1, Dpose, Dpoint); diff --git a/gtsam/geometry/tests/testPinholePose.cpp b/gtsam/geometry/tests/testPinholePose.cpp index dc294899f..8f3eadc51 100644 --- a/gtsam/geometry/tests/testPinholePose.cpp +++ b/gtsam/geometry/tests/testPinholePose.cpp @@ -172,7 +172,7 @@ TEST( PinholePose, Dproject2) // Add a test with more arbitrary rotation TEST( CalibratedCamera, Dproject3) { - static const Pose3 pose1(Rot3::ypr(0.1, -0.1, 0.4), Point3(0, 0, -10)); + static const Pose3 pose1(Rot3::Ypr(0.1, -0.1, 0.4), Point3(0, 0, -10)); static const Camera camera(pose1); Matrix Dpose, Dpoint; camera.project2(point1, Dpose, Dpoint); diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp index 6811ed122..246d23c9a 100644 --- a/gtsam/geometry/tests/testPoint3.cpp +++ b/gtsam/geometry/tests/testPoint3.cpp @@ -133,6 +133,17 @@ TEST (Point3, distance) { EXPECT(assert_equal(numH2, H2, 1e-8)); } +/* ************************************************************************* */ +TEST(Point3, cross) { + Matrix aH1, aH2; + boost::function f = + boost::bind(&Point3::cross, _1, _2, boost::none, boost::none); + const Point3 omega(0, 1, 0), theta(4, 6, 8); + omega.cross(theta, aH1, aH2); + EXPECT(assert_equal(numericalDerivative21(f, omega, theta), aH1)); + EXPECT(assert_equal(numericalDerivative22(f, omega, theta), aH2)); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 9007ce1bd..56e1e022c 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -556,12 +556,12 @@ TEST( Pose3, between ) /* ************************************************************************* */ // some shared test values - pulled from equivalent test in Pose2 Point3 l1(1, 0, 0), l2(1, 1, 0), l3(2, 2, 0), l4(1, 4,-4); -Pose3 x1, x2(Rot3::ypr(0.0, 0.0, 0.0), l2), x3(Rot3::ypr(M_PI/4.0, 0.0, 0.0), l2); +Pose3 x1, x2(Rot3::Ypr(0.0, 0.0, 0.0), l2), x3(Rot3::Ypr(M_PI/4.0, 0.0, 0.0), l2); Pose3 - xl1(Rot3::ypr(0.0, 0.0, 0.0), Point3(1, 0, 0)), - xl2(Rot3::ypr(0.0, 1.0, 0.0), Point3(1, 1, 0)), - xl3(Rot3::ypr(1.0, 0.0, 0.0), Point3(2, 2, 0)), - xl4(Rot3::ypr(0.0, 0.0, 1.0), Point3(1, 4,-4)); + xl1(Rot3::Ypr(0.0, 0.0, 0.0), Point3(1, 0, 0)), + xl2(Rot3::Ypr(0.0, 1.0, 0.0), Point3(1, 1, 0)), + xl3(Rot3::Ypr(1.0, 0.0, 0.0), Point3(2, 2, 0)), + xl4(Rot3::Ypr(0.0, 0.0, 1.0), Point3(1, 4,-4)); /* ************************************************************************* */ double range_proxy(const Pose3& pose, const Point3& point) { @@ -654,9 +654,9 @@ TEST( Pose3, unicycle ) { // velocity in X should be X in inertial frame, rather than global frame Vector x_step = delta(6,3,1.0); - EXPECT(assert_equal(Pose3(Rot3::ypr(0,0,0), l1), expmap_default(x1, x_step), tol)); - EXPECT(assert_equal(Pose3(Rot3::ypr(0,0,0), Point3(2,1,0)), expmap_default(x2, x_step), tol)); - EXPECT(assert_equal(Pose3(Rot3::ypr(M_PI/4.0,0,0), Point3(2,2,0)), expmap_default(x3, sqrt(2.0) * x_step), tol)); + EXPECT(assert_equal(Pose3(Rot3::Ypr(0,0,0), l1), expmap_default(x1, x_step), tol)); + EXPECT(assert_equal(Pose3(Rot3::Ypr(0,0,0), Point3(2,1,0)), expmap_default(x2, x_step), tol)); + EXPECT(assert_equal(Pose3(Rot3::Ypr(M_PI/4.0,0,0), Point3(2,2,0)), expmap_default(x3, sqrt(2.0) * x_step), tol)); } /* ************************************************************************* */ @@ -715,7 +715,42 @@ TEST( Pose3, ExpmapDerivative1) { } /* ************************************************************************* */ -TEST( Pose3, LogmapDerivative1) { +TEST(Pose3, ExpmapDerivative2) { + // Iserles05an (Lie-group Methods) says: + // scalar is easy: d exp(a(t)) / dt = exp(a(t)) a'(t) + // matrix is hard: d exp(A(t)) / dt = exp(A(t)) dexp[-A(t)] A'(t) + // where A(t): T -> se(3) is a trajectory in the tangent space of SE(3) + // and dexp[A] is a linear map from 4*4 to 4*4 derivatives of se(3) + // Hence, the above matrix equation is typed: 4*4 = SE(3) * linear_map(4*4) + + // In GTSAM, we don't work with the Lie-algebra elements A directly, but with 6-vectors. + // xi is easy: d Expmap(xi(t)) / dt = ExmapDerivative[xi(t)] * xi'(t) + + // Let's verify the above formula. + + auto xi = [](double t) { + Vector6 v; + v << 2 * t, sin(t), 4 * t * t, 2 * t, sin(t), 4 * t * t; + return v; + }; + auto xi_dot = [](double t) { + Vector6 v; + v << 2, cos(t), 8 * t, 2, cos(t), 8 * t; + return v; + }; + + // We define a function T + auto T = [xi](double t) { return Pose3::Expmap(xi(t)); }; + + for (double t = -2.0; t < 2.0; t += 0.3) { + const Matrix expected = numericalDerivative11(T, t); + const Matrix actual = Pose3::ExpmapDerivative(xi(t)) * xi_dot(t); + CHECK(assert_equal(expected, actual, 1e-7)); + } +} + +/* ************************************************************************* */ +TEST( Pose3, LogmapDerivative) { Matrix6 actualH; Vector6 w; w << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0; Pose3 p = Pose3::Expmap(w); diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index a61467b82..25ddd16ce 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -244,44 +244,99 @@ TEST(Rot3, retract_localCoordinates2) EXPECT(assert_equal(t1, t2.retract(d21))); } /* ************************************************************************* */ -Vector w = Vector3(0.1, 0.27, -0.2); - -// Left trivialization Derivative of exp(w) wrpt w: +namespace exmap_derivative { +static const Vector3 w(0.1, 0.27, -0.2); +} +// Left trivialized Derivative of exp(w) wrpt w: // How does exp(w) change when w changes? // We find a y such that: exp(w) exp(y) = exp(w + dw) for dw --> 0 // => y = log (exp(-w) * exp(w+dw)) Vector3 testDexpL(const Vector3& dw) { + using exmap_derivative::w; return Rot3::Logmap(Rot3::Expmap(-w) * Rot3::Expmap(w + dw)); } TEST( Rot3, ExpmapDerivative) { - Matrix actualDexpL = Rot3::ExpmapDerivative(w); - Matrix expectedDexpL = numericalDerivative11(testDexpL, + using exmap_derivative::w; + const Matrix actualDexpL = Rot3::ExpmapDerivative(w); + const Matrix expectedDexpL = numericalDerivative11(testDexpL, Vector3::Zero(), 1e-2); EXPECT(assert_equal(expectedDexpL, actualDexpL,1e-7)); - Matrix actualDexpInvL = Rot3::LogmapDerivative(w); + const Matrix actualDexpInvL = Rot3::LogmapDerivative(w); EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL,1e-7)); + } + +/* ************************************************************************* */ +TEST( Rot3, ExpmapDerivative2) +{ + const Vector3 theta(0.1, 0, 0.1); + const Matrix Jexpected = numericalDerivative11( + boost::bind(&Rot3::Expmap, _1, boost::none), theta); + + CHECK(assert_equal(Jexpected, Rot3::ExpmapDerivative(theta))); + CHECK(assert_equal(Matrix3(Jexpected.transpose()), Rot3::ExpmapDerivative(-theta))); } /* ************************************************************************* */ -Vector3 thetahat(0.1, 0, 0.1); -TEST( Rot3, ExpmapDerivative2) +TEST( Rot3, ExpmapDerivative3) { - Matrix Jexpected = numericalDerivative11( - boost::bind(&Rot3::Expmap, _1, boost::none), thetahat); + const Vector3 theta(10, 20, 30); + const Matrix Jexpected = numericalDerivative11( + boost::bind(&Rot3::Expmap, _1, boost::none), theta); - Matrix Jactual = Rot3::ExpmapDerivative(thetahat); - CHECK(assert_equal(Jexpected, Jactual)); + CHECK(assert_equal(Jexpected, Rot3::ExpmapDerivative(theta))); + CHECK(assert_equal(Matrix3(Jexpected.transpose()), Rot3::ExpmapDerivative(-theta))); +} - Matrix Jactual2 = Rot3::ExpmapDerivative(thetahat); - CHECK(assert_equal(Jexpected, Jactual2)); +/* ************************************************************************* */ +TEST(Rot3, ExpmapDerivative4) { + // Iserles05an (Lie-group Methods) says: + // scalar is easy: d exp(a(t)) / dt = exp(a(t)) a'(t) + // matrix is hard: d exp(A(t)) / dt = exp(A(t)) dexp[-A(t)] A'(t) + // where A(t): R -> so(3) is a trajectory in the tangent space of SO(3) + // and dexp[A] is a linear map from 3*3 to 3*3 derivatives of se(3) + // Hence, the above matrix equation is typed: 3*3 = SO(3) * linear_map(3*3) + + // In GTSAM, we don't work with the skew-symmetric matrices A directly, but with 3-vectors. + // omega is easy: d Expmap(w(t)) / dt = ExmapDerivative[w(t)] * w'(t) + + // Let's verify the above formula. + + auto w = [](double t) { return Vector3(2 * t, sin(t), 4 * t * t); }; + auto w_dot = [](double t) { return Vector3(2, cos(t), 8 * t); }; + + // We define a function R + auto R = [w](double t) { return Rot3::Expmap(w(t)); }; + + for (double t = -2.0; t < 2.0; t += 0.3) { + const Matrix expected = numericalDerivative11(R, t); + const Matrix actual = Rot3::ExpmapDerivative(w(t)) * w_dot(t); + CHECK(assert_equal(expected, actual, 1e-7)); + } +} + +/* ************************************************************************* */ +TEST(Rot3, ExpmapDerivative5) { + auto w = [](double t) { return Vector3(2 * t, sin(t), 4 * t * t); }; + auto w_dot = [](double t) { return Vector3(2, cos(t), 8 * t); }; + + // Same as above, but define R as mapping local coordinates to neighborhood aroud R0. + const Rot3 R0 = Rot3::Rodrigues(0.1, 0.4, 0.2); + auto R = [R0, w](double t) { return R0.expmap(w(t)); }; + + for (double t = -2.0; t < 2.0; t += 0.3) { + const Matrix expected = numericalDerivative11(R, t); + const Matrix actual = Rot3::ExpmapDerivative(w(t)) * w_dot(t); + CHECK(assert_equal(expected, actual, 1e-7)); + } } /* ************************************************************************* */ TEST( Rot3, jacobianExpmap ) { - Matrix Jexpected = numericalDerivative11(boost::bind( + const Vector3 thetahat(0.1, 0, 0.1); + const Matrix Jexpected = numericalDerivative11(boost::bind( &Rot3::Expmap, _1, boost::none), thetahat); Matrix3 Jactual; const Rot3 R = Rot3::Expmap(thetahat, Jactual); @@ -291,18 +346,20 @@ TEST( Rot3, jacobianExpmap ) /* ************************************************************************* */ TEST( Rot3, LogmapDerivative ) { - Rot3 R = Rot3::Expmap(thetahat); // some rotation - Matrix Jexpected = numericalDerivative11(boost::bind( + const Vector3 thetahat(0.1, 0, 0.1); + const Rot3 R = Rot3::Expmap(thetahat); // some rotation + const Matrix Jexpected = numericalDerivative11(boost::bind( &Rot3::Logmap, _1, boost::none), R); - Matrix3 Jactual = Rot3::LogmapDerivative(thetahat); + const Matrix3 Jactual = Rot3::LogmapDerivative(thetahat); EXPECT(assert_equal(Jexpected, Jactual)); } /* ************************************************************************* */ -TEST( Rot3, jacobianLogmap ) +TEST( Rot3, JacobianLogmap ) { - Rot3 R = Rot3::Expmap(thetahat); // some rotation - Matrix Jexpected = numericalDerivative11(boost::bind( + const Vector3 thetahat(0.1, 0, 0.1); + const Rot3 R = Rot3::Expmap(thetahat); // some rotation + const Matrix Jexpected = numericalDerivative11(boost::bind( &Rot3::Logmap, _1, boost::none), R); Matrix3 Jactual; Rot3::Logmap(R, Jactual); @@ -501,17 +558,17 @@ TEST( Rot3, yaw_pitch_roll ) double t = 0.1; // yaw is around z axis - CHECK(assert_equal(Rot3::Rz(t),Rot3::yaw(t))); + CHECK(assert_equal(Rot3::Rz(t),Rot3::Yaw(t))); // pitch is around y axis - CHECK(assert_equal(Rot3::Ry(t),Rot3::pitch(t))); + CHECK(assert_equal(Rot3::Ry(t),Rot3::Pitch(t))); // roll is around x axis - CHECK(assert_equal(Rot3::Rx(t),Rot3::roll(t))); + CHECK(assert_equal(Rot3::Rx(t),Rot3::Roll(t))); // Check compound rotation - Rot3 expected = Rot3::yaw(0.1) * Rot3::pitch(0.2) * Rot3::roll(0.3); - CHECK(assert_equal(expected,Rot3::ypr(0.1,0.2,0.3))); + Rot3 expected = Rot3::Yaw(0.1) * Rot3::Pitch(0.2) * Rot3::Roll(0.3); + CHECK(assert_equal(expected,Rot3::Ypr(0.1,0.2,0.3))); CHECK(assert_equal((Vector)Vector3(0.1, 0.2, 0.3),expected.ypr())); } @@ -531,14 +588,14 @@ TEST( Rot3, RQ) CHECK(assert_equal(expected,R.xyz(),1e-6)); CHECK(assert_equal((Vector)Vector3(0.1,0.2,0.3),Rot3::RzRyRx(0.1,0.2,0.3).xyz())); - // Try using ypr call, asserting that Rot3::ypr(y,p,r).ypr()==[y;p;r] - CHECK(assert_equal((Vector)Vector3(0.1,0.2,0.3),Rot3::ypr(0.1,0.2,0.3).ypr())); - CHECK(assert_equal((Vector)Vector3(0.3,0.2,0.1),Rot3::ypr(0.1,0.2,0.3).rpy())); + // Try using ypr call, asserting that Rot3::Ypr(y,p,r).ypr()==[y;p;r] + CHECK(assert_equal((Vector)Vector3(0.1,0.2,0.3),Rot3::Ypr(0.1,0.2,0.3).ypr())); + CHECK(assert_equal((Vector)Vector3(0.3,0.2,0.1),Rot3::Ypr(0.1,0.2,0.3).rpy())); // Try ypr for pure yaw-pitch-roll matrices - CHECK(assert_equal((Vector)Vector3(0.1,0.0,0.0),Rot3::yaw (0.1).ypr())); - CHECK(assert_equal((Vector)Vector3(0.0,0.1,0.0),Rot3::pitch(0.1).ypr())); - CHECK(assert_equal((Vector)Vector3(0.0,0.0,0.1),Rot3::roll (0.1).ypr())); + CHECK(assert_equal((Vector)Vector3(0.1,0.0,0.0),Rot3::Yaw (0.1).ypr())); + CHECK(assert_equal((Vector)Vector3(0.0,0.1,0.0),Rot3::Pitch(0.1).ypr())); + CHECK(assert_equal((Vector)Vector3(0.0,0.0,0.1),Rot3::Roll (0.1).ypr())); // Try RQ to recover calibration from 3*3 sub-block of projection matrix Matrix K = (Matrix(3, 3) << 500.0, 0.0, 320.0, 0.0, 500.0, 240.0, 0.0, 0.0, 1.0).finished(); @@ -594,9 +651,9 @@ TEST(Rot3, quaternion) { // Check creating Rot3 from quaternion EXPECT(assert_equal(R1, Rot3(q1))); - EXPECT(assert_equal(R1, Rot3::quaternion(q1.w(), q1.x(), q1.y(), q1.z()))); + EXPECT(assert_equal(R1, Rot3::Quaternion(q1.w(), q1.x(), q1.y(), q1.z()))); EXPECT(assert_equal(R2, Rot3(q2))); - EXPECT(assert_equal(R2, Rot3::quaternion(q2.w(), q2.x(), q2.y(), q2.z()))); + EXPECT(assert_equal(R2, Rot3::Quaternion(q2.w(), q2.x(), q2.y(), q2.z()))); // Check converting Rot3 to quaterion EXPECT(assert_equal(Vector(R1.toQuaternion().coeffs()), Vector(q1.coeffs()))); diff --git a/gtsam/geometry/tests/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp index bd18143cb..c3df95abc 100644 --- a/gtsam/geometry/tests/testTriangulation.cpp +++ b/gtsam/geometry/tests/testTriangulation.cpp @@ -40,7 +40,7 @@ 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 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); @@ -150,7 +150,7 @@ TEST( triangulation, fourPoses) { EXPECT(assert_equal(landmark, *actual2, 1e-2)); // 3. Add a slightly rotated third camera above, again with measurement noise - Pose3 pose3 = pose1 * Pose3(Rot3::ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); + Pose3 pose3 = pose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); SimpleCamera camera3(pose3, *sharedCal); Point2 z3 = camera3.project(landmark); @@ -167,7 +167,7 @@ TEST( triangulation, fourPoses) { EXPECT(assert_equal(landmark, *triangulated_3cameras_opt, 1e-2)); // 4. Test failure: Add a 4th camera facing the wrong way - Pose3 pose4 = Pose3(Rot3::ypr(M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); + Pose3 pose4 = Pose3(Rot3::Ypr(M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); SimpleCamera camera4(pose4, *sharedCal); #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION @@ -214,7 +214,7 @@ TEST( triangulation, fourPoses_distinct_Ks) { EXPECT(assert_equal(landmark, *actual2, 1e-2)); // 3. Add a slightly rotated third camera above, again with measurement noise - Pose3 pose3 = pose1 * Pose3(Rot3::ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); + Pose3 pose3 = pose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); Cal3_S2 K3(700, 500, 0, 640, 480); SimpleCamera camera3(pose3, K3); Point2 z3 = camera3.project(landmark); @@ -232,7 +232,7 @@ TEST( triangulation, fourPoses_distinct_Ks) { EXPECT(assert_equal(landmark, *triangulated_3cameras_opt, 1e-2)); // 4. Test failure: Add a 4th camera facing the wrong way - Pose3 pose4 = Pose3(Rot3::ypr(M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); + Pose3 pose4 = Pose3(Rot3::Ypr(M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); Cal3_S2 K4(700, 500, 0, 640, 480); SimpleCamera camera4(pose4, K4); diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp index 3cfffa0da..dbe315807 100644 --- a/gtsam/geometry/tests/testUnit3.cpp +++ b/gtsam/geometry/tests/testUnit3.cpp @@ -70,7 +70,7 @@ static Unit3 rotate_(const Rot3& R, const Unit3& p) { } TEST(Unit3, rotate) { - Rot3 R = Rot3::yaw(0.5); + Rot3 R = Rot3::Yaw(0.5); Unit3 p(1, 0, 0); Unit3 expected = Unit3(R.column(1)); Unit3 actual = R * p; @@ -95,7 +95,7 @@ static Unit3 unrotate_(const Rot3& R, const Unit3& p) { } TEST(Unit3, unrotate) { - Rot3 R = Rot3::yaw(-M_PI / 4.0); + Rot3 R = Rot3::Yaw(-M_PI / 4.0); Unit3 p(1, 0, 0); Unit3 expected = Unit3(1, 1, 0); Unit3 actual = R.unrotate(p); diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index bec901830..c6e613b14 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -19,6 +19,7 @@ #pragma once #include +#include #include #include #include diff --git a/gtsam/linear/GaussianBayesNet.cpp b/gtsam/linear/GaussianBayesNet.cpp index b3b8b9a41..6db13adb6 100644 --- a/gtsam/linear/GaussianBayesNet.cpp +++ b/gtsam/linear/GaussianBayesNet.cpp @@ -141,7 +141,20 @@ namespace gtsam { /* ************************************************************************* */ pair GaussianBayesNet::matrix() const { - return GaussianFactorGraph(*this).jacobian(); + GaussianFactorGraph factorGraph(*this); + KeySet keys = factorGraph.keys(); + // add frontal keys in order + Ordering ordering; + BOOST_FOREACH (const sharedConditional& cg, *this) + BOOST_FOREACH (Key key, cg->frontals()) { + ordering.push_back(key); + keys.erase(key); + } + // add remaining keys in case Bayes net is incomplete + BOOST_FOREACH (Key key, keys) + ordering.push_back(key); + // return matrix and RHS + return factorGraph.jacobian(ordering); } ///* ************************************************************************* */ diff --git a/gtsam/navigation/GPSFactor.cpp b/gtsam/navigation/GPSFactor.cpp index bfd3ebb52..87913cda6 100644 --- a/gtsam/navigation/GPSFactor.cpp +++ b/gtsam/navigation/GPSFactor.cpp @@ -58,10 +58,10 @@ pair GPSFactor::EstimateState(double t1, const Point3& NED1, // Estimate Rotation double yaw = atan2(nV.y(), nV.x()); - Rot3 nRy = Rot3::yaw(yaw); // yaw frame + Rot3 nRy = Rot3::Yaw(yaw); // yaw frame Point3 yV = nRy.inverse() * nV; // velocity in yaw frame double pitch = -atan2(yV.z(), yV.x()), roll = 0; - Rot3 nRb = Rot3::ypr(yaw, pitch, roll); + Rot3 nRb = Rot3::Ypr(yaw, pitch, roll); // Construct initial pose Pose3 nTb(nRb, nT); // nTb diff --git a/gtsam/navigation/MagFactor.h b/gtsam/navigation/MagFactor.h index f70bec8c6..fc1e69190 100644 --- a/gtsam/navigation/MagFactor.h +++ b/gtsam/navigation/MagFactor.h @@ -60,7 +60,7 @@ public: static Point3 unrotate(const Rot2& R, const Point3& p, boost::optional HR = boost::none) { - Point3 q = Rot3::yaw(R.theta()).unrotate(p, HR, boost::none); + Point3 q = Rot3::Yaw(R.theta()).unrotate(p, HR, boost::none); if (HR) { // assign to temporary first to avoid error in Win-Debug mode Matrix H = HR->col(2); diff --git a/gtsam/navigation/tests/testAHRSFactor.cpp b/gtsam/navigation/tests/testAHRSFactor.cpp index 2121eda35..02911acb1 100644 --- a/gtsam/navigation/tests/testAHRSFactor.cpp +++ b/gtsam/navigation/tests/testAHRSFactor.cpp @@ -423,7 +423,7 @@ TEST (AHRSFactor, predictTest) { // Predict Rot3 x; - Rot3 expectedRot = Rot3().ypr(20*M_PI, 0, 0); + Rot3 expectedRot = Rot3::Ypr(20*M_PI, 0, 0); Rot3 actualRot = factor.predict(x, bias, pim, kZeroOmegaCoriolis); EXPECT(assert_equal(expectedRot, actualRot, 1e-6)); diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index d473207da..e3d366d55 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -293,10 +293,10 @@ TEST(CombinedImuFactor, PredictRotation) { gravity, omegaCoriolis); // Predict - Pose3 x(Rot3().ypr(0, 0, 0), Point3(0, 0, 0)), x2; + Pose3 x(Rot3::Ypr(0, 0, 0), Point3(0, 0, 0)), x2; Vector3 v(0, 0, 0), v2; CombinedImuFactor::Predict(x, v, x2, v2, bias, pim, gravity, omegaCoriolis); - Pose3 expectedPose(Rot3().ypr(M_PI / 10, 0, 0), Point3(0, 0, 0)); + Pose3 expectedPose(Rot3::Ypr(M_PI / 10, 0, 0), Point3(0, 0, 0)); EXPECT(assert_equal(expectedPose, x2, tol)); } diff --git a/gtsam/navigation/tests/testGPSFactor.cpp b/gtsam/navigation/tests/testGPSFactor.cpp index 8c93020c9..6149c1651 100644 --- a/gtsam/navigation/tests/testGPSFactor.cpp +++ b/gtsam/navigation/tests/testGPSFactor.cpp @@ -89,7 +89,7 @@ TEST(GPSData, init) { // Check values values EXPECT(assert_equal((Vector )Vector3(29.9575, -29.0564, -1.95993), nV, 1e-4)); - EXPECT( assert_equal(Rot3::ypr(-0.770131, 0.046928, 0), T.rotation(), 1e-5)); + EXPECT( assert_equal(Rot3::Ypr(-0.770131, 0.046928, 0), T.rotation(), 1e-5)); Point3 expectedT(2.38461, -2.31289, -0.156011); EXPECT(assert_equal(expectedT, T.translation(), 1e-5)); } diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 25a6e732c..b7893281d 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -813,7 +813,7 @@ TEST(ImuFactor, PredictRotation) { Vector3 v2; ImuFactor::Predict(x1, v1, x2, v2, bias, pim, kGravityAlongNavZDown, kZeroOmegaCoriolis); - Pose3 expectedPose(Rot3().ypr(M_PI / 10, 0, 0), Point3(0, 0, 0)); + Pose3 expectedPose(Rot3::Ypr(M_PI / 10, 0, 0), Point3(0, 0, 0)); Vector3 expectedVelocity; expectedVelocity << 0, 0, 0; EXPECT(assert_equal(expectedPose, x2)); @@ -891,7 +891,7 @@ TEST(ImuFactor, bodyPSensorNoBias) { double dt = 0.001; // Rotate sensor (z-down) to body (same as navigation) i.e. z-up - Pose3 body_P_sensor(Rot3::ypr(0, 0, M_PI), Point3(0, 0, 0)); + Pose3 body_P_sensor(Rot3::Ypr(0, 0, M_PI), Point3(0, 0, 0)); ImuFactor::PreintegratedMeasurements pim(bias, Z_3x3, Z_3x3, Z_3x3, true); @@ -907,7 +907,7 @@ TEST(ImuFactor, bodyPSensorNoBias) { PoseVelocityBias poseVelocity = pim.predict(x1, v1, bias, n_gravity, omegaCoriolis); - Pose3 expectedPose(Rot3().ypr(-M_PI / 10, 0, 0), Point3(0, 0, 0)); + Pose3 expectedPose(Rot3::Ypr(-M_PI / 10, 0, 0), Point3(0, 0, 0)); EXPECT(assert_equal(expectedPose, poseVelocity.pose)); Vector3 expectedVelocity(0, 0, 0); @@ -942,7 +942,7 @@ TEST(ImuFactor, bodyPSensorWithBias) { // table exerts an equal and opposite force w.r.t gravity Vector3 measuredAcc(0, 0, -9.81); - Pose3 body_P_sensor(Rot3::ypr(0, 0, M_PI), Point3()); + Pose3 body_P_sensor(Rot3::Ypr(0, 0, M_PI), Point3()); Matrix3 accCov = 1e-7 * I_3x3; Matrix3 gyroCov = 1e-8 * I_3x3; @@ -1025,7 +1025,7 @@ TEST(ImuFactor, serialization) { using namespace gtsam::serializationTestHelpers; Vector3 n_gravity(0, 0, -9.81); - Pose3 body_P_sensor(Rot3::ypr(0, 0, M_PI), Point3()); + Pose3 body_P_sensor(Rot3::Ypr(0, 0, M_PI), Point3()); Matrix3 accCov = 1e-7 * I_3x3; Matrix3 gyroCov = 1e-8 * I_3x3; Matrix3 integrationCov = 1e-9 * I_3x3; diff --git a/gtsam/navigation/tests/testMagFactor.cpp b/gtsam/navigation/tests/testMagFactor.cpp index 950c6ce63..38aecfcbc 100644 --- a/gtsam/navigation/tests/testMagFactor.cpp +++ b/gtsam/navigation/tests/testMagFactor.cpp @@ -40,7 +40,7 @@ Point3 nM(22653.29982, -1956.83010, 44202.47862); // Let's assume scale factor, double scale = 255.0 / 50000.0; // ...ground truth orientation, -Rot3 nRb = Rot3::yaw(-0.1); +Rot3 nRb = Rot3::Yaw(-0.1); Rot2 theta = nRb.yaw(); // ...and bias Point3 bias(10, -10, 50); diff --git a/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp index 377d6cd34..16117845e 100644 --- a/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp +++ b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp @@ -62,7 +62,7 @@ using namespace gtsam; // Check that ceres rotation convention is the same TEST(AdaptAutoDiff, Rotation) { Vector3 axisAngle(0.1, 0.2, 0.3); - Matrix3 expected = Rot3::rodriguez(axisAngle).matrix(); + Matrix3 expected = Rot3::Rodrigues(axisAngle).matrix(); Matrix3 actual; ceres::AngleAxisToRotationMatrix(axisAngle.data(), actual.data()); EXPECT(assert_equal(expected, actual)); diff --git a/gtsam/sam/RangeFactor.h b/gtsam/sam/RangeFactor.h index 1ad27865d..a5bcac822 100644 --- a/gtsam/sam/RangeFactor.h +++ b/gtsam/sam/RangeFactor.h @@ -31,8 +31,7 @@ struct Range; * Works for any two types A1,A2 for which the functor Range() is defined * @addtogroup SAM */ -template ::result_type> +template class RangeFactor : public ExpressionFactor2 { private: typedef RangeFactor This; diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 68c27e76b..50516afe4 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -510,7 +510,7 @@ GraphAndValues load3D(const string& filename) { Key id; double x, y, z, roll, pitch, yaw; ls >> id >> x >> y >> z >> roll >> pitch >> yaw; - Rot3 R = Rot3::ypr(yaw,pitch,roll); + Rot3 R = Rot3::Ypr(yaw,pitch,roll); Point3 t = Point3(x, y, z); initial->insert(id, Pose3(R,t)); } @@ -518,7 +518,7 @@ GraphAndValues load3D(const string& filename) { Key id; double x, y, z, qx, qy, qz, qw; ls >> id >> x >> y >> z >> qx >> qy >> qz >> qw; - Rot3 R = Rot3::quaternion(qw, qx, qy, qz); + Rot3 R = Rot3::Quaternion(qw, qx, qy, qz); Point3 t = Point3(x, y, z); initial->insert(id, Pose3(R,t)); } @@ -537,7 +537,7 @@ GraphAndValues load3D(const string& filename) { Key id1, id2; double x, y, z, roll, pitch, yaw; ls >> id1 >> id2 >> x >> y >> z >> roll >> pitch >> yaw; - Rot3 R = Rot3::ypr(yaw,pitch,roll); + Rot3 R = Rot3::Ypr(yaw,pitch,roll); Point3 t = Point3(x, y, z); Matrix m = eye(6); for (int i = 0; i < 6; i++) @@ -553,7 +553,7 @@ GraphAndValues load3D(const string& filename) { Key id1, id2; double x, y, z, qx, qy, qz, qw; ls >> id1 >> id2 >> x >> y >> z >> qx >> qy >> qz >> qw; - Rot3 R = Rot3::quaternion(qw, qx, qy, qz); + Rot3 R = Rot3::Quaternion(qw, qx, qy, qz); Point3 t = Point3(x, y, z); for (int i = 0; i < 6; i++){ for (int j = i; j < 6; j++){ diff --git a/gtsam/slam/tests/smartFactorScenarios.h b/gtsam/slam/tests/smartFactorScenarios.h index 8e83ec503..ccad83f01 100644 --- a/gtsam/slam/tests/smartFactorScenarios.h +++ b/gtsam/slam/tests/smartFactorScenarios.h @@ -34,7 +34,7 @@ Point3 landmark4(10, 0.5, 1.2); Point3 landmark5(10, -0.5, 1.2); // First camera pose, looking along X-axis, 1 meter above ground plane (x-y) -Pose3 level_pose = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); +Pose3 level_pose = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); // Second camera 1 meter to the right of first camera Pose3 pose_right = level_pose * Pose3(Rot3(), Point3(1, 0, 0)); // Third camera 1 meter above the first camera @@ -123,7 +123,7 @@ Camera cam3(pose_above, sharedBundlerK); template CAMERA perturbCameraPose(const CAMERA& camera) { - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10), + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), Point3(0.5, 0.1, 0.3)); Pose3 cameraPose = camera.pose(); Pose3 perturbedCameraPose = cameraPose.compose(noise_pose); diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index 0406c3d27..4e8e3dbf3 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -137,23 +137,23 @@ TEST( dataSet, readG2o3D) boost::tie(actualGraph, actualValues) = readG2o(g2oFile, is3D); Values expectedValues; - Rot3 R0 = Rot3::quaternion(1.000000, 0.000000, 0.000000, 0.000000 ); + Rot3 R0 = Rot3::Quaternion(1.000000, 0.000000, 0.000000, 0.000000 ); Point3 p0 = Point3(0.000000, 0.000000, 0.000000); expectedValues.insert(0, Pose3(R0, p0)); - Rot3 R1 = Rot3::quaternion(0.854230, 0.190253, 0.283162, -0.392318 ); + Rot3 R1 = Rot3::Quaternion(0.854230, 0.190253, 0.283162, -0.392318 ); Point3 p1 = Point3(1.001367, 0.015390, 0.004948); expectedValues.insert(1, Pose3(R1, p1)); - Rot3 R2 = Rot3::quaternion(0.421446, -0.351729, -0.597838, 0.584174 ); + Rot3 R2 = Rot3::Quaternion(0.421446, -0.351729, -0.597838, 0.584174 ); Point3 p2 = Point3(1.993500, 0.023275, 0.003793); expectedValues.insert(2, Pose3(R2, p2)); - Rot3 R3 = Rot3::quaternion(0.067024, 0.331798, -0.200659, 0.919323); + Rot3 R3 = Rot3::Quaternion(0.067024, 0.331798, -0.200659, 0.919323); Point3 p3 = Point3(2.004291, 1.024305, 0.018047); expectedValues.insert(3, Pose3(R3, p3)); - Rot3 R4 = Rot3::quaternion(0.765488, -0.035697, -0.462490, 0.445933); + Rot3 R4 = Rot3::Quaternion(0.765488, -0.035697, -0.462490, 0.445933); Point3 p4 = Point3(0.999908, 1.055073, 0.020212); expectedValues.insert(4, Pose3(R4, p4)); @@ -163,27 +163,27 @@ TEST( dataSet, readG2o3D) NonlinearFactorGraph expectedGraph; Point3 p01 = Point3(1.001367, 0.015390, 0.004948); - Rot3 R01 = Rot3::quaternion(0.854230, 0.190253, 0.283162, -0.392318 ); + Rot3 R01 = Rot3::Quaternion(0.854230, 0.190253, 0.283162, -0.392318 ); expectedGraph.add(BetweenFactor(0, 1, Pose3(R01,p01), model)); Point3 p12 = Point3(0.523923, 0.776654, 0.326659); - Rot3 R12 = Rot3::quaternion(0.105373 , 0.311512, 0.656877, -0.678505 ); + Rot3 R12 = Rot3::Quaternion(0.105373 , 0.311512, 0.656877, -0.678505 ); expectedGraph.add(BetweenFactor(1, 2, Pose3(R12,p12), model)); Point3 p23 = Point3(0.910927, 0.055169, -0.411761); - Rot3 R23 = Rot3::quaternion(0.568551 , 0.595795, -0.561677, 0.079353 ); + Rot3 R23 = Rot3::Quaternion(0.568551 , 0.595795, -0.561677, 0.079353 ); expectedGraph.add(BetweenFactor(2, 3, Pose3(R23,p23), model)); Point3 p34 = Point3(0.775288, 0.228798, -0.596923); - Rot3 R34 = Rot3::quaternion(0.542221 , -0.592077, 0.303380, -0.513226 ); + Rot3 R34 = Rot3::Quaternion(0.542221 , -0.592077, 0.303380, -0.513226 ); expectedGraph.add(BetweenFactor(3, 4, Pose3(R34,p34), model)); Point3 p14 = Point3(-0.577841, 0.628016, -0.543592); - Rot3 R14 = Rot3::quaternion(0.327419 , -0.125250, -0.534379, 0.769122 ); + Rot3 R14 = Rot3::Quaternion(0.327419 , -0.125250, -0.534379, 0.769122 ); expectedGraph.add(BetweenFactor(1, 4, Pose3(R14,p14), model)); Point3 p30 = Point3(-0.623267, 0.086928, 0.773222); - Rot3 R30 = Rot3::quaternion(0.083672 , 0.104639, 0.627755, 0.766795 ); + Rot3 R30 = Rot3::Quaternion(0.083672 , 0.104639, 0.627755, 0.766795 ); expectedGraph.add(BetweenFactor(3, 0, Pose3(R30,p30), model)); EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5)); @@ -199,11 +199,11 @@ TEST( dataSet, readG2o3DNonDiagonalNoise) boost::tie(actualGraph, actualValues) = readG2o(g2oFile, is3D); Values expectedValues; - Rot3 R0 = Rot3::quaternion(1.000000, 0.000000, 0.000000, 0.000000 ); + Rot3 R0 = Rot3::Quaternion(1.000000, 0.000000, 0.000000, 0.000000 ); Point3 p0 = Point3(0.000000, 0.000000, 0.000000); expectedValues.insert(0, Pose3(R0, p0)); - Rot3 R1 = Rot3::quaternion(0.854230, 0.190253, 0.283162, -0.392318 ); + Rot3 R1 = Rot3::Quaternion(0.854230, 0.190253, 0.283162, -0.392318 ); Point3 p1 = Point3(1.001367, 0.015390, 0.004948); expectedValues.insert(1, Pose3(R1, p1)); @@ -223,7 +223,7 @@ TEST( dataSet, readG2o3DNonDiagonalNoise) noiseModel::Gaussian::shared_ptr model = noiseModel::Gaussian::Covariance(Info.inverse()); NonlinearFactorGraph expectedGraph; Point3 p01 = Point3(1.001367, 0.015390, 0.004948); - Rot3 R01 = Rot3::quaternion(0.854230, 0.190253, 0.283162, -0.392318 ); + Rot3 R01 = Rot3::Quaternion(0.854230, 0.190253, 0.283162, -0.392318 ); expectedGraph.add(BetweenFactor(0, 1, Pose3(R01,p01), model)); EXPECT(assert_equal(expectedGraph,*actualGraph,1e-2)); @@ -452,7 +452,7 @@ TEST( dataSet, writeBALfromValues_Dubrovnik){ SfM_data readData; readBAL(filenameToRead, readData); - Pose3 poseChange = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.3,0.1,0.3)); + Pose3 poseChange = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.3,0.1,0.3)); Values value; for(size_t i=0; i < readData.number_cameras(); i++){ // for each camera diff --git a/gtsam/slam/tests/testOrientedPlane3Factor.cpp b/gtsam/slam/tests/testOrientedPlane3Factor.cpp index 4a7b4c3fe..f1e830d03 100644 --- a/gtsam/slam/tests/testOrientedPlane3Factor.cpp +++ b/gtsam/slam/tests/testOrientedPlane3Factor.cpp @@ -47,7 +47,7 @@ TEST (OrientedPlane3Factor, lm_translation_error) { // Init pose and prior. Pose Prior is needed since a single plane measurement does not fully constrain the pose Symbol init_sym('x', 0); - Pose3 init_pose(Rot3::ypr(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0)); + Pose3 init_pose(Rot3::Ypr(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0)); Vector sigmas(6); sigmas << 0.001, 0.001, 0.001, 0.001, 0.001, 0.001; PriorFactor pose_prior(init_sym, init_pose, @@ -94,7 +94,7 @@ TEST (OrientedPlane3Factor, lm_rotation_error) { // Init pose and prior. Pose Prior is needed since a single plane measurement does not fully constrain the pose Symbol init_sym('x', 0); - Pose3 init_pose(Rot3::ypr(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0)); + Pose3 init_pose(Rot3::Ypr(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0)); PriorFactor pose_prior(init_sym, init_pose, noiseModel::Diagonal::Sigmas( (Vector(6) << 0.001, 0.001, 0.001, 0.001, 0.001, 0.001).finished())); diff --git a/gtsam/slam/tests/testPoseRotationPrior.cpp b/gtsam/slam/tests/testPoseRotationPrior.cpp index db04a74eb..3c7d5f2b2 100644 --- a/gtsam/slam/tests/testPoseRotationPrior.cpp +++ b/gtsam/slam/tests/testPoseRotationPrior.cpp @@ -30,7 +30,7 @@ const gtsam::Key poseKey = 1; // Pose3 examples const Point3 point3A(1.0, 2.0, 3.0), point3B(4.0, 6.0, 8.0); -const Rot3 rot3A, rot3B = Rot3::pitch(-M_PI_2), rot3C = Rot3::Expmap(Vector3(0.1, 0.2, 0.3)); +const Rot3 rot3A, rot3B = Rot3::Pitch(-M_PI_2), rot3C = Rot3::Expmap(Vector3(0.1, 0.2, 0.3)); // Pose2 examples const Point2 point2A(1.0, 2.0), point2B(4.0, 6.0); diff --git a/gtsam/slam/tests/testPoseTranslationPrior.cpp b/gtsam/slam/tests/testPoseTranslationPrior.cpp index 2f39701f7..2fd471c9c 100644 --- a/gtsam/slam/tests/testPoseTranslationPrior.cpp +++ b/gtsam/slam/tests/testPoseTranslationPrior.cpp @@ -27,7 +27,7 @@ const gtsam::Key poseKey = 1; // Pose3 examples const Point3 point3A(1.0, 2.0, 3.0), point3B(4.0, 6.0, 8.0); -const Rot3 rot3A, rot3B = Rot3::pitch(-M_PI_2), rot3C = Rot3::RzRyRx(0.1, 0.2, 0.3); +const Rot3 rot3A, rot3B = Rot3::Pitch(-M_PI_2), rot3C = Rot3::RzRyRx(0.1, 0.2, 0.3); // Pose2 examples const Point2 point2A(1.0, 2.0), point2B(4.0, 6.0); diff --git a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp index 54bbd6c22..467aefe91 100644 --- a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp @@ -47,7 +47,7 @@ template PinholeCamera perturbCameraPoseAndCalibration( const PinholeCamera& camera) { GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION) - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10), + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), Point3(0.5, 0.1, 0.3)); Pose3 cameraPose = camera.pose(); Pose3 perturbedCameraPose = cameraPose.compose(noise_pose); @@ -61,7 +61,7 @@ PinholeCamera perturbCameraPoseAndCalibration( /* ************************************************************************* */ TEST( SmartProjectionCameraFactor, perturbCameraPose) { using namespace vanilla; - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10), + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), Point3(0.5, 0.1, 0.3)); Pose3 perturbed_level_pose = level_pose.compose(noise_pose); Camera actualCamera(perturbed_level_pose, K2); diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index 0e2429840..1c1bc3c03 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -163,7 +163,7 @@ TEST( SmartProjectionPoseFactor, noisy ) { Values values; values.insert(x1, cam1.pose()); - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10), + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), Point3(0.5, 0.1, 0.3)); values.insert(x2, pose_right.compose(noise_pose)); @@ -196,7 +196,7 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){ Cal3_S2::shared_ptr K(new Cal3_S2(fov,w,h)); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 cameraPose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); // body poses + Pose3 cameraPose1 = Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); // body poses Pose3 cameraPose2 = cameraPose1 * Pose3(Rot3(), Point3(1,0,0)); Pose3 cameraPose3 = cameraPose1 * Pose3(Rot3(), Point3(0,-1,0)); @@ -205,7 +205,7 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){ SimpleCamera cam3(cameraPose3, *K); // create arbitrary body_Pose_sensor (transforms from sensor to body) - Pose3 sensor_to_body = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(1, 1, 1)); // Pose3(); // + Pose3 sensor_to_body = Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(1, 1, 1)); // Pose3(); // // These are the poses we want to estimate, from camera measurements Pose3 bodyPose1 = cameraPose1.compose(sensor_to_body.inverse()); @@ -263,7 +263,7 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){ double expectedError = 0.0; DOUBLES_EQUAL(expectedError, actualError, 1e-7) - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); Values values; values.insert(x1, bodyPose1); values.insert(x2, bodyPose2); @@ -317,8 +317,8 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) { groundTruth.insert(x3, cam3.pose()); DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, cam1.pose()); @@ -539,8 +539,8 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) { graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); graph.push_back(PriorFactor(x2, cam2.pose(), noisePrior)); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, cam1.pose()); @@ -606,8 +606,8 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ) { graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); graph.push_back(PriorFactor(x2, cam2.pose(), noisePrior)); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, cam1.pose()); @@ -667,8 +667,8 @@ TEST( SmartProjectionPoseFactor, landmarkDistance ) { graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); graph.push_back(PriorFactor(x2, cam2.pose(), noisePrior)); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, cam1.pose()); @@ -792,7 +792,7 @@ TEST( SmartProjectionPoseFactor, jacobianQ ) { graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); graph.push_back(PriorFactor(x2, cam2.pose(), noisePrior)); - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, cam1.pose()); @@ -844,7 +844,7 @@ TEST( SmartProjectionPoseFactor, 3poses_projection_factor ) { graph.push_back(PriorFactor(x1, level_pose, noisePrior)); graph.push_back(PriorFactor(x2, pose_right, noisePrior)); - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10), + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), Point3(0.5, 0.1, 0.3)); Values values; values.insert(x1, level_pose); @@ -908,8 +908,8 @@ TEST( SmartProjectionPoseFactor, CheckHessian) { graph.push_back(smartFactor2); graph.push_back(smartFactor3); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, cam1.pose()); @@ -995,7 +995,7 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac graph.push_back( PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, cam1.pose()); @@ -1063,8 +1063,8 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ) graph.push_back( PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, cam1.pose()); @@ -1108,7 +1108,7 @@ TEST( SmartProjectionPoseFactor, Hessian ) { SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedK2)); smartFactor1->add(measurements_cam1, views); - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10), + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), Point3(0.5, 0.1, 0.3)); Values values; values.insert(x1, cam1.pose()); @@ -1148,7 +1148,7 @@ TEST( SmartProjectionPoseFactor, HessianWithRotation ) { boost::shared_ptr factor = smartFactorInstance->linearize( values); - Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0)); + Pose3 poseDrift = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0)); Values rotValues; rotValues.insert(x1, poseDrift.compose(level_pose)); @@ -1161,7 +1161,7 @@ TEST( SmartProjectionPoseFactor, HessianWithRotation ) { // Hessian is invariant to rotations in the nondegenerate case EXPECT(assert_equal(factor->information(), factorRot->information(), 1e-7)); - Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2), + Pose3 poseDrift2 = Pose3(Rot3::Ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2), Point3(10, -4, 5)); Values tranValues; @@ -1203,7 +1203,7 @@ TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ) { boost::shared_ptr factor = smartFactor->linearize(values); - Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0)); + Pose3 poseDrift = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0)); Values rotValues; rotValues.insert(x1, poseDrift.compose(level_pose)); @@ -1216,7 +1216,7 @@ TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ) { // Hessian is invariant to rotations in the nondegenerate case EXPECT(assert_equal(factor->information(), factorRot->information(), 1e-7)); - Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2), + Pose3 poseDrift2 = Pose3(Rot3::Ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2), Point3(10, -4, 5)); Values tranValues; @@ -1278,8 +1278,8 @@ TEST( SmartProjectionPoseFactor, Cal3Bundler ) { graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); graph.push_back(PriorFactor(x2, cam2.pose(), noisePrior)); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, cam1.pose()); @@ -1357,8 +1357,8 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) { graph.push_back( PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, cam1.pose()); diff --git a/gtsam/slam/tests/testTriangulationFactor.cpp b/gtsam/slam/tests/testTriangulationFactor.cpp index 5ac92b4a9..1d2baefee 100644 --- a/gtsam/slam/tests/testTriangulationFactor.cpp +++ b/gtsam/slam/tests/testTriangulationFactor.cpp @@ -37,7 +37,7 @@ 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 Rot3 upright = Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2); static const Pose3 pose1 = Pose3(upright, gtsam::Point3(0, 0, 1)); SimpleCamera camera1(pose1, *sharedCal); diff --git a/gtsam_unstable/dynamics/PoseRTV.cpp b/gtsam_unstable/dynamics/PoseRTV.cpp index 51737285a..1a7fdf3de 100644 --- a/gtsam_unstable/dynamics/PoseRTV.cpp +++ b/gtsam_unstable/dynamics/PoseRTV.cpp @@ -100,7 +100,7 @@ PoseRTV PoseRTV::flyingDynamics( double pitch2 = r2.pitch(); double forward_accel = -thrust * sin(pitch2); // r2, pitch (in global frame?) controls forward force double loss_lift = lift*fabs(sin(pitch2)); - Rot3 yaw_correction_bn = Rot3::yaw(yaw2); + Rot3 yaw_correction_bn = Rot3::Yaw(yaw2); Point3 forward(forward_accel, 0.0, 0.0); Vector Acc_n = yaw_correction_bn.rotate(forward).vector() // applies locally forward force in the global frame diff --git a/gtsam_unstable/dynamics/tests/testIMUSystem.cpp b/gtsam_unstable/dynamics/tests/testIMUSystem.cpp index 3fff06de1..3fc6a0197 100644 --- a/gtsam_unstable/dynamics/tests/testIMUSystem.cpp +++ b/gtsam_unstable/dynamics/tests/testIMUSystem.cpp @@ -56,9 +56,9 @@ TEST( testIMUSystem, optimize_chain ) { // create a simple chain of poses to generate IMU measurements const double dt = 1.0; PoseRTV pose1, - pose2(Point3(1.0, 1.0, 0.0), Rot3::ypr(0.1, 0.0, 0.0), Velocity3(2.0, 2.0, 0.0)), - pose3(Point3(2.0, 2.0, 0.0), Rot3::ypr(0.2, 0.0, 0.0), Velocity3(0.0, 0.0, 0.0)), - pose4(Point3(3.0, 3.0, 0.0), Rot3::ypr(0.3, 0.0, 0.0), Velocity3(2.0, 2.0, 0.0)); + pose2(Point3(1.0, 1.0, 0.0), Rot3::Ypr(0.1, 0.0, 0.0), Velocity3(2.0, 2.0, 0.0)), + pose3(Point3(2.0, 2.0, 0.0), Rot3::Ypr(0.2, 0.0, 0.0), Velocity3(0.0, 0.0, 0.0)), + pose4(Point3(3.0, 3.0, 0.0), Rot3::Ypr(0.3, 0.0, 0.0), Velocity3(2.0, 2.0, 0.0)); // create measurements SharedDiagonal model = noiseModel::Unit::Create(6); @@ -102,9 +102,9 @@ TEST( testIMUSystem, optimize_chain_fullfactor ) { // create a simple chain of poses to generate IMU measurements const double dt = 1.0; PoseRTV pose1, - pose2(Point3(1.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), Velocity3(1.0, 0.0, 0.0)), - pose3(Point3(2.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), Velocity3(1.0, 0.0, 0.0)), - pose4(Point3(3.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), Velocity3(1.0, 0.0, 0.0)); + pose2(Point3(1.0, 0.0, 0.0), Rot3::Ypr(0.0, 0.0, 0.0), Velocity3(1.0, 0.0, 0.0)), + pose3(Point3(2.0, 0.0, 0.0), Rot3::Ypr(0.0, 0.0, 0.0), Velocity3(1.0, 0.0, 0.0)), + pose4(Point3(3.0, 0.0, 0.0), Rot3::Ypr(0.0, 0.0, 0.0), Velocity3(1.0, 0.0, 0.0)); // create measurements SharedDiagonal model = noiseModel::Isotropic::Sigma(9, 1.0); diff --git a/gtsam_unstable/dynamics/tests/testPoseRTV.cpp b/gtsam_unstable/dynamics/tests/testPoseRTV.cpp index 0386d8bcd..db2f8f7f8 100644 --- a/gtsam_unstable/dynamics/tests/testPoseRTV.cpp +++ b/gtsam_unstable/dynamics/tests/testPoseRTV.cpp @@ -193,7 +193,7 @@ TEST( testPoseRTV, transformed_from_1 ) { Point3 T(1.0, 2.0, 3.0); Velocity3 V(2.0, 3.0, 4.0); PoseRTV start(R, T, V); - Pose3 transform(Rot3::yaw(M_PI_2), Point3(1.0, 2.0, 3.0)); + Pose3 transform(Rot3::Yaw(M_PI_2), Point3(1.0, 2.0, 3.0)); Matrix actDTrans, actDGlobal; PoseRTV actual = start.transformed_from(transform, actDGlobal, actDTrans); @@ -212,7 +212,7 @@ TEST( testPoseRTV, transformed_from_2 ) { Point3 T(1.0, 2.0, 3.0); Velocity3 V(2.0, 3.0, 4.0); PoseRTV start(R, T, V); - Pose3 transform(Rot3::yaw(M_PI_2), Point3(1.0, 2.0, 3.0)); + Pose3 transform(Rot3::Yaw(M_PI_2), Point3(1.0, 2.0, 3.0)); Matrix actDTrans, actDGlobal; PoseRTV actual = start.transformed_from(transform, actDGlobal, actDTrans); @@ -229,14 +229,14 @@ TEST( testPoseRTV, transformed_from_2 ) { TEST(testPoseRTV, RRTMbn) { EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMbn(kZero3))); EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMbn(Rot3()))); - EXPECT(assert_equal(PoseRTV::RRTMbn(Vector3(0.3, 0.2, 0.1)), PoseRTV::RRTMbn(Rot3::ypr(0.1, 0.2, 0.3)))); + EXPECT(assert_equal(PoseRTV::RRTMbn(Vector3(0.3, 0.2, 0.1)), PoseRTV::RRTMbn(Rot3::Ypr(0.1, 0.2, 0.3)))); } /* ************************************************************************* */ TEST(testPoseRTV, RRTMnb) { EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMnb(kZero3))); EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMnb(Rot3()))); - EXPECT(assert_equal(PoseRTV::RRTMnb(Vector3(0.3, 0.2, 0.1)), PoseRTV::RRTMnb(Rot3::ypr(0.1, 0.2, 0.3)))); + EXPECT(assert_equal(PoseRTV::RRTMnb(Vector3(0.3, 0.2, 0.1)), PoseRTV::RRTMnb(Rot3::Ypr(0.1, 0.2, 0.3)))); } /* ************************************************************************* */ diff --git a/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp b/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp index a0d969c4d..1fb2cf39e 100644 --- a/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp +++ b/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp @@ -16,7 +16,7 @@ const double tol=1e-5; const double h = 0.01; //const double deg2rad = M_PI/180.0; -//Pose3 g1(Rot3::ypr(deg2rad*10.0, deg2rad*20.0, deg2rad*30.0), Point3(100.0, 200.0, 300.0)); +//Pose3 g1(Rot3::Ypr(deg2rad*10.0, deg2rad*20.0, deg2rad*30.0), Point3(100.0, 200.0, 300.0)); Pose3 g1(Rot3(), Point3(100.0, 0.0, 300.0)); //Vector6 v1((Vector(6) << 0.1, 0.05, 0.02, 10.0, 20.0, 30.0).finished()); Vector6 V1_w((Vector(6) << 0.0, 0.0, M_PI/3, 0.0, 0.0, 30.0).finished()); diff --git a/gtsam_unstable/geometry/Pose3Upright.cpp b/gtsam_unstable/geometry/Pose3Upright.cpp index 2ab3a6a9e..f83cb19fb 100644 --- a/gtsam_unstable/geometry/Pose3Upright.cpp +++ b/gtsam_unstable/geometry/Pose3Upright.cpp @@ -64,7 +64,7 @@ Rot2 Pose3Upright::rotation2() const { /* ************************************************************************* */ Rot3 Pose3Upright::rotation() const { - return Rot3::yaw(theta()); + return Rot3::Yaw(theta()); } /* ************************************************************************* */ diff --git a/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp b/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp index 2f3c763dd..6ac3389ed 100644 --- a/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp +++ b/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp @@ -17,7 +17,7 @@ using namespace std; using namespace gtsam; static Cal3_S2::shared_ptr K(new Cal3_S2(1500, 1200, 0, 640, 480)); -Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); +Pose3 level_pose = Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); SimpleCamera level_camera(level_pose, *K); /* ************************************************************************* */ @@ -142,7 +142,7 @@ TEST(InvDepthFactor, backproject2) // backwards facing camera Vector expected((Vector(5) << -5.,-5.,2., 3., -0.1).finished()); double inv_depth(1./10); - InvDepthCamera3 inv_camera(Pose3(Rot3::ypr(1.5,0.1, -1.5), Point3(-5, -5, 2)),K); + InvDepthCamera3 inv_camera(Pose3(Rot3::Ypr(1.5,0.1, -1.5), Point3(-5, -5, 2)),K); Point2 z = inv_camera.project(expected, inv_depth); Vector5 actual_vec; diff --git a/gtsam_unstable/geometry/tests/testPose3Upright.cpp b/gtsam_unstable/geometry/tests/testPose3Upright.cpp index 0929035b6..cd54a8813 100644 --- a/gtsam_unstable/geometry/tests/testPose3Upright.cpp +++ b/gtsam_unstable/geometry/tests/testPose3Upright.cpp @@ -58,9 +58,9 @@ TEST( testPose3Upright, conversions ) { EXPECT(assert_equal(Point3(1.0, 2.0, 3.0), pose.translation(), tol)); EXPECT(assert_equal(Point2(1.0, 2.0), pose.translation2(), tol)); EXPECT(assert_equal(Rot2::fromAngle(0.1), pose.rotation2(), tol)); - EXPECT(assert_equal(Rot3::yaw(0.1), pose.rotation(), tol)); + EXPECT(assert_equal(Rot3::Yaw(0.1), pose.rotation(), tol)); EXPECT(assert_equal(Pose2(1.0, 2.0, 0.1), pose.pose2(), tol)); - EXPECT(assert_equal(Pose3(Rot3::yaw(0.1), Point3(1.0, 2.0, 3.0)), pose.pose(), tol)); + EXPECT(assert_equal(Pose3(Rot3::Yaw(0.1), Point3(1.0, 2.0, 3.0)), pose.pose(), tol)); } /* ************************************************************************* */ diff --git a/gtsam_unstable/geometry/tests/testSimilarity3.cpp b/gtsam_unstable/geometry/tests/testSimilarity3.cpp index 14dfb8f1a..849206a7d 100644 --- a/gtsam_unstable/geometry/tests/testSimilarity3.cpp +++ b/gtsam_unstable/geometry/tests/testSimilarity3.cpp @@ -56,14 +56,14 @@ TEST(Similarity3, Getters) { //****************************************************************************** TEST(Similarity3, Getters2) { - Similarity3 test(Rot3::ypr(1, 2, 3), Point3(4, 5, 6), 7); - EXPECT(assert_equal(Rot3::ypr(1, 2, 3), test.rotation())); + Similarity3 test(Rot3::Ypr(1, 2, 3), Point3(4, 5, 6), 7); + EXPECT(assert_equal(Rot3::Ypr(1, 2, 3), test.rotation())); EXPECT(assert_equal(Point3(4, 5, 6), test.translation())); EXPECT_DOUBLES_EQUAL(7.0, test.scale(), 1e-9); } TEST(Similarity3, AdjointMap) { - Similarity3 test(Rot3::ypr(1,2,3).inverse(), Point3(4,5,6), 7); + Similarity3 test(Rot3::Ypr(1,2,3).inverse(), Point3(4,5,6), 7); Matrix7 result; result << -1.5739, -2.4512, -6.3651, -50.7671, -11.2503, 16.8859, -28.0000, 6.3167, -2.9884, -0.4111, 0.8502, 8.6373, -49.7260, -35.0000, @@ -76,7 +76,7 @@ TEST(Similarity3, AdjointMap) { } TEST(Similarity3, inverse) { - Similarity3 test(Rot3::ypr(1,2,3).inverse(), Point3(4,5,6), 7); + Similarity3 test(Rot3::Ypr(1,2,3).inverse(), Point3(4,5,6), 7); Matrix3 Re; Re << -0.2248, 0.9024, -0.3676, -0.3502, -0.4269, -0.8337, @@ -87,8 +87,8 @@ TEST(Similarity3, inverse) { } TEST(Similarity3, multiplication) { - Similarity3 test1(Rot3::ypr(1,2,3).inverse(), Point3(4,5,6), 7); - Similarity3 test2(Rot3::ypr(1,2,3).inverse(), Point3(8,9,10), 11); + Similarity3 test1(Rot3::Ypr(1,2,3).inverse(), Point3(4,5,6), 7); + Similarity3 test2(Rot3::Ypr(1,2,3).inverse(), Point3(8,9,10), 11); Matrix3 re; re << 0.0688, 0.9863, -0.1496, -0.5665, -0.0848, -0.8197, @@ -117,14 +117,14 @@ TEST(Similarity3, Manifold) { v3 << 0, 0, 0, 1, 2, 3, 0; EXPECT(assert_equal(v3, sim2.localCoordinates(sim3))); -// Similarity3 other = Similarity3(Rot3::ypr(0.01, 0.02, 0.03), Point3(0.4, 0.5, 0.6), 1); - Similarity3 other = Similarity3(Rot3::ypr(0.1, 0.2, 0.3),Point3(4,5,6),1); +// Similarity3 other = Similarity3(Rot3::Ypr(0.01, 0.02, 0.03), Point3(0.4, 0.5, 0.6), 1); + Similarity3 other = Similarity3(Rot3::Ypr(0.1, 0.2, 0.3),Point3(4,5,6),1); Vector vlocal = sim.localCoordinates(other); EXPECT(assert_equal(sim.retract(vlocal), other, 1e-2)); - Similarity3 other2 = Similarity3(Rot3::ypr(0.3, 0, 0),Point3(4,5,6),1); + Similarity3 other2 = Similarity3(Rot3::Ypr(0.3, 0, 0),Point3(4,5,6),1); Rot3 R = Rot3::Rodrigues(0.3,0,0); Vector vlocal2 = sim.localCoordinates(other2); @@ -167,7 +167,7 @@ TEST(Similarity3, manifold_first_order) } TEST(Similarity3, Optimization) { - Similarity3 prior = Similarity3(Rot3::ypr(0.1, 0.2, 0.3), Point3(1, 2, 3), 4); + Similarity3 prior = Similarity3(Rot3::Ypr(0.1, 0.2, 0.3), Point3(1, 2, 3), 4); noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(7, 1); Symbol key('x',1); PriorFactor factor(key, prior, model); @@ -187,10 +187,10 @@ TEST(Similarity3, Optimization) { TEST(Similarity3, Optimization2) { Similarity3 prior = Similarity3(); - Similarity3 m1 = Similarity3(Rot3::ypr(M_PI/4.0, 0, 0), Point3(2.0, 0, 0), 1.0); - Similarity3 m2 = Similarity3(Rot3::ypr(M_PI/2.0, 0, 0), Point3(sqrt(8)*0.9, 0, 0), 1.0); - Similarity3 m3 = Similarity3(Rot3::ypr(3*M_PI/4.0, 0, 0), Point3(sqrt(32)*0.8, 0, 0), 1.0); - Similarity3 m4 = Similarity3(Rot3::ypr(M_PI/2.0, 0, 0), Point3(6*0.7, 0, 0), 1.0); + Similarity3 m1 = Similarity3(Rot3::Ypr(M_PI/4.0, 0, 0), Point3(2.0, 0, 0), 1.0); + Similarity3 m2 = Similarity3(Rot3::Ypr(M_PI/2.0, 0, 0), Point3(sqrt(8)*0.9, 0, 0), 1.0); + Similarity3 m3 = Similarity3(Rot3::Ypr(3*M_PI/4.0, 0, 0), Point3(sqrt(32)*0.8, 0, 0), 1.0); + Similarity3 m4 = Similarity3(Rot3::Ypr(M_PI/2.0, 0, 0), Point3(6*0.7, 0, 0), 1.0); Similarity3 loop = Similarity3(1.42); //prior.print("Goal Transform"); @@ -220,10 +220,10 @@ TEST(Similarity3, Optimization2) { Values initial; initial.insert(X(1), Similarity3()); - initial.insert(X(2), Similarity3(Rot3::ypr(M_PI/2.0, 0, 0), Point3(1, 0, 0), 1.1)); - initial.insert(X(3), Similarity3(Rot3::ypr(2.0*M_PI/2.0, 0, 0), Point3(0.9, 1.1, 0), 1.2)); - initial.insert(X(4), Similarity3(Rot3::ypr(3.0*M_PI/2.0, 0, 0), Point3(0, 1, 0), 1.3)); - initial.insert(X(5), Similarity3(Rot3::ypr(4.0*M_PI/2.0, 0, 0), Point3(0, 0, 0), 1.0)); + initial.insert(X(2), Similarity3(Rot3::Ypr(M_PI/2.0, 0, 0), Point3(1, 0, 0), 1.1)); + initial.insert(X(3), Similarity3(Rot3::Ypr(2.0*M_PI/2.0, 0, 0), Point3(0.9, 1.1, 0), 1.2)); + initial.insert(X(4), Similarity3(Rot3::Ypr(3.0*M_PI/2.0, 0, 0), Point3(0, 1, 0), 1.3)); + initial.insert(X(5), Similarity3(Rot3::Ypr(4.0*M_PI/2.0, 0, 0), Point3(0, 0, 0), 1.0)); //initial.print("Initial Estimate\n"); diff --git a/gtsam_unstable/slam/Mechanization_bRn2.cpp b/gtsam_unstable/slam/Mechanization_bRn2.cpp index f6f1c4a5c..20ffbdd4f 100644 --- a/gtsam_unstable/slam/Mechanization_bRn2.cpp +++ b/gtsam_unstable/slam/Mechanization_bRn2.cpp @@ -60,7 +60,7 @@ Mechanization_bRn2 Mechanization_bRn2::initialize(const Matrix& U, double pitch = atan2(-g1, sqrt(g2 * g2 + g3 * g3)); double yaw = 0; // This returns body-to-nav nRb - Rot3 bRn = Rot3::ypr(yaw, pitch, roll).inverse(); + Rot3 bRn = Rot3::Ypr(yaw, pitch, roll).inverse(); return Mechanization_bRn2(bRn, x_g, x_a); } diff --git a/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp b/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp index 407c9a345..99f494ad9 100644 --- a/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp +++ b/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp @@ -22,7 +22,7 @@ static Cal3_S2::shared_ptr K(new Cal3_S2(1500, 1200, 0, 640, 480)); static SharedNoiseModel sigma(noiseModel::Unit::Create(2)); // camera pose at (0,0,1) looking straight along the x-axis. -Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); +Pose3 level_pose = Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); SimpleCamera level_camera(level_pose, *K); typedef InvDepthFactor3 InverseDepthFactor; diff --git a/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp b/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp index 99a4f90a4..3aa758701 100644 --- a/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp +++ b/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp @@ -24,8 +24,8 @@ using namespace gtsam; TEST( InvDepthFactorVariant1, optimize) { // Create two poses looking in the x-direction - Pose3 pose1(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1.0)); - Pose3 pose2(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1.5)); + Pose3 pose1(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1.0)); + Pose3 pose2(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1.5)); // Create a landmark 5 meters in front of pose1 (camera center at (0,0,1)) Point3 landmark(5, 0, 1); diff --git a/gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp b/gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp index 2c85b3dad..d20fc000c 100644 --- a/gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp +++ b/gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp @@ -24,8 +24,8 @@ using namespace gtsam; TEST( InvDepthFactorVariant2, optimize) { // Create two poses looking in the x-direction - Pose3 pose1(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1.0)); - Pose3 pose2(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1.5)); + Pose3 pose1(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1.0)); + Pose3 pose2(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1.5)); // Create a landmark 5 meters in front of pose1 (camera center at (0,0,1)) Point3 landmark(5, 0, 1); diff --git a/gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp b/gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp index ec9aa90c3..a6eed54d5 100644 --- a/gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp +++ b/gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp @@ -24,8 +24,8 @@ using namespace gtsam; TEST( InvDepthFactorVariant3, optimize) { // Create two poses looking in the x-direction - Pose3 pose1(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1.0)); - Pose3 pose2(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1.5)); + Pose3 pose1(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1.0)); + Pose3 pose2(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1.5)); // Create a landmark 5 meters in front of pose1 (camera center at (0,0,1)) Point3 landmark(5, 0, 1); diff --git a/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp b/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp index c6ad9a38b..a06c39182 100644 --- a/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp +++ b/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp @@ -18,7 +18,7 @@ SharedNoiseModel model1 = noiseModel::Unit::Create(1); const double tol = 1e-5; const Pose3 pose1(Rot3(), Point3(2.0, 3.0, 4.0)); -const Pose3 pose2(Rot3::pitch(-M_PI_2), Point3(2.0, 3.0, 4.0)); +const Pose3 pose2(Rot3::Pitch(-M_PI_2), Point3(2.0, 3.0, 4.0)); const Pose3 pose3(Rot3::RzRyRx(0.1, 0.2, 0.3), Point3(2.0, 3.0, 4.0)); const Point3 point1(3.0, 4.0, 2.0); const gtsam::Key poseKey = 1, pointKey = 2; diff --git a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp index 2981f675d..5bad0e171 100644 --- a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp @@ -115,7 +115,7 @@ TEST( SmartStereoProjectionPoseFactor, Equals ) { /* *************************************************************************/ TEST_UNSAFE( SmartStereoProjectionPoseFactor, noiseless ) { // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 level_pose = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), + Pose3 level_pose = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); StereoCamera level_camera(level_pose, K2); @@ -154,7 +154,7 @@ TEST_UNSAFE( SmartStereoProjectionPoseFactor, noiseless ) { /* *************************************************************************/ TEST( SmartStereoProjectionPoseFactor, noisy ) { // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 level_pose = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), + Pose3 level_pose = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); StereoCamera level_camera(level_pose, K2); @@ -172,7 +172,7 @@ TEST( SmartStereoProjectionPoseFactor, noisy ) { Values values; values.insert(x1, level_pose); - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10), + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), Point3(0.5, 0.1, 0.3)); values.insert(x2, level_pose_right.compose(noise_pose)); @@ -206,7 +206,7 @@ TEST( SmartStereoProjectionPoseFactor, noisy ) { TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) { // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); + Pose3 pose1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); StereoCamera cam1(pose1, K2); // create second camera 1 meter to the right of first camera @@ -257,8 +257,8 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) { graph.push_back(PriorFactor(x1, pose1, noisePrior)); graph.push_back(PriorFactor(x2, pose2, noisePrior)); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, pose1); @@ -354,7 +354,7 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) { views.push_back(x3); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); + Pose3 pose1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); StereoCamera cam1(pose1, K); // create second camera 1 meter to the right of first camera Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); @@ -397,8 +397,8 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) { graph.push_back(PriorFactor(x1, pose1, noisePrior)); graph.push_back(PriorFactor(x2, pose2, noisePrior)); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, pose1); @@ -422,7 +422,7 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) { views.push_back(x3); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); + Pose3 pose1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); StereoCamera cam1(pose1, K); // create second camera 1 meter to the right of first camera Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); @@ -466,8 +466,8 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) { graph.push_back(PriorFactor(x1, pose1, noisePrior)); graph.push_back(PriorFactor(x2, pose2, noisePrior)); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, pose1); @@ -490,7 +490,7 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) { views.push_back(x3); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); + Pose3 pose1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); StereoCamera cam1(pose1, K); // create second camera 1 meter to the right of first camera Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); @@ -548,7 +548,7 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) { graph.push_back(PriorFactor(x1, pose1, noisePrior)); graph.push_back(PriorFactor(x2, pose2, noisePrior)); - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, pose1); @@ -587,7 +587,7 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) { // views.push_back(x3); // // // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) -// Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1)); +// Pose3 pose1 = Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1)); // StereoCamera cam1(pose1, K); // // create second camera 1 meter to the right of first camera // Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); @@ -626,8 +626,8 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) { // graph.push_back(PriorFactor(x1, pose1, noisePrior)); // graph.push_back(PriorFactor(x2, pose2, noisePrior)); // -// // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below -// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), Point3(0.1,0.1,0.1)); // smaller noise +// // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below +// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/100, 0., -M_PI/100), Point3(0.1,0.1,0.1)); // smaller noise // Values values; // values.insert(x1, pose1); // values.insert(x2, pose2); @@ -648,7 +648,7 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) { // views.push_back(x3); // // // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) -// Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1)); +// Pose3 pose1 = Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1)); // StereoCamera cam1(pose1, K2); // // // create second camera 1 meter to the right of first camera @@ -684,7 +684,7 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) { // graph.push_back(PriorFactor(x1, pose1, noisePrior)); // graph.push_back(PriorFactor(x2, pose2, noisePrior)); // -// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); +// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // Values values; // values.insert(x1, pose1); // values.insert(x2, pose2); @@ -708,7 +708,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) { views.push_back(x3); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); + Pose3 pose1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); StereoCamera cam1(pose1, K); // create second camera @@ -754,7 +754,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) { values.insert(x1, pose1); values.insert(x2, pose2); // initialize third pose with some noise, we expect it to move back to original pose3 - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise values.insert(x3, pose3 * noise_pose); @@ -796,7 +796,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) { // views.push_back(x3); // // // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) -// Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1)); +// Pose3 pose1 = Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1)); // StereoCamera cam1(pose1, K2); // // // create second camera 1 meter to the right of first camera @@ -835,7 +835,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) { // graph.push_back(PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); // graph.push_back(PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); // -// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.1,0.1,0.1)); // smaller noise +// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.1,0.1,0.1)); // smaller noise // Values values; // values.insert(x1, pose1); // values.insert(x2, pose2*noise_pose); @@ -862,7 +862,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) { // views.push_back(x3); // // // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) -// Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1)); +// Pose3 pose1 = Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1)); // StereoCamera cam1(pose1, K); // // // create second camera 1 meter to the right of first camera @@ -908,8 +908,8 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) { // graph.push_back(PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); // graph.push_back(PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); // -// // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below -// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), Point3(0.1,0.1,0.1)); // smaller noise +// // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below +// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/100, 0., -M_PI/100), Point3(0.1,0.1,0.1)); // smaller noise // Values values; // values.insert(x1, pose1); // values.insert(x2, pose2); @@ -935,7 +935,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) { // views.push_back(x2); // // // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) -// Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1)); +// Pose3 pose1 = Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1)); // StereoCamera cam1(pose1, K2); // // // create second camera 1 meter to the right of first camera @@ -955,7 +955,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) { // SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor()); // smartFactor1->add(measurements_cam1,views, model, K2); // -// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); +// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // Values values; // values.insert(x1, pose1); // values.insert(x2, pose2); @@ -977,7 +977,7 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ) { views.push_back(x3); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); + Pose3 pose1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); StereoCamera cam1(pose1, K); // create second camera 1 meter to the right of first camera @@ -1005,7 +1005,7 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ) { smartFactorInstance->linearize(values); // hessianFactor->print("Hessian factor \n"); - Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0)); + Pose3 poseDrift = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0)); Values rotValues; rotValues.insert(x1, poseDrift.compose(pose1)); @@ -1021,7 +1021,7 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ) { assert_equal(hessianFactor->information(), hessianFactorRot->information(), 1e-7)); - Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2), + Pose3 poseDrift2 = Pose3(Rot3::Ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2), Point3(10, -4, 5)); Values tranValues; @@ -1047,7 +1047,7 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) { views.push_back(x3); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); + Pose3 pose1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); StereoCamera cam1(pose1, K2); // Second and third cameras in same place, which is a degenerate configuration @@ -1072,7 +1072,7 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) { boost::shared_ptr hessianFactor = smartFactor->linearize( values); - Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0)); + Pose3 poseDrift = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0)); Values rotValues; rotValues.insert(x1, poseDrift.compose(pose1)); @@ -1087,7 +1087,7 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) { assert_equal(hessianFactor->information(), hessianFactorRot->information(), 1e-6)); - Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2), + Pose3 poseDrift2 = Pose3(Rot3::Ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2), Point3(10, -4, 5)); Values tranValues; diff --git a/gtsampy.h b/gtsampy.h new file mode 100644 index 000000000..ca014ad5d --- /dev/null +++ b/gtsampy.h @@ -0,0 +1,2669 @@ +/** + + * GTSAM Wrap Module Definition + * + * These are the current classes available through the matlab toolbox interface, + * add more functions/classes as they are available. + * + * Requirements: + * Classes must start with an uppercase letter + * - Can wrap a typedef + * Only one Method/Constructor per line, though methods/constructors can extend across multiple lines + * Methods can return + * - Eigen types: Matrix, Vector + * - C/C++ basic types: string, bool, size_t, int, double, char, unsigned char + * - void + * - Any class with which be copied with boost::make_shared() + * - boost::shared_ptr of any object type + * Constructors + * - Overloads are supported + * - A class with no constructors can be returned from other functions but not allocated directly in MATLAB + * Methods + * - Constness has no effect + * - Specify by-value (not reference) return types, even if C++ method returns reference + * - Must start with a letter (upper or lowercase) + * - Overloads are supported + * Static methods + * - Must start with a letter (upper or lowercase) and use the "static" keyword + * - The first letter will be made uppercase in the generated MATLAB interface + * - Overloads are supported + * Arguments to functions any of + * - Eigen types: Matrix, Vector + * - Eigen types and classes as an optionally const reference + * - C/C++ basic types: string, bool, size_t, size_t, double, char, unsigned char + * - Any class with which be copied with boost::make_shared() (except Eigen) + * - boost::shared_ptr of any object type (except Eigen) + * Comments can use either C++ or C style, with multiple lines + * Namespace definitions + * - Names of namespaces must start with a lowercase letter + * - start a namespace with "namespace {" + * - end a namespace with exactly "}" + * - Namespaces can be nested + * Namespace usage + * - Namespaces can be specified for classes in arguments and return values + * - In each case, the namespace must be fully specified, e.g., "namespace1::namespace2::ClassName" + * Includes in C++ wrappers + * - All includes will be collected and added in a single file + * - All namespaces must have angle brackets: + * - No default includes will be added + * Global/Namespace functions + * - Functions specified outside of a class are global + * - Can be overloaded with different arguments + * - Can have multiple functions of the same name in different namespaces + * Using classes defined in other modules + * - If you are using a class 'OtherClass' not wrapped in this definition file, add "class OtherClass;" to avoid a dependency error + * Virtual inheritance + * - Specify fully-qualified base classes, i.e. "virtual class Derived : ns::Base {" where "ns" is the namespace + * - Mark with 'virtual' keyword, e.g. "virtual class Base {", and also "virtual class Derived : ns::Base {" + * - Forward declarations must also be marked virtual, e.g. "virtual class ns::Base;" and + * also "virtual class ns::Derived;" + * - Pure virtual (abstract) classes should list no constructors in this interface file + * - Virtual classes must have a clone() function in C++ (though it does not have to be included + * in the MATLAB interface). clone() will be called whenever an object copy is needed, instead + * of using the copy constructor (which is used for non-virtual objects). + * - Signature of clone function - will be called virtually, so must appear at least at the top of the inheritance tree + * virtual boost::shared_ptr clone() const; + * Class Templates + * - Basic templates are supported either with an explicit list of types to instantiate, + * e.g. template class Class1 { ... }; + * or with typedefs, e.g. + * template class Class2 { ... }; + * typedef Class2 MyInstantiatedClass; + * - In the class definition, appearances of the template argument(s) will be replaced with their + * instantiated types, e.g. 'void setValue(const T& value);'. + * - To refer to the instantiation of the template class itself, use 'This', i.e. 'static This Create();' + * - To create new instantiations in other modules, you must copy-and-paste the whole class definition + * into the new module, but use only your new instantiation types. + * - When forward-declaring template instantiations, use the generated/typedefed name, e.g. + * class gtsam::Class1Pose2; + * class gtsam::MyInstantiatedClass; + * Boost.serialization within Matlab: + * - you need to mark classes as being serializable in the markup file (see this file for an example). + * - There are two options currently, depending on the class. To "mark" a class as serializable, + * add a function with a particular signature so that wrap will catch it. + * - Add "void serialize()" to a class to create serialization functions for a class. + * Adding this flag subsumes the serializable() flag below. Requirements: + * - A default constructor must be publicly accessible + * - Must not be an abstract base class + * - The class must have an actual boost.serialization serialize() function. + * - Add "void serializable()" to a class if you only want the class to be serialized as a + * part of a container (such as noisemodel). This version does not require a publicly + * accessible default constructor. + */ + +/** + * Status: + * - TODO: default values for arguments + * - WORKAROUND: make multiple versions of the same function for different configurations of default arguments + * - TODO: Handle gtsam::Rot3M conversions to quaternions + * - TODO: Parse return of const ref arguments + * - TODO: Parse std::string variants and convert directly to special string + * - TODO: Add enum support + * - TODO: Add generalized serialization support via boost.serialization with hooks to matlab save/load + */ + +namespace std { + #include + template + class vector + { + //Do we need these? + //Capacity + /*size_t size() const; + size_t max_size() const; + //void resize(size_t sz); + size_t capacity() const; + bool empty() const; + void reserve(size_t n); + + //Element access + T* at(size_t n); + T* front(); + T* back(); + + //Modifiers + void assign(size_t n, const T& u); + void push_back(const T& x); + void pop_back();*/ + }; + //typedef std::vector + + #include + template + class list + { + + + }; + +} + +namespace gtsam { + +//************************************************************************* +// base +//************************************************************************* + +/** gtsam namespace functions */ +bool linear_independent(Matrix A, Matrix B, double tol); + +virtual class Value { + // No constructors because this is an abstract class + + // Testable + void print(string s) const; + + // Manifold + size_t dim() const; +}; + +#include +class LieScalar { + // Standard constructors + LieScalar(); + LieScalar(double d); + + // Standard interface + double value() const; + + // Testable + void print(string s) const; + bool equals(const gtsam::LieScalar& expected, double tol) const; + + // Group + static gtsam::LieScalar identity(); + gtsam::LieScalar inverse() const; + gtsam::LieScalar compose(const gtsam::LieScalar& p) const; + gtsam::LieScalar between(const gtsam::LieScalar& l2) const; + + // Manifold + size_t dim() const; + gtsam::LieScalar retract(Vector v) const; + Vector localCoordinates(const gtsam::LieScalar& t2) const; + + // Lie group + static gtsam::LieScalar Expmap(Vector v); + static Vector Logmap(const gtsam::LieScalar& p); +}; + +#include +class LieVector { + // Standard constructors + LieVector(); + LieVector(Vector v); + + // Standard interface + Vector vector() const; + + // Testable + void print(string s) const; + bool equals(const gtsam::LieVector& expected, double tol) const; + + // Group + static gtsam::LieVector identity(); + gtsam::LieVector inverse() const; + gtsam::LieVector compose(const gtsam::LieVector& p) const; + gtsam::LieVector between(const gtsam::LieVector& l2) const; + + // Manifold + size_t dim() const; + gtsam::LieVector retract(Vector v) const; + Vector localCoordinates(const gtsam::LieVector& t2) const; + + // Lie group + static gtsam::LieVector Expmap(Vector v); + static Vector Logmap(const gtsam::LieVector& p); + + // enabling serialization functionality + void serialize() const; +}; + +#include +class LieMatrix { + // Standard constructors + LieMatrix(); + LieMatrix(Matrix v); + + // Standard interface + Matrix matrix() const; + + // Testable + void print(string s) const; + bool equals(const gtsam::LieMatrix& expected, double tol) const; + + // Group + static gtsam::LieMatrix identity(); + gtsam::LieMatrix inverse() const; + gtsam::LieMatrix compose(const gtsam::LieMatrix& p) const; + gtsam::LieMatrix between(const gtsam::LieMatrix& l2) const; + + // Manifold + size_t dim() const; + gtsam::LieMatrix retract(Vector v) const; + Vector localCoordinates(const gtsam::LieMatrix & t2) const; + + // Lie group + static gtsam::LieMatrix Expmap(Vector v); + static Vector Logmap(const gtsam::LieMatrix& p); + + // enabling serialization functionality + void serialize() const; +}; + +//************************************************************************* +// geometry +//************************************************************************* + +class Point2 { + // Standard Constructors + Point2(); + Point2(double x, double y); + Point2(Vector v); + + // Testable + void print(string s) const; + bool equals(const gtsam::Point2& pose, double tol) const; + + // Group + static gtsam::Point2 identity(); + gtsam::Point2 inverse() const; + gtsam::Point2 compose(const gtsam::Point2& p2) const; + gtsam::Point2 between(const gtsam::Point2& p2) const; + + // Manifold + gtsam::Point2 retract(Vector v) const; + Vector localCoordinates(const gtsam::Point2& p) const; + + // Lie Group + static gtsam::Point2 Expmap(Vector v); + static Vector Logmap(const gtsam::Point2& p); + + // Standard Interface + double x() const; + double y() const; + Vector vector() const; + double dist(const gtsam::Point2& p2) const; + double norm() const; + + // enabling serialization functionality + void serialize() const; +}; + +// std::vector +class Point2Vector +{ + // Constructors + Point2Vector(); + Point2Vector(const gtsam::Point2Vector& v); + + //Capacity + size_t size() const; + size_t max_size() const; + void resize(size_t sz); + size_t capacity() const; + bool empty() const; + void reserve(size_t n); + + //Element access + gtsam::Point2 at(size_t n) const; + gtsam::Point2 front() const; + gtsam::Point2 back() const; + + //Modifiers + void assign(size_t n, const gtsam::Point2& u); + void push_back(const gtsam::Point2& x); + void pop_back(); +}; + +class StereoPoint2 { + // Standard Constructors + StereoPoint2(); + StereoPoint2(double uL, double uR, double v); + + // Testable + void print(string s) const; + bool equals(const gtsam::StereoPoint2& point, double tol) const; + + // Group + static gtsam::StereoPoint2 identity(); + gtsam::StereoPoint2 inverse() const; + gtsam::StereoPoint2 compose(const gtsam::StereoPoint2& p2) const; + gtsam::StereoPoint2 between(const gtsam::StereoPoint2& p2) const; + + // Manifold + gtsam::StereoPoint2 retract(Vector v) const; + Vector localCoordinates(const gtsam::StereoPoint2& p) const; + + // Lie Group + static gtsam::StereoPoint2 Expmap(Vector v); + static Vector Logmap(const gtsam::StereoPoint2& p); + + // Standard Interface + Vector vector() const; + double uL() const; + double uR() const; + double v() const; + + // enabling serialization functionality + void serialize() const; +}; + +class Point3 { + // Standard Constructors + Point3(); + Point3(double x, double y, double z); + Point3(Vector v); + + // Testable + void print(string s) const; + bool equals(const gtsam::Point3& p, double tol) const; + + // Group + static gtsam::Point3 identity(); + gtsam::Point3 inverse() const; + gtsam::Point3 compose(const gtsam::Point3& p2) const; + gtsam::Point3 between(const gtsam::Point3& p2) const; + + // Manifold + gtsam::Point3 retract(Vector v) const; + Vector localCoordinates(const gtsam::Point3& p) const; + + // Lie Group + static gtsam::Point3 Expmap(Vector v); + static Vector Logmap(const gtsam::Point3& p); + + // Standard Interface + Vector vector() const; + double x() const; + double y() const; + double z() const; + + // enabling serialization functionality + void serialize() const; +}; + +class Rot2 { + // Standard Constructors and Named Constructors + Rot2(); + Rot2(double theta); + static gtsam::Rot2 fromAngle(double theta); + static gtsam::Rot2 fromDegrees(double theta); + static gtsam::Rot2 fromCosSin(double c, double s); + + // Testable + void print(string s) const; + bool equals(const gtsam::Rot2& rot, double tol) const; + + // Group + static gtsam::Rot2 identity(); + gtsam::Rot2 inverse(); + gtsam::Rot2 compose(const gtsam::Rot2& p2) const; + gtsam::Rot2 between(const gtsam::Rot2& p2) const; + + // Manifold + gtsam::Rot2 retract(Vector v) const; + Vector localCoordinates(const gtsam::Rot2& p) const; + + // Lie Group + static gtsam::Rot2 Expmap(Vector v); + static Vector Logmap(const gtsam::Rot2& p); + + // Group Action on Point2 + gtsam::Point2 rotate(const gtsam::Point2& point) const; + gtsam::Point2 unrotate(const gtsam::Point2& point) const; + + // Standard Interface + static gtsam::Rot2 relativeBearing(const gtsam::Point2& d); // Ignoring derivative + static gtsam::Rot2 atan2(double y, double x); + double theta() const; + double degrees() const; + double c() const; + double s() const; + Matrix matrix() const; + + // enabling serialization functionality + void serialize() const; +}; + +class Rot3 { + // Standard Constructors and Named Constructors + Rot3(); + Rot3(Matrix R); + static gtsam::Rot3 Rx(double t); + static gtsam::Rot3 Ry(double t); + static gtsam::Rot3 Rz(double t); + static gtsam::Rot3 RzRyRx(double x, double y, double z); + static gtsam::Rot3 RzRyRx(Vector xyz); + static gtsam::Rot3 yaw(double t); // positive yaw is to right (as in aircraft heading) + static gtsam::Rot3 pitch(double t); // positive pitch is up (increasing aircraft altitude) + static gtsam::Rot3 roll(double t); // positive roll is to right (increasing yaw in aircraft) + static gtsam::Rot3 ypr(double y, double p, double r); + static gtsam::Rot3 quaternion(double w, double x, double y, double z); + static gtsam::Rot3 Rodrigues(Vector v); + + // Testable + void print(string s) const; + bool equals(const gtsam::Rot3& rot, double tol) const; + + // Group + static gtsam::Rot3 identity(); + gtsam::Rot3 inverse() const; + gtsam::Rot3 compose(const gtsam::Rot3& p2) const; + gtsam::Rot3 between(const gtsam::Rot3& p2) const; + + // Manifold + //gtsam::Rot3 retractCayley(Vector v) const; // FIXME, does not exist in both Matrix and Quaternion options + gtsam::Rot3 retract(Vector v) const; + Vector localCoordinates(const gtsam::Rot3& p) const; + + // Group Action on Point3 + gtsam::Point3 rotate(const gtsam::Point3& p) const; + gtsam::Point3 unrotate(const gtsam::Point3& p) const; + + // Standard Interface + static gtsam::Rot3 Expmap(Vector v); + static Vector Logmap(const gtsam::Rot3& p); + Matrix matrix() const; + Matrix transpose() const; + gtsam::Point3 column(size_t index) const; + Vector xyz() const; + Vector ypr() const; + Vector rpy() const; + double roll() const; + double pitch() const; + double yaw() const; +// Vector toQuaternion() const; // FIXME: Can't cast to Vector properly + Vector quaternion() const; + + // enabling serialization functionality + void serialize() const; +}; + +class Pose2 { + // Standard Constructor + Pose2(); + Pose2(const gtsam::Pose2& pose); + Pose2(double x, double y, double theta); + Pose2(double theta, const gtsam::Point2& t); + Pose2(const gtsam::Rot2& r, const gtsam::Point2& t); + Pose2(Vector v); + + // Testable + void print(string s) const; + bool equals(const gtsam::Pose2& pose, double tol) const; + + // Group + static gtsam::Pose2 identity(); + gtsam::Pose2 inverse() const; + gtsam::Pose2 compose(const gtsam::Pose2& p2) const; + gtsam::Pose2 between(const gtsam::Pose2& p2) const; + + // Manifold + gtsam::Pose2 retract(Vector v) const; + Vector localCoordinates(const gtsam::Pose2& p) const; + + // Lie Group + static gtsam::Pose2 Expmap(Vector v); + static Vector Logmap(const gtsam::Pose2& p); + Matrix AdjointMap() const; + Vector Adjoint(const Vector& xi) const; + static Matrix wedge(double vx, double vy, double w); + + // Group Actions on Point2 + gtsam::Point2 transform_from(const gtsam::Point2& p) const; + gtsam::Point2 transform_to(const gtsam::Point2& p) const; + + // Standard Interface + double x() const; + double y() const; + double theta() const; + gtsam::Rot2 bearing(const gtsam::Point2& point) const; + double range(const gtsam::Point2& point) const; + gtsam::Point2 translation() const; + gtsam::Rot2 rotation() const; + Matrix matrix() const; + + // enabling serialization functionality + void serialize() const; +}; + +class Pose3 { + // Standard Constructors + Pose3(); + Pose3(const gtsam::Pose3& pose); + Pose3(const gtsam::Rot3& r, const gtsam::Point3& t); + Pose3(const gtsam::Pose2& pose2); // FIXME: shadows Pose3(Pose3 pose) + Pose3(Matrix t); + + // Testable + void print(string s) const; + bool equals(const gtsam::Pose3& pose, double tol) const; + + // Group + static gtsam::Pose3 identity(); + gtsam::Pose3 inverse() const; + gtsam::Pose3 compose(const gtsam::Pose3& p2) const; + gtsam::Pose3 between(const gtsam::Pose3& p2) const; + + // Manifold + gtsam::Pose3 retract(Vector v) const; + Vector localCoordinates(const gtsam::Pose3& T2) const; + + // Lie Group + static gtsam::Pose3 Expmap(Vector v); + static Vector Logmap(const gtsam::Pose3& p); + Matrix AdjointMap() const; + Vector Adjoint(Vector xi) const; + static Matrix wedge(double wx, double wy, double wz, double vx, double vy, double vz); + + // Group Action on Point3 + gtsam::Point3 transform_from(const gtsam::Point3& p) const; + gtsam::Point3 transform_to(const gtsam::Point3& p) const; + + // Standard Interface + gtsam::Rot3 rotation() const; + gtsam::Point3 translation() const; + double x() const; + double y() const; + double z() const; + Matrix matrix() const; + gtsam::Pose3 transform_to(const gtsam::Pose3& pose) const; // FIXME: shadows other transform_to() + double range(const gtsam::Point3& point); + double range(const gtsam::Pose3& pose); + + // enabling serialization functionality + void serialize() const; +}; + +// std::vector +class Pose3Vector +{ + Pose3Vector(); + size_t size() const; + bool empty() const; + gtsam::Pose3 at(size_t n) const; + void push_back(const gtsam::Pose3& x); +}; + +#include +class Unit3 { + // Standard Constructors + Unit3(); + Unit3(const gtsam::Point3& pose); + + // Testable + void print(string s) const; + bool equals(const gtsam::Unit3& pose, double tol) const; + + // Other functionality + Matrix basis() const; + Matrix skew() const; + + // Manifold + static size_t Dim(); + size_t dim() const; + gtsam::Unit3 retract(Vector v) const; + Vector localCoordinates(const gtsam::Unit3& s) const; +}; + +#include +class EssentialMatrix { + // Standard Constructors + EssentialMatrix(const gtsam::Rot3& aRb, const gtsam::Unit3& aTb); + + // Testable + void print(string s) const; + bool equals(const gtsam::EssentialMatrix& pose, double tol) const; + + // Manifold + static size_t Dim(); + size_t dim() const; + gtsam::EssentialMatrix retract(Vector v) const; + Vector localCoordinates(const gtsam::EssentialMatrix& s) const; + + // Other methods: + gtsam::Rot3 rotation() const; + gtsam::Unit3 direction() const; + Matrix matrix() const; + double error(Vector vA, Vector vB); +}; + +class Cal3_S2 { + // Standard Constructors + Cal3_S2(); + Cal3_S2(double fx, double fy, double s, double u0, double v0); + Cal3_S2(Vector v); + Cal3_S2(double fov, int w, int h); + + // Testable + void print(string s) const; + bool equals(const gtsam::Cal3_S2& rhs, double tol) const; + + // Manifold + static size_t Dim(); + size_t dim() const; + gtsam::Cal3_S2 retract(Vector v) const; + Vector localCoordinates(const gtsam::Cal3_S2& c) const; + + // Action on Point2 + gtsam::Point2 calibrate(const gtsam::Point2& p) const; + gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; + + // Standard Interface + double fx() const; + double fy() const; + double skew() const; + double px() const; + double py() const; + gtsam::Point2 principalPoint() const; + Vector vector() const; + Matrix matrix() const; + Matrix matrix_inverse() const; + + // enabling serialization functionality + void serialize() const; +}; + +#include +virtual class Cal3DS2_Base { + // Standard Constructors + Cal3DS2_Base(); + + // Testable + void print(string s) const; + + // Standard Interface + double fx() const; + double fy() const; + double skew() const; + double px() const; + double py() const; + double k1() const; + double k2() const; + + // Action on Point2 + gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; + gtsam::Point2 calibrate(const gtsam::Point2& p, double tol) const; + gtsam::Point2 calibrate(const gtsam::Point2& p) const; + + // enabling serialization functionality + void serialize() const; +}; + +#include +virtual class Cal3DS2 : gtsam::Cal3DS2_Base { + // Standard Constructors + Cal3DS2(); + Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2); + Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2, double p1, double p2); + Cal3DS2(Vector v); + + // Testable + bool equals(const gtsam::Cal3DS2& rhs, double tol) const; + + // Manifold + size_t dim() const; + static size_t Dim(); + gtsam::Cal3DS2 retract(Vector v) const; + Vector localCoordinates(const gtsam::Cal3DS2& c) const; + + // enabling serialization functionality + void serialize() const; +}; + +#include +virtual class Cal3Unified : gtsam::Cal3DS2_Base { + // Standard Constructors + Cal3Unified(); + Cal3Unified(double fx, double fy, double s, double u0, double v0, double k1, double k2); + Cal3Unified(double fx, double fy, double s, double u0, double v0, double k1, double k2, double p1, double p2, double xi); + Cal3Unified(Vector v); + + // Testable + bool equals(const gtsam::Cal3Unified& rhs, double tol) const; + + // Standard Interface + double xi() const; + gtsam::Point2 spaceToNPlane(const gtsam::Point2& p) const; + gtsam::Point2 nPlaneToSpace(const gtsam::Point2& p) const; + + // Manifold + size_t dim() const; + static size_t Dim(); + gtsam::Cal3Unified retract(Vector v) const; + Vector localCoordinates(const gtsam::Cal3Unified& c) const; + + // enabling serialization functionality + void serialize() const; +}; + +#include +class Cal3_S2Stereo { + // Standard Constructors + Cal3_S2Stereo(); + Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b); + Cal3_S2Stereo(Vector v); + + // Testable + void print(string s) const; + bool equals(const gtsam::Cal3_S2Stereo& K, double tol) const; + + // Standard Interface + double fx() const; + double fy() const; + double skew() const; + double px() const; + double py() const; + gtsam::Point2 principalPoint() const; + double baseline() const; +}; + +#include +class Cal3Bundler { + // Standard Constructors + Cal3Bundler(); + Cal3Bundler(double fx, double k1, double k2, double u0, double v0); + + // Testable + void print(string s) const; + bool equals(const gtsam::Cal3Bundler& rhs, double tol) const; + + // Manifold + static size_t Dim(); + size_t dim() const; + gtsam::Cal3Bundler retract(Vector v) const; + Vector localCoordinates(const gtsam::Cal3Bundler& c) const; + + // Action on Point2 + gtsam::Point2 calibrate(const gtsam::Point2& p, double tol) const; + gtsam::Point2 calibrate(const gtsam::Point2& p) const; + gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; + + // Standard Interface + double fx() const; + double fy() const; + double k1() const; + double k2() const; + double u0() const; + double v0() const; + Vector vector() const; + Vector k() const; + //Matrix K() const; //FIXME: Uppercase + + // enabling serialization functionality + void serialize() const; +}; + +class CalibratedCamera { + // Standard Constructors and Named Constructors + CalibratedCamera(); + CalibratedCamera(const gtsam::Pose3& pose); + CalibratedCamera(const Vector& v); + static gtsam::CalibratedCamera Level(const gtsam::Pose2& pose2, double height); + + // Testable + void print(string s) const; + bool equals(const gtsam::CalibratedCamera& camera, double tol) const; + + // Manifold + static size_t Dim(); + size_t dim() const; + gtsam::CalibratedCamera retract(const Vector& d) const; + Vector localCoordinates(const gtsam::CalibratedCamera& T2) const; + + // Action on Point3 + gtsam::Point2 project(const gtsam::Point3& point) const; + static gtsam::Point2 Project(const gtsam::Point3& cameraPoint); + + // Standard Interface + gtsam::Pose3 pose() const; + double range(const gtsam::Point3& p) const; // TODO: Other overloaded range methods + + // enabling serialization functionality + void serialize() const; +}; + +template +class PinholeCamera { + // Standard Constructors and Named Constructors + PinholeCamera(); + PinholeCamera(const gtsam::Pose3& pose); + PinholeCamera(const gtsam::Pose3& pose, const CALIBRATION& K); + static This Level(const CALIBRATION& K, const gtsam::Pose2& pose, double height); + static This Level(const gtsam::Pose2& pose, double height); + static This Lookat(const gtsam::Point3& eye, const gtsam::Point3& target, + const gtsam::Point3& upVector, const CALIBRATION& K); + + // Testable + void print(string s) const; + bool equals(const This& camera, double tol) const; + + // Standard Interface + gtsam::Pose3 pose() const; + CALIBRATION calibration() const; + + // Manifold + This retract(const Vector& d) const; + Vector localCoordinates(const This& T2) const; + size_t dim() const; + static size_t Dim(); + + // Transformations and measurement functions + static gtsam::Point2 Project(const gtsam::Point3& cameraPoint); + pair projectSafe(const gtsam::Point3& pw) const; + gtsam::Point2 project(const gtsam::Point3& point); + gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const; + double range(const gtsam::Point3& point); + double range(const gtsam::Pose3& point); + + // enabling serialization functionality + void serialize() const; +}; + +virtual class SimpleCamera { + // Standard Constructors and Named Constructors + SimpleCamera(); + SimpleCamera(const gtsam::Pose3& pose); + SimpleCamera(const gtsam::Pose3& pose, const gtsam::Cal3_S2& K); + static gtsam::SimpleCamera Level(const gtsam::Cal3_S2& K, const gtsam::Pose2& pose, double height); + static gtsam::SimpleCamera Level(const gtsam::Pose2& pose, double height); + static gtsam::SimpleCamera Lookat(const gtsam::Point3& eye, const gtsam::Point3& target, + const gtsam::Point3& upVector, const gtsam::Cal3_S2& K); + + // Testable + void print(string s) const; + bool equals(const gtsam::SimpleCamera& camera, double tol) const; + + // Standard Interface + gtsam::Pose3 pose() const; + gtsam::Cal3_S2 calibration() const; + + // Manifold + gtsam::SimpleCamera retract(const Vector& d) const; + Vector localCoordinates(const gtsam::SimpleCamera& T2) const; + size_t dim() const; + static size_t Dim(); + + // Transformations and measurement functions + static gtsam::Point2 Project(const gtsam::Point3& cameraPoint); + pair projectSafe(const gtsam::Point3& pw) const; + gtsam::Point2 project(const gtsam::Point3& point); + gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const; + double range(const gtsam::Point3& point); + double range(const gtsam::Pose3& point); + + // enabling serialization functionality + void serialize() const; + +}; + +// Some typedefs for common camera types +// PinholeCameraCal3_S2 is the same as SimpleCamera above +typedef gtsam::PinholeCamera PinholeCameraCal3_S2; +typedef gtsam::PinholeCamera PinholeCameraCal3DS2; +typedef gtsam::PinholeCamera PinholeCameraCal3Unified; +typedef gtsam::PinholeCamera PinholeCameraCal3Bundler; + +class StereoCamera { + // Standard Constructors and Named Constructors + StereoCamera(); + StereoCamera(const gtsam::Pose3& pose, const gtsam::Cal3_S2Stereo* K); + + // Testable + void print(string s) const; + bool equals(const gtsam::StereoCamera& camera, double tol) const; + + // Standard Interface + gtsam::Pose3 pose() const; + double baseline() const; + gtsam::Cal3_S2Stereo calibration() const; + + // Manifold + gtsam::StereoCamera retract(const Vector& d) const; + Vector localCoordinates(const gtsam::StereoCamera& T2) const; + size_t dim() const; + static size_t Dim(); + + // Transformations and measurement functions + gtsam::StereoPoint2 project(const gtsam::Point3& point); + gtsam::Point3 backproject(const gtsam::StereoPoint2& p) const; + + // enabling serialization functionality + void serialize() const; +}; + +#include + +// Templates appear not yet supported for free functions +gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, + gtsam::Cal3_S2* sharedCal, const gtsam::Point2Vector& measurements, + double rank_tol, bool optimize); +gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, + gtsam::Cal3Bundler* sharedCal, const gtsam::Point2Vector& measurements, + double rank_tol, bool optimize); + +//************************************************************************* +// Symbolic +//************************************************************************* + +#include +virtual class SymbolicFactor { + // Standard Constructors and Named Constructors + SymbolicFactor(const gtsam::SymbolicFactor& f); + SymbolicFactor(); + SymbolicFactor(size_t j); + SymbolicFactor(size_t j1, size_t j2); + SymbolicFactor(size_t j1, size_t j2, size_t j3); + SymbolicFactor(size_t j1, size_t j2, size_t j3, size_t j4); + SymbolicFactor(size_t j1, size_t j2, size_t j3, size_t j4, size_t j5); + SymbolicFactor(size_t j1, size_t j2, size_t j3, size_t j4, size_t j5, size_t j6); + static gtsam::SymbolicFactor FromKeys(const gtsam::KeyVector& js); + + // From Factor + size_t size() const; + void print(string s) const; + bool equals(const gtsam::SymbolicFactor& other, double tol) const; + gtsam::KeyVector keys(); +}; + +#include +virtual class SymbolicFactorGraph { + SymbolicFactorGraph(); + SymbolicFactorGraph(const gtsam::SymbolicBayesNet& bayesNet); + SymbolicFactorGraph(const gtsam::SymbolicBayesTree& bayesTree); + + // From FactorGraph + void push_back(gtsam::SymbolicFactor* factor); + void print(string s) const; + bool equals(const gtsam::SymbolicFactorGraph& rhs, double tol) const; + size_t size() const; + bool exists(size_t idx) const; + + // Standard interface + gtsam::KeySet keys() const; + void push_back(gtsam::SymbolicFactor* factor); + void push_back(const gtsam::SymbolicFactorGraph& graph); + void push_back(const gtsam::SymbolicBayesNet& bayesNet); + void push_back(const gtsam::SymbolicBayesTree& bayesTree); + + //Advanced Interface + void push_factor(size_t key); + void push_factor(size_t key1, size_t key2); + void push_factor(size_t key1, size_t key2, size_t key3); + void push_factor(size_t key1, size_t key2, size_t key3, size_t key4); + + gtsam::SymbolicBayesNet* eliminateSequential(); + gtsam::SymbolicBayesNet* eliminateSequential(const gtsam::Ordering& ordering); + gtsam::SymbolicBayesTree* eliminateMultifrontal(); + gtsam::SymbolicBayesTree* eliminateMultifrontal(const gtsam::Ordering& ordering); + pair eliminatePartialSequential( + const gtsam::Ordering& ordering); + pair eliminatePartialSequential( + const gtsam::KeyVector& keys); + pair eliminatePartialMultifrontal( + const gtsam::Ordering& ordering); + pair eliminatePartialMultifrontal( + const gtsam::KeyVector& keys); + gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& variables); + gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& variables); + gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& variables, + const gtsam::Ordering& marginalizedVariableOrdering); + gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& variables, + const gtsam::Ordering& marginalizedVariableOrdering); + gtsam::SymbolicFactorGraph* marginal(const gtsam::KeyVector& variables); +}; + +#include +virtual class SymbolicConditional : gtsam::SymbolicFactor { + // Standard Constructors and Named Constructors + SymbolicConditional(); + SymbolicConditional(const gtsam::SymbolicConditional& other); + SymbolicConditional(size_t key); + SymbolicConditional(size_t key, size_t parent); + SymbolicConditional(size_t key, size_t parent1, size_t parent2); + SymbolicConditional(size_t key, size_t parent1, size_t parent2, size_t parent3); + static gtsam::SymbolicConditional FromKeys(const gtsam::KeyVector& keys, size_t nrFrontals); + + // Testable + void print(string s) const; + bool equals(const gtsam::SymbolicConditional& other, double tol) const; + + // Standard interface + size_t nrFrontals() const; + size_t nrParents() const; +}; + +#include +class SymbolicBayesNet { + SymbolicBayesNet(); + SymbolicBayesNet(const gtsam::SymbolicBayesNet& other); + // Testable + void print(string s) const; + bool equals(const gtsam::SymbolicBayesNet& other, double tol) const; + + // Standard interface + size_t size() const; + void saveGraph(string s) const; + gtsam::SymbolicConditional* at(size_t idx) const; + gtsam::SymbolicConditional* front() const; + gtsam::SymbolicConditional* back() const; + void push_back(gtsam::SymbolicConditional* conditional); + void push_back(const gtsam::SymbolicBayesNet& bayesNet); +}; + +#include +class SymbolicBayesTree { + + //Constructors + SymbolicBayesTree(); + SymbolicBayesTree(const gtsam::SymbolicBayesTree& other); + + // Testable + void print(string s); + bool equals(const gtsam::SymbolicBayesTree& other, double tol) const; + + //Standard Interface + //size_t findParentClique(const gtsam::IndexVector& parents) const; + size_t size(); + void saveGraph(string s) const; + void clear(); + void deleteCachedShortcuts(); + size_t numCachedSeparatorMarginals() const; + + gtsam::SymbolicConditional* marginalFactor(size_t key) const; + gtsam::SymbolicFactorGraph* joint(size_t key1, size_t key2) const; + gtsam::SymbolicBayesNet* jointBayesNet(size_t key1, size_t key2) const; +}; + +// class SymbolicBayesTreeClique { +// BayesTreeClique(); +// BayesTreeClique(CONDITIONAL* conditional); +// // BayesTreeClique(const std::pair& result) : Base(result) {} +// +// bool equals(const This& other, double tol) const; +// void print(string s) const; +// void printTree() const; // Default indent of "" +// void printTree(string indent) const; +// size_t numCachedSeparatorMarginals() const; +// +// CONDITIONAL* conditional() const; +// bool isRoot() const; +// size_t treeSize() const; +// // const std::list& children() const { return children_; } +// // derived_ptr parent() const { return parent_.lock(); } +// +// // FIXME: need wrapped versions graphs, BayesNet +// // BayesNet shortcut(derived_ptr root, Eliminate function) const; +// // FactorGraph marginal(derived_ptr root, Eliminate function) const; +// // FactorGraph joint(derived_ptr C2, derived_ptr root, Eliminate function) const; +// +// void deleteCachedShortcuts(); +// }; + +#include +class VariableIndex { + // Standard Constructors and Named Constructors + VariableIndex(); + // TODO: Templetize constructor when wrap supports it + //template + //VariableIndex(const T& factorGraph, size_t nVariables); + //VariableIndex(const T& factorGraph); + VariableIndex(const gtsam::SymbolicFactorGraph& factorGraph); + VariableIndex(const gtsam::GaussianFactorGraph& factorGraph); + VariableIndex(const gtsam::NonlinearFactorGraph& factorGraph); + VariableIndex(const gtsam::VariableIndex& other); + + // Testable + bool equals(const gtsam::VariableIndex& other, double tol) const; + void print(string s) const; + + // Standard interface + size_t size() const; + size_t nFactors() const; + size_t nEntries() const; +}; + +//************************************************************************* +// linear +//************************************************************************* + +namespace noiseModel { +#include +virtual class Base { +}; + +virtual class Gaussian : gtsam::noiseModel::Base { + static gtsam::noiseModel::Gaussian* SqrtInformation(Matrix R); + static gtsam::noiseModel::Gaussian* Covariance(Matrix R); + Matrix R() const; + bool equals(gtsam::noiseModel::Base& expected, double tol); + void print(string s) const; + + // enabling serialization functionality + void serializable() const; +}; + +virtual class Diagonal : gtsam::noiseModel::Gaussian { + static gtsam::noiseModel::Diagonal* Sigmas(Vector sigmas); + static gtsam::noiseModel::Diagonal* Variances(Vector variances); + static gtsam::noiseModel::Diagonal* Precisions(Vector precisions); + Matrix R() const; + void print(string s) const; + + // enabling serialization functionality + void serializable() const; +}; + +virtual class Constrained : gtsam::noiseModel::Diagonal { + static gtsam::noiseModel::Constrained* MixedSigmas(const Vector& mu, const Vector& sigmas); + static gtsam::noiseModel::Constrained* MixedSigmas(double m, const Vector& sigmas); + static gtsam::noiseModel::Constrained* MixedVariances(const Vector& mu, const Vector& variances); + static gtsam::noiseModel::Constrained* MixedVariances(const Vector& variances); + static gtsam::noiseModel::Constrained* MixedPrecisions(const Vector& mu, const Vector& precisions); + static gtsam::noiseModel::Constrained* MixedPrecisions(const Vector& precisions); + + static gtsam::noiseModel::Constrained* All(size_t dim); + static gtsam::noiseModel::Constrained* All(size_t dim, double mu); + + gtsam::noiseModel::Constrained* unit() const; + + // enabling serialization functionality + void serializable() const; +}; + +virtual class Isotropic : gtsam::noiseModel::Diagonal { + static gtsam::noiseModel::Isotropic* Sigma(size_t dim, double sigma); + static gtsam::noiseModel::Isotropic* Variance(size_t dim, double varianace); + static gtsam::noiseModel::Isotropic* Precision(size_t dim, double precision); + void print(string s) const; + + // enabling serialization functionality + void serializable() const; +}; + +virtual class Unit : gtsam::noiseModel::Isotropic { + static gtsam::noiseModel::Unit* Create(size_t dim); + void print(string s) const; + + // enabling serialization functionality + void serializable() const; +}; + +namespace mEstimator { +virtual class Base { +}; + +virtual class Null: gtsam::noiseModel::mEstimator::Base { + Null(); + void print(string s) const; + static gtsam::noiseModel::mEstimator::Null* Create(); + + // enabling serialization functionality + void serializable() const; +}; + +virtual class Fair: gtsam::noiseModel::mEstimator::Base { + Fair(double c); + void print(string s) const; + static gtsam::noiseModel::mEstimator::Fair* Create(double c); + + // enabling serialization functionality + void serializable() const; +}; + +virtual class Huber: gtsam::noiseModel::mEstimator::Base { + Huber(double k); + void print(string s) const; + static gtsam::noiseModel::mEstimator::Huber* Create(double k); + + // enabling serialization functionality + void serializable() const; +}; + +virtual class Tukey: gtsam::noiseModel::mEstimator::Base { + Tukey(double k); + void print(string s) const; + static gtsam::noiseModel::mEstimator::Tukey* Create(double k); + + // enabling serialization functionality + void serializable() const; +}; + +}///\namespace mEstimator + +virtual class Robust : gtsam::noiseModel::Base { + Robust(const gtsam::noiseModel::mEstimator::Base* robust, const gtsam::noiseModel::Base* noise); + static gtsam::noiseModel::Robust* Create(const gtsam::noiseModel::mEstimator::Base* robust, const gtsam::noiseModel::Base* noise); + void print(string s) const; + + // enabling serialization functionality + void serializable() const; +}; + +}///\namespace noiseModel + +#include +class Sampler { + //Constructors + Sampler(gtsam::noiseModel::Diagonal* model, int seed); + Sampler(Vector sigmas, int seed); + Sampler(int seed); + + //Standard Interface + size_t dim() const; + Vector sigmas() const; + gtsam::noiseModel::Diagonal* model() const; + Vector sample(); + Vector sampleNewModel(gtsam::noiseModel::Diagonal* model); +}; + +#include +class VectorValues { + //Constructors + VectorValues(); + VectorValues(const gtsam::VectorValues& other); + + //Named Constructors + static gtsam::VectorValues Zero(const gtsam::VectorValues& model); + + //Standard Interface + size_t size() const; + size_t dim(size_t j) const; + bool exists(size_t j) const; + void print(string s) const; + bool equals(const gtsam::VectorValues& expected, double tol) const; + void insert(size_t j, Vector value); + Vector vector() const; + Vector at(size_t j) const; + void update(const gtsam::VectorValues& values); + + //Advanced Interface + void setZero(); + + gtsam::VectorValues add(const gtsam::VectorValues& c) const; + void addInPlace(const gtsam::VectorValues& c); + gtsam::VectorValues subtract(const gtsam::VectorValues& c) const; + gtsam::VectorValues scale(double a) const; + void scaleInPlace(double a); + + bool hasSameStructure(const gtsam::VectorValues& other) const; + double dot(const gtsam::VectorValues& V) const; + double norm() const; + double squaredNorm() const; + + // enabling serialization functionality + void serialize() const; +}; + +#include +virtual class GaussianFactor { + gtsam::KeyVector keys() const; + void print(string s) const; + bool equals(const gtsam::GaussianFactor& lf, double tol) const; + double error(const gtsam::VectorValues& c) const; + gtsam::GaussianFactor* clone() const; + gtsam::GaussianFactor* negate() const; + Matrix augmentedInformation() const; + Matrix information() const; + Matrix augmentedJacobian() const; + pair jacobian() const; + size_t size() const; + bool empty() const; +}; + +#include +virtual class JacobianFactor : gtsam::GaussianFactor { + //Constructors + JacobianFactor(); + JacobianFactor(const gtsam::GaussianFactor& factor); + JacobianFactor(Vector b_in); + JacobianFactor(size_t i1, Matrix A1, Vector b, + const gtsam::noiseModel::Diagonal* model); + JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, Vector b, + const gtsam::noiseModel::Diagonal* model); + JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, size_t i3, Matrix A3, + Vector b, const gtsam::noiseModel::Diagonal* model); + JacobianFactor(const gtsam::GaussianFactorGraph& graph); + + //Testable + void print(string s) const; + void printKeys(string s) const; + bool equals(const gtsam::GaussianFactor& lf, double tol) const; + size_t size() const; + Vector unweighted_error(const gtsam::VectorValues& c) const; + Vector error_vector(const gtsam::VectorValues& c) const; + double error(const gtsam::VectorValues& c) const; + + //Standard Interface + Matrix getA() const; + Vector getb() const; + size_t rows() const; + size_t cols() const; + bool isConstrained() const; + pair jacobianUnweighted() const; + Matrix augmentedJacobianUnweighted() const; + + void transposeMultiplyAdd(double alpha, const Vector& e, gtsam::VectorValues& x) const; + gtsam::JacobianFactor whiten() const; + + pair eliminate(const gtsam::Ordering& keys) const; + + void setModel(bool anyConstrained, const Vector& sigmas); + + gtsam::noiseModel::Diagonal* get_model() const; + + // enabling serialization functionality + void serialize() const; +}; + +#include +virtual class HessianFactor : gtsam::GaussianFactor { + //Constructors + HessianFactor(); + HessianFactor(const gtsam::GaussianFactor& factor); + HessianFactor(size_t j, Matrix G, Vector g, double f); + HessianFactor(size_t j, Vector mu, Matrix Sigma); + HessianFactor(size_t j1, size_t j2, Matrix G11, Matrix G12, Vector g1, Matrix G22, + Vector g2, double f); + HessianFactor(size_t j1, size_t j2, size_t j3, Matrix G11, Matrix G12, Matrix G13, + Vector g1, Matrix G22, Matrix G23, Vector g2, Matrix G33, Vector g3, + double f); + HessianFactor(const gtsam::GaussianFactorGraph& factors); + + //Testable + size_t size() const; + void print(string s) const; + void printKeys(string s) const; + bool equals(const gtsam::GaussianFactor& lf, double tol) const; + double error(const gtsam::VectorValues& c) const; + + //Standard Interface + size_t rows() const; + Matrix info() const; + double constantTerm() const; + Vector linearTerm() const; + + // enabling serialization functionality + void serialize() const; +}; + +#include +class GaussianFactorGraph { + GaussianFactorGraph(); + GaussianFactorGraph(const gtsam::GaussianBayesNet& bayesNet); + GaussianFactorGraph(const gtsam::GaussianBayesTree& bayesTree); + + // From FactorGraph + void print(string s) const; + bool equals(const gtsam::GaussianFactorGraph& lfgraph, double tol) const; + size_t size() const; + gtsam::GaussianFactor* at(size_t idx) const; + gtsam::KeySet keys() const; + bool exists(size_t idx) const; + + // Building the graph + void push_back(const gtsam::GaussianFactor* factor); + void push_back(const gtsam::GaussianConditional* factor); + void push_back(const gtsam::GaussianFactorGraph& graph); + void push_back(const gtsam::GaussianBayesNet& bayesNet); + void push_back(const gtsam::GaussianBayesTree& bayesTree); + void add(const gtsam::GaussianFactor& factor); + void add(Vector b); + void add(size_t key1, Matrix A1, Vector b, const gtsam::noiseModel::Diagonal* model); + void add(size_t key1, Matrix A1, size_t key2, Matrix A2, Vector b, + const gtsam::noiseModel::Diagonal* model); + void add(size_t key1, Matrix A1, size_t key2, Matrix A2, size_t key3, Matrix A3, + Vector b, const gtsam::noiseModel::Diagonal* model); + + // error and probability + double error(const gtsam::VectorValues& c) const; + double probPrime(const gtsam::VectorValues& c) const; + + gtsam::GaussianFactorGraph clone() const; + gtsam::GaussianFactorGraph negate() const; + + // Optimizing and linear algebra + gtsam::VectorValues optimize() const; + gtsam::VectorValues optimize(const gtsam::Ordering& ordering) const; + gtsam::VectorValues optimizeGradientSearch() const; + gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const; + gtsam::VectorValues gradientAtZero() const; + + // Elimination and marginals + gtsam::GaussianBayesNet* eliminateSequential(); + gtsam::GaussianBayesNet* eliminateSequential(const gtsam::Ordering& ordering); + gtsam::GaussianBayesTree* eliminateMultifrontal(); + gtsam::GaussianBayesTree* eliminateMultifrontal(const gtsam::Ordering& ordering); + pair eliminatePartialSequential( + const gtsam::Ordering& ordering); + pair eliminatePartialSequential( + const gtsam::KeyVector& keys); + pair eliminatePartialMultifrontal( + const gtsam::Ordering& ordering); + pair eliminatePartialMultifrontal( + const gtsam::KeyVector& keys); + gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& variables); + gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& variables); + gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& variables, + const gtsam::Ordering& marginalizedVariableOrdering); + gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& variables, + const gtsam::Ordering& marginalizedVariableOrdering); + gtsam::GaussianFactorGraph* marginal(const gtsam::KeyVector& variables); + + // Conversion to matrices + Matrix sparseJacobian_() const; + Matrix augmentedJacobian() const; + Matrix augmentedJacobian(const gtsam::Ordering& ordering) const; + pair jacobian() const; + pair jacobian(const gtsam::Ordering& ordering) const; + Matrix augmentedHessian() const; + Matrix augmentedHessian(const gtsam::Ordering& ordering) const; + pair hessian() const; + pair hessian(const gtsam::Ordering& ordering) const; + + // enabling serialization functionality + void serialize() const; +}; + +#include +virtual class GaussianConditional : gtsam::GaussianFactor { + //Constructors + GaussianConditional(size_t key, Vector d, Matrix R, const gtsam::noiseModel::Diagonal* sigmas); + GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, + const gtsam::noiseModel::Diagonal* sigmas); + GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, + size_t name2, Matrix T, const gtsam::noiseModel::Diagonal* sigmas); + + //Constructors with no noise model + GaussianConditional(size_t key, Vector d, Matrix R); + GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S); + GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, + size_t name2, Matrix T); + + //Standard Interface + void print(string s) const; + bool equals(const gtsam::GaussianConditional &cg, double tol) const; + + //Advanced Interface + gtsam::VectorValues solve(const gtsam::VectorValues& parents) const; + gtsam::VectorValues solveOtherRHS(const gtsam::VectorValues& parents, const gtsam::VectorValues& rhs) const; + void solveTransposeInPlace(gtsam::VectorValues& gy) const; + void scaleFrontalsBySigma(gtsam::VectorValues& gy) const; + + // enabling serialization functionality + void serialize() const; +}; + +#include +virtual class GaussianDensity : gtsam::GaussianConditional { + //Constructors + GaussianDensity(size_t key, Vector d, Matrix R, const gtsam::noiseModel::Diagonal* sigmas); + + //Standard Interface + void print(string s) const; + bool equals(const gtsam::GaussianDensity &cg, double tol) const; + Vector mean() const; + Matrix covariance() const; +}; + +#include +virtual class GaussianBayesNet { + //Constructors + GaussianBayesNet(); + GaussianBayesNet(const gtsam::GaussianConditional* conditional); + + // Testable + void print(string s) const; + bool equals(const gtsam::GaussianBayesNet& other, double tol) const; + size_t size() const; + + // FactorGraph derived interface + size_t size() const; + gtsam::GaussianConditional* at(size_t idx) const; + gtsam::KeySet keys() const; + bool exists(size_t idx) const; + + gtsam::GaussianConditional* front() const; + gtsam::GaussianConditional* back() const; + void push_back(gtsam::GaussianConditional* conditional); + void push_back(const gtsam::GaussianBayesNet& bayesNet); + + gtsam::VectorValues optimize() const; + gtsam::VectorValues optimize(gtsam::VectorValues& solutionForMissing) const; + gtsam::VectorValues optimizeGradientSearch() const; + gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const; + gtsam::VectorValues gradientAtZero() const; + double error(const gtsam::VectorValues& x) const; + double determinant() const; + double logDeterminant() const; + gtsam::VectorValues backSubstitute(const gtsam::VectorValues& gx) const; + gtsam::VectorValues backSubstituteTranspose(const gtsam::VectorValues& gx) const; +}; + +#include +virtual class GaussianBayesTree { + // Standard Constructors and Named Constructors + GaussianBayesTree(); + GaussianBayesTree(const gtsam::GaussianBayesTree& other); + bool equals(const gtsam::GaussianBayesTree& other, double tol) const; + void print(string s); + size_t size() const; + bool empty() const; + size_t numCachedSeparatorMarginals() const; + void saveGraph(string s) const; + + gtsam::VectorValues optimize() const; + gtsam::VectorValues optimizeGradientSearch() const; + gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const; + gtsam::VectorValues gradientAtZero() const; + double error(const gtsam::VectorValues& x) const; + double determinant() const; + double logDeterminant() const; + Matrix marginalCovariance(size_t key) const; + gtsam::GaussianConditional* marginalFactor(size_t key) const; + gtsam::GaussianFactorGraph* joint(size_t key1, size_t key2) const; + gtsam::GaussianBayesNet* jointBayesNet(size_t key1, size_t key2) const; +}; + +class Errors { + //Constructors + Errors(); + Errors(const gtsam::VectorValues& V); + + //Testable + void print(string s); + bool equals(const gtsam::Errors& expected, double tol) const; +}; + +class GaussianISAM { + //Constructor + GaussianISAM(); + + //Standard Interface + void update(const gtsam::GaussianFactorGraph& newFactors); + void saveGraph(string s) const; + void clear(); +}; + +#include +virtual class IterativeOptimizationParameters { + string getVerbosity() const; + void setVerbosity(string s) ; + void print() const; +}; + +//virtual class IterativeSolver { +// IterativeSolver(); +// gtsam::VectorValues optimize (); +//}; + +#include +virtual class ConjugateGradientParameters : gtsam::IterativeOptimizationParameters { + ConjugateGradientParameters(); + int getMinIterations() const ; + int getMaxIterations() const ; + int getReset() const; + double getEpsilon_rel() const; + double getEpsilon_abs() const; + + void setMinIterations(int value); + void setMaxIterations(int value); + void setReset(int value); + void setEpsilon_rel(double value); + void setEpsilon_abs(double value); + void print(); +}; + +#include +virtual class SubgraphSolverParameters : gtsam::ConjugateGradientParameters { + SubgraphSolverParameters(); + void print() const; +}; + +virtual class SubgraphSolver { + SubgraphSolver(const gtsam::GaussianFactorGraph &A, const gtsam::SubgraphSolverParameters ¶meters, const gtsam::Ordering& ordering); + SubgraphSolver(const gtsam::GaussianFactorGraph &Ab1, const gtsam::GaussianFactorGraph &Ab2, const gtsam::SubgraphSolverParameters ¶meters, const gtsam::Ordering& ordering); + gtsam::VectorValues optimize() const; +}; + +#include +class KalmanFilter { + KalmanFilter(size_t n); + // gtsam::GaussianDensity* init(Vector x0, const gtsam::SharedDiagonal& P0); + gtsam::GaussianDensity* init(Vector x0, Matrix P0); + void print(string s) const; + static size_t step(gtsam::GaussianDensity* p); + gtsam::GaussianDensity* predict(gtsam::GaussianDensity* p, Matrix F, + Matrix B, Vector u, const gtsam::noiseModel::Diagonal* modelQ); + gtsam::GaussianDensity* predictQ(gtsam::GaussianDensity* p, Matrix F, + Matrix B, Vector u, Matrix Q); + gtsam::GaussianDensity* predict2(gtsam::GaussianDensity* p, Matrix A0, + Matrix A1, Vector b, const gtsam::noiseModel::Diagonal* model); + gtsam::GaussianDensity* update(gtsam::GaussianDensity* p, Matrix H, + Vector z, const gtsam::noiseModel::Diagonal* model); + gtsam::GaussianDensity* updateQ(gtsam::GaussianDensity* p, Matrix H, + Vector z, Matrix Q); +}; + +//************************************************************************* +// nonlinear +//************************************************************************* + +#include +size_t symbol(char chr, size_t index); +char symbolChr(size_t key); +size_t symbolIndex(size_t key); + +// Default keyformatter +void PrintKeyList (const gtsam::KeyList& keys); +void PrintKeyList (const gtsam::KeyList& keys, string s); +void PrintKeyVector(const gtsam::KeyVector& keys); +void PrintKeyVector(const gtsam::KeyVector& keys, string s); +void PrintKeySet (const gtsam::KeySet& keys); +void PrintKeySet (const gtsam::KeySet& keys, string s); + +#include +class LabeledSymbol { + LabeledSymbol(size_t full_key); + LabeledSymbol(const gtsam::LabeledSymbol& key); + LabeledSymbol(unsigned char valType, unsigned char label, size_t j); + + size_t key() const; + unsigned char label() const; + unsigned char chr() const; + size_t index() const; + + gtsam::LabeledSymbol upper() const; + gtsam::LabeledSymbol lower() const; + gtsam::LabeledSymbol newChr(unsigned char c) const; + gtsam::LabeledSymbol newLabel(unsigned char label) const; + + void print(string s) const; +}; + +size_t mrsymbol(unsigned char c, unsigned char label, size_t j); +unsigned char mrsymbolChr(size_t key); +unsigned char mrsymbolLabel(size_t key); +size_t mrsymbolIndex(size_t key); + +#include +class Ordering { + // Standard Constructors and Named Constructors + Ordering(); + Ordering(const gtsam::Ordering& other); + + // Testable + void print(string s) const; + bool equals(const gtsam::Ordering& ord, double tol) const; + + // Standard interface + size_t size() const; + size_t at(size_t key) const; + void push_back(size_t key); + + // enabling serialization functionality + void serialize() const; +}; + +class NonlinearFactorGraph { + NonlinearFactorGraph(); + NonlinearFactorGraph(const gtsam::NonlinearFactorGraph& graph); + + // FactorGraph + void print(string s) const; + bool equals(const gtsam::NonlinearFactorGraph& fg, double tol) const; + size_t size() const; + bool empty() const; + void remove(size_t i); + size_t nrFactors() const; + gtsam::NonlinearFactor* at(size_t idx) const; + void push_back(const gtsam::NonlinearFactorGraph& factors); + void push_back(gtsam::NonlinearFactor* factor); + void add(gtsam::NonlinearFactor* factor); + bool exists(size_t idx) const; + gtsam::KeySet keys() const; + + // NonlinearFactorGraph + double error(const gtsam::Values& values) const; + double probPrime(const gtsam::Values& values) const; + gtsam::Ordering orderingCOLAMD() const; + // Ordering* orderingCOLAMDConstrained(const gtsam::Values& c, const std::map& constraints) const; + gtsam::GaussianFactorGraph* linearize(const gtsam::Values& values) const; + gtsam::NonlinearFactorGraph clone() const; + + // enabling serialization functionality + void serialize() const; +}; + +virtual class NonlinearFactor { + // Factor base class + size_t size() const; + gtsam::KeyVector keys() const; + void print(string s) const; + void printKeys(string s) const; + // NonlinearFactor + void equals(const gtsam::NonlinearFactor& other, double tol) const; + double error(const gtsam::Values& c) const; + size_t dim() const; + bool active(const gtsam::Values& c) const; + gtsam::GaussianFactor* linearize(const gtsam::Values& c) const; + gtsam::NonlinearFactor* clone() const; + // gtsam::NonlinearFactor* rekey(const gtsam::KeyVector& newKeys) const; //FIXME: Conversion from KeyVector to std::vector does not happen +}; + +virtual class NoiseModelFactor: gtsam::NonlinearFactor { + void equals(const gtsam::NoiseModelFactor& other, double tol) const; + gtsam::noiseModel::Base* get_noiseModel() const; + Vector unwhitenedError(const gtsam::Values& x) const; + Vector whitenedError(const gtsam::Values& x) const; +}; + +#include +class Values { + Values(); + Values(const gtsam::Values& other); + + size_t size() const; + bool empty() const; + void clear(); + size_t dim() const; + + void print(string s) const; + bool equals(const gtsam::Values& other, double tol) const; + + void insert(const gtsam::Values& values); + void update(const gtsam::Values& values); + void erase(size_t j); + void swap(gtsam::Values& values); + + bool exists(size_t j) const; + gtsam::KeyVector keys() const; + + gtsam::VectorValues zeroVectors() const; + + gtsam::Values retract(const gtsam::VectorValues& delta) const; + gtsam::VectorValues localCoordinates(const gtsam::Values& cp) const; + + // enabling serialization functionality + void serialize() const; + + // New in 4.0, we have to specialize every insert/update/at to generate wrappers + // Instead of the old: + // void insert(size_t j, const gtsam::Value& value); + // void update(size_t j, const gtsam::Value& val); + // gtsam::Value at(size_t j) const; + + void insert(size_t j, const gtsam::Point2& t); + void insert(size_t j, const gtsam::Point3& t); + void insert(size_t j, const gtsam::Rot2& t); + void insert(size_t j, const gtsam::Pose2& t); + void insert(size_t j, const gtsam::Rot3& t); + void insert(size_t j, const gtsam::Pose3& t); + void insert(size_t j, const gtsam::Cal3_S2& t); + void insert(size_t j, const gtsam::Cal3DS2& t); + void insert(size_t j, const gtsam::Cal3Bundler& t); + void insert(size_t j, const gtsam::EssentialMatrix& t); + void insert(size_t j, const gtsam::SimpleCamera& t); + void insert(size_t j, const gtsam::imuBias::ConstantBias& t); + void insert(size_t j, Vector t); + void insert(size_t j, Matrix t); + + // Fixed size version + void insertFixed(size_t j, Vector t, size_t n); + + void update(size_t j, const gtsam::Point2& t); + void update(size_t j, const gtsam::Point3& t); + void update(size_t j, const gtsam::Rot2& t); + void update(size_t j, const gtsam::Pose2& t); + void update(size_t j, const gtsam::Rot3& t); + void update(size_t j, const gtsam::Pose3& t); + void update(size_t j, const gtsam::Cal3_S2& t); + void update(size_t j, const gtsam::Cal3DS2& t); + void update(size_t j, const gtsam::Cal3Bundler& t); + void update(size_t j, const gtsam::EssentialMatrix& t); + void update(size_t j, const gtsam::imuBias::ConstantBias& t); + void update(size_t j, Vector t); + void update(size_t j, Matrix t); + + template + T at(size_t j); + + /// Fixed size versions, for n in 1..9 + void insertFixed(size_t j, Vector t, size_t n); + + /// Fixed size versions, for n in 1..9 + Vector atFixed(size_t j, size_t n); + + /// version for double + void insertDouble(size_t j, double c); + double atDouble(size_t j) const; +}; + +// Actually a FastList +#include +class KeyList { + KeyList(); + KeyList(const gtsam::KeyList& other); + + // Note: no print function + + // common STL methods + size_t size() const; + bool empty() const; + void clear(); + + // structure specific methods + size_t front() const; + size_t back() const; + void push_back(size_t key); + void push_front(size_t key); + void pop_back(); + void pop_front(); + void sort(); + void remove(size_t key); + + void serialize() const; +}; + +// Actually a FastSet +class KeySet { + KeySet(); + KeySet(const gtsam::KeySet& other); + KeySet(const gtsam::KeyVector& other); + KeySet(const gtsam::KeyList& other); + + // Testable + void print(string s) const; + bool equals(const gtsam::KeySet& other) const; + + // common STL methods + size_t size() const; + bool empty() const; + void clear(); + + // structure specific methods + void insert(size_t key); + void merge(gtsam::KeySet& other); + bool erase(size_t key); // returns true if value was removed + bool count(size_t key) const; // returns true if value exists + + void serialize() const; +}; + +// Actually a vector +class KeyVector { + KeyVector(); + KeyVector(const gtsam::KeyVector& other); + + // Note: no print function + + // common STL methods + size_t size() const; + bool empty() const; + void clear(); + + // structure specific methods + size_t at(size_t i) const; + size_t front() const; + size_t back() const; + void push_back(size_t key) const; + + void serialize() const; +}; + +// Actually a FastMap +class KeyGroupMap { + KeyGroupMap(); + + // Note: no print function + + // common STL methods + size_t size() const; + bool empty() const; + void clear(); + + // structure specific methods + size_t at(size_t key) const; + int erase(size_t key); + bool insert2(size_t key, int val); +}; + +#include +class Marginals { + Marginals(const gtsam::NonlinearFactorGraph& graph, + const gtsam::Values& solution); + + void print(string s) const; + Matrix marginalCovariance(size_t variable) const; + Matrix marginalInformation(size_t variable) const; + gtsam::JointMarginal jointMarginalCovariance(const gtsam::KeyVector& variables) const; + gtsam::JointMarginal jointMarginalInformation(const gtsam::KeyVector& variables) const; +}; + +class JointMarginal { + Matrix at(size_t iVariable, size_t jVariable) const; + Matrix fullMatrix() const; + void print(string s) const; + void print() const; +}; + +#include +virtual class LinearContainerFactor : gtsam::NonlinearFactor { + + LinearContainerFactor(gtsam::GaussianFactor* factor, const gtsam::Values& linearizationPoint); + LinearContainerFactor(gtsam::GaussianFactor* factor); + + gtsam::GaussianFactor* factor() const; +// const boost::optional& linearizationPoint() const; + + bool isJacobian() const; + gtsam::JacobianFactor* toJacobian() const; + gtsam::HessianFactor* toHessian() const; + + static gtsam::NonlinearFactorGraph convertLinearGraph(const gtsam::GaussianFactorGraph& linear_graph, + const gtsam::Values& linearizationPoint); + + static gtsam::NonlinearFactorGraph convertLinearGraph(const gtsam::GaussianFactorGraph& linear_graph); + + // enabling serialization functionality + void serializable() const; +}; // \class LinearContainerFactor + +// Summarization functionality +//#include +// +//// Uses partial QR approach by default +//gtsam::GaussianFactorGraph summarize( +// const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values, +// const gtsam::KeySet& saved_keys); +// +//gtsam::NonlinearFactorGraph summarizeAsNonlinearContainer( +// const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values, +// const gtsam::KeySet& saved_keys); + +//************************************************************************* +// Nonlinear optimizers +//************************************************************************* + +#include +#include +virtual class NonlinearOptimizerParams { + NonlinearOptimizerParams(); + void print(string s) const; + + int getMaxIterations() const; + double getRelativeErrorTol() const; + double getAbsoluteErrorTol() const; + double getErrorTol() const; + string getVerbosity() const; + + void setMaxIterations(int value); + void setRelativeErrorTol(double value); + void setAbsoluteErrorTol(double value); + void setErrorTol(double value); + void setVerbosity(string s); + + string getLinearSolverType() const; + + void setLinearSolverType(string solver); + void setOrdering(const gtsam::Ordering& ordering); + void setIterativeParams(gtsam::IterativeOptimizationParameters* params); + + bool isMultifrontal() const; + bool isSequential() const; + bool isCholmod() const; + bool isIterative() const; +}; + +bool checkConvergence(double relativeErrorTreshold, + double absoluteErrorTreshold, double errorThreshold, + double currentError, double newError); + +#include +virtual class GaussNewtonParams : gtsam::NonlinearOptimizerParams { + GaussNewtonParams(); +}; + +#include +virtual class LevenbergMarquardtParams : gtsam::NonlinearOptimizerParams { + LevenbergMarquardtParams(); + + double getlambdaInitial() const; + double getlambdaFactor() const; + double getlambdaUpperBound() const; + string getVerbosityLM() const; + + void setlambdaInitial(double value); + void setlambdaFactor(double value); + void setlambdaUpperBound(double value); + void setVerbosityLM(string s); +}; + +#include +virtual class DoglegParams : gtsam::NonlinearOptimizerParams { + DoglegParams(); + + double getDeltaInitial() const; + string getVerbosityDL() const; + + void setDeltaInitial(double deltaInitial) const; + void setVerbosityDL(string verbosityDL) const; +}; + +virtual class NonlinearOptimizer { + gtsam::Values optimize(); + gtsam::Values optimizeSafely(); + double error() const; + int iterations() const; + gtsam::Values values() const; + void iterate() const; +}; + +virtual class GaussNewtonOptimizer : gtsam::NonlinearOptimizer { + GaussNewtonOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues); + GaussNewtonOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues, const gtsam::GaussNewtonParams& params); +}; + +virtual class DoglegOptimizer : gtsam::NonlinearOptimizer { + DoglegOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues); + DoglegOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues, const gtsam::DoglegParams& params); + double getDelta() const; +}; + +virtual class LevenbergMarquardtOptimizer : gtsam::NonlinearOptimizer { + LevenbergMarquardtOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues); + LevenbergMarquardtOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues, const gtsam::LevenbergMarquardtParams& params); + double lambda() const; + void print(string str) const; +}; + +#include +class ISAM2GaussNewtonParams { + ISAM2GaussNewtonParams(); + + void print(string str) const; + + /** Getters and Setters for all properties */ + double getWildfireThreshold() const; + void setWildfireThreshold(double wildfireThreshold); +}; + +class ISAM2DoglegParams { + ISAM2DoglegParams(); + + void print(string str) const; + + /** Getters and Setters for all properties */ + double getWildfireThreshold() const; + void setWildfireThreshold(double wildfireThreshold); + double getInitialDelta() const; + void setInitialDelta(double initialDelta); + string getAdaptationMode() const; + void setAdaptationMode(string adaptationMode); + bool isVerbose() const; + void setVerbose(bool verbose); +}; + +class ISAM2ThresholdMapValue { + ISAM2ThresholdMapValue(char c, Vector thresholds); + ISAM2ThresholdMapValue(const gtsam::ISAM2ThresholdMapValue& other); +}; + +class ISAM2ThresholdMap { + ISAM2ThresholdMap(); + ISAM2ThresholdMap(const gtsam::ISAM2ThresholdMap& other); + + // Note: no print function + + // common STL methods + size_t size() const; + bool empty() const; + void clear(); + + // structure specific methods + void insert(const gtsam::ISAM2ThresholdMapValue& value) const; +}; + +class ISAM2Params { + ISAM2Params(); + + void print(string str) const; + + /** Getters and Setters for all properties */ + void setOptimizationParams(const gtsam::ISAM2GaussNewtonParams& params); + void setOptimizationParams(const gtsam::ISAM2DoglegParams& params); + void setRelinearizeThreshold(double relinearizeThreshold); + void setRelinearizeThreshold(const gtsam::ISAM2ThresholdMap& relinearizeThreshold); + int getRelinearizeSkip() const; + void setRelinearizeSkip(int relinearizeSkip); + bool isEnableRelinearization() const; + void setEnableRelinearization(bool enableRelinearization); + bool isEvaluateNonlinearError() const; + void setEvaluateNonlinearError(bool evaluateNonlinearError); + string getFactorization() const; + void setFactorization(string factorization); + bool isCacheLinearizedFactors() const; + void setCacheLinearizedFactors(bool cacheLinearizedFactors); + bool isEnableDetailedResults() const; + void setEnableDetailedResults(bool enableDetailedResults); + bool isEnablePartialRelinearizationCheck() const; + void setEnablePartialRelinearizationCheck(bool enablePartialRelinearizationCheck); +}; + +class ISAM2Clique { + + //Constructors + ISAM2Clique(); + + //Standard Interface + Vector gradientContribution() const; + void print(string s); +}; + +class ISAM2Result { + ISAM2Result(); + + void print(string str) const; + + /** Getters and Setters for all properties */ + size_t getVariablesRelinearized() const; + size_t getVariablesReeliminated() const; + size_t getCliques() const; +}; + +class ISAM2 { + ISAM2(); + ISAM2(const gtsam::ISAM2Params& params); + ISAM2(const gtsam::ISAM2& other); + + bool equals(const gtsam::ISAM2& other, double tol) const; + void print(string s) const; + void printStats() const; + void saveGraph(string s) const; + + gtsam::ISAM2Result update(); + gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta); + gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& removeFactorIndices); + gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& removeFactorIndices, const gtsam::KeyGroupMap& constrainedKeys); + // TODO: wrap the full version of update + //void update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& removeFactorIndices, FastMap& constrainedKeys); + //void update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& removeFactorIndices, FastMap& constrainedKeys, bool force_relinearize); + + gtsam::Values getLinearizationPoint() const; + gtsam::Values calculateEstimate() const; + gtsam::Value calculateEstimate(size_t key) const; + gtsam::Values calculateBestEstimate() const; + Matrix marginalCovariance(size_t key) const; + gtsam::VectorValues getDelta() const; + gtsam::NonlinearFactorGraph getFactorsUnsafe() const; + gtsam::VariableIndex getVariableIndex() const; + gtsam::ISAM2Params params() const; +}; + +#include +class NonlinearISAM { + NonlinearISAM(); + NonlinearISAM(int reorderInterval); + void print(string s) const; + void printStats() const; + void saveGraph(string s) const; + gtsam::Values estimate() const; + Matrix marginalCovariance(size_t key) const; + int reorderInterval() const; + int reorderCounter() const; + void update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& initialValues); + void reorder_relinearize(); + + // These might be expensive as instead of a reference the wrapper will make a copy + gtsam::GaussianISAM bayesTree() const; + gtsam::Values getLinearizationPoint() const; + gtsam::NonlinearFactorGraph getFactorsUnsafe() const; +}; + +//************************************************************************* +// Nonlinear factor types +//************************************************************************* +#include +#include +#include + +#include +template +virtual class PriorFactor : gtsam::NoiseModelFactor { + PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); + T prior() const; + + // enabling serialization functionality + void serialize() const; +}; + + +#include +template +virtual class BetweenFactor : gtsam::NoiseModelFactor { + BetweenFactor(size_t key1, size_t key2, const T& relativePose, const gtsam::noiseModel::Base* noiseModel); + T measured() const; + + // enabling serialization functionality + void serialize() const; +}; + + + +#include +template +virtual class NonlinearEquality : gtsam::NoiseModelFactor { + // Constructor - forces exact evaluation + NonlinearEquality(size_t j, const T& feasible); + // Constructor - allows inexact evaluation + NonlinearEquality(size_t j, const T& feasible, double error_gain); + + // enabling serialization functionality + void serialize() const; +}; + + +#include +template +virtual class RangeFactor : gtsam::NoiseModelFactor { + RangeFactor(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base* noiseModel); +}; + +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; + + +#include +template +virtual class BearingFactor : gtsam::NoiseModelFactor { + BearingFactor(size_t key1, size_t key2, const BEARING& measured, const gtsam::noiseModel::Base* noiseModel); + + // enabling serialization functionality + void serialize() const; +}; + +typedef gtsam::BearingFactor BearingFactor2D; + +#include +template +virtual class BearingRangeFactor : gtsam::NoiseModelFactor { + BearingRangeFactor(size_t poseKey, size_t pointKey, + const BEARING& measuredBearing, const RANGE& measuredRange, + const gtsam::noiseModel::Base* noiseModel); + + // enabling serialization functionality + void serialize() const; +}; + +typedef gtsam::BearingRangeFactor BearingRangeFactor2D; + + +#include +template +virtual class GenericProjectionFactor : gtsam::NoiseModelFactor { + GenericProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel, + size_t poseKey, size_t pointKey, const CALIBRATION* k); + GenericProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel, + size_t poseKey, size_t pointKey, const CALIBRATION* k, const POSE& body_P_sensor); + + GenericProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel, + size_t poseKey, size_t pointKey, const CALIBRATION* k, bool throwCheirality, bool verboseCheirality); + GenericProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel, + size_t poseKey, size_t pointKey, const CALIBRATION* k, bool throwCheirality, bool verboseCheirality, + const POSE& body_P_sensor); + + gtsam::Point2 measured() const; + CALIBRATION* calibration() const; + bool verboseCheirality() const; + bool throwCheirality() const; + + // enabling serialization functionality + void serialize() const; +}; +typedef gtsam::GenericProjectionFactor GenericProjectionFactorCal3_S2; +typedef gtsam::GenericProjectionFactor GenericProjectionFactorCal3DS2; + + +#include +template +virtual class GeneralSFMFactor : gtsam::NoiseModelFactor { + GeneralSFMFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* model, size_t cameraKey, size_t landmarkKey); + gtsam::Point2 measured() const; +}; +typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3_S2; +typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3DS2; + +template +virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor { + GeneralSFMFactor2(const gtsam::Point2& measured, const gtsam::noiseModel::Base* model, size_t poseKey, size_t landmarkKey, size_t calibKey); + gtsam::Point2 measured() const; + + // enabling serialization functionality + void serialize() const; +}; + +#include +class SmartProjectionParams { + SmartProjectionParams(); + // TODO(frank): make these work: + // void setLinearizationMode(LinearizationMode linMode); + // void setDegeneracyMode(DegeneracyMode degMode); + void setRankTolerance(double rankTol); + void setEnableEPI(bool enableEPI); + void setLandmarkDistanceThreshold(bool landmarkDistanceThreshold); + void setDynamicOutlierRejectionThreshold(bool dynOutRejectionThreshold); +}; + +#include +template +virtual class SmartProjectionPoseFactor: gtsam::NonlinearFactor { + + SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise, + const CALIBRATION* K); + SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise, + const CALIBRATION* K, + const gtsam::Pose3& body_P_sensor); + SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise, + const CALIBRATION* K, + const gtsam::Pose3& body_P_sensor, + const gtsam::SmartProjectionParams& params); + + void add(const gtsam::Point2& measured_i, size_t poseKey_i); + + // enabling serialization functionality + //void serialize() const; +}; + +typedef gtsam::SmartProjectionPoseFactor SmartProjectionPose3Factor; + + +#include +template +virtual class GenericStereoFactor : gtsam::NoiseModelFactor { + GenericStereoFactor(const gtsam::StereoPoint2& measured, const gtsam::noiseModel::Base* noiseModel, + size_t poseKey, size_t landmarkKey, const gtsam::Cal3_S2Stereo* K); + gtsam::StereoPoint2 measured() const; + gtsam::Cal3_S2Stereo* calibration() const; + + // enabling serialization functionality + void serialize() const; +}; +typedef gtsam::GenericStereoFactor GenericStereoFactor3D; + +#include +template +virtual class PoseTranslationPrior : gtsam::NoiseModelFactor { + PoseTranslationPrior(size_t key, const POSE& pose_z, const gtsam::noiseModel::Base* noiseModel); +}; + +typedef gtsam::PoseTranslationPrior PoseTranslationPrior2D; +typedef gtsam::PoseTranslationPrior PoseTranslationPrior3D; + +#include +template +virtual class PoseRotationPrior : gtsam::NoiseModelFactor { + PoseRotationPrior(size_t key, const POSE& pose_z, const gtsam::noiseModel::Base* noiseModel); +}; + +typedef gtsam::PoseRotationPrior PoseRotationPrior2D; +typedef gtsam::PoseRotationPrior PoseRotationPrior3D; + +#include +virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor { + EssentialMatrixFactor(size_t key, const gtsam::Point2& pA, const gtsam::Point2& pB, + const gtsam::noiseModel::Base* noiseModel); +}; + +#include +pair load2D(string filename, + gtsam::noiseModel::Diagonal* model, int maxID, bool addNoise, bool smart); +pair load2D(string filename, + gtsam::noiseModel::Diagonal* model, int maxID, bool addNoise); +pair load2D(string filename, + gtsam::noiseModel::Diagonal* model, int maxID); +pair load2D(string filename, + gtsam::noiseModel::Diagonal* model); +pair load2D(string filename); +pair load2D_robust(string filename, + gtsam::noiseModel::Base* model); +void save2D(const gtsam::NonlinearFactorGraph& graph, + const gtsam::Values& config, gtsam::noiseModel::Diagonal* model, + string filename); + +pair readG2o(string filename); +void writeG2o(const gtsam::NonlinearFactorGraph& graph, + const gtsam::Values& estimate, string filename); + +//************************************************************************* +// Navigation +//************************************************************************* +namespace imuBias { +#include + +class ConstantBias { + // Standard Constructor + ConstantBias(); + ConstantBias(Vector biasAcc, Vector biasGyro); + + // Testable + void print(string s) const; + bool equals(const gtsam::imuBias::ConstantBias& expected, double tol) const; + + // Group + static gtsam::imuBias::ConstantBias identity(); + gtsam::imuBias::ConstantBias inverse() const; + gtsam::imuBias::ConstantBias compose(const gtsam::imuBias::ConstantBias& b) const; + gtsam::imuBias::ConstantBias between(const gtsam::imuBias::ConstantBias& b) const; + + // Manifold + gtsam::imuBias::ConstantBias retract(Vector v) const; + Vector localCoordinates(const gtsam::imuBias::ConstantBias& b) const; + + // Lie Group + static gtsam::imuBias::ConstantBias Expmap(Vector v); + static Vector Logmap(const gtsam::imuBias::ConstantBias& b); + + // Standard Interface + Vector vector() const; + Vector accelerometer() const; + Vector gyroscope() const; + Vector correctAccelerometer(Vector measurement) const; + Vector correctGyroscope(Vector measurement) const; +}; + +}///\namespace imuBias + +#include +class PoseVelocityBias{ + PoseVelocityBias(const gtsam::Pose3& pose, Vector velocity, const gtsam::imuBias::ConstantBias& bias); + }; +class PreintegratedImuMeasurements { + // Standard Constructor + PreintegratedImuMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance,Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance, bool use2ndOrderIntegration); + PreintegratedImuMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance,Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance); + // PreintegratedImuMeasurements(const gtsam::PreintegratedImuMeasurements& rhs); + + // Testable + void print(string s) const; + bool equals(const gtsam::PreintegratedImuMeasurements& expected, double tol); + + double deltaTij() const; + gtsam::Rot3 deltaRij() const; + Vector deltaPij() const; + Vector deltaVij() const; + Vector biasHatVector() const; + Matrix delPdelBiasAcc() const; + Matrix delPdelBiasOmega() const; + Matrix delVdelBiasAcc() const; + Matrix delVdelBiasOmega() const; + Matrix delRdelBiasOmega() const; + Matrix preintMeasCov() const; + + // Standard Interface + void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT); + gtsam::PoseVelocityBias predict(const gtsam::Pose3& pose_i, Vector vel_i, const gtsam::imuBias::ConstantBias& bias, + Vector gravity, Vector omegaCoriolis) const; +}; + +virtual class ImuFactor : gtsam::NonlinearFactor { + ImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias, + const gtsam::PreintegratedImuMeasurements& preintegratedMeasurements, Vector gravity, Vector omegaCoriolis); + ImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias, + const gtsam::PreintegratedImuMeasurements& preintegratedMeasurements, Vector gravity, Vector omegaCoriolis, + const gtsam::Pose3& body_P_sensor); + // Standard Interface + gtsam::PreintegratedImuMeasurements preintegratedMeasurements() const; +}; + +#include +class PreintegratedCombinedMeasurements { + // Standard Constructor + PreintegratedCombinedMeasurements( + const gtsam::imuBias::ConstantBias& bias, + Matrix measuredAccCovariance, + Matrix measuredOmegaCovariance, + Matrix integrationErrorCovariance, + Matrix biasAccCovariance, + Matrix biasOmegaCovariance, + Matrix biasAccOmegaInit); + PreintegratedCombinedMeasurements( + const gtsam::imuBias::ConstantBias& bias, + Matrix measuredAccCovariance, + Matrix measuredOmegaCovariance, + Matrix integrationErrorCovariance, + Matrix biasAccCovariance, + Matrix biasOmegaCovariance, + Matrix biasAccOmegaInit, + bool use2ndOrderIntegration); + // PreintegratedCombinedMeasurements(const gtsam::PreintegratedCombinedMeasurements& rhs); + + // Testable + void print(string s) const; + bool equals(const gtsam::PreintegratedCombinedMeasurements& expected, double tol); + + double deltaTij() const; + gtsam::Rot3 deltaRij() const; + Vector deltaPij() const; + Vector deltaVij() const; + Vector biasHatVector() const; + Matrix delPdelBiasAcc() const; + Matrix delPdelBiasOmega() const; + Matrix delVdelBiasAcc() const; + Matrix delVdelBiasOmega() const; + Matrix delRdelBiasOmega() const; + Matrix preintMeasCov() const; + + // Standard Interface + void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT); + gtsam::PoseVelocityBias predict(const gtsam::Pose3& pose_i, Vector vel_i, const gtsam::imuBias::ConstantBias& bias, + Vector gravity, Vector omegaCoriolis) const; +}; + +virtual class CombinedImuFactor : gtsam::NonlinearFactor { + CombinedImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias_i, size_t bias_j, + const gtsam::PreintegratedCombinedMeasurements& CombinedPreintegratedMeasurements, Vector gravity, Vector omegaCoriolis); + // Standard Interface + gtsam::PreintegratedCombinedMeasurements preintegratedMeasurements() const; +}; + +#include +class PreintegratedAhrsMeasurements { + // Standard Constructor + PreintegratedAhrsMeasurements(Vector bias, Matrix measuredOmegaCovariance); + PreintegratedAhrsMeasurements(Vector bias, Matrix measuredOmegaCovariance); + PreintegratedAhrsMeasurements(const gtsam::PreintegratedAhrsMeasurements& rhs); + + // Testable + void print(string s) const; + bool equals(const gtsam::PreintegratedAhrsMeasurements& expected, double tol); + + // get Data + gtsam::Rot3 deltaRij() const; + double deltaTij() const; + Vector biasHat() const; + + // Standard Interface + void integrateMeasurement(Vector measuredOmega, double deltaT); + void resetIntegration() ; +}; + +virtual class AHRSFactor : gtsam::NonlinearFactor { + AHRSFactor(size_t rot_i, size_t rot_j,size_t bias, + const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, Vector omegaCoriolis); + AHRSFactor(size_t rot_i, size_t rot_j, size_t bias, + const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, Vector omegaCoriolis, + const gtsam::Pose3& body_P_sensor); + + // Standard Interface + gtsam::PreintegratedAhrsMeasurements preintegratedMeasurements() const; + Vector evaluateError(const gtsam::Rot3& rot_i, const gtsam::Rot3& rot_j, + Vector bias) const; + gtsam::Rot3 predict(const gtsam::Rot3& rot_i, Vector bias, + const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, + Vector omegaCoriolis) const; +}; + +#include +//virtual class AttitudeFactor : gtsam::NonlinearFactor { +// AttitudeFactor(const Unit3& nZ, const Unit3& bRef); +// AttitudeFactor(); +//}; +virtual class Rot3AttitudeFactor : gtsam::NonlinearFactor{ + Rot3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model, + const gtsam::Unit3& bRef); + Rot3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model); + Rot3AttitudeFactor(); + void print(string s) const; + bool equals(const gtsam::NonlinearFactor& expected, double tol) const; + gtsam::Unit3 nZ() const; + gtsam::Unit3 bRef() const; +}; + +virtual class Pose3AttitudeFactor : gtsam::NonlinearFactor{ + Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model, + const gtsam::Unit3& bRef); + Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model); + Pose3AttitudeFactor(); + void print(string s) const; + bool equals(const gtsam::NonlinearFactor& expected, double tol) const; + gtsam::Unit3 nZ() const; + gtsam::Unit3 bRef() const; +}; + +//************************************************************************* +// Utilities +//************************************************************************* + +namespace utilities { + + #include + gtsam::KeyList createKeyList(Vector I); + gtsam::KeyList createKeyList(string s, Vector I); + gtsam::KeyVector createKeyVector(Vector I); + gtsam::KeyVector createKeyVector(string s, Vector I); + gtsam::KeySet createKeySet(Vector I); + gtsam::KeySet createKeySet(string s, Vector I); + Matrix extractPoint2(const gtsam::Values& values); + Matrix extractPoint3(const gtsam::Values& values); + Matrix extractPose2(const gtsam::Values& values); + gtsam::Values allPose3s(gtsam::Values& values); + Matrix extractPose3(const gtsam::Values& values); + void perturbPoint2(gtsam::Values& values, double sigma, int seed); + void perturbPose2 (gtsam::Values& values, double sigmaT, double sigmaR, int seed); + void perturbPoint3(gtsam::Values& values, double sigma, int seed); + void insertBackprojections(gtsam::Values& values, const gtsam::SimpleCamera& c, Vector J, Matrix Z, double depth); + void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K); + void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K, const gtsam::Pose3& body_P_sensor); + Matrix reprojectionErrors(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values); + gtsam::Values localToWorld(const gtsam::Values& local, const gtsam::Pose2& base); + gtsam::Values localToWorld(const gtsam::Values& local, const gtsam::Pose2& base, const gtsam::KeyVector& keys); + +} //\namespace utilities + +} diff --git a/python/.gitignore b/python/.gitignore new file mode 100644 index 000000000..d16386367 --- /dev/null +++ b/python/.gitignore @@ -0,0 +1 @@ +build/ \ No newline at end of file diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt new file mode 100644 index 000000000..f7ceb62b3 --- /dev/null +++ b/python/CMakeLists.txt @@ -0,0 +1,96 @@ +# Guard to avoid breaking this code in ccmake if by accident GTSAM_PYTHON_VERSION is set to an empty string +if(GTSAM_PYTHON_VERSION STREQUAL "") + set(GTSAM_PYTHON_VERSION "Default" CACHE STRING "Target python version for GTSAM python module. Use 'Default' to chose the default version" FORCE) +endif() + +# The code below allows to clear the PythonLibs cache if we change GTSAM_PYTHON_VERSION +# Inspired from the solution found here: http://blog.bethcodes.com/cmake-tips-tricks-drop-down-list +if(NOT DEFINED GTSAM_LAST_PYTHON_VERSION) + set(GTSAM_LAST_PYTHON_VERSION ${GTSAM_PYTHON_VERSION} CACHE STRING "Python version used in the last build") + mark_as_advanced(FORCE GTSAM_LAST_PYTHON_VERSION) +endif() +if(NOT (${GTSAM_PYTHON_VERSION} MATCHES ${GTSAM_LAST_PYTHON_VERSION})) + unset(PYTHON_INCLUDE_DIR CACHE) + unset(PYTHON_INCLUDE_DIR2 CACHE) + unset(PYTHON_LIBRARY CACHE) + unset(PYTHON_LIBRARY_DEBUG CACHE) + set(GTSAM_LAST_PYTHON_VERSION ${GTSAM_PYTHON_VERSION} CACHE STRING "Updating python version used in the last build" FORCE) +endif() + +if(GTSAM_PYTHON_VERSION STREQUAL "Default") + # Search the default version. + find_package(PythonInterp) + find_package(PythonLibs) +else() + find_package(PythonInterp ${GTSAM_PYTHON_VERSION}) + find_package(PythonLibs ${GTSAM_PYTHON_VERSION}) +endif() + +# Find NumPy C-API -- this is part of the numpy package +find_package(NumPy) + +# Compose strings used to specify the boost python version. They will be empty if we want to use the defaut +if(NOT GTSAM_PYTHON_VERSION STREQUAL "Default") + string(REPLACE "." "" BOOST_PYTHON_VERSION_SUFFIX ${GTSAM_PYTHON_VERSION}) # Remove '.' from version + string(SUBSTRING ${BOOST_PYTHON_VERSION_SUFFIX} 0 2 BOOST_PYTHON_VERSION_SUFFIX) # Trim version number to 2 digits + set(BOOST_PYTHON_VERSION_SUFFIX "-py${BOOST_PYTHON_VERSION_SUFFIX}") # Append '-py' prefix + string(TOUPPER ${BOOST_PYTHON_VERSION_SUFFIX} BOOST_PYTHON_VERSION_SUFFIX_UPPERCASE) # Get uppercase string +else() + set(BOOST_PYTHON_VERSION_SUFFIX "") + set(BOOST_PYTHON_VERSION_SUFFIX_UPPERCASE "") +endif() + +# Find Boost Python +find_package(Boost COMPONENTS python${BOOST_PYTHON_VERSION_SUFFIX}) + +if(Boost_PYTHON${BOOST_PYTHON_VERSION_SUFFIX_UPPERCASE}_FOUND AND PYTHONLIBS_FOUND AND NUMPY_FOUND) + # Build library + include_directories(${NUMPY_INCLUDE_DIRS}) + include_directories(${PYTHON_INCLUDE_DIRS}) + include_directories(${Boost_INCLUDE_DIRS}) + include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include/) + + # Build the python module library + add_subdirectory(handwritten) + + # Create and invoke setup.py, see https://bloerg.net/2012/11/10/cmake-and-distutils.html + set(SETUP_PY_IN "${CMAKE_CURRENT_SOURCE_DIR}/setup.py.in") + set(SETUP_PY "${CMAKE_CURRENT_BINARY_DIR}/setup.py") + + # Hacky way to figure out install folder - valid for Linux & Mac + # default pattern: prefix/lib/pythonX.Y/site-packages from https://docs.python.org/2/install/ + SET(PY_INSTALL_FOLDER "${CMAKE_INSTALL_PREFIX}/lib/python${PYTHON_VERSION_MAJOR}.${PYTHON_VERSION_MINOR}/site-packages") + + configure_file(${SETUP_PY_IN} ${SETUP_PY}) + + # TODO(frank): possibly support a different prefix a la matlab wrapper + install(CODE "execute_process(WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR} COMMAND ${PYTHON_EXECUTABLE} setup.py -v install --prefix ${CMAKE_INSTALL_PREFIX})") +else() + # Disable python module if we didn't find required libraries + # message will print at end of main CMakeLists.txt + SET(GTSAM_PYTHON_WARNINGS "Python dependencies not found - Python module will not be built. Set GTSAM_BUILD_PYTHON to 'Off' to disable this warning. Details:") + + if(NOT PYTHONLIBS_FOUND) + if(GTSAM_PYTHON_VERSION STREQUAL "Default") + SET(GTSAM_PYTHON_WARNINGS "${GTSAM_PYTHON_WARNINGS}\n -- Default PythonLibs not found") + else() + SET(GTSAM_PYTHON_WARNINGS "${GTSAM_PYTHON_WARNINGS}\n -- PythonLibs version ${GTSAM_PYTHON_VERSION} not found") + endif() + endif() + + if(NOT NUMPY_FOUND) + SET(GTSAM_PYTHON_WARNINGS "${GTSAM_PYTHON_WARNINGS}\n -- Numpy not found") + endif() + + if(NOT Boost_PYTHON${BOOST_PYTHON_VERSION_SUFFIX_UPPERCASE}_FOUND) + if(GTSAM_PYTHON_VERSION STREQUAL "Default") + SET(GTSAM_PYTHON_WARNINGS "${GTSAM_PYTHON_WARNINGS}\n -- Default Boost python not found") + else() + SET(GTSAM_PYTHON_WARNINGS "${GTSAM_PYTHON_WARNINGS}\n -- Boost Python for python ${GTSAM_PYTHON_VERSION} not found") + endif() + endif() + + # make available at top-level + SET(GTSAM_PYTHON_WARNINGS ${GTSAM_PYTHON_WARNINGS} PARENT_SCOPE) + +endif() diff --git a/python/README.md b/python/README.md new file mode 100644 index 000000000..f1c218cfb --- /dev/null +++ b/python/README.md @@ -0,0 +1,18 @@ +Python Wrapper and Packaging +============================ + +This directory contains the basic setup script and directory structure for the gtsam python module. +During the build of gtsam, when GTSAM_BUILD_PYTHON is enabled, the following instructions will run. + +* The handwritten module source files are compiled and linked with Boost Python, generating a shared + library which can then be imported by python +* A setup.py script is configured from setup.py.in +* The gtsam packages 'gtsam', 'gtsam_utils', 'gtsam_examples', and 'gtsam_tests' are installed into + the site-packages folder within the (possibly non-default) installation prefix folder. If + installing to a non-standard prefix, make sure that _prefix_/lib/pythonX.Y/site-packages is + present in your $PYTHONPATH + +The target version of Python to create the module can be set by defining GTSAM_PYTHON_VERSION to 'X.Y' (Example: 2.7 or 3.4), or 'Default' if you want to use the default python installed in your system. Note that if you specify a target version of python, you should also have the correspondening Boost +Python version installed (Example: libboost_python-py27.so or libboost_python-py34.so on Linux). +If you're using the default version, your default Boost Python library (Example: libboost_python.so on Linux) should correspond to the default python version in your system. + diff --git a/python/gtsam/__init__.py b/python/gtsam/__init__.py new file mode 100644 index 000000000..f9e40517d --- /dev/null +++ b/python/gtsam/__init__.py @@ -0,0 +1 @@ +from ._libgtsam_python import * diff --git a/python/gtsam_examples/OdometryExample.py b/python/gtsam_examples/OdometryExample.py new file mode 100644 index 000000000..0800bab4e --- /dev/null +++ b/python/gtsam_examples/OdometryExample.py @@ -0,0 +1,36 @@ +#!/usr/bin/env python +from __future__ import print_function +import gtsam +import numpy as np + +# Create an empty nonlinear factor graph +graph = gtsam.NonlinearFactorGraph() + +# Add a prior on the first pose, setting it to the origin +# A prior factor consists of a mean and a noise model (covariance matrix) +priorMean = gtsam.Pose2(0.0, 0.0, 0.0) # prior at origin +priorNoise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.1])) +graph.add(gtsam.PriorFactorPose2(1, priorMean, priorNoise)) + +# Add odometry factors +odometry = gtsam.Pose2(2.0, 0.0, 0.0) +# For simplicity, we will use the same noise model for each odometry factor +odometryNoise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1])) +# Create odometry (Between) factors between consecutive poses +graph.add(gtsam.BetweenFactorPose2(1, 2, odometry, odometryNoise)) +graph.add(gtsam.BetweenFactorPose2(2, 3, odometry, odometryNoise)) +graph.print("\nFactor Graph:\n") + +# Create the data structure to hold the initialEstimate estimate to the solution +# For illustrative purposes, these have been deliberately set to incorrect values +initial = gtsam.Values() +initial.insert(1, gtsam.Pose2(0.5, 0.0, 0.2)) +initial.insert(2, gtsam.Pose2(2.3, 0.1, -0.2)) +initial.insert(3, gtsam.Pose2(4.1, 0.1, 0.1)) +initial.print("\nInitial Estimate:\n") + +# optimize using Levenberg-Marquardt optimization +params = gtsam.LevenbergMarquardtParams() +optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params) +result = optimizer.optimize() +result.print("\nFinal Result:\n") diff --git a/python/gtsam_examples/SFMdata.py b/python/gtsam_examples/SFMdata.py new file mode 100644 index 000000000..21a438226 --- /dev/null +++ b/python/gtsam_examples/SFMdata.py @@ -0,0 +1,32 @@ + + # A structure-from-motion example with landmarks + # - The landmarks form a 10 meter cube + # - The robot rotates around the landmarks, always facing towards the cube + +import gtsam +import numpy as np + +def createPoints(): + # Create the set of ground-truth landmarks + points = [gtsam.Point3(10.0,10.0,10.0), + gtsam.Point3(-10.0,10.0,10.0), + gtsam.Point3(-10.0,-10.0,10.0), + gtsam.Point3(10.0,-10.0,10.0), + gtsam.Point3(10.0,10.0,-10.0), + gtsam.Point3(-10.0,10.0,-10.0), + gtsam.Point3(-10.0,-10.0,-10.0), + gtsam.Point3(10.0,-10.0,-10.0)] + return points + +def createPoses(): + # Create the set of ground-truth poses + radius = 30.0 + angles = np.linspace(0,2*np.pi,8,endpoint=False) + up = gtsam.Point3(0,0,1) + target = gtsam.Point3(0,0,0) + poses = [] + for theta in angles: + position = gtsam.Point3(radius*np.cos(theta), radius*np.sin(theta), 0.0) + camera = gtsam.PinholeCameraCal3_S2.Lookat(position, target, up) + poses.append(camera.pose()) + return poses diff --git a/python/gtsam_examples/VisualISAM2Example.py b/python/gtsam_examples/VisualISAM2Example.py new file mode 100644 index 000000000..9dafa13e7 --- /dev/null +++ b/python/gtsam_examples/VisualISAM2Example.py @@ -0,0 +1,132 @@ +from __future__ import print_function + +import matplotlib.pyplot as plt +from mpl_toolkits.mplot3d import Axes3D +import numpy as np +import time # for sleep() + +import gtsam +from gtsam_examples import SFMdata +import gtsam_utils + +# shorthand symbols: +X = lambda i: int(gtsam.Symbol('x', i)) +L = lambda j: int(gtsam.Symbol('l', j)) + +def visual_ISAM2_plot(poses, points, result): + # VisualISAMPlot plots current state of ISAM2 object + # Author: Ellon Paiva + # Based on MATLAB version by: Duy Nguyen Ta and Frank Dellaert + + # Declare an id for the figure + fignum = 0 + + fig = plt.figure(fignum) + ax = fig.gca(projection='3d') + plt.cla() + + # Plot points + # Can't use data because current frame might not see all points + # marginals = Marginals(isam.getFactorsUnsafe(), isam.calculateEstimate()) # TODO - this is slow + # gtsam.plot3DPoints(result, [], marginals) + gtsam_utils.plot3DPoints(fignum, result, 'rx') + + # Plot cameras + i = 0 + while result.exists(X(i)): + pose_i = result.atPose3(X(i)) + gtsam_utils.plotPose3(fignum, pose_i, 10) + i += 1 + + # draw + ax.set_xlim3d(-40, 40) + ax.set_ylim3d(-40, 40) + ax.set_zlim3d(-40, 40) + plt.pause(1) + +def visual_ISAM2_example(): + plt.ion() + + # Define the camera calibration parameters + K = gtsam.Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0) + + # Define the camera observation noise model + measurementNoise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0) # one pixel in u and v + + # Create the set of ground-truth landmarks + points = SFMdata.createPoints() + + # Create the set of ground-truth poses + poses = SFMdata.createPoses() + + # Create an iSAM2 object. Unlike iSAM1, which performs periodic batch steps to maintain proper linearization + # and efficient variable ordering, iSAM2 performs partial relinearization/reordering at each step. A parameter + # structure is available that allows the user to set various properties, such as the relinearization threshold + # and type of linear solver. For this example, we we set the relinearization threshold small so the iSAM2 result + # will approach the batch result. + parameters = gtsam.ISAM2Params() + parameters.relinearize_threshold = 0.01 + parameters.relinearize_skip = 1 + isam = gtsam.ISAM2(parameters) + + # Create a Factor Graph and Values to hold the new data + graph = gtsam.NonlinearFactorGraph() + initialEstimate = gtsam.Values() + + # Loop over the different poses, adding the observations to iSAM incrementally + for i, pose in enumerate(poses): + + # Add factors for each landmark observation + for j, point in enumerate(points): + camera = gtsam.PinholeCameraCal3_S2(pose, K) + measurement = camera.project(point) + graph.push_back(gtsam.GenericProjectionFactorCal3_S2(measurement, measurementNoise, X(i), L(j), K)) + + # Add an initial guess for the current pose + # Intentionally initialize the variables off from the ground truth + initialEstimate.insert(X(i), pose.compose(gtsam.Pose3(gtsam.Rot3.Rodrigues(-0.1, 0.2, 0.25), gtsam.Point3(0.05, -0.10, 0.20)))) + + # If this is the first iteration, add a prior on the first pose to set the coordinate frame + # and a prior on the first landmark to set the scale + # Also, as iSAM solves incrementally, we must wait until each is observed at least twice before + # adding it to iSAM. + if(i == 0): + # Add a prior on pose x0 + poseNoise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1])) # 30cm std on x,y,z 0.1 rad on roll,pitch,yaw + graph.push_back(gtsam.PriorFactorPose3(X(0), poses[0], poseNoise)) + + # Add a prior on landmark l0 + pointNoise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1) + graph.push_back(gtsam.PriorFactorPoint3(L(0), points[0], pointNoise)) # add directly to graph + + # Add initial guesses to all observed landmarks + # Intentionally initialize the variables off from the ground truth + for j, point in enumerate(points): + initialEstimate.insert(L(j), point + gtsam.Point3(-0.25, 0.20, 0.15)) + else: + # Update iSAM with the new factors + isam.update(graph, initialEstimate) + # Each call to iSAM2 update(*) performs one iteration of the iterative nonlinear solver. + # If accuracy is desired at the expense of time, update(*) can be called additional times + # to perform multiple optimizer iterations every step. + isam.update() + currentEstimate = isam.calculate_estimate() + print("****************************************************") + print("Frame", i, ":") + for j in range(i + 1): + print(X(j), ":", currentEstimate.atPose3(X(j))) + + for j in range(len(points)): + print(L(j), ":", currentEstimate.atPoint3(L(j))) + + visual_ISAM2_plot(poses, points, currentEstimate) + + # Clear the factor graph and values for the next iteration + graph.resize(0) + initialEstimate.clear() + + plt.ioff() + plt.show() + +if __name__ == '__main__': + visual_ISAM2_example() diff --git a/python/gtsam_examples/__init__.py b/python/gtsam_examples/__init__.py new file mode 100644 index 000000000..f9d032d54 --- /dev/null +++ b/python/gtsam_examples/__init__.py @@ -0,0 +1,2 @@ +from . import SFMdata +from . import VisualISAM2Example diff --git a/python/gtsam_tests/__init__.py b/python/gtsam_tests/__init__.py new file mode 100644 index 000000000..8b1378917 --- /dev/null +++ b/python/gtsam_tests/__init__.py @@ -0,0 +1 @@ + diff --git a/python/gtsam_tests/testPoint2.py b/python/gtsam_tests/testPoint2.py new file mode 100644 index 000000000..80a6f6cbf --- /dev/null +++ b/python/gtsam_tests/testPoint2.py @@ -0,0 +1,13 @@ +import unittest +from gtsam import * + +#https://docs.python.org/2/library/unittest.html +class TestPoint2(unittest.TestCase): + def setUp(self): + self.point = Point2() + + def test_constructor(self): + pass + +if __name__ == '__main__': + unittest.main() diff --git a/python/gtsam_utils/__init__.py b/python/gtsam_utils/__init__.py new file mode 100644 index 000000000..56c55aa94 --- /dev/null +++ b/python/gtsam_utils/__init__.py @@ -0,0 +1 @@ +from .plot import * diff --git a/python/gtsam_utils/_plot.py b/python/gtsam_utils/_plot.py new file mode 100644 index 000000000..f1603bbf5 --- /dev/null +++ b/python/gtsam_utils/_plot.py @@ -0,0 +1,59 @@ +import numpy as _np +import matplotlib.pyplot as _plt +from mpl_toolkits.mplot3d import Axes3D as _Axes3D + +def plotPoint3(fignum, point, linespec): + fig = _plt.figure(fignum) + ax = fig.gca(projection='3d') + ax.plot([point.x()],[point.y()],[point.z()], linespec) + + +def plot3DPoints(fignum, values, linespec, marginals=None): + # PLOT3DPOINTS Plots the Point3's in a values, with optional covariances + # Finds all the Point3 objects in the given Values object and plots them. + # If a Marginals object is given, this function will also plot marginal + # covariance ellipses for each point. + + keys = values.keys() + + # Plot points and covariance matrices + for key in keys: + try: + p = values.point3_at(key); + # if haveMarginals + # P = marginals.marginalCovariance(key); + # gtsam.plotPoint3(p, linespec, P); + # else + plotPoint3(fignum, p, linespec); + except RuntimeError: + continue + #I guess it's not a Point3 + +def plotPose3(fignum, pose, axisLength=0.1): + # get figure object + fig = _plt.figure(fignum) + ax = fig.gca(projection='3d') + + # get rotation and translation (center) + gRp = pose.rotation().matrix() # rotation from pose to global + C = pose.translation().vector() + + # draw the camera axes + xAxis = C+gRp[:,0]*axisLength + L = _np.append(C[_np.newaxis], xAxis[_np.newaxis], axis=0) + ax.plot(L[:,0],L[:,1],L[:,2],'r-') + + yAxis = C+gRp[:,1]*axisLength + L = _np.append(C[_np.newaxis], yAxis[_np.newaxis], axis=0) + ax.plot(L[:,0],L[:,1],L[:,2],'g-') + + zAxis = C+gRp[:,2]*axisLength + L = _np.append(C[_np.newaxis], zAxis[_np.newaxis], axis=0) + ax.plot(L[:,0],L[:,1],L[:,2],'b-') + + # # plot the covariance + # if (nargin>2) && (~isempty(P)) + # pPp = P(4:6,4:6); % covariance matrix in pose coordinate frame + # gPp = gRp*pPp*gRp'; % convert the covariance matrix to global coordinate frame + # gtsam.covarianceEllipse3D(C,gPp); + # end \ No newline at end of file diff --git a/python/gtsam_utils/plot.py b/python/gtsam_utils/plot.py new file mode 100644 index 000000000..84a388bbb --- /dev/null +++ b/python/gtsam_utils/plot.py @@ -0,0 +1,59 @@ +import numpy as np +import matplotlib.pyplot as plt +from mpl_toolkits.mplot3d import Axes3D as _Axes3D + +def plotPoint3(fignum, point, linespec): + fig = plt.figure(fignum) + ax = fig.gca(projection='3d') + ax.plot([point.x()], [point.y()], [point.z()], linespec) + + +def plot3DPoints(fignum, values, linespec, marginals=None): + # PLOT3DPOINTS Plots the Point3's in a values, with optional covariances + # Finds all the Point3 objects in the given Values object and plots them. + # If a Marginals object is given, this function will also plot marginal + # covariance ellipses for each point. + + keys = values.keys() + + # Plot points and covariance matrices + for key in keys: + try: + p = values.atPoint3(key); + # if haveMarginals + # P = marginals.marginalCovariance(key); + # gtsam.plotPoint3(p, linespec, P); + # else + plotPoint3(fignum, p, linespec); + except RuntimeError: + continue + # I guess it's not a Point3 + +def plotPose3(fignum, pose, axisLength=0.1): + # get figure object + fig = plt.figure(fignum) + ax = fig.gca(projection='3d') + + # get rotation and translation (center) + gRp = pose.rotation().matrix() # rotation from pose to global + C = pose.translation().vector() + + # draw the camera axes + xAxis = C + gRp[:, 0] * axisLength + L = np.append(C[np.newaxis], xAxis[np.newaxis], axis=0) + ax.plot(L[:, 0], L[:, 1], L[:, 2], 'r-') + + yAxis = C + gRp[:, 1] * axisLength + L = np.append(C[np.newaxis], yAxis[np.newaxis], axis=0) + ax.plot(L[:, 0], L[:, 1], L[:, 2], 'g-') + + zAxis = C + gRp[:, 2] * axisLength + L = np.append(C[np.newaxis], zAxis[np.newaxis], axis=0) + ax.plot(L[:, 0], L[:, 1], L[:, 2], 'b-') + + # # plot the covariance + # if (nargin>2) && (~isempty(P)) + # pPp = P(4:6,4:6); % covariance matrix in pose coordinate frame + # gPp = gRp*pPp*gRp'; % convert the covariance matrix to global coordinate frame + # gtsam.covarianceEllipse3D(C,gPp); + # end diff --git a/python/handwritten/CMakeLists.txt b/python/handwritten/CMakeLists.txt new file mode 100644 index 000000000..9d4f9151a --- /dev/null +++ b/python/handwritten/CMakeLists.txt @@ -0,0 +1,30 @@ +# get subdirectories list +subdirlist(SUBDIRS ${CMAKE_CURRENT_SOURCE_DIR}) + +# get the sources needed to compile gtsam python module +set(gtsam_python_srcs "") +foreach(subdir ${SUBDIRS}) + file(GLOB ${subdir}_src "${subdir}/*.cpp") + list(APPEND gtsam_python_srcs ${${subdir}_src}) +endforeach() + +# Create the library +add_library(gtsam_python SHARED exportgtsam.cpp ${gtsam_python_srcs}) +set_target_properties(gtsam_python PROPERTIES + OUTPUT_NAME gtsam_python + SKIP_BUILD_RPATH TRUE + CLEAN_DIRECT_OUTPUT 1 +) +target_link_libraries(gtsam_python + ${Boost_PYTHON${BOOST_PYTHON_VERSION_SUFFIX_UPPERCASE}_LIBRARY} + ${PYTHON_LIBRARY} gtsam) + +# Cause the library to be output in the correct directory. +# TODO: Change below to work on different systems (currently works only with Linux) +add_custom_command( + OUTPUT ${CMAKE_BINARY_DIR}/python/gtsam/_libgtsam_python.so + DEPENDS gtsam_python + COMMAND ${CMAKE_COMMAND} -E copy $ ${CMAKE_BINARY_DIR}/python/gtsam/_libgtsam_python.so + COMMENT "Copying extension module to python/gtsam/_libgtsam_python.so" +) +add_custom_target(copy_gtsam_python_module ALL DEPENDS ${CMAKE_BINARY_DIR}/python/gtsam/_libgtsam_python.so) \ No newline at end of file diff --git a/python/handwritten/base/FastVector.cpp b/python/handwritten/base/FastVector.cpp new file mode 100644 index 000000000..1c3582813 --- /dev/null +++ b/python/handwritten/base/FastVector.cpp @@ -0,0 +1,37 @@ + /* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/** + * @brief wraps FastVector instances to python + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include +#include + +#define NO_IMPORT_ARRAY +#include + +#include "gtsam/base/FastVector.h" +#include "gtsam/base/types.h" // for Key definition + +using namespace boost::python; +using namespace gtsam; + +void exportFastVectors(){ + + typedef FastVector KeyVector; + + class_("KeyVector") + .def(vector_indexing_suite()) + ; + +} \ No newline at end of file diff --git a/python/handwritten/exportgtsam.cpp b/python/handwritten/exportgtsam.cpp new file mode 100644 index 000000000..da8382d71 --- /dev/null +++ b/python/handwritten/exportgtsam.cpp @@ -0,0 +1,97 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/** + * @brief exports the python module + * @author Andrew Melim + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include +#include + +#include + +// Base +void exportFastVectors(); + +// Geometry +void exportPoint2(); +void exportPoint3(); +void exportRot2(); +void exportRot3(); +void exportPose2(); +void exportPose3(); +void exportPinholeBaseK(); +void exportPinholeCamera(); +void exportCal3_S2(); + +// Inference +void exportSymbol(); + +// Linear +void exportNoiseModels(); + +// Nonlinear +void exportValues(); +void exportNonlinearFactor(); +void exportNonlinearFactorGraph(); +void exportLevenbergMarquardtOptimizer(); +void exportISAM2(); + +// Slam +void exportPriorFactors(); +void exportBetweenFactors(); +void exportGenericProjectionFactor(); + +// Utils (or Python wrapper specific functions) +void registerNumpyEigenConversions(); + +//-----------------------------------// + +BOOST_PYTHON_MODULE(_libgtsam_python){ + + // NOTE: We need to call import_array1() instead of import_array() to support both python 2 + // and 3. The reason is that BOOST_PYTHON_MODULE puts all its contents in a function + // returning void, and import_array() is a macro that when expanded for python 3, adds + // a 'return __null' statement to that function. For more info check files: + // boost/python/module_init.hpp and numpy/__multiarray_api.h (bottom of the file). + // Should be the first thing to be done + import_array1(); + + registerNumpyEigenConversions(); + + exportFastVectors(); + + exportPoint2(); + exportPoint3(); + exportRot2(); + exportRot3(); + exportPose2(); + exportPose3(); + exportPinholeBaseK(); + exportPinholeCamera(); + exportCal3_S2(); + + exportSymbol(); + + exportNoiseModels(); + + exportValues(); + exportNonlinearFactor(); + exportNonlinearFactorGraph(); + exportLevenbergMarquardtOptimizer(); + exportISAM2(); + + exportPriorFactors(); + exportBetweenFactors(); + exportGenericProjectionFactor(); +} \ No newline at end of file diff --git a/python/handwritten/geometry/Cal3_S2.cpp b/python/handwritten/geometry/Cal3_S2.cpp new file mode 100644 index 000000000..339cd6a3c --- /dev/null +++ b/python/handwritten/geometry/Cal3_S2.cpp @@ -0,0 +1,62 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/** + * @brief wraps Cal3_S2 class to python + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include + +#define NO_IMPORT_ARRAY +#include + +#include "gtsam/geometry/Cal3_S2.h" + +using namespace boost::python; +using namespace gtsam; + +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, Cal3_S2::print, 0, 1) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, Cal3_S2::equals, 1, 2) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(uncalibrate_overloads, Cal3_S2::uncalibrate, 1, 3) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(calibrate_overloads, Cal3_S2::calibrate, 1, 3) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(between_overloads, Cal3_S2::between, 1, 3) + +// Function pointers to desambiguate Cal3_S2::calibrate calls +Point2 (Cal3_S2::*calibrate1)(const Point2 &, OptionalJacobian< 2, 5 > Dcal, OptionalJacobian< 2, 2 > Dp) const = &Cal3_S2::calibrate; +Vector3 (Cal3_S2::*calibrate2)(const Vector3 &) const = &Cal3_S2::calibrate; + +void exportCal3_S2(){ + +class_("Cal3_S2", init<>()) + .def(init()) + .def(init()) + .def(init()) + .def(init()) + .def("print", &Cal3_S2::print, print_overloads(args("s"))) + .def("equals", &Cal3_S2::equals, equals_overloads(args("q","tol"))) + .def("fx",&Cal3_S2::fx) + .def("fy",&Cal3_S2::fy) + .def("skew",&Cal3_S2::skew) + .def("px",&Cal3_S2::px) + .def("py",&Cal3_S2::py) + .def("principal_point",&Cal3_S2::principalPoint) + .def("vector",&Cal3_S2::vector) + .def("k",&Cal3_S2::K) + .def("matrix",&Cal3_S2::matrix) + .def("matrix_inverse",&Cal3_S2::matrix_inverse) + .def("uncalibrate",&Cal3_S2::uncalibrate, uncalibrate_overloads()) + .def("calibrate",calibrate1, calibrate_overloads()) + .def("calibrate",calibrate2) + .def("between",&Cal3_S2::between, between_overloads()) +; + +} \ No newline at end of file diff --git a/python/handwritten/geometry/PinholeBaseK.cpp b/python/handwritten/geometry/PinholeBaseK.cpp new file mode 100644 index 000000000..da98783e2 --- /dev/null +++ b/python/handwritten/geometry/PinholeBaseK.cpp @@ -0,0 +1,66 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/** + * @brief wraps PinholeCamera classes to python + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include + +#define NO_IMPORT_ARRAY +#include + +#include "gtsam/geometry/PinholeCamera.h" +#include "gtsam/geometry/Cal3_S2.h" + + +using namespace boost::python; +using namespace gtsam; + +typedef PinholeBaseK PinholeBaseKCal3_S2; + +// Wrapper on PinholeBaseK because it has pure virtual method calibration() +struct PinholeBaseKCal3_S2Callback : PinholeBaseKCal3_S2, wrapper +{ + const Cal3_S2 & calibration () const { + return this->get_override("calibration")(); + } +}; + +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(project_overloads, PinholeBaseKCal3_S2::project, 2, 4) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(range_overloads, PinholeBaseKCal3_S2::range, 1, 3) + +// Function pointers to desambiguate project() calls +Point2 (PinholeBaseKCal3_S2::*project1) (const Point3 &pw) const = &PinholeBaseKCal3_S2::project; +Point2 (PinholeBaseKCal3_S2::*project2) (const Point3 &pw, OptionalJacobian< 2, 6 > Dpose, OptionalJacobian< 2, 3 > Dpoint, OptionalJacobian< 2, FixedDimension::value > Dcal) const = &PinholeBaseKCal3_S2::project; +Point2 (PinholeBaseKCal3_S2::*project3) (const Unit3 &pw, OptionalJacobian< 2, 6 > Dpose, OptionalJacobian< 2, 2 > Dpoint, OptionalJacobian< 2, FixedDimension::value > Dcal) const = &PinholeBaseKCal3_S2::project; + +// function pointers to desambiguate range() calls +double (PinholeBaseKCal3_S2::*range1) (const Point3 &point, OptionalJacobian< 1, 6 > Dcamera, OptionalJacobian< 1, 3 > Dpoint) const = &PinholeBaseKCal3_S2::range; +double (PinholeBaseKCal3_S2::*range2) (const Pose3 &pose, OptionalJacobian< 1, 6 > Dcamera, OptionalJacobian< 1, 6 > Dpose) const = &PinholeBaseKCal3_S2::range; +double (PinholeBaseKCal3_S2::*range3) (const CalibratedCamera &camera, OptionalJacobian< 1, 6 > Dcamera, OptionalJacobian< 1, 6 > Dother) const = &PinholeBaseKCal3_S2::range; + +void exportPinholeBaseK(){ + + class_("PinholeBaseKCal3_S2", no_init) + .def("calibration", pure_virtual(&PinholeBaseKCal3_S2::calibration), return_value_policy()) + .def("project", project1) + .def("project", project2, project_overloads()) + .def("project", project3, project_overloads()) + .def("backproject", &PinholeBaseKCal3_S2::backproject) + .def("backproject_point_at_infinity", &PinholeBaseKCal3_S2::backprojectPointAtInfinity) + .def("range", range1, range_overloads()) + .def("range", range2, range_overloads()) + .def("range", range3, range_overloads()) + ; + +} \ No newline at end of file diff --git a/python/handwritten/geometry/PinholeCamera.cpp b/python/handwritten/geometry/PinholeCamera.cpp new file mode 100644 index 000000000..21ffcdeb7 --- /dev/null +++ b/python/handwritten/geometry/PinholeCamera.cpp @@ -0,0 +1,51 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/** + * @brief wraps PinholeCamera classes to python + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include + +#define NO_IMPORT_ARRAY +#include + +#include "gtsam/geometry/PinholeCamera.h" +#include "gtsam/geometry/Cal3_S2.h" + +using namespace boost::python; +using namespace gtsam; + +typedef PinholeBaseK PinholeBaseKCal3_S2; +typedef PinholeCamera PinholeCameraCal3_S2; + +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, PinholeCameraCal3_S2::print, 0, 1) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, PinholeCameraCal3_S2::equals, 1, 2) +BOOST_PYTHON_FUNCTION_OVERLOADS(Lookat_overloads, PinholeCameraCal3_S2::Lookat, 3, 4) + +void exportPinholeCamera(){ + +class_ >("PinholeCameraCal3_S2", init<>()) + .def(init()) + .def(init()) + .def(init()) + .def(init()) + .def("print", &PinholeCameraCal3_S2::print, print_overloads(args("s"))) + .def("equals", &PinholeCameraCal3_S2::equals, equals_overloads(args("q","tol"))) + .def("pose", &PinholeCameraCal3_S2::pose, return_value_policy()) + // We don't need to define calibration() here because it's already defined as virtual in the base class PinholeBaseKCal3_S2 + // .def("calibration", &PinholeCameraCal3_S2::calibration, return_value_policy()) + .def("Lookat", &PinholeCameraCal3_S2::Lookat, Lookat_overloads()) + .staticmethod("Lookat") +; + +} \ No newline at end of file diff --git a/python/handwritten/geometry/Point2.cpp b/python/handwritten/geometry/Point2.cpp new file mode 100644 index 000000000..7af3f8cf6 --- /dev/null +++ b/python/handwritten/geometry/Point2.cpp @@ -0,0 +1,58 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/** + * @brief wraps Point2 class to python + * @author Andrew Melim + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include + +#define NO_IMPORT_ARRAY +#include + +#include "gtsam/geometry/Point2.h" + +using namespace boost::python; +using namespace gtsam; + +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, Point2::print, 0, 1) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, Point2::equals, 1, 2) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(compose_overloads, Point2::compose, 1, 3) + +void exportPoint2(){ + + class_("Point2", init<>()) + .def(init()) + .def(init()) + .def("identity", &Point2::identity) + .def("dist", &Point2::dist) + .def("distance", &Point2::distance) + .def("equals", &Point2::equals, equals_overloads(args("q","tol"))) + .def("norm", &Point2::norm) + .def("print", &Point2::print, print_overloads(args("s"))) + .def("unit", &Point2::unit) + .def("vector", &Point2::vector) + .def("x", &Point2::x) + .def("y", &Point2::y) + .def(self * other()) // __mult__ + .def(other() * self) // __mult__ + .def(self + self) + .def(-self) + .def(self - self) + .def(self / other()) + .def(self_ns::str(self)) + .def(repr(self)) + .def(self == self) + ; + +} \ No newline at end of file diff --git a/python/handwritten/geometry/Point3.cpp b/python/handwritten/geometry/Point3.cpp new file mode 100644 index 000000000..664b4ffda --- /dev/null +++ b/python/handwritten/geometry/Point3.cpp @@ -0,0 +1,64 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/** + * @brief wraps Point3 class to python + * @author Andrew Melim + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include + +#define NO_IMPORT_ARRAY +#include + +#include "gtsam/geometry/Point3.h" + +using namespace boost::python; +using namespace gtsam; + +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, Point3::print, 0, 1) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, Point3::equals, 1, 2) + +void exportPoint3(){ + +class_("Point3") + .def(init<>()) + .def(init()) + .def(init()) + .def("identity", &Point3::identity) + .staticmethod("identity") + .def("add", &Point3::add) + .def("cross", &Point3::cross) + .def("dist", &Point3::dist) + .def("distance", &Point3::distance) + .def("dot", &Point3::dot) + .def("equals", &Point3::equals, equals_overloads(args("q","tol"))) + .def("norm", &Point3::norm) + .def("normalize", &Point3::normalize) + .def("print", &Point3::print, print_overloads(args("s"))) + .def("sub", &Point3::sub) + .def("vector", &Point3::vector) + .def("x", &Point3::x) + .def("y", &Point3::y) + .def("z", &Point3::z) + .def(self * other()) + .def(other() * self) + .def(self + self) + .def(-self) + .def(self - self) + .def(self / other()) + .def(self_ns::str(self)) + .def(repr(self)) + .def(self == self) +; + +} \ No newline at end of file diff --git a/python/handwritten/geometry/Pose2.cpp b/python/handwritten/geometry/Pose2.cpp new file mode 100644 index 000000000..4f402df7e --- /dev/null +++ b/python/handwritten/geometry/Pose2.cpp @@ -0,0 +1,93 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/** + * @brief wraps Pose2 class to python + * @author Andrew Melim + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include + +#define NO_IMPORT_ARRAY +#include + +#include "gtsam/geometry/Pose2.h" + +using namespace boost::python; +using namespace gtsam; + +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, Pose2::print, 0, 1) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, Pose2::equals, 1, 2) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(compose_overloads, Pose2::compose, 1, 3) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(between_overloads, Pose2::between, 1, 3) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(transform_to_overloads, Pose2::transform_to, 1, 3) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(transform_from_overloads, Pose2::transform_from, 1, 3) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(bearing_overloads, Pose2::bearing, 1, 3) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(range_overloads, Pose2::range, 1, 3) + +// Manually wrap + +void exportPose2(){ + + // double (Pose2::*range1)(const Pose2&, boost::optional, boost::optional) const + // = &Pose2::range; + // double (Pose2::*range2)(const Point2&, boost::optional, boost::optional) const + // = &Pose2::range; + + // Rot2 (Pose2::*bearing1)(const Pose2&, boost::optional, boost::optional) const + // = &Pose2::bearing; + // Rot2 (Pose2::*bearing2)(const Point2&, boost::optional, boost::optional) const + // = &Pose2::bearing; + + class_("Pose2", init<>()) + .def(init()) + .def(init()) + .def(init()) + .def("print", &Pose2::print, print_overloads(args("s"))) + + .def("equals", &Pose2::equals, equals_overloads(args("pose","tol"))) + // .def("inverse", &Pose2::inverse) + // .def("compose", &Pose2::compose, compose_overloads(args("p2", "H1", "H2"))) + // .def("between", &Pose2::between, between_overloads(args("p2", "H1", "H2"))) + // .def("dim", &Pose2::dim) + // .def("retract", &Pose2::retract) + + .def("transform_to", &Pose2::transform_to, + transform_to_overloads(args("point", "H1", "H2"))) + .def("transform_from", &Pose2::transform_from, + transform_to_overloads(args("point", "H1", "H2"))) + + .def("x", &Pose2::x) + .def("y", &Pose2::y) + .def("theta", &Pose2::theta) + // See documentation on call policy for more information + // https://wiki.python.org/moin/boost.python/CallPolicy + .def("t", &Pose2::t, return_value_policy()) + .def("r", &Pose2::r, return_value_policy()) + .def("translation", &Pose2::translation, return_value_policy()) + .def("rotation", &Pose2::rotation, return_value_policy()) + + // .def("bearing", bearing1, bearing_overloads()) + // .def("bearing", bearing2, bearing_overloads()) + + // Function overload example + // .def("range", range1, range_overloads()) + // .def("range", range2, range_overloads()) + + + .def("Expmap", &Pose2::Expmap) + .staticmethod("Expmap") + + .def(self * self) // __mult__ + ; + +} \ No newline at end of file diff --git a/python/handwritten/geometry/Pose3.cpp b/python/handwritten/geometry/Pose3.cpp new file mode 100644 index 000000000..dfdfe8ad1 --- /dev/null +++ b/python/handwritten/geometry/Pose3.cpp @@ -0,0 +1,92 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/** + * @brief wraps Pose3 class to python + * @author Andrew Melim + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include + +#define NO_IMPORT_ARRAY +#include + +#include "gtsam/geometry/Pose3.h" +#include "gtsam/geometry/Pose2.h" +#include "gtsam/geometry/Point3.h" +#include "gtsam/geometry/Rot3.h" + +using namespace boost::python; +using namespace gtsam; + +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, Pose3::print, 0, 1) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, Pose3::equals, 1, 2) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(transform_to_overloads, Pose3::transform_to, 1, 3) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(transform_from_overloads, Pose3::transform_from, 1, 3) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(translation_overloads, Pose3::translation, 0, 1) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(compose_overloads, Pose3::compose, 2, 3) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(between_overloads, Pose3::between, 2, 3) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(bearing_overloads, Pose3::bearing, 1, 3) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(range_overloads, Pose3::range, 1, 3) + +void exportPose3(){ + + // function pointers to desambiguate transform_to() calls + Point3 (Pose3::*transform_to1)(const Point3&, OptionalJacobian< 3, 6 >, OptionalJacobian< 3, 3 > ) const = &Pose3::transform_to; + Pose3 (Pose3::*transform_to2)(const Pose3&) const = &Pose3::transform_to; + // function pointers to desambiguate compose() calls + Pose3 (Pose3::*compose1)(const Pose3 &g) const = &Pose3::compose; + Pose3 (Pose3::*compose2)(const Pose3 &g, typename Pose3::ChartJacobian, typename Pose3::ChartJacobian) const = &Pose3::compose; + // function pointers to desambiguate between() calls + Pose3 (Pose3::*between1)(const Pose3 &g) const = &Pose3::between; + Pose3 (Pose3::*between2)(const Pose3 &g, typename Pose3::ChartJacobian, typename Pose3::ChartJacobian) const = &Pose3::between; + // function pointers to desambiguate range() calls + double (Pose3::*range1)(const Point3 &, OptionalJacobian<1,6>, OptionalJacobian<1,3>) const = &Pose3::range; + double (Pose3::*range2)(const Pose3 &, OptionalJacobian<1,6>, OptionalJacobian<1,6>) const = &Pose3::range; + + class_("Pose3") + .def(init<>()) + .def(init()) + .def(init()) + .def(init()) + .def(init()) + .def(init()) + .def("print", &Pose3::print, print_overloads(args("s"))) + .def("equals", &Pose3::equals, equals_overloads(args("pose","tol"))) + .def("identity", &Pose3::identity) + .staticmethod("identity") + .def("bearing", &Pose3::bearing) + .def("matrix", &Pose3::matrix) + .def("transform_from", &Pose3::transform_from, + transform_from_overloads(args("point", "H1", "H2"))) + .def("transform_to", transform_to1, + transform_to_overloads(args("point", "H1", "H2"))) + .def("transform_to", transform_to2) + .def("x", &Pose3::x) + .def("y", &Pose3::y) + .def("z", &Pose3::z) + .def("translation", &Pose3::translation, + translation_overloads()[return_value_policy()]) + .def("rotation", &Pose3::rotation, return_value_policy()) + .def(self * self) // __mult__ + .def(self * other()) // __mult__ + .def(self_ns::str(self)) // __str__ + .def(repr(self)) // __repr__ + .def("compose", compose1) + .def("compose", compose2, compose_overloads()) + .def("between", between1) + .def("between", between2, between_overloads()) + .def("range", range1, range_overloads()) + .def("range", range2, range_overloads()) + .def("bearing", &Pose3::bearing, bearing_overloads()) + ; +} \ No newline at end of file diff --git a/python/handwritten/geometry/Rot2.cpp b/python/handwritten/geometry/Rot2.cpp new file mode 100644 index 000000000..59b4ce4e8 --- /dev/null +++ b/python/handwritten/geometry/Rot2.cpp @@ -0,0 +1,65 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/** + * @brief wraps Rot2 class to python + * @author Andrew Melim + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include + +#define NO_IMPORT_ARRAY +#include + +#include "gtsam/geometry/Rot2.h" + +using namespace boost::python; +using namespace gtsam; + +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, Rot2::print, 0, 1) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, Rot2::equals, 1, 2) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(compose_overloads, Rot2::compose, 1, 3) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(relativeBearing_overloads, Rot2::relativeBearing, 1, 3) + +void exportRot2(){ + + class_("Rot2", init<>()) + .def(init()) + .def("Expmap", &Rot2::Expmap) + .staticmethod("Expmap") + .def("Logmap", &Rot2::Logmap) + .staticmethod("Logmap") + .def("atan2", &Rot2::atan2) + .staticmethod("atan2") + .def("fromAngle", &Rot2::fromAngle) + .staticmethod("fromAngle") + .def("fromCosSin", &Rot2::fromCosSin) + .staticmethod("fromCosSin") + .def("fromDegrees", &Rot2::fromDegrees) + .staticmethod("fromDegrees") + .def("identity", &Rot2::identity) + .staticmethod("identity") + .def("relativeBearing", &Rot2::relativeBearing) + .staticmethod("relativeBearing") + .def("c", &Rot2::c) + .def("degrees", &Rot2::degrees) + .def("equals", &Rot2::equals, equals_overloads(args("q","tol"))) + .def("matrix", &Rot2::matrix) + .def("print", &Rot2::print, print_overloads(args("s"))) + .def("rotate", &Rot2::rotate) + .def("s", &Rot2::s) + .def("theta", &Rot2::theta) + .def("unrotate", &Rot2::unrotate) + .def(self * self) // __mult__ + ; + +} \ No newline at end of file diff --git a/python/handwritten/geometry/Rot3.cpp b/python/handwritten/geometry/Rot3.cpp new file mode 100644 index 000000000..68643fe2c --- /dev/null +++ b/python/handwritten/geometry/Rot3.cpp @@ -0,0 +1,111 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/** + * @brief wraps Rot3 class to python + * @author Andrew Melim + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include + +#define NO_IMPORT_ARRAY +#include + +#include "gtsam/geometry/Rot3.h" + +using namespace boost::python; +using namespace gtsam; + +static Rot3 Quaternion_0(const Vector4& q) +{ + return Rot3::Quaternion(q[0],q[1],q[2],q[3]); +} + +static Rot3 Quaternion_1(double w, double x, double y, double z) +{ + return Rot3::Quaternion(w,x,y,z); +} + +// Prototypes used to perform overloading +// See: http://www.boost.org/doc/libs/1_59_0/libs/python/doc/tutorial/doc/html/python/functions.html +gtsam::Rot3 (*AxisAngle_0)(const Vector3&, double) = &Rot3::AxisAngle; +gtsam::Rot3 (*AxisAngle_1)(const gtsam::Point3&, double) = &Rot3::AxisAngle; +gtsam::Rot3 (*Rodrigues_0)(const Vector3&) = &Rot3::Rodrigues; +gtsam::Rot3 (*Rodrigues_1)(double, double, double) = &Rot3::Rodrigues; +gtsam::Rot3 (*RzRyRx_0)(double, double, double) = &Rot3::RzRyRx; +gtsam::Rot3 (*RzRyRx_1)(const Vector&) = &Rot3::RzRyRx; +Vector (Rot3::*quaternion_0)() const = &Rot3::quaternion; + +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, Rot3::print, 0, 1) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, Rot3::equals, 1, 2) + +void exportRot3(){ + + class_("Rot3") + .def(init()) + .def(init()) + .def(init()) + .def(init()) + .def("Quaternion", Quaternion_0, arg("q"), "Creates a Rot3 from an array [w,x,y,z] representing a quaternion") + .def("Quaternion", Quaternion_1, (arg("w"),arg("x"),arg("y"),arg("z")) ) + .staticmethod("Quaternion") + .def("AxisAngle", AxisAngle_0) + .def("AxisAngle", AxisAngle_1) + .staticmethod("AxisAngle") + .def("Expmap", &Rot3::Expmap) + .staticmethod("Expmap") + .def("ExpmapDerivative", &Rot3::ExpmapDerivative) + .staticmethod("ExpmapDerivative") + .def("Logmap", &Rot3::Logmap) + .staticmethod("Logmap") + .def("LogmapDerivative", &Rot3::LogmapDerivative) + .staticmethod("LogmapDerivative") + .def("Rodrigues", Rodrigues_0) + .def("Rodrigues", Rodrigues_1) + .staticmethod("Rodrigues") + .def("Rx", &Rot3::Rx) + .staticmethod("Rx") + .def("Ry", &Rot3::Ry) + .staticmethod("Ry") + .def("Rz", &Rot3::Rz) + .staticmethod("Rz") + .def("RzRyRx", RzRyRx_0, (arg("x"),arg("y"),arg("z")), "Rotations around Z, Y, then X axes as in http://en.wikipedia.org/wiki/Rotation_matrix, counterclockwise when looking from unchanging axis" ) + .def("RzRyRx", RzRyRx_1, arg("xyz"), "Rotations around Z, Y, then X axes as in http://en.wikipedia.org/wiki/Rotation_matrix, counterclockwise when looking from unchanging axis" ) + .staticmethod("RzRyRx") + .def("identity", &Rot3::identity) + .staticmethod("identity") + .def("AdjointMap", &Rot3::AdjointMap) + .def("column", &Rot3::column) + .def("conjugate", &Rot3::conjugate) + .def("equals", &Rot3::equals, equals_overloads(args("q","tol"))) +#ifndef GTSAM_USE_QUATERNIONS + .def("localCayley", &Rot3::localCayley) + .def("retractCayley", &Rot3::retractCayley) +#endif + .def("matrix", &Rot3::matrix) + .def("print", &Rot3::print, print_overloads(args("s"))) + .def("r1", &Rot3::r1) + .def("r2", &Rot3::r2) + .def("r3", &Rot3::r3) + .def("rpy", &Rot3::rpy) + .def("slerp", &Rot3::slerp) + .def("transpose", &Rot3::transpose) + .def("xyz", &Rot3::xyz) + .def("quaternion", quaternion_0) + .def(self * self) + .def(self * other()) + .def(self * other()) + .def(self_ns::str(self)) // __str__ + .def(repr(self)) // __repr__ + ; + +} diff --git a/python/handwritten/inference/Symbol.cpp b/python/handwritten/inference/Symbol.cpp new file mode 100644 index 000000000..9fc5b74e7 --- /dev/null +++ b/python/handwritten/inference/Symbol.cpp @@ -0,0 +1,83 @@ + /* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/** + * @brief wraps Symbol class to python + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include +#include + +#define NO_IMPORT_ARRAY +#include + +#include // for stringstream + +#include "gtsam/inference/Symbol.h" + +using namespace boost::python; +using namespace gtsam; + +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, Symbol::print, 0, 1) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, Symbol::equals, 1, 2) + +// Helper function to allow building a symbol from a python string and a index. +static boost::shared_ptr makeSymbol(const std::string &str, size_t j) +{ + if(str.size() > 1) + throw std::runtime_error("string argument must have one character only"); + + return boost::make_shared(str.at(0),j); +} + +// Helper function to print the symbol as "char-and-index" in python +std::string selfToString(const Symbol & self) +{ + return (std::string)self; +} + +// Helper function to convert a Symbol to int using int() cast in python +size_t selfToKey(const Symbol & self) +{ + return self.key(); +} + +// Helper function to recover symbol's unsigned char as string +std::string chrFromSelf(const Symbol & self) +{ + std::stringstream ss; + ss << self.chr(); + return ss.str(); +} + +void exportSymbol(){ + +class_ >("Symbol") + .def(init<>()) + .def(init()) + .def("__init__", make_constructor(makeSymbol)) + .def(init()) + .def("print", &Symbol::print, print_overloads(args("s"))) + .def("equals", &Symbol::equals, equals_overloads(args("q","tol"))) + .def("key", &Symbol::key) + .def("index", &Symbol::index) + .def(self < self) + .def(self == self) + .def(self == other()) + .def(self != self) + .def(self != other()) + .def("__repr__", &selfToString) + .def("__int__", &selfToKey) + .def("chr", &chrFromSelf) +; + +} \ No newline at end of file diff --git a/python/handwritten/linear/NoiseModel.cpp b/python/handwritten/linear/NoiseModel.cpp new file mode 100644 index 000000000..3453184bd --- /dev/null +++ b/python/handwritten/linear/NoiseModel.cpp @@ -0,0 +1,137 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/** + * @brief wraps the noise model classes into the noiseModel module + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + + /** TODOs Summary: + * + * TODO(Ellon): Don't know yet it it's worth/needed to add 'Wrap structs' for each of the noise models. + * I think it's only worthy if we want to access virtual the virtual functions from python. + * TODO(Ellon): Wrap non-pure virtual methods of Base on BaseWrap + */ + +#include + +#define NO_IMPORT_ARRAY +#include + +#include "gtsam/linear/NoiseModel.h" + +using namespace boost::python; +using namespace gtsam; +using namespace gtsam::noiseModel; + +// Wrap around pure virtual class Base. +// All pure virtual methods should be wrapped. Non-pure may be wrapped if we want to mimic the +// overloading through inheritance in Python. +// See: http://www.boost.org/doc/libs/1_59_0/libs/python/doc/tutorial/doc/html/python/exposing.html#python.class_virtual_functions +struct BaseCallback : Base, wrapper +{ + void print (const std::string & name="") const { + this->get_override("print")(); + } + bool equals (const Base & expected, double tol=1e-9) const { + return this->get_override("equals")(); + } + Vector whiten (const Vector & v) const { + return this->get_override("whiten")(); + } + Matrix Whiten (const Matrix & v) const { + return this->get_override("Whiten")(); + } + Vector unwhiten (const Vector & v) const { + return this->get_override("unwhiten")(); + } + double distance (const Vector & v) const { + return this->get_override("distance")(); + } + void WhitenSystem (std::vector< Matrix > &A, Vector &b) const { + this->get_override("WhitenSystem")(); + } + void WhitenSystem (Matrix &A, Vector &b) const { + this->get_override("WhitenSystem")(); + } + void WhitenSystem (Matrix &A1, Matrix &A2, Vector &b) const { + this->get_override("WhitenSystem")(); + } + void WhitenSystem (Matrix &A1, Matrix &A2, Matrix &A3, Vector &b) const { + this->get_override("WhitenSystem")(); + } + + // TODO(Ellon): Wrap non-pure virtual methods should go here. + // See: http://www.boost.org/doc/libs/1_59_0/libs/python/doc/tutorial/doc/html/python/exposing.html#python.virtual_functions_with_default_implementations + +}; + +// Overloads for named constructors. Named constructors are static, so we declare them +// using BOOST_PYTHON_FUNCTION_OVERLOADS instead of BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS +// See: http://www.boost.org/doc/libs/1_59_0/libs/python/doc/tutorial/doc/html/python/functions.html#python.default_arguments +BOOST_PYTHON_FUNCTION_OVERLOADS(Gaussian_SqrtInformation_overloads, Gaussian::SqrtInformation, 1, 2) +BOOST_PYTHON_FUNCTION_OVERLOADS(Gaussian_Information_overloads, Gaussian::Information, 1, 2) +BOOST_PYTHON_FUNCTION_OVERLOADS(Gaussian_Covariance_overloads, Gaussian::Covariance, 1, 2) +BOOST_PYTHON_FUNCTION_OVERLOADS(Diagonal_Sigmas_overloads, Diagonal::Sigmas, 1, 2) +BOOST_PYTHON_FUNCTION_OVERLOADS(Diagonal_Variances_overloads, Diagonal::Variances, 1, 2) +BOOST_PYTHON_FUNCTION_OVERLOADS(Diagonal_Precisions_overloads, Diagonal::Precisions, 1, 2) +BOOST_PYTHON_FUNCTION_OVERLOADS(Isotropic_Sigma_overloads, Isotropic::Sigma, 2, 3) +BOOST_PYTHON_FUNCTION_OVERLOADS(Isotropic_Variance_overloads, Isotropic::Variance, 2, 3) +BOOST_PYTHON_FUNCTION_OVERLOADS(Isotropic_Precision_overloads, Isotropic::Precision, 2, 3) + + +void exportNoiseModels(){ + + // Create a scope "noiseModel". See: http://isolation-nation.blogspot.fr/2008/09/packages-in-python-extension-modules.html + std::string noiseModel_name = extract(scope().attr("__name__") + ".noiseModel"); + object noiseModel_module(handle<>(borrowed(PyImport_AddModule(noiseModel_name.c_str())))); + scope().attr("noiseModel") = noiseModel_module; + scope noiseModel_scope = noiseModel_module; + + // Then export our classes in the noiseModel scope + class_("Base") + .def("print", pure_virtual(&Base::print)) + ; + + // NOTE: We should use "Base" in "bases<...>", and not "BaseCallback" (it was not clear at the begining) + class_, bases >("Gaussian", no_init) + .def("SqrtInformation",&Gaussian::SqrtInformation, Gaussian_SqrtInformation_overloads()) + .staticmethod("SqrtInformation") + .def("Information",&Gaussian::Information, Gaussian_Information_overloads()) + .staticmethod("Information") + .def("Covariance",&Gaussian::Covariance, Gaussian_Covariance_overloads()) + .staticmethod("Covariance") + ; + + class_, bases >("Diagonal", no_init) + .def("Sigmas",&Diagonal::Sigmas, Diagonal_Sigmas_overloads()) + .staticmethod("Sigmas") + .def("Variances",&Diagonal::Variances, Diagonal_Variances_overloads()) + .staticmethod("Variances") + .def("Precisions",&Diagonal::Precisions, Diagonal_Precisions_overloads()) + .staticmethod("Precisions") + ; + + class_, bases >("Isotropic", no_init) + .def("Sigma",&Isotropic::Sigma, Isotropic_Sigma_overloads()) + .staticmethod("Sigma") + .def("Variance",&Isotropic::Variance, Isotropic_Variance_overloads()) + .staticmethod("Variance") + .def("Precision",&Isotropic::Precision, Isotropic_Precision_overloads()) + .staticmethod("Precision") + ; + + class_, bases >("Unit", no_init) + .def("Create",&Unit::Create) + .staticmethod("Create") + ; + +} \ No newline at end of file diff --git a/python/handwritten/nonlinear/ISAM2.cpp b/python/handwritten/nonlinear/ISAM2.cpp new file mode 100644 index 000000000..f80c5df99 --- /dev/null +++ b/python/handwritten/nonlinear/ISAM2.cpp @@ -0,0 +1,67 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @brief exports ISAM2 class to python + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include + +#define NO_IMPORT_ARRAY +#include + +#include "gtsam/nonlinear/ISAM2.h" +#include "gtsam/geometry/Pose3.h" + +using namespace boost::python; +using namespace gtsam; + +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(update_overloads, ISAM2::update, 0, 7) + +void exportISAM2(){ + +// TODO(Ellon): Export all properties of ISAM2Params +class_("ISAM2Params") + .add_property("relinearize_skip", &ISAM2Params::getRelinearizeSkip, &ISAM2Params::setRelinearizeSkip) + .add_property("enable_relinearization", &ISAM2Params::isEnableRelinearization, &ISAM2Params::setEnableRelinearization) + .add_property("evaluate_non_linear_error", &ISAM2Params::isEvaluateNonlinearError, &ISAM2Params::setEvaluateNonlinearError) + .add_property("factorization", &ISAM2Params::getFactorization, &ISAM2Params::setFactorization) + .add_property("cache_linearized_factors", &ISAM2Params::isCacheLinearizedFactors, &ISAM2Params::setCacheLinearizedFactors) + .add_property("enable_detailed_results", &ISAM2Params::isEnableDetailedResults, &ISAM2Params::setEnableDetailedResults) + .add_property("enable_partial_linearization_check", &ISAM2Params::isEnablePartialRelinearizationCheck, &ISAM2Params::setEnablePartialRelinearizationCheck) + // TODO(Ellon): Check if it works with FastMap; Implement properly if it doesn't. + .add_property("relinearization_threshold", &ISAM2Params::getRelinearizeThreshold, &ISAM2Params::setRelinearizeThreshold) + // TODO(Ellon): Wrap the following setters/getters: + // void setOptimizationParams (OptimizationParams optimizationParams) + // OptimizationParams getOptimizationParams () const + // void setKeyFormatter (KeyFormatter keyFormatter) + // KeyFormatter getKeyFormatter () const + // GaussianFactorGraph::Eliminate getEliminationFunction () const +; + +// TODO(Ellon): Export useful methods/properties of ISAM2Result +class_("ISAM2Result") +; + +// Function pointers for overloads in ISAM2 +Values (ISAM2::*calculateEstimate_0)() const = &ISAM2::calculateEstimate; + +class_("ISAM2") + .def(init()) + // TODO(Ellon): wrap all optional values of update + .def("update",&ISAM2::update, update_overloads()) + .def("calculate_estimate", calculateEstimate_0) + .def("calculate_pose3_estimate", &ISAM2::calculateEstimate, (arg("self"), arg("key")) ) + .def("value_exists", &ISAM2::valueExists) +; + +} \ No newline at end of file diff --git a/python/handwritten/nonlinear/LevenbergMarquardtOptimizer.cpp b/python/handwritten/nonlinear/LevenbergMarquardtOptimizer.cpp new file mode 100644 index 000000000..7c7cdf3cc --- /dev/null +++ b/python/handwritten/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -0,0 +1,27 @@ +#include + +#define NO_IMPORT_ARRAY +#include + +#include + +using namespace boost::python; +using namespace gtsam; + +void exportLevenbergMarquardtOptimizer(){ + class_("LevenbergMarquardtParams", init<>()) + .def("setDiagonalDamping", &LevenbergMarquardtParams::setDiagonalDamping) + .def("setlambdaFactor", &LevenbergMarquardtParams::setlambdaFactor) + .def("setlambdaInitial", &LevenbergMarquardtParams::setlambdaInitial) + .def("setlambdaLowerBound", &LevenbergMarquardtParams::setlambdaLowerBound) + .def("setlambdaUpperBound", &LevenbergMarquardtParams::setlambdaUpperBound) + .def("setLogFile", &LevenbergMarquardtParams::setLogFile) + .def("setUseFixedLambdaFactor", &LevenbergMarquardtParams::setUseFixedLambdaFactor) + .def("setVerbosityLM", &LevenbergMarquardtParams::setVerbosityLM) + ; + + class_("LevenbergMarquardtOptimizer", + init()) + .def("optimize", &LevenbergMarquardtOptimizer::optimize, return_value_policy()) + ; +} diff --git a/python/handwritten/nonlinear/NonlinearFactor.cpp b/python/handwritten/nonlinear/NonlinearFactor.cpp new file mode 100644 index 000000000..9a130d8e9 --- /dev/null +++ b/python/handwritten/nonlinear/NonlinearFactor.cpp @@ -0,0 +1,49 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/** + * @brief exports virtual class NonlinearFactor to python + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include + +#define NO_IMPORT_ARRAY +#include + +#include "gtsam/nonlinear/NonlinearFactor.h" + +using namespace boost::python; +using namespace gtsam; + +// Wrap around pure virtual class NonlinearFactor. +// All pure virtual methods should be wrapped. Non-pure may be wrapped if we want to mimic the +// overloading through inheritance in Python. +// See: http://www.boost.org/doc/libs/1_59_0/libs/python/doc/tutorial/doc/html/python/exposing.html#python.class_virtual_functions +struct NonlinearFactorCallback : NonlinearFactor, wrapper +{ + double error (const Values & values) const { + return this->get_override("error")(values); + } + size_t dim () const { + return this->get_override("dim")(); + } + boost::shared_ptr linearize(const Values & values) const { + return this->get_override("linearize")(values); + } +}; + +void exportNonlinearFactor(){ + + class_("NonlinearFactor") + ; + +} \ No newline at end of file diff --git a/python/handwritten/nonlinear/NonlinearFactorGraph.cpp b/python/handwritten/nonlinear/NonlinearFactorGraph.cpp new file mode 100644 index 000000000..4ba4ba008 --- /dev/null +++ b/python/handwritten/nonlinear/NonlinearFactorGraph.cpp @@ -0,0 +1,56 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/** + * @brief exports NonlinearFactorGraph class to python + * @author Andrew Melim + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include + +#define NO_IMPORT_ARRAY +#include + +#include "gtsam/nonlinear/NonlinearFactorGraph.h" +#include "gtsam/nonlinear/NonlinearFactor.h" + +using namespace boost::python; +using namespace gtsam; + +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, NonlinearFactorGraph::print, 0, 1); + +boost::shared_ptr getNonlinearFactor( + const NonlinearFactorGraph& graph, size_t idx) { + auto p = boost::dynamic_pointer_cast(graph.at(idx)); + if (!p) throw std::runtime_error("No NonlinearFactor at requested index"); + return p; +}; + +void exportNonlinearFactorGraph(){ + + typedef NonlinearFactorGraph::sharedFactor sharedFactor; + + void (NonlinearFactorGraph::*push_back1)(const sharedFactor&) = &NonlinearFactorGraph::push_back; + void (NonlinearFactorGraph::*add1)(const sharedFactor&) = &NonlinearFactorGraph::add; + + class_("NonlinearFactorGraph", init<>()) + .def("size",&NonlinearFactorGraph::size) + .def("push_back", push_back1) + .def("add", add1) + .def("resize", &NonlinearFactorGraph::resize) + .def("empty", &NonlinearFactorGraph::empty) + .def("print", &NonlinearFactorGraph::print, print_overloads(args("s"))) + ; + + def("getNonlinearFactor", getNonlinearFactor); + +} diff --git a/python/handwritten/nonlinear/Values.cpp b/python/handwritten/nonlinear/Values.cpp new file mode 100644 index 000000000..84e82f376 --- /dev/null +++ b/python/handwritten/nonlinear/Values.cpp @@ -0,0 +1,80 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/** + * @brief wraps Values class to python + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include + +#define NO_IMPORT_ARRAY +#include + +#include "gtsam/nonlinear/Values.h" +#include "gtsam/geometry/Point2.h" +#include "gtsam/geometry/Rot2.h" +#include "gtsam/geometry/Pose2.h" +#include "gtsam/geometry/Point3.h" +#include "gtsam/geometry/Rot3.h" +#include "gtsam/geometry/Pose3.h" +#include "gtsam/navigation/ImuBias.h" + +using namespace boost::python; +using namespace gtsam; + +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, Values::print, 0, 1); + +void exportValues(){ + + typedef imuBias::ConstantBias Bias; + + bool (Values::*exists1)(Key) const = &Values::exists; + void (Values::*insert_point2)(Key, const gtsam::Point2&) = &Values::insert; + void (Values::*insert_rot2) (Key, const gtsam::Rot2&) = &Values::insert; + void (Values::*insert_pose2) (Key, const gtsam::Pose2&) = &Values::insert; + void (Values::*insert_point3)(Key, const gtsam::Point3&) = &Values::insert; + void (Values::*insert_rot3) (Key, const gtsam::Rot3&) = &Values::insert; + void (Values::*insert_pose3) (Key, const gtsam::Pose3&) = &Values::insert; + void (Values::*insert_bias) (Key, const Bias&) = &Values::insert; + void (Values::*insert_vector3) (Key, const gtsam::Vector3&) = &Values::insert; + + class_("Values", init<>()) + .def(init()) + .def("clear", &Values::clear) + .def("dim", &Values::dim) + .def("empty", &Values::empty) + .def("equals", &Values::equals) + .def("erase", &Values::erase) + .def("insert_fixed", &Values::insertFixed) + .def("print", &Values::print, print_overloads(args("s"))) + .def("size", &Values::size) + .def("swap", &Values::swap) + .def("insert", insert_point2) + .def("insert", insert_rot2) + .def("insert", insert_pose2) + .def("insert", insert_point3) + .def("insert", insert_rot3) + .def("insert", insert_pose3) + .def("insert", insert_bias) + .def("insert", insert_vector3) + .def("atPoint2", &Values::at, return_value_policy()) + .def("atRot2", &Values::at, return_value_policy()) + .def("atPose2", &Values::at, return_value_policy()) + .def("atPoint3", &Values::at, return_value_policy()) + .def("atRot3", &Values::at, return_value_policy()) + .def("atPose3", &Values::at, return_value_policy()) + .def("atConstantBias", &Values::at, return_value_policy()) + .def("atVector3", &Values::at, return_value_policy()) + .def("exists", exists1) + .def("keys", &Values::keys) + ; +} diff --git a/python/handwritten/slam/BearingFactor.cpp b/python/handwritten/slam/BearingFactor.cpp new file mode 100644 index 000000000..2d4688bde --- /dev/null +++ b/python/handwritten/slam/BearingFactor.cpp @@ -0,0 +1,16 @@ +#include + +#define NO_IMPORT_ARRAY +#include + +#include + +using namespace boost::python; +using namespace gtsam; + +using namespace std; + +template +void exportBearingFactor(const std::string& name) { + class_(name, init<>()); +} diff --git a/python/handwritten/slam/BetweenFactor.cpp b/python/handwritten/slam/BetweenFactor.cpp new file mode 100644 index 000000000..0e0949fb5 --- /dev/null +++ b/python/handwritten/slam/BetweenFactor.cpp @@ -0,0 +1,57 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/** + * @brief wraps BetweenFactor for several values to python + * @author Andrew Melim + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include + +#define NO_IMPORT_ARRAY +#include + +#include "gtsam/slam/BetweenFactor.h" +#include "gtsam/geometry/Point2.h" +#include "gtsam/geometry/Rot2.h" +#include "gtsam/geometry/Pose2.h" +#include "gtsam/geometry/Point3.h" +#include "gtsam/geometry/Rot3.h" +#include "gtsam/geometry/Pose3.h" + +using namespace boost::python; +using namespace gtsam; + +using namespace std; + +// template +// void exportBetweenFactor(const std::string& name){ +// class_(name, init<>()) +// .def(init()) +// ; +// } + +#define BETWEENFACTOR(T) \ + class_< BetweenFactor, bases, boost::shared_ptr< BetweenFactor > >("BetweenFactor"#T) \ + .def(init()) \ + .def("measured", &BetweenFactor::measured, return_internal_reference<>()) \ +; + +void exportBetweenFactors() +{ + BETWEENFACTOR(Point2) + BETWEENFACTOR(Rot2) + BETWEENFACTOR(Pose2) + BETWEENFACTOR(Point3) + BETWEENFACTOR(Rot3) + BETWEENFACTOR(Pose3) +} diff --git a/python/handwritten/slam/GenericProjectionFactor.cpp b/python/handwritten/slam/GenericProjectionFactor.cpp new file mode 100644 index 000000000..dd932ccd4 --- /dev/null +++ b/python/handwritten/slam/GenericProjectionFactor.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 + + * -------------------------------------------------------------------------- */ + +/** + * @brief wraps GenericProjectionFactor for several values to python + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include + +#define NO_IMPORT_ARRAY +#include + +#include "gtsam/slam/ProjectionFactor.h" +#include "gtsam/geometry/Pose3.h" +#include "gtsam/geometry/Point3.h" +#include "gtsam/geometry/Cal3_S2.h" + +using namespace boost::python; +using namespace gtsam; + +using namespace std; + +typedef GenericProjectionFactor GenericProjectionFactorCal3_S2; + +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, GenericProjectionFactorCal3_S2::print, 0, 1) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, GenericProjectionFactorCal3_S2::equals, 1, 2) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(evaluateError_overloads, GenericProjectionFactorCal3_S2::evaluateError, 2, 4) + +void exportGenericProjectionFactor() +{ + + class_ >("GenericProjectionFactorCal3_S2", init<>()) + .def(init &, optional >()) + .def(init &, bool, bool, optional >()) + .def("print", &GenericProjectionFactorCal3_S2::print, print_overloads(args("s"))) + .def("equals", &GenericProjectionFactorCal3_S2::equals, equals_overloads(args("q","tol"))) + .def("evaluate_error", &GenericProjectionFactorCal3_S2::evaluateError, evaluateError_overloads()) + .def("measured", &GenericProjectionFactorCal3_S2::measured, return_value_policy()) + // TODO(Ellon): Find the right return policy when returning a 'const shared_ptr<...> &' + // .def("calibration", &GenericProjectionFactorCal3_S2::calibration, return_value_policy()) + .def("verbose_cheirality", &GenericProjectionFactorCal3_S2::verboseCheirality) + .def("throw_cheirality", &GenericProjectionFactorCal3_S2::throwCheirality) + ; + +} diff --git a/python/handwritten/slam/PriorFactor.cpp b/python/handwritten/slam/PriorFactor.cpp new file mode 100644 index 000000000..3ada91f43 --- /dev/null +++ b/python/handwritten/slam/PriorFactor.cpp @@ -0,0 +1,58 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/** + * @brief wraps PriorFactor for several values to python + * @author Andrew Melim + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include + +#define NO_IMPORT_ARRAY +#include + +#include "gtsam/slam/PriorFactor.h" +#include "gtsam/geometry/Point2.h" +#include "gtsam/geometry/Rot2.h" +#include "gtsam/geometry/Pose2.h" +#include "gtsam/geometry/Point3.h" +#include "gtsam/geometry/Rot3.h" +#include "gtsam/geometry/Pose3.h" + +using namespace boost::python; +using namespace gtsam; + +using namespace std; + +// template< class FACTOR, class VALUE > +// void exportPriorFactor(const std::string& name){ +// class_< FACTOR >(name.c_str(), init<>()) +// .def(init< Key, VALUE&, SharedNoiseModel >()) +// ; +// } + +#define PRIORFACTOR(VALUE) \ + class_< PriorFactor, bases, boost::shared_ptr< PriorFactor > >("PriorFactor"#VALUE) \ + .def(init()) \ + .def("prior", &PriorFactor::prior, return_internal_reference<>()) \ +; + +void exportPriorFactors() +{ + PRIORFACTOR(Point2) + PRIORFACTOR(Rot2) + PRIORFACTOR(Pose2) + PRIORFACTOR(Point3) + PRIORFACTOR(Rot3) + PRIORFACTOR(Pose3) + PRIORFACTOR(Vector3) +} diff --git a/python/handwritten/utils/NumpyEigen.cpp b/python/handwritten/utils/NumpyEigen.cpp new file mode 100644 index 000000000..d7cebe7ad --- /dev/null +++ b/python/handwritten/utils/NumpyEigen.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 + + * -------------------------------------------------------------------------- */ + +/** + * @brief register conversion matrix between numpy and Eigen + * @author Ellon Paiva Mendes (LAAS-CNRS) + **/ + +#include + +#define NO_IMPORT_ARRAY +#include + +#include "gtsam/base/Matrix.h" +#include "gtsam/base/Vector.h" + +using namespace boost::python; +using namespace gtsam; + +void registerNumpyEigenConversions() +{ + // NOTE: import array should be called only in the cpp defining the module + // import_array(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + NumpyEigenConverter::register_converter(); + +} diff --git a/python/include/numpy_eigen/NumpyEigenConverter.hpp b/python/include/numpy_eigen/NumpyEigenConverter.hpp new file mode 100755 index 000000000..4cf16a974 --- /dev/null +++ b/python/include/numpy_eigen/NumpyEigenConverter.hpp @@ -0,0 +1,334 @@ +/** + * @file NumpyEigenConverter.hpp + * @author Paul Furgale + * @date Fri Feb 4 11:17:25 2011 + * + * @brief Classes to support conversion from numpy arrays in Python + * to Eigen3 matrices in c++ + * + * + */ + +#ifndef NUMPY_EIGEN_CONVERTER_HPP +#define NUMPY_EIGEN_CONVERTER_HPP + +#include +//#include + +#include "numpy/numpyconfig.h" +#ifdef NPY_1_7_API_VERSION +#define NPY_NO_DEPRECATED_API NPY_1_7_API_VERSION +#define NPE_PY_ARRAY_OBJECT PyArrayObject +#else +//TODO Remove this as soon as support for Numpy version before 1.7 is dropped +#define NPE_PY_ARRAY_OBJECT PyObject +#endif + +#define PY_ARRAY_UNIQUE_SYMBOL NP_Eigen_AS +#include + +#include "type_traits.hpp" +#include +#include "copy_routines.hpp" + + + +/** + * @class NumpyEigenConverter + * @tparam the Eigen3 matrix type this class is specialized for + * + * adapted from http://misspent.wordpress.com/2009/09/27/how-to-write-boost-python-converters/ + * General help available http://docs.scipy.org/doc/numpy/reference/c-api.array.html + * + * To use: + * + * #include + * + * + * BOOST_PYTHON_MODULE(libmy_module_python) + * { + * // The converters will cause a segfault unless import_array() is called before the first one + * import_array(); + * NumpyEigenConverter >::register_converter(); + * NumpyEigenConverter >::register_converter(); + * } + * + */ +template +struct NumpyEigenConverter +{ + + typedef EIGEN_MATRIX_T matrix_t; + typedef typename matrix_t::Scalar scalar_t; + + enum { + RowsAtCompileTime = matrix_t::RowsAtCompileTime, + ColsAtCompileTime = matrix_t::ColsAtCompileTime, + MaxRowsAtCompileTime = matrix_t::MaxRowsAtCompileTime, + MaxColsAtCompileTime = matrix_t::MaxColsAtCompileTime, + NpyType = TypeToNumPy::NpyType, + //Flags = ei_compute_matrix_flags<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>::ret, + //CoeffReadCost = NumTraits::ReadCost, + Options = matrix_t::Options + //InnerStrideAtCompileTime = 1, + //OuterStrideAtCompileTime = (Options&RowMajor) ? ColsAtCompileTime : RowsAtCompileTime + }; + + static std::string castSizeOption(int option) + { + if(option == Eigen::Dynamic) + return "Dynamic"; + else + return boost::lexical_cast(option); + } + + static std::string toString() + { + return std::string() + "Eigen::Matrix<" + TypeToNumPy::typeString() + ", " + + castSizeOption(RowsAtCompileTime) + ", " + + castSizeOption(ColsAtCompileTime) + ", " + + boost::lexical_cast((int)Options) + ", " + + castSizeOption(MaxRowsAtCompileTime) + ", " + + castSizeOption(MaxColsAtCompileTime) + ">"; + } + + // The "Convert from C to Python" API + static PyObject * convert(const matrix_t & M) + { + PyObject * P = NULL; + if(RowsAtCompileTime == 1 || ColsAtCompileTime == 1) + { + // Create a 1D array + npy_intp dimensions[1]; + dimensions[0] = M.size(); + P = PyArray_SimpleNew(1, dimensions, TypeToNumPy::NpyType); + numpyTypeDemuxer< CopyEigenToNumpyVector >(&M, reinterpret_cast(P)); + } + else + { + // create a 2D array. + npy_intp dimensions[2]; + dimensions[0] = M.rows(); + dimensions[1] = M.cols(); + P = PyArray_SimpleNew(2, dimensions, TypeToNumPy::NpyType); + numpyTypeDemuxer< CopyEigenToNumpyMatrix >(&M, reinterpret_cast(P)); + } + + // incrementing the reference seems to cause a memory leak. + // boost::python::incref(P); + // This agrees with the sample code found here: + // http://mail.python.org/pipermail/cplusplus-sig/2008-October/013825.html + return P; + } + + static bool isDimensionValid(int requestedSize, int sizeAtCompileTime, int maxSizeAtCompileTime) + { + bool valid = true; + if(sizeAtCompileTime == Eigen::Dynamic) + { + // Check for dynamic fixed size + // http://eigen.tuxfamily.org/dox-devel/TutorialMatrixClass.html#TutorialMatrixOptTemplParams + if(!(maxSizeAtCompileTime == Eigen::Dynamic || requestedSize <= maxSizeAtCompileTime)) + { + valid = false; + } + } + else if(sizeAtCompileTime != requestedSize) + { + valid = false; + } + return valid; + } + + static void checkMatrixSizes(NPE_PY_ARRAY_OBJECT * obj_ptr) + { + int rows = PyArray_DIM(obj_ptr, 0); + int cols = PyArray_DIM(obj_ptr, 1); + + bool rowsValid = isDimensionValid(rows, RowsAtCompileTime, MaxRowsAtCompileTime); + bool colsValid = isDimensionValid(cols, ColsAtCompileTime, MaxColsAtCompileTime); + if(!rowsValid || !colsValid) + { + THROW_TYPE_ERROR("Can not convert " << npyArrayTypeString(obj_ptr) << " to " << toString() + << ". Mismatched sizes."); + } + } + + static void checkRowVectorSizes(NPE_PY_ARRAY_OBJECT * obj_ptr, int cols) + { + if(!isDimensionValid(cols, ColsAtCompileTime, MaxColsAtCompileTime)) + { + THROW_TYPE_ERROR("Can not convert " << npyArrayTypeString(obj_ptr) << " to " << toString() + << ". Mismatched sizes."); + } + } + + static void checkColumnVectorSizes(NPE_PY_ARRAY_OBJECT * obj_ptr, int rows) + { + // Check if the type can accomidate one column. + if(ColsAtCompileTime == Eigen::Dynamic || ColsAtCompileTime == 1) + { + if(!isDimensionValid(rows, RowsAtCompileTime, MaxRowsAtCompileTime)) + { + THROW_TYPE_ERROR("Can not convert " << npyArrayTypeString(obj_ptr) << " to " << toString() + << ". Mismatched sizes."); + } + } + else + { + THROW_TYPE_ERROR("Can not convert " << npyArrayTypeString(obj_ptr) << " to " << toString() + << ". Mismatched sizes."); + } + + } + + static void checkVectorSizes(NPE_PY_ARRAY_OBJECT * obj_ptr) + { + int size = PyArray_DIM(obj_ptr, 0); + + // If the number of rows is fixed at 1, assume that is the sense of the vector. + // Otherwise, assume it is a column. + if(RowsAtCompileTime == 1) + { + checkRowVectorSizes(obj_ptr, size); + } + else + { + checkColumnVectorSizes(obj_ptr, size); + } + } + + + static void* convertible(PyObject *obj_ptr) + { + // Check for a null pointer. + if(!obj_ptr) + { + //THROW_TYPE_ERROR("PyObject pointer was null"); + return 0; + } + + // Make sure this is a numpy array. + if (!PyArray_Check(obj_ptr)) + { + //THROW_TYPE_ERROR("Conversion is only defined for numpy array and matrix types"); + return 0; + } + + NPE_PY_ARRAY_OBJECT * array_ptr = reinterpret_cast(obj_ptr); + + // Check the type of the array. + int npyType = getNpyType(array_ptr); + + if(!TypeToNumPy::canConvert(npyType)) + { + //THROW_TYPE_ERROR("Can not convert " << npyArrayTypeString(obj_ptr) << " to " << toString() + // << ". Mismatched types."); + return 0; + } + + + + // Check the array dimensions. + int nd = PyArray_NDIM(array_ptr); + + if(nd != 1 && nd != 2) + { + THROW_TYPE_ERROR("Conversion is only valid for arrays with 1 or 2 dimensions. Argument has " << nd << " dimensions"); + } + + if(nd == 1) + { + checkVectorSizes(array_ptr); + } + else + { + // Two-dimensional matrix type. + checkMatrixSizes(array_ptr); + } + + + return obj_ptr; + } + + + static void construct(PyObject *obj_ptr, boost::python::converter::rvalue_from_python_stage1_data *data) + { + boost::python::converter::rvalue_from_python_storage * matData = reinterpret_cast * >(data); + void* storage = matData->storage.bytes; + + // Make sure storage is 16byte aligned. With help from code from Memory.h + void * aligned = reinterpret_cast((reinterpret_cast(storage) & ~(size_t(15))) + 16); + + matrix_t * Mp = new (aligned) matrix_t(); + // Stash the memory chunk pointer for later use by boost.python + // This signals boost::python that the new value must be deleted eventually + data->convertible = storage; + + + // std::cout << "Creating aligned pointer " << aligned << " from storage " << storage << std::endl; + // std::cout << "matrix size: " << sizeof(matrix_t) << std::endl; + // std::cout << "referent size: " << boost::python::detail::referent_size< matrix_t & >::value << std::endl; + // std::cout << "sizeof(storage): " << sizeof(matData->storage) << std::endl; + // std::cout << "sizeof(bytes): " << sizeof(matData->storage.bytes) << std::endl; + + + + matrix_t & M = *Mp; + + if (!PyArray_Check(obj_ptr)) + { + THROW_TYPE_ERROR("construct is only defined for numpy array and matrix types"); + } + + NPE_PY_ARRAY_OBJECT * array_ptr = reinterpret_cast(obj_ptr); + + int nd = PyArray_NDIM(array_ptr); + if(nd == 1) + { + int size = PyArray_DIM(array_ptr, 0); + // This is a vector type + if(RowsAtCompileTime == 1) + { + // Row Vector + M.resize(1,size); + } + else + { + // Column Vector + M.resize(size,1); + } + numpyTypeDemuxer< CopyNumpyToEigenVector >(&M, array_ptr); + } + else + { + int rows = PyArray_DIM(array_ptr, 0); + int cols = PyArray_DIM(array_ptr, 1); + + M.resize(rows,cols); + numpyTypeDemuxer< CopyNumpyToEigenMatrix >(&M, array_ptr); + } + + + + + } + + + // The registration function. + static void register_converter() + { + boost::python::to_python_converter(); + boost::python::converter::registry::push_back( + &NumpyEigenConverter::convertible, + &NumpyEigenConverter::construct, + boost::python::type_id()); + + } + +}; + + + + +#endif /* NUMPY_EIGEN_CONVERTER_HPP */ diff --git a/python/include/numpy_eigen/README.md b/python/include/numpy_eigen/README.md new file mode 100644 index 000000000..bd5a3f44d --- /dev/null +++ b/python/include/numpy_eigen/README.md @@ -0,0 +1,6 @@ +numpy_eigen +=========== + +A boost python converter that handles the copy of matrices from the Eigen linear algebra library in C++ to numpy in Python. + +This is a minimal version based on the original from Paul Furgale (https://github.com/ethz-asl/Schweizer-Messer/tree/master/numpy_eigen) \ No newline at end of file diff --git a/python/include/numpy_eigen/boost_python_headers.hpp b/python/include/numpy_eigen/boost_python_headers.hpp new file mode 100755 index 000000000..5499d2917 --- /dev/null +++ b/python/include/numpy_eigen/boost_python_headers.hpp @@ -0,0 +1,250 @@ +/** + * @file boost_python_headers.hpp + * @author Paul Furgale + * @date Mon Dec 12 10:36:03 2011 + * + * @brief A header that specializes boost-python to work with fixed-sized Eigen types. + * + * The original version of this library did not include these specializations and this caused + * assert failures when running on Ubuntu 10.04 32-bit. More information about fixed-size + * vectorizable types in Eigen is available here: + * http://eigen.tuxfamily.org/dox-devel/TopicFixedSizeVectorizable.html + * + * This code has been tested on Ubunutu 10.04 64 and 32 bit, OSX Snow Leopard and OSX Lion. + * + * This code is derived from boost/python/converter/arg_from_python.hpp + * Copyright David Abrahams 2002. + * Distributed under the Boost Software License, Version 1.0. (See http://www.boost.org/LICENSE_1_0.txt) + * + */ +#ifndef NUMPY_EIGEN_CONVERTERS_HPP +#define NUMPY_EIGEN_CONVERTERS_HPP + +#include +#include +#include +#include +#include +#include + + +namespace boost { namespace python { namespace detail { + template + struct referent_size; + + // This bit of code makes sure we have 16 extra bytes to do the pointer alignment for fixed-sized Eigen types + template + struct referent_size< Eigen::Matrix& > + { + // Add 16 bytes so we can get alignment + BOOST_STATIC_CONSTANT( std::size_t, value = sizeof(Eigen::Matrix) + 16); + }; + + // This bit of code makes sure we have 16 extra bytes to do the pointer alignment for fixed-sized Eigen types + template + struct referent_size< Eigen::Matrix const & > + { + // Add 16 bytes so we can get alignment + BOOST_STATIC_CONSTANT( std::size_t, value = sizeof(Eigen::Matrix) + 16); + }; + + // This bit of code makes sure we have 16 extra bytes to do the pointer alignment for fixed-sized Eigen types + template + struct referent_size< Eigen::Matrix > + { + // Add 16 bytes so we can get alignment + BOOST_STATIC_CONSTANT( std::size_t, value = sizeof(Eigen::Matrix) + 16); + }; + + + }}} + +namespace boost { namespace python { namespace converter { + + + template + struct rvalue_from_python_data< Eigen::Matrix const &> : rvalue_from_python_storage< Eigen::Matrix const & > + { + typedef typename Eigen::Matrix T; +# if (!defined(__MWERKS__) || __MWERKS__ >= 0x3000) \ + && (!defined(__EDG_VERSION__) || __EDG_VERSION__ >= 245) \ + && (!defined(__DECCXX_VER) || __DECCXX_VER > 60590014) \ + && !defined(BOOST_PYTHON_SYNOPSIS) /* Synopsis' OpenCXX has trouble parsing this */ + // This must always be a POD struct with m_data its first member. + BOOST_STATIC_ASSERT(BOOST_PYTHON_OFFSETOF(rvalue_from_python_storage,stage1) == 0); +# endif + + // The usual constructor + rvalue_from_python_data(rvalue_from_python_stage1_data const & _stage1) + { + this->stage1 = _stage1; + } + + + // This constructor just sets m_convertible -- used by + // implicitly_convertible<> to perform the final step of the + // conversion, where the construct() function is already known. + rvalue_from_python_data(void* convertible) + { + this->stage1.convertible = convertible; + } + + // Destroys any object constructed in the storage. + ~rvalue_from_python_data() + { + // Realign the pointer and destroy + if (this->stage1.convertible == this->storage.bytes) + { + void * storage = reinterpret_cast(this->storage.bytes); + T * aligned = reinterpret_cast(reinterpret_cast((reinterpret_cast(storage) & ~(size_t(15))) + 16)); + + //std::cout << "Destroying " << (void*)aligned << std::endl; + aligned->T::~T(); + } + } + private: + typedef typename add_reference::type>::type ref_type; + }; + + + + + + // Used when T is a plain value (non-pointer, non-reference) type or + // a (non-volatile) const reference to a plain value type. + template + struct arg_rvalue_from_python< Eigen::Matrix > + { + typedef Eigen::Matrix const & T; + typedef typename boost::add_reference< + T + // We can't add_const here, or it would be impossible to pass + // auto_ptr args from Python to C++ + >::type result_type; + + arg_rvalue_from_python(PyObject * obj) : m_data(converter::rvalue_from_python_stage1(obj, registered::converters)) + , m_source(obj) + { + + } + bool convertible() const + { + return m_data.stage1.convertible != 0; + } + + +# if BOOST_MSVC < 1301 || _MSC_FULL_VER > 13102196 + typename arg_rvalue_from_python:: +# endif + result_type operator()() + { + if (m_data.stage1.construct != 0) + m_data.stage1.construct(m_source, &m_data.stage1); + + // Here is the magic... + // Realign the pointer + void * storage = reinterpret_cast(m_data.storage.bytes); + void * aligned = reinterpret_cast((reinterpret_cast(storage) & ~(size_t(15))) + 16); + + return python::detail::void_ptr_to_reference(aligned, (result_type(*)())0); + } + + private: + rvalue_from_python_data m_data; + PyObject* m_source; + }; + + + // Used when T is a plain value (non-pointer, non-reference) type or + // a (non-volatile) const reference to a plain value type. + template + struct arg_rvalue_from_python< Eigen::Matrix const & > + { + typedef Eigen::Matrix const & T; + typedef typename boost::add_reference< + T + // We can't add_const here, or it would be impossible to pass + // auto_ptr args from Python to C++ + >::type result_type; + + arg_rvalue_from_python(PyObject * obj) : m_data(converter::rvalue_from_python_stage1(obj, registered::converters)) + , m_source(obj) + { + + } + bool convertible() const + { + return m_data.stage1.convertible != 0; + } + + +# if BOOST_MSVC < 1301 || _MSC_FULL_VER > 13102196 + typename arg_rvalue_from_python:: +# endif + result_type operator()() + { + if (m_data.stage1.construct != 0) + m_data.stage1.construct(m_source, &m_data.stage1); + + // Here is the magic... + // Realign the pointer + void * storage = reinterpret_cast(m_data.storage.bytes); + void * aligned = reinterpret_cast((reinterpret_cast(storage) & ~(size_t(15))) + 16); + + return python::detail::void_ptr_to_reference(aligned, (result_type(*)())0); + } + + private: + rvalue_from_python_data m_data; + PyObject* m_source; + }; + + // Used when T is a plain value (non-pointer, non-reference) type or + // a (non-volatile) const reference to a plain value type. + template + struct arg_rvalue_from_python< Eigen::Matrix const > + { + typedef Eigen::Matrix const & T; + typedef typename boost::add_reference< + T + // We can't add_const here, or it would be impossible to pass + // auto_ptr args from Python to C++ + >::type result_type; + + arg_rvalue_from_python(PyObject * obj) : m_data(converter::rvalue_from_python_stage1(obj, registered::converters)) + , m_source(obj) + { + + } + bool convertible() const + { + return m_data.stage1.convertible != 0; + } + + +# if BOOST_MSVC < 1301 || _MSC_FULL_VER > 13102196 + typename arg_rvalue_from_python:: +# endif + result_type operator()() + { + if (m_data.stage1.construct != 0) + m_data.stage1.construct(m_source, &m_data.stage1); + + // Here is the magic... + // Realign the pointer + void * storage = reinterpret_cast(m_data.storage.bytes); + void * aligned = reinterpret_cast((reinterpret_cast(storage) & ~(size_t(15))) + 16); + + return python::detail::void_ptr_to_reference(aligned, (result_type(*)())0); + } + + private: + rvalue_from_python_data m_data; + PyObject* m_source; + }; + + + }}} + + +#endif /* NUMPY_EIGEN_CONVERTERS_HPP */ diff --git a/python/include/numpy_eigen/copy_routines.hpp b/python/include/numpy_eigen/copy_routines.hpp new file mode 100755 index 000000000..b112a7426 --- /dev/null +++ b/python/include/numpy_eigen/copy_routines.hpp @@ -0,0 +1,149 @@ +#ifndef NUMPY_EIGEN_COPY_ROUTINES_HPP +#define NUMPY_EIGEN_COPY_ROUTINES_HPP + + +template +struct CopyNumpyToEigenMatrix +{ + typedef EIGEN_T matrix_t; + typedef typename matrix_t::Scalar scalar_t; + + template + void exec(EIGEN_T * M_, NPE_PY_ARRAY_OBJECT * P_) + { + // Assumes M is already initialized. + for(int r = 0; r < M_->rows(); r++) + { + for(int c = 0; c < M_->cols(); c++) + { + T * p = static_cast(PyArray_GETPTR2(P_, r, c)); + (*M_)(r,c) = static_cast(*p); + } + } + } + +}; + +template +struct CopyEigenToNumpyMatrix +{ + typedef EIGEN_T matrix_t; + typedef typename matrix_t::Scalar scalar_t; + + template + void exec(EIGEN_T * M_, NPE_PY_ARRAY_OBJECT * P_) + { + // Assumes M is already initialized. + for(int r = 0; r < M_->rows(); r++) + { + for(int c = 0; c < M_->cols(); c++) + { + T * p = static_cast(PyArray_GETPTR2(P_, r, c)); + *p = static_cast((*M_)(r,c)); + } + } + } + +}; + +template +struct CopyEigenToNumpyVector +{ + typedef EIGEN_T matrix_t; + typedef typename matrix_t::Scalar scalar_t; + + template + void exec(EIGEN_T * M_, NPE_PY_ARRAY_OBJECT * P_) + { + // Assumes M is already initialized. + for(int i = 0; i < M_->size(); i++) + { + T * p = static_cast(PyArray_GETPTR1(P_, i)); + *p = static_cast((*M_)(i)); + } + } + +}; + + +template +struct CopyNumpyToEigenVector +{ + typedef EIGEN_T matrix_t; + typedef typename matrix_t::Scalar scalar_t; + + template + void exec(EIGEN_T * M_, NPE_PY_ARRAY_OBJECT * P_) + { + // Assumes M is already initialized. + for(int i = 0; i < M_->size(); i++) + { + T * p = static_cast(PyArray_GETPTR1(P_, i)); + (*M_)(i) = static_cast(*p); + } + } + +}; + + + + +// Crazy syntax in this function was found here: +// http://stackoverflow.com/questions/1840253/c-template-member-function-of-template-class-called-from-template-function/1840318#1840318 +template< typename FUNCTOR_T> +inline void numpyTypeDemuxer(typename FUNCTOR_T::matrix_t * M, NPE_PY_ARRAY_OBJECT * P) +{ + FUNCTOR_T f; + + int npyType = getNpyType(P); + switch(npyType) + { + case NPY_BOOL: + f.template exec(M,P); + break; + case NPY_BYTE: + f.template exec(M,P); + break; + case NPY_UBYTE: + f.template exec(M,P); + break; + case NPY_SHORT: + f.template exec(M,P); + break; + case NPY_USHORT: + f.template exec(M,P); + break; + case NPY_INT: + f.template exec(M,P); + break; + case NPY_UINT: + f.template exec(M,P); + break; + case NPY_LONG: + f.template exec(M,P); + break; + case NPY_ULONG: + f.template exec(M,P); + break; + case NPY_LONGLONG: + f.template exec(M,P); + break; + case NPY_ULONGLONG: + f.template exec(M,P); + break; + case NPY_FLOAT: + f.template exec(M,P); + break; + case NPY_DOUBLE: + f.template exec(M,P); + break; + case NPY_LONGDOUBLE: + f.template exec(M,P); + break; + default: + THROW_TYPE_ERROR("Unsupported type: " << npyTypeToString(npyType)); + } +} + + +#endif /* NUMPY_EIGEN_COPY_ROUTINES_HPP */ diff --git a/python/include/numpy_eigen/type_traits.hpp b/python/include/numpy_eigen/type_traits.hpp new file mode 100755 index 000000000..97e4bb5a0 --- /dev/null +++ b/python/include/numpy_eigen/type_traits.hpp @@ -0,0 +1,191 @@ +#ifndef NUMPY_EIGEN_TYPE_TRAITS_HPP +#define NUMPY_EIGEN_TYPE_TRAITS_HPP + +#define THROW_TYPE_ERROR(msg) \ + { \ + std::stringstream type_error_ss; \ + type_error_ss << msg; \ + PyErr_SetString(PyExc_TypeError, type_error_ss.str().c_str()); \ + throw boost::python::error_already_set(); \ + } + + +//////////////////////////////////////////////// +// TypeToNumPy +// Defines helper functions based on the Eigen3 matrix type that +// decide what conversions can happen Eigen3 --> NumPy +// Also, converts a type to a NumPy enum. +template struct TypeToNumPy; + +template<> struct TypeToNumPy +{ + enum { NpyType = NPY_INT }; + static const char * npyString() { return "NPY_INT"; } + static const char * typeString() { return "int"; } + static bool canConvert(int type) + { + return type == NPY_INT || type == NPY_LONG; + } +}; + +template<> struct TypeToNumPy +{ + enum { NpyType = NPY_LONG }; + static const char * npyString() { return "NPY_LONG"; } + static const char * typeString() { return "long"; } + static bool canConvert(int type) + { + return type == NPY_INT || type == NPY_LONG; + } +}; + +template<> struct TypeToNumPy +{ + enum { NpyType = NPY_UINT64 }; + static const char * npyString() { return "NPY_UINT64"; } + static const char * typeString() { return "uint64"; } + static bool canConvert(int type) + { + return type == NPY_UINT8 || type == NPY_USHORT || + type == NPY_UINT16 || type == NPY_UINT32 || + type == NPY_ULONG || type == NPY_ULONGLONG || + type == NPY_UINT64; + } +}; + +template<> struct TypeToNumPy +{ + enum { NpyType = NPY_UBYTE }; + static const char * npyString() { return "NPY_UBYTE"; } + static const char * typeString() { return "unsigned char"; } + static bool canConvert(int type) + { + return type == NPY_UBYTE || type == NPY_BYTE || type == NPY_CHAR; + } +}; + +template<> struct TypeToNumPy +{ + enum { NpyType = NPY_BYTE }; + static const char * npyString() { return "NPY_BYTE"; } + static const char * typeString() { return "char"; } + static bool canConvert(int type) + { + return type == NPY_UBYTE || type == NPY_BYTE || type == NPY_CHAR; + } +}; + + +template<> struct TypeToNumPy +{ + enum { NpyType = NPY_FLOAT }; + static const char * npyString() { return "NPY_FLOAT"; } + static const char * typeString() { return "float"; } + static bool canConvert(int type) + { + return type == NPY_INT || type == NPY_FLOAT || type == NPY_LONG; + } +}; + +template<> struct TypeToNumPy +{ + enum { NpyType = NPY_DOUBLE }; + static const char * npyString() { return "NPY_DOUBLE"; } + static const char * typeString() { return "double"; } + static bool canConvert(int type) + { + return type == NPY_INT || type == NPY_FLOAT || type == NPY_DOUBLE || type == NPY_LONG; + } +}; + + +inline int getNpyType(PyObject * obj_ptr){ + return PyArray_ObjectType(obj_ptr, 0); +} + +#ifdef NPY_1_7_API_VERSION +inline int getNpyType(PyArrayObject * obj_ptr){ + PyArray_Descr * descr = PyArray_MinScalarType(obj_ptr); + if (descr == NULL){ + THROW_TYPE_ERROR("Unsupported type: PyArray_MinScalarType returned null!"); + } + return descr->type_num; +} +#endif + +inline const char * npyTypeToString(int npyType) +{ + switch(npyType) + { + case NPY_BOOL: + return "NPY_BOOL"; + case NPY_BYTE: + return "NPY_BYTE"; + case NPY_UBYTE: + return "NPY_UBYTE"; + case NPY_SHORT: + return "NPY_SHORT"; + case NPY_USHORT: + return "NPY_USHORT"; + case NPY_INT: + return "NPY_INT"; + case NPY_UINT: + return "NPY_UINT"; + case NPY_LONG: + return "NPY_LONG"; + case NPY_ULONG: + return "NPY_ULONG"; + case NPY_LONGLONG: + return "NPY_LONGLONG"; + case NPY_ULONGLONG: + return "NPY_ULONGLONG"; + case NPY_FLOAT: + return "NPY_FLOAT"; + case NPY_DOUBLE: + return "NPY_DOUBLE"; + case NPY_LONGDOUBLE: + return "NPY_LONGDOUBLE"; + case NPY_CFLOAT: + return "NPY_CFLOAT"; + case NPY_CDOUBLE: + return "NPY_CDOUBLE"; + case NPY_CLONGDOUBLE: + return "NPY_CLONGDOUBLE"; + case NPY_OBJECT: + return "NPY_OBJECT"; + case NPY_STRING: + return "NPY_STRING"; + case NPY_UNICODE: + return "NPY_UNICODE"; + case NPY_VOID: + return "NPY_VOID"; + case NPY_NTYPES: + return "NPY_NTYPES"; + case NPY_NOTYPE: + return "NPY_NOTYPE"; + case NPY_CHAR: + return "NPY_CHAR"; + default: + return "Unknown type"; + } +} + +inline std::string npyArrayTypeString(NPE_PY_ARRAY_OBJECT * obj_ptr) +{ + std::stringstream ss; + int nd = PyArray_NDIM(obj_ptr); + ss << "numpy.array<" << npyTypeToString(getNpyType(obj_ptr)) << ">["; + if(nd > 0) + { + ss << PyArray_DIM(obj_ptr, 0); + for(int i = 1; i < nd; i++) + { + ss << ", " << PyArray_DIM(obj_ptr, i); + } + } + ss << "]"; + return ss.str(); +} + + +#endif /* NUMPY_EIGEN_TYPE_TRAITS_HPP */ diff --git a/python/setup.py.in b/python/setup.py.in new file mode 100644 index 000000000..d3b5fcde4 --- /dev/null +++ b/python/setup.py.in @@ -0,0 +1,15 @@ +from distutils.core import setup + +setup(name='gtsam', + version='${GTSAM_VERSION_STRING}', + description='GTSAM Python wrapper', + license = "BSD", + author='Frank Dellaert et. al', + author_email='frank.dellaert@gatech.edu', + maintainer_email='gtsam@lists.gatech.edu', + url='https://collab.cc.gatech.edu/borg/gtsam', + package_dir={ '': '${CMAKE_CURRENT_SOURCE_DIR}' }, + packages=['gtsam', 'gtsam_utils', 'gtsam_examples', 'gtsam_tests'], + #package_data={'gtsam' : ['_libgtsam_python.so']}, # location of .so file is relative to package_dir + data_files=[('${PY_INSTALL_FOLDER}/gtsam/', ['gtsam/_libgtsam_python.so'])], # location of .so file relative to setup.py + )