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

Merged from branch 'branches/windows'

release/4.3a0
Richard Roberts 2012-06-05 14:09:58 +00:00
parent 7b48e56d56 90578e2532
commit bef45d5b7b
101 changed files with 739 additions and 609 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

51
CMakeLists.txt
View File

@ -1,3 +1,4 @@
project(GTSAM CXX C) project(GTSAM CXX C)
cmake_minimum_required(VERSION 2.6) cmake_minimum_required(VERSION 2.6)
@ -9,8 +10,12 @@ set (GTSAM_VERSION_PATCH 0)
# Set the default install path to home # Set the default install path to home
#set (CMAKE_INSTALL_PREFIX ${HOME} CACHE PATH "Install prefix for library") #set (CMAKE_INSTALL_PREFIX ${HOME} CACHE PATH "Install prefix for library")
set(CMAKE_MODULE_PATH "${CMAKE_MODULE_PATH}" "${CMAKE_CURRENT_SOURCE_DIR}/cmake")
# Load build type flags and default to Debug mode
include(GtsamBuildTypes)
# Use macros for creating tests/timing scripts # Use macros for creating tests/timing scripts
set(CMAKE_MODULE_PATH "${CMAKE_MODULE_PATH}" "${PROJECT_SOURCE_DIR}/cmake")
include(GtsamTesting) include(GtsamTesting)
include(GtsamPrinting) include(GtsamPrinting)
@ -26,24 +31,6 @@ else()
set(GTSAM_UNSTABLE_AVAILABLE 0) set(GTSAM_UNSTABLE_AVAILABLE 0)
endif() endif()
# Load build type flags and default to Debug mode
include(GtsamBuildTypes)
# Check build types
if(${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} VERSION_GREATER 2.8 OR ${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} VERSION_EQUAL 2.8)
set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS None Debug Release Timing Profiling RelWithDebInfo MinSizeRel)
endif()
string(TOLOWER "${CMAKE_BUILD_TYPE}" cmake_build_type_tolower)
if( NOT cmake_build_type_tolower STREQUAL ""
AND NOT cmake_build_type_tolower STREQUAL "none"
AND NOT cmake_build_type_tolower STREQUAL "debug"
AND NOT cmake_build_type_tolower STREQUAL "release"
AND NOT cmake_build_type_tolower STREQUAL "timing"
AND NOT cmake_build_type_tolower STREQUAL "profiling"
AND NOT cmake_build_type_tolower STREQUAL "relwithdebinfo")
message(FATAL_ERROR "Unknown build type \"${CMAKE_BUILD_TYPE}\". Allowed values are None, Debug, Release, Timing, Profiling, RelWithDebInfo (case-insensitive).")
endif()
# Configurable Options # Configurable Options
option(GTSAM_BUILD_TESTS "Enable/Disable building of tests" ON) option(GTSAM_BUILD_TESTS "Enable/Disable building of tests" ON)
option(GTSAM_BUILD_TIMING "Enable/Disable building of timing scripts" ON) option(GTSAM_BUILD_TIMING "Enable/Disable building of timing scripts" ON)
@ -52,10 +39,18 @@ if(GTSAM_UNSTABLE_AVAILABLE)
option(GTSAM_BUILD_UNSTABLE "Enable/Disable libgtsam_unstable" OFF) option(GTSAM_BUILD_UNSTABLE "Enable/Disable libgtsam_unstable" OFF)
endif() endif()
option(GTSAM_BUILD_WRAP "Enable/Disable building of matlab wrap utility (necessary for matlab interface)" ON) option(GTSAM_BUILD_WRAP "Enable/Disable building of matlab wrap utility (necessary for matlab interface)" ON)
option(GTSAM_BUILD_SHARED_LIBRARY "Enable/Disable building of a shared version of gtsam" ON) if(MSVC)
option(GTSAM_BUILD_SHARED_LIBRARY "Enable/Disable building of a shared version of gtsam" OFF)
else()
option(GTSAM_BUILD_SHARED_LIBRARY "Enable/Disable building of a shared version of gtsam" ON)
endif()
option(GTSAM_BUILD_STATIC_LIBRARY "Enable/Disable building of a static version of gtsam" ON) option(GTSAM_BUILD_STATIC_LIBRARY "Enable/Disable building of a static version of gtsam" ON)
option(GTSAM_USE_QUATERNIONS "Enable/Disable using an internal Quaternion representation for rotations instead of rotation matrices" OFF) option(GTSAM_USE_QUATERNIONS "Enable/Disable using an internal Quaternion representation for rotations instead of rotation matrices" OFF)
option(GTSAM_BUILD_CONVENIENCE_LIBRARIES "Enable/Disable use of convenience libraries for faster development rebuilds, but slower install" ON) if(MSVC)
option(GTSAM_BUILD_CONVENIENCE_LIBRARIES "Enable/Disable use of convenience libraries for faster development rebuilds, but slower install" OFF)
else()
option(GTSAM_BUILD_CONVENIENCE_LIBRARIES "Enable/Disable use of convenience libraries for faster development rebuilds, but slower install" ON)
endif()
option(GTSAM_INSTALL_MATLAB_TOOLBOX "Enable/Disable installation of matlab toolbox" ON) option(GTSAM_INSTALL_MATLAB_TOOLBOX "Enable/Disable installation of matlab toolbox" ON)
option(GTSAM_INSTALL_MATLAB_EXAMPLES "Enable/Disable installation of matlab examples" ON) option(GTSAM_INSTALL_MATLAB_EXAMPLES "Enable/Disable installation of matlab examples" ON)
option(GTSAM_INSTALL_MATLAB_TESTS "Enable/Disable installation of matlab tests" ON) option(GTSAM_INSTALL_MATLAB_TESTS "Enable/Disable installation of matlab tests" ON)
@ -80,8 +75,8 @@ endif()
# Add the Quaternion Build Flag if requested # Add the Quaternion Build Flag if requested
if (GTSAM_USE_QUATERNIONS) if (GTSAM_USE_QUATERNIONS)
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -DGTSAM_DEFAULT_QUATERNIONS") set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -DGTSAM_DEFAULT_QUATERNIONS")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DGTSAM_DEFAULT_QUATERNIONS") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DGTSAM_DEFAULT_QUATERNIONS")
endif(GTSAM_USE_QUATERNIONS) endif(GTSAM_USE_QUATERNIONS)
# Avoid building non-installed exes and unit tests when installing # Avoid building non-installed exes and unit tests when installing
@ -102,7 +97,10 @@ if (GTSAM_BUILD_TESTS)
endif() endif()
# Find boost # Find boost
find_package(Boost 1.40 COMPONENTS serialization REQUIRED) if(CYGWIN OR MSVC OR WIN32)
set(Boost_USE_STATIC_LIBS 1)
endif()
find_package(Boost 1.40 COMPONENTS serialization system chrono filesystem thread REQUIRED)
# General build settings # General build settings
include_directories( include_directories(
@ -137,6 +135,11 @@ if (GTSAM_BUILD_UNSTABLE)
add_subdirectory(gtsam_unstable) add_subdirectory(gtsam_unstable)
endif(GTSAM_BUILD_UNSTABLE) endif(GTSAM_BUILD_UNSTABLE)
# Make config files
include(GtsamMakeConfigFile)
GtsamMakeConfigFile(GTSAM gtsam-static)
GtsamMakeConfigFile(CppUnitLite CppUnitLite)
# Set up CPack # Set up CPack
set(CPACK_PACKAGE_DESCRIPTION_SUMMARY "GTSAM") set(CPACK_PACKAGE_DESCRIPTION_SUMMARY "GTSAM")
set(CPACK_PACKAGE_VENDOR "Frank Dellaert, Georgia Institute of Technology") set(CPACK_PACKAGE_VENDOR "Frank Dellaert, Georgia Institute of Technology")

2
CppUnitLite/CMakeLists.txt
View File

@ -5,6 +5,8 @@ file(GLOB cppunitlite_src "*.cpp")
add_library(CppUnitLite STATIC ${cppunitlite_src}) add_library(CppUnitLite STATIC ${cppunitlite_src})
gtsam_assign_source_folders("${cppunitlite_headers};${cppunitlite_src}") # MSVC project structure
option(GTSAM_INSTALL_CPPUNITLITE "Enable/Disable installation of CppUnitLite library" ON) option(GTSAM_INSTALL_CPPUNITLITE "Enable/Disable installation of CppUnitLite library" ON)
if (GTSAM_INSTALL_CPPUNITLITE) if (GTSAM_INSTALL_CPPUNITLITE)
install(FILES ${cppunitlite_headers} DESTINATION include/CppUnitLite) install(FILES ${cppunitlite_headers} DESTINATION include/CppUnitLite)

18
examples/CMakeLists.txt
View File

@ -1,4 +1,6 @@
add_custom_target(examples) if(NOT MSVC)
add_custom_target(examples)
endif()
# Build example executables # Build example executables
FILE(GLOB example_srcs "*.cpp") FILE(GLOB example_srcs "*.cpp")
@ -6,7 +8,9 @@ foreach(example_src ${example_srcs} )
get_filename_component(example_base ${example_src} NAME_WE) get_filename_component(example_base ${example_src} NAME_WE)
set( example_bin ${example_base} ) set( example_bin ${example_base} )
message(STATUS "Adding Example ${example_bin}") message(STATUS "Adding Example ${example_bin}")
add_dependencies(examples ${example_bin}) if(NOT MSVC)
add_dependencies(examples ${example_bin})
endif()
add_executable(${example_bin} ${example_src}) add_executable(${example_bin} ${example_src})
# Disable building during make all/install # Disable building during make all/install
@ -15,6 +19,14 @@ foreach(example_src ${example_srcs} )
endif() endif()
target_link_libraries(${example_bin} gtsam-static) target_link_libraries(${example_bin} gtsam-static)
add_custom_target(${example_bin}.run ${EXECUTABLE_OUTPUT_PATH}${example_bin} ${ARGN}) if(NOT MSVC)
add_custom_target(${example_bin}.run ${EXECUTABLE_OUTPUT_PATH}${example_bin} ${ARGN})
endif()
# Set up Visual Studio folder
if(MSVC)
set_property(TARGET ${example_bin} PROPERTY FOLDER "Examples")
endif()
endforeach(example_src) endforeach(example_src)

2
examples/PlanarSLAMExample.cpp
View File

@ -60,7 +60,7 @@ int main(int argc, char** argv) {
Rot2 bearing11 = Rot2::fromDegrees(45), Rot2 bearing11 = Rot2::fromDegrees(45),
bearing21 = Rot2::fromDegrees(90), bearing21 = Rot2::fromDegrees(90),
bearing32 = Rot2::fromDegrees(90); bearing32 = Rot2::fromDegrees(90);
double range11 = sqrt(4+4), double range11 = std::sqrt(4.0+4.0),
range21 = 2.0, range21 = 2.0,
range32 = 2.0; range32 = 2.0;

2
examples/PlanarSLAMExample_selfcontained.cpp
View File

@ -83,7 +83,7 @@ int main(int argc, char** argv) {
Rot2 bearing11 = Rot2::fromDegrees(45), Rot2 bearing11 = Rot2::fromDegrees(45),
bearing21 = Rot2::fromDegrees(90), bearing21 = Rot2::fromDegrees(90),
bearing32 = Rot2::fromDegrees(90); bearing32 = Rot2::fromDegrees(90);
double range11 = sqrt(4+4), double range11 = sqrt(4.0+4.0),
range21 = 2.0, range21 = 2.0,
range32 = 2.0; range32 = 2.0;

1
gtsam.h
View File

@ -253,6 +253,7 @@ class Rot3 {
double pitch() const; double pitch() const;
double yaw() const; double yaw() const;
// Vector toQuaternion() const; // FIXME: Can't cast to Vector properly // Vector toQuaternion() const; // FIXME: Can't cast to Vector properly
Matrix matrix() const;
}; };
class Pose2 { class Pose2 {

22
gtsam/CMakeLists.txt
View File

@ -19,9 +19,10 @@ add_subdirectory(3rdparty)
# build convenience library # build convenience library
set (3rdparty_srcs set (3rdparty_srcs
3rdparty/CCOLAMD/Source/ccolamd.c ${CMAKE_CURRENT_SOURCE_DIR}/3rdparty/CCOLAMD/Source/ccolamd.c
3rdparty/CCOLAMD/Source/ccolamd_global.c ${CMAKE_CURRENT_SOURCE_DIR}/3rdparty/CCOLAMD/Source/ccolamd_global.c
3rdparty/UFconfig/UFconfig.c) ${CMAKE_CURRENT_SOURCE_DIR}/3rdparty/UFconfig/UFconfig.c)
gtsam_assign_source_folders("${3rdparty_srcs}") # Create MSVC structure
if (GTSAM_BUILD_CONVENIENCE_LIBRARIES) if (GTSAM_BUILD_CONVENIENCE_LIBRARIES)
message(STATUS "Building Convenience Library: ccolamd") message(STATUS "Building Convenience Library: ccolamd")
add_library(ccolamd STATIC ${3rdparty_srcs}) add_library(ccolamd STATIC ${3rdparty_srcs})
@ -46,6 +47,9 @@ endif()
foreach(subdir ${gtsam_subdirs}) foreach(subdir ${gtsam_subdirs})
# Build convenience libraries # Build convenience libraries
file(GLOB subdir_srcs "${subdir}/*.cpp") file(GLOB subdir_srcs "${subdir}/*.cpp")
file(GLOB subdir_headers "${subdir}/*.h")
set(subdir_srcs ${subdir_srcs} ${subdir_headers}) # Include header files so they show up in Visual Studio
gtsam_assign_source_folders("${subdir_srcs}") # Create MSVC structure
list(REMOVE_ITEM subdir_srcs ${excluded_sources}) list(REMOVE_ITEM subdir_srcs ${excluded_sources})
set(${subdir}_srcs ${subdir_srcs}) set(${subdir}_srcs ${subdir_srcs})
if (GTSAM_BUILD_CONVENIENCE_LIBRARIES) if (GTSAM_BUILD_CONVENIENCE_LIBRARIES)
@ -81,22 +85,32 @@ message(STATUS "Install prefix: ${CMAKE_INSTALL_PREFIX}")
if (GTSAM_BUILD_STATIC_LIBRARY) if (GTSAM_BUILD_STATIC_LIBRARY)
message(STATUS "Building GTSAM - static") message(STATUS "Building GTSAM - static")
add_library(gtsam-static STATIC ${gtsam_srcs}) add_library(gtsam-static STATIC ${gtsam_srcs})
target_link_libraries(gtsam-static ${Boost_LIBRARIES})
set_target_properties(gtsam-static PROPERTIES set_target_properties(gtsam-static PROPERTIES
OUTPUT_NAME gtsam OUTPUT_NAME gtsam
CLEAN_DIRECT_OUTPUT 1 CLEAN_DIRECT_OUTPUT 1
VERSION ${gtsam_version} VERSION ${gtsam_version}
SOVERSION ${gtsam_soversion}) SOVERSION ${gtsam_soversion})
if(MSVC)
set_target_properties(gtsam-static PROPERTIES
COMPILE_FLAGS "/MP")
endif()
install(TARGETS gtsam-static ARCHIVE DESTINATION lib) install(TARGETS gtsam-static ARCHIVE DESTINATION lib)
endif (GTSAM_BUILD_STATIC_LIBRARY) endif (GTSAM_BUILD_STATIC_LIBRARY)
if (GTSAM_BUILD_SHARED_LIBRARY) if (GTSAM_BUILD_SHARED_LIBRARY)
message(STATUS "Building GTSAM - shared") message(STATUS "Building GTSAM - shared")
add_library(gtsam-shared SHARED ${gtsam_srcs}) add_library(gtsam-shared SHARED ${gtsam_srcs})
target_link_libraries(gtsam-shared ${Boost_LIBRARIES})
set_target_properties(gtsam-shared PROPERTIES set_target_properties(gtsam-shared PROPERTIES
OUTPUT_NAME gtsam OUTPUT_NAME gtsam
CLEAN_DIRECT_OUTPUT 1 CLEAN_DIRECT_OUTPUT 1
VERSION ${gtsam_version} VERSION ${gtsam_version}
SOVERSION ${gtsam_soversion}) SOVERSION ${gtsam_soversion})
install(TARGETS gtsam-shared LIBRARY DESTINATION lib ) if(MSVC)
set_target_properties(gtsam-shared PROPERTIES
COMPILE_FLAGS "/MP")
endif()
install(TARGETS gtsam-shared LIBRARY DESTINATION lib ARCHIVE DESTINATION lib RUNTIME DESTINATION bin)
endif(GTSAM_BUILD_SHARED_LIBRARY) endif(GTSAM_BUILD_SHARED_LIBRARY)

2
gtsam/base/DerivedValue.h
View File

@ -50,7 +50,7 @@ public:
* Destroy and deallocate this object, only if it was originally allocated using clone_(). * Destroy and deallocate this object, only if it was originally allocated using clone_().
*/ */
virtual void deallocate_() const { virtual void deallocate_() const {
this->~Value(); this->Value::~Value();
boost::singleton_pool<PoolTag, sizeof(DERIVED)>::free((void*)this); boost::singleton_pool<PoolTag, sizeof(DERIVED)>::free((void*)this);
} }

2
gtsam/base/LieVector.cpp
View File

@ -15,7 +15,7 @@
* @author Alex Cunningham * @author Alex Cunningham
*/ */
#include <stdarg.h> #include <cstdarg>
#include <gtsam/base/LieVector.h> #include <gtsam/base/LieVector.h>
using namespace std; using namespace std;

32
gtsam/base/Matrix.cpp
View File

@ -15,6 +15,7 @@
* @author Christian Potthast * @author Christian Potthast
*/ */
#include <gtsam/base/types.h>
#include <gtsam/base/Matrix.h> #include <gtsam/base/Matrix.h>
#include <gtsam/base/timing.h> #include <gtsam/base/timing.h>
#include <gtsam/base/Vector.h> #include <gtsam/base/Vector.h>
@ -25,8 +26,8 @@
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp> #include <boost/tuple/tuple.hpp>
#include <stdarg.h> #include <cstdarg>
#include <string.h> #include <cstring>
#include <iomanip> #include <iomanip>
#include <list> #include <list>
#include <fstream> #include <fstream>
@ -235,8 +236,31 @@ void save(const Matrix& A, const string &s, const string& filename) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
void insertSub(Matrix& big, const Matrix& small, size_t i, size_t j) { //istream& operator>>(istream& inputStream, Matrix& destinationMatrix) {
big.block(i, j, small.rows(), small.cols()) = small; // destinationMatrix.resize(0,0);
// string line;
// bool first = true;
// while(getline(inputStream, line)) {
// // Read coefficients from file
// vector<double> coeffs;
// std::copy(istream_iterator<double>(stringstream(line)), istream_iterator<double>(), coeffs.end());
// if(first) {
// destinationMatrix.resize(1,
// }
// if(coeffs.size() != dimLatent()) {
// cout << "Error reading motion file, latent variable dimension does not match file" << endl;
// exit(1);
// }
//
// // Copy coefficients to alignment matrix
// alignment_.conservativeResize(alignment_.rows() + 1, dimLatent());
// alignment_.row(alignment_.rows() - 1) = Eigen::Map<Vector>(&coeffs[0], dimLatent()).transpose();
// }
//}
/* ************************************************************************* */
void insertSub(Matrix& fullMatrix, const Matrix& subMatrix, size_t i, size_t j) {
fullMatrix.block(i, j, subMatrix.rows(), subMatrix.cols()) = subMatrix;
} }
/* ************************************************************************* */ /* ************************************************************************* */

16
gtsam/base/Matrix.h
View File

@ -26,6 +26,7 @@
#include <gtsam/3rdparty/Eigen/Eigen/QR> #include <gtsam/3rdparty/Eigen/Eigen/QR>
#include <boost/format.hpp> #include <boost/format.hpp>
#include <boost/tuple/tuple.hpp> #include <boost/tuple/tuple.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
/** /**
* Matrix is a typedef in the gtsam namespace * Matrix is a typedef in the gtsam namespace
@ -87,7 +88,7 @@ bool equal_with_abs_tol(const Eigen::DenseBase<MATRIX>& A, const Eigen::DenseBas
for(size_t i=0; i<m1; i++) for(size_t i=0; i<m1; i++)
for(size_t j=0; j<n1; j++) { for(size_t j=0; j<n1; j++) {
if(std::isnan(A(i,j)) xor std::isnan(B(i,j))) if(boost::math::isnan(A(i,j)) ^ boost::math::isnan(B(i,j)))
return false; return false;
else if(fabs(A(i,j) - B(i,j)) > tol) else if(fabs(A(i,j) - B(i,j)) > tol)
return false; return false;
@ -182,6 +183,13 @@ void print(const Matrix& A, const std::string& s = "", std::ostream& stream = st
*/ */
void save(const Matrix& A, const std::string &s, const std::string& filename); void save(const Matrix& A, const std::string &s, const std::string& filename);
/**
* Read a matrix from an input stream, such as a file. Entries can be either
* tab-, space-, or comma-separated, similar to the format read by the MATLAB
* dlmread command.
*/
//istream& operator>>(istream& inputStream, Matrix& destinationMatrix);
/** /**
* extract submatrix, slice semantics, i.e. range = [i1,i2[ excluding i2 * extract submatrix, slice semantics, i.e. range = [i1,i2[ excluding i2
* @param A matrix * @param A matrix
@ -200,12 +208,12 @@ Eigen::Block<const MATRIX> sub(const MATRIX& A, size_t i1, size_t i2, size_t j1,
/** /**
* insert a submatrix IN PLACE at a specified location in a larger matrix * insert a submatrix IN PLACE at a specified location in a larger matrix
* NOTE: there is no size checking * NOTE: there is no size checking
* @param large matrix to be updated * @param fullMatrix matrix to be updated
* @param small matrix to be inserted * @param subMatrix matrix to be inserted
* @param i is the row of the upper left corner insert location * @param i is the row of the upper left corner insert location
* @param j is the column of the upper left corner insert location * @param j is the column of the upper left corner insert location
*/ */
void insertSub(Matrix& big, const Matrix& small, size_t i, size_t j); void insertSub(Matrix& fullMatrix, const Matrix& subMatrix, size_t i, size_t j);
/** /**
* Extracts a column view from a matrix that avoids a copy * Extracts a column view from a matrix that avoids a copy

37
gtsam/base/Vector.cpp
View File

@ -16,7 +16,7 @@
* @author Frank Dellaert * @author Frank Dellaert
*/ */
#include <stdarg.h> #include <cstdarg>
#include <limits> #include <limits>
#include <iostream> #include <iostream>
#include <fstream> #include <fstream>
@ -25,16 +25,17 @@
#include <cmath> #include <cmath>
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
#include <boost/optional.hpp> #include <boost/optional.hpp>
#include <stdio.h> #include <cstdio>
#ifdef WIN32
#include <Windows.h>
#endif
#include <boost/random/normal_distribution.hpp> #include <boost/random/normal_distribution.hpp>
#include <boost/random/variate_generator.hpp> #include <boost/random/variate_generator.hpp>
#include <gtsam/base/Vector.h> #include <gtsam/base/Vector.h>
#include <gtsam/base/types.h>
//#ifdef WIN32
//#include <Windows.h>
//#endif
using namespace std; using namespace std;
@ -55,11 +56,11 @@ void odprintf_(const char *format, ostream& stream, ...) {
#endif #endif
va_end(args); va_end(args);
#ifdef WIN32 //#ifdef WIN32
OutputDebugString(buf); // OutputDebugString(buf);
#else //#else
stream << buf; stream << buf;
#endif //#endif
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -76,11 +77,11 @@ void odprintf(const char *format, ...) {
#endif #endif
va_end(args); va_end(args);
#ifdef WIN32 //#ifdef WIN32
OutputDebugString(buf); // OutputDebugString(buf);
#else //#else
cout << buf; cout << buf;
#endif //#endif
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -169,7 +170,7 @@ bool equal_with_abs_tol(const Vector& vec1, const Vector& vec2, double tol) {
if (vec1.size()!=vec2.size()) return false; if (vec1.size()!=vec2.size()) return false;
size_t m = vec1.size(); size_t m = vec1.size();
for(size_t i=0; i<m; ++i) { for(size_t i=0; i<m; ++i) {
if(isnan(vec1[i]) xor isnan(vec2[i])) if(isnan(vec1[i]) ^ isnan(vec2[i]))
return false; return false;
if(fabs(vec1[i] - vec2[i]) > tol) if(fabs(vec1[i] - vec2[i]) > tol)
return false; return false;
@ -182,7 +183,7 @@ bool equal_with_abs_tol(const SubVector& vec1, const SubVector& vec2, double tol
if (vec1.size()!=vec2.size()) return false; if (vec1.size()!=vec2.size()) return false;
size_t m = vec1.size(); size_t m = vec1.size();
for(size_t i=0; i<m; ++i) { for(size_t i=0; i<m; ++i) {
if(isnan(vec1[i]) xor isnan(vec2[i])) if(isnan(vec1[i]) ^ isnan(vec2[i]))
return false; return false;
if(fabs(vec1[i] - vec2[i]) > tol) if(fabs(vec1[i] - vec2[i]) > tol)
return false; return false;
@ -250,8 +251,8 @@ ConstSubVector sub(const Vector &v, size_t i1, size_t i2) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
void subInsert(Vector& big, const Vector& small, size_t i) { void subInsert(Vector& fullVector, const Vector& subVector, size_t i) {
big.segment(i, small.size()) = small; fullVector.segment(i, subVector.size()) = subVector;
} }
/* ************************************************************************* */ /* ************************************************************************* */

9
gtsam/base/Vector.h
View File

@ -22,7 +22,8 @@
#include <list> #include <list>
#include <vector> #include <vector>
#include <gtsam/3rdparty/Eigen/Eigen/Core> #include <gtsam/base/types.h>
#include <gtsam/3rdparty/Eigen/Eigen/Dense>
#include <boost/random/linear_congruential.hpp> #include <boost/random/linear_congruential.hpp>
/** /**
@ -208,11 +209,11 @@ ConstSubVector sub(const Vector &v, size_t i1, size_t i2);
/** /**
* Inserts a subvector into a vector IN PLACE * Inserts a subvector into a vector IN PLACE
* @param big is the vector to be changed * @param fullVector is the vector to be changed
* @param small is the vector to insert * @param subVector is the vector to insert
* @param i is the index where the subvector should be inserted * @param i is the index where the subvector should be inserted
*/ */
void subInsert(Vector& big, const Vector& small, size_t i); void subInsert(Vector& fullVector, const Vector& subVector, size_t i);
/** /**
* elementwise multiplication * elementwise multiplication

13
gtsam/base/cholesky.cpp
View File

@ -16,6 +16,10 @@
* @date Nov 5, 2010 * @date Nov 5, 2010
*/ */
#include <functional>
#include <boost/format.hpp>
#include <boost/bind.hpp>
#include <gtsam/base/debug.h> #include <gtsam/base/debug.h>
#include <gtsam/base/cholesky.h> #include <gtsam/base/cholesky.h>
#include <gtsam/base/timing.h> #include <gtsam/base/timing.h>
@ -23,9 +27,6 @@
#include <gtsam/3rdparty/Eigen/Eigen/Core> #include <gtsam/3rdparty/Eigen/Eigen/Core>
#include <gtsam/3rdparty/Eigen/Eigen/Dense> #include <gtsam/3rdparty/Eigen/Eigen/Dense>
#include <boost/format.hpp>
#include <boost/bind.hpp>
using namespace std; using namespace std;
namespace gtsam { namespace gtsam {
@ -129,7 +130,7 @@ void choleskyPartial(Matrix& ABC, size_t nFrontal) {
tic(1, "lld"); tic(1, "lld");
ABC.block(0,0,nFrontal,nFrontal).triangularView<Eigen::Upper>() = ABC.block(0,0,nFrontal,nFrontal).triangularView<Eigen::Upper>() =
ABC.block(0,0,nFrontal,nFrontal).selfadjointView<Eigen::Upper>().llt().matrixU(); ABC.block(0,0,nFrontal,nFrontal).selfadjointView<Eigen::Upper>().llt().matrixU();
assert(ABC.topLeftCorner(nFrontal,nFrontal).triangularView<Eigen::Upper>().toDenseMatrix().unaryExpr(&isfinite<double>).all()); assert(ABC.topLeftCorner(nFrontal,nFrontal).triangularView<Eigen::Upper>().toDenseMatrix().unaryExpr(ptr_fun(isfinite<double>)).all());
toc(1, "lld"); toc(1, "lld");
if(debug) cout << "R:\n" << Eigen::MatrixXd(ABC.topLeftCorner(nFrontal,nFrontal).triangularView<Eigen::Upper>()) << endl; if(debug) cout << "R:\n" << Eigen::MatrixXd(ABC.topLeftCorner(nFrontal,nFrontal).triangularView<Eigen::Upper>()) << endl;
@ -140,7 +141,7 @@ void choleskyPartial(Matrix& ABC, size_t nFrontal) {
ABC.topLeftCorner(nFrontal,nFrontal).triangularView<Eigen::Upper>().transpose().solveInPlace( ABC.topLeftCorner(nFrontal,nFrontal).triangularView<Eigen::Upper>().transpose().solveInPlace(
ABC.topRightCorner(nFrontal, n-nFrontal)); ABC.topRightCorner(nFrontal, n-nFrontal));
} }
assert(ABC.topRightCorner(nFrontal, n-nFrontal).unaryExpr(&isfinite<double>).all()); assert(ABC.topRightCorner(nFrontal, n-nFrontal).unaryExpr(ptr_fun(isfinite<double>)).all());
if(debug) cout << "S:\n" << ABC.topRightCorner(nFrontal, n-nFrontal) << endl; if(debug) cout << "S:\n" << ABC.topRightCorner(nFrontal, n-nFrontal) << endl;
toc(2, "compute S"); toc(2, "compute S");
@ -150,7 +151,7 @@ void choleskyPartial(Matrix& ABC, size_t nFrontal) {
if(n - nFrontal > 0) if(n - nFrontal > 0)
ABC.bottomRightCorner(n-nFrontal,n-nFrontal).selfadjointView<Eigen::Upper>().rankUpdate( ABC.bottomRightCorner(n-nFrontal,n-nFrontal).selfadjointView<Eigen::Upper>().rankUpdate(
ABC.topRightCorner(nFrontal, n-nFrontal).transpose(), -1.0); ABC.topRightCorner(nFrontal, n-nFrontal).transpose(), -1.0);
assert(ABC.bottomRightCorner(n-nFrontal,n-nFrontal).selfadjointView<Eigen::Upper>().toDenseMatrix().unaryExpr(&isfinite<double>).all()); assert(ABC.bottomRightCorner(n-nFrontal,n-nFrontal).selfadjointView<Eigen::Upper>().toDenseMatrix().unaryExpr(ptr_fun(isfinite<double>)).all());
if(debug) cout << "L:\n" << Eigen::MatrixXd(ABC.bottomRightCorner(n-nFrontal,n-nFrontal).selfadjointView<Eigen::Upper>()) << endl; if(debug) cout << "L:\n" << Eigen::MatrixXd(ABC.bottomRightCorner(n-nFrontal,n-nFrontal).selfadjointView<Eigen::Upper>()) << endl;
toc(3, "compute L"); toc(3, "compute L");
} }

2
gtsam/base/cholesky.h
View File

@ -34,7 +34,7 @@ struct NegativeMatrixException : public std::exception {
Matrix A; ///< The original matrix attempted to factor Matrix A; ///< The original matrix attempted to factor
Matrix U; ///< The produced upper-triangular factor Matrix U; ///< The produced upper-triangular factor
Matrix D; ///< The produced diagonal factor Matrix D; ///< The produced diagonal factor
Detail(const Matrix& _A, const Matrix& _U, const Matrix& _D) /**< Detail constructor */ : A(_A), U(_U), D(_D) {} Detail(const Matrix& A, const Matrix& U, const Matrix& D) /**< Detail constructor */ : A(A), U(U), D(D) {}
void print(const std::string& str = "") const { void print(const std::string& str = "") const {
std::cout << str << "\n"; std::cout << str << "\n";
gtsam::print(A, " A: "); gtsam::print(A, " A: ");

2
gtsam/base/tests/testVector.cpp
View File

@ -251,7 +251,7 @@ TEST( TestVector, axpy )
/* ************************************************************************* */ /* ************************************************************************* */
TEST( TestVector, equals ) TEST( TestVector, equals )
{ {
Vector v1 = Vector_(1, 0.0/0.0); //testing nan Vector v1 = Vector_(1, 0.0/std::numeric_limits<double>::quiet_NaN()); //testing nan
Vector v2 = Vector_(1, 1.0); Vector v2 = Vector_(1, 1.0);
double tol = 1.; double tol = 1.;
EXPECT(!equal_with_abs_tol(v1, v2, tol)); EXPECT(!equal_with_abs_tol(v1, v2, tol));

8
gtsam/base/tests/timeVirtual.cpp
View File

@ -87,13 +87,13 @@ int main(int argc, char *argv[]) {
tic_("shared plain alloc, dealloc"); tic_("shared plain alloc, dealloc");
for(size_t i=0; i<trials; ++i) { for(size_t i=0; i<trials; ++i) {
shared_ptr<Plain> obj(new Plain(i)); boost::shared_ptr<Plain> obj(new Plain(i));
} }
toc_("shared plain alloc, dealloc"); toc_("shared plain alloc, dealloc");
tic_("shared virtual alloc, dealloc"); tic_("shared virtual alloc, dealloc");
for(size_t i=0; i<trials; ++i) { for(size_t i=0; i<trials; ++i) {
shared_ptr<Virtual> obj(new Virtual(i)); boost::shared_ptr<Virtual> obj(new Virtual(i));
} }
toc_("shared virtual alloc, dealloc"); toc_("shared virtual alloc, dealloc");
@ -130,14 +130,14 @@ int main(int argc, char *argv[]) {
tic_("shared plain alloc, dealloc, call"); tic_("shared plain alloc, dealloc, call");
for(size_t i=0; i<trials; ++i) { for(size_t i=0; i<trials; ++i) {
shared_ptr<Plain> obj(new Plain(i)); boost::shared_ptr<Plain> obj(new Plain(i));
obj->setData(i+1); obj->setData(i+1);
} }
toc_("shared plain alloc, dealloc, call"); toc_("shared plain alloc, dealloc, call");
tic_("shared virtual alloc, dealloc, call"); tic_("shared virtual alloc, dealloc, call");
for(size_t i=0; i<trials; ++i) { for(size_t i=0; i<trials; ++i) {
shared_ptr<Virtual> obj(new Virtual(i)); boost::shared_ptr<Virtual> obj(new Virtual(i));
obj->setData(i+1); obj->setData(i+1);
} }
toc_("shared virtual alloc, dealloc, call"); toc_("shared virtual alloc, dealloc, call");

15
gtsam/base/timing.cpp
View File

@ -20,7 +20,6 @@
#include <stdio.h> #include <stdio.h>
#include <iostream> #include <iostream>
#include <iomanip> #include <iomanip>
#include <sys/time.h>
#include <stdlib.h> #include <stdlib.h>
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
#include <boost/format.hpp> #include <boost/format.hpp>
@ -153,15 +152,13 @@ const boost::shared_ptr<TimingOutline>& TimingOutline::child(size_t child, const
void TimingOutline::tic() { void TimingOutline::tic() {
assert(!timerActive_); assert(!timerActive_);
timerActive_ = true; timerActive_ = true;
gettimeofday(&t0_, NULL); t0_ = clock_t::now();
} }
/* ************************************************************************* */ /* ************************************************************************* */
void TimingOutline::toc() { void TimingOutline::toc() {
struct timeval t;
gettimeofday(&t, NULL);
assert(timerActive_); assert(timerActive_);
add(t.tv_sec*1000000 + t.tv_usec - (t0_.tv_sec*1000000 + t0_.tv_usec)); add(boost::chrono::duration_cast<duration_t>(clock_t::now() - t0_).count());
timerActive_ = false; timerActive_ = false;
} }
@ -244,9 +241,11 @@ void Timing::print() {
/* ************************************************************************* */ /* ************************************************************************* */
double _tic_() { double _tic_() {
struct timeval t; typedef boost::chrono::high_resolution_clock clock_t;
gettimeofday(&t, NULL); typedef boost::chrono::duration<double> duration_t;
return ((double)t.tv_sec + ((double)t.tv_usec)/1000000.);
clock_t::time_point t = clock_t::now();
return boost::chrono::duration_cast< duration_t >(t.time_since_epoch()).count();
} }
/* ************************************************************************* */ /* ************************************************************************* */

6
gtsam/base/timing.h
View File

@ -22,6 +22,7 @@
#include <vector> #include <vector>
#include <boost/shared_ptr.hpp> #include <boost/shared_ptr.hpp>
#include <boost/weak_ptr.hpp> #include <boost/weak_ptr.hpp>
#include <boost/chrono.hpp>
class TimingOutline; class TimingOutline;
extern boost::shared_ptr<TimingOutline> timingRoot; extern boost::shared_ptr<TimingOutline> timingRoot;
@ -29,6 +30,9 @@ extern boost::weak_ptr<TimingOutline> timingCurrent;
class TimingOutline { class TimingOutline {
protected: protected:
typedef boost::chrono::high_resolution_clock clock_t;
typedef boost::chrono::microseconds duration_t;
size_t t_; size_t t_;
double t2_ ; /* cache the \sum t_i^2 */ double t2_ ; /* cache the \sum t_i^2 */
size_t tIt_; size_t tIt_;
@ -39,7 +43,7 @@ protected:
boost::weak_ptr<TimingOutline> parent_; boost::weak_ptr<TimingOutline> parent_;
std::vector<boost::shared_ptr<TimingOutline> > children_; std::vector<boost::shared_ptr<TimingOutline> > children_;
struct timeval t0_; clock_t::time_point t0_;
bool timerActive_; bool timerActive_;
void add(size_t usecs); void add(size_t usecs);

30
gtsam/base/types.h
View File

@ -19,7 +19,7 @@
#pragma once #pragma once
#include <unistd.h> #include <cstddef>
namespace gtsam { namespace gtsam {
@ -71,3 +71,31 @@ namespace gtsam {
} }
#ifdef _MSC_VER
#include <boost/math/special_functions/fpclassify.hpp>
using boost::math::isfinite;
using boost::math::isnan;
using boost::math::isinf;
#include <boost/math/constants/constants.hpp>
#ifndef M_PI
#define M_PI (boost::math::constants::pi<double>())
#endif
#ifndef M_PI_2
#define M_PI_2 (boost::math::constants::pi<double>() / 2.0)
#endif
#ifndef M_PI_4
#define M_PI_4 (boost::math::constants::pi<double>() / 4.0)
#endif
#endif
#ifdef min
#undef min
#endif
#ifdef max
#undef max
#endif

2
gtsam/discrete/DecisionTree-inl.h
View File

@ -81,7 +81,7 @@ namespace gtsam {
bool equals(const Node& q, double tol) const { bool equals(const Node& q, double tol) const {
const Leaf* other = dynamic_cast<const Leaf*> (&q); const Leaf* other = dynamic_cast<const Leaf*> (&q);
if (!other) return false; if (!other) return false;
return fabs(this->constant_ - other->constant_) < tol; return fabs(double(this->constant_ - other->constant_)) < tol;
} }
/** print */ /** print */

11
gtsam/discrete/Signature.cpp
View File

@ -33,6 +33,7 @@ namespace gtsam {
#ifdef BOOST_HAVE_PARSER #ifdef BOOST_HAVE_PARSER
namespace qi = boost::spirit::qi; namespace qi = boost::spirit::qi;
namespace ph = boost::phoenix;
// parser for strings of form "99/1 80/20" etc... // parser for strings of form "99/1 80/20" etc...
namespace parser { namespace parser {
@ -85,9 +86,9 @@ namespace gtsam {
// check for OR, AND on whole phrase // check for OR, AND on whole phrase
It f = spec.begin(), l = spec.end(); It f = spec.begin(), l = spec.end();
if (qi::parse(f, l, if (qi::parse(f, l,
qi::lit("OR")[ref(table) = logic(false, true, true, true)]) || qi::lit("OR")[ph::ref(table) = logic(false, true, true, true)]) ||
qi::parse(f, l, qi::parse(f, l,
qi::lit("AND")[ref(table) = logic(false, false, false, true)])) qi::lit("AND")[ph::ref(table) = logic(false, false, false, true)]))
return true; return true;
// tokenize into separate rows // tokenize into separate rows
@ -97,9 +98,9 @@ namespace gtsam {
Signature::Row values; Signature::Row values;
It tf = token.begin(), tl = token.end(); It tf = token.begin(), tl = token.end();
bool r = qi::parse(tf, tl, bool r = qi::parse(tf, tl,
qi::double_[push_back(ref(values), qi::_1)] >> +("/" >> qi::double_[push_back(ref(values), qi::_1)]) | qi::double_[push_back(ph::ref(values), qi::_1)] >> +("/" >> qi::double_[push_back(ph::ref(values), qi::_1)]) |
qi::lit("T")[ref(values) = T] | qi::lit("T")[ph::ref(values) = T] |
qi::lit("F")[ref(values) = F] ); qi::lit("F")[ph::ref(values) = F] );
if (!r) if (!r)
return false; return false;
table.push_back(values); table.push_back(values);

3
gtsam/geometry/Point3.h
View File

@ -21,6 +21,7 @@
#pragma once #pragma once
#include <cmath>
#include <boost/serialization/nvp.hpp> #include <boost/serialization/nvp.hpp>
#include <gtsam/base/Matrix.h> #include <gtsam/base/Matrix.h>
@ -150,7 +151,7 @@ namespace gtsam {
/** distance between two points */ /** distance between two points */
double dist(const Point3& p2) const { double dist(const Point3& p2) const {
return sqrt(pow(x()-p2.x(),2.0) + pow(y()-p2.y(),2.0) + pow(z()-p2.z(),2.0)); return std::sqrt(pow(x()-p2.x(),2.0) + pow(y()-p2.y(),2.0) + pow(z()-p2.z(),2.0));
} }
/** Distance of the point from the origin */ /** Distance of the point from the origin */

10
gtsam/geometry/StereoPoint2.h
View File

@ -119,6 +119,11 @@ namespace gtsam {
return p.vector(); return p.vector();
} }
/** The difference between another point and this point */
inline StereoPoint2 between(const StereoPoint2& p2) const {
return gtsam::between_default(*this, p2);
}
/// @} /// @}
/// @name Standard Interface /// @name Standard Interface
/// @{ /// @{
@ -133,11 +138,6 @@ namespace gtsam {
return Point2(uL_, v_); return Point2(uL_, v_);
} }
///TODO comment
inline StereoPoint2 between(const StereoPoint2& p2) const {
return gtsam::between_default(*this, p2);
}
private: private:
/// @} /// @}

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

@ -65,7 +65,7 @@ TEST( CalibratedCamera, level1)
TEST( CalibratedCamera, level2) TEST( CalibratedCamera, level2)
{ {
// Create a level camera, looking in Y-direction // Create a level camera, looking in Y-direction
Pose2 pose2(0.4,0.3,M_PI_2); Pose2 pose2(0.4,0.3,M_PI/2.0);
CalibratedCamera camera = CalibratedCamera::level(pose2, 0.1); CalibratedCamera camera = CalibratedCamera::level(pose2, 0.1);
// expected // expected

8
gtsam/geometry/tests/testFundamental.cpp
View File

@ -34,11 +34,11 @@ using namespace tensors;
/* ************************************************************************* */ /* ************************************************************************* */
// Indices // Indices
Index<3, 'a'> a; tensors::Index<3, 'a'> a;
Index<3, 'b'> b; tensors::Index<3, 'b'> b;
Index<4, 'A'> A; tensors::Index<4, 'A'> A;
Index<4, 'B'> B; tensors::Index<4, 'B'> B;
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Tensors, FundamentalMatrix) TEST( Tensors, FundamentalMatrix)

18
gtsam/geometry/tests/testHomography2.cpp
View File

@ -36,9 +36,9 @@ using namespace tensors;
/* ************************************************************************* */ /* ************************************************************************* */
// Indices // Indices
Index<3, 'a'> a, _a; tensors::Index<3, 'a'> a, _a;
Index<3, 'b'> b, _b; tensors::Index<3, 'b'> b, _b;
Index<3, 'c'> c, _c; tensors::Index<3, 'c'> c, _c;
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Homography2, RealImages) TEST( Homography2, RealImages)
@ -120,8 +120,8 @@ Homography2 patchH(const Pose3& tEc) {
namespace gtsam { namespace gtsam {
// size_t dim(const tensors::Tensor2<3, 3>& H) {return 9;} // size_t dim(const tensors::Tensor2<3, 3>& H) {return 9;}
Vector toVector(const tensors::Tensor2<3, 3>& H) { Vector toVector(const tensors::Tensor2<3, 3>& H) {
Index<3, 'T'> _T; // covariant 2D template tensors::Index<3, 'T'> _T; // covariant 2D template
Index<3, 'C'> I; // contravariant 2D camera tensors::Index<3, 'C'> I; // contravariant 2D camera
return toVector(H(I,_T)); return toVector(H(I,_T));
} }
Vector localCoordinates(const tensors::Tensor2<3, 3>& A, const tensors::Tensor2<3, 3>& B) { Vector localCoordinates(const tensors::Tensor2<3, 3>& A, const tensors::Tensor2<3, 3>& B) {
@ -134,8 +134,8 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Homography2, patchH) TEST( Homography2, patchH)
{ {
Index<3, 'T'> _T; // covariant 2D template tensors::Index<3, 'T'> _T; // covariant 2D template
Index<3, 'C'> I; // contravariant 2D camera tensors::Index<3, 'C'> I; // contravariant 2D camera
// data[_T][I] // data[_T][I]
double data1[3][3] = {{1,0,0},{0,-1,0},{0,0,10}}; double data1[3][3] = {{1,0,0},{0,-1,0},{0,0,10}};
@ -160,8 +160,8 @@ TEST( Homography2, patchH)
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Homography2, patchH2) TEST( Homography2, patchH2)
{ {
Index<3, 'T'> _T; // covariant 2D template tensors::Index<3, 'T'> _T; // covariant 2D template
Index<3, 'C'> I; // contravariant 2D camera tensors::Index<3, 'C'> I; // contravariant 2D camera
// data[_T][I] // data[_T][I]
double data1[3][3] = {{1,0,0},{0,-1,0},{0,0,10}}; double data1[3][3] = {{1,0,0},{0,-1,0},{0,0,10}};

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

@ -72,7 +72,7 @@ TEST( Point2, arithmetic)
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Point2, norm) TEST( Point2, norm)
{ {
Point2 p0(cos(5), sin(5)); Point2 p0(cos(5.0), sin(5.0));
DOUBLES_EQUAL(1,p0.norm(),1e-6); DOUBLES_EQUAL(1,p0.norm(),1e-6);
Point2 p1(4, 5), p2(1, 1); Point2 p1(4, 5), p2(1, 1);
DOUBLES_EQUAL( 5,p1.dist(p2),1e-6); DOUBLES_EQUAL( 5,p1.dist(p2),1e-6);
@ -85,7 +85,7 @@ TEST( Point2, unit)
Point2 p0(10.0, 0.0), p1(0.0,-10.0), p2(10.0, 10.0); Point2 p0(10.0, 0.0), p1(0.0,-10.0), p2(10.0, 10.0);
EXPECT(assert_equal(Point2(1.0, 0.0), p0.unit(), 1e-6)); EXPECT(assert_equal(Point2(1.0, 0.0), p0.unit(), 1e-6));
EXPECT(assert_equal(Point2(0.0,-1.0), p1.unit(), 1e-6)); EXPECT(assert_equal(Point2(0.0,-1.0), p1.unit(), 1e-6));
EXPECT(assert_equal(Point2(sqrt(2)/2.0, sqrt(2)/2.0), p2.unit(), 1e-6)); EXPECT(assert_equal(Point2(sqrt(2.0)/2.0, sqrt(2.0)/2.0), p2.unit(), 1e-6));
} }
/* ************************************************************************* */ /* ************************************************************************* */

74
gtsam/geometry/tests/testPose2.cpp
View File

@ -44,14 +44,14 @@ TEST(Pose2, constructors) {
Pose2 pose(0,p); Pose2 pose(0,p);
Pose2 origin; Pose2 origin;
assert_equal(pose,origin); assert_equal(pose,origin);
Pose2 t(M_PI_2+0.018, Point2(1.015, 2.01)); Pose2 t(M_PI/2.0+0.018, Point2(1.015, 2.01));
EXPECT(assert_equal(t,Pose2(t.matrix()))); EXPECT(assert_equal(t,Pose2(t.matrix())));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Pose2, manifold) { TEST(Pose2, manifold) {
Pose2 t1(M_PI_2, Point2(1, 2)); Pose2 t1(M_PI/2.0, Point2(1, 2));
Pose2 t2(M_PI_2+0.018, Point2(1.015, 2.01)); Pose2 t2(M_PI/2.0+0.018, Point2(1.015, 2.01));
Pose2 origin; Pose2 origin;
Vector d12 = t1.localCoordinates(t2); Vector d12 = t1.localCoordinates(t2);
EXPECT(assert_equal(t2, t1.retract(d12))); EXPECT(assert_equal(t2, t1.retract(d12)));
@ -63,11 +63,11 @@ TEST(Pose2, manifold) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Pose2, retract) { TEST(Pose2, retract) {
Pose2 pose(M_PI_2, Point2(1, 2)); Pose2 pose(M_PI/2.0, Point2(1, 2));
#ifdef SLOW_BUT_CORRECT_EXPMAP #ifdef SLOW_BUT_CORRECT_EXPMAP
Pose2 expected(1.00811, 2.01528, 2.5608); Pose2 expected(1.00811, 2.01528, 2.5608);
#else #else
Pose2 expected(M_PI_2+0.99, Point2(1.015, 2.01)); Pose2 expected(M_PI/2.0+0.99, Point2(1.015, 2.01));
#endif #endif
Pose2 actual = pose.retract(Vector_(3, 0.01, -0.015, 0.99)); Pose2 actual = pose.retract(Vector_(3, 0.01, -0.015, 0.99));
EXPECT(assert_equal(expected, actual, 1e-5)); EXPECT(assert_equal(expected, actual, 1e-5));
@ -75,7 +75,7 @@ TEST(Pose2, retract) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Pose2, expmap) { TEST(Pose2, expmap) {
Pose2 pose(M_PI_2, Point2(1, 2)); Pose2 pose(M_PI/2.0, Point2(1, 2));
Pose2 expected(1.00811, 2.01528, 2.5608); Pose2 expected(1.00811, 2.01528, 2.5608);
Pose2 actual = expmap_default<Pose2>(pose, Vector_(3, 0.01, -0.015, 0.99)); Pose2 actual = expmap_default<Pose2>(pose, Vector_(3, 0.01, -0.015, 0.99));
EXPECT(assert_equal(expected, actual, 1e-5)); EXPECT(assert_equal(expected, actual, 1e-5));
@ -83,7 +83,7 @@ TEST(Pose2, expmap) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Pose2, expmap2) { TEST(Pose2, expmap2) {
Pose2 pose(M_PI_2, Point2(1, 2)); Pose2 pose(M_PI/2.0, Point2(1, 2));
Pose2 expected(1.00811, 2.01528, 2.5608); Pose2 expected(1.00811, 2.01528, 2.5608);
Pose2 actual = expmap_default<Pose2>(pose, Vector_(3, 0.01, -0.015, 0.99)); Pose2 actual = expmap_default<Pose2>(pose, Vector_(3, 0.01, -0.015, 0.99));
EXPECT(assert_equal(expected, actual, 1e-5)); EXPECT(assert_equal(expected, actual, 1e-5));
@ -110,11 +110,11 @@ TEST(Pose2, expmap3) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Pose2, expmap0) { TEST(Pose2, expmap0) {
Pose2 pose(M_PI_2, Point2(1, 2)); Pose2 pose(M_PI/2.0, Point2(1, 2));
//#ifdef SLOW_BUT_CORRECT_EXPMAP //#ifdef SLOW_BUT_CORRECT_EXPMAP
Pose2 expected(1.01491, 2.01013, 1.5888); Pose2 expected(1.01491, 2.01013, 1.5888);
//#else //#else
// Pose2 expected(M_PI_2+0.018, Point2(1.015, 2.01)); // Pose2 expected(M_PI/2.0+0.018, Point2(1.015, 2.01));
//#endif //#endif
Pose2 actual = pose * (Pose2::Expmap(Vector_(3, 0.01, -0.015, 0.018))); Pose2 actual = pose * (Pose2::Expmap(Vector_(3, 0.01, -0.015, 0.018)));
EXPECT(assert_equal(expected, actual, 1e-5)); EXPECT(assert_equal(expected, actual, 1e-5));
@ -122,7 +122,7 @@ TEST(Pose2, expmap0) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Pose2, expmap0_full) { TEST(Pose2, expmap0_full) {
Pose2 pose(M_PI_2, Point2(1, 2)); Pose2 pose(M_PI/2.0, Point2(1, 2));
Pose2 expected(1.01491, 2.01013, 1.5888); Pose2 expected(1.01491, 2.01013, 1.5888);
Pose2 actual = pose * Pose2::Expmap(Vector_(3, 0.01, -0.015, 0.018)); Pose2 actual = pose * Pose2::Expmap(Vector_(3, 0.01, -0.015, 0.018));
EXPECT(assert_equal(expected, actual, 1e-5)); EXPECT(assert_equal(expected, actual, 1e-5));
@ -130,7 +130,7 @@ TEST(Pose2, expmap0_full) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Pose2, expmap0_full2) { TEST(Pose2, expmap0_full2) {
Pose2 pose(M_PI_2, Point2(1, 2)); Pose2 pose(M_PI/2.0, Point2(1, 2));
Pose2 expected(1.01491, 2.01013, 1.5888); Pose2 expected(1.01491, 2.01013, 1.5888);
Pose2 actual = pose * Pose2::Expmap(Vector_(3, 0.01, -0.015, 0.018)); Pose2 actual = pose * Pose2::Expmap(Vector_(3, 0.01, -0.015, 0.018));
EXPECT(assert_equal(expected, actual, 1e-5)); EXPECT(assert_equal(expected, actual, 1e-5));
@ -170,8 +170,8 @@ TEST(Pose3, expmap_c_full)
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Pose2, logmap) { TEST(Pose2, logmap) {
Pose2 pose0(M_PI_2, Point2(1, 2)); Pose2 pose0(M_PI/2.0, Point2(1, 2));
Pose2 pose(M_PI_2+0.018, Point2(1.015, 2.01)); Pose2 pose(M_PI/2.0+0.018, Point2(1.015, 2.01));
#ifdef SLOW_BUT_CORRECT_EXPMAP #ifdef SLOW_BUT_CORRECT_EXPMAP
Vector expected = Vector_(3, 0.00986473, -0.0150896, 0.018); Vector expected = Vector_(3, 0.00986473, -0.0150896, 0.018);
#else #else
@ -183,8 +183,8 @@ TEST(Pose2, logmap) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Pose2, logmap_full) { TEST(Pose2, logmap_full) {
Pose2 pose0(M_PI_2, Point2(1, 2)); Pose2 pose0(M_PI/2.0, Point2(1, 2));
Pose2 pose(M_PI_2+0.018, Point2(1.015, 2.01)); Pose2 pose(M_PI/2.0+0.018, Point2(1.015, 2.01));
Vector expected = Vector_(3, 0.00986473, -0.0150896, 0.018); Vector expected = Vector_(3, 0.00986473, -0.0150896, 0.018);
Vector actual = logmap_default<Pose2>(pose0, pose); Vector actual = logmap_default<Pose2>(pose0, pose);
EXPECT(assert_equal(expected, actual, 1e-5)); EXPECT(assert_equal(expected, actual, 1e-5));
@ -197,7 +197,7 @@ Point2 transform_to_proxy(const Pose2& pose, const Point2& point) {
TEST( Pose2, transform_to ) TEST( Pose2, transform_to )
{ {
Pose2 pose(M_PI_2, Point2(1,2)); // robot at (1,2) looking towards y Pose2 pose(M_PI/2.0, Point2(1,2)); // robot at (1,2) looking towards y
Point2 point(-1,4); // landmark at (-1,4) Point2 point(-1,4); // landmark at (-1,4)
// expected // expected
@ -226,7 +226,7 @@ Point2 transform_from_proxy(const Pose2& pose, const Point2& point) {
TEST (Pose2, transform_from) TEST (Pose2, transform_from)
{ {
Pose2 pose(1., 0., M_PI_2); Pose2 pose(1., 0., M_PI/2.0);
Point2 pt(2., 1.); Point2 pt(2., 1.);
Matrix H1, H2; Matrix H1, H2;
Point2 actual = pose.transform_from(pt, H1, H2); Point2 actual = pose.transform_from(pt, H1, H2);
@ -326,7 +326,7 @@ TEST(Pose2, compose_c)
TEST(Pose2, inverse ) TEST(Pose2, inverse )
{ {
Point2 origin, t(1,2); Point2 origin, t(1,2);
Pose2 gTl(M_PI_2, t); // robot at (1,2) looking towards y Pose2 gTl(M_PI/2.0, t); // robot at (1,2) looking towards y
Pose2 identity, lTg = gTl.inverse(); Pose2 identity, lTg = gTl.inverse();
EXPECT(assert_equal(identity,lTg.compose(gTl))); EXPECT(assert_equal(identity,lTg.compose(gTl)));
@ -362,7 +362,7 @@ Matrix matrix(const Pose2& gTl) {
TEST( Pose2, matrix ) TEST( Pose2, matrix )
{ {
Point2 origin, t(1,2); Point2 origin, t(1,2);
Pose2 gTl(M_PI_2, t); // robot at (1,2) looking towards y Pose2 gTl(M_PI/2.0, t); // robot at (1,2) looking towards y
Matrix gMl = matrix(gTl); Matrix gMl = matrix(gTl);
EXPECT(assert_equal(Matrix_(3,3, EXPECT(assert_equal(Matrix_(3,3,
0.0, -1.0, 1.0, 0.0, -1.0, 1.0,
@ -393,7 +393,7 @@ TEST( Pose2, matrix )
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Pose2, compose_matrix ) TEST( Pose2, compose_matrix )
{ {
Pose2 gT1(M_PI_2, Point2(1,2)); // robot at (1,2) looking towards y Pose2 gT1(M_PI/2.0, Point2(1,2)); // robot at (1,2) looking towards y
Pose2 _1T2(M_PI, Point2(-1,4)); // local robot at (-1,4) loooking at negative x Pose2 _1T2(M_PI, Point2(-1,4)); // local robot at (-1,4) loooking at negative x
Matrix gM1(matrix(gT1)),_1M2(matrix(_1T2)); Matrix gM1(matrix(gT1)),_1M2(matrix(_1T2));
EXPECT(assert_equal(gM1*_1M2,matrix(gT1.compose(_1T2)))); // RIGHT DOES NOT EXPECT(assert_equal(gM1*_1M2,matrix(gT1.compose(_1T2)))); // RIGHT DOES NOT
@ -407,11 +407,11 @@ TEST( Pose2, between )
// ^ // ^
// //
// *--0--*--* // *--0--*--*
Pose2 gT1(M_PI_2, Point2(1,2)); // robot at (1,2) looking towards y Pose2 gT1(M_PI/2.0, Point2(1,2)); // robot at (1,2) looking towards y
Pose2 gT2(M_PI, Point2(-1,4)); // robot at (-1,4) loooking at negative x Pose2 gT2(M_PI, Point2(-1,4)); // robot at (-1,4) loooking at negative x
Matrix actualH1,actualH2; Matrix actualH1,actualH2;
Pose2 expected(M_PI_2, Point2(2,2)); Pose2 expected(M_PI/2.0, Point2(2,2));
Pose2 actual1 = gT1.between(gT2); Pose2 actual1 = gT1.between(gT2);
Pose2 actual2 = gT1.between(gT2,actualH1,actualH2); Pose2 actual2 = gT1.between(gT2,actualH1,actualH2);
EXPECT(assert_equal(expected,actual1)); EXPECT(assert_equal(expected,actual1));
@ -443,7 +443,7 @@ TEST( Pose2, between )
// reverse situation for extra test // reverse situation for extra test
TEST( Pose2, between2 ) TEST( Pose2, between2 )
{ {
Pose2 p2(M_PI_2, Point2(1,2)); // robot at (1,2) looking towards y Pose2 p2(M_PI/2.0, Point2(1,2)); // robot at (1,2) looking towards y
Pose2 p1(M_PI, Point2(-1,4)); // robot at (-1,4) loooking at negative x Pose2 p1(M_PI, Point2(-1,4)); // robot at (-1,4) loooking at negative x
Matrix actualH1,actualH2; Matrix actualH1,actualH2;
@ -472,7 +472,7 @@ TEST(Pose2, members)
/* ************************************************************************* */ /* ************************************************************************* */
// some shared test values // some shared test values
Pose2 x1, x2(1, 1, 0), x3(1, 1, M_PI_4); Pose2 x1, x2(1, 1, 0), x3(1, 1, M_PI/4.0);
Point2 l1(1, 0), l2(1, 1), l3(2, 2), l4(1, 3); Point2 l1(1, 0), l2(1, 1), l3(2, 2), l4(1, 3);
/* ************************************************************************* */ /* ************************************************************************* */
@ -488,11 +488,11 @@ TEST( Pose2, bearing )
EXPECT(assert_equal(Rot2(),x1.bearing(l1))); EXPECT(assert_equal(Rot2(),x1.bearing(l1)));
// establish bearing is indeed 45 degrees // establish bearing is indeed 45 degrees
EXPECT(assert_equal(Rot2::fromAngle(M_PI_4),x1.bearing(l2))); EXPECT(assert_equal(Rot2::fromAngle(M_PI/4.0),x1.bearing(l2)));
// establish bearing is indeed 45 degrees even if shifted // establish bearing is indeed 45 degrees even if shifted
Rot2 actual23 = x2.bearing(l3, actualH1, actualH2); Rot2 actual23 = x2.bearing(l3, actualH1, actualH2);
EXPECT(assert_equal(Rot2::fromAngle(M_PI_4),actual23)); EXPECT(assert_equal(Rot2::fromAngle(M_PI/4.0),actual23));
// Check numerical derivatives // Check numerical derivatives
expectedH1 = numericalDerivative21(bearing_proxy, x2, l3); expectedH1 = numericalDerivative21(bearing_proxy, x2, l3);
@ -502,7 +502,7 @@ TEST( Pose2, bearing )
// establish bearing is indeed 45 degrees even if rotated // establish bearing is indeed 45 degrees even if rotated
Rot2 actual34 = x3.bearing(l4, actualH1, actualH2); Rot2 actual34 = x3.bearing(l4, actualH1, actualH2);
EXPECT(assert_equal(Rot2::fromAngle(M_PI_4),actual34)); EXPECT(assert_equal(Rot2::fromAngle(M_PI/4.0),actual34));
// Check numerical derivatives // Check numerical derivatives
expectedH1 = numericalDerivative21(bearing_proxy, x3, l4); expectedH1 = numericalDerivative21(bearing_proxy, x3, l4);
@ -518,7 +518,7 @@ Rot2 bearing_pose_proxy(const Pose2& pose, const Pose2& pt) {
TEST( Pose2, bearing_pose ) TEST( Pose2, bearing_pose )
{ {
Pose2 xl1(1, 0, M_PI_2), xl2(1, 1, M_PI), xl3(2.0, 2.0,-M_PI_2), xl4(1, 3, 0); Pose2 xl1(1, 0, M_PI/2.0), xl2(1, 1, M_PI), xl3(2.0, 2.0,-M_PI/2.0), xl4(1, 3, 0);
Matrix expectedH1, actualH1, expectedH2, actualH2; Matrix expectedH1, actualH1, expectedH2, actualH2;
@ -526,11 +526,11 @@ TEST( Pose2, bearing_pose )
EXPECT(assert_equal(Rot2(),x1.bearing(xl1))); EXPECT(assert_equal(Rot2(),x1.bearing(xl1)));
// establish bearing is indeed 45 degrees // establish bearing is indeed 45 degrees
EXPECT(assert_equal(Rot2::fromAngle(M_PI_4),x1.bearing(xl2))); EXPECT(assert_equal(Rot2::fromAngle(M_PI/4.0),x1.bearing(xl2)));
// establish bearing is indeed 45 degrees even if shifted // establish bearing is indeed 45 degrees even if shifted
Rot2 actual23 = x2.bearing(xl3, actualH1, actualH2); Rot2 actual23 = x2.bearing(xl3, actualH1, actualH2);
EXPECT(assert_equal(Rot2::fromAngle(M_PI_4),actual23)); EXPECT(assert_equal(Rot2::fromAngle(M_PI/4.0),actual23));
// Check numerical derivatives // Check numerical derivatives
expectedH1 = numericalDerivative21(bearing_pose_proxy, x2, xl3); expectedH1 = numericalDerivative21(bearing_pose_proxy, x2, xl3);
@ -540,7 +540,7 @@ TEST( Pose2, bearing_pose )
// establish bearing is indeed 45 degrees even if rotated // establish bearing is indeed 45 degrees even if rotated
Rot2 actual34 = x3.bearing(xl4, actualH1, actualH2); Rot2 actual34 = x3.bearing(xl4, actualH1, actualH2);
EXPECT(assert_equal(Rot2::fromAngle(M_PI_4),actual34)); EXPECT(assert_equal(Rot2::fromAngle(M_PI/4.0),actual34));
// Check numerical derivatives // Check numerical derivatives
expectedH1 = numericalDerivative21(bearing_pose_proxy, x3, xl4); expectedH1 = numericalDerivative21(bearing_pose_proxy, x3, xl4);
@ -561,11 +561,11 @@ TEST( Pose2, range )
EXPECT_DOUBLES_EQUAL(1,x1.range(l1),1e-9); EXPECT_DOUBLES_EQUAL(1,x1.range(l1),1e-9);
// establish range is indeed 45 degrees // establish range is indeed 45 degrees
EXPECT_DOUBLES_EQUAL(sqrt(2),x1.range(l2),1e-9); EXPECT_DOUBLES_EQUAL(sqrt(2.0),x1.range(l2),1e-9);
// Another pair // Another pair
double actual23 = x2.range(l3, actualH1, actualH2); double actual23 = x2.range(l3, actualH1, actualH2);
EXPECT_DOUBLES_EQUAL(sqrt(2),actual23,1e-9); EXPECT_DOUBLES_EQUAL(sqrt(2.0),actual23,1e-9);
// Check numerical derivatives // Check numerical derivatives
expectedH1 = numericalDerivative21(range_proxy, x2, l3); expectedH1 = numericalDerivative21(range_proxy, x2, l3);
@ -590,7 +590,7 @@ LieVector range_pose_proxy(const Pose2& pose, const Pose2& point) {
} }
TEST( Pose2, range_pose ) TEST( Pose2, range_pose )
{ {
Pose2 xl1(1, 0, M_PI_2), xl2(1, 1, M_PI), xl3(2.0, 2.0,-M_PI_2), xl4(1, 3, 0); Pose2 xl1(1, 0, M_PI/2.0), xl2(1, 1, M_PI), xl3(2.0, 2.0,-M_PI/2.0), xl4(1, 3, 0);
Matrix expectedH1, actualH1, expectedH2, actualH2; Matrix expectedH1, actualH1, expectedH2, actualH2;
@ -598,11 +598,11 @@ TEST( Pose2, range_pose )
EXPECT_DOUBLES_EQUAL(1,x1.range(xl1),1e-9); EXPECT_DOUBLES_EQUAL(1,x1.range(xl1),1e-9);
// establish range is indeed 45 degrees // establish range is indeed 45 degrees
EXPECT_DOUBLES_EQUAL(sqrt(2),x1.range(xl2),1e-9); EXPECT_DOUBLES_EQUAL(sqrt(2.0),x1.range(xl2),1e-9);
// Another pair // Another pair
double actual23 = x2.range(xl3, actualH1, actualH2); double actual23 = x2.range(xl3, actualH1, actualH2);
EXPECT_DOUBLES_EQUAL(sqrt(2),actual23,1e-9); EXPECT_DOUBLES_EQUAL(sqrt(2.0),actual23,1e-9);
// Check numerical derivatives // Check numerical derivatives
expectedH1 = numericalDerivative21(range_pose_proxy, x2, xl3); expectedH1 = numericalDerivative21(range_pose_proxy, x2, xl3);
@ -637,7 +637,7 @@ TEST(Pose2, align_1) {
TEST(Pose2, align_2) { TEST(Pose2, align_2) {
Point2 t(20,10); Point2 t(20,10);
Rot2 R = Rot2::fromAngle(M_PI_2); Rot2 R = Rot2::fromAngle(M_PI/2.0);
Pose2 expected(R, t); Pose2 expected(R, t);
vector<Point2Pair> correspondences; vector<Point2Pair> correspondences;

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

@ -535,7 +535,7 @@ TEST( Pose3, between )
/* ************************************************************************* */ /* ************************************************************************* */
// some shared test values - pulled from equivalent test in Pose2 // 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); 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), 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 Pose3
xl1(Rot3::ypr(0.0, 0.0, 0.0), Point3(1, 0, 0)), 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)), xl2(Rot3::ypr(0.0, 1.0, 0.0), Point3(1, 1, 0)),
@ -554,11 +554,11 @@ TEST( Pose3, range )
EXPECT_DOUBLES_EQUAL(1,x1.range(l1),1e-9); EXPECT_DOUBLES_EQUAL(1,x1.range(l1),1e-9);
// establish range is indeed sqrt2 // establish range is indeed sqrt2
EXPECT_DOUBLES_EQUAL(sqrt(2),x1.range(l2),1e-9); EXPECT_DOUBLES_EQUAL(sqrt(2.0),x1.range(l2),1e-9);
// Another pair // Another pair
double actual23 = x2.range(l3, actualH1, actualH2); double actual23 = x2.range(l3, actualH1, actualH2);
EXPECT_DOUBLES_EQUAL(sqrt(2),actual23,1e-9); EXPECT_DOUBLES_EQUAL(sqrt(2.0),actual23,1e-9);
// Check numerical derivatives // Check numerical derivatives
expectedH1 = numericalDerivative21(range_proxy, x2, l3); expectedH1 = numericalDerivative21(range_proxy, x2, l3);
@ -589,11 +589,11 @@ TEST( Pose3, range_pose )
EXPECT_DOUBLES_EQUAL(1,x1.range(xl1),1e-9); EXPECT_DOUBLES_EQUAL(1,x1.range(xl1),1e-9);
// establish range is indeed sqrt2 // establish range is indeed sqrt2
EXPECT_DOUBLES_EQUAL(sqrt(2),x1.range(xl2),1e-9); EXPECT_DOUBLES_EQUAL(sqrt(2.0),x1.range(xl2),1e-9);
// Another pair // Another pair
double actual23 = x2.range(xl3, actualH1, actualH2); double actual23 = x2.range(xl3, actualH1, actualH2);
EXPECT_DOUBLES_EQUAL(sqrt(2),actual23,1e-9); EXPECT_DOUBLES_EQUAL(sqrt(2.0),actual23,1e-9);
// Check numerical derivatives // Check numerical derivatives
expectedH1 = numericalDerivative21(range_pose_proxy, x2, xl3); expectedH1 = numericalDerivative21(range_pose_proxy, x2, xl3);
@ -619,7 +619,7 @@ TEST( Pose3, unicycle )
Vector x_step = delta(6,3,1.0); 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), 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(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), Point3(2,2,0)), expmap_default<Pose3>(x3, sqrt(2) * 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));
} }
/* ************************************************************************* */ /* ************************************************************************* */

8
gtsam/geometry/tests/testRot2.cpp
View File

@ -43,7 +43,7 @@ TEST( Rot2, constructors_and_angle)
TEST( Rot2, unit) TEST( Rot2, unit)
{ {
EXPECT(assert_equal(Point2(1.0, 0.0), Rot2::fromAngle(0).unit())); EXPECT(assert_equal(Point2(1.0, 0.0), Rot2::fromAngle(0).unit()));
EXPECT(assert_equal(Point2(0.0, 1.0), Rot2::fromAngle(M_PI_2).unit())); EXPECT(assert_equal(Point2(0.0, 1.0), Rot2::fromAngle(M_PI/2.0).unit()));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -94,9 +94,9 @@ TEST( Rot2, expmap)
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Rot2, logmap) TEST(Rot2, logmap)
{ {
Rot2 rot0(Rot2::fromAngle(M_PI_2)); Rot2 rot0(Rot2::fromAngle(M_PI/2.0));
Rot2 rot(Rot2::fromAngle(M_PI)); Rot2 rot(Rot2::fromAngle(M_PI));
Vector expected = Vector_(1, M_PI_2); Vector expected = Vector_(1, M_PI/2.0);
Vector actual = rot0.localCoordinates(rot); Vector actual = rot0.localCoordinates(rot);
CHECK(assert_equal(expected, actual)); CHECK(assert_equal(expected, actual));
} }
@ -146,7 +146,7 @@ TEST( Rot2, relativeBearing )
// establish relativeBearing is indeed 45 degrees // establish relativeBearing is indeed 45 degrees
Rot2 actual2 = Rot2::relativeBearing(l2, actualH); Rot2 actual2 = Rot2::relativeBearing(l2, actualH);
CHECK(assert_equal(Rot2::fromAngle(M_PI_4),actual2)); CHECK(assert_equal(Rot2::fromAngle(M_PI/4.0),actual2));
// Check numerical derivative // Check numerical derivative
expectedH = numericalDerivative11(relativeBearing_, l2); expectedH = numericalDerivative11(relativeBearing_, l2);

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

@ -131,7 +131,7 @@ TEST( Rot3, rodriguez3)
TEST( Rot3, rodriguez4) TEST( Rot3, rodriguez4)
{ {
Vector axis = Vector_(3,0.,0.,1.); // rotation around Z Vector axis = Vector_(3,0.,0.,1.); // rotation around Z
double angle = M_PI_2; double angle = M_PI/2.0;
Rot3 actual = Rot3::rodriguez(axis, angle); Rot3 actual = Rot3::rodriguez(axis, angle);
double c=cos(angle),s=sin(angle); double c=cos(angle),s=sin(angle);
Rot3 expected(c,-s, 0, Rot3 expected(c,-s, 0,

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

@ -53,7 +53,7 @@ TEST( SimpleCamera, constructor)
TEST( SimpleCamera, level2) TEST( SimpleCamera, level2)
{ {
// Create a level camera, looking in Y-direction // Create a level camera, looking in Y-direction
Pose2 pose2(0.4,0.3,M_PI_2); Pose2 pose2(0.4,0.3,M_PI/2.0);
SimpleCamera camera = SimpleCamera::level(K, pose2, 0.1); SimpleCamera camera = SimpleCamera::level(K, pose2, 0.1);
// expected // expected

12
gtsam/geometry/tests/testTensors.cpp
View File

@ -34,12 +34,12 @@ using namespace tensors;
/* ************************************************************************* */ /* ************************************************************************* */
// Indices // Indices
Index<3, 'a'> a, _a; tensors::Index<3, 'a'> a, _a;
Index<3, 'b'> b, _b; tensors::Index<3, 'b'> b, _b;
Index<3, 'c'> c, _c; tensors::Index<3, 'c'> c, _c;
Index<4, 'A'> A; tensors::Index<4, 'A'> A;
Index<4, 'B'> B; tensors::Index<4, 'B'> B;
/* ************************************************************************* */ /* ************************************************************************* */
// Tensor1 // Tensor1
@ -58,7 +58,7 @@ TEST(Tensor1, Basics)
CHECK(assert_equivalent(p(a),q(a))) // projectively equivalent CHECK(assert_equivalent(p(a),q(a))) // projectively equivalent
// and you can take a norm, typically for normalization to the sphere // and you can take a norm, typically for normalization to the sphere
DOUBLES_EQUAL(sqrt(14),norm(p(a)),1e-9) DOUBLES_EQUAL(sqrt(14.0),norm(p(a)),1e-9)
} }
/* ************************************************************************* */ /* ************************************************************************* */

4
gtsam/inference/BayesTree-inl.h
View File

@ -53,7 +53,7 @@ namespace gtsam {
template<class CONDITIONAL, class CLIQUE> template<class CONDITIONAL, class CLIQUE>
void BayesTree<CONDITIONAL,CLIQUE>::getCliqueData(CliqueData& data, void BayesTree<CONDITIONAL,CLIQUE>::getCliqueData(CliqueData& data,
BayesTree<CONDITIONAL,CLIQUE>::sharedClique clique) const { typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique clique) const {
data.conditionalSizes.push_back((*clique)->nrFrontals()); data.conditionalSizes.push_back((*clique)->nrFrontals());
data.separatorSizes.push_back((*clique)->nrParents()); data.separatorSizes.push_back((*clique)->nrParents());
BOOST_FOREACH(sharedClique c, clique->children_) { BOOST_FOREACH(sharedClique c, clique->children_) {
@ -74,7 +74,7 @@ namespace gtsam {
template<class CONDITIONAL, class CLIQUE> template<class CONDITIONAL, class CLIQUE>
void BayesTree<CONDITIONAL,CLIQUE>::saveGraph(ostream &s, void BayesTree<CONDITIONAL,CLIQUE>::saveGraph(ostream &s,
BayesTree<CONDITIONAL,CLIQUE>::sharedClique clique, typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique clique,
int parentnum) const { int parentnum) const {
static int num = 0; static int num = 0;
bool first = true; bool first = true;

2
gtsam/inference/ClusterTree-inl.h
View File

@ -58,7 +58,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class FG> template<class FG>
bool ClusterTree<FG>::Cluster::equals(const ClusterTree<FG>::Cluster& other) const { bool ClusterTree<FG>::Cluster::equals(const Cluster& other) const {
if (frontal != other.frontal) return false; if (frontal != other.frontal) return false;
if (separator != other.separator) return false; if (separator != other.separator) return false;
if (children_.size() != other.children_.size()) return false; if (children_.size() != other.children_.size()) return false;

9
gtsam/inference/GenericMultifrontalSolver-inl.h
View File

@ -49,15 +49,14 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class F, class JT> template<class FACTOR, class JUNCTIONTREE>
typename BayesTree<typename F::ConditionalType>::shared_ptr GenericMultifrontalSolver<F, JT>::eliminate( typename BayesTree<typename FACTOR::ConditionalType>::shared_ptr GenericMultifrontalSolver<FACTOR, JUNCTIONTREE>::eliminate(Eliminate function) const {
typename FactorGraph<F>::Eliminate function) const {
// eliminate junction tree, returns pointer to root // eliminate junction tree, returns pointer to root
typename BayesTree<typename F::ConditionalType>::sharedClique root = junctionTree_->eliminate(function); typename BayesTree<typename FACTOR::ConditionalType>::sharedClique root = junctionTree_->eliminate(function);
// create an empty Bayes tree and insert root clique // create an empty Bayes tree and insert root clique
typename BayesTree<typename F::ConditionalType>::shared_ptr bayesTree(new BayesTree<typename F::ConditionalType>); typename BayesTree<typename FACTOR::ConditionalType>::shared_ptr bayesTree(new BayesTree<typename FACTOR::ConditionalType>);
bayesTree->insert(root); bayesTree->insert(root);
// return the Bayes tree // return the Bayes tree

4
gtsam/inference/graph-inl.h
View File

@ -196,8 +196,8 @@ boost::shared_ptr<Values> composePoses(const G& graph, const PredecessorMap<KEY>
PoseVertex v2 = key2vertex.find(key2)->second; PoseVertex v2 = key2vertex.find(key2)->second;
POSE l1Xl2 = factor->measured(); POSE l1Xl2 = factor->measured();
tie(edge12, found1) = boost::edge(v1, v2, g); boost::tie(edge12, found1) = boost::edge(v1, v2, g);
tie(edge21, found2) = boost::edge(v2, v1, g); boost::tie(edge21, found2) = boost::edge(v2, v1, g);
if (found1 && found2) throw invalid_argument ("composePoses: invalid spanning tree"); if (found1 && found2) throw invalid_argument ("composePoses: invalid spanning tree");
if (!found1 && !found2) continue; if (!found1 && !found2) continue;
if (found1) if (found1)

4
gtsam/linear/GaussianConditional.cpp
View File

@ -73,7 +73,7 @@ GaussianConditional::GaussianConditional(Index key, const Vector& d, const Matri
const Matrix& R, const list<pair<Index, Matrix> >& parents, const Vector& sigmas) : const Matrix& R, const list<pair<Index, Matrix> >& parents, const Vector& sigmas) :
IndexConditional(key, GetKeys(parents.size(), parents.begin(), parents.end())), rsd_(matrix_), sigmas_(sigmas) { IndexConditional(key, GetKeys(parents.size(), parents.begin(), parents.end())), rsd_(matrix_), sigmas_(sigmas) {
assert(R.rows() <= R.cols()); assert(R.rows() <= R.cols());
size_t dims[1+parents.size()+1]; size_t* dims = (size_t*)alloca(sizeof(size_t)*(1+parents.size()+1)); // FIXME: alloca is bad, just ask Google.
dims[0] = R.cols(); dims[0] = R.cols();
size_t j=1; size_t j=1;
std::list<std::pair<Index, Matrix> >::const_iterator parent=parents.begin(); std::list<std::pair<Index, Matrix> >::const_iterator parent=parents.begin();
@ -95,7 +95,7 @@ GaussianConditional::GaussianConditional(const std::list<std::pair<Index, Matrix
const size_t nrFrontals, const Vector& d, const Vector& sigmas) : const size_t nrFrontals, const Vector& d, const Vector& sigmas) :
IndexConditional(GetKeys(terms.size(), terms.begin(), terms.end()), nrFrontals), IndexConditional(GetKeys(terms.size(), terms.begin(), terms.end()), nrFrontals),
rsd_(matrix_), sigmas_(sigmas) { rsd_(matrix_), sigmas_(sigmas) {
size_t dims[terms.size()+1]; size_t* dims = (size_t*)alloca(sizeof(size_t)*(terms.size()+1)); // FIXME: alloca is bad, just ask Google.
size_t j=0; size_t j=0;
typedef pair<Index, Matrix> Index_Matrix; typedef pair<Index, Matrix> Index_Matrix;
BOOST_FOREACH(const Index_Matrix& term, terms) { BOOST_FOREACH(const Index_Matrix& term, terms) {

6
gtsam/linear/HessianFactor.cpp
View File

@ -188,7 +188,7 @@ HessianFactor::HessianFactor(const std::vector<Index>& js, const std::vector<Mat
} }
// Create the dims vector // Create the dims vector
size_t dims[variable_count+1]; size_t* dims = (size_t*)alloca(sizeof(size_t)*(variable_count+1)); // FIXME: alloca is bad, just ask Google.
size_t total_size = 0; size_t total_size = 0;
for(unsigned int i = 0; i < variable_count; ++i){ for(unsigned int i = 0; i < variable_count; ++i){
dims[i] = gs[i].size(); dims[i] = gs[i].size();
@ -350,7 +350,7 @@ void HessianFactor::updateATA(const HessianFactor& update, const Scatter& scatte
// First build an array of slots // First build an array of slots
tic(1, "slots"); tic(1, "slots");
size_t slots[update.size()]; size_t* slots = (size_t*)alloca(sizeof(size_t)*update.size()); // FIXME: alloca is bad, just ask Google.
size_t slot = 0; size_t slot = 0;
BOOST_FOREACH(Index j, update) { BOOST_FOREACH(Index j, update) {
slots[slot] = scatter.find(j)->second.slot; slots[slot] = scatter.find(j)->second.slot;
@ -404,7 +404,7 @@ void HessianFactor::updateATA(const JacobianFactor& update, const Scatter& scatt
// First build an array of slots // First build an array of slots
tic(1, "slots"); tic(1, "slots");
size_t slots[update.size()]; size_t* slots = (size_t*)alloca(sizeof(size_t)*update.size()); // FIXME: alloca is bad, just ask Google.
size_t slot = 0; size_t slot = 0;
BOOST_FOREACH(Index j, update) { BOOST_FOREACH(Index j, update) {
slots[slot] = scatter.find(j)->second.slot; slots[slot] = scatter.find(j)->second.slot;

6
gtsam/linear/JacobianFactor.cpp
View File

@ -118,7 +118,7 @@ namespace gtsam {
GaussianFactor(GetKeys(terms.size(), terms.begin(), terms.end())), GaussianFactor(GetKeys(terms.size(), terms.begin(), terms.end())),
model_(model), firstNonzeroBlocks_(b.size(), 0), Ab_(matrix_) model_(model), firstNonzeroBlocks_(b.size(), 0), Ab_(matrix_)
{ {
size_t dims[terms.size()+1]; size_t* dims = (size_t*)alloca(sizeof(size_t)*(terms.size()+1)); // FIXME: alloca is bad, just ask Google.
for(size_t j=0; j<terms.size(); ++j) for(size_t j=0; j<terms.size(); ++j)
dims[j] = terms[j].second.cols(); dims[j] = terms[j].second.cols();
dims[terms.size()] = 1; dims[terms.size()] = 1;
@ -135,7 +135,7 @@ namespace gtsam {
GaussianFactor(GetKeys(terms.size(), terms.begin(), terms.end())), GaussianFactor(GetKeys(terms.size(), terms.begin(), terms.end())),
model_(model), firstNonzeroBlocks_(b.size(), 0), Ab_(matrix_) model_(model), firstNonzeroBlocks_(b.size(), 0), Ab_(matrix_)
{ {
size_t dims[terms.size()+1]; size_t* dims=(size_t*)alloca(sizeof(size_t)*(terms.size()+1)); // FIXME: alloca is bad, just ask Google.
size_t j=0; size_t j=0;
std::list<std::pair<Index, Matrix> >::const_iterator term=terms.begin(); std::list<std::pair<Index, Matrix> >::const_iterator term=terms.begin();
for(; term!=terms.end(); ++term,++j) for(; term!=terms.end(); ++term,++j)
@ -494,7 +494,7 @@ namespace gtsam {
size_t>& varDims, size_t m) { size_t>& varDims, size_t m) {
keys_.resize(variableSlots.size()); keys_.resize(variableSlots.size());
std::transform(variableSlots.begin(), variableSlots.end(), begin(), std::transform(variableSlots.begin(), variableSlots.end(), begin(),
bind(&VariableSlots::const_iterator::value_type::first, boost::bind(&VariableSlots::const_iterator::value_type::first,
boost::lambda::_1)); boost::lambda::_1));
varDims.push_back(1); varDims.push_back(1);
Ab_.copyStructureFrom(BlockAb(matrix_, varDims.begin(), varDims.end(), m)); Ab_.copyStructureFrom(BlockAb(matrix_, varDims.begin(), varDims.end(), m));

3
gtsam/linear/NoiseModel.h
View File

@ -20,6 +20,7 @@
#include <boost/serialization/nvp.hpp> #include <boost/serialization/nvp.hpp>
#include <gtsam/base/Matrix.h> #include <gtsam/base/Matrix.h>
#include <cmath>
namespace gtsam { namespace gtsam {
@ -609,7 +610,7 @@ namespace gtsam {
virtual bool equals(const Base& expected, const double tol=1e-8) const = 0; virtual bool equals(const Base& expected, const double tol=1e-8) const = 0;
inline double sqrtWeight(const double &error) const inline double sqrtWeight(const double &error) const
{ return sqrt(weight(error)); } { return std::sqrt(weight(error)); }
/** produce a weight vector according to an error vector and the implemented /** produce a weight vector according to an error vector and the implemented
* robust function */ * robust function */

9
gtsam/nonlinear/DoglegOptimizerImpl.cpp
View File

@ -15,6 +15,7 @@
* @author Richard Roberts * @author Richard Roberts
*/ */
#include <cmath>
#include <gtsam/nonlinear/DoglegOptimizerImpl.h> #include <gtsam/nonlinear/DoglegOptimizerImpl.h>
namespace gtsam { namespace gtsam {
@ -27,12 +28,12 @@ VectorValues DoglegOptimizerImpl::ComputeDoglegPoint(
double DeltaSq = Delta*Delta; double DeltaSq = Delta*Delta;
double x_u_norm_sq = dx_u.vector().squaredNorm(); double x_u_norm_sq = dx_u.vector().squaredNorm();
double x_n_norm_sq = dx_n.vector().squaredNorm(); double x_n_norm_sq = dx_n.vector().squaredNorm();
if(verbose) cout << "Steepest descent magnitude " << sqrt(x_u_norm_sq) << ", Newton's method magnitude " << sqrt(x_n_norm_sq) << endl; if(verbose) cout << "Steepest descent magnitude " << std::sqrt(x_u_norm_sq) << ", Newton's method magnitude " << std::sqrt(x_n_norm_sq) << endl;
if(DeltaSq < x_u_norm_sq) { if(DeltaSq < x_u_norm_sq) {
// Trust region is smaller than steepest descent update // Trust region is smaller than steepest descent update
VectorValues x_d = VectorValues::SameStructure(dx_u); VectorValues x_d = VectorValues::SameStructure(dx_u);
x_d.vector() = dx_u.vector() * sqrt(DeltaSq / x_u_norm_sq); x_d.vector() = dx_u.vector() * std::sqrt(DeltaSq / x_u_norm_sq);
if(verbose) cout << "In steepest descent region with fraction " << sqrt(DeltaSq / x_u_norm_sq) << " of steepest descent magnitude" << endl; if(verbose) cout << "In steepest descent region with fraction " << std::sqrt(DeltaSq / x_u_norm_sq) << " of steepest descent magnitude" << endl;
return x_d; return x_d;
} else if(DeltaSq < x_n_norm_sq) { } else if(DeltaSq < x_n_norm_sq) {
// Trust region boundary is between steepest descent point and Newton's method point // Trust region boundary is between steepest descent point and Newton's method point
@ -59,7 +60,7 @@ VectorValues DoglegOptimizerImpl::ComputeBlend(double Delta, const VectorValues&
const double a = uu - 2.*un + nn; const double a = uu - 2.*un + nn;
const double b = 2. * (un - uu); const double b = 2. * (un - uu);
const double c = uu - Delta*Delta; const double c = uu - Delta*Delta;
double sqrt_b_m4ac = sqrt(b*b - 4*a*c); double sqrt_b_m4ac = std::sqrt(b*b - 4*a*c);
// Compute blending parameter // Compute blending parameter
double tau1 = (-b + sqrt_b_m4ac) / (2.*a); double tau1 = (-b + sqrt_b_m4ac) / (2.*a);

5
gtsam/nonlinear/ISAM2-impl.cpp
View File

@ -19,6 +19,7 @@
#include <gtsam/nonlinear/ISAM2-impl.h> #include <gtsam/nonlinear/ISAM2-impl.h>
#include <gtsam/nonlinear/Symbol.h> // for selective linearization thresholds #include <gtsam/nonlinear/Symbol.h> // for selective linearization thresholds
#include <gtsam/base/debug.h> #include <gtsam/base/debug.h>
#include <functional>
#include <boost/range/adaptors.hpp> #include <boost/range/adaptors.hpp>
#include <boost/range/algorithm.hpp> #include <boost/range/algorithm.hpp>
@ -151,7 +152,7 @@ void ISAM2::Impl::ExpmapMasked(Values& values, const Permuted<VectorValues>& del
cout << " " << keyFormatter(key_value->key) << " (j = " << var << "), delta = " << delta[var].transpose() << endl; cout << " " << keyFormatter(key_value->key) << " (j = " << var << "), delta = " << delta[var].transpose() << endl;
} }
assert(delta[var].size() == (int)key_value->value.dim()); assert(delta[var].size() == (int)key_value->value.dim());
assert(delta[var].unaryExpr(&isfinite<double>).all()); assert(delta[var].unaryExpr(ptr_fun(isfinite<double>)).all());
if(mask[var]) { if(mask[var]) {
Value* retracted = key_value->value.retract_(delta[var]); Value* retracted = key_value->value.retract_(delta[var]);
key_value->value = *retracted; key_value->value = *retracted;
@ -307,7 +308,7 @@ size_t ISAM2::Impl::UpdateDelta(const boost::shared_ptr<ISAM2Clique>& root, std:
#ifndef NDEBUG #ifndef NDEBUG
for(size_t j=0; j<delta.container().size(); ++j) for(size_t j=0; j<delta.container().size(); ++j)
assert(delta.container()[j].unaryExpr(&isfinite<double>).all()); assert(delta.container()[j].unaryExpr(ptr_fun(isfinite<double>)).all());
#endif #endif
} }

16
gtsam/nonlinear/ISAM2.cpp
View File

@ -348,7 +348,7 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
// Reeliminated keys for detailed results // Reeliminated keys for detailed results
if(params_.enableDetailedResults) { if(params_.enableDetailedResults) {
BOOST_FOREACH(Key key, theta_.keys()) { BOOST_FOREACH(Key key, theta_.keys()) {
result.detail->variableStatus[key].isReeliminated = true; result.detail->variableStatus[key].isReeliminated = true;
} }
} }
@ -372,7 +372,7 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
// Reeliminated keys for detailed results // Reeliminated keys for detailed results
if(params_.enableDetailedResults) { if(params_.enableDetailedResults) {
BOOST_FOREACH(Index index, affectedAndNewKeys) { BOOST_FOREACH(Index index, affectedAndNewKeys) {
result.detail->variableStatus[inverseOrdering_->at(index)].isReeliminated = true; result.detail->variableStatus[inverseOrdering_->at(index)].isReeliminated = true;
} }
} }
@ -485,7 +485,7 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
// Root clique variables for detailed results // Root clique variables for detailed results
if(params_.enableDetailedResults) { if(params_.enableDetailedResults) {
BOOST_FOREACH(Index index, this->root()->conditional()->frontals()) { BOOST_FOREACH(Index index, this->root()->conditional()->frontals()) {
result.detail->variableStatus[inverseOrdering_->at(index)].inRootClique = true; result.detail->variableStatus[inverseOrdering_->at(index)].inRootClique = true;
} }
} }
@ -572,7 +572,7 @@ ISAM2Result ISAM2::update(
// Observed keys for detailed results // Observed keys for detailed results
if(params_.enableDetailedResults) { if(params_.enableDetailedResults) {
BOOST_FOREACH(Index index, markedKeys) { BOOST_FOREACH(Index index, markedKeys) {
result.detail->variableStatus[inverseOrdering_->at(index)].isObserved = true; result.detail->variableStatus[inverseOrdering_->at(index)].isObserved = true;
} }
} }
// NOTE: we use assign instead of the iterator constructor here because this // NOTE: we use assign instead of the iterator constructor here because this
@ -732,10 +732,10 @@ Values ISAM2::calculateEstimate() const {
// We use ExpmapMasked here instead of regular expmap because the former // We use ExpmapMasked here instead of regular expmap because the former
// handles Permuted<VectorValues> // handles Permuted<VectorValues>
tic(1, "Copy Values"); tic(1, "Copy Values");
Values ret(theta_); Values ret(theta_);
toc(1, "Copy Values"); toc(1, "Copy Values");
tic(2, "getDelta"); tic(2, "getDelta");
const Permuted<VectorValues>& delta(getDelta()); const Permuted<VectorValues>& delta(getDelta());
toc(2, "getDelta"); toc(2, "getDelta");
tic(3, "Expmap"); tic(3, "Expmap");
vector<bool> mask(ordering_.nVars(), true); vector<bool> mask(ordering_.nVars(), true);

4
gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
View File

@ -16,6 +16,8 @@
* @created Feb 26, 2012 * @created Feb 26, 2012
*/ */
#include <cmath>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h> #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/base/cholesky.h> // For NegativeMatrixException #include <gtsam/base/cholesky.h> // For NegativeMatrixException
@ -46,7 +48,7 @@ void LevenbergMarquardtOptimizer::iterate() {
// TODO: replace this dampening with a backsubstitution approach // TODO: replace this dampening with a backsubstitution approach
GaussianFactorGraph dampedSystem(*linear); GaussianFactorGraph dampedSystem(*linear);
{ {
double sigma = 1.0 / sqrt(state_.lambda); double sigma = 1.0 / std::sqrt(state_.lambda);
dampedSystem.reserve(dampedSystem.size() + dimensions_.size()); dampedSystem.reserve(dampedSystem.size() + dimensions_.size());
// for each of the variables, add a prior // for each of the variables, add a prior
for(Index j=0; j<dimensions_.size(); ++j) { for(Index j=0; j<dimensions_.size(); ++j) {

13
gtsam/nonlinear/Marginals.cpp
View File

@ -74,6 +74,19 @@ Matrix Marginals::marginalInformation(Key variable) const {
return info.topLeftCorner(dim,dim); // Take the non-augmented part of the information matrix return info.topLeftCorner(dim,dim); // Take the non-augmented part of the information matrix
} }
/* ************************************************************************* */
JointMarginal::JointMarginal(const JointMarginal& other) :
blockView_(fullMatrix_) {
*this = other;
}
/* ************************************************************************* */
JointMarginal& JointMarginal::operator=(const JointMarginal& rhs) {
indices_ = rhs.indices_;
blockView_.assignNoalias(rhs.blockView_);
return *this;
}
/* ************************************************************************* */ /* ************************************************************************* */
JointMarginal Marginals::jointMarginalCovariance(const std::vector<Key>& variables) const { JointMarginal Marginals::jointMarginalCovariance(const std::vector<Key>& variables) const {
JointMarginal info = jointMarginalInformation(variables); JointMarginal info = jointMarginalInformation(variables);

6
gtsam/nonlinear/Marginals.h
View File

@ -116,6 +116,12 @@ public:
*/ */
Block operator()(Key iVariable, Key jVariable) const { Block operator()(Key iVariable, Key jVariable) const {
return blockView_(indices_[iVariable], indices_[jVariable]); } return blockView_(indices_[iVariable], indices_[jVariable]); }
/** Copy constructor */
JointMarginal(const JointMarginal& other);
/** Assignment operator */
JointMarginal& operator=(const JointMarginal& rhs);
}; };
} /* namespace gtsam */ } /* namespace gtsam */

4
gtsam/slam/CMakeLists.txt
View File

@ -24,6 +24,10 @@ if (GTSAM_BUILD_TESTS)
gtsam_add_subdir_tests(slam "${slam_local_libs}" "gtsam-static" "${slam_excluded_files}") gtsam_add_subdir_tests(slam "${slam_local_libs}" "gtsam-static" "${slam_excluded_files}")
endif(GTSAM_BUILD_TESTS) endif(GTSAM_BUILD_TESTS)
if(MSVC)
add_definitions("/bigobj")
endif()
# Build timing scripts # Build timing scripts
if (GTSAM_BUILD_TIMING) if (GTSAM_BUILD_TIMING)
gtsam_add_subdir_timing(slam "${slam_local_libs}" "gtsam-static" "${slam_excluded_files}") gtsam_add_subdir_timing(slam "${slam_local_libs}" "gtsam-static" "${slam_excluded_files}")

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

@ -149,8 +149,8 @@ vector<GeneralCamera> genCameraVariableCalibration() {
return X ; return X ;
} }
shared_ptr<Ordering> getOrdering(const vector<GeneralCamera>& cameras, const vector<Point3>& landmarks) { boost::shared_ptr<Ordering> getOrdering(const vector<GeneralCamera>& cameras, const vector<Point3>& landmarks) {
shared_ptr<Ordering> ordering(new Ordering); boost::shared_ptr<Ordering> ordering(new Ordering);
for ( size_t i = 0 ; i < landmarks.size() ; ++i ) ordering->push_back(L(i)) ; for ( size_t i = 0 ; i < landmarks.size() ; ++i ) ordering->push_back(L(i)) ;
for ( size_t i = 0 ; i < cameras.size() ; ++i ) ordering->push_back(X(i)) ; for ( size_t i = 0 ; i < cameras.size() ; ++i ) ordering->push_back(X(i)) ;
return ordering ; return ordering ;

6
gtsam/slam/tests/testPlanarSLAM.cpp
View File

@ -76,7 +76,7 @@ TEST( planarSLAM, BearingFactor_equals )
TEST( planarSLAM, RangeFactor ) TEST( planarSLAM, RangeFactor )
{ {
// Create factor // Create factor
double z(sqrt(2) - 0.22); // h(x) - z = 0.22 double z(sqrt(2.0) - 0.22); // h(x) - z = 0.22
planarSLAM::Range factor(i2, j3, z, sigma); planarSLAM::Range factor(i2, j3, z, sigma);
// create config // create config
@ -103,7 +103,7 @@ TEST( planarSLAM, BearingRangeFactor )
{ {
// Create factor // Create factor
Rot2 r = Rot2::fromAngle(M_PI_4 + 0.1); // h(x) - z = -0.1 Rot2 r = Rot2::fromAngle(M_PI_4 + 0.1); // h(x) - z = -0.1
double b(sqrt(2) - 0.22); // h(x) - z = 0.22 double b(sqrt(2.0) - 0.22); // h(x) - z = 0.22
planarSLAM::BearingRange factor(i2, j3, r, b, sigma2); planarSLAM::BearingRange factor(i2, j3, r, b, sigma2);
// create config // create config
@ -166,7 +166,7 @@ TEST( planarSLAM, constructor )
G.addBearing(i2, j3, z1, sigma); G.addBearing(i2, j3, z1, sigma);
// Create range factor // Create range factor
double z2(sqrt(2) - 0.22); // h(x) - z = 0.22 double z2(sqrt(2.0) - 0.22); // h(x) - z = 0.22
G.addRange(i2, j3, z2, sigma); G.addRange(i2, j3, z2, sigma);
Vector expected0 = Vector_(3, 0.0, 0.0, 0.0); Vector expected0 = Vector_(3, 0.0, 0.0, 0.0);

5
tests/testDoglegOptimizer.cpp
View File

@ -33,7 +33,6 @@
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
using boost::shared_ptr;
// Convenience for named keys // Convenience for named keys
using symbol_shorthand::X; using symbol_shorthand::X;
@ -387,7 +386,7 @@ TEST(DoglegOptimizer, ComputeDoglegPoint) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(DoglegOptimizer, Iterate) { TEST(DoglegOptimizer, Iterate) {
// really non-linear factor graph // really non-linear factor graph
shared_ptr<example::Graph> fg(new example::Graph( boost::shared_ptr<example::Graph> fg(new example::Graph(
example::createReallyNonlinearFactorGraph())); example::createReallyNonlinearFactorGraph()));
// config far from minimum // config far from minimum
@ -396,7 +395,7 @@ TEST(DoglegOptimizer, Iterate) {
config->insert(X(1), x0); config->insert(X(1), x0);
// ordering // ordering
shared_ptr<Ordering> ord(new Ordering()); boost::shared_ptr<Ordering> ord(new Ordering());
ord->push_back(X(1)); ord->push_back(X(1));
double Delta = 1.0; double Delta = 1.0;

4
tests/testGaussianFactorGraphB.cpp
View File

@ -233,7 +233,7 @@ TEST( GaussianFactorGraph, eliminateOne_l1 )
GaussianConditional::shared_ptr actual = inference::eliminateOne(fg, 0, EliminateQR).first; GaussianConditional::shared_ptr actual = inference::eliminateOne(fg, 0, EliminateQR).first;
// create expected Conditional Gaussian // create expected Conditional Gaussian
double sig = sqrt(2)/10.; double sig = sqrt(2.0)/10.;
Matrix I = eye(2)/sig, R11 = I, S12 = -0.5*I, S13 = -0.5*I; Matrix I = eye(2)/sig, R11 = I, S12 = -0.5*I, S13 = -0.5*I;
Vector d = Vector_(2, -0.1, 0.25)/sig, sigma = ones(2); Vector d = Vector_(2, -0.1, 0.25)/sig, sigma = ones(2);
GaussianConditional expected(ordering[L(1)],d,R11,ordering[X(1)],S12,ordering[X(2)],S13,sigma); GaussianConditional expected(ordering[L(1)],d,R11,ordering[X(1)],S12,ordering[X(2)],S13,sigma);
@ -296,7 +296,7 @@ TEST( GaussianFactorGraph, eliminateOne_l1_fast )
GaussianConditional::shared_ptr actual = inference::eliminateOne(fg, ordering[L(1)], EliminateQR).first; GaussianConditional::shared_ptr actual = inference::eliminateOne(fg, ordering[L(1)], EliminateQR).first;
// create expected Conditional Gaussian // create expected Conditional Gaussian
double sig = sqrt(2)/10.; double sig = sqrt(2.0)/10.;
Matrix I = eye(2)/sig, R11 = I, S12 = -0.5*I, S13 = -0.5*I; Matrix I = eye(2)/sig, R11 = I, S12 = -0.5*I, S13 = -0.5*I;
Vector d = Vector_(2, -0.1, 0.25)/sig, sigma = ones(2); Vector d = Vector_(2, -0.1, 0.25)/sig, sigma = ones(2);
GaussianConditional expected(ordering[L(1)],d,R11,ordering[X(1)],S12,ordering[X(2)],S13,sigma); GaussianConditional expected(ordering[L(1)],d,R11,ordering[X(1)],S12,ordering[X(2)],S13,sigma);

2
tests/testGraph.cpp
View File

@ -66,7 +66,7 @@ TEST( Graph, predecessorMap2Graph )
p_map.insert(1, 2); p_map.insert(1, 2);
p_map.insert(2, 2); p_map.insert(2, 2);
p_map.insert(3, 2); p_map.insert(3, 2);
tie(graph, root, key2vertex) = predecessorMap2Graph<SGraph<Key>, SVertex, Key>(p_map); boost::tie(graph, root, key2vertex) = predecessorMap2Graph<SGraph<Key>, SVertex, Key>(p_map);
LONGS_EQUAL(3, boost::num_vertices(graph)); LONGS_EQUAL(3, boost::num_vertices(graph));
CHECK(root == key2vertex[2]); CHECK(root == key2vertex[2]);

2
tests/testInferenceB.cpp
View File

@ -46,7 +46,7 @@ TEST( inference, marginals )
*GaussianSequentialSolver(GaussianFactorGraph(cbn)).jointFactorGraph(xvar)).eliminate(); *GaussianSequentialSolver(GaussianFactorGraph(cbn)).jointFactorGraph(xvar)).eliminate();
// expected is just scalar Gaussian on x // expected is just scalar Gaussian on x
GaussianBayesNet expected = scalarGaussian(0, 4, sqrt(2)); GaussianBayesNet expected = scalarGaussian(0, 4, sqrt(2.0));
CHECK(assert_equal(expected,actual)); CHECK(assert_equal(expected,actual));
} }

2
tests/testMarginals.cpp
View File

@ -71,7 +71,7 @@ TEST(Marginals, planarSLAMmarginals) {
Rot2 bearing11 = Rot2::fromDegrees(45), Rot2 bearing11 = Rot2::fromDegrees(45),
bearing21 = Rot2::fromDegrees(90), bearing21 = Rot2::fromDegrees(90),
bearing32 = Rot2::fromDegrees(90); bearing32 = Rot2::fromDegrees(90);
double range11 = sqrt(4+4), double range11 = sqrt(4.0+4.0),
range21 = 2.0, range21 = 2.0,
range32 = 2.0; range32 = 2.0;

2
tests/timeMultifrontalOnDataset.cpp
View File

@ -44,7 +44,7 @@ int main(int argc, char *argv[]) {
else if (argc == 4) else if (argc == 4)
nrTrials = strtoul(argv[3], NULL, 10); nrTrials = strtoul(argv[3], NULL, 10);
pair<shared_ptr<pose2SLAM::Graph>, shared_ptr<Values> > data = load2D(dataset(datasetname)); pair<boost::shared_ptr<pose2SLAM::Graph>, boost::shared_ptr<Values> > data = load2D(dataset(datasetname));
// Add a prior on the first pose // Add a prior on the first pose
if (soft_prior) if (soft_prior)

2
tests/timeSequentialOnDataset.cpp
View File

@ -44,7 +44,7 @@ int main(int argc, char *argv[]) {
else if (argc == 4) else if (argc == 4)
nrTrials = strtoul(argv[3], NULL, 10); nrTrials = strtoul(argv[3], NULL, 10);
pair<shared_ptr<pose2SLAM::Graph>, shared_ptr<Values> > data = load2D(dataset(datasetname)); pair<boost::shared_ptr<pose2SLAM::Graph>, boost::shared_ptr<Values> > data = load2D(dataset(datasetname));
// Add a prior on the first pose // Add a prior on the first pose
if (soft_prior) if (soft_prior)

2
wrap/Argument.cpp
View File

@ -46,7 +46,7 @@ void Argument::matlab_unwrap(FileWriter& file, const string& matlabName) const {
if (is_ptr) if (is_ptr)
// A pointer: emit an "unwrap_shared_ptr" call which returns a pointer // A pointer: emit an "unwrap_shared_ptr" call which returns a pointer
file.oss << "shared_ptr<" << cppType << "> " << name << " = unwrap_shared_ptr< "; file.oss << "boost::shared_ptr<" << cppType << "> " << name << " = unwrap_shared_ptr< ";
else if (is_ref) else if (is_ref)
// A reference: emit an "unwrap_shared_ptr" call and de-reference the pointer // A reference: emit an "unwrap_shared_ptr" call and de-reference the pointer
file.oss << cppType << "& " << name << " = *unwrap_shared_ptr< "; file.oss << cppType << "& " << name << " = *unwrap_shared_ptr< ";

8
wrap/CMakeLists.txt
View File

@ -5,7 +5,7 @@ file(GLOB wrap_srcs "*.cpp")
list(REMOVE_ITEM wrap_srcs wrap.cpp) list(REMOVE_ITEM wrap_srcs wrap.cpp)
add_library(wrap_lib STATIC ${wrap_srcs}) add_library(wrap_lib STATIC ${wrap_srcs})
add_executable(wrap wrap.cpp) add_executable(wrap wrap.cpp)
target_link_libraries(wrap wrap_lib) target_link_libraries(wrap wrap_lib ${Boost_SYSTEM_LIBRARY} ${Boost_FILESYSTEM_LIBRARY})
# Install wrap binary # Install wrap binary
if (GTSAM_INSTALL_WRAP) if (GTSAM_INSTALL_WRAP)
@ -32,6 +32,9 @@ endif(GTSAM_BUILD_TESTS)
# [mexFlags] : extra flags for the mex command # [mexFlags] : extra flags for the mex command
set(mexFlags "-I${Boost_INCLUDE_DIR} -I${CMAKE_INSTALL_PREFIX}/include -I${CMAKE_INSTALL_PREFIX}/include/gtsam -I${CMAKE_INSTALL_PREFIX}/include/gtsam/base -I${CMAKE_INSTALL_PREFIX}/include/gtsam/geometry -I${CMAKE_INSTALL_PREFIX}/include/gtsam/linear -I${CMAKE_INSTALL_PREFIX}/include/gtsam/discrete -I${CMAKE_INSTALL_PREFIX}/include/gtsam/inference -I${CMAKE_INSTALL_PREFIX}/include/gtsam/nonlinear -I${CMAKE_INSTALL_PREFIX}/include/gtsam/slam -L${CMAKE_INSTALL_PREFIX}/lib -lgtsam") set(mexFlags "-I${Boost_INCLUDE_DIR} -I${CMAKE_INSTALL_PREFIX}/include -I${CMAKE_INSTALL_PREFIX}/include/gtsam -I${CMAKE_INSTALL_PREFIX}/include/gtsam/base -I${CMAKE_INSTALL_PREFIX}/include/gtsam/geometry -I${CMAKE_INSTALL_PREFIX}/include/gtsam/linear -I${CMAKE_INSTALL_PREFIX}/include/gtsam/discrete -I${CMAKE_INSTALL_PREFIX}/include/gtsam/inference -I${CMAKE_INSTALL_PREFIX}/include/gtsam/nonlinear -I${CMAKE_INSTALL_PREFIX}/include/gtsam/slam -L${CMAKE_INSTALL_PREFIX}/lib -lgtsam")
if(MSVC OR CYGWIN OR WINGW)
set(mexFlags "${mexFlags} LINKFLAGS='$LINKFLAGS /LIBPATH:${Boost_LIBRARY_DIRS}'")
endif()
set(toolbox_path ${CMAKE_BINARY_DIR}/wrap/gtsam) set(toolbox_path ${CMAKE_BINARY_DIR}/wrap/gtsam)
set(moduleName gtsam) set(moduleName gtsam)
@ -44,8 +47,9 @@ if (NOT EXECUTABLE_OUTPUT_PATH)
endif() endif()
# Code generation command # Code generation command
get_property(WRAP_EXE TARGET wrap PROPERTY LOCATION)
add_custom_target(wrap_gtsam ALL COMMAND add_custom_target(wrap_gtsam ALL COMMAND
${EXECUTABLE_OUTPUT_PATH}/wrap ${WRAP_EXE}
${MEX_COMMAND} ${MEX_COMMAND}
${GTSAM_MEX_BIN_EXTENSION} ${GTSAM_MEX_BIN_EXTENSION}
${CMAKE_CURRENT_SOURCE_DIR}/../ ${CMAKE_CURRENT_SOURCE_DIR}/../

2
wrap/Method.cpp
View File

@ -73,7 +73,7 @@ void Method::matlab_wrapper(const string& classPath,
// get class pointer // get class pointer
// example: shared_ptr<Test> = unwrap_shared_ptr< Test >(in[0], "Test"); // example: shared_ptr<Test> = unwrap_shared_ptr< Test >(in[0], "Test");
file.oss << " shared_ptr<" << cppClassName << "> self = unwrap_shared_ptr< " << cppClassName file.oss << " boost::shared_ptr<" << cppClassName << "> self = unwrap_shared_ptr< " << cppClassName
<< " >(in[0],\"" << matlabClassName << "\");" << endl; << " >(in[0],\"" << matlabClassName << "\");" << endl;
// unwrap arguments, see Argument.cpp // unwrap arguments, see Argument.cpp

11
wrap/Module.cpp
View File

@ -25,12 +25,14 @@
#include <boost/spirit/include/classic_confix.hpp> #include <boost/spirit/include/classic_confix.hpp>
#include <boost/spirit/include/classic_clear_actor.hpp> #include <boost/spirit/include/classic_clear_actor.hpp>
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
#include <boost/filesystem.hpp>
#include <iostream> #include <iostream>
using namespace std; using namespace std;
using namespace wrap; using namespace wrap;
using namespace BOOST_SPIRIT_CLASSIC_NS; using namespace BOOST_SPIRIT_CLASSIC_NS;
namespace fs = boost::filesystem;
typedef rule<BOOST_SPIRIT_CLASSIC_NS::phrase_scanner_t> Rule; typedef rule<BOOST_SPIRIT_CLASSIC_NS::phrase_scanner_t> Rule;
@ -279,8 +281,8 @@ void verifyReturnTypes(const vector<string>& validtypes, const vector<T>& vt) {
/* ************************************************************************* */ /* ************************************************************************* */
void Module::matlab_code(const string& mexCommand, const string& toolboxPath, void Module::matlab_code(const string& mexCommand, const string& toolboxPath,
const string& mexExt, const string& mexFlags) const { const string& mexExt, const string& mexFlags) const {
string installCmd = "install -d " + toolboxPath;
system(installCmd.c_str()); fs::create_directories(toolboxPath);
// create make m-file // create make m-file
string matlabMakeFileName = toolboxPath + "/make_" + name + ".m"; string matlabMakeFileName = toolboxPath + "/make_" + name + ".m";
@ -292,7 +294,7 @@ void Module::matlab_code(const string& mexCommand, const string& toolboxPath,
makeModuleMfile.oss << "echo on" << endl << endl; makeModuleMfile.oss << "echo on" << endl << endl;
makeModuleMfile.oss << "toolboxpath = mfilename('fullpath');" << endl; makeModuleMfile.oss << "toolboxpath = mfilename('fullpath');" << endl;
makeModuleMfile.oss << "delims = find(toolboxpath == '/');" << endl; makeModuleMfile.oss << "delims = find(toolboxpath == '/' | toolboxpath == '\\');" << endl;
makeModuleMfile.oss << "toolboxpath = toolboxpath(1:(delims(end)-1));" << endl; makeModuleMfile.oss << "toolboxpath = toolboxpath(1:(delims(end)-1));" << endl;
makeModuleMfile.oss << "clear delims" << endl; makeModuleMfile.oss << "clear delims" << endl;
makeModuleMfile.oss << "addpath(toolboxpath);" << endl << endl; makeModuleMfile.oss << "addpath(toolboxpath);" << endl << endl;
@ -327,8 +329,7 @@ void Module::matlab_code(const string& mexCommand, const string& toolboxPath,
BOOST_FOREACH(Class cls, classes) { BOOST_FOREACH(Class cls, classes) {
// create directory if needed // create directory if needed
string classPath = toolboxPath + "/@" + cls.qualifiedName(); string classPath = toolboxPath + "/@" + cls.qualifiedName();
string installCmd = "install -d " + classPath; fs::create_directories(classPath);
system(installCmd.c_str());
// create proxy class // create proxy class
string classFile = classPath + "/" + cls.qualifiedName() + ".m"; string classFile = classPath + "/" + cls.qualifiedName() + ".m";

6
wrap/ReturnValue.cpp
View File

@ -53,7 +53,7 @@ void ReturnValue::wrap_result(FileWriter& file) const {
if (isPtr1) // if we already have a pointer if (isPtr1) // if we already have a pointer
file.oss << " out[0] = wrap_shared_ptr(result.first,\"" << matlabType1 << "\");\n"; file.oss << " out[0] = wrap_shared_ptr(result.first,\"" << matlabType1 << "\");\n";
else if (category1 == ReturnValue::CLASS) // if we are going to make one else if (category1 == ReturnValue::CLASS) // if we are going to make one
file.oss << " out[0] = wrap_shared_ptr(make_shared< " << cppType1 << " >(result.first),\"" << matlabType1 << "\");\n"; file.oss << " out[0] = wrap_shared_ptr(boost::make_shared< " << cppType1 << " >(result.first),\"" << matlabType1 << "\");\n";
else // if basis type else // if basis type
file.oss << " out[0] = wrap< " << return_type(true,arg1) << " >(result.first);\n"; file.oss << " out[0] = wrap< " << return_type(true,arg1) << " >(result.first);\n";
@ -61,14 +61,14 @@ void ReturnValue::wrap_result(FileWriter& file) const {
if (isPtr2) // if we already have a pointer if (isPtr2) // if we already have a pointer
file.oss << " out[1] = wrap_shared_ptr(result.second,\"" << matlabType2 << "\");\n"; file.oss << " out[1] = wrap_shared_ptr(result.second,\"" << matlabType2 << "\");\n";
else if (category2 == ReturnValue::CLASS) // if we are going to make one else if (category2 == ReturnValue::CLASS) // if we are going to make one
file.oss << " out[1] = wrap_shared_ptr(make_shared< " << cppType2 << " >(result.second),\"" << matlabType2 << "\");\n"; file.oss << " out[1] = wrap_shared_ptr(boost::make_shared< " << cppType2 << " >(result.second),\"" << matlabType2 << "\");\n";
else else
file.oss << " out[1] = wrap< " << return_type(true,arg2) << " >(result.second);\n"; file.oss << " out[1] = wrap< " << return_type(true,arg2) << " >(result.second);\n";
} }
else if (isPtr1) else if (isPtr1)
file.oss << " out[0] = wrap_shared_ptr(result,\"" << matlabType1 << "\");\n"; file.oss << " out[0] = wrap_shared_ptr(result,\"" << matlabType1 << "\");\n";
else if (category1 == ReturnValue::CLASS) else if (category1 == ReturnValue::CLASS)
file.oss << " out[0] = wrap_shared_ptr(make_shared< " << cppType1 << " >(result),\"" << matlabType1 << "\");\n"; file.oss << " out[0] = wrap_shared_ptr(boost::make_shared< " << cppType1 << " >(result),\"" << matlabType1 << "\");\n";
else if (matlabType1!="void") else if (matlabType1!="void")
file.oss << " out[0] = wrap< " << return_type(true,arg1) << " >(result);\n"; file.oss << " out[0] = wrap< " << return_type(true,arg1) << " >(result);\n";
} }

12
wrap/matlab.h
View File

@ -315,19 +315,19 @@ class ObjectHandle {
private: private:
ObjectHandle* signature; // use 'this' as a unique object signature ObjectHandle* signature; // use 'this' as a unique object signature
const std::type_info* type; // type checking information const std::type_info* type; // type checking information
shared_ptr<T> t; // object pointer boost::shared_ptr<T> t; // object pointer
public: public:
// Constructor for free-store allocated objects. // Constructor for free-store allocated objects.
// Creates shared pointer, will delete if is last one to hold pointer // Creates shared pointer, will delete if is last one to hold pointer
ObjectHandle(T* ptr) : ObjectHandle(T* ptr) :
type(&typeid(T)), t(shared_ptr<T> (ptr)) { type(&typeid(T)), t(boost::shared_ptr<T> (ptr)) {
signature = this; signature = this;
} }
// Constructor for shared pointers // Constructor for shared pointers
// Creates shared pointer, will delete if is last one to hold pointer // Creates shared pointer, will delete if is last one to hold pointer
ObjectHandle(shared_ptr<T> ptr) : ObjectHandle(boost::shared_ptr<T> ptr) :
/*type(&typeid(T)),*/ t(ptr) { /*type(&typeid(T)),*/ t(ptr) {
signature = this; signature = this;
} }
@ -338,7 +338,7 @@ public:
} }
// Get the actual object contained by handle // Get the actual object contained by handle
shared_ptr<T> get_object() const { boost::shared_ptr<T> get_object() const {
return t; return t;
} }
@ -434,7 +434,7 @@ mxArray* create_object(const char *classname, mxArray* h) {
class to matlab. class to matlab.
*/ */
template <typename Class> template <typename Class>
mxArray* wrap_shared_ptr(shared_ptr< Class > shared_ptr, const char *classname) { mxArray* wrap_shared_ptr(boost::shared_ptr< Class > shared_ptr, const char *classname) {
ObjectHandle<Class>* handle = new ObjectHandle<Class>(shared_ptr); ObjectHandle<Class>* handle = new ObjectHandle<Class>(shared_ptr);
return create_object(classname,handle->to_mex_handle()); return create_object(classname,handle->to_mex_handle());
} }
@ -451,7 +451,7 @@ mxArray* wrap_shared_ptr(shared_ptr< Class > shared_ptr, const char *classname)
to the object. to the object.
*/ */
template <typename Class> template <typename Class>
shared_ptr<Class> unwrap_shared_ptr(const mxArray* obj, const string& className) { boost::shared_ptr<Class> unwrap_shared_ptr(const mxArray* obj, const string& className) {
//Why is this here? //Why is this here?
#ifndef UNSAFE_WRAP #ifndef UNSAFE_WRAP
bool isClass = mxIsClass(obj, className.c_str()); bool isClass = mxIsClass(obj, className.c_str());

2
wrap/tests/expected/@Point2/argChar.cpp
View File

@ -4,7 +4,7 @@
void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
{ {
checkArguments("argChar",nargout,nargin-1,1); checkArguments("argChar",nargout,nargin-1,1);
shared_ptr<Point2> self = unwrap_shared_ptr< Point2 >(in[0],"Point2"); boost::shared_ptr<Point2> self = unwrap_shared_ptr< Point2 >(in[0],"Point2");
char a = unwrap< char >(in[1]); char a = unwrap< char >(in[1]);
self->argChar(a); self->argChar(a);
} }

2
wrap/tests/expected/@Point2/argUChar.cpp
View File

@ -4,7 +4,7 @@
void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
{ {
checkArguments("argUChar",nargout,nargin-1,1); checkArguments("argUChar",nargout,nargin-1,1);
shared_ptr<Point2> self = unwrap_shared_ptr< Point2 >(in[0],"Point2"); boost::shared_ptr<Point2> self = unwrap_shared_ptr< Point2 >(in[0],"Point2");
unsigned char a = unwrap< unsigned char >(in[1]); unsigned char a = unwrap< unsigned char >(in[1]);
self->argUChar(a); self->argUChar(a);
} }

2
wrap/tests/expected/@Point2/dim.cpp
View File

@ -4,7 +4,7 @@
void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
{ {
checkArguments("dim",nargout,nargin-1,0); checkArguments("dim",nargout,nargin-1,0);
shared_ptr<Point2> self = unwrap_shared_ptr< Point2 >(in[0],"Point2"); boost::shared_ptr<Point2> self = unwrap_shared_ptr< Point2 >(in[0],"Point2");
int result = self->dim(); int result = self->dim();
out[0] = wrap< int >(result); out[0] = wrap< int >(result);
} }

2
wrap/tests/expected/@Point2/returnChar.cpp
View File

@ -4,7 +4,7 @@
void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
{ {
checkArguments("returnChar",nargout,nargin-1,0); checkArguments("returnChar",nargout,nargin-1,0);
shared_ptr<Point2> self = unwrap_shared_ptr< Point2 >(in[0],"Point2"); boost::shared_ptr<Point2> self = unwrap_shared_ptr< Point2 >(in[0],"Point2");
char result = self->returnChar(); char result = self->returnChar();
out[0] = wrap< char >(result); out[0] = wrap< char >(result);
} }

4
wrap/tests/expected/@Point2/vectorConfusion.cpp
View File

@ -4,7 +4,7 @@
void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
{ {
checkArguments("vectorConfusion",nargout,nargin-1,0); checkArguments("vectorConfusion",nargout,nargin-1,0);
shared_ptr<Point2> self = unwrap_shared_ptr< Point2 >(in[0],"Point2"); boost::shared_ptr<Point2> self = unwrap_shared_ptr< Point2 >(in[0],"Point2");
VectorNotEigen result = self->vectorConfusion(); VectorNotEigen result = self->vectorConfusion();
out[0] = wrap_shared_ptr(make_shared< VectorNotEigen >(result),"VectorNotEigen"); out[0] = wrap_shared_ptr(boost::make_shared< VectorNotEigen >(result),"VectorNotEigen");
} }

2
wrap/tests/expected/@Point2/x.cpp
View File

@ -4,7 +4,7 @@
void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
{ {
checkArguments("x",nargout,nargin-1,0); checkArguments("x",nargout,nargin-1,0);
shared_ptr<Point2> self = unwrap_shared_ptr< Point2 >(in[0],"Point2"); boost::shared_ptr<Point2> self = unwrap_shared_ptr< Point2 >(in[0],"Point2");
double result = self->x(); double result = self->x();
out[0] = wrap< double >(result); out[0] = wrap< double >(result);
} }

2
wrap/tests/expected/@Point2/y.cpp
View File

@ -4,7 +4,7 @@
void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
{ {
checkArguments("y",nargout,nargin-1,0); checkArguments("y",nargout,nargin-1,0);
shared_ptr<Point2> self = unwrap_shared_ptr< Point2 >(in[0],"Point2"); boost::shared_ptr<Point2> self = unwrap_shared_ptr< Point2 >(in[0],"Point2");
double result = self->y(); double result = self->y();
out[0] = wrap< double >(result); out[0] = wrap< double >(result);
} }

2
wrap/tests/expected/@Point3/norm.cpp
View File

@ -5,7 +5,7 @@ using namespace geometry;
void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
{ {
checkArguments("norm",nargout,nargin-1,0); checkArguments("norm",nargout,nargin-1,0);
shared_ptr<Point3> self = unwrap_shared_ptr< Point3 >(in[0],"Point3"); boost::shared_ptr<Point3> self = unwrap_shared_ptr< Point3 >(in[0],"Point3");
double result = self->norm(); double result = self->norm();
out[0] = wrap< double >(result); out[0] = wrap< double >(result);
} }

2
wrap/tests/expected/@Test/arg_EigenConstRef.cpp
View File

@ -5,7 +5,7 @@ using namespace geometry;
void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
{ {
checkArguments("arg_EigenConstRef",nargout,nargin-1,1); checkArguments("arg_EigenConstRef",nargout,nargin-1,1);
shared_ptr<Test> self = unwrap_shared_ptr< Test >(in[0],"Test"); boost::shared_ptr<Test> self = unwrap_shared_ptr< Test >(in[0],"Test");
Matrix& value = *unwrap_shared_ptr< Matrix >(in[1], "Matrix"); Matrix& value = *unwrap_shared_ptr< Matrix >(in[1], "Matrix");
self->arg_EigenConstRef(value); self->arg_EigenConstRef(value);
} }

6
wrap/tests/expected/@Test/create_MixedPtrs.cpp
View File

@ -5,8 +5,8 @@ using namespace geometry;
void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
{ {
checkArguments("create_MixedPtrs",nargout,nargin-1,0); checkArguments("create_MixedPtrs",nargout,nargin-1,0);
shared_ptr<Test> self = unwrap_shared_ptr< Test >(in[0],"Test"); boost::shared_ptr<Test> self = unwrap_shared_ptr< Test >(in[0],"Test");
pair< Test, shared_ptr<Test> > result = self->create_MixedPtrs(); pair< Test, boost::shared_ptr<Test> > result = self->create_MixedPtrs();
out[0] = wrap_shared_ptr(make_shared< Test >(result.first),"Test"); out[0] = wrap_shared_ptr(boost::make_shared< Test >(result.first),"Test");
out[1] = wrap_shared_ptr(result.second,"Test"); out[1] = wrap_shared_ptr(result.second,"Test");
} }

4
wrap/tests/expected/@Test/create_ptrs.cpp
View File

@ -5,8 +5,8 @@ using namespace geometry;
void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
{ {
checkArguments("create_ptrs",nargout,nargin-1,0); checkArguments("create_ptrs",nargout,nargin-1,0);
shared_ptr<Test> self = unwrap_shared_ptr< Test >(in[0],"Test"); boost::shared_ptr<Test> self = unwrap_shared_ptr< Test >(in[0],"Test");
pair< shared_ptr<Test>, shared_ptr<Test> > result = self->create_ptrs(); pair< boost::shared_ptr<Test>, boost::shared_ptr<Test> > result = self->create_ptrs();
out[0] = wrap_shared_ptr(result.first,"Test"); out[0] = wrap_shared_ptr(result.first,"Test");
out[1] = wrap_shared_ptr(result.second,"Test"); out[1] = wrap_shared_ptr(result.second,"Test");
} }

2
wrap/tests/expected/@Test/print.cpp
View File

@ -5,6 +5,6 @@ using namespace geometry;
void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
{ {
checkArguments("print",nargout,nargin-1,0); checkArguments("print",nargout,nargin-1,0);
shared_ptr<Test> self = unwrap_shared_ptr< Test >(in[0],"Test"); boost::shared_ptr<Test> self = unwrap_shared_ptr< Test >(in[0],"Test");
self->print(); self->print();
} }

4
wrap/tests/expected/@Test/return_Point2Ptr.cpp
View File

@ -5,8 +5,8 @@ using namespace geometry;
void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
{ {
checkArguments("return_Point2Ptr",nargout,nargin-1,1); checkArguments("return_Point2Ptr",nargout,nargin-1,1);
shared_ptr<Test> self = unwrap_shared_ptr< Test >(in[0],"Test"); boost::shared_ptr<Test> self = unwrap_shared_ptr< Test >(in[0],"Test");
bool value = unwrap< bool >(in[1]); bool value = unwrap< bool >(in[1]);
shared_ptr<Point2> result = self->return_Point2Ptr(value); boost::shared_ptr<Point2> result = self->return_Point2Ptr(value);
out[0] = wrap_shared_ptr(result,"Point2"); out[0] = wrap_shared_ptr(result,"Point2");
} }

6
wrap/tests/expected/@Test/return_Test.cpp
View File

@ -5,8 +5,8 @@ using namespace geometry;
void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
{ {
checkArguments("return_Test",nargout,nargin-1,1); checkArguments("return_Test",nargout,nargin-1,1);
shared_ptr<Test> self = unwrap_shared_ptr< Test >(in[0],"Test"); boost::shared_ptr<Test> self = unwrap_shared_ptr< Test >(in[0],"Test");
shared_ptr<Test> value = unwrap_shared_ptr< Test >(in[1], "Test"); boost::shared_ptr<Test> value = unwrap_shared_ptr< Test >(in[1], "Test");
Test result = self->return_Test(value); Test result = self->return_Test(value);
out[0] = wrap_shared_ptr(make_shared< Test >(result),"Test"); out[0] = wrap_shared_ptr(boost::make_shared< Test >(result),"Test");
} }

6
wrap/tests/expected/@Test/return_TestPtr.cpp
View File

@ -5,8 +5,8 @@ using namespace geometry;
void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
{ {
checkArguments("return_TestPtr",nargout,nargin-1,1); checkArguments("return_TestPtr",nargout,nargin-1,1);
shared_ptr<Test> self = unwrap_shared_ptr< Test >(in[0],"Test"); boost::shared_ptr<Test> self = unwrap_shared_ptr< Test >(in[0],"Test");
shared_ptr<Test> value = unwrap_shared_ptr< Test >(in[1], "Test"); boost::shared_ptr<Test> value = unwrap_shared_ptr< Test >(in[1], "Test");
shared_ptr<Test> result = self->return_TestPtr(value); boost::shared_ptr<Test> result = self->return_TestPtr(value);
out[0] = wrap_shared_ptr(result,"Test"); out[0] = wrap_shared_ptr(result,"Test");
} }

2
wrap/tests/expected/@Test/return_bool.cpp
View File

@ -5,7 +5,7 @@ using namespace geometry;
void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
{ {
checkArguments("return_bool",nargout,nargin-1,1); checkArguments("return_bool",nargout,nargin-1,1);
shared_ptr<Test> self = unwrap_shared_ptr< Test >(in[0],"Test"); boost::shared_ptr<Test> self = unwrap_shared_ptr< Test >(in[0],"Test");
bool value = unwrap< bool >(in[1]); bool value = unwrap< bool >(in[1]);
bool result = self->return_bool(value); bool result = self->return_bool(value);
out[0] = wrap< bool >(result); out[0] = wrap< bool >(result);

2
wrap/tests/expected/@Test/return_double.cpp
View File

@ -5,7 +5,7 @@ using namespace geometry;
void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
{ {
checkArguments("return_double",nargout,nargin-1,1); checkArguments("return_double",nargout,nargin-1,1);
shared_ptr<Test> self = unwrap_shared_ptr< Test >(in[0],"Test"); boost::shared_ptr<Test> self = unwrap_shared_ptr< Test >(in[0],"Test");
double value = unwrap< double >(in[1]); double value = unwrap< double >(in[1]);
double result = self->return_double(value); double result = self->return_double(value);
out[0] = wrap< double >(result); out[0] = wrap< double >(result);

2
wrap/tests/expected/@Test/return_field.cpp
View File

@ -5,7 +5,7 @@ using namespace geometry;
void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
{ {
checkArguments("return_field",nargout,nargin-1,1); checkArguments("return_field",nargout,nargin-1,1);
shared_ptr<Test> self = unwrap_shared_ptr< Test >(in[0],"Test"); boost::shared_ptr<Test> self = unwrap_shared_ptr< Test >(in[0],"Test");
Test& t = *unwrap_shared_ptr< Test >(in[1], "Test"); Test& t = *unwrap_shared_ptr< Test >(in[1], "Test");
bool result = self->return_field(t); bool result = self->return_field(t);
out[0] = wrap< bool >(result); out[0] = wrap< bool >(result);

2
wrap/tests/expected/@Test/return_int.cpp
View File

@ -5,7 +5,7 @@ using namespace geometry;
void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
{ {
checkArguments("return_int",nargout,nargin-1,1); checkArguments("return_int",nargout,nargin-1,1);
shared_ptr<Test> self = unwrap_shared_ptr< Test >(in[0],"Test"); boost::shared_ptr<Test> self = unwrap_shared_ptr< Test >(in[0],"Test");
int value = unwrap< int >(in[1]); int value = unwrap< int >(in[1]);
int result = self->return_int(value); int result = self->return_int(value);
out[0] = wrap< int >(result); out[0] = wrap< int >(result);

2
wrap/tests/expected/@Test/return_matrix1.cpp
View File

@ -5,7 +5,7 @@ using namespace geometry;
void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
{ {
checkArguments("return_matrix1",nargout,nargin-1,1); checkArguments("return_matrix1",nargout,nargin-1,1);
shared_ptr<Test> self = unwrap_shared_ptr< Test >(in[0],"Test"); boost::shared_ptr<Test> self = unwrap_shared_ptr< Test >(in[0],"Test");
Matrix value = unwrap< Matrix >(in[1]); Matrix value = unwrap< Matrix >(in[1]);
Matrix result = self->return_matrix1(value); Matrix result = self->return_matrix1(value);
out[0] = wrap< Matrix >(result); out[0] = wrap< Matrix >(result);

2
wrap/tests/expected/@Test/return_matrix2.cpp
View File

@ -5,7 +5,7 @@ using namespace geometry;
void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
{ {
checkArguments("return_matrix2",nargout,nargin-1,1); checkArguments("return_matrix2",nargout,nargin-1,1);
shared_ptr<Test> self = unwrap_shared_ptr< Test >(in[0],"Test"); boost::shared_ptr<Test> self = unwrap_shared_ptr< Test >(in[0],"Test");
Matrix value = unwrap< Matrix >(in[1]); Matrix value = unwrap< Matrix >(in[1]);
Matrix result = self->return_matrix2(value); Matrix result = self->return_matrix2(value);
out[0] = wrap< Matrix >(result); out[0] = wrap< Matrix >(result);

2
wrap/tests/expected/@Test/return_pair.cpp
View File

@ -5,7 +5,7 @@ using namespace geometry;
void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
{ {
checkArguments("return_pair",nargout,nargin-1,2); checkArguments("return_pair",nargout,nargin-1,2);
shared_ptr<Test> self = unwrap_shared_ptr< Test >(in[0],"Test"); boost::shared_ptr<Test> self = unwrap_shared_ptr< Test >(in[0],"Test");
Vector v = unwrap< Vector >(in[1]); Vector v = unwrap< Vector >(in[1]);
Matrix A = unwrap< Matrix >(in[2]); Matrix A = unwrap< Matrix >(in[2]);
pair< Vector, Matrix > result = self->return_pair(v,A); pair< Vector, Matrix > result = self->return_pair(v,A);

8
wrap/tests/expected/@Test/return_ptrs.cpp
View File

@ -5,10 +5,10 @@ using namespace geometry;
void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
{ {
checkArguments("return_ptrs",nargout,nargin-1,2); checkArguments("return_ptrs",nargout,nargin-1,2);
shared_ptr<Test> self = unwrap_shared_ptr< Test >(in[0],"Test"); boost::shared_ptr<Test> self = unwrap_shared_ptr< Test >(in[0],"Test");
shared_ptr<Test> p1 = unwrap_shared_ptr< Test >(in[1], "Test"); boost::shared_ptr<Test> p1 = unwrap_shared_ptr< Test >(in[1], "Test");
shared_ptr<Test> p2 = unwrap_shared_ptr< Test >(in[2], "Test"); boost::shared_ptr<Test> p2 = unwrap_shared_ptr< Test >(in[2], "Test");
pair< shared_ptr<Test>, shared_ptr<Test> > result = self->return_ptrs(p1,p2); pair< boost::shared_ptr<Test>, boost::shared_ptr<Test> > result = self->return_ptrs(p1,p2);
out[0] = wrap_shared_ptr(result.first,"Test"); out[0] = wrap_shared_ptr(result.first,"Test");
out[1] = wrap_shared_ptr(result.second,"Test"); out[1] = wrap_shared_ptr(result.second,"Test");
} }

2
wrap/tests/expected/@Test/return_size_t.cpp
View File

@ -5,7 +5,7 @@ using namespace geometry;
void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
{ {
checkArguments("return_size_t",nargout,nargin-1,1); checkArguments("return_size_t",nargout,nargin-1,1);
shared_ptr<Test> self = unwrap_shared_ptr< Test >(in[0],"Test"); boost::shared_ptr<Test> self = unwrap_shared_ptr< Test >(in[0],"Test");
size_t value = unwrap< size_t >(in[1]); size_t value = unwrap< size_t >(in[1]);
size_t result = self->return_size_t(value); size_t result = self->return_size_t(value);
out[0] = wrap< size_t >(result); out[0] = wrap< size_t >(result);

2
wrap/tests/expected/@Test/return_string.cpp
View File

@ -5,7 +5,7 @@ using namespace geometry;
void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
{ {
checkArguments("return_string",nargout,nargin-1,1); checkArguments("return_string",nargout,nargin-1,1);
shared_ptr<Test> self = unwrap_shared_ptr< Test >(in[0],"Test"); boost::shared_ptr<Test> self = unwrap_shared_ptr< Test >(in[0],"Test");
string value = unwrap< string >(in[1]); string value = unwrap< string >(in[1]);
string result = self->return_string(value); string result = self->return_string(value);
out[0] = wrap< string >(result); out[0] = wrap< string >(result);

2
wrap/tests/expected/@Test/return_vector1.cpp
View File

@ -5,7 +5,7 @@ using namespace geometry;
void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
{ {
checkArguments("return_vector1",nargout,nargin-1,1); checkArguments("return_vector1",nargout,nargin-1,1);
shared_ptr<Test> self = unwrap_shared_ptr< Test >(in[0],"Test"); boost::shared_ptr<Test> self = unwrap_shared_ptr< Test >(in[0],"Test");
Vector value = unwrap< Vector >(in[1]); Vector value = unwrap< Vector >(in[1]);
Vector result = self->return_vector1(value); Vector result = self->return_vector1(value);
out[0] = wrap< Vector >(result); out[0] = wrap< Vector >(result);

2
wrap/tests/expected/@Test/return_vector2.cpp
View File

@ -5,7 +5,7 @@ using namespace geometry;
void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
{ {
checkArguments("return_vector2",nargout,nargin-1,1); checkArguments("return_vector2",nargout,nargin-1,1);
shared_ptr<Test> self = unwrap_shared_ptr< Test >(in[0],"Test"); boost::shared_ptr<Test> self = unwrap_shared_ptr< Test >(in[0],"Test");
Vector value = unwrap< Vector >(in[1]); Vector value = unwrap< Vector >(in[1]);
Vector result = self->return_vector2(value); Vector result = self->return_vector2(value);
out[0] = wrap< Vector >(result); out[0] = wrap< Vector >(result);

2
wrap/tests/expected/Point3_StaticFunctionRet.cpp
View File

@ -7,5 +7,5 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
checkArguments("Point3_StaticFunctionRet",nargout,nargin,1); checkArguments("Point3_StaticFunctionRet",nargout,nargin,1);
double z = unwrap< double >(in[0]); double z = unwrap< double >(in[0]);
Point3 result = Point3::StaticFunctionRet(z); Point3 result = Point3::StaticFunctionRet(z);
out[0] = wrap_shared_ptr(make_shared< Point3 >(result),"Point3"); out[0] = wrap_shared_ptr(boost::make_shared< Point3 >(result),"Point3");
} }

2
wrap/tests/expected/make_geometry.m
View File

@ -2,7 +2,7 @@
echo on echo on
toolboxpath = mfilename('fullpath'); toolboxpath = mfilename('fullpath');
delims = find(toolboxpath == '/'); delims = find(toolboxpath == '/' | toolboxpath == '\');
toolboxpath = toolboxpath(1:(delims(end)-1)); toolboxpath = toolboxpath(1:(delims(end)-1));
clear delims clear delims
addpath(toolboxpath); addpath(toolboxpath);

2
wrap/tests/expected_namespaces/@ns2ClassA/memberFunction.cpp
View File

@ -5,7 +5,7 @@
void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
{ {
checkArguments("memberFunction",nargout,nargin-1,0); checkArguments("memberFunction",nargout,nargin-1,0);
shared_ptr<ns2::ClassA> self = unwrap_shared_ptr< ns2::ClassA >(in[0],"ns2ClassA"); boost::shared_ptr<ns2::ClassA> self = unwrap_shared_ptr< ns2::ClassA >(in[0],"ns2ClassA");
double result = self->memberFunction(); double result = self->memberFunction();
out[0] = wrap< double >(result); out[0] = wrap< double >(result);
} }

2
wrap/tests/expected_namespaces/@ns2ClassA/nsArg.cpp
View File

@ -5,7 +5,7 @@
void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
{ {
checkArguments("nsArg",nargout,nargin-1,1); checkArguments("nsArg",nargout,nargin-1,1);
shared_ptr<ns2::ClassA> self = unwrap_shared_ptr< ns2::ClassA >(in[0],"ns2ClassA"); boost::shared_ptr<ns2::ClassA> self = unwrap_shared_ptr< ns2::ClassA >(in[0],"ns2ClassA");
ns1::ClassB& arg = *unwrap_shared_ptr< ns1::ClassB >(in[1], "ns1ClassB"); ns1::ClassB& arg = *unwrap_shared_ptr< ns1::ClassB >(in[1], "ns1ClassB");
int result = self->nsArg(arg); int result = self->nsArg(arg);
out[0] = wrap< int >(result); out[0] = wrap< int >(result);

4
wrap/tests/expected_namespaces/@ns2ClassA/nsReturn.cpp
View File

@ -5,8 +5,8 @@
void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
{ {
checkArguments("nsReturn",nargout,nargin-1,1); checkArguments("nsReturn",nargout,nargin-1,1);
shared_ptr<ns2::ClassA> self = unwrap_shared_ptr< ns2::ClassA >(in[0],"ns2ClassA"); boost::shared_ptr<ns2::ClassA> self = unwrap_shared_ptr< ns2::ClassA >(in[0],"ns2ClassA");
double q = unwrap< double >(in[1]); double q = unwrap< double >(in[1]);
ns2::ns3::ClassB result = self->nsReturn(q); ns2::ns3::ClassB result = self->nsReturn(q);
out[0] = wrap_shared_ptr(make_shared< ns2::ns3::ClassB >(result),"ns2ns3ClassB"); out[0] = wrap_shared_ptr(boost::make_shared< ns2::ns3::ClassB >(result),"ns2ns3ClassB");
} }

2
wrap/tests/expected_namespaces/make_testNamespaces.m
View File

@ -2,7 +2,7 @@
echo on echo on
toolboxpath = mfilename('fullpath'); toolboxpath = mfilename('fullpath');
delims = find(toolboxpath == '/'); delims = find(toolboxpath == '/' | toolboxpath == '\');
toolboxpath = toolboxpath(1:(delims(end)-1)); toolboxpath = toolboxpath(1:(delims(end)-1));
clear delims clear delims
addpath(toolboxpath); addpath(toolboxpath);

11
wrap/tests/testWrap.cpp
View File

@ -20,6 +20,7 @@
#include <fstream> #include <fstream>
#include <sstream> #include <sstream>
#include <boost/assign/std/vector.hpp> #include <boost/assign/std/vector.hpp>
#include <boost/filesystem.hpp>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <wrap/utilities.h> #include <wrap/utilities.h>
@ -28,6 +29,7 @@
using namespace std; using namespace std;
using namespace boost::assign; using namespace boost::assign;
using namespace wrap; using namespace wrap;
namespace fs = boost::filesystem;
static bool enable_verbose = false; static bool enable_verbose = false;
#ifdef TOPSRCDIR #ifdef TOPSRCDIR
static string topdir = TOPSRCDIR; static string topdir = TOPSRCDIR;
@ -57,8 +59,7 @@ TEST( wrap, check_exception ) {
CHECK_EXCEPTION(Module("/alsonotarealpath", "geometry",enable_verbose), CantOpenFile); CHECK_EXCEPTION(Module("/alsonotarealpath", "geometry",enable_verbose), CantOpenFile);
// clean out previous generated code // clean out previous generated code
string cleanCmd = "rm -rf actual_deps"; fs::remove_all("actual_deps");
system(cleanCmd.c_str());
string path = topdir + "/wrap/tests"; string path = topdir + "/wrap/tests";
Module module(path.c_str(), "testDependencies",enable_verbose); Module module(path.c_str(), "testDependencies",enable_verbose);
@ -208,8 +209,7 @@ TEST( wrap, matlab_code_namespaces ) {
string path = topdir + "/wrap"; string path = topdir + "/wrap";
// clean out previous generated code // clean out previous generated code
string cleanCmd = "rm -rf actual_namespaces"; fs::remove_all("actual_namespaces");
system(cleanCmd.c_str());
// emit MATLAB code // emit MATLAB code
string exp_path = path + "/tests/expected_namespaces/"; string exp_path = path + "/tests/expected_namespaces/";
@ -263,8 +263,7 @@ TEST( wrap, matlab_code ) {
string path = topdir + "/wrap"; string path = topdir + "/wrap";
// clean out previous generated code // clean out previous generated code
string cleanCmd = "rm -rf actual"; fs::remove_all("actual");
system(cleanCmd.c_str());
// emit MATLAB code // emit MATLAB code
// make_geometry will not compile, use make testwrap to generate real make // make_geometry will not compile, use make testwrap to generate real make

2
wrap/utilities.cpp
View File

@ -104,7 +104,7 @@ bool files_equal(const string& expected, const string& actual, bool skipheader)
/* ************************************************************************* */ /* ************************************************************************* */
string maybe_shared_ptr(bool add, const string& type) { string maybe_shared_ptr(bool add, const string& type) {
string str = add? "shared_ptr<" : ""; string str = add? "boost::shared_ptr<" : "";
str += type; str += type;
if (add) str += ">"; if (add) str += ">";
return str; return str;