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)
cmake_minimum_required(VERSION 2.6)
@ -9,8 +10,12 @@ set (GTSAM_VERSION_PATCH 0)
# Set the default install path to home
#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
set(CMAKE_MODULE_PATH "${CMAKE_MODULE_PATH}" "${PROJECT_SOURCE_DIR}/cmake")
include(GtsamTesting)
include(GtsamPrinting)
@ -26,24 +31,6 @@ else()
set(GTSAM_UNSTABLE_AVAILABLE 0)
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
option(GTSAM_BUILD_TESTS "Enable/Disable building of tests" 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)
endif()
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_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_EXAMPLES "Enable/Disable installation of matlab examples" 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
if (GTSAM_USE_QUATERNIONS)
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -DGTSAM_DEFAULT_QUATERNIONS")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DGTSAM_DEFAULT_QUATERNIONS")
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -DGTSAM_DEFAULT_QUATERNIONS")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DGTSAM_DEFAULT_QUATERNIONS")
endif(GTSAM_USE_QUATERNIONS)
# Avoid building non-installed exes and unit tests when installing
@ -102,7 +97,10 @@ if (GTSAM_BUILD_TESTS)
endif()
# 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
include_directories(
@ -137,6 +135,11 @@ if (GTSAM_BUILD_UNSTABLE)
add_subdirectory(gtsam_unstable)
endif(GTSAM_BUILD_UNSTABLE)
# Make config files
include(GtsamMakeConfigFile)
GtsamMakeConfigFile(GTSAM gtsam-static)
GtsamMakeConfigFile(CppUnitLite CppUnitLite)
# Set up CPack
set(CPACK_PACKAGE_DESCRIPTION_SUMMARY "GTSAM")
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})
gtsam_assign_source_folders("${cppunitlite_headers};${cppunitlite_src}") # MSVC project structure
option(GTSAM_INSTALL_CPPUNITLITE "Enable/Disable installation of CppUnitLite library" ON)
if (GTSAM_INSTALL_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
FILE(GLOB example_srcs "*.cpp")
@ -6,7 +8,9 @@ foreach(example_src ${example_srcs} )
get_filename_component(example_base ${example_src} NAME_WE)
set( example_bin ${example_base} )
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})
# Disable building during make all/install
@ -15,6 +19,14 @@ foreach(example_src ${example_srcs} )
endif()
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)

2
examples/PlanarSLAMExample.cpp
View File

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

1
gtsam.h
View File

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

22
gtsam/CMakeLists.txt
View File

@ -19,9 +19,10 @@ add_subdirectory(3rdparty)
# build convenience library
set (3rdparty_srcs
3rdparty/CCOLAMD/Source/ccolamd.c
3rdparty/CCOLAMD/Source/ccolamd_global.c
3rdparty/UFconfig/UFconfig.c)
${CMAKE_CURRENT_SOURCE_DIR}/3rdparty/CCOLAMD/Source/ccolamd.c
${CMAKE_CURRENT_SOURCE_DIR}/3rdparty/CCOLAMD/Source/ccolamd_global.c
${CMAKE_CURRENT_SOURCE_DIR}/3rdparty/UFconfig/UFconfig.c)
gtsam_assign_source_folders("${3rdparty_srcs}") # Create MSVC structure
if (GTSAM_BUILD_CONVENIENCE_LIBRARIES)
message(STATUS "Building Convenience Library: ccolamd")
add_library(ccolamd STATIC ${3rdparty_srcs})
@ -46,6 +47,9 @@ endif()
foreach(subdir ${gtsam_subdirs})
# Build convenience libraries
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})
set(${subdir}_srcs ${subdir_srcs})
if (GTSAM_BUILD_CONVENIENCE_LIBRARIES)
@ -81,22 +85,32 @@ message(STATUS "Install prefix: ${CMAKE_INSTALL_PREFIX}")
if (GTSAM_BUILD_STATIC_LIBRARY)
message(STATUS "Building GTSAM - static")
add_library(gtsam-static STATIC ${gtsam_srcs})
target_link_libraries(gtsam-static ${Boost_LIBRARIES})
set_target_properties(gtsam-static PROPERTIES
OUTPUT_NAME gtsam
CLEAN_DIRECT_OUTPUT 1
VERSION ${gtsam_version}
SOVERSION ${gtsam_soversion})
if(MSVC)
set_target_properties(gtsam-static PROPERTIES
COMPILE_FLAGS "/MP")
endif()
install(TARGETS gtsam-static ARCHIVE DESTINATION lib)
endif (GTSAM_BUILD_STATIC_LIBRARY)
if (GTSAM_BUILD_SHARED_LIBRARY)
message(STATUS "Building GTSAM - shared")
add_library(gtsam-shared SHARED ${gtsam_srcs})
target_link_libraries(gtsam-shared ${Boost_LIBRARIES})
set_target_properties(gtsam-shared PROPERTIES
OUTPUT_NAME gtsam
CLEAN_DIRECT_OUTPUT 1
VERSION ${gtsam_version}
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)

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_().
*/
virtual void deallocate_() const {
this->~Value();
this->Value::~Value();
boost::singleton_pool<PoolTag, sizeof(DERIVED)>::free((void*)this);
}

2
gtsam/base/LieVector.cpp
View File

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

32
gtsam/base/Matrix.cpp
View File

@ -15,6 +15,7 @@
* @author Christian Potthast
*/
#include <gtsam/base/types.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/timing.h>
#include <gtsam/base/Vector.h>
@ -25,8 +26,8 @@
#include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp>
#include <stdarg.h>
#include <string.h>
#include <cstdarg>
#include <cstring>
#include <iomanip>
#include <list>
#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) {
big.block(i, j, small.rows(), small.cols()) = small;
//istream& operator>>(istream& inputStream, Matrix& destinationMatrix) {
// 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 <boost/format.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
/**
* 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 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;
else if(fabs(A(i,j) - B(i,j)) > tol)
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);
/**
* 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
* @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
* NOTE: there is no size checking
* @param large matrix to be updated
* @param small matrix to be inserted
* @param fullMatrix matrix to be updated
* @param subMatrix matrix to be inserted
* @param i is the row 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

37
gtsam/base/Vector.cpp
View File

@ -16,7 +16,7 @@
* @author Frank Dellaert
*/
#include <stdarg.h>
#include <cstdarg>
#include <limits>
#include <iostream>
#include <fstream>
@ -25,16 +25,17 @@
#include <cmath>
#include <boost/foreach.hpp>
#include <boost/optional.hpp>
#include <stdio.h>
#ifdef WIN32
#include <Windows.h>
#endif
#include <cstdio>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/variate_generator.hpp>
#include <gtsam/base/Vector.h>
#include <gtsam/base/types.h>
//#ifdef WIN32
//#include <Windows.h>
//#endif
using namespace std;
@ -55,11 +56,11 @@ void odprintf_(const char *format, ostream& stream, ...) {
#endif
va_end(args);
#ifdef WIN32
OutputDebugString(buf);
#else
//#ifdef WIN32
// OutputDebugString(buf);
//#else
stream << buf;
#endif
//#endif
}
/* ************************************************************************* */
@ -76,11 +77,11 @@ void odprintf(const char *format, ...) {
#endif
va_end(args);
#ifdef WIN32
OutputDebugString(buf);
#else
//#ifdef WIN32
// OutputDebugString(buf);
//#else
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;
size_t m = vec1.size();
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;
if(fabs(vec1[i] - vec2[i]) > tol)
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;
size_t m = vec1.size();
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;
if(fabs(vec1[i] - vec2[i]) > tol)
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) {
big.segment(i, small.size()) = small;
void subInsert(Vector& fullVector, const Vector& subVector, size_t i) {
fullVector.segment(i, subVector.size()) = subVector;
}
/* ************************************************************************* */

9
gtsam/base/Vector.h
View File

@ -22,7 +22,8 @@
#include <list>
#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>
/**
@ -208,11 +209,11 @@ ConstSubVector sub(const Vector &v, size_t i1, size_t i2);
/**
* Inserts a subvector into a vector IN PLACE
* @param big is the vector to be changed
* @param small is the vector to insert
* @param fullVector is the vector to be changed
* @param subVector is the vector to insert
* @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

13
gtsam/base/cholesky.cpp
View File

@ -16,6 +16,10 @@
* @date Nov 5, 2010
*/
#include <functional>
#include <boost/format.hpp>
#include <boost/bind.hpp>
#include <gtsam/base/debug.h>
#include <gtsam/base/cholesky.h>
#include <gtsam/base/timing.h>
@ -23,9 +27,6 @@
#include <gtsam/3rdparty/Eigen/Eigen/Core>
#include <gtsam/3rdparty/Eigen/Eigen/Dense>
#include <boost/format.hpp>
#include <boost/bind.hpp>
using namespace std;
namespace gtsam {
@ -129,7 +130,7 @@ void choleskyPartial(Matrix& ABC, size_t nFrontal) {
tic(1, "lld");
ABC.block(0,0,nFrontal,nFrontal).triangularView<Eigen::Upper>() =
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");
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.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;
toc(2, "compute S");
@ -150,7 +151,7 @@ void choleskyPartial(Matrix& ABC, size_t nFrontal) {
if(n - nFrontal > 0)
ABC.bottomRightCorner(n-nFrontal,n-nFrontal).selfadjointView<Eigen::Upper>().rankUpdate(
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;
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 U; ///< The produced upper-triangular 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 {
std::cout << str << "\n";
gtsam::print(A, " A: ");

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

@ -251,7 +251,7 @@ TEST( TestVector, axpy )
/* ************************************************************************* */
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);
double tol = 1.;
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");
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");
tic_("shared virtual alloc, dealloc");
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");
@ -130,14 +130,14 @@ int main(int argc, char *argv[]) {
tic_("shared plain alloc, dealloc, call");
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);
}
toc_("shared plain alloc, dealloc, call");
tic_("shared virtual alloc, dealloc, call");
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);
}
toc_("shared virtual alloc, dealloc, call");

15
gtsam/base/timing.cpp
View File

@ -20,7 +20,6 @@
#include <stdio.h>
#include <iostream>
#include <iomanip>
#include <sys/time.h>
#include <stdlib.h>
#include <boost/foreach.hpp>
#include <boost/format.hpp>
@ -153,15 +152,13 @@ const boost::shared_ptr<TimingOutline>& TimingOutline::child(size_t child, const
void TimingOutline::tic() {
assert(!timerActive_);
timerActive_ = true;
gettimeofday(&t0_, NULL);
t0_ = clock_t::now();
}
/* ************************************************************************* */
void TimingOutline::toc() {
struct timeval t;
gettimeofday(&t, NULL);
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;
}
@ -244,9 +241,11 @@ void Timing::print() {
/* ************************************************************************* */
double _tic_() {
struct timeval t;
gettimeofday(&t, NULL);
return ((double)t.tv_sec + ((double)t.tv_usec)/1000000.);
typedef boost::chrono::high_resolution_clock clock_t;
typedef boost::chrono::duration<double> duration_t;
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 <boost/shared_ptr.hpp>
#include <boost/weak_ptr.hpp>
#include <boost/chrono.hpp>
class TimingOutline;
extern boost::shared_ptr<TimingOutline> timingRoot;
@ -29,6 +30,9 @@ extern boost::weak_ptr<TimingOutline> timingCurrent;
class TimingOutline {
protected:
typedef boost::chrono::high_resolution_clock clock_t;
typedef boost::chrono::microseconds duration_t;
size_t t_;
double t2_ ; /* cache the \sum t_i^2 */
size_t tIt_;
@ -39,7 +43,7 @@ protected:
boost::weak_ptr<TimingOutline> parent_;
std::vector<boost::shared_ptr<TimingOutline> > children_;
struct timeval t0_;
clock_t::time_point t0_;
bool timerActive_;
void add(size_t usecs);

30
gtsam/base/types.h
View File

@ -19,7 +19,7 @@
#pragma once
#include <unistd.h>
#include <cstddef>
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 {
const Leaf* other = dynamic_cast<const Leaf*> (&q);
if (!other) return false;
return fabs(this->constant_ - other->constant_) < tol;
return fabs(double(this->constant_ - other->constant_)) < tol;
}
/** print */

11
gtsam/discrete/Signature.cpp
View File

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

3
gtsam/geometry/Point3.h
View File

@ -21,6 +21,7 @@
#pragma once
#include <cmath>
#include <boost/serialization/nvp.hpp>
#include <gtsam/base/Matrix.h>
@ -150,7 +151,7 @@ namespace gtsam {
/** distance between two points */
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 */

10
gtsam/geometry/StereoPoint2.h
View File

@ -119,6 +119,11 @@ namespace gtsam {
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
/// @{
@ -133,11 +138,6 @@ namespace gtsam {
return Point2(uL_, v_);
}
///TODO comment
inline StereoPoint2 between(const StereoPoint2& p2) const {
return gtsam::between_default(*this, p2);
}
private:
/// @}

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

@ -65,7 +65,7 @@ TEST( CalibratedCamera, level1)
TEST( CalibratedCamera, level2)
{
// 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);
// expected

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

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

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

@ -36,9 +36,9 @@ using namespace tensors;
/* ************************************************************************* */
// Indices
Index<3, 'a'> a, _a;
Index<3, 'b'> b, _b;
Index<3, 'c'> c, _c;
tensors::Index<3, 'a'> a, _a;
tensors::Index<3, 'b'> b, _b;
tensors::Index<3, 'c'> c, _c;
/* ************************************************************************* */
TEST( Homography2, RealImages)
@ -120,8 +120,8 @@ Homography2 patchH(const Pose3& tEc) {
namespace gtsam {
// size_t dim(const tensors::Tensor2<3, 3>& H) {return 9;}
Vector toVector(const tensors::Tensor2<3, 3>& H) {
Index<3, 'T'> _T; // covariant 2D template
Index<3, 'C'> I; // contravariant 2D camera
tensors::Index<3, 'T'> _T; // covariant 2D template
tensors::Index<3, 'C'> I; // contravariant 2D camera
return toVector(H(I,_T));
}
Vector localCoordinates(const tensors::Tensor2<3, 3>& A, const tensors::Tensor2<3, 3>& B) {
@ -134,8 +134,8 @@ namespace gtsam {
/* ************************************************************************* */
TEST( Homography2, patchH)
{
Index<3, 'T'> _T; // covariant 2D template
Index<3, 'C'> I; // contravariant 2D camera
tensors::Index<3, 'T'> _T; // covariant 2D template
tensors::Index<3, 'C'> I; // contravariant 2D camera
// data[_T][I]
double data1[3][3] = {{1,0,0},{0,-1,0},{0,0,10}};
@ -160,8 +160,8 @@ TEST( Homography2, patchH)
/* ************************************************************************* */
TEST( Homography2, patchH2)
{
Index<3, 'T'> _T; // covariant 2D template
Index<3, 'C'> I; // contravariant 2D camera
tensors::Index<3, 'T'> _T; // covariant 2D template
tensors::Index<3, 'C'> I; // contravariant 2D camera
// data[_T][I]
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)
{
Point2 p0(cos(5), sin(5));
Point2 p0(cos(5.0), sin(5.0));
DOUBLES_EQUAL(1,p0.norm(),1e-6);
Point2 p1(4, 5), p2(1, 1);
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);
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(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 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())));
}
/* ************************************************************************* */
TEST(Pose2, manifold) {
Pose2 t1(M_PI_2, Point2(1, 2));
Pose2 t2(M_PI_2+0.018, Point2(1.015, 2.01));
Pose2 t1(M_PI/2.0, Point2(1, 2));
Pose2 t2(M_PI/2.0+0.018, Point2(1.015, 2.01));
Pose2 origin;
Vector d12 = t1.localCoordinates(t2);
EXPECT(assert_equal(t2, t1.retract(d12)));
@ -63,11 +63,11 @@ TEST(Pose2, manifold) {
/* ************************************************************************* */
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
Pose2 expected(1.00811, 2.01528, 2.5608);
#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
Pose2 actual = pose.retract(Vector_(3, 0.01, -0.015, 0.99));
EXPECT(assert_equal(expected, actual, 1e-5));
@ -75,7 +75,7 @@ TEST(Pose2, retract) {
/* ************************************************************************* */
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 actual = expmap_default<Pose2>(pose, Vector_(3, 0.01, -0.015, 0.99));
EXPECT(assert_equal(expected, actual, 1e-5));
@ -83,7 +83,7 @@ TEST(Pose2, expmap) {
/* ************************************************************************* */
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 actual = expmap_default<Pose2>(pose, Vector_(3, 0.01, -0.015, 0.99));
EXPECT(assert_equal(expected, actual, 1e-5));
@ -110,11 +110,11 @@ TEST(Pose2, expmap3) {
/* ************************************************************************* */
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
Pose2 expected(1.01491, 2.01013, 1.5888);
//#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
Pose2 actual = pose * (Pose2::Expmap(Vector_(3, 0.01, -0.015, 0.018)));
EXPECT(assert_equal(expected, actual, 1e-5));
@ -122,7 +122,7 @@ TEST(Pose2, expmap0) {
/* ************************************************************************* */
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 actual = pose * Pose2::Expmap(Vector_(3, 0.01, -0.015, 0.018));
EXPECT(assert_equal(expected, actual, 1e-5));
@ -130,7 +130,7 @@ TEST(Pose2, expmap0_full) {
/* ************************************************************************* */
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 actual = pose * Pose2::Expmap(Vector_(3, 0.01, -0.015, 0.018));
EXPECT(assert_equal(expected, actual, 1e-5));
@ -170,8 +170,8 @@ TEST(Pose3, expmap_c_full)
/* ************************************************************************* */
TEST(Pose2, logmap) {
Pose2 pose0(M_PI_2, Point2(1, 2));
Pose2 pose(M_PI_2+0.018, Point2(1.015, 2.01));
Pose2 pose0(M_PI/2.0, Point2(1, 2));
Pose2 pose(M_PI/2.0+0.018, Point2(1.015, 2.01));
#ifdef SLOW_BUT_CORRECT_EXPMAP
Vector expected = Vector_(3, 0.00986473, -0.0150896, 0.018);
#else
@ -183,8 +183,8 @@ TEST(Pose2, logmap) {
/* ************************************************************************* */
TEST(Pose2, logmap_full) {
Pose2 pose0(M_PI_2, Point2(1, 2));
Pose2 pose(M_PI_2+0.018, Point2(1.015, 2.01));
Pose2 pose0(M_PI/2.0, Point2(1, 2));
Pose2 pose(M_PI/2.0+0.018, Point2(1.015, 2.01));
Vector expected = Vector_(3, 0.00986473, -0.0150896, 0.018);
Vector actual = logmap_default<Pose2>(pose0, pose);
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 )
{
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)
// expected
@ -226,7 +226,7 @@ Point2 transform_from_proxy(const Pose2& pose, const Point2& point) {
TEST (Pose2, transform_from)
{
Pose2 pose(1., 0., M_PI_2);
Pose2 pose(1., 0., M_PI/2.0);
Point2 pt(2., 1.);
Matrix H1, H2;
Point2 actual = pose.transform_from(pt, H1, H2);
@ -326,7 +326,7 @@ TEST(Pose2, compose_c)
TEST(Pose2, inverse )
{
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();
EXPECT(assert_equal(identity,lTg.compose(gTl)));
@ -362,7 +362,7 @@ Matrix matrix(const Pose2& gTl) {
TEST( Pose2, matrix )
{
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);
EXPECT(assert_equal(Matrix_(3,3,
0.0, -1.0, 1.0,
@ -393,7 +393,7 @@ TEST( Pose2, 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
Matrix gM1(matrix(gT1)),_1M2(matrix(_1T2));
EXPECT(assert_equal(gM1*_1M2,matrix(gT1.compose(_1T2)))); // RIGHT DOES NOT
@ -407,11 +407,11 @@ TEST( Pose2, between )
// ^
//
// *--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
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 actual2 = gT1.between(gT2,actualH1,actualH2);
EXPECT(assert_equal(expected,actual1));
@ -443,7 +443,7 @@ TEST( Pose2, between )
// reverse situation for extra test
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
Matrix actualH1,actualH2;
@ -472,7 +472,7 @@ TEST(Pose2, members)
/* ************************************************************************* */
// 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);
/* ************************************************************************* */
@ -488,11 +488,11 @@ TEST( Pose2, bearing )
EXPECT(assert_equal(Rot2(),x1.bearing(l1)));
// 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
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
expectedH1 = numericalDerivative21(bearing_proxy, x2, l3);
@ -502,7 +502,7 @@ TEST( Pose2, bearing )
// establish bearing is indeed 45 degrees even if rotated
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
expectedH1 = numericalDerivative21(bearing_proxy, x3, l4);
@ -518,7 +518,7 @@ Rot2 bearing_pose_proxy(const Pose2& pose, const Pose2& pt) {
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;
@ -526,11 +526,11 @@ TEST( Pose2, bearing_pose )
EXPECT(assert_equal(Rot2(),x1.bearing(xl1)));
// 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
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
expectedH1 = numericalDerivative21(bearing_pose_proxy, x2, xl3);
@ -540,7 +540,7 @@ TEST( Pose2, bearing_pose )
// establish bearing is indeed 45 degrees even if rotated
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
expectedH1 = numericalDerivative21(bearing_pose_proxy, x3, xl4);
@ -561,11 +561,11 @@ TEST( Pose2, range )
EXPECT_DOUBLES_EQUAL(1,x1.range(l1),1e-9);
// 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
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
expectedH1 = numericalDerivative21(range_proxy, x2, l3);
@ -590,7 +590,7 @@ LieVector range_pose_proxy(const Pose2& pose, const Pose2& point) {
}
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;
@ -598,11 +598,11 @@ TEST( Pose2, range_pose )
EXPECT_DOUBLES_EQUAL(1,x1.range(xl1),1e-9);
// 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
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
expectedH1 = numericalDerivative21(range_pose_proxy, x2, xl3);
@ -637,7 +637,7 @@ TEST(Pose2, align_1) {
TEST(Pose2, align_2) {
Point2 t(20,10);
Rot2 R = Rot2::fromAngle(M_PI_2);
Rot2 R = Rot2::fromAngle(M_PI/2.0);
Pose2 expected(R, t);
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
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
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)),
@ -554,11 +554,11 @@ TEST( Pose3, range )
EXPECT_DOUBLES_EQUAL(1,x1.range(l1),1e-9);
// 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
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
expectedH1 = numericalDerivative21(range_proxy, x2, l3);
@ -589,11 +589,11 @@ TEST( Pose3, range_pose )
EXPECT_DOUBLES_EQUAL(1,x1.range(xl1),1e-9);
// 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
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
expectedH1 = numericalDerivative21(range_pose_proxy, x2, xl3);
@ -619,7 +619,7 @@ TEST( Pose3, unicycle )
Vector x_step = delta(6,3,1.0);
EXPECT(assert_equal(Pose3(Rot3::ypr(0,0,0), l1), expmap_default<Pose3>(x1, x_step), tol));
EXPECT(assert_equal(Pose3(Rot3::ypr(0,0,0), Point3(2,1,0)), expmap_default<Pose3>(x2, x_step), tol));
EXPECT(assert_equal(Pose3(Rot3::ypr(M_PI_4,0,0), 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)
{
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)
{
Rot2 rot0(Rot2::fromAngle(M_PI_2));
Rot2 rot0(Rot2::fromAngle(M_PI/2.0));
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);
CHECK(assert_equal(expected, actual));
}
@ -146,7 +146,7 @@ TEST( Rot2, relativeBearing )
// establish relativeBearing is indeed 45 degrees
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
expectedH = numericalDerivative11(relativeBearing_, l2);

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

@ -131,7 +131,7 @@ TEST( Rot3, rodriguez3)
TEST( Rot3, rodriguez4)
{
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);
double c=cos(angle),s=sin(angle);
Rot3 expected(c,-s, 0,

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

@ -53,7 +53,7 @@ TEST( SimpleCamera, constructor)
TEST( SimpleCamera, level2)
{
// 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);
// expected

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

@ -34,12 +34,12 @@ using namespace tensors;
/* ************************************************************************* */
// Indices
Index<3, 'a'> a, _a;
Index<3, 'b'> b, _b;
Index<3, 'c'> c, _c;
tensors::Index<3, 'a'> a, _a;
tensors::Index<3, 'b'> b, _b;
tensors::Index<3, 'c'> c, _c;
Index<4, 'A'> A;
Index<4, 'B'> B;
tensors::Index<4, 'A'> A;
tensors::Index<4, 'B'> B;
/* ************************************************************************* */
// Tensor1
@ -58,7 +58,7 @@ TEST(Tensor1, Basics)
CHECK(assert_equivalent(p(a),q(a))) // projectively equivalent
// 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>
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.separatorSizes.push_back((*clique)->nrParents());
BOOST_FOREACH(sharedClique c, clique->children_) {
@ -74,7 +74,7 @@ namespace gtsam {
template<class CONDITIONAL, class CLIQUE>
void BayesTree<CONDITIONAL,CLIQUE>::saveGraph(ostream &s,
BayesTree<CONDITIONAL,CLIQUE>::sharedClique clique,
typename BayesTree<CONDITIONAL,CLIQUE>::sharedClique clique,
int parentnum) const {
static int num = 0;
bool first = true;

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

@ -58,7 +58,7 @@ namespace gtsam {
/* ************************************************************************* */
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 (separator != other.separator) 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>
typename BayesTree<typename F::ConditionalType>::shared_ptr GenericMultifrontalSolver<F, JT>::eliminate(
typename FactorGraph<F>::Eliminate function) const {
template<class FACTOR, class JUNCTIONTREE>
typename BayesTree<typename FACTOR::ConditionalType>::shared_ptr GenericMultifrontalSolver<FACTOR, JUNCTIONTREE>::eliminate(Eliminate function) const {
// 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
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);
// 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;
POSE l1Xl2 = factor->measured();
tie(edge12, found1) = boost::edge(v1, v2, g);
tie(edge21, found2) = boost::edge(v2, v1, g);
boost::tie(edge12, found1) = boost::edge(v1, v2, g);
boost::tie(edge21, found2) = boost::edge(v2, v1, g);
if (found1 && found2) throw invalid_argument ("composePoses: invalid spanning tree");
if (!found1 && !found2) continue;
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) :
IndexConditional(key, GetKeys(parents.size(), parents.begin(), parents.end())), rsd_(matrix_), sigmas_(sigmas) {
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();
size_t j=1;
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) :
IndexConditional(GetKeys(terms.size(), terms.begin(), terms.end()), nrFrontals),
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;
typedef pair<Index, Matrix> Index_Matrix;
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
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;
for(unsigned int i = 0; i < variable_count; ++i){
dims[i] = gs[i].size();
@ -350,7 +350,7 @@ void HessianFactor::updateATA(const HessianFactor& update, const Scatter& scatte
// First build an array of 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;
BOOST_FOREACH(Index j, update) {
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
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;
BOOST_FOREACH(Index j, update) {
slots[slot] = scatter.find(j)->second.slot;

618
gtsam/linear/HessianFactor.h
View File

@ -1,274 +1,274 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file HessianFactor.h
* @brief Contains the HessianFactor class, a general quadratic factor
* @author Richard Roberts
* @date Dec 8, 2010
*/
#pragma once
#include <gtsam/base/blockMatrices.h>
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/linear/GaussianFactor.h>
// Forward declarations for friend unit tests
class ConversionConstructorHessianFactorTest;
class Constructor1HessianFactorTest;
class Constructor1bHessianFactorTest;
class combineHessianFactorTest;
namespace gtsam {
// Forward declarations
class JacobianFactor;
class SharedDiagonal;
class GaussianConditional;
template<class C> class BayesNet;
// Definition of Scatter, which is an intermediate data structure used when
// building a HessianFactor incrementally, to get the keys in the right
// order.
struct SlotEntry {
size_t slot;
size_t dimension;
SlotEntry(size_t _slot, size_t _dimension)
: slot(_slot), dimension(_dimension) {}
std::string toString() const;
};
typedef FastMap<Index, SlotEntry> Scatter;
/**
* @brief A Gaussian factor using the canonical parameters (information form)
*
* HessianFactor implements a general quadratic factor of the form
* \f[ E(x) = 0.5 x^T G x - x^T g + 0.5 f \f]
* that stores the matrix \f$ G \f$, the vector \f$ g \f$, and the constant term \f$ f \f$.
*
* When \f$ G \f$ is positive semidefinite, this factor represents a Gaussian,
* in which case \f$ G \f$ is the information matrix \f$ \Lambda \f$,
* \f$ g \f$ is the information vector \f$ \eta \f$, and \f$ f \f$ is the residual
* sum-square-error at the mean, when \f$ x = \mu \f$.
*
* Indeed, the negative log-likelihood of a Gaussian is (up to a constant)
* @f$ E(x) = 0.5(x-\mu)^T P^{-1} (x-\mu) @f$
* with @f$ \mu @f$ the mean and @f$ P @f$ the covariance matrix. Expanding the product we get
* @f[
* E(x) = 0.5 x^T P^{-1} x - x^T P^{-1} \mu + 0.5 \mu^T P^{-1} \mu
* @f]
* We define the Information matrix (or Hessian) @f$ \Lambda = P^{-1} @f$
* and the information vector @f$ \eta = P^{-1} \mu = \Lambda \mu @f$
* to arrive at the canonical form of the Gaussian:
* @f[
* E(x) = 0.5 x^T \Lambda x - x^T \eta + 0.5 \mu^T \Lambda \mu
* @f]
*
* This factor is one of the factors that can be in a GaussianFactorGraph.
* It may be returned from NonlinearFactor::linearize(), but is also
* used internally to store the Hessian during Cholesky elimination.
*
* This can represent a quadratic factor with characteristics that cannot be
* represented using a JacobianFactor (which has the form
* \f$ E(x) = \Vert Ax - b \Vert^2 \f$ and stores the Jacobian \f$ A \f$
* and error vector \f$ b \f$, i.e. is a sum-of-squares factor). For example,
* a HessianFactor need not be positive semidefinite, it can be indefinite or
* even negative semidefinite.
*
* If a HessianFactor is indefinite or negative semi-definite, then in order
* for solving the linear system to be possible,
* the Hessian of the full system must be positive definite (i.e. when all
* small Hessians are combined, the result must be positive definite). If
* this is not the case, an error will occur during elimination.
*
* This class stores G, g, and f as an augmented matrix HessianFactor::matrix_.
* The upper-left n x n blocks of HessianFactor::matrix_ store the upper-right
* triangle of G, the upper-right-most column of length n of HessianFactor::matrix_
* stores g, and the lower-right entry of HessianFactor::matrix_ stores f, i.e.
* \code
HessianFactor::matrix_ = [ G11 G12 G13 ... g1
0 G22 G23 ... g2
0 0 G33 ... g3
: : : :
0 0 0 ... f ]
\endcode
Blocks can be accessed as follows:
\code
G11 = info(begin(), begin());
G12 = info(begin(), begin()+1);
G23 = info(begin()+1, begin()+2);
g2 = linearTerm(begin()+1);
f = constantTerm();
.......
\endcode
*/
class HessianFactor : public GaussianFactor {
protected:
typedef Matrix InfoMatrix; ///< The full augmented Hessian
typedef SymmetricBlockView<InfoMatrix> BlockInfo; ///< A blockwise view of the Hessian
typedef GaussianFactor Base; ///< Typedef to base class
InfoMatrix matrix_; ///< The full augmented information matrix, s.t. the quadratic error is 0.5*[x -1]'*H*[x -1]
BlockInfo info_; ///< The block view of the full information matrix.
/** Update the factor by adding the information from the JacobianFactor
* (used internally during elimination).
* @param update The JacobianFactor containing the new information to add
* @param scatter A mapping from variable index to slot index in this HessianFactor
*/
void updateATA(const JacobianFactor& update, const Scatter& scatter);
/** Update the factor by adding the information from the HessianFactor
* (used internally during elimination).
* @param update The HessianFactor containing the new information to add
* @param scatter A mapping from variable index to slot index in this HessianFactor
*/
void updateATA(const HessianFactor& update, const Scatter& scatter);
public:
typedef boost::shared_ptr<HessianFactor> shared_ptr; ///< A shared_ptr to this
typedef BlockInfo::Block Block; ///< A block from the Hessian matrix
typedef BlockInfo::constBlock constBlock; ///< A block from the Hessian matrix (const version)
typedef BlockInfo::Column Column; ///< A column containing the linear term h
typedef BlockInfo::constColumn constColumn; ///< A column containing the linear term h (const version)
/** Copy constructor */
HessianFactor(const HessianFactor& gf);
/** default constructor for I/O */
HessianFactor();
/** Construct a unary factor. G is the quadratic term (Hessian matrix), g
* the linear term (a vector), and f the constant term. The quadratic
* error is:
* 0.5*(f - 2*x'*g + x'*G*x)
*/
HessianFactor(Index j, const Matrix& G, const Vector& g, double f);
/** Construct a unary factor, given a mean and covariance matrix.
* error is 0.5*(x-mu)'*inv(Sigma)*(x-mu)
*/
HessianFactor(Index j, const Vector& mu, const Matrix& Sigma);
/** Construct a binary factor. Gxx are the upper-triangle blocks of the
* quadratic term (the Hessian matrix), gx the pieces of the linear vector
* term, and f the constant term.
* JacobianFactor error is \f[ 0.5* (Ax-b)' M (Ax-b) = 0.5*x'A'MAx - x'A'Mb + 0.5*b'Mb \f]
* HessianFactor error is \f[ 0.5*(x'Gx - 2x'g + f) = 0.5*x'Gx - x'*g + 0.5*f \f]
* So, with \f$ A = [A1 A2] \f$ and \f$ G=A*'M*A = [A1';A2']*M*[A1 A2] \f$ we have
\code
n1*n1 G11 = A1'*M*A1
n1*n2 G12 = A1'*M*A2
n2*n2 G22 = A2'*M*A2
n1*1 g1 = A1'*M*b
n2*1 g2 = A2'*M*b
1*1 f = b'*M*b
\endcode
*/
HessianFactor(Index j1, Index j2,
const Matrix& G11, const Matrix& G12, const Vector& g1,
const Matrix& G22, const Vector& g2, double f);
/** Construct a ternary factor. Gxx are the upper-triangle blocks of the
* quadratic term (the Hessian matrix), gx the pieces of the linear vector
* term, and f the constant term.
*/
HessianFactor(Index j1, Index j2, Index j3,
const Matrix& G11, const Matrix& G12, const Matrix& G13, const Vector& g1,
const Matrix& G22, const Matrix& G23, const Vector& g2,
const Matrix& G33, const Vector& g3, double f);
/** Construct an n-way factor. Gs contains the upper-triangle blocks of the
* quadratic term (the Hessian matrix) provided in row-order, gs the pieces
* of the linear vector term, and f the constant term.
*/
HessianFactor(const std::vector<Index>& js, const std::vector<Matrix>& Gs,
const std::vector<Vector>& gs, double f);
/** Construct from Conditional Gaussian */
HessianFactor(const GaussianConditional& cg);
/** Convert from a JacobianFactor (computes A^T * A) or HessianFactor */
HessianFactor(const GaussianFactor& factor);
/** Special constructor used in EliminateCholesky which combines the given factors */
HessianFactor(const FactorGraph<GaussianFactor>& factors,
const std::vector<size_t>& dimensions, const Scatter& scatter);
/** Destructor */
virtual ~HessianFactor() {}
/** Aassignment operator */
HessianFactor& operator=(const HessianFactor& rhs);
/** Clone this JacobianFactor */
virtual GaussianFactor::shared_ptr clone() const {
return boost::static_pointer_cast<GaussianFactor>(
shared_ptr(new HessianFactor(*this)));
}
/** Print the factor for debugging and testing (implementing Testable) */
virtual void print(const std::string& s = "") const;
/** Compare to another factor for testing (implementing Testable) */
virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const;
/** Evaluate the factor error f(x), see above. */
virtual double error(const VectorValues& c) const; /** 0.5*[x -1]'*H*[x -1] (also see constructor documentation) */
/** Return the dimension of the variable pointed to by the given key iterator
* todo: Remove this in favor of keeping track of dimensions with variables?
* @param variable An iterator pointing to the slot in this factor. You can
* use, for example, begin() + 2 to get the 3rd variable in this factor.
*/
virtual size_t getDim(const_iterator variable) const { return info_(variable-this->begin(), 0).rows(); }
/** Return the number of columns and rows of the Hessian matrix */
size_t rows() const { return info_.rows(); }
/** Return a view of the block at (j1,j2) of the <emph>upper-triangular part</emph> of the
* information matrix \f$ H \f$, no data is copied. See HessianFactor class documentation
* above to explain that only the upper-triangular part of the information matrix is stored
* and returned by this function.
* @param j1 Which block row to get, as an iterator pointing to the slot in this factor. You can
* use, for example, begin() + 2 to get the 3rd variable in this factor.
* @param j2 Which block column to get, as an iterator pointing to the slot in this factor. You can
* use, for example, begin() + 2 to get the 3rd variable in this factor.
* @return A view of the requested block, not a copy.
*/
constBlock info(const_iterator j1, const_iterator j2) const { return info_(j1-begin(), j2-begin()); }
/** Return the <emph>upper-triangular part</emph> of the full *augmented* information matrix,
* as described above. See HessianFactor class documentation above to explain that only the
* upper-triangular part of the information matrix is stored and returned by this function.
*/
constBlock info() const { return info_.full(); }
/** Return the constant term \f$ f \f$ as described above
* @return The constant term \f$ f \f$
*/
double constantTerm() const;
/** Return the part of linear term \f$ g \f$ as described above corresponding to the requested variable.
* @param j Which block row to get, as an iterator pointing to the slot in this factor. You can
* use, for example, begin() + 2 to get the 3rd variable in this factor.
* @return The linear term \f$ g \f$ */
constColumn linearTerm(const_iterator j) const { return info_.column(j-begin(), size(), 0); }
/** Return the complete linear term \f$ g \f$ as described above.
* @return The linear term \f$ g \f$ */
constColumn linearTerm() const;
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file HessianFactor.h
* @brief Contains the HessianFactor class, a general quadratic factor
* @author Richard Roberts
* @date Dec 8, 2010
*/
#pragma once
#include <gtsam/base/blockMatrices.h>
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/linear/GaussianFactor.h>
// Forward declarations for friend unit tests
class ConversionConstructorHessianFactorTest;
class Constructor1HessianFactorTest;
class Constructor1bHessianFactorTest;
class combineHessianFactorTest;
namespace gtsam {
// Forward declarations
class JacobianFactor;
class SharedDiagonal;
class GaussianConditional;
template<class C> class BayesNet;
// Definition of Scatter, which is an intermediate data structure used when
// building a HessianFactor incrementally, to get the keys in the right
// order.
struct SlotEntry {
size_t slot;
size_t dimension;
SlotEntry(size_t _slot, size_t _dimension)
: slot(_slot), dimension(_dimension) {}
std::string toString() const;
};
typedef FastMap<Index, SlotEntry> Scatter;
/**
* @brief A Gaussian factor using the canonical parameters (information form)
*
* HessianFactor implements a general quadratic factor of the form
* \f[ E(x) = 0.5 x^T G x - x^T g + 0.5 f \f]
* that stores the matrix \f$ G \f$, the vector \f$ g \f$, and the constant term \f$ f \f$.
*
* When \f$ G \f$ is positive semidefinite, this factor represents a Gaussian,
* in which case \f$ G \f$ is the information matrix \f$ \Lambda \f$,
* \f$ g \f$ is the information vector \f$ \eta \f$, and \f$ f \f$ is the residual
* sum-square-error at the mean, when \f$ x = \mu \f$.
*
* Indeed, the negative log-likelihood of a Gaussian is (up to a constant)
* @f$ E(x) = 0.5(x-\mu)^T P^{-1} (x-\mu) @f$
* with @f$ \mu @f$ the mean and @f$ P @f$ the covariance matrix. Expanding the product we get
* @f[
* E(x) = 0.5 x^T P^{-1} x - x^T P^{-1} \mu + 0.5 \mu^T P^{-1} \mu
* @f]
* We define the Information matrix (or Hessian) @f$ \Lambda = P^{-1} @f$
* and the information vector @f$ \eta = P^{-1} \mu = \Lambda \mu @f$
* to arrive at the canonical form of the Gaussian:
* @f[
* E(x) = 0.5 x^T \Lambda x - x^T \eta + 0.5 \mu^T \Lambda \mu
* @f]
*
* This factor is one of the factors that can be in a GaussianFactorGraph.
* It may be returned from NonlinearFactor::linearize(), but is also
* used internally to store the Hessian during Cholesky elimination.
*
* This can represent a quadratic factor with characteristics that cannot be
* represented using a JacobianFactor (which has the form
* \f$ E(x) = \Vert Ax - b \Vert^2 \f$ and stores the Jacobian \f$ A \f$
* and error vector \f$ b \f$, i.e. is a sum-of-squares factor). For example,
* a HessianFactor need not be positive semidefinite, it can be indefinite or
* even negative semidefinite.
*
* If a HessianFactor is indefinite or negative semi-definite, then in order
* for solving the linear system to be possible,
* the Hessian of the full system must be positive definite (i.e. when all
* small Hessians are combined, the result must be positive definite). If
* this is not the case, an error will occur during elimination.
*
* This class stores G, g, and f as an augmented matrix HessianFactor::matrix_.
* The upper-left n x n blocks of HessianFactor::matrix_ store the upper-right
* triangle of G, the upper-right-most column of length n of HessianFactor::matrix_
* stores g, and the lower-right entry of HessianFactor::matrix_ stores f, i.e.
* \code
HessianFactor::matrix_ = [ G11 G12 G13 ... g1
0 G22 G23 ... g2
0 0 G33 ... g3
: : : :
0 0 0 ... f ]
\endcode
Blocks can be accessed as follows:
\code
G11 = info(begin(), begin());
G12 = info(begin(), begin()+1);
G23 = info(begin()+1, begin()+2);
g2 = linearTerm(begin()+1);
f = constantTerm();
.......
\endcode
*/
class HessianFactor : public GaussianFactor {
protected:
typedef Matrix InfoMatrix; ///< The full augmented Hessian
typedef SymmetricBlockView<InfoMatrix> BlockInfo; ///< A blockwise view of the Hessian
typedef GaussianFactor Base; ///< Typedef to base class
InfoMatrix matrix_; ///< The full augmented information matrix, s.t. the quadratic error is 0.5*[x -1]'*H*[x -1]
BlockInfo info_; ///< The block view of the full information matrix.
/** Update the factor by adding the information from the JacobianFactor
* (used internally during elimination).
* @param update The JacobianFactor containing the new information to add
* @param scatter A mapping from variable index to slot index in this HessianFactor
*/
void updateATA(const JacobianFactor& update, const Scatter& scatter);
/** Update the factor by adding the information from the HessianFactor
* (used internally during elimination).
* @param update The HessianFactor containing the new information to add
* @param scatter A mapping from variable index to slot index in this HessianFactor
*/
void updateATA(const HessianFactor& update, const Scatter& scatter);
public:
typedef boost::shared_ptr<HessianFactor> shared_ptr; ///< A shared_ptr to this
typedef BlockInfo::Block Block; ///< A block from the Hessian matrix
typedef BlockInfo::constBlock constBlock; ///< A block from the Hessian matrix (const version)
typedef BlockInfo::Column Column; ///< A column containing the linear term h
typedef BlockInfo::constColumn constColumn; ///< A column containing the linear term h (const version)
/** Copy constructor */
HessianFactor(const HessianFactor& gf);
/** default constructor for I/O */
HessianFactor();
/** Construct a unary factor. G is the quadratic term (Hessian matrix), g
* the linear term (a vector), and f the constant term. The quadratic
* error is:
* 0.5*(f - 2*x'*g + x'*G*x)
*/
HessianFactor(Index j, const Matrix& G, const Vector& g, double f);
/** Construct a unary factor, given a mean and covariance matrix.
* error is 0.5*(x-mu)'*inv(Sigma)*(x-mu)
*/
HessianFactor(Index j, const Vector& mu, const Matrix& Sigma);
/** Construct a binary factor. Gxx are the upper-triangle blocks of the
* quadratic term (the Hessian matrix), gx the pieces of the linear vector
* term, and f the constant term.
* JacobianFactor error is \f[ 0.5* (Ax-b)' M (Ax-b) = 0.5*x'A'MAx - x'A'Mb + 0.5*b'Mb \f]
* HessianFactor error is \f[ 0.5*(x'Gx - 2x'g + f) = 0.5*x'Gx - x'*g + 0.5*f \f]
* So, with \f$ A = [A1 A2] \f$ and \f$ G=A*'M*A = [A1';A2']*M*[A1 A2] \f$ we have
\code
n1*n1 G11 = A1'*M*A1
n1*n2 G12 = A1'*M*A2
n2*n2 G22 = A2'*M*A2
n1*1 g1 = A1'*M*b
n2*1 g2 = A2'*M*b
1*1 f = b'*M*b
\endcode
*/
HessianFactor(Index j1, Index j2,
const Matrix& G11, const Matrix& G12, const Vector& g1,
const Matrix& G22, const Vector& g2, double f);
/** Construct a ternary factor. Gxx are the upper-triangle blocks of the
* quadratic term (the Hessian matrix), gx the pieces of the linear vector
* term, and f the constant term.
*/
HessianFactor(Index j1, Index j2, Index j3,
const Matrix& G11, const Matrix& G12, const Matrix& G13, const Vector& g1,
const Matrix& G22, const Matrix& G23, const Vector& g2,
const Matrix& G33, const Vector& g3, double f);
/** Construct an n-way factor. Gs contains the upper-triangle blocks of the
* quadratic term (the Hessian matrix) provided in row-order, gs the pieces
* of the linear vector term, and f the constant term.
*/
HessianFactor(const std::vector<Index>& js, const std::vector<Matrix>& Gs,
const std::vector<Vector>& gs, double f);
/** Construct from Conditional Gaussian */
HessianFactor(const GaussianConditional& cg);
/** Convert from a JacobianFactor (computes A^T * A) or HessianFactor */
HessianFactor(const GaussianFactor& factor);
/** Special constructor used in EliminateCholesky which combines the given factors */
HessianFactor(const FactorGraph<GaussianFactor>& factors,
const std::vector<size_t>& dimensions, const Scatter& scatter);
/** Destructor */
virtual ~HessianFactor() {}
/** Aassignment operator */
HessianFactor& operator=(const HessianFactor& rhs);
/** Clone this JacobianFactor */
virtual GaussianFactor::shared_ptr clone() const {
return boost::static_pointer_cast<GaussianFactor>(
shared_ptr(new HessianFactor(*this)));
}
/** Print the factor for debugging and testing (implementing Testable) */
virtual void print(const std::string& s = "") const;
/** Compare to another factor for testing (implementing Testable) */
virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const;
/** Evaluate the factor error f(x), see above. */
virtual double error(const VectorValues& c) const; /** 0.5*[x -1]'*H*[x -1] (also see constructor documentation) */
/** Return the dimension of the variable pointed to by the given key iterator
* todo: Remove this in favor of keeping track of dimensions with variables?
* @param variable An iterator pointing to the slot in this factor. You can
* use, for example, begin() + 2 to get the 3rd variable in this factor.
*/
virtual size_t getDim(const_iterator variable) const { return info_(variable-this->begin(), 0).rows(); }
/** Return the number of columns and rows of the Hessian matrix */
size_t rows() const { return info_.rows(); }
/** Return a view of the block at (j1,j2) of the <emph>upper-triangular part</emph> of the
* information matrix \f$ H \f$, no data is copied. See HessianFactor class documentation
* above to explain that only the upper-triangular part of the information matrix is stored
* and returned by this function.
* @param j1 Which block row to get, as an iterator pointing to the slot in this factor. You can
* use, for example, begin() + 2 to get the 3rd variable in this factor.
* @param j2 Which block column to get, as an iterator pointing to the slot in this factor. You can
* use, for example, begin() + 2 to get the 3rd variable in this factor.
* @return A view of the requested block, not a copy.
*/
constBlock info(const_iterator j1, const_iterator j2) const { return info_(j1-begin(), j2-begin()); }
/** Return the <emph>upper-triangular part</emph> of the full *augmented* information matrix,
* as described above. See HessianFactor class documentation above to explain that only the
* upper-triangular part of the information matrix is stored and returned by this function.
*/
constBlock info() const { return info_.full(); }
/** Return the constant term \f$ f \f$ as described above
* @return The constant term \f$ f \f$
*/
double constantTerm() const;
/** Return the part of linear term \f$ g \f$ as described above corresponding to the requested variable.
* @param j Which block row to get, as an iterator pointing to the slot in this factor. You can
* use, for example, begin() + 2 to get the 3rd variable in this factor.
* @return The linear term \f$ g \f$ */
constColumn linearTerm(const_iterator j) const { return info_.column(j-begin(), size(), 0); }
/** Return the complete linear term \f$ g \f$ as described above.
* @return The linear term \f$ g \f$ */
constColumn linearTerm() const;
/** Return the augmented information matrix represented by this GaussianFactor.
* The augmented information matrix contains the information matrix with an
@ -286,41 +286,41 @@ namespace gtsam {
* upper triangle.
*/
virtual Matrix computeInformation() const;
// Friend unit test classes
friend class ::ConversionConstructorHessianFactorTest;
friend class ::Constructor1HessianFactorTest;
friend class ::Constructor1bHessianFactorTest;
friend class ::combineHessianFactorTest;
// Friend JacobianFactor for conversion
friend class JacobianFactor;
// used in eliminateCholesky:
/**
* Do Cholesky. Note that after this, the lower triangle still contains
* some untouched non-zeros that should be zero. We zero them while
* extracting submatrices in splitEliminatedFactor. Frank says :-(
*/
void partialCholesky(size_t nrFrontals);
/** split partially eliminated factor */
boost::shared_ptr<GaussianConditional> splitEliminatedFactor(size_t nrFrontals);
/** assert invariants */
void assertInvariants() const;
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(GaussianFactor);
ar & BOOST_SERIALIZATION_NVP(info_);
ar & BOOST_SERIALIZATION_NVP(matrix_);
}
};
}
// Friend unit test classes
friend class ::ConversionConstructorHessianFactorTest;
friend class ::Constructor1HessianFactorTest;
friend class ::Constructor1bHessianFactorTest;
friend class ::combineHessianFactorTest;
// Friend JacobianFactor for conversion
friend class JacobianFactor;
// used in eliminateCholesky:
/**
* Do Cholesky. Note that after this, the lower triangle still contains
* some untouched non-zeros that should be zero. We zero them while
* extracting submatrices in splitEliminatedFactor. Frank says :-(
*/
void partialCholesky(size_t nrFrontals);
/** split partially eliminated factor */
boost::shared_ptr<GaussianConditional> splitEliminatedFactor(size_t nrFrontals);
/** assert invariants */
void assertInvariants() const;
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(GaussianFactor);
ar & BOOST_SERIALIZATION_NVP(info_);
ar & BOOST_SERIALIZATION_NVP(matrix_);
}
};
}

6
gtsam/linear/JacobianFactor.cpp
View File

@ -118,7 +118,7 @@ namespace gtsam {
GaussianFactor(GetKeys(terms.size(), terms.begin(), terms.end())),
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)
dims[j] = terms[j].second.cols();
dims[terms.size()] = 1;
@ -135,7 +135,7 @@ namespace gtsam {
GaussianFactor(GetKeys(terms.size(), terms.begin(), terms.end())),
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;
std::list<std::pair<Index, Matrix> >::const_iterator term=terms.begin();
for(; term!=terms.end(); ++term,++j)
@ -494,7 +494,7 @@ namespace gtsam {
size_t>& varDims, size_t m) {
keys_.resize(variableSlots.size());
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));
varDims.push_back(1);
Ab_.copyStructureFrom(BlockAb(matrix_, varDims.begin(), varDims.end(), m));

20
gtsam/linear/JacobianFactor.h
View File

@ -154,16 +154,16 @@ namespace gtsam {
Vector unweighted_error(const VectorValues& c) const; /** (A*x-b) */
Vector error_vector(const VectorValues& c) const; /** (A*x-b)/sigma */
virtual double error(const VectorValues& c) const; /** 0.5*(A*x-b)'*D*(A*x-b) */
/** Return the augmented information matrix represented by this GaussianFactor.
* The augmented information matrix contains the information matrix with an
* additional column holding the information vector, and an additional row
* holding the transpose of the information vector. The lower-right entry
* contains the constant error term (when \f$ \delta x = 0 \f$). The
* augmented information matrix is described in more detail in HessianFactor,
* which in fact stores an augmented information matrix.
*/
virtual Matrix computeInformation() const;
/** Return the augmented information matrix represented by this GaussianFactor.
* The augmented information matrix contains the information matrix with an
* additional column holding the information vector, and an additional row
* holding the transpose of the information vector. The lower-right entry
* contains the constant error term (when \f$ \delta x = 0 \f$). The
* augmented information matrix is described in more detail in HessianFactor,
* which in fact stores an augmented information matrix.
*/
virtual Matrix computeInformation() const;
/** Check if the factor contains no information, i.e. zero rows. This does
* not necessarily mean that the factor involves no variables (to check for

3
gtsam/linear/NoiseModel.h
View File

@ -20,6 +20,7 @@
#include <boost/serialization/nvp.hpp>
#include <gtsam/base/Matrix.h>
#include <cmath>
namespace gtsam {
@ -609,7 +610,7 @@ namespace gtsam {
virtual bool equals(const Base& expected, const double tol=1e-8) const = 0;
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
* robust function */

9
gtsam/nonlinear/DoglegOptimizerImpl.cpp
View File

@ -15,6 +15,7 @@
* @author Richard Roberts
*/
#include <cmath>
#include <gtsam/nonlinear/DoglegOptimizerImpl.h>
namespace gtsam {
@ -27,12 +28,12 @@ VectorValues DoglegOptimizerImpl::ComputeDoglegPoint(
double DeltaSq = Delta*Delta;
double x_u_norm_sq = dx_u.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) {
// Trust region is smaller than steepest descent update
VectorValues x_d = VectorValues::SameStructure(dx_u);
x_d.vector() = dx_u.vector() * 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;
x_d.vector() = dx_u.vector() * std::sqrt(DeltaSq / x_u_norm_sq);
if(verbose) cout << "In steepest descent region with fraction " << std::sqrt(DeltaSq / x_u_norm_sq) << " of steepest descent magnitude" << endl;
return x_d;
} else if(DeltaSq < x_n_norm_sq) {
// 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 b = 2. * (un - uu);
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
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/Symbol.h> // for selective linearization thresholds
#include <gtsam/base/debug.h>
#include <functional>
#include <boost/range/adaptors.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;
}
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]) {
Value* retracted = key_value->value.retract_(delta[var]);
key_value->value = *retracted;
@ -307,7 +308,7 @@ size_t ISAM2::Impl::UpdateDelta(const boost::shared_ptr<ISAM2Clique>& root, std:
#ifndef NDEBUG
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
}

16
gtsam/nonlinear/ISAM2.cpp
View File

@ -348,7 +348,7 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
// Reeliminated keys for detailed results
if(params_.enableDetailedResults) {
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
if(params_.enableDetailedResults) {
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
if(params_.enableDetailedResults) {
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
if(params_.enableDetailedResults) {
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
@ -732,10 +732,10 @@ Values ISAM2::calculateEstimate() const {
// We use ExpmapMasked here instead of regular expmap because the former
// handles Permuted<VectorValues>
tic(1, "Copy Values");
Values ret(theta_);
toc(1, "Copy Values");
tic(2, "getDelta");
const Permuted<VectorValues>& delta(getDelta());
Values ret(theta_);
toc(1, "Copy Values");
tic(2, "getDelta");
const Permuted<VectorValues>& delta(getDelta());
toc(2, "getDelta");
tic(3, "Expmap");
vector<bool> mask(ordering_.nVars(), true);

4
gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
View File

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

6
gtsam/nonlinear/Marginals.h
View File

@ -116,6 +116,12 @@ public:
*/
Block operator()(Key iVariable, Key jVariable) const {
return blockView_(indices_[iVariable], indices_[jVariable]); }
/** Copy constructor */
JointMarginal(const JointMarginal& other);
/** Assignment operator */
JointMarginal& operator=(const JointMarginal& rhs);
};
} /* 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}")
endif(GTSAM_BUILD_TESTS)
if(MSVC)
add_definitions("/bigobj")
endif()
# Build timing scripts
if (GTSAM_BUILD_TIMING)
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 ;
}
shared_ptr<Ordering> getOrdering(const vector<GeneralCamera>& cameras, const vector<Point3>& landmarks) {
shared_ptr<Ordering> ordering(new Ordering);
boost::shared_ptr<Ordering> getOrdering(const vector<GeneralCamera>& cameras, const vector<Point3>& landmarks) {
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 < cameras.size() ; ++i ) ordering->push_back(X(i)) ;
return ordering ;

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

@ -76,7 +76,7 @@ TEST( planarSLAM, BearingFactor_equals )
TEST( planarSLAM, RangeFactor )
{
// 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);
// create config
@ -103,7 +103,7 @@ TEST( planarSLAM, BearingRangeFactor )
{
// Create factor
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);
// create config
@ -166,7 +166,7 @@ TEST( planarSLAM, constructor )
G.addBearing(i2, j3, z1, sigma);
// 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);
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 gtsam;
using boost::shared_ptr;
// Convenience for named keys
using symbol_shorthand::X;
@ -387,7 +386,7 @@ TEST(DoglegOptimizer, ComputeDoglegPoint) {
/* ************************************************************************* */
TEST(DoglegOptimizer, Iterate) {
// really non-linear factor graph
shared_ptr<example::Graph> fg(new example::Graph(
boost::shared_ptr<example::Graph> fg(new example::Graph(
example::createReallyNonlinearFactorGraph()));
// config far from minimum
@ -396,7 +395,7 @@ TEST(DoglegOptimizer, Iterate) {
config->insert(X(1), x0);
// ordering
shared_ptr<Ordering> ord(new Ordering());
boost::shared_ptr<Ordering> ord(new Ordering());
ord->push_back(X(1));
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;
// 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;
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);
@ -296,7 +296,7 @@ TEST( GaussianFactorGraph, eliminateOne_l1_fast )
GaussianConditional::shared_ptr actual = inference::eliminateOne(fg, ordering[L(1)], EliminateQR).first;
// 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;
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);

2
tests/testGraph.cpp
View File

@ -66,7 +66,7 @@ TEST( Graph, predecessorMap2Graph )
p_map.insert(1, 2);
p_map.insert(2, 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));
CHECK(root == key2vertex[2]);

2
tests/testInferenceB.cpp
View File

@ -46,7 +46,7 @@ TEST( inference, marginals )
*GaussianSequentialSolver(GaussianFactorGraph(cbn)).jointFactorGraph(xvar)).eliminate();
// 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));
}

2
tests/testMarginals.cpp
View File

@ -71,7 +71,7 @@ TEST(Marginals, planarSLAMmarginals) {
Rot2 bearing11 = Rot2::fromDegrees(45),
bearing21 = Rot2::fromDegrees(90),
bearing32 = Rot2::fromDegrees(90);
double range11 = sqrt(4+4),
double range11 = sqrt(4.0+4.0),
range21 = 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)
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
if (soft_prior)

2
tests/timeSequentialOnDataset.cpp
View File

@ -44,7 +44,7 @@ int main(int argc, char *argv[]) {
else if (argc == 4)
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
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)
// 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)
// A reference: emit an "unwrap_shared_ptr" call and de-reference the pointer
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)
add_library(wrap_lib STATIC ${wrap_srcs})
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
if (GTSAM_INSTALL_WRAP)
@ -32,6 +32,9 @@ endif(GTSAM_BUILD_TESTS)
# [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")
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(moduleName gtsam)
@ -44,8 +47,9 @@ if (NOT EXECUTABLE_OUTPUT_PATH)
endif()
# Code generation command
get_property(WRAP_EXE TARGET wrap PROPERTY LOCATION)
add_custom_target(wrap_gtsam ALL COMMAND
${EXECUTABLE_OUTPUT_PATH}/wrap
${WRAP_EXE}
${MEX_COMMAND}
${GTSAM_MEX_BIN_EXTENSION}
${CMAKE_CURRENT_SOURCE_DIR}/../

2
wrap/Method.cpp
View File

@ -73,7 +73,7 @@ void Method::matlab_wrapper(const string& classPath,
// get class pointer
// 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;
// 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_clear_actor.hpp>
#include <boost/foreach.hpp>
#include <boost/filesystem.hpp>
#include <iostream>
using namespace std;
using namespace wrap;
using namespace BOOST_SPIRIT_CLASSIC_NS;
namespace fs = boost::filesystem;
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,
const string& mexExt, const string& mexFlags) const {
string installCmd = "install -d " + toolboxPath;
system(installCmd.c_str());
fs::create_directories(toolboxPath);
// create make m-file
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 << "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 << "clear delims" << 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) {
// create directory if needed
string classPath = toolboxPath + "/@" + cls.qualifiedName();
string installCmd = "install -d " + classPath;
system(installCmd.c_str());
fs::create_directories(classPath);
// create proxy class
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
file.oss << " out[0] = wrap_shared_ptr(result.first,\"" << matlabType1 << "\");\n";
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
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
file.oss << " out[1] = wrap_shared_ptr(result.second,\"" << matlabType2 << "\");\n";
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
file.oss << " out[1] = wrap< " << return_type(true,arg2) << " >(result.second);\n";
}
else if (isPtr1)
file.oss << " out[0] = wrap_shared_ptr(result,\"" << matlabType1 << "\");\n";
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")
file.oss << " out[0] = wrap< " << return_type(true,arg1) << " >(result);\n";
}

12
wrap/matlab.h
View File

@ -315,19 +315,19 @@ class ObjectHandle {
private:
ObjectHandle* signature; // use 'this' as a unique object signature
const std::type_info* type; // type checking information
shared_ptr<T> t; // object pointer
boost::shared_ptr<T> t; // object pointer
public:
// Constructor for free-store allocated objects.
// Creates shared pointer, will delete if is last one to hold pointer
ObjectHandle(T* ptr) :
type(&typeid(T)), t(shared_ptr<T> (ptr)) {
type(&typeid(T)), t(boost::shared_ptr<T> (ptr)) {
signature = this;
}
// Constructor for shared pointers
// 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) {
signature = this;
}
@ -338,7 +338,7 @@ public:
}
// Get the actual object contained by handle
shared_ptr<T> get_object() const {
boost::shared_ptr<T> get_object() const {
return t;
}
@ -434,7 +434,7 @@ mxArray* create_object(const char *classname, mxArray* h) {
class to matlab.
*/
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);
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.
*/
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?
#ifndef UNSAFE_WRAP
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[])
{
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]);
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[])
{
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]);
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[])
{
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();
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[])
{
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();
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[])
{
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();
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[])
{
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();
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[])
{
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();
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[])
{
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();
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[])
{
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");
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[])
{
checkArguments("create_MixedPtrs",nargout,nargin-1,0);
shared_ptr<Test> self = unwrap_shared_ptr< Test >(in[0],"Test");
pair< Test, shared_ptr<Test> > result = self->create_MixedPtrs();
out[0] = wrap_shared_ptr(make_shared< Test >(result.first),"Test");
boost::shared_ptr<Test> self = unwrap_shared_ptr< Test >(in[0],"Test");
pair< Test, boost::shared_ptr<Test> > result = self->create_MixedPtrs();
out[0] = wrap_shared_ptr(boost::make_shared< Test >(result.first),"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[])
{
checkArguments("create_ptrs",nargout,nargin-1,0);
shared_ptr<Test> self = unwrap_shared_ptr< Test >(in[0],"Test");
pair< shared_ptr<Test>, shared_ptr<Test> > result = self->create_ptrs();
boost::shared_ptr<Test> self = unwrap_shared_ptr< Test >(in[0],"Test");
pair< boost::shared_ptr<Test>, boost::shared_ptr<Test> > result = self->create_ptrs();
out[0] = wrap_shared_ptr(result.first,"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[])
{
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();
}

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[])
{
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]);
shared_ptr<Point2> result = self->return_Point2Ptr(value);
boost::shared_ptr<Point2> result = self->return_Point2Ptr(value);
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[])
{
checkArguments("return_Test",nargout,nargin-1,1);
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> self = unwrap_shared_ptr< Test >(in[0],"Test");
boost::shared_ptr<Test> value = unwrap_shared_ptr< Test >(in[1], "Test");
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[])
{
checkArguments("return_TestPtr",nargout,nargin-1,1);
shared_ptr<Test> self = unwrap_shared_ptr< Test >(in[0],"Test");
shared_ptr<Test> value = unwrap_shared_ptr< Test >(in[1], "Test");
shared_ptr<Test> result = self->return_TestPtr(value);
boost::shared_ptr<Test> self = unwrap_shared_ptr< Test >(in[0],"Test");
boost::shared_ptr<Test> value = unwrap_shared_ptr< Test >(in[1], "Test");
boost::shared_ptr<Test> result = self->return_TestPtr(value);
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[])
{
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 result = self->return_bool(value);
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[])
{
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 result = self->return_double(value);
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[])
{
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");
bool result = self->return_field(t);
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[])
{
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 result = self->return_int(value);
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[])
{
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 result = self->return_matrix1(value);
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[])
{
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 result = self->return_matrix2(value);
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[])
{
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]);
Matrix A = unwrap< Matrix >(in[2]);
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[])
{
checkArguments("return_ptrs",nargout,nargin-1,2);
shared_ptr<Test> self = unwrap_shared_ptr< Test >(in[0],"Test");
shared_ptr<Test> p1 = unwrap_shared_ptr< Test >(in[1], "Test");
shared_ptr<Test> p2 = unwrap_shared_ptr< Test >(in[2], "Test");
pair< shared_ptr<Test>, shared_ptr<Test> > result = self->return_ptrs(p1,p2);
boost::shared_ptr<Test> self = unwrap_shared_ptr< Test >(in[0],"Test");
boost::shared_ptr<Test> p1 = unwrap_shared_ptr< Test >(in[1], "Test");
boost::shared_ptr<Test> p2 = unwrap_shared_ptr< Test >(in[2], "Test");
pair< boost::shared_ptr<Test>, boost::shared_ptr<Test> > result = self->return_ptrs(p1,p2);
out[0] = wrap_shared_ptr(result.first,"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[])
{
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 result = self->return_size_t(value);
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[])
{
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 result = self->return_string(value);
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[])
{
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 result = self->return_vector1(value);
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[])
{
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 result = self->return_vector2(value);
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);
double z = unwrap< double >(in[0]);
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
toolboxpath = mfilename('fullpath');
delims = find(toolboxpath == '/');
delims = find(toolboxpath == '/' | toolboxpath == '\');
toolboxpath = toolboxpath(1:(delims(end)-1));
clear delims
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[])
{
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();
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[])
{
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");
int result = self->nsArg(arg);
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[])
{
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]);
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
toolboxpath = mfilename('fullpath');
delims = find(toolboxpath == '/');
delims = find(toolboxpath == '/' | toolboxpath == '\');
toolboxpath = toolboxpath(1:(delims(end)-1));
clear delims
addpath(toolboxpath);

11
wrap/tests/testWrap.cpp
View File

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

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