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

Merge remote-tracking branch 'origin/develop' into feature/LPSolver

release/4.3a0
dellaert 2016-01-29 09:12:50 -08:00
parent 26a7647629 0c9910466e
commit 1720adbbd7
102 changed files with 6203 additions and 271 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
.gitignore vendored
View File

@ -7,3 +7,5 @@
/examples/Data/pose3example-rewritten.txt
*.txt.user
*.txt.user.6d59f0c
/python-build/
*.pydevproject

35
CMakeLists.txt
View File

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

4
THANKS
View File

@ -38,6 +38,10 @@ at Uni Zurich:
* Christian Forster
at LAAS-CNRS
* Ellon Paiva
Many thanks for your hard work!!!!
Frank Dellaert

102
cmake/FindNumPy.cmake Normal file
View File

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

69
cmake/GtsamPythonWrap.cmake
View File

@ -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)
# Create a static library version
add_library(${TARGET_NAME} SHARED ${ARGN})
target_link_libraries(${TARGET_NAME} ${Boost_PYTHON_LIBRARY} ${PYTHON_LIBRARY} gtsam-shared)
set_target_properties(${TARGET_NAME} PROPERTIES
OUTPUT_NAME ${TARGET_NAME}
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)
SOVERSION 0
SUFFIX ".pyd")
target_link_libraries(${moduleName}_python ${Boost_PYTHON_LIBRARY} ${PYTHON_LIBRARY} ${gtsamLib}) #temp
set(PYLIB_OUTPUT_FILE $<TARGET_FILE:${moduleName}_python>)
message(${PYLIB_OUTPUT_FILE})
get_filename_component(PYLIB_OUTPUT_NAME ${PYLIB_OUTPUT_FILE} NAME_WE)
set(PYLIB_SO_NAME ${PYLIB_OUTPUT_NAME}.pyd)
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 ${TARGET_NAME} PROPERTY LOCATION)
get_property(PYLIB_OUTPUT_FILE TARGET ${moduleName}_python PROPERTY LOCATION)
set(PYLIB_OUTPUT_FILE $<TARGET_FILE:${moduleName}_python>)
message(${PYLIB_OUTPUT_FILE})
get_filename_component(PYLIB_OUTPUT_NAME ${PYLIB_OUTPUT_FILE} NAME_WE)
set(PYLIB_SO_NAME ${PYLIB_OUTPUT_NAME}.so)
set(PYLIB_SO_NAME lib${moduleName}_python.so)
ENDIF(MSVC)
# 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}
@ -65,3 +88,15 @@ function(wrap_python TARGET_NAME PYTHON_MODULE_DIRECTORY)
list(APPEND AMCF ${PYTHON_MODULE_DIRECTORY}/${PYLIB_SO_NAME})
set_directory_properties(PROPERTIES ADDITIONAL_MAKE_CLEAN_FILES "${AMCF}")
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()

6
examples/CreateSFMExampleData.cpp
View File

@ -35,7 +35,7 @@ void createExampleBALFile(const string& filename, const vector<Point3>& 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<Point3>& 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);

10
gtsam.h
View File

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

4
gtsam/geometry/Pose3.cpp
View File

@ -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<double>::epsilon()) {
double omega_v = omega.dot(v); // translation parallel to axis
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 + omega_v * omega) / theta2;
Point3 t = (omega_cross_v - R * omega_cross_v + t_parallel) / theta2;
return Pose3(R, t);
} else {
return Pose3(R, v);

2
gtsam/geometry/Rot3.cpp
View File

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

34
gtsam/geometry/Rot3.h
View File

@ -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<double>(angle, axis));
return gtsam::Quaternion(Eigen::AngleAxis<double>(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<Quaternion>::Expmap(v);
return traits<gtsam::Quaternion>::Expmap(v);
#else
return traits<SO3>::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:

6
gtsam/geometry/Rot3M.cpp
View File

@ -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_);
}
/* ************************************************************************* */

18
gtsam/geometry/Rot3Q.cpp
View File

@ -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<Quaternion>::Logmap(R.quaternion_, H);
return traits<gtsam::Quaternion>::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_; }
/* ************************************************************************* */

3
gtsam/geometry/SO3.cpp
View File

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

4
gtsam/geometry/tests/testCalibratedCamera.cpp
View File

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

10
gtsam/geometry/tests/testEssentialMatrix.cpp
View File

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

2
gtsam/geometry/tests/testOrientedPlane3.cpp
View File

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

2
gtsam/geometry/tests/testPinholeCamera.cpp
View File

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

2
gtsam/geometry/tests/testPinholePose.cpp
View File

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

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

@ -133,6 +133,17 @@ TEST (Point3, distance) {
EXPECT(assert_equal(numH2, H2, 1e-8));
}
/* ************************************************************************* */
TEST(Point3, cross) {
Matrix aH1, aH2;
boost::function<Point3(const Point3&, const Point3&)> 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;

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

@ -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<Pose3>(x1, x_step), tol));
EXPECT(assert_equal(Pose3(Rot3::ypr(0,0,0), Point3(2,1,0)), expmap_default<Pose3>(x2, x_step), tol));
EXPECT(assert_equal(Pose3(Rot3::ypr(M_PI/4.0,0,0), Point3(2,2,0)), expmap_default<Pose3>(x3, sqrt(2.0) * x_step), tol));
EXPECT(assert_equal(Pose3(Rot3::Ypr(0,0,0), l1), expmap_default<Pose3>(x1, x_step), tol));
EXPECT(assert_equal(Pose3(Rot3::Ypr(0,0,0), Point3(2,1,0)), expmap_default<Pose3>(x2, x_step), tol));
EXPECT(assert_equal(Pose3(Rot3::Ypr(M_PI/4.0,0,0), Point3(2,2,0)), expmap_default<Pose3>(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<Pose3, double>(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);

123
gtsam/geometry/tests/testRot3.cpp
View File

@ -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<Vector3, Vector3>(testDexpL,
using exmap_derivative::w;
const Matrix actualDexpL = Rot3::ExpmapDerivative(w);
const Matrix expectedDexpL = numericalDerivative11<Vector3, Vector3>(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));
}
/* ************************************************************************* */
Vector3 thetahat(0.1, 0, 0.1);
TEST( Rot3, ExpmapDerivative2)
{
Matrix Jexpected = numericalDerivative11<Rot3, Vector3>(
boost::bind(&Rot3::Expmap, _1, boost::none), thetahat);
const Vector3 theta(0.1, 0, 0.1);
const Matrix Jexpected = numericalDerivative11<Rot3, Vector3>(
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, ExpmapDerivative3)
{
const Vector3 theta(10, 20, 30);
const Matrix Jexpected = numericalDerivative11<Rot3, Vector3>(
boost::bind(&Rot3::Expmap, _1, boost::none), theta);
CHECK(assert_equal(Jexpected, Rot3::ExpmapDerivative(theta)));
CHECK(assert_equal(Matrix3(Jexpected.transpose()), Rot3::ExpmapDerivative(-theta)));
}
/* ************************************************************************* */
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<Rot3, double>(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<Rot3, double>(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<Rot3, Vector3>(boost::bind(
const Vector3 thetahat(0.1, 0, 0.1);
const Matrix Jexpected = numericalDerivative11<Rot3, Vector3>(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<Vector,Rot3>(boost::bind(
const Vector3 thetahat(0.1, 0, 0.1);
const Rot3 R = Rot3::Expmap(thetahat); // some rotation
const Matrix Jexpected = numericalDerivative11<Vector,Rot3>(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<Vector,Rot3>(boost::bind(
const Vector3 thetahat(0.1, 0, 0.1);
const Rot3 R = Rot3::Expmap(thetahat); // some rotation
const Matrix Jexpected = numericalDerivative11<Vector,Rot3>(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())));

10
gtsam/geometry/tests/testTriangulation.cpp
View File

@ -40,7 +40,7 @@ static const boost::shared_ptr<Cal3_S2> sharedCal = //
boost::make_shared<Cal3_S2>(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<Cal3_S2> 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);

4
gtsam/geometry/tests/testUnit3.cpp
View File

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

1
gtsam/geometry/triangulation.h
View File

@ -19,6 +19,7 @@
#pragma once
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/slam/TriangulationFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>

15
gtsam/linear/GaussianBayesNet.cpp
View File

@ -141,7 +141,20 @@ namespace gtsam {
/* ************************************************************************* */
pair<Matrix,Vector> 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);
}
///* ************************************************************************* */

4
gtsam/navigation/GPSFactor.cpp
View File

@ -58,10 +58,10 @@ pair<Pose3, Vector3> 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

2
gtsam/navigation/MagFactor.h
View File

@ -60,7 +60,7 @@ public:
static Point3 unrotate(const Rot2& R, const Point3& p,
boost::optional<Matrix&> 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);

2
gtsam/navigation/tests/testAHRSFactor.cpp
View File

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

4
gtsam/navigation/tests/testCombinedImuFactor.cpp
View File

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

2
gtsam/navigation/tests/testGPSFactor.cpp
View File

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

10
gtsam/navigation/tests/testImuFactor.cpp
View File

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

2
gtsam/navigation/tests/testMagFactor.cpp
View File

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

2
gtsam/nonlinear/tests/testAdaptAutoDiff.cpp
View File

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

3
gtsam/sam/RangeFactor.h
View File

@ -31,8 +31,7 @@ struct Range;
* Works for any two types A1,A2 for which the functor Range<A1,A2>() is defined
* @addtogroup SAM
*/
template <typename A1, typename A2 = A1,
typename T = typename Range<A1, A2>::result_type>
template <typename A1, typename A2 = A1, typename T = double>
class RangeFactor : public ExpressionFactor2<T, A1, A2> {
private:
typedef RangeFactor<A1, A2> This;

8
gtsam/slam/dataset.cpp
View File

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

4
gtsam/slam/tests/smartFactorScenarios.h
View File

@ -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<class CAMERA>
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);

30
gtsam/slam/tests/testDataset.cpp
View File

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

4
gtsam/slam/tests/testOrientedPlane3Factor.cpp
View File

@ -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<Pose3> 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<Pose3> pose_prior(init_sym, init_pose,
noiseModel::Diagonal::Sigmas(
(Vector(6) << 0.001, 0.001, 0.001, 0.001, 0.001, 0.001).finished()));

2
gtsam/slam/tests/testPoseRotationPrior.cpp
View File

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

2
gtsam/slam/tests/testPoseTranslationPrior.cpp
View File

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

4
gtsam/slam/tests/testSmartProjectionCameraFactor.cpp
View File

@ -47,7 +47,7 @@ template<class CALIBRATION>
PinholeCamera<CALIBRATION> perturbCameraPoseAndCalibration(
const PinholeCamera<CALIBRATION>& 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<CALIBRATION> 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);

56
gtsam/slam/tests/testSmartProjectionPoseFactor.cpp
View File

@ -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<Pose3>(x1, cam1.pose(), noisePrior));
graph.push_back(PriorFactor<Pose3>(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<Pose3>(x1, cam1.pose(), noisePrior));
graph.push_back(PriorFactor<Pose3>(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<Pose3>(x1, cam1.pose(), noisePrior));
graph.push_back(PriorFactor<Pose3>(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<Pose3>(x1, cam1.pose(), noisePrior));
graph.push_back(PriorFactor<Pose3>(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<Pose3>(x1, level_pose, noisePrior));
graph.push_back(PriorFactor<Pose3>(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<Pose3>(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<Pose3>(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<GaussianFactor> 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<GaussianFactor> 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<Pose3>(x1, cam1.pose(), noisePrior));
graph.push_back(PriorFactor<Pose3>(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<Pose3>(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());

2
gtsam/slam/tests/testTriangulationFactor.cpp
View File

@ -37,7 +37,7 @@ static const boost::shared_ptr<Cal3_S2> sharedCal = //
boost::make_shared<Cal3_S2>(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);

2
gtsam_unstable/dynamics/PoseRTV.cpp
View File

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

12
gtsam_unstable/dynamics/tests/testIMUSystem.cpp
View File

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

8
gtsam_unstable/dynamics/tests/testPoseRTV.cpp
View File

@ -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))));
}
/* ************************************************************************* */

2
gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp
View File

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

2
gtsam_unstable/geometry/Pose3Upright.cpp
View File

@ -64,7 +64,7 @@ Rot2 Pose3Upright::rotation2() const {
/* ************************************************************************* */
Rot3 Pose3Upright::rotation() const {
return Rot3::yaw(theta());
return Rot3::Yaw(theta());
}
/* ************************************************************************* */

4
gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp
View File

@ -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<Cal3_S2> inv_camera(Pose3(Rot3::ypr(1.5,0.1, -1.5), Point3(-5, -5, 2)),K);
InvDepthCamera3<Cal3_S2> 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;

4
gtsam_unstable/geometry/tests/testPose3Upright.cpp
View File

@ -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));
}
/* ************************************************************************* */

36
gtsam_unstable/geometry/tests/testSimilarity3.cpp
View File

@ -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<Similarity3> 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<Similarity3>(X(1), Similarity3());
initial.insert<Similarity3>(X(2), Similarity3(Rot3::ypr(M_PI/2.0, 0, 0), Point3(1, 0, 0), 1.1));
initial.insert<Similarity3>(X(3), Similarity3(Rot3::ypr(2.0*M_PI/2.0, 0, 0), Point3(0.9, 1.1, 0), 1.2));
initial.insert<Similarity3>(X(4), Similarity3(Rot3::ypr(3.0*M_PI/2.0, 0, 0), Point3(0, 1, 0), 1.3));
initial.insert<Similarity3>(X(5), Similarity3(Rot3::ypr(4.0*M_PI/2.0, 0, 0), Point3(0, 0, 0), 1.0));
initial.insert<Similarity3>(X(2), Similarity3(Rot3::Ypr(M_PI/2.0, 0, 0), Point3(1, 0, 0), 1.1));
initial.insert<Similarity3>(X(3), Similarity3(Rot3::Ypr(2.0*M_PI/2.0, 0, 0), Point3(0.9, 1.1, 0), 1.2));
initial.insert<Similarity3>(X(4), Similarity3(Rot3::Ypr(3.0*M_PI/2.0, 0, 0), Point3(0, 1, 0), 1.3));
initial.insert<Similarity3>(X(5), Similarity3(Rot3::Ypr(4.0*M_PI/2.0, 0, 0), Point3(0, 0, 0), 1.0));
//initial.print("Initial Estimate\n");

2
gtsam_unstable/slam/Mechanization_bRn2.cpp
View File

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

2
gtsam_unstable/slam/tests/testInvDepthFactor3.cpp
View File

@ -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<Pose3, Vector5, double> InverseDepthFactor;

4
gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp
View File

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

4
gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp
View File

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

4
gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp
View File

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

2
gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp
View File

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

68
gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp
View File

@ -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<Pose3>(x1, pose1, noisePrior));
graph.push_back(PriorFactor<Pose3>(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<Pose3>(x1, pose1, noisePrior));
graph.push_back(PriorFactor<Pose3>(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<Pose3>(x1, pose1, noisePrior));
graph.push_back(PriorFactor<Pose3>(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<Pose3>(x1, pose1, noisePrior));
graph.push_back(PriorFactor<Pose3>(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<Pose3>(x1, pose1, noisePrior));
// graph.push_back(PriorFactor<Pose3>(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<Pose3>(x1, pose1, noisePrior));
// graph.push_back(PriorFactor<Pose3>(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<Pose3>(x2, positionPrior, noisePriorTranslation));
// graph.push_back(PoseTranslationPrior<Pose3>(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<Pose3>(x2, positionPrior, noisePriorTranslation));
// graph.push_back(PoseTranslationPrior<Pose3>(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<GaussianFactor> 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;

2669
gtsampy.h Normal file
View File

File diff suppressed because it is too large Load Diff

1
python/.gitignore vendored Normal file
View File

@ -0,0 +1 @@
build/

96
python/CMakeLists.txt Normal file
View File

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

18
python/README.md Normal file
View File

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

1
python/gtsam/__init__.py Normal file
View File

@ -0,0 +1 @@
from ._libgtsam_python import *

36
python/gtsam_examples/OdometryExample.py Normal file
View File

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

32
python/gtsam_examples/SFMdata.py Normal file
View File

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

132
python/gtsam_examples/VisualISAM2Example.py Normal file
View File

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

2
python/gtsam_examples/__init__.py Normal file
View File

@ -0,0 +1,2 @@
from . import SFMdata
from . import VisualISAM2Example

1
python/gtsam_tests/__init__.py Normal file
View File

@ -0,0 +1 @@

13
python/gtsam_tests/testPoint2.py Normal file
View File

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

1
python/gtsam_utils/__init__.py Normal file
View File

@ -0,0 +1 @@
from .plot import *

59
python/gtsam_utils/_plot.py Normal file
View File

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

59
python/gtsam_utils/plot.py Normal file
View File

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

30
python/handwritten/CMakeLists.txt Normal file
View File

@ -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 $<TARGET_FILE:gtsam_python> ${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)

37
python/handwritten/base/FastVector.cpp Normal file
View File

@ -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 <boost/python.hpp>
#include <boost/python/suite/indexing/vector_indexing_suite.hpp>
#define NO_IMPORT_ARRAY
#include <numpy_eigen/NumpyEigenConverter.hpp>
#include "gtsam/base/FastVector.h"
#include "gtsam/base/types.h" // for Key definition
using namespace boost::python;
using namespace gtsam;
void exportFastVectors(){
typedef FastVector<Key> KeyVector;
class_<KeyVector>("KeyVector")
.def(vector_indexing_suite<KeyVector>())
;
}

97
python/handwritten/exportgtsam.cpp Normal file
View File

@ -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 <boost/python.hpp>
#include <boost/cstdint.hpp>
#include <numpy_eigen/NumpyEigenConverter.hpp>
// 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();
}

62
python/handwritten/geometry/Cal3_S2.cpp Normal file
View File

@ -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 <boost/python.hpp>
#define NO_IMPORT_ARRAY
#include <numpy_eigen/NumpyEigenConverter.hpp>
#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>("Cal3_S2", init<>())
.def(init<double,double,double,double,double>())
.def(init<const Vector &>())
.def(init<double,int,int>())
.def(init<std::string>())
.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())
;
}

66
python/handwritten/geometry/PinholeBaseK.cpp Normal file
View File

@ -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 <boost/python.hpp>
#define NO_IMPORT_ARRAY
#include <numpy_eigen/NumpyEigenConverter.hpp>
#include "gtsam/geometry/PinholeCamera.h"
#include "gtsam/geometry/Cal3_S2.h"
using namespace boost::python;
using namespace gtsam;
typedef PinholeBaseK<Cal3_S2> PinholeBaseKCal3_S2;
// Wrapper on PinholeBaseK<Cal3_S2> because it has pure virtual method calibration()
struct PinholeBaseKCal3_S2Callback : PinholeBaseKCal3_S2, wrapper<PinholeBaseKCal3_S2>
{
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<Cal3_S2>::value > Dcal) const = &PinholeBaseKCal3_S2::project;
Point2 (PinholeBaseKCal3_S2::*project3) (const Unit3 &pw, OptionalJacobian< 2, 6 > Dpose, OptionalJacobian< 2, 2 > Dpoint, OptionalJacobian< 2, FixedDimension<Cal3_S2>::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_S2Callback, boost::noncopyable>("PinholeBaseKCal3_S2", no_init)
.def("calibration", pure_virtual(&PinholeBaseKCal3_S2::calibration), return_value_policy<copy_const_reference>())
.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())
;
}

51
python/handwritten/geometry/PinholeCamera.cpp Normal file
View File

@ -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 <boost/python.hpp>
#define NO_IMPORT_ARRAY
#include <numpy_eigen/NumpyEigenConverter.hpp>
#include "gtsam/geometry/PinholeCamera.h"
#include "gtsam/geometry/Cal3_S2.h"
using namespace boost::python;
using namespace gtsam;
typedef PinholeBaseK<Cal3_S2> PinholeBaseKCal3_S2;
typedef PinholeCamera<Cal3_S2> 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, bases<PinholeBaseKCal3_S2> >("PinholeCameraCal3_S2", init<>())
.def(init<const Pose3 &>())
.def(init<const Pose3 &, const Cal3_S2 &>())
.def(init<const Vector &>())
.def(init<const Vector &, const Vector &>())
.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<copy_const_reference>())
// 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<copy_const_reference>())
.def("Lookat", &PinholeCameraCal3_S2::Lookat, Lookat_overloads())
.staticmethod("Lookat")
;
}

58
python/handwritten/geometry/Point2.cpp Normal file
View File

@ -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 <boost/python.hpp>
#define NO_IMPORT_ARRAY
#include <numpy_eigen/NumpyEigenConverter.hpp>
#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>("Point2", init<>())
.def(init<double, double>())
.def(init<const Vector2 &>())
.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<double>()) // __mult__
.def(other<double>() * self) // __mult__
.def(self + self)
.def(-self)
.def(self - self)
.def(self / other<double>())
.def(self_ns::str(self))
.def(repr(self))
.def(self == self)
;
}

64
python/handwritten/geometry/Point3.cpp Normal file
View File

@ -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 <boost/python.hpp>
#define NO_IMPORT_ARRAY
#include <numpy_eigen/NumpyEigenConverter.hpp>
#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>("Point3")
.def(init<>())
.def(init<double,double,double>())
.def(init<const Vector3 &>())
.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<double>())
.def(other<double>() * self)
.def(self + self)
.def(-self)
.def(self - self)
.def(self / other<double>())
.def(self_ns::str(self))
.def(repr(self))
.def(self == self)
;
}

93
python/handwritten/geometry/Pose2.cpp Normal file
View File

@ -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 <boost/python.hpp>
#define NO_IMPORT_ARRAY
#include <numpy_eigen/NumpyEigenConverter.hpp>
#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<Matrix&>, boost::optional<Matrix&>) const
// = &Pose2::range;
// double (Pose2::*range2)(const Point2&, boost::optional<Matrix&>, boost::optional<Matrix&>) const
// = &Pose2::range;
// Rot2 (Pose2::*bearing1)(const Pose2&, boost::optional<Matrix&>, boost::optional<Matrix&>) const
// = &Pose2::bearing;
// Rot2 (Pose2::*bearing2)(const Point2&, boost::optional<Matrix&>, boost::optional<Matrix&>) const
// = &Pose2::bearing;
class_<Pose2>("Pose2", init<>())
.def(init<Pose2>())
.def(init<double, double, double>())
.def(init<double, Point2>())
.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<copy_const_reference>())
.def("r", &Pose2::r, return_value_policy<copy_const_reference>())
.def("translation", &Pose2::translation, return_value_policy<copy_const_reference>())
.def("rotation", &Pose2::rotation, return_value_policy<copy_const_reference>())
// .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__
;
}

92
python/handwritten/geometry/Pose3.cpp Normal file
View File

@ -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 <boost/python.hpp>
#define NO_IMPORT_ARRAY
#include <numpy_eigen/NumpyEigenConverter.hpp>
#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>("Pose3")
.def(init<>())
.def(init<const Pose3 &>())
.def(init<const Rot3 &,const Point3 &>())
.def(init<const Rot3 &,const Vector3 &>())
.def(init<const Pose2 &>())
.def(init<const Matrix &>())
.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<copy_const_reference>()])
.def("rotation", &Pose3::rotation, return_value_policy<copy_const_reference>())
.def(self * self) // __mult__
.def(self * other<Point3>()) // __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())
;
}

65
python/handwritten/geometry/Rot2.cpp Normal file
View File

@ -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 <boost/python.hpp>
#define NO_IMPORT_ARRAY
#include <numpy_eigen/NumpyEigenConverter.hpp>
#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>("Rot2", init<>())
.def(init<double>())
.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__
;
}

111
python/handwritten/geometry/Rot3.cpp Normal file
View File

@ -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 <boost/python.hpp>
#define NO_IMPORT_ARRAY
#include <numpy_eigen/NumpyEigenConverter.hpp>
#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>("Rot3")
.def(init<Point3,Point3,Point3>())
.def(init<double,double,double,double,double,double,double,double,double>())
.def(init<const Matrix3 &>())
.def(init<const Matrix &>())
.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<Point3>())
.def(self * other<Unit3>())
.def(self_ns::str(self)) // __str__
.def(repr(self)) // __repr__
;
}

83
python/handwritten/inference/Symbol.cpp Normal file
View File

@ -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 <boost/python.hpp>
#include <boost/make_shared.hpp>
#define NO_IMPORT_ARRAY
#include <numpy_eigen/NumpyEigenConverter.hpp>
#include <sstream> // 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<Symbol> 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<Symbol>(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, boost::shared_ptr<Symbol> >("Symbol")
.def(init<>())
.def(init<const Symbol &>())
.def("__init__", make_constructor(makeSymbol))
.def(init<Key>())
.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<Key>())
.def(self != self)
.def(self != other<Key>())
.def("__repr__", &selfToString)
.def("__int__", &selfToKey)
.def("chr", &chrFromSelf)
;
}

137
python/handwritten/linear/NoiseModel.cpp Normal file
View File

@ -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 <boost/python.hpp>
#define NO_IMPORT_ARRAY
#include <numpy_eigen/NumpyEigenConverter.hpp>
#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<Base>
{
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<std::string>(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_<BaseCallback,boost::noncopyable>("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_<Gaussian, boost::shared_ptr<Gaussian>, bases<Base> >("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_<Diagonal, boost::shared_ptr<Diagonal>, bases<Gaussian> >("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_<Isotropic, boost::shared_ptr<Isotropic>, bases<Diagonal> >("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_<Unit, boost::shared_ptr<Unit>, bases<Isotropic> >("Unit", no_init)
.def("Create",&Unit::Create)
.staticmethod("Create")
;
}

67
python/handwritten/nonlinear/ISAM2.cpp Normal file
View File

@ -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 <boost/python.hpp>
#define NO_IMPORT_ARRAY
#include <numpy_eigen/NumpyEigenConverter.hpp>
#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>("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>("ISAM2Result")
;
// Function pointers for overloads in ISAM2
Values (ISAM2::*calculateEstimate_0)() const = &ISAM2::calculateEstimate;
class_<ISAM2>("ISAM2")
.def(init<const ISAM2Params &>())
// 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<Pose3>, (arg("self"), arg("key")) )
.def("value_exists", &ISAM2::valueExists)
;
}

27
python/handwritten/nonlinear/LevenbergMarquardtOptimizer.cpp Normal file
View File

@ -0,0 +1,27 @@
#include <boost/python.hpp>
#define NO_IMPORT_ARRAY
#include <numpy_eigen/NumpyEigenConverter.hpp>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
using namespace boost::python;
using namespace gtsam;
void exportLevenbergMarquardtOptimizer(){
class_<LevenbergMarquardtParams>("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>("LevenbergMarquardtOptimizer",
init<const NonlinearFactorGraph&, const Values&, const LevenbergMarquardtParams&>())
.def("optimize", &LevenbergMarquardtOptimizer::optimize, return_value_policy<copy_const_reference>())
;
}

49
python/handwritten/nonlinear/NonlinearFactor.cpp Normal file
View File

@ -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 <boost/python.hpp>
#define NO_IMPORT_ARRAY
#include <numpy_eigen/NumpyEigenConverter.hpp>
#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<NonlinearFactor>
{
double error (const Values & values) const {
return this->get_override("error")(values);
}
size_t dim () const {
return this->get_override("dim")();
}
boost::shared_ptr<GaussianFactor> linearize(const Values & values) const {
return this->get_override("linearize")(values);
}
};
void exportNonlinearFactor(){
class_<NonlinearFactorCallback,boost::noncopyable>("NonlinearFactor")
;
}

56
python/handwritten/nonlinear/NonlinearFactorGraph.cpp Normal file
View File

@ -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 <boost/python.hpp>
#define NO_IMPORT_ARRAY
#include <numpy_eigen/NumpyEigenConverter.hpp>
#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<NonlinearFactor> getNonlinearFactor(
const NonlinearFactorGraph& graph, size_t idx) {
auto p = boost::dynamic_pointer_cast<NonlinearFactor>(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>("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);
}

80
python/handwritten/nonlinear/Values.cpp Normal file
View File

@ -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 <boost/python.hpp>
#define NO_IMPORT_ARRAY
#include <numpy_eigen/NumpyEigenConverter.hpp>
#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>("Values", init<>())
.def(init<Values>())
.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<Point2>, return_value_policy<copy_const_reference>())
.def("atRot2", &Values::at<Rot2>, return_value_policy<copy_const_reference>())
.def("atPose2", &Values::at<Pose2>, return_value_policy<copy_const_reference>())
.def("atPoint3", &Values::at<Point3>, return_value_policy<copy_const_reference>())
.def("atRot3", &Values::at<Rot3>, return_value_policy<copy_const_reference>())
.def("atPose3", &Values::at<Pose3>, return_value_policy<copy_const_reference>())
.def("atConstantBias", &Values::at<Bias>, return_value_policy<copy_const_reference>())
.def("atVector3", &Values::at<Vector3>, return_value_policy<copy_const_reference>())
.def("exists", exists1)
.def("keys", &Values::keys)
;
}

16
python/handwritten/slam/BearingFactor.cpp Normal file
View File

@ -0,0 +1,16 @@
#include <boost/python.hpp>
#define NO_IMPORT_ARRAY
#include <numpy_eigen/NumpyEigenConverter.hpp>
#include <gtsam/sam/BearingFactor.h>
using namespace boost::python;
using namespace gtsam;
using namespace std;
template <class VALUE>
void exportBearingFactor(const std::string& name) {
class_<VALUE>(name, init<>());
}

57
python/handwritten/slam/BetweenFactor.cpp Normal file
View File

@ -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 <boost/python.hpp>
#define NO_IMPORT_ARRAY
#include <numpy_eigen/NumpyEigenConverter.hpp>
#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<class T>
// void exportBetweenFactor(const std::string& name){
// class_<T>(name, init<>())
// .def(init<Key, Key, T, SharedNoiseModel>())
// ;
// }
#define BETWEENFACTOR(T) \
class_< BetweenFactor<T>, bases<NonlinearFactor>, boost::shared_ptr< BetweenFactor<T> > >("BetweenFactor"#T) \
.def(init<Key,Key,T,noiseModel::Base::shared_ptr>()) \
.def("measured", &BetweenFactor<T>::measured, return_internal_reference<>()) \
;
void exportBetweenFactors()
{
BETWEENFACTOR(Point2)
BETWEENFACTOR(Rot2)
BETWEENFACTOR(Pose2)
BETWEENFACTOR(Point3)
BETWEENFACTOR(Rot3)
BETWEENFACTOR(Pose3)
}

54
python/handwritten/slam/GenericProjectionFactor.cpp Normal file
View File

@ -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 <boost/python.hpp>
#define NO_IMPORT_ARRAY
#include <numpy_eigen/NumpyEigenConverter.hpp>
#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<Pose3, Point3, Cal3_S2> 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, bases<NonlinearFactor> >("GenericProjectionFactorCal3_S2", init<>())
.def(init<const Point2 &, SharedNoiseModel, Key, Key, const boost::shared_ptr<Cal3_S2> &, optional<Pose3> >())
.def(init<const Point2 &, SharedNoiseModel, Key, Key, const boost::shared_ptr<Cal3_S2> &, bool, bool, optional<Pose3> >())
.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<copy_const_reference>())
// TODO(Ellon): Find the right return policy when returning a 'const shared_ptr<...> &'
// .def("calibration", &GenericProjectionFactorCal3_S2::calibration, return_value_policy<copy_const_reference>())
.def("verbose_cheirality", &GenericProjectionFactorCal3_S2::verboseCheirality)
.def("throw_cheirality", &GenericProjectionFactorCal3_S2::throwCheirality)
;
}

58
python/handwritten/slam/PriorFactor.cpp Normal file
View File

@ -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 <boost/python.hpp>
#define NO_IMPORT_ARRAY
#include <numpy_eigen/NumpyEigenConverter.hpp>
#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<VALUE>, bases<NonlinearFactor>, boost::shared_ptr< PriorFactor<VALUE> > >("PriorFactor"#VALUE) \
.def(init<Key,VALUE,noiseModel::Base::shared_ptr>()) \
.def("prior", &PriorFactor<VALUE>::prior, return_internal_reference<>()) \
;
void exportPriorFactors()
{
PRIORFACTOR(Point2)
PRIORFACTOR(Rot2)
PRIORFACTOR(Pose2)
PRIORFACTOR(Point3)
PRIORFACTOR(Rot3)
PRIORFACTOR(Pose3)
PRIORFACTOR(Vector3)
}

54
python/handwritten/utils/NumpyEigen.cpp Normal file
View File

@ -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 <boost/python.hpp>
#define NO_IMPORT_ARRAY
#include <numpy_eigen/NumpyEigenConverter.hpp>
#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<Vector>::register_converter();
NumpyEigenConverter<Vector1>::register_converter();
NumpyEigenConverter<Vector2>::register_converter();
NumpyEigenConverter<Vector3>::register_converter();
NumpyEigenConverter<Vector4>::register_converter();
NumpyEigenConverter<Vector5>::register_converter();
NumpyEigenConverter<Vector6>::register_converter();
NumpyEigenConverter<Vector7>::register_converter();
NumpyEigenConverter<Vector8>::register_converter();
NumpyEigenConverter<Vector9>::register_converter();
NumpyEigenConverter<Vector10>::register_converter();
NumpyEigenConverter<Matrix>::register_converter();
NumpyEigenConverter<Matrix2>::register_converter();
NumpyEigenConverter<Matrix3>::register_converter();
NumpyEigenConverter<Matrix4>::register_converter();
NumpyEigenConverter<Matrix5>::register_converter();
NumpyEigenConverter<Matrix6>::register_converter();
NumpyEigenConverter<Matrix7>::register_converter();
NumpyEigenConverter<Matrix8>::register_converter();
NumpyEigenConverter<Matrix9>::register_converter();
}

334
python/include/numpy_eigen/NumpyEigenConverter.hpp Executable file
View File

@ -0,0 +1,334 @@
/**
* @file NumpyEigenConverter.hpp
* @author Paul Furgale <paul.furgale@utoronto.ca>
* @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 <numpy_eigen/boost_python_headers.hpp>
//#include <iostream>
#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 <numpy/arrayobject.h>
#include "type_traits.hpp"
#include <boost/lexical_cast.hpp>
#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 <NumpyEigenConverter.hpp>
*
*
* BOOST_PYTHON_MODULE(libmy_module_python)
* {
* // The converters will cause a segfault unless import_array() is called before the first one
* import_array();
* NumpyEigenConverter<Eigen::Matrix< double, 1, 1 > >::register_converter();
* NumpyEigenConverter<Eigen::Matrix< double, 2, 1 > >::register_converter();
* }
*
*/
template<typename EIGEN_MATRIX_T>
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<scalar_t>::NpyType,
//Flags = ei_compute_matrix_flags<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>::ret,
//CoeffReadCost = NumTraits<Scalar>::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<std::string>(option);
}
static std::string toString()
{
return std::string() + "Eigen::Matrix<" + TypeToNumPy<scalar_t>::typeString() + ", " +
castSizeOption(RowsAtCompileTime) + ", " +
castSizeOption(ColsAtCompileTime) + ", " +
boost::lexical_cast<std::string>((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<scalar_t>::NpyType);
numpyTypeDemuxer< CopyEigenToNumpyVector<const matrix_t> >(&M, reinterpret_cast<NPE_PY_ARRAY_OBJECT*>(P));
}
else
{
// create a 2D array.
npy_intp dimensions[2];
dimensions[0] = M.rows();
dimensions[1] = M.cols();
P = PyArray_SimpleNew(2, dimensions, TypeToNumPy<scalar_t>::NpyType);
numpyTypeDemuxer< CopyEigenToNumpyMatrix<const matrix_t> >(&M, reinterpret_cast<NPE_PY_ARRAY_OBJECT*>(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<NPE_PY_ARRAY_OBJECT*>(obj_ptr);
// Check the type of the array.
int npyType = getNpyType(array_ptr);
if(!TypeToNumPy<scalar_t>::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<matrix_t> * matData = reinterpret_cast<boost::python::converter::rvalue_from_python_storage<matrix_t> * >(data);
void* storage = matData->storage.bytes;
// Make sure storage is 16byte aligned. With help from code from Memory.h
void * aligned = reinterpret_cast<void*>((reinterpret_cast<size_t>(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<NPE_PY_ARRAY_OBJECT*>(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<matrix_t> >(&M, array_ptr);
}
else
{
int rows = PyArray_DIM(array_ptr, 0);
int cols = PyArray_DIM(array_ptr, 1);
M.resize(rows,cols);
numpyTypeDemuxer< CopyNumpyToEigenMatrix<matrix_t> >(&M, array_ptr);
}
}
// The registration function.
static void register_converter()
{
boost::python::to_python_converter<matrix_t,NumpyEigenConverter>();
boost::python::converter::registry::push_back(
&NumpyEigenConverter::convertible,
&NumpyEigenConverter::construct,
boost::python::type_id<matrix_t>());
}
};
#endif /* NUMPY_EIGEN_CONVERTER_HPP */

6
python/include/numpy_eigen/README.md Normal file
View File

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

250
python/include/numpy_eigen/boost_python_headers.hpp Executable file
View File

@ -0,0 +1,250 @@
/**
* @file boost_python_headers.hpp
* @author Paul Furgale <paul.furgale@gmail.com>
* @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 <Eigen/Core>
#include <boost/python.hpp>
#include <boost/python/detail/referent_storage.hpp>
#include <boost/python/converter/arg_from_python.hpp>
#include <boost/python/converter/rvalue_from_python_data.hpp>
#include <boost/python/tuple.hpp>
namespace boost { namespace python { namespace detail {
template<typename T>
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<typename T, int A, int B, int C, int D, int E>
struct referent_size< Eigen::Matrix<T,A,B,C,D,E>& >
{
// Add 16 bytes so we can get alignment
BOOST_STATIC_CONSTANT( std::size_t, value = sizeof(Eigen::Matrix<T,A,B,C,D,E>) + 16);
};
// This bit of code makes sure we have 16 extra bytes to do the pointer alignment for fixed-sized Eigen types
template<typename T, int A, int B, int C, int D, int E>
struct referent_size< Eigen::Matrix<T,A,B,C,D,E> const & >
{
// Add 16 bytes so we can get alignment
BOOST_STATIC_CONSTANT( std::size_t, value = sizeof(Eigen::Matrix<T,A,B,C,D,E>) + 16);
};
// This bit of code makes sure we have 16 extra bytes to do the pointer alignment for fixed-sized Eigen types
template<typename T, int A, int B, int C, int D, int E>
struct referent_size< Eigen::Matrix<T,A,B,C,D,E> >
{
// Add 16 bytes so we can get alignment
BOOST_STATIC_CONSTANT( std::size_t, value = sizeof(Eigen::Matrix<T,A,B,C,D,E>) + 16);
};
}}}
namespace boost { namespace python { namespace converter {
template<typename S, int A, int B, int C, int D, int E>
struct rvalue_from_python_data< Eigen::Matrix<S,A,B,C,D,E> const &> : rvalue_from_python_storage< Eigen::Matrix<S,A,B,C,D,E> const & >
{
typedef typename Eigen::Matrix<S,A,B,C,D,E> 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<T>,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<void *>(this->storage.bytes);
T * aligned = reinterpret_cast<T *>(reinterpret_cast<void *>((reinterpret_cast<size_t>(storage) & ~(size_t(15))) + 16));
//std::cout << "Destroying " << (void*)aligned << std::endl;
aligned->T::~T();
}
}
private:
typedef typename add_reference<typename add_cv<T>::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<typename S, int A, int B, int C, int D, int E>
struct arg_rvalue_from_python< Eigen::Matrix<S,A,B,C,D,E> >
{
typedef Eigen::Matrix<S,A,B,C,D,E> const & T;
typedef typename boost::add_reference<
T
// We can't add_const here, or it would be impossible to pass
// auto_ptr<U> args from Python to C++
>::type result_type;
arg_rvalue_from_python(PyObject * obj) : m_data(converter::rvalue_from_python_stage1(obj, registered<T>::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<T>::
# 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<void *>(m_data.storage.bytes);
void * aligned = reinterpret_cast<void*>((reinterpret_cast<size_t>(storage) & ~(size_t(15))) + 16);
return python::detail::void_ptr_to_reference(aligned, (result_type(*)())0);
}
private:
rvalue_from_python_data<result_type> 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<typename S, int A, int B, int C, int D, int E>
struct arg_rvalue_from_python< Eigen::Matrix<S,A,B,C,D,E> const & >
{
typedef Eigen::Matrix<S,A,B,C,D,E> const & T;
typedef typename boost::add_reference<
T
// We can't add_const here, or it would be impossible to pass
// auto_ptr<U> args from Python to C++
>::type result_type;
arg_rvalue_from_python(PyObject * obj) : m_data(converter::rvalue_from_python_stage1(obj, registered<T>::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<T>::
# 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<void *>(m_data.storage.bytes);
void * aligned = reinterpret_cast<void*>((reinterpret_cast<size_t>(storage) & ~(size_t(15))) + 16);
return python::detail::void_ptr_to_reference(aligned, (result_type(*)())0);
}
private:
rvalue_from_python_data<result_type> 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<typename S, int A, int B, int C, int D, int E>
struct arg_rvalue_from_python< Eigen::Matrix<S,A,B,C,D,E> const >
{
typedef Eigen::Matrix<S,A,B,C,D,E> const & T;
typedef typename boost::add_reference<
T
// We can't add_const here, or it would be impossible to pass
// auto_ptr<U> args from Python to C++
>::type result_type;
arg_rvalue_from_python(PyObject * obj) : m_data(converter::rvalue_from_python_stage1(obj, registered<T>::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<T>::
# 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<void *>(m_data.storage.bytes);
void * aligned = reinterpret_cast<void*>((reinterpret_cast<size_t>(storage) & ~(size_t(15))) + 16);
return python::detail::void_ptr_to_reference(aligned, (result_type(*)())0);
}
private:
rvalue_from_python_data<result_type> m_data;
PyObject* m_source;
};
}}}
#endif /* NUMPY_EIGEN_CONVERTERS_HPP */

149
python/include/numpy_eigen/copy_routines.hpp Executable file
View File

@ -0,0 +1,149 @@
#ifndef NUMPY_EIGEN_COPY_ROUTINES_HPP
#define NUMPY_EIGEN_COPY_ROUTINES_HPP
template<typename EIGEN_T>
struct CopyNumpyToEigenMatrix
{
typedef EIGEN_T matrix_t;
typedef typename matrix_t::Scalar scalar_t;
template<typename T>
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<T*>(PyArray_GETPTR2(P_, r, c));
(*M_)(r,c) = static_cast<scalar_t>(*p);
}
}
}
};
template<typename EIGEN_T>
struct CopyEigenToNumpyMatrix
{
typedef EIGEN_T matrix_t;
typedef typename matrix_t::Scalar scalar_t;
template<typename T>
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<T*>(PyArray_GETPTR2(P_, r, c));
*p = static_cast<T>((*M_)(r,c));
}
}
}
};
template<typename EIGEN_T>
struct CopyEigenToNumpyVector
{
typedef EIGEN_T matrix_t;
typedef typename matrix_t::Scalar scalar_t;
template<typename T>
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<T*>(PyArray_GETPTR1(P_, i));
*p = static_cast<T>((*M_)(i));
}
}
};
template<typename EIGEN_T>
struct CopyNumpyToEigenVector
{
typedef EIGEN_T matrix_t;
typedef typename matrix_t::Scalar scalar_t;
template<typename T>
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<T*>(PyArray_GETPTR1(P_, i));
(*M_)(i) = static_cast<scalar_t>(*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<bool>(M,P);
break;
case NPY_BYTE:
f.template exec<char>(M,P);
break;
case NPY_UBYTE:
f.template exec<unsigned char>(M,P);
break;
case NPY_SHORT:
f.template exec<short>(M,P);
break;
case NPY_USHORT:
f.template exec<unsigned short>(M,P);
break;
case NPY_INT:
f.template exec<int>(M,P);
break;
case NPY_UINT:
f.template exec<unsigned int>(M,P);
break;
case NPY_LONG:
f.template exec<long>(M,P);
break;
case NPY_ULONG:
f.template exec<unsigned long>(M,P);
break;
case NPY_LONGLONG:
f.template exec<long long>(M,P);
break;
case NPY_ULONGLONG:
f.template exec<unsigned long long>(M,P);
break;
case NPY_FLOAT:
f.template exec<float>(M,P);
break;
case NPY_DOUBLE:
f.template exec<double>(M,P);
break;
case NPY_LONGDOUBLE:
f.template exec<long double>(M,P);
break;
default:
THROW_TYPE_ERROR("Unsupported type: " << npyTypeToString(npyType));
}
}
#endif /* NUMPY_EIGEN_COPY_ROUTINES_HPP */

Some files were not shown because too many files have changed in this diff Show More