From a164a66b7715b52c10c6b0ec752f4a14c0422c04 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Mon, 21 May 2012 23:38:25 +0000 Subject: [PATCH 02/35] In progress with cmake config files --- CMakeLists.txt | 7 +++++++ gtsam/CMakeLists.txt | 2 +- gtsam/base/cholesky.h | 2 +- 3 files changed, 9 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index e6c9bc422..eb55a989a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -93,6 +93,9 @@ if (GTSAM_BUILD_TESTS) endif() # Find boost +if(CYGWIN OR MSVC OR WIN32) + set(Boost_USE_STATIC_LIBS 1) +endif() find_package(Boost 1.40 COMPONENTS serialization REQUIRED) # General build settings @@ -128,6 +131,10 @@ if (GTSAM_BUILD_UNSTABLE) add_subdirectory(gtsam_unstable) endif(GTSAM_BUILD_UNSTABLE) +# Make config file +include(GtsamMakeConfigFile) +GtsamMakeConfigFile(gtsam) + # Set up CPack set(CPACK_PACKAGE_DESCRIPTION_SUMMARY "GTSAM") set(CPACK_PACKAGE_VENDOR "Frank Dellaert, Georgia Institute of Technology") diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index b3e639917..a905b6944 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -97,6 +97,6 @@ if (GTSAM_BUILD_SHARED_LIBRARY) CLEAN_DIRECT_OUTPUT 1 VERSION ${gtsam_version} SOVERSION ${gtsam_soversion}) - install(TARGETS gtsam-shared LIBRARY DESTINATION lib ) + install(TARGETS gtsam-shared DESTINATION lib ) endif(GTSAM_BUILD_SHARED_LIBRARY) diff --git a/gtsam/base/cholesky.h b/gtsam/base/cholesky.h index 50d3b24a8..46d342d24 100644 --- a/gtsam/base/cholesky.h +++ b/gtsam/base/cholesky.h @@ -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: "); From 2060f1dd22b9e960bb83076b1a60f07e871802e4 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Tue, 22 May 2012 20:37:13 +0000 Subject: [PATCH 04/35] Initial changes to compile on windows --- CMakeLists.txt | 5 ++++- gtsam/CMakeLists.txt | 4 +++- gtsam/base/timing.cpp | 7 ++----- gtsam/base/timing.h | 6 +++++- 4 files changed, 14 insertions(+), 8 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 5d2acb464..8bdbc486a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -15,7 +15,7 @@ include(GtsamTesting) include(GtsamPrinting) # guard against in-source builds -if(${CMAKE_SOURCE_DIR} STREQUAL ${CMAKE_BINARY_DIR}) +if(${CMAKE_SOURCE_DIR} STREQUAL ${CMAKE_BINARY_DIR} AND NOT MSVC) message(FATAL_ERROR "In-source builds not allowed. Please make a new directory (called a build directory) and run CMake from there. You may need to remove CMakeCache.txt. ") endif() @@ -94,6 +94,9 @@ if (GTSAM_BUILD_TESTS) endif() # Find boost +if(MSVC) + set(Boost_USE_STATIC_LIBS 1) +endif() find_package(Boost 1.40 COMPONENTS serialization REQUIRED) # General build settings diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index b3e639917..2adc83365 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -71,6 +71,8 @@ set(gtsam_srcs ${slam_srcs} ) +#gtsam_assign_source_folders("${gtsam_srcs}") + # Versions set(gtsam_version ${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSION_PATCH}) set(gtsam_soversion ${GTSAM_VERSION_MAJOR}) @@ -97,6 +99,6 @@ if (GTSAM_BUILD_SHARED_LIBRARY) CLEAN_DIRECT_OUTPUT 1 VERSION ${gtsam_version} SOVERSION ${gtsam_soversion}) - install(TARGETS gtsam-shared LIBRARY DESTINATION lib ) + install(TARGETS gtsam-shared LIBRARY DESTINATION lib ARCHIVE DESTINATION lib RUNTIME DESTINATION bin) endif(GTSAM_BUILD_SHARED_LIBRARY) diff --git a/gtsam/base/timing.cpp b/gtsam/base/timing.cpp index 1388c2f18..393791e6a 100644 --- a/gtsam/base/timing.cpp +++ b/gtsam/base/timing.cpp @@ -20,7 +20,6 @@ #include #include #include -#include #include #include #include @@ -153,15 +152,13 @@ const boost::shared_ptr& 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(clock_t::now() - t0_).count()); timerActive_ = false; } diff --git a/gtsam/base/timing.h b/gtsam/base/timing.h index 94498c3b3..56ac1a762 100644 --- a/gtsam/base/timing.h +++ b/gtsam/base/timing.h @@ -22,6 +22,7 @@ #include #include #include +#include class TimingOutline; extern boost::shared_ptr timingRoot; @@ -29,6 +30,9 @@ extern boost::weak_ptr 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 parent_; std::vector > children_; - struct timeval t0_; + clock_t::time_point t0_; bool timerActive_; void add(size_t usecs); From 25a53815e01664d52847eaa41ade058d216b18ff Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Tue, 22 May 2012 22:52:08 +0000 Subject: [PATCH 05/35] Initial changes to compile on windows --- gtsam/base/DerivedValue.h | 2 +- gtsam/base/Matrix.cpp | 1 + gtsam/base/Matrix.h | 3 ++- gtsam/base/Vector.cpp | 5 +++-- gtsam/base/cholesky.cpp | 13 +++++++------ gtsam/base/timing.cpp | 8 +++++--- gtsam/base/types.h | 16 +++++++++++++++- 7 files changed, 34 insertions(+), 14 deletions(-) diff --git a/gtsam/base/DerivedValue.h b/gtsam/base/DerivedValue.h index 1919f9540..83ecea510 100644 --- a/gtsam/base/DerivedValue.h +++ b/gtsam/base/DerivedValue.h @@ -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::free((void*)this); } diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp index 849dc3f5a..050f66ff0 100644 --- a/gtsam/base/Matrix.cpp +++ b/gtsam/base/Matrix.cpp @@ -15,6 +15,7 @@ * @author Christian Potthast */ +#include #include #include #include diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index 3a70d9b76..fd7dac1fc 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -26,6 +26,7 @@ #include #include #include +#include /** * Matrix is a typedef in the gtsam namespace @@ -87,7 +88,7 @@ bool equal_with_abs_tol(const Eigen::DenseBase& A, const Eigen::DenseBas for(size_t i=0; i tol) return false; diff --git a/gtsam/base/Vector.cpp b/gtsam/base/Vector.cpp index 27b56830c..2d319961d 100644 --- a/gtsam/base/Vector.cpp +++ b/gtsam/base/Vector.cpp @@ -35,6 +35,7 @@ #include #include +#include using namespace std; @@ -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 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 tol) return false; diff --git a/gtsam/base/cholesky.cpp b/gtsam/base/cholesky.cpp index 66bbbde55..84c03f78b 100644 --- a/gtsam/base/cholesky.cpp +++ b/gtsam/base/cholesky.cpp @@ -16,6 +16,10 @@ * @date Nov 5, 2010 */ +#include +#include +#include + #include #include #include @@ -23,9 +27,6 @@ #include #include -#include -#include - 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() = ABC.block(0,0,nFrontal,nFrontal).selfadjointView().llt().matrixU(); - assert(ABC.topLeftCorner(nFrontal,nFrontal).triangularView().toDenseMatrix().unaryExpr(&isfinite).all()); + assert(ABC.topLeftCorner(nFrontal,nFrontal).triangularView().toDenseMatrix().unaryExpr(ptr_fun(isfinite)).all()); toc(1, "lld"); if(debug) cout << "R:\n" << Eigen::MatrixXd(ABC.topLeftCorner(nFrontal,nFrontal).triangularView()) << endl; @@ -140,7 +141,7 @@ void choleskyPartial(Matrix& ABC, size_t nFrontal) { ABC.topLeftCorner(nFrontal,nFrontal).triangularView().transpose().solveInPlace( ABC.topRightCorner(nFrontal, n-nFrontal)); } - assert(ABC.topRightCorner(nFrontal, n-nFrontal).unaryExpr(&isfinite).all()); + assert(ABC.topRightCorner(nFrontal, n-nFrontal).unaryExpr(ptr_fun(isfinite)).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().rankUpdate( ABC.topRightCorner(nFrontal, n-nFrontal).transpose(), -1.0); - assert(ABC.bottomRightCorner(n-nFrontal,n-nFrontal).selfadjointView().toDenseMatrix().unaryExpr(&isfinite).all()); + assert(ABC.bottomRightCorner(n-nFrontal,n-nFrontal).selfadjointView().toDenseMatrix().unaryExpr(ptr_fun(isfinite)).all()); if(debug) cout << "L:\n" << Eigen::MatrixXd(ABC.bottomRightCorner(n-nFrontal,n-nFrontal).selfadjointView()) << endl; toc(3, "compute L"); } diff --git a/gtsam/base/timing.cpp b/gtsam/base/timing.cpp index 393791e6a..305f86a74 100644 --- a/gtsam/base/timing.cpp +++ b/gtsam/base/timing.cpp @@ -241,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 duration_t; + + clock_t::time_point t = clock_t::now(); + return boost::chrono::duration_cast< duration_t >(t.time_since_epoch()).count(); } /* ************************************************************************* */ diff --git a/gtsam/base/types.h b/gtsam/base/types.h index b6c5617a5..6106f49fd 100644 --- a/gtsam/base/types.h +++ b/gtsam/base/types.h @@ -19,7 +19,7 @@ #pragma once -#include +#include namespace gtsam { @@ -71,3 +71,17 @@ namespace gtsam { } +#ifdef _MSC_VER + +#include +using boost::math::isfinite; +using boost::math::isnan; +using boost::math::isinf; + +#include +#ifndef M_PI +#define M_PI (boost::math::constants::pi()) +#endif + +#endif + From 510e2eacac8ca366bae1e075ccb798d83393eeed Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Tue, 22 May 2012 22:52:17 +0000 Subject: [PATCH 06/35] More progress in compiling on windows --- gtsam/base/Matrix.cpp | 4 +-- gtsam/base/Vector.cpp | 28 +++++++++---------- gtsam/base/Vector.h | 1 + gtsam/base/types.h | 8 ++++++ gtsam/discrete/Signature.cpp | 11 ++++---- gtsam/geometry/Point3.h | 3 +- gtsam/inference/BayesTree-inl.h | 4 +-- gtsam/inference/ClusterTree-inl.h | 2 +- .../inference/GenericMultifrontalSolver-inl.h | 9 +++--- gtsam/linear/GaussianConditional.cpp | 4 +-- gtsam/linear/HessianFactor.cpp | 6 ++-- gtsam/linear/JacobianFactor.cpp | 6 ++-- gtsam/linear/NoiseModel.h | 3 +- gtsam/nonlinear/DoglegOptimizerImpl.cpp | 9 +++--- gtsam/nonlinear/ISAM2-impl.cpp | 5 ++-- .../nonlinear/LevenbergMarquardtOptimizer.cpp | 4 ++- gtsam/slam/pose2SLAM.cpp | 2 +- 17 files changed, 62 insertions(+), 47 deletions(-) diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp index 050f66ff0..57a45d4a1 100644 --- a/gtsam/base/Matrix.cpp +++ b/gtsam/base/Matrix.cpp @@ -26,8 +26,8 @@ #include #include -#include -#include +#include +#include #include #include #include diff --git a/gtsam/base/Vector.cpp b/gtsam/base/Vector.cpp index 2d319961d..98ad43e05 100644 --- a/gtsam/base/Vector.cpp +++ b/gtsam/base/Vector.cpp @@ -16,7 +16,7 @@ * @author Frank Dellaert */ -#include +#include #include #include #include @@ -25,11 +25,7 @@ #include #include #include -#include - -#ifdef WIN32 -#include -#endif +#include #include #include @@ -37,6 +33,10 @@ #include #include +//#ifdef WIN32 +//#include +//#endif + using namespace std; boost::minstd_rand generator(42u); @@ -56,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 } /* ************************************************************************* */ @@ -77,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 } /* ************************************************************************* */ diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index 84fd506cb..b4be74f4e 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -22,6 +22,7 @@ #include #include +#include #include #include diff --git a/gtsam/base/types.h b/gtsam/base/types.h index 6106f49fd..e9264d516 100644 --- a/gtsam/base/types.h +++ b/gtsam/base/types.h @@ -84,4 +84,12 @@ using boost::math::isinf; #endif #endif + +#ifdef min +#undef min +#endif + +#ifdef max +#undef max +#endif diff --git a/gtsam/discrete/Signature.cpp b/gtsam/discrete/Signature.cpp index ee7c1e59a..439f20864 100644 --- a/gtsam/discrete/Signature.cpp +++ b/gtsam/discrete/Signature.cpp @@ -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); diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 9eeb748db..7f7845dfb 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -21,6 +21,7 @@ #pragma once +#include #include #include @@ -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 */ diff --git a/gtsam/inference/BayesTree-inl.h b/gtsam/inference/BayesTree-inl.h index 95b070739..00cd38609 100644 --- a/gtsam/inference/BayesTree-inl.h +++ b/gtsam/inference/BayesTree-inl.h @@ -53,7 +53,7 @@ namespace gtsam { template void BayesTree::getCliqueData(CliqueData& data, - BayesTree::sharedClique clique) const { + typename BayesTree::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 void BayesTree::saveGraph(ostream &s, - BayesTree::sharedClique clique, + typename BayesTree::sharedClique clique, int parentnum) const { static int num = 0; bool first = true; diff --git a/gtsam/inference/ClusterTree-inl.h b/gtsam/inference/ClusterTree-inl.h index 0f1f823af..0a0b9e64e 100644 --- a/gtsam/inference/ClusterTree-inl.h +++ b/gtsam/inference/ClusterTree-inl.h @@ -58,7 +58,7 @@ namespace gtsam { /* ************************************************************************* */ template - bool ClusterTree::Cluster::equals(const ClusterTree::Cluster& other) const { + bool ClusterTree::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; diff --git a/gtsam/inference/GenericMultifrontalSolver-inl.h b/gtsam/inference/GenericMultifrontalSolver-inl.h index 652db02ca..b24db8484 100644 --- a/gtsam/inference/GenericMultifrontalSolver-inl.h +++ b/gtsam/inference/GenericMultifrontalSolver-inl.h @@ -49,15 +49,14 @@ namespace gtsam { } /* ************************************************************************* */ - template - typename BayesTree::shared_ptr GenericMultifrontalSolver::eliminate( - typename FactorGraph::Eliminate function) const { + template + typename BayesTree::shared_ptr GenericMultifrontalSolver::eliminate(Eliminate function) const { // eliminate junction tree, returns pointer to root - typename BayesTree::sharedClique root = junctionTree_->eliminate(function); + typename BayesTree::sharedClique root = junctionTree_->eliminate(function); // create an empty Bayes tree and insert root clique - typename BayesTree::shared_ptr bayesTree(new BayesTree); + typename BayesTree::shared_ptr bayesTree(new BayesTree); bayesTree->insert(root); // return the Bayes tree diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index de459a00f..c048c8dd8 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -73,7 +73,7 @@ GaussianConditional::GaussianConditional(Index key, const Vector& d, const Matri const Matrix& R, const list >& 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 >::const_iterator parent=parents.begin(); @@ -95,7 +95,7 @@ GaussianConditional::GaussianConditional(const std::list Index_Matrix; BOOST_FOREACH(const Index_Matrix& term, terms) { diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index b1f829005..ebe94514b 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -188,7 +188,7 @@ HessianFactor::HessianFactor(const std::vector& js, const std::vectorsecond.slot; @@ -399,7 +399,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; diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index e2facec40..37fd74c39 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -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 >::const_iterator term=terms.begin(); for(; term!=terms.end(); ++term,++j) @@ -488,7 +488,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)); diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index b6cf4deac..87391be0a 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -20,6 +20,7 @@ #include #include +#include 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 */ diff --git a/gtsam/nonlinear/DoglegOptimizerImpl.cpp b/gtsam/nonlinear/DoglegOptimizerImpl.cpp index 480d76f6a..ccfdf781a 100644 --- a/gtsam/nonlinear/DoglegOptimizerImpl.cpp +++ b/gtsam/nonlinear/DoglegOptimizerImpl.cpp @@ -15,6 +15,7 @@ * @author Richard Roberts */ +#include #include 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); diff --git a/gtsam/nonlinear/ISAM2-impl.cpp b/gtsam/nonlinear/ISAM2-impl.cpp index 8fe117e34..e8847e5c0 100644 --- a/gtsam/nonlinear/ISAM2-impl.cpp +++ b/gtsam/nonlinear/ISAM2-impl.cpp @@ -15,6 +15,7 @@ * @author Michael Kaess, Richard Roberts */ +#include #include #include #include @@ -149,7 +150,7 @@ void ISAM2::Impl::ExpmapMasked(Values& values, const Permuted& 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).all()); + assert(delta[var].unaryExpr(ptr_fun(isfinite)).all()); if(mask[var]) { Value* retracted = key_value->value.retract_(delta[var]); key_value->value = *retracted; @@ -305,7 +306,7 @@ size_t ISAM2::Impl::UpdateDelta(const boost::shared_ptr& root, std: #ifndef NDEBUG for(size_t j=0; j).all()); + assert(delta.container()[j].unaryExpr(ptr_fun(isfinite)).all()); #endif } diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index 1b19cfe75..b9523b538 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -16,6 +16,8 @@ * @created Feb 26, 2012 */ +#include + #include #include // For NegativeMatrixException @@ -48,7 +50,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 Date: Wed, 23 May 2012 18:51:39 +0000 Subject: [PATCH 07/35] Fixed missing copy constructor and assignment operators in Marginals --- gtsam/nonlinear/Marginals.cpp | 13 +++++++++++++ gtsam/nonlinear/Marginals.h | 6 ++++++ 2 files changed, 19 insertions(+) diff --git a/gtsam/nonlinear/Marginals.cpp b/gtsam/nonlinear/Marginals.cpp index 34d620bc2..6df7dfdac 100644 --- a/gtsam/nonlinear/Marginals.cpp +++ b/gtsam/nonlinear/Marginals.cpp @@ -73,6 +73,19 @@ Matrix Marginals::marginalInformation(Key variable) const { } } +/* ************************************************************************* */ +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& variables) const { JointMarginal info = jointMarginalInformation(variables); diff --git a/gtsam/nonlinear/Marginals.h b/gtsam/nonlinear/Marginals.h index 77bccdfd2..fe5d4b21d 100644 --- a/gtsam/nonlinear/Marginals.h +++ b/gtsam/nonlinear/Marginals.h @@ -102,6 +102,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); + protected: Matrix fullMatrix_; BlockView blockView_; From 7cdd8e19da3cf899461ee73c909fb882e24e2622 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Wed, 23 May 2012 18:51:42 +0000 Subject: [PATCH 08/35] Tweaking build scripts for visual studio --- CMakeLists.txt | 14 +++++++++++--- examples/CMakeLists.txt | 4 +++- gtsam/CMakeLists.txt | 10 +++++++++- gtsam/slam/CMakeLists.txt | 4 ++++ 4 files changed, 27 insertions(+), 5 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 8bdbc486a..7198c5dfb 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -52,10 +52,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) @@ -97,7 +105,7 @@ endif() if(MSVC) set(Boost_USE_STATIC_LIBS 1) endif() -find_package(Boost 1.40 COMPONENTS serialization REQUIRED) +find_package(Boost 1.40 COMPONENTS serialization system chrono REQUIRED) # General build settings include_directories( diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 116379489..00a93904a 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -15,7 +15,9 @@ 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() endforeach(example_src) add_subdirectory(vSLAMexample) diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index 2adc83365..e0cc0eae1 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -46,6 +46,8 @@ 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 list(REMOVE_ITEM subdir_srcs ${excluded_sources}) set(${subdir}_srcs ${subdir_srcs}) if (GTSAM_BUILD_CONVENIENCE_LIBRARIES) @@ -71,7 +73,11 @@ set(gtsam_srcs ${slam_srcs} ) -#gtsam_assign_source_folders("${gtsam_srcs}") +# Create MSVC structure +file(GLOB_RECURSE all_c_srcs "*.c") +file(GLOB_RECURSE all_cpp_srcs "*.cpp") +file(GLOB_RECURSE all_headers "*.h") +gtsam_assign_source_folders("${all_c_srcs};${all_cpp_srcs};${all_headers}") # Versions set(gtsam_version ${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSION_PATCH}) @@ -83,6 +89,7 @@ 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 @@ -94,6 +101,7 @@ 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 diff --git a/gtsam/slam/CMakeLists.txt b/gtsam/slam/CMakeLists.txt index c25e32bd4..768ab2307 100644 --- a/gtsam/slam/CMakeLists.txt +++ b/gtsam/slam/CMakeLists.txt @@ -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}") From c2c9c4a982bccf1d233f81c5c7151ae24a31a777 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Wed, 23 May 2012 18:51:49 +0000 Subject: [PATCH 09/35] Almost everything compiles and passes in windows --- examples/PlanarSLAMExample_easy.cpp | 2 +- gtsam/base/tests/testVector.cpp | 2 +- gtsam/discrete/DecisionTree-inl.h | 2 +- gtsam/geometry/tests/testCalibratedCamera.cpp | 2 +- gtsam/geometry/tests/testFundamental.cpp | 8 +- gtsam/geometry/tests/testHomography2.cpp | 18 ++--- gtsam/geometry/tests/testPoint2.cpp | 4 +- gtsam/geometry/tests/testPose2.cpp | 74 +++++++++---------- gtsam/geometry/tests/testPose3.cpp | 12 +-- gtsam/geometry/tests/testRot2.cpp | 8 +- gtsam/geometry/tests/testRot3M.cpp | 2 +- gtsam/geometry/tests/testSimpleCamera.cpp | 2 +- gtsam/geometry/tests/testTensors.cpp | 12 +-- gtsam/inference/graph-inl.h | 4 +- gtsam/slam/tests/testGeneralSFMFactor.cpp | 4 +- .../testGeneralSFMFactor_Cal3Bundler.cpp | 4 +- gtsam/slam/tests/testPlanarSLAM.cpp | 16 ++-- gtsam/slam/tests/testPose2SLAM.cpp | 32 ++++---- tests/testDoglegOptimizer.cpp | 5 +- tests/testGaussianFactor.cpp | 4 +- tests/testGaussianFactorGraphB.cpp | 4 +- tests/testGraph.cpp | 2 +- tests/testInferenceB.cpp | 2 +- tests/testMarginals.cpp | 2 +- 24 files changed, 113 insertions(+), 114 deletions(-) diff --git a/examples/PlanarSLAMExample_easy.cpp b/examples/PlanarSLAMExample_easy.cpp index 5039b1145..808de3ba3 100644 --- a/examples/PlanarSLAMExample_easy.cpp +++ b/examples/PlanarSLAMExample_easy.cpp @@ -53,7 +53,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; diff --git a/gtsam/base/tests/testVector.cpp b/gtsam/base/tests/testVector.cpp index 8b8757027..6657d41f9 100644 --- a/gtsam/base/tests/testVector.cpp +++ b/gtsam/base/tests/testVector.cpp @@ -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::quiet_NaN()); //testing nan Vector v2 = Vector_(1, 1.0); double tol = 1.; EXPECT(!equal_with_abs_tol(v1, v2, tol)); diff --git a/gtsam/discrete/DecisionTree-inl.h b/gtsam/discrete/DecisionTree-inl.h index 1fa750ace..a9953d506 100644 --- a/gtsam/discrete/DecisionTree-inl.h +++ b/gtsam/discrete/DecisionTree-inl.h @@ -81,7 +81,7 @@ namespace gtsam { bool equals(const Node& q, double tol) const { const Leaf* other = dynamic_cast (&q); if (!other) return false; - return fabs(this->constant_ - other->constant_) < tol; + return fabs(double(this->constant_ - other->constant_)) < tol; } /** print */ diff --git a/gtsam/geometry/tests/testCalibratedCamera.cpp b/gtsam/geometry/tests/testCalibratedCamera.cpp index 5bb1ebe65..7fe88bce0 100644 --- a/gtsam/geometry/tests/testCalibratedCamera.cpp +++ b/gtsam/geometry/tests/testCalibratedCamera.cpp @@ -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 diff --git a/gtsam/geometry/tests/testFundamental.cpp b/gtsam/geometry/tests/testFundamental.cpp index 297b36036..46a051609 100644 --- a/gtsam/geometry/tests/testFundamental.cpp +++ b/gtsam/geometry/tests/testFundamental.cpp @@ -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) diff --git a/gtsam/geometry/tests/testHomography2.cpp b/gtsam/geometry/tests/testHomography2.cpp index 417d41977..062de9d1d 100644 --- a/gtsam/geometry/tests/testHomography2.cpp +++ b/gtsam/geometry/tests/testHomography2.cpp @@ -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}}; diff --git a/gtsam/geometry/tests/testPoint2.cpp b/gtsam/geometry/tests/testPoint2.cpp index 3416361cd..f7fee0418 100644 --- a/gtsam/geometry/tests/testPoint2.cpp +++ b/gtsam/geometry/tests/testPoint2.cpp @@ -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)); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index 14e529613..cdde3cd54 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -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(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(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(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 correspondences; diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index eb2d410f8..4ec1c6aee 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -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(x1, x_step), tol)); EXPECT(assert_equal(Pose3(Rot3::ypr(0,0,0), Point3(2,1,0)), expmap_default(x2, x_step), tol)); - EXPECT(assert_equal(Pose3(Rot3::ypr(M_PI_4,0,0), Point3(2,2,0)), expmap_default(x3, sqrt(2) * x_step), tol)); + EXPECT(assert_equal(Pose3(Rot3::ypr(M_PI/4.0,0,0), Point3(2,2,0)), expmap_default(x3, sqrt(2.0) * x_step), tol)); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testRot2.cpp b/gtsam/geometry/tests/testRot2.cpp index e23224898..acf385fab 100644 --- a/gtsam/geometry/tests/testRot2.cpp +++ b/gtsam/geometry/tests/testRot2.cpp @@ -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); diff --git a/gtsam/geometry/tests/testRot3M.cpp b/gtsam/geometry/tests/testRot3M.cpp index a5047f14b..7c8bd04bd 100644 --- a/gtsam/geometry/tests/testRot3M.cpp +++ b/gtsam/geometry/tests/testRot3M.cpp @@ -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, diff --git a/gtsam/geometry/tests/testSimpleCamera.cpp b/gtsam/geometry/tests/testSimpleCamera.cpp index b3df0d8f8..1990ad00a 100644 --- a/gtsam/geometry/tests/testSimpleCamera.cpp +++ b/gtsam/geometry/tests/testSimpleCamera.cpp @@ -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 diff --git a/gtsam/geometry/tests/testTensors.cpp b/gtsam/geometry/tests/testTensors.cpp index 755b274a6..85437b850 100644 --- a/gtsam/geometry/tests/testTensors.cpp +++ b/gtsam/geometry/tests/testTensors.cpp @@ -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) } /* ************************************************************************* */ diff --git a/gtsam/inference/graph-inl.h b/gtsam/inference/graph-inl.h index d8395942a..f9f8aa07c 100644 --- a/gtsam/inference/graph-inl.h +++ b/gtsam/inference/graph-inl.h @@ -196,8 +196,8 @@ boost::shared_ptr composePoses(const G& graph, const PredecessorMap 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) diff --git a/gtsam/slam/tests/testGeneralSFMFactor.cpp b/gtsam/slam/tests/testGeneralSFMFactor.cpp index 5b8a217bc..341549309 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor.cpp @@ -144,8 +144,8 @@ vector genCameraVariableCalibration() { return X ; } -shared_ptr getOrdering(const vector& X, const vector& L) { - shared_ptr ordering(new Ordering); +boost::shared_ptr getOrdering(const vector& X, const vector& L) { + boost::shared_ptr ordering(new Ordering); for ( size_t i = 0 ; i < L.size() ; ++i ) ordering->push_back(Symbol('l', i)) ; for ( size_t i = 0 ; i < X.size() ; ++i ) ordering->push_back(Symbol('x', i)) ; return ordering ; diff --git a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp index 76cf7d180..34391c070 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp @@ -146,8 +146,8 @@ vector genCameraVariableCalibration() { return X ; } -shared_ptr getOrdering(const vector& X, const vector& L) { - shared_ptr ordering(new Ordering); +boost::shared_ptr getOrdering(const vector& X, const vector& L) { + boost::shared_ptr ordering(new Ordering); for ( size_t i = 0 ; i < L.size() ; ++i ) ordering->push_back(Symbol('l', i)) ; for ( size_t i = 0 ; i < X.size() ; ++i ) ordering->push_back(Symbol('x', i)) ; return ordering ; diff --git a/gtsam/slam/tests/testPlanarSLAM.cpp b/gtsam/slam/tests/testPlanarSLAM.cpp index b32bf9a9c..4b69252b3 100644 --- a/gtsam/slam/tests/testPlanarSLAM.cpp +++ b/gtsam/slam/tests/testPlanarSLAM.cpp @@ -26,7 +26,7 @@ using namespace gtsam; using namespace planarSLAM; // some shared test values -static Pose2 x1, x2(1, 1, 0), x3(1, 1, M_PI_4); +static Pose2 x1, x2(1, 1, 0), x3(1, 1, M_PI/4.0); static Point2 l1(1, 0), l2(1, 1), l3(2, 2), l4(1, 3); SharedNoiseModel @@ -47,7 +47,7 @@ TEST( planarSLAM, PriorFactor_equals ) TEST( planarSLAM, BearingFactor ) { // Create factor - Rot2 z = Rot2::fromAngle(M_PI_4 + 0.1); // h(x) - z = -0.1 + Rot2 z = Rot2::fromAngle(M_PI/4.0 + 0.1); // h(x) - z = -0.1 planarSLAM::Bearing factor(PoseKey(2), PointKey(3), z, sigma); // create config @@ -75,7 +75,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(PoseKey(2), PointKey(3), z, sigma); // create config @@ -101,8 +101,8 @@ TEST( planarSLAM, RangeFactor_equals ) 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 + Rot2 r = Rot2::fromAngle(M_PI/4.0 + 0.1); // h(x) - z = -0.1 + double b(sqrt(2.0) - 0.22); // h(x) - z = 0.22 planarSLAM::BearingRange factor(PoseKey(2), PointKey(3), r, b, sigma2); // create config @@ -158,14 +158,14 @@ TEST( planarSLAM, constructor ) G.addPoseConstraint(2, x2); // make it feasible :-) // Add odometry - G.addOdometry(2, 3, Pose2(0, 0, M_PI_4), I3); + G.addOdometry(2, 3, Pose2(0, 0, M_PI/4.0), I3); // Create bearing factor - Rot2 z1 = Rot2::fromAngle(M_PI_4 + 0.1); // h(x) - z = -0.1 + Rot2 z1 = Rot2::fromAngle(M_PI/4.0 + 0.1); // h(x) - z = -0.1 G.addBearing(2, 3, 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(2, 3, z2, sigma); Vector expected0 = Vector_(3, 0.0, 0.0, 0.0); diff --git a/gtsam/slam/tests/testPose2SLAM.cpp b/gtsam/slam/tests/testPose2SLAM.cpp index 5aa633bbf..d97f99c72 100644 --- a/gtsam/slam/tests/testPose2SLAM.cpp +++ b/gtsam/slam/tests/testPose2SLAM.cpp @@ -72,7 +72,7 @@ TEST( Pose2SLAM, constraint1 ) /* ************************************************************************* */ // Test constraint, large displacement Vector f2(const Pose2& pose1, const Pose2& pose2) { - Pose2 z(2,2,M_PI_2); + Pose2 z(2,2,M_PI/2.0); Pose2Factor constraint(PoseKey(1), PoseKey(2), z, covariance); return constraint.evaluateError(pose1, pose2); } @@ -80,7 +80,7 @@ Vector f2(const Pose2& pose1, const Pose2& pose2) { TEST( Pose2SLAM, constraint2 ) { // create a factor between unknown poses p1 and p2 - Pose2 pose1, pose2(2,2,M_PI_2); + Pose2 pose1, pose2(2,2,M_PI/2.0); Pose2Factor constraint(PoseKey(1), PoseKey(2), pose2, covariance); Matrix H1, H2; Vector actual = constraint.evaluateError(pose1, pose2, H1, H2); @@ -96,7 +96,7 @@ TEST( Pose2SLAM, constraint2 ) TEST( Pose2SLAM, constructor ) { // create a factor between unknown poses p1 and p2 - Pose2 measured(2,2,M_PI_2); + Pose2 measured(2,2,M_PI/2.0); pose2SLAM::Graph graph; graph.addOdometry(1,2,measured, covariance); // get the size of the graph @@ -110,13 +110,13 @@ TEST( Pose2SLAM, constructor ) TEST( Pose2SLAM, linearization ) { // create a factor between unknown poses p1 and p2 - Pose2 measured(2,2,M_PI_2); + Pose2 measured(2,2,M_PI/2.0); Pose2Factor constraint(PoseKey(1), PoseKey(2), measured, covariance); pose2SLAM::Graph graph; graph.addOdometry(1,2,measured, covariance); // Choose a linearization point - Pose2 p1(1.1,2,M_PI_2); // robot at (1.1,2) looking towards y (ground truth is at 1,2, see testPose2) + Pose2 p1(1.1,2,M_PI/2.0); // robot at (1.1,2) looking towards y (ground truth is at 1,2, see testPose2) Pose2 p2(-1,4.1,M_PI); // robot at (-1,4) looking at negative (ground truth is at 4.1,2) pose2SLAM::Values config; config.insert(pose2SLAM::PoseKey(1),p1); @@ -151,7 +151,7 @@ TEST(Pose2SLAM, optimize) { // create a Pose graph with one equality constraint and one measurement pose2SLAM::Graph fg; fg.addPoseConstraint(0, Pose2(0,0,0)); - fg.addOdometry(0, 1, Pose2(1,2,M_PI_2), covariance); + fg.addOdometry(0, 1, Pose2(1,2,M_PI/2.0), covariance); // Create initial config Values initial; @@ -170,7 +170,7 @@ TEST(Pose2SLAM, optimize) { // Check with expected config Values expected; expected.insert(pose2SLAM::PoseKey(0), Pose2(0,0,0)); - expected.insert(pose2SLAM::PoseKey(1), Pose2(1,2,M_PI_2)); + expected.insert(pose2SLAM::PoseKey(1), Pose2(1,2,M_PI/2.0)); CHECK(assert_equal(expected, actual)); // Check marginals @@ -348,9 +348,9 @@ TEST(Pose2Values, pose2Circle ) { // expected is 4 poses tangent to circle with radius 1m pose2SLAM::Values expected; - expected.insert(pose2SLAM::PoseKey(0), Pose2( 1, 0, M_PI_2)); + expected.insert(pose2SLAM::PoseKey(0), Pose2( 1, 0, M_PI/2.0)); expected.insert(pose2SLAM::PoseKey(1), Pose2( 0, 1, - M_PI )); - expected.insert(pose2SLAM::PoseKey(2), Pose2(-1, 0, - M_PI_2)); + expected.insert(pose2SLAM::PoseKey(2), Pose2(-1, 0, - M_PI/2.0)); expected.insert(pose2SLAM::PoseKey(3), Pose2( 0, -1, 0 )); pose2SLAM::Values actual = pose2SLAM::circle(4,1.0); @@ -362,9 +362,9 @@ TEST(Pose2SLAM, expmap ) { // expected is circle shifted to right pose2SLAM::Values expected; - expected.insert(pose2SLAM::PoseKey(0), Pose2( 1.1, 0, M_PI_2)); + expected.insert(pose2SLAM::PoseKey(0), Pose2( 1.1, 0, M_PI/2.0)); expected.insert(pose2SLAM::PoseKey(1), Pose2( 0.1, 1, - M_PI )); - expected.insert(pose2SLAM::PoseKey(2), Pose2(-0.9, 0, - M_PI_2)); + expected.insert(pose2SLAM::PoseKey(2), Pose2(-0.9, 0, - M_PI/2.0)); expected.insert(pose2SLAM::PoseKey(3), Pose2( 0.1, -1, 0 )); // Note expmap coordinates are in local coordinates, so shifting to right requires thought !!! @@ -418,7 +418,7 @@ TEST( Pose2Prior, error ) /* ************************************************************************* */ // common Pose2Prior for tests below -static gtsam::Pose2 priorVal(2,2,M_PI_2); +static gtsam::Pose2 priorVal(2,2,M_PI/2.0); static pose2SLAM::Prior priorFactor(PoseKey(1), priorVal, sigmas); /* ************************************************************************* */ @@ -484,14 +484,14 @@ TEST( Pose2Factor, error ) /* ************************************************************************* */ // common Pose2Factor for tests below -static Pose2 measured(2,2,M_PI_2); +static Pose2 measured(2,2,M_PI/2.0); static Pose2Factor factor(PoseKey(1),PoseKey(2),measured, covariance); /* ************************************************************************* */ TEST( Pose2Factor, rhs ) { // Choose a linearization point - Pose2 p1(1.1,2,M_PI_2); // robot at (1.1,2) looking towards y (ground truth is at 1,2, see testPose2) + Pose2 p1(1.1,2,M_PI/2.0); // robot at (1.1,2) looking towards y (ground truth is at 1,2, see testPose2) Pose2 p2(-1,4.1,M_PI); // robot at (-1,4.1) looking at negative (ground truth is at -1,4) pose2SLAM::Values x0; x0.insert(pose2SLAM::PoseKey(1),p1); @@ -504,7 +504,7 @@ TEST( Pose2Factor, rhs ) // Check RHS Pose2 hx0 = p1.between(p2); - CHECK(assert_equal(Pose2(2.1, 2.1, M_PI_2),hx0)); + CHECK(assert_equal(Pose2(2.1, 2.1, M_PI/2.0),hx0)); Vector expected_b = Vector_(3, -0.1/sx, 0.1/sy, 0.0); CHECK(assert_equal(expected_b,-factor.whitenedError(x0))); CHECK(assert_equal(expected_b,linear->getb())); @@ -521,7 +521,7 @@ LieVector h(const Pose2& p1,const Pose2& p2) { TEST( Pose2Factor, linearize ) { // Choose a linearization point at ground truth - Pose2 p1(1,2,M_PI_2); + Pose2 p1(1,2,M_PI/2.0); Pose2 p2(-1,4,M_PI); pose2SLAM::Values x0; x0.insert(pose2SLAM::PoseKey(1),p1); diff --git a/tests/testDoglegOptimizer.cpp b/tests/testDoglegOptimizer.cpp index b16ac0cdc..c6241124a 100644 --- a/tests/testDoglegOptimizer.cpp +++ b/tests/testDoglegOptimizer.cpp @@ -31,7 +31,6 @@ using namespace std; using namespace gtsam; -using boost::shared_ptr; /* ************************************************************************* */ double computeError(const GaussianBayesNet& gbn, const LieVector& values) { @@ -381,7 +380,7 @@ TEST(DoglegOptimizer, ComputeDoglegPoint) { /* ************************************************************************* */ TEST(DoglegOptimizer, Iterate) { // really non-linear factor graph - shared_ptr fg(new example::Graph( + boost::shared_ptr fg(new example::Graph( example::createReallyNonlinearFactorGraph())); // config far from minimum @@ -390,7 +389,7 @@ TEST(DoglegOptimizer, Iterate) { config->insert(simulated2D::PoseKey(1), x0); // ordering - shared_ptr ord(new Ordering()); + boost::shared_ptr ord(new Ordering()); ord->push_back(simulated2D::PoseKey(1)); double Delta = 1.0; diff --git a/tests/testGaussianFactor.cpp b/tests/testGaussianFactor.cpp index 2cfb7a076..cb3ea18e7 100644 --- a/tests/testGaussianFactor.cpp +++ b/tests/testGaussianFactor.cpp @@ -263,7 +263,7 @@ TEST( GaussianFactor, matrix ) EQUALITY(b_act2,b2); // Ensure that whitening is consistent - shared_ptr model = lf->get_model(); + boost::shared_ptr model = lf->get_model(); model->WhitenSystem(A_act2, b_act2); EQUALITY(A_act1, A_act2); EQUALITY(b_act1, b_act2); @@ -307,7 +307,7 @@ TEST( GaussianFactor, matrix_aug ) EQUALITY(Ab_act2,Ab2); // Ensure that whitening is consistent - shared_ptr model = lf->get_model(); + boost::shared_ptr model = lf->get_model(); model->WhitenInPlace(Ab_act1); EQUALITY(Ab_act1, Ab_act2); } diff --git a/tests/testGaussianFactorGraphB.cpp b/tests/testGaussianFactorGraphB.cpp index 2e68200f5..82e8401d6 100644 --- a/tests/testGaussianFactorGraphB.cpp +++ b/tests/testGaussianFactorGraphB.cpp @@ -232,7 +232,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[kl(1)],d,R11,ordering[kx(1)],S12,ordering[kx(2)],S13,sigma); @@ -295,7 +295,7 @@ TEST( GaussianFactorGraph, eliminateOne_l1_fast ) GaussianConditional::shared_ptr actual = inference::eliminateOne(fg, ordering[kl(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[kl(1)],d,R11,ordering[kx(1)],S12,ordering[kx(2)],S13,sigma); diff --git a/tests/testGraph.cpp b/tests/testGraph.cpp index f56efae82..6ab543d1e 100644 --- a/tests/testGraph.cpp +++ b/tests/testGraph.cpp @@ -70,7 +70,7 @@ TEST( Graph, predecessorMap2Graph ) p_map.insert(kx(1), kx(2)); p_map.insert(kx(2), kx(2)); p_map.insert(kx(3), kx(2)); - tie(graph, root, key2vertex) = predecessorMap2Graph, SVertex, Key>(p_map); + boost::tie(graph, root, key2vertex) = predecessorMap2Graph, SVertex, Key>(p_map); LONGS_EQUAL(3, boost::num_vertices(graph)); CHECK(root == key2vertex[kx(2)]); diff --git a/tests/testInferenceB.cpp b/tests/testInferenceB.cpp index 6ad070698..d399f6eac 100644 --- a/tests/testInferenceB.cpp +++ b/tests/testInferenceB.cpp @@ -41,7 +41,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)); } diff --git a/tests/testMarginals.cpp b/tests/testMarginals.cpp index a91e6b576..1563e395e 100644 --- a/tests/testMarginals.cpp +++ b/tests/testMarginals.cpp @@ -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; From 5a3316af9e2f7ce3df6a0fe9ad951494a2a3404a Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Wed, 23 May 2012 20:56:19 +0000 Subject: [PATCH 10/35] Tweaked more build settings for visual studio --- gtsam/CMakeLists.txt | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index e0cc0eae1..e8dc712aa 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -95,6 +95,10 @@ if (GTSAM_BUILD_STATIC_LIBRARY) 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) @@ -107,6 +111,10 @@ if (GTSAM_BUILD_SHARED_LIBRARY) CLEAN_DIRECT_OUTPUT 1 VERSION ${gtsam_version} SOVERSION ${gtsam_soversion}) + 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) From 6e2312294cf22e86d9237dba756256e5577e45f7 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Wed, 23 May 2012 20:56:22 +0000 Subject: [PATCH 11/35] Added computeInformation function to GaussianFactor to properly compute information matrix including noise models, and using it to fix bug in Marginals where noise model was not being accounted for (only affects when hard constraints are used) --- gtsam/linear/GaussianFactor.h | 11 +++++++++++ gtsam/linear/HessianFactor.cpp | 11 ++++++++--- gtsam/linear/HessianFactor.h | 17 +++++++++++++++++ gtsam/linear/JacobianFactor.cpp | 10 ++++++++-- gtsam/linear/JacobianFactor.h | 10 ++++++++++ gtsam/nonlinear/Marginals.cpp | 13 +++---------- 6 files changed, 57 insertions(+), 15 deletions(-) diff --git a/gtsam/linear/GaussianFactor.h b/gtsam/linear/GaussianFactor.h index 3fdad19c6..e2537b7b4 100644 --- a/gtsam/linear/GaussianFactor.h +++ b/gtsam/linear/GaussianFactor.h @@ -20,6 +20,7 @@ #pragma once +#include #include #include @@ -88,6 +89,16 @@ namespace gtsam { /** Return the dimension of the variable pointed to by the given key iterator */ virtual size_t getDim(const_iterator variable) const = 0; + /** 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 = 0; + /** Clone a factor (make a deep copy) */ virtual GaussianFactor::shared_ptr clone() const = 0; diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index ebe94514b..8628c95ef 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -188,7 +188,7 @@ HessianFactor::HessianFactor(const std::vector& js, const std::vectorsize(), this->size(), 0); } +/* ************************************************************************* */ +Matrix HessianFactor::computeInformation() const { + return info_.full().selfadjointView(); +} + /* ************************************************************************* */ double HessianFactor::error(const VectorValues& c) const { // error 0.5*(f - 2*x'*g + x'*G*x) @@ -345,7 +350,7 @@ void HessianFactor::updateATA(const HessianFactor& update, const Scatter& scatte // First build an array of slots tic(1, "slots"); - size_t* slots = (size_t*)alloca(sizeof(size_t)*update.size()); // FIXME: alloca is bad, just ask Google. + 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; @@ -399,7 +404,7 @@ void HessianFactor::updateATA(const JacobianFactor& update, const Scatter& scatt // First build an array of slots tic(1, "slots"); - size_t* slots = (size_t*)alloca(sizeof(size_t)*update.size()); // FIXME: alloca is bad, just ask Google. + 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; diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index d6ff23159..1454e7a36 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -269,6 +269,23 @@ namespace gtsam { /** 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 + * 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. + * + * For HessianFactor, this is the same as info() except that this function + * returns a complete symmetric matrix whereas info() returns a matrix where + * only the upper triangle is valid, but should be interpreted as symmetric. + * This is because info() returns only a reference to the internal + * representation of the augmented information matrix, which stores only the + * upper triangle. + */ + virtual Matrix computeInformation() const; // Friend unit test classes friend class ::ConversionConstructorHessianFactorTest; diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 37fd74c39..2b884e1bd 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -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 = (size_t*)alloca(sizeof(size_t)*(terms.size()+1)); // FIXME: alloca is bad, just ask Google. + 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 >::const_iterator term=terms.begin(); for(; term!=terms.end(); ++term,++j) @@ -271,6 +271,12 @@ namespace gtsam { return 0.5 * weighted.dot(weighted); } + /* ************************************************************************* */ + Matrix JacobianFactor::computeInformation() const { + Matrix AbWhitened = Ab_.full(); + model_->WhitenInPlace(AbWhitened); + return AbWhitened.transpose() * AbWhitened; + } /* ************************************************************************* */ Vector JacobianFactor::operator*(const VectorValues& x) const { diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index b5a2dd500..048d44981 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -154,6 +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; /** 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 diff --git a/gtsam/nonlinear/Marginals.cpp b/gtsam/nonlinear/Marginals.cpp index 61f7f8dad..936cfb806 100644 --- a/gtsam/nonlinear/Marginals.cpp +++ b/gtsam/nonlinear/Marginals.cpp @@ -69,16 +69,9 @@ Matrix Marginals::marginalInformation(Key variable) const { marginalFactor = bayesTree_.marginalFactor(index, EliminateQR); // Get information matrix (only store upper-right triangle) - if(typeid(*marginalFactor) == typeid(JacobianFactor)) { - JacobianFactor::constABlock A = static_cast(*marginalFactor).getA(); - return A.transpose() * A; // Compute A'A - } else if(typeid(*marginalFactor) == typeid(HessianFactor)) { - const HessianFactor& hessian = static_cast(*marginalFactor); - const size_t dim = hessian.getDim(hessian.begin()); - return hessian.info().topLeftCorner(dim,dim).selfadjointView(); // Take the non-augmented part of the information matrix - } else { - throw runtime_error("Internal error: Marginals::marginalInformation expected either a JacobianFactor or HessianFactor"); - } + Matrix info = marginalFactor->computeInformation(); + const int dim = info.rows() - 1; + return info.topLeftCorner(dim,dim); // Take the non-augmented part of the information matrix } /* ************************************************************************* */ From ac150043bdf83240add766c5810943d69a7d5a91 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Wed, 23 May 2012 21:23:27 +0000 Subject: [PATCH 12/35] Fixed wrap under windows --- wrap/Module.cpp | 9 +++++---- wrap/tests/testWrap.cpp | 11 +++++------ 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/wrap/Module.cpp b/wrap/Module.cpp index dfd781016..98525e65d 100644 --- a/wrap/Module.cpp +++ b/wrap/Module.cpp @@ -25,12 +25,14 @@ #include #include #include +#include #include using namespace std; using namespace wrap; using namespace BOOST_SPIRIT_CLASSIC_NS; +namespace fs = boost::filesystem; typedef rule Rule; @@ -279,8 +281,8 @@ void verifyReturnTypes(const vector& validtypes, const vector& vt) { /* ************************************************************************* */ void Module::matlab_code(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"; @@ -327,8 +329,7 @@ void Module::matlab_code(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"; diff --git a/wrap/tests/testWrap.cpp b/wrap/tests/testWrap.cpp index cdee692a5..91b2161f7 100644 --- a/wrap/tests/testWrap.cpp +++ b/wrap/tests/testWrap.cpp @@ -20,6 +20,7 @@ #include #include #include +#include #include #include @@ -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 From 90717dd5836d99f9c2d7fcd881423a5dc8541794 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Wed, 23 May 2012 21:23:32 +0000 Subject: [PATCH 13/35] Tweaking build settings for visual studio --- CMakeLists.txt | 2 +- examples/CMakeLists.txt | 14 ++++++++++++-- examples/vSLAMexample/CMakeLists.txt | 12 ++++++++++-- 3 files changed, 23 insertions(+), 5 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 7198c5dfb..7f0b09e3c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -105,7 +105,7 @@ endif() if(MSVC) set(Boost_USE_STATIC_LIBS 1) endif() -find_package(Boost 1.40 COMPONENTS serialization system chrono REQUIRED) +find_package(Boost 1.40 COMPONENTS serialization system chrono filesystem REQUIRED) # General build settings include_directories( diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 00a93904a..f92312296 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -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 @@ -18,6 +22,12 @@ foreach(example_src ${example_srcs} ) 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) add_subdirectory(vSLAMexample) diff --git a/examples/vSLAMexample/CMakeLists.txt b/examples/vSLAMexample/CMakeLists.txt index 57fd6bf6d..e231a03e6 100644 --- a/examples/vSLAMexample/CMakeLists.txt +++ b/examples/vSLAMexample/CMakeLists.txt @@ -3,12 +3,20 @@ message(STATUS "Adding Example vISAMexample") add_executable(vISAMexample vISAMexample.cpp vSLAMutils.cpp) target_link_libraries(vISAMexample gtsam-static) -add_dependencies(examples vISAMexample) +if(NOT MSVC) + add_dependencies(examples vISAMexample) +else() + set_property(TARGET vISAMexample PROPERTY FOLDER "Examples") +endif() message(STATUS "Adding Example vSFMexample") add_executable(vSFMexample vSFMexample.cpp vSLAMutils.cpp) target_link_libraries(vSFMexample gtsam-static) -add_dependencies(examples vSFMexample) +if(NOT MSVC) + add_dependencies(examples vSFMexample) +else() + set_property(TARGET vSFMexample PROPERTY FOLDER "Examples") +endif() From e943ecfcdf7cf2276b807ddbf44f818712af12ab Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Thu, 24 May 2012 20:11:36 +0000 Subject: [PATCH 15/35] Removed leading-underscore names that conflict with windows macros --- gtsam/base/cholesky.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/base/cholesky.h b/gtsam/base/cholesky.h index 50d3b24a8..46d342d24 100644 --- a/gtsam/base/cholesky.h +++ b/gtsam/base/cholesky.h @@ -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: "); From d945dd8b6ad68a63234a66bb53f36a7b31aebc91 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Thu, 24 May 2012 20:11:38 +0000 Subject: [PATCH 16/35] Tweaking build settings for visual studio --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 5b598bacf..983fece25 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -15,7 +15,7 @@ include(GtsamTesting) include(GtsamPrinting) # guard against in-source builds -if(${CMAKE_SOURCE_DIR} STREQUAL ${CMAKE_BINARY_DIR} AND NOT MSVC) +if(${CMAKE_SOURCE_DIR} STREQUAL ${CMAKE_BINARY_DIR}) message(FATAL_ERROR "In-source builds not allowed. Please make a new directory (called a build directory) and run CMake from there. You may need to remove CMakeCache.txt. ") endif() From da5b01ff0fc4e588e83874961b80f9e3928b644d Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Thu, 24 May 2012 20:11:40 +0000 Subject: [PATCH 17/35] Updated boost version requirement in README --- README | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README b/README index 45698af8e..c38c12306 100644 --- a/README +++ b/README @@ -51,7 +51,7 @@ Important Installation Notes 1) GTSAM requires the following libraries to be installed on your system: - - BOOST version 1.40 or greater (install through Linux repositories or MacPorts) + - BOOST version 1.43 or greater (install through Linux repositories or MacPorts) - Cmake version 2.6 or higher - Support for XCode 4.3 command line tools on Mac requires CMake 2.8.8 or higher From 9cf1d8649d106e3779aedb5cd74b08858ccae1ea Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Thu, 24 May 2012 20:11:42 +0000 Subject: [PATCH 18/35] Build script fixes --- gtsam/CMakeLists.txt | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index e8dc712aa..8dd9a72ce 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -74,10 +74,7 @@ set(gtsam_srcs ) # Create MSVC structure -file(GLOB_RECURSE all_c_srcs "*.c") -file(GLOB_RECURSE all_cpp_srcs "*.cpp") -file(GLOB_RECURSE all_headers "*.h") -gtsam_assign_source_folders("${all_c_srcs};${all_cpp_srcs};${all_headers}") +gtsam_assign_all_source_folders() # Versions set(gtsam_version ${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSION_PATCH}) From 8f43298d2142df5b975912c86fb10478058665af Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Thu, 24 May 2012 20:11:45 +0000 Subject: [PATCH 19/35] Whitespace --- gtsam/linear/JacobianFactor.h | 20 ++++++++++---------- gtsam/nonlinear/ISAM2.cpp | 16 ++++++++-------- 2 files changed, 18 insertions(+), 18 deletions(-) diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index 048d44981..30e8e19bb 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -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 diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index bae2252d6..d785657a3 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -348,7 +348,7 @@ boost::shared_ptr > 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 > 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 > 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 tic(1, "Copy Values"); - Values ret(theta_); - toc(1, "Copy Values"); - tic(2, "getDelta"); - const Permuted& delta(getDelta()); + Values ret(theta_); + toc(1, "Copy Values"); + tic(2, "getDelta"); + const Permuted& delta(getDelta()); toc(2, "getDelta"); tic(3, "Expmap"); vector mask(ordering_.nVars(), true); From c3052fe5362b9124f5c0892db9ae5575dcddd905 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Thu, 24 May 2012 20:11:48 +0000 Subject: [PATCH 20/35] Fixed conflicts with build-in windows names --- gtsam/base/Matrix.cpp | 4 ++-- gtsam/base/Matrix.h | 6 +++--- gtsam/base/Vector.cpp | 4 ++-- gtsam/base/Vector.h | 6 +++--- 4 files changed, 10 insertions(+), 10 deletions(-) diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp index 57a45d4a1..320dbe896 100644 --- a/gtsam/base/Matrix.cpp +++ b/gtsam/base/Matrix.cpp @@ -236,8 +236,8 @@ 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; +void insertSub(Matrix& fullMatrix, const Matrix& subMatrix, size_t i, size_t j) { + fullMatrix.block(i, j, subMatrix.rows(), subMatrix.cols()) = subMatrix; } /* ************************************************************************* */ diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index fd7dac1fc..9093fb99d 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -201,12 +201,12 @@ Eigen::Block 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 diff --git a/gtsam/base/Vector.cpp b/gtsam/base/Vector.cpp index 98ad43e05..19546016f 100644 --- a/gtsam/base/Vector.cpp +++ b/gtsam/base/Vector.cpp @@ -251,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; } /* ************************************************************************* */ diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index b4be74f4e..40fed145c 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -209,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 From 2a98ba5857147b59c1c819c9b4f543d63a726441 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Mon, 28 May 2012 12:20:49 +0000 Subject: [PATCH 22/35] Whitespace --- gtsam/linear/HessianFactor.h | 34 +++++++++++++++++----------------- 1 file changed, 17 insertions(+), 17 deletions(-) diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index 1454e7a36..aa31bc9c0 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -269,23 +269,23 @@ namespace gtsam { /** 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 - * 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. - * - * For HessianFactor, this is the same as info() except that this function - * returns a complete symmetric matrix whereas info() returns a matrix where - * only the upper triangle is valid, but should be interpreted as symmetric. - * This is because info() returns only a reference to the internal - * representation of the augmented information matrix, which stores only the - * upper triangle. - */ - 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. + * + * For HessianFactor, this is the same as info() except that this function + * returns a complete symmetric matrix whereas info() returns a matrix where + * only the upper triangle is valid, but should be interpreted as symmetric. + * This is because info() returns only a reference to the internal + * representation of the augmented information matrix, which stores only the + * upper triangle. + */ + virtual Matrix computeInformation() const; // Friend unit test classes friend class ::ConversionConstructorHessianFactorTest; From e378d13e1ab0a1b271a6b051818c865eb8b2c0ff Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Mon, 28 May 2012 14:59:48 +0000 Subject: [PATCH 23/35] Fixed duplicate function --- gtsam/linear/JacobianFactor.h | 10 ---------- 1 file changed, 10 deletions(-) diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index 1cdf9e1c7..30e8e19bb 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -154,16 +154,6 @@ 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 From 363b44057bd04f6fe18424b156db296f784e4c30 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Mon, 28 May 2012 14:59:50 +0000 Subject: [PATCH 24/35] Made new cmake config scripts find correct build configuration --- CMakeLists.txt | 29 ++++++++--------------------- 1 file changed, 8 insertions(+), 21 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 046209aa7..12f1e614f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -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) @@ -83,8 +70,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 From 069be2b143767eabb4d9c95a4af4965fead2aaec Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Mon, 28 May 2012 20:48:32 +0000 Subject: [PATCH 25/35] Fixed examples compiling on windows --- examples/PlanarSLAMSelfContained_advanced.cpp | 2 +- examples/Pose2SLAMExample_easy.cpp | 14 +++++++------- examples/vSLAMexample/vISAMexample.cpp | 10 +++++----- 3 files changed, 13 insertions(+), 13 deletions(-) diff --git a/examples/PlanarSLAMSelfContained_advanced.cpp b/examples/PlanarSLAMSelfContained_advanced.cpp index 36f9ad8ca..102dcc315 100644 --- a/examples/PlanarSLAMSelfContained_advanced.cpp +++ b/examples/PlanarSLAMSelfContained_advanced.cpp @@ -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; diff --git a/examples/Pose2SLAMExample_easy.cpp b/examples/Pose2SLAMExample_easy.cpp index b6507d7fc..ef704c5fc 100644 --- a/examples/Pose2SLAMExample_easy.cpp +++ b/examples/Pose2SLAMExample_easy.cpp @@ -17,8 +17,8 @@ */ // pull in the Pose2 SLAM domain with all typedefs and helper functions defined -#include #include +#include using namespace std; using namespace gtsam; @@ -36,13 +36,13 @@ int main(int argc, char** argv) { // 2b. Add odometry factors SharedDiagonal odometryNoise(Vector_(3, 0.2, 0.2, 0.1)); graph.addOdometry(1, 2, Pose2(2.0, 0.0, 0.0 ), odometryNoise); - graph.addOdometry(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise); - graph.addOdometry(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise); - graph.addOdometry(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise); + graph.addOdometry(2, 3, Pose2(2.0, 0.0, M_PI/2.0), odometryNoise); + graph.addOdometry(3, 4, Pose2(2.0, 0.0, M_PI/2.0), odometryNoise); + graph.addOdometry(4, 5, Pose2(2.0, 0.0, M_PI/2.0), odometryNoise); // 2c. Add pose constraint SharedDiagonal constraintUncertainty(Vector_(3, 0.2, 0.2, 0.1)); - graph.addConstraint(5, 2, Pose2(2.0, 0.0, M_PI_2), constraintUncertainty); + graph.addConstraint(5, 2, Pose2(2.0, 0.0, M_PI/2.0), constraintUncertainty); // print graph.print("\nFactor graph:\n"); @@ -51,9 +51,9 @@ int main(int argc, char** argv) { pose2SLAM::Values initialEstimate; Pose2 x1(0.5, 0.0, 0.2 ); initialEstimate.insertPose(1, x1); Pose2 x2(2.3, 0.1,-0.2 ); initialEstimate.insertPose(2, x2); - Pose2 x3(4.1, 0.1, M_PI_2); initialEstimate.insertPose(3, x3); + Pose2 x3(4.1, 0.1, M_PI/2.0); initialEstimate.insertPose(3, x3); Pose2 x4(4.0, 2.0, M_PI ); initialEstimate.insertPose(4, x4); - Pose2 x5(2.1, 2.1,-M_PI_2); initialEstimate.insertPose(5, x5); + Pose2 x5(2.1, 2.1,-M_PI/2.0); initialEstimate.insertPose(5, x5); initialEstimate.print("\nInitial estimate:\n "); // 4. Single Step Optimization using Levenberg-Marquardt diff --git a/examples/vSLAMexample/vISAMexample.cpp b/examples/vSLAMexample/vISAMexample.cpp index 71d5516a2..7c16072d8 100644 --- a/examples/vSLAMexample/vISAMexample.cpp +++ b/examples/vSLAMexample/vISAMexample.cpp @@ -76,11 +76,11 @@ void readAllDataISAM() { /** * Setup newFactors and initialValues for each new pose and set of measurements at each frame. */ -void createNewFactors(shared_ptr& newFactors, boost::shared_ptr& initialValues, +void createNewFactors(boost::shared_ptr& newFactors, boost::shared_ptr& initialValues, int pose_id, const Pose3& pose, const std::vector& measurements, SharedNoiseModel measurementSigma, shared_ptrK calib) { // Create a graph of newFactors with new measurements - newFactors = shared_ptr (new Graph()); + newFactors = boost::shared_ptr (new Graph()); for (size_t i = 0; i < measurements.size(); i++) { newFactors->addMeasurement( measurements[i].m_p, @@ -97,7 +97,7 @@ void createNewFactors(shared_ptr& newFactors, boost::shared_ptr& } // Create initial values for all nodes in the newFactors - initialValues = shared_ptr (new Values()); + initialValues = boost::shared_ptr (new Values()); initialValues->insert(PoseKey(pose_id), pose); for (size_t i = 0; i < measurements.size(); i++) { initialValues->insert(PointKey(measurements[i].m_idLandmark), g_landmarks[measurements[i].m_idLandmark]); @@ -127,8 +127,8 @@ int main(int argc, char* argv[]) { typedef std::map > FeatureMap; BOOST_FOREACH(const FeatureMap::value_type& features, g_measurements) { const int poseId = features.first; - shared_ptr newFactors; - shared_ptr initialValues; + boost::shared_ptr newFactors; + boost::shared_ptr initialValues; createNewFactors(newFactors, initialValues, poseId, g_poses[poseId], features.second, measurementSigma, g_calib); From 592a251a053ed6159c700e12387a879ac9f4aec0 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Mon, 28 May 2012 20:48:36 +0000 Subject: [PATCH 26/35] Fixed path and compile problems with matlab wrapper on windows --- CMakeLists.txt | 2 +- wrap/Argument.cpp | 2 +- wrap/CMakeLists.txt | 6 +++++- wrap/Method.cpp | 2 +- wrap/Module.cpp | 2 +- wrap/ReturnValue.cpp | 6 +++--- wrap/matlab.h | 12 ++++++------ wrap/utilities.cpp | 2 +- 8 files changed, 19 insertions(+), 15 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 12f1e614f..7be750b02 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -95,7 +95,7 @@ endif() if(CYGWIN OR MSVC OR WIN32) set(Boost_USE_STATIC_LIBS 1) endif() -find_package(Boost 1.40 COMPONENTS serialization system chrono filesystem REQUIRED) +find_package(Boost 1.40 COMPONENTS serialization system chrono filesystem thread REQUIRED) # General build settings include_directories( diff --git a/wrap/Argument.cpp b/wrap/Argument.cpp index ff42b33b1..b03431d72 100644 --- a/wrap/Argument.cpp +++ b/wrap/Argument.cpp @@ -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< "; diff --git a/wrap/CMakeLists.txt b/wrap/CMakeLists.txt index 36415df35..a4286df98 100644 --- a/wrap/CMakeLists.txt +++ b/wrap/CMakeLists.txt @@ -31,6 +31,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/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) @@ -43,8 +46,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 ${GTSAM_MEX_BIN_EXTENSION} ${CMAKE_CURRENT_SOURCE_DIR}/../ ${moduleName} ${toolbox_path} "${mexFlags}" + ${WRAP_EXE} ${GTSAM_MEX_BIN_EXTENSION} ${CMAKE_CURRENT_SOURCE_DIR}/../ ${moduleName} ${toolbox_path} "${mexFlags}" DEPENDS wrap) # Build command diff --git a/wrap/Method.cpp b/wrap/Method.cpp index 3d53956f1..7261c88a0 100644 --- a/wrap/Method.cpp +++ b/wrap/Method.cpp @@ -73,7 +73,7 @@ void Method::matlab_wrapper(const string& classPath, // get class pointer // example: shared_ptr = 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 diff --git a/wrap/Module.cpp b/wrap/Module.cpp index 98525e65d..af2238038 100644 --- a/wrap/Module.cpp +++ b/wrap/Module.cpp @@ -294,7 +294,7 @@ void Module::matlab_code(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; diff --git a/wrap/ReturnValue.cpp b/wrap/ReturnValue.cpp index 2095abc5e..ace1159ec 100644 --- a/wrap/ReturnValue.cpp +++ b/wrap/ReturnValue.cpp @@ -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"; } diff --git a/wrap/matlab.h b/wrap/matlab.h index b6d200e43..a69d8aba4 100644 --- a/wrap/matlab.h +++ b/wrap/matlab.h @@ -300,19 +300,19 @@ class ObjectHandle { private: ObjectHandle* signature; // use 'this' as a unique object signature const std::type_info* type; // type checking information - shared_ptr t; // object pointer + boost::shared_ptr 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 (ptr)) { + type(&typeid(T)), t(boost::shared_ptr (ptr)) { signature = this; } // Constructor for shared pointers // Creates shared pointer, will delete if is last one to hold pointer - ObjectHandle(shared_ptr ptr) : + ObjectHandle(boost::shared_ptr ptr) : /*type(&typeid(T)),*/ t(ptr) { signature = this; } @@ -323,7 +323,7 @@ public: } // Get the actual object contained by handle - shared_ptr get_object() const { + boost::shared_ptr get_object() const { return t; } @@ -419,7 +419,7 @@ mxArray* create_object(const char *classname, mxArray* h) { class to matlab. */ template -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* handle = new ObjectHandle(shared_ptr); return create_object(classname,handle->to_mex_handle()); } @@ -436,7 +436,7 @@ mxArray* wrap_shared_ptr(shared_ptr< Class > shared_ptr, const char *classname) to the object. */ template -shared_ptr unwrap_shared_ptr(const mxArray* obj, const string& className) { +boost::shared_ptr unwrap_shared_ptr(const mxArray* obj, const string& className) { //Why is this here? #ifndef UNSAFE_WRAP bool isClass = mxIsClass(obj, className.c_str()); diff --git a/wrap/utilities.cpp b/wrap/utilities.cpp index 7b82d9b9d..bfd1e77df 100644 --- a/wrap/utilities.cpp +++ b/wrap/utilities.cpp @@ -104,7 +104,7 @@ bool files_equal(const string& expected, const string& actual, bool skipheader) /* ************************************************************************* */ string maybe_shared_ptr(bool add, const string& type) { - string str = add? "shared_ptr<" : ""; + string str = add? "boost::shared_ptr<" : ""; str += type; if (add) str += ">"; return str; From dc036a0891aed075bb53b05fd8ad73a3497839e7 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Tue, 29 May 2012 19:29:29 +0000 Subject: [PATCH 27/35] Updated wrap unit tests --- wrap/tests/expected/@Point2/argChar.cpp | 2 +- wrap/tests/expected/@Point2/argUChar.cpp | 2 +- wrap/tests/expected/@Point2/dim.cpp | 2 +- wrap/tests/expected/@Point2/returnChar.cpp | 2 +- wrap/tests/expected/@Point2/vectorConfusion.cpp | 2 +- wrap/tests/expected/@Point2/x.cpp | 2 +- wrap/tests/expected/@Point2/y.cpp | 2 +- wrap/tests/expected/@Point3/norm.cpp | 2 +- wrap/tests/expected/@Test/arg_EigenConstRef.cpp | 2 +- wrap/tests/expected/@Test/create_MixedPtrs.cpp | 4 ++-- wrap/tests/expected/@Test/create_ptrs.cpp | 4 ++-- wrap/tests/expected/@Test/print.cpp | 2 +- wrap/tests/expected/@Test/return_Point2Ptr.cpp | 4 ++-- wrap/tests/expected/@Test/return_Test.cpp | 4 ++-- wrap/tests/expected/@Test/return_TestPtr.cpp | 6 +++--- wrap/tests/expected/@Test/return_bool.cpp | 2 +- wrap/tests/expected/@Test/return_double.cpp | 2 +- wrap/tests/expected/@Test/return_field.cpp | 2 +- wrap/tests/expected/@Test/return_int.cpp | 2 +- wrap/tests/expected/@Test/return_matrix1.cpp | 2 +- wrap/tests/expected/@Test/return_matrix2.cpp | 2 +- wrap/tests/expected/@Test/return_pair.cpp | 2 +- wrap/tests/expected/@Test/return_ptrs.cpp | 8 ++++---- wrap/tests/expected/@Test/return_size_t.cpp | 2 +- wrap/tests/expected/@Test/return_string.cpp | 2 +- wrap/tests/expected/@Test/return_vector1.cpp | 2 +- wrap/tests/expected/@Test/return_vector2.cpp | 2 +- wrap/tests/expected/make_geometry.m | 2 +- .../expected_namespaces/@ns2ClassA/memberFunction.cpp | 2 +- wrap/tests/expected_namespaces/@ns2ClassA/nsArg.cpp | 2 +- wrap/tests/expected_namespaces/@ns2ClassA/nsReturn.cpp | 2 +- wrap/tests/expected_namespaces/make_testNamespaces.m | 2 +- 32 files changed, 41 insertions(+), 41 deletions(-) diff --git a/wrap/tests/expected/@Point2/argChar.cpp b/wrap/tests/expected/@Point2/argChar.cpp index 017fe1761..18548ee8f 100644 --- a/wrap/tests/expected/@Point2/argChar.cpp +++ b/wrap/tests/expected/@Point2/argChar.cpp @@ -4,7 +4,7 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("argChar",nargout,nargin-1,1); - shared_ptr self = unwrap_shared_ptr< Point2 >(in[0],"Point2"); + boost::shared_ptr self = unwrap_shared_ptr< Point2 >(in[0],"Point2"); char a = unwrap< char >(in[1]); self->argChar(a); } diff --git a/wrap/tests/expected/@Point2/argUChar.cpp b/wrap/tests/expected/@Point2/argUChar.cpp index 341e4f8a7..bbaa65a8f 100644 --- a/wrap/tests/expected/@Point2/argUChar.cpp +++ b/wrap/tests/expected/@Point2/argUChar.cpp @@ -4,7 +4,7 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("argUChar",nargout,nargin-1,1); - shared_ptr self = unwrap_shared_ptr< Point2 >(in[0],"Point2"); + boost::shared_ptr self = unwrap_shared_ptr< Point2 >(in[0],"Point2"); unsigned char a = unwrap< unsigned char >(in[1]); self->argUChar(a); } diff --git a/wrap/tests/expected/@Point2/dim.cpp b/wrap/tests/expected/@Point2/dim.cpp index 454153690..1349dc267 100644 --- a/wrap/tests/expected/@Point2/dim.cpp +++ b/wrap/tests/expected/@Point2/dim.cpp @@ -4,7 +4,7 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("dim",nargout,nargin-1,0); - shared_ptr self = unwrap_shared_ptr< Point2 >(in[0],"Point2"); + boost::shared_ptr self = unwrap_shared_ptr< Point2 >(in[0],"Point2"); int result = self->dim(); out[0] = wrap< int >(result); } diff --git a/wrap/tests/expected/@Point2/returnChar.cpp b/wrap/tests/expected/@Point2/returnChar.cpp index 3b7733862..c5b67a018 100644 --- a/wrap/tests/expected/@Point2/returnChar.cpp +++ b/wrap/tests/expected/@Point2/returnChar.cpp @@ -4,7 +4,7 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("returnChar",nargout,nargin-1,0); - shared_ptr self = unwrap_shared_ptr< Point2 >(in[0],"Point2"); + boost::shared_ptr self = unwrap_shared_ptr< Point2 >(in[0],"Point2"); char result = self->returnChar(); out[0] = wrap< char >(result); } diff --git a/wrap/tests/expected/@Point2/vectorConfusion.cpp b/wrap/tests/expected/@Point2/vectorConfusion.cpp index 90be4194b..bb6f02927 100644 --- a/wrap/tests/expected/@Point2/vectorConfusion.cpp +++ b/wrap/tests/expected/@Point2/vectorConfusion.cpp @@ -4,7 +4,7 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("vectorConfusion",nargout,nargin-1,0); - shared_ptr self = unwrap_shared_ptr< Point2 >(in[0],"Point2"); + boost::shared_ptr self = unwrap_shared_ptr< Point2 >(in[0],"Point2"); VectorNotEigen result = self->vectorConfusion(); out[0] = wrap_shared_ptr(make_shared< VectorNotEigen >(result),"VectorNotEigen"); } diff --git a/wrap/tests/expected/@Point2/x.cpp b/wrap/tests/expected/@Point2/x.cpp index 8df8c6bbc..65e56cae5 100644 --- a/wrap/tests/expected/@Point2/x.cpp +++ b/wrap/tests/expected/@Point2/x.cpp @@ -4,7 +4,7 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("x",nargout,nargin-1,0); - shared_ptr self = unwrap_shared_ptr< Point2 >(in[0],"Point2"); + boost::shared_ptr self = unwrap_shared_ptr< Point2 >(in[0],"Point2"); double result = self->x(); out[0] = wrap< double >(result); } diff --git a/wrap/tests/expected/@Point2/y.cpp b/wrap/tests/expected/@Point2/y.cpp index 14e3663ed..f8e10dc5d 100644 --- a/wrap/tests/expected/@Point2/y.cpp +++ b/wrap/tests/expected/@Point2/y.cpp @@ -4,7 +4,7 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("y",nargout,nargin-1,0); - shared_ptr self = unwrap_shared_ptr< Point2 >(in[0],"Point2"); + boost::shared_ptr self = unwrap_shared_ptr< Point2 >(in[0],"Point2"); double result = self->y(); out[0] = wrap< double >(result); } diff --git a/wrap/tests/expected/@Point3/norm.cpp b/wrap/tests/expected/@Point3/norm.cpp index 9936958d7..0c7ac2038 100644 --- a/wrap/tests/expected/@Point3/norm.cpp +++ b/wrap/tests/expected/@Point3/norm.cpp @@ -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 self = unwrap_shared_ptr< Point3 >(in[0],"Point3"); + boost::shared_ptr self = unwrap_shared_ptr< Point3 >(in[0],"Point3"); double result = self->norm(); out[0] = wrap< double >(result); } diff --git a/wrap/tests/expected/@Test/arg_EigenConstRef.cpp b/wrap/tests/expected/@Test/arg_EigenConstRef.cpp index 487741c07..09a5c6f62 100644 --- a/wrap/tests/expected/@Test/arg_EigenConstRef.cpp +++ b/wrap/tests/expected/@Test/arg_EigenConstRef.cpp @@ -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 self = unwrap_shared_ptr< Test >(in[0],"Test"); + boost::shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); Matrix& value = *unwrap_shared_ptr< Matrix >(in[1], "Matrix"); self->arg_EigenConstRef(value); } diff --git a/wrap/tests/expected/@Test/create_MixedPtrs.cpp b/wrap/tests/expected/@Test/create_MixedPtrs.cpp index 269bb990c..9c685d6cf 100644 --- a/wrap/tests/expected/@Test/create_MixedPtrs.cpp +++ b/wrap/tests/expected/@Test/create_MixedPtrs.cpp @@ -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 self = unwrap_shared_ptr< Test >(in[0],"Test"); - pair< Test, shared_ptr > result = self->create_MixedPtrs(); + boost::shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); + pair< Test, boost::shared_ptr > result = self->create_MixedPtrs(); out[0] = wrap_shared_ptr(make_shared< Test >(result.first),"Test"); out[1] = wrap_shared_ptr(result.second,"Test"); } diff --git a/wrap/tests/expected/@Test/create_ptrs.cpp b/wrap/tests/expected/@Test/create_ptrs.cpp index bfd9f39bf..830d62a12 100644 --- a/wrap/tests/expected/@Test/create_ptrs.cpp +++ b/wrap/tests/expected/@Test/create_ptrs.cpp @@ -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 self = unwrap_shared_ptr< Test >(in[0],"Test"); - pair< shared_ptr, shared_ptr > result = self->create_ptrs(); + boost::shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); + pair< boost::shared_ptr, boost::shared_ptr > result = self->create_ptrs(); out[0] = wrap_shared_ptr(result.first,"Test"); out[1] = wrap_shared_ptr(result.second,"Test"); } diff --git a/wrap/tests/expected/@Test/print.cpp b/wrap/tests/expected/@Test/print.cpp index e93f5f09a..1d259f2e8 100644 --- a/wrap/tests/expected/@Test/print.cpp +++ b/wrap/tests/expected/@Test/print.cpp @@ -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 self = unwrap_shared_ptr< Test >(in[0],"Test"); + boost::shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); self->print(); } diff --git a/wrap/tests/expected/@Test/return_Point2Ptr.cpp b/wrap/tests/expected/@Test/return_Point2Ptr.cpp index 9ad2a4360..e6990198e 100644 --- a/wrap/tests/expected/@Test/return_Point2Ptr.cpp +++ b/wrap/tests/expected/@Test/return_Point2Ptr.cpp @@ -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 self = unwrap_shared_ptr< Test >(in[0],"Test"); + boost::shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); bool value = unwrap< bool >(in[1]); - shared_ptr result = self->return_Point2Ptr(value); + boost::shared_ptr result = self->return_Point2Ptr(value); out[0] = wrap_shared_ptr(result,"Point2"); } diff --git a/wrap/tests/expected/@Test/return_Test.cpp b/wrap/tests/expected/@Test/return_Test.cpp index 73c63a899..458e0c74d 100644 --- a/wrap/tests/expected/@Test/return_Test.cpp +++ b/wrap/tests/expected/@Test/return_Test.cpp @@ -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 self = unwrap_shared_ptr< Test >(in[0],"Test"); - shared_ptr value = unwrap_shared_ptr< Test >(in[1], "Test"); + boost::shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); + boost::shared_ptr value = unwrap_shared_ptr< Test >(in[1], "Test"); Test result = self->return_Test(value); out[0] = wrap_shared_ptr(make_shared< Test >(result),"Test"); } diff --git a/wrap/tests/expected/@Test/return_TestPtr.cpp b/wrap/tests/expected/@Test/return_TestPtr.cpp index 607e48cfd..3c053791d 100644 --- a/wrap/tests/expected/@Test/return_TestPtr.cpp +++ b/wrap/tests/expected/@Test/return_TestPtr.cpp @@ -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 self = unwrap_shared_ptr< Test >(in[0],"Test"); - shared_ptr value = unwrap_shared_ptr< Test >(in[1], "Test"); - shared_ptr result = self->return_TestPtr(value); + boost::shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); + boost::shared_ptr value = unwrap_shared_ptr< Test >(in[1], "Test"); + boost::shared_ptr result = self->return_TestPtr(value); out[0] = wrap_shared_ptr(result,"Test"); } diff --git a/wrap/tests/expected/@Test/return_bool.cpp b/wrap/tests/expected/@Test/return_bool.cpp index d700ead2b..92612a279 100644 --- a/wrap/tests/expected/@Test/return_bool.cpp +++ b/wrap/tests/expected/@Test/return_bool.cpp @@ -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 self = unwrap_shared_ptr< Test >(in[0],"Test"); + boost::shared_ptr 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); diff --git a/wrap/tests/expected/@Test/return_double.cpp b/wrap/tests/expected/@Test/return_double.cpp index 38770e1cd..e167a16c0 100644 --- a/wrap/tests/expected/@Test/return_double.cpp +++ b/wrap/tests/expected/@Test/return_double.cpp @@ -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 self = unwrap_shared_ptr< Test >(in[0],"Test"); + boost::shared_ptr 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); diff --git a/wrap/tests/expected/@Test/return_field.cpp b/wrap/tests/expected/@Test/return_field.cpp index b5e067801..838bab0a4 100644 --- a/wrap/tests/expected/@Test/return_field.cpp +++ b/wrap/tests/expected/@Test/return_field.cpp @@ -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 self = unwrap_shared_ptr< Test >(in[0],"Test"); + boost::shared_ptr 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); diff --git a/wrap/tests/expected/@Test/return_int.cpp b/wrap/tests/expected/@Test/return_int.cpp index e2dfd25e8..4cdaf5abc 100644 --- a/wrap/tests/expected/@Test/return_int.cpp +++ b/wrap/tests/expected/@Test/return_int.cpp @@ -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 self = unwrap_shared_ptr< Test >(in[0],"Test"); + boost::shared_ptr 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); diff --git a/wrap/tests/expected/@Test/return_matrix1.cpp b/wrap/tests/expected/@Test/return_matrix1.cpp index 8bb9acff8..f7fb72040 100644 --- a/wrap/tests/expected/@Test/return_matrix1.cpp +++ b/wrap/tests/expected/@Test/return_matrix1.cpp @@ -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 self = unwrap_shared_ptr< Test >(in[0],"Test"); + boost::shared_ptr 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); diff --git a/wrap/tests/expected/@Test/return_matrix2.cpp b/wrap/tests/expected/@Test/return_matrix2.cpp index f1a22da47..f8b6823fa 100644 --- a/wrap/tests/expected/@Test/return_matrix2.cpp +++ b/wrap/tests/expected/@Test/return_matrix2.cpp @@ -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 self = unwrap_shared_ptr< Test >(in[0],"Test"); + boost::shared_ptr 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); diff --git a/wrap/tests/expected/@Test/return_pair.cpp b/wrap/tests/expected/@Test/return_pair.cpp index 8f6949d7c..54b3f6522 100644 --- a/wrap/tests/expected/@Test/return_pair.cpp +++ b/wrap/tests/expected/@Test/return_pair.cpp @@ -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 self = unwrap_shared_ptr< Test >(in[0],"Test"); + boost::shared_ptr 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); diff --git a/wrap/tests/expected/@Test/return_ptrs.cpp b/wrap/tests/expected/@Test/return_ptrs.cpp index 20d6c57fe..eea94ca3c 100644 --- a/wrap/tests/expected/@Test/return_ptrs.cpp +++ b/wrap/tests/expected/@Test/return_ptrs.cpp @@ -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 self = unwrap_shared_ptr< Test >(in[0],"Test"); - shared_ptr p1 = unwrap_shared_ptr< Test >(in[1], "Test"); - shared_ptr p2 = unwrap_shared_ptr< Test >(in[2], "Test"); - pair< shared_ptr, shared_ptr > result = self->return_ptrs(p1,p2); + boost::shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); + boost::shared_ptr p1 = unwrap_shared_ptr< Test >(in[1], "Test"); + boost::shared_ptr p2 = unwrap_shared_ptr< Test >(in[2], "Test"); + pair< boost::shared_ptr, boost::shared_ptr > result = self->return_ptrs(p1,p2); out[0] = wrap_shared_ptr(result.first,"Test"); out[1] = wrap_shared_ptr(result.second,"Test"); } diff --git a/wrap/tests/expected/@Test/return_size_t.cpp b/wrap/tests/expected/@Test/return_size_t.cpp index 6b12a167b..901c5c9bd 100644 --- a/wrap/tests/expected/@Test/return_size_t.cpp +++ b/wrap/tests/expected/@Test/return_size_t.cpp @@ -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 self = unwrap_shared_ptr< Test >(in[0],"Test"); + boost::shared_ptr 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); diff --git a/wrap/tests/expected/@Test/return_string.cpp b/wrap/tests/expected/@Test/return_string.cpp index c05a57fba..778e07522 100644 --- a/wrap/tests/expected/@Test/return_string.cpp +++ b/wrap/tests/expected/@Test/return_string.cpp @@ -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 self = unwrap_shared_ptr< Test >(in[0],"Test"); + boost::shared_ptr 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); diff --git a/wrap/tests/expected/@Test/return_vector1.cpp b/wrap/tests/expected/@Test/return_vector1.cpp index 3710e5d48..5e8aed397 100644 --- a/wrap/tests/expected/@Test/return_vector1.cpp +++ b/wrap/tests/expected/@Test/return_vector1.cpp @@ -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 self = unwrap_shared_ptr< Test >(in[0],"Test"); + boost::shared_ptr 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); diff --git a/wrap/tests/expected/@Test/return_vector2.cpp b/wrap/tests/expected/@Test/return_vector2.cpp index 72564f05c..4c3242f2e 100644 --- a/wrap/tests/expected/@Test/return_vector2.cpp +++ b/wrap/tests/expected/@Test/return_vector2.cpp @@ -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 self = unwrap_shared_ptr< Test >(in[0],"Test"); + boost::shared_ptr 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); diff --git a/wrap/tests/expected/make_geometry.m b/wrap/tests/expected/make_geometry.m index 7baac182f..2858f2994 100644 --- a/wrap/tests/expected/make_geometry.m +++ b/wrap/tests/expected/make_geometry.m @@ -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); diff --git a/wrap/tests/expected_namespaces/@ns2ClassA/memberFunction.cpp b/wrap/tests/expected_namespaces/@ns2ClassA/memberFunction.cpp index 8e5bc3960..514e5db08 100644 --- a/wrap/tests/expected_namespaces/@ns2ClassA/memberFunction.cpp +++ b/wrap/tests/expected_namespaces/@ns2ClassA/memberFunction.cpp @@ -5,7 +5,7 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("memberFunction",nargout,nargin-1,0); - shared_ptr self = unwrap_shared_ptr< ns2::ClassA >(in[0],"ns2ClassA"); + boost::shared_ptr self = unwrap_shared_ptr< ns2::ClassA >(in[0],"ns2ClassA"); double result = self->memberFunction(); out[0] = wrap< double >(result); } diff --git a/wrap/tests/expected_namespaces/@ns2ClassA/nsArg.cpp b/wrap/tests/expected_namespaces/@ns2ClassA/nsArg.cpp index 7c06bcfdc..789b0815e 100644 --- a/wrap/tests/expected_namespaces/@ns2ClassA/nsArg.cpp +++ b/wrap/tests/expected_namespaces/@ns2ClassA/nsArg.cpp @@ -5,7 +5,7 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("nsArg",nargout,nargin-1,1); - shared_ptr self = unwrap_shared_ptr< ns2::ClassA >(in[0],"ns2ClassA"); + boost::shared_ptr 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); diff --git a/wrap/tests/expected_namespaces/@ns2ClassA/nsReturn.cpp b/wrap/tests/expected_namespaces/@ns2ClassA/nsReturn.cpp index c6714ee9f..826e30c8c 100644 --- a/wrap/tests/expected_namespaces/@ns2ClassA/nsReturn.cpp +++ b/wrap/tests/expected_namespaces/@ns2ClassA/nsReturn.cpp @@ -5,7 +5,7 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("nsReturn",nargout,nargin-1,1); - shared_ptr self = unwrap_shared_ptr< ns2::ClassA >(in[0],"ns2ClassA"); + boost::shared_ptr 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"); diff --git a/wrap/tests/expected_namespaces/make_testNamespaces.m b/wrap/tests/expected_namespaces/make_testNamespaces.m index eac4146c8..d835a2da0 100644 --- a/wrap/tests/expected_namespaces/make_testNamespaces.m +++ b/wrap/tests/expected_namespaces/make_testNamespaces.m @@ -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); From d636534e407d3944857698ad527aab0744317048 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Tue, 29 May 2012 19:30:59 +0000 Subject: [PATCH 28/35] Updated wrap unit tests --- wrap/tests/expected/@Point2/vectorConfusion.cpp | 2 +- wrap/tests/expected/@Test/create_MixedPtrs.cpp | 2 +- wrap/tests/expected/@Test/return_Test.cpp | 2 +- wrap/tests/expected/Point3_StaticFunctionRet.cpp | 2 +- wrap/tests/expected_namespaces/@ns2ClassA/nsReturn.cpp | 2 +- 5 files changed, 5 insertions(+), 5 deletions(-) diff --git a/wrap/tests/expected/@Point2/vectorConfusion.cpp b/wrap/tests/expected/@Point2/vectorConfusion.cpp index bb6f02927..d992d1d94 100644 --- a/wrap/tests/expected/@Point2/vectorConfusion.cpp +++ b/wrap/tests/expected/@Point2/vectorConfusion.cpp @@ -6,5 +6,5 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) checkArguments("vectorConfusion",nargout,nargin-1,0); boost::shared_ptr 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"); } diff --git a/wrap/tests/expected/@Test/create_MixedPtrs.cpp b/wrap/tests/expected/@Test/create_MixedPtrs.cpp index 9c685d6cf..81bcdc5d8 100644 --- a/wrap/tests/expected/@Test/create_MixedPtrs.cpp +++ b/wrap/tests/expected/@Test/create_MixedPtrs.cpp @@ -7,6 +7,6 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) checkArguments("create_MixedPtrs",nargout,nargin-1,0); boost::shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); pair< Test, boost::shared_ptr > result = self->create_MixedPtrs(); - out[0] = wrap_shared_ptr(make_shared< Test >(result.first),"Test"); + out[0] = wrap_shared_ptr(boost::make_shared< Test >(result.first),"Test"); out[1] = wrap_shared_ptr(result.second,"Test"); } diff --git a/wrap/tests/expected/@Test/return_Test.cpp b/wrap/tests/expected/@Test/return_Test.cpp index 458e0c74d..63e9f5a3b 100644 --- a/wrap/tests/expected/@Test/return_Test.cpp +++ b/wrap/tests/expected/@Test/return_Test.cpp @@ -8,5 +8,5 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) boost::shared_ptr self = unwrap_shared_ptr< Test >(in[0],"Test"); boost::shared_ptr 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"); } diff --git a/wrap/tests/expected/Point3_StaticFunctionRet.cpp b/wrap/tests/expected/Point3_StaticFunctionRet.cpp index 062194bd5..652d8713e 100644 --- a/wrap/tests/expected/Point3_StaticFunctionRet.cpp +++ b/wrap/tests/expected/Point3_StaticFunctionRet.cpp @@ -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"); } diff --git a/wrap/tests/expected_namespaces/@ns2ClassA/nsReturn.cpp b/wrap/tests/expected_namespaces/@ns2ClassA/nsReturn.cpp index 826e30c8c..dbeb42f60 100644 --- a/wrap/tests/expected_namespaces/@ns2ClassA/nsReturn.cpp +++ b/wrap/tests/expected_namespaces/@ns2ClassA/nsReturn.cpp @@ -8,5 +8,5 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) boost::shared_ptr 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"); } From cf232fe4a8aad4bc4993ffb68cdca7e719534918 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Tue, 29 May 2012 20:03:15 +0000 Subject: [PATCH 29/35] Added missing boost filesystem library to link with wrap --- wrap/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/wrap/CMakeLists.txt b/wrap/CMakeLists.txt index a4286df98..01d873bac 100644 --- a/wrap/CMakeLists.txt +++ b/wrap/CMakeLists.txt @@ -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) From 695f2756db632d39254a6b15e248182c25fd4473 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Mon, 4 Jun 2012 13:44:02 +0000 Subject: [PATCH 30/35] Build scripts --- CMakeLists.txt | 5 +++-- CppUnitLite/CMakeLists.txt | 2 ++ gtsam/CMakeLists.txt | 11 +++++------ 3 files changed, 10 insertions(+), 8 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 7be750b02..335fc76c1 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -130,9 +130,10 @@ if (GTSAM_BUILD_UNSTABLE) add_subdirectory(gtsam_unstable) endif(GTSAM_BUILD_UNSTABLE) -# Make config file +# Make config files include(GtsamMakeConfigFile) -GtsamMakeConfigFile(gtsam) +GtsamMakeConfigFile(GTSAM gtsam-static) +GtsamMakeConfigFile(CppUnitLite CppUnitLite) # Set up CPack set(CPACK_PACKAGE_DESCRIPTION_SUMMARY "GTSAM") diff --git a/CppUnitLite/CMakeLists.txt b/CppUnitLite/CMakeLists.txt index 642e8e462..286a39258 100644 --- a/CppUnitLite/CMakeLists.txt +++ b/CppUnitLite/CMakeLists.txt @@ -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) diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index 8dd9a72ce..80c57c15c 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -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}) @@ -48,6 +49,7 @@ foreach(subdir ${gtsam_subdirs}) 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) @@ -73,9 +75,6 @@ set(gtsam_srcs ${slam_srcs} ) -# Create MSVC structure -gtsam_assign_all_source_folders() - # Versions set(gtsam_version ${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSION_PATCH}) set(gtsam_soversion ${GTSAM_VERSION_MAJOR}) From ec29518456e95b96f9ff5520eb222f579ab07183 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Mon, 4 Jun 2012 13:44:19 +0000 Subject: [PATCH 31/35] Header files --- gtsam/base/LieVector.cpp | 2 +- gtsam/base/Vector.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/base/LieVector.cpp b/gtsam/base/LieVector.cpp index 876c8fa6e..d5ae5d504 100644 --- a/gtsam/base/LieVector.cpp +++ b/gtsam/base/LieVector.cpp @@ -15,7 +15,7 @@ * @author Alex Cunningham */ -#include +#include #include using namespace std; diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index 40fed145c..6019bbd33 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -23,7 +23,7 @@ #include #include #include -#include +#include #include /** From 318dd96dfb483aa4c4f8ddf315dc26ad9a40408b Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Mon, 4 Jun 2012 13:44:21 +0000 Subject: [PATCH 32/35] Matrix stream input operator --- gtsam/base/Matrix.cpp | 23 +++++++++++++++++++++++ gtsam/base/Matrix.h | 7 +++++++ 2 files changed, 30 insertions(+) diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp index 320dbe896..2d32ccc04 100644 --- a/gtsam/base/Matrix.cpp +++ b/gtsam/base/Matrix.cpp @@ -235,6 +235,29 @@ void save(const Matrix& A, const string &s, const string& filename) { stream.close(); } +/* ************************************************************************* */ +//istream& operator>>(istream& inputStream, Matrix& destinationMatrix) { +// destinationMatrix.resize(0,0); +// string line; +// bool first = true; +// while(getline(inputStream, line)) { +// // Read coefficients from file +// vector coeffs; +// std::copy(istream_iterator(stringstream(line)), istream_iterator(), 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(&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; diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index 9093fb99d..553fb5b89 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -183,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 From 6b1e862688df5494e22588d45bca85ef714a8624 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Mon, 4 Jun 2012 18:46:05 +0000 Subject: [PATCH 33/35] Fix errors --- gtsam/base/tests/timeVirtual.cpp | 8 ++++---- gtsam/base/types.h | 6 ++++++ gtsam/geometry/StereoPoint2.h | 10 +++++----- tests/testGraph.cpp | 2 +- tests/timeMultifrontalOnDataset.cpp | 2 +- tests/timeSequentialOnDataset.cpp | 2 +- 6 files changed, 18 insertions(+), 12 deletions(-) diff --git a/gtsam/base/tests/timeVirtual.cpp b/gtsam/base/tests/timeVirtual.cpp index a61ce7b77..697a219b1 100644 --- a/gtsam/base/tests/timeVirtual.cpp +++ b/gtsam/base/tests/timeVirtual.cpp @@ -87,13 +87,13 @@ int main(int argc, char *argv[]) { tic_("shared plain alloc, dealloc"); for(size_t i=0; i obj(new Plain(i)); + boost::shared_ptr obj(new Plain(i)); } toc_("shared plain alloc, dealloc"); tic_("shared virtual alloc, dealloc"); for(size_t i=0; i obj(new Virtual(i)); + boost::shared_ptr 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 obj(new Plain(i)); + boost::shared_ptr 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 obj(new Virtual(i)); + boost::shared_ptr obj(new Virtual(i)); obj->setData(i+1); } toc_("shared virtual alloc, dealloc, call"); diff --git a/gtsam/base/types.h b/gtsam/base/types.h index e9264d516..2490a0cde 100644 --- a/gtsam/base/types.h +++ b/gtsam/base/types.h @@ -82,6 +82,12 @@ using boost::math::isinf; #ifndef M_PI #define M_PI (boost::math::constants::pi()) #endif +#ifndef M_PI_2 +#define M_PI_2 (boost::math::constants::pi() / 2.0) +#endif +#ifndef M_PI_4 +#define M_PI_4 (boost::math::constants::pi() / 4.0) +#endif #endif diff --git a/gtsam/geometry/StereoPoint2.h b/gtsam/geometry/StereoPoint2.h index 43701e4e8..022296c21 100644 --- a/gtsam/geometry/StereoPoint2.h +++ b/gtsam/geometry/StereoPoint2.h @@ -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: /// @} diff --git a/tests/testGraph.cpp b/tests/testGraph.cpp index c75c42b49..63cbd43c8 100644 --- a/tests/testGraph.cpp +++ b/tests/testGraph.cpp @@ -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, SVertex, Key>(p_map); + boost::tie(graph, root, key2vertex) = predecessorMap2Graph, SVertex, Key>(p_map); LONGS_EQUAL(3, boost::num_vertices(graph)); CHECK(root == key2vertex[2]); diff --git a/tests/timeMultifrontalOnDataset.cpp b/tests/timeMultifrontalOnDataset.cpp index 874ccf8bb..3d8ccf77e 100644 --- a/tests/timeMultifrontalOnDataset.cpp +++ b/tests/timeMultifrontalOnDataset.cpp @@ -44,7 +44,7 @@ int main(int argc, char *argv[]) { else if (argc == 4) nrTrials = strtoul(argv[3], NULL, 10); - pair, shared_ptr > data = load2D(dataset(datasetname)); + pair, boost::shared_ptr > data = load2D(dataset(datasetname)); // Add a prior on the first pose if (soft_prior) diff --git a/tests/timeSequentialOnDataset.cpp b/tests/timeSequentialOnDataset.cpp index 9e1b986f9..a37b9910c 100644 --- a/tests/timeSequentialOnDataset.cpp +++ b/tests/timeSequentialOnDataset.cpp @@ -44,7 +44,7 @@ int main(int argc, char *argv[]) { else if (argc == 4) nrTrials = strtoul(argv[3], NULL, 10); - pair, shared_ptr > data = load2D(dataset(datasetname)); + pair, boost::shared_ptr > data = load2D(dataset(datasetname)); // Add a prior on the first pose if (soft_prior) From f8d2d7a7720014929060504cfe97f9474551c930 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Mon, 4 Jun 2012 19:53:30 +0000 Subject: [PATCH 34/35] Wrapped and grouped lots of geometry methods --- gtsam.h | 286 ++++++++++++++++++++++++++++++--------- gtsam/geometry/Pose2.cpp | 6 +- gtsam/geometry/Pose2.h | 6 +- 3 files changed, 230 insertions(+), 68 deletions(-) diff --git a/gtsam.h b/gtsam.h index 2015705ae..a08c753f1 100644 --- a/gtsam.h +++ b/gtsam.h @@ -68,72 +68,142 @@ namespace gtsam { //************************************************************************* class Point2 { + // Standard Constructors Point2(); Point2(double x, double y); - static gtsam::Point2 Expmap(Vector v); + + // Testable + void print(string s) const; + bool equals(const gtsam::Point2& pose, double tol) const; + + // Group + static gtsam::Point2 identity(); + gtsam::Point2 inverse() const; + gtsam::Point2 compose(const gtsam::Point2& p2) const; + gtsam::Point2 between(const gtsam::Point2& p2) const; + + // Manifold + static size_t Dim(); + size_t dim() const; + gtsam::Point2 retract(Vector v) const; + Vector localCoordinates(const gtsam::Point2& p) const; + + // Lie Group + static gtsam::Point2 Expmap(Vector v); static Vector Logmap(const gtsam::Point2& p); - void print(string s) const; - double x(); - double y(); - Vector localCoordinates(const gtsam::Point2& p); - gtsam::Point2 compose(const gtsam::Point2& p2); - gtsam::Point2 between(const gtsam::Point2& p2); - gtsam::Point2 retract(Vector v); + + // Standard Interface + double x() const; + double y() const; + Vector vector() const; }; class StereoPoint2 { + // Standard Constructors StereoPoint2(); StereoPoint2(double uL, double uR, double v); + + // Testable + void print(string s) const; + bool equals(const gtsam::StereoPoint2& point, double tol) const; + + // Group + static gtsam::StereoPoint2 identity(); + gtsam::StereoPoint2 inverse() const; + gtsam::StereoPoint2 compose(const gtsam::StereoPoint2& p2) const; + gtsam::StereoPoint2 between(const gtsam::StereoPoint2& p2) const; + + // Manifold + static size_t Dim(); + size_t dim() const; + gtsam::StereoPoint2 retract(Vector v) const; + Vector localCoordinates(const gtsam::StereoPoint2& p) const; + + // Lie Group static gtsam::StereoPoint2 Expmap(Vector v); static Vector Logmap(const gtsam::StereoPoint2& p); - void print(string s) const; - Vector localCoordinates(const gtsam::StereoPoint2& p); - gtsam::StereoPoint2 compose(const gtsam::StereoPoint2& p2); - gtsam::StereoPoint2 between(const gtsam::StereoPoint2& p2); - gtsam::StereoPoint2 retract(Vector v); + + // Standard Interface + Vector vector() const; }; class Point3 { + // Standard Constructors Point3(); Point3(double x, double y, double z); Point3(Vector v); - static gtsam::Point3 Expmap(Vector v); + + // Testable + void print(string s) const; + bool equals(const gtsam::Point3& p, double tol) const; + + // Group + static gtsam::Point3 identity(); + gtsam::Point3 inverse() const; + gtsam::Point3 compose(const gtsam::Point3& p2) const; + gtsam::Point3 between(const gtsam::Point3& p2) const; + + // Manifold + static size_t Dim(); + size_t dim() const; + gtsam::Point3 retract(Vector v) const; + Vector localCoordinates(const gtsam::Point3& p) const; + + // Lie Group + static gtsam::Point3 Expmap(Vector v); static Vector Logmap(const gtsam::Point3& p); - void print(string s) const; - bool equals(const gtsam::Point3& p, double tol); + + // Standard Interface Vector vector() const; - double x(); - double y(); - double z(); - Vector localCoordinates(const gtsam::Point3& p); - gtsam::Point3 retract(Vector v); - gtsam::Point3 compose(const gtsam::Point3& p2); - gtsam::Point3 between(const gtsam::Point3& p2); + double x() const; + double y() const; + double z() const; }; class Rot2 { + // Standard Constructors and Named Constructors Rot2(); Rot2(double theta); - static gtsam::Rot2 Expmap(Vector v); + static gtsam::Rot2 fromAngle(double theta); + static gtsam::Rot2 fromDegrees(double theta); + static gtsam::Rot2 fromCosSin(double c, double s); + + // Testable + void print(string s) const; + bool equals(const gtsam::Rot2& rot, double tol) const; + + // Group + static gtsam::Rot2 identity(); + static gtsam::Rot2 inverse(); + gtsam::Rot2 compose(const gtsam::Rot2& p2) const; + gtsam::Rot2 between(const gtsam::Rot2& p2) const; + + // Manifold + static size_t Dim(); + size_t dim() const; + gtsam::Rot2 retract(Vector v) const; + Vector localCoordinates(const gtsam::Rot2& p) const; + + // Lie Group + static gtsam::Rot2 Expmap(Vector v); static Vector Logmap(const gtsam::Rot2& p); - static gtsam::Rot2 fromAngle(double theta); - static gtsam::Rot2 fromDegrees(double theta); - static gtsam::Rot2 fromCosSin(double c, double s); + + // Group Action on Point2 + gtsam::Point2 rotate(const gtsam::Point2& point) const; + gtsam::Point2 unrotate(const gtsam::Point2& point) const; + + // Standard Interface static gtsam::Rot2 relativeBearing(const gtsam::Point2& d); // Ignoring derivative static gtsam::Rot2 atan2(double y, double x); - void print(string s) const; - bool equals(const gtsam::Rot2& rot, double tol) const; double theta() const; double degrees() const; double c() const; double s() const; - Vector localCoordinates(const gtsam::Rot2& p); - gtsam::Rot2 retract(Vector v); - gtsam::Rot2 compose(const gtsam::Rot2& p2); - gtsam::Rot2 between(const gtsam::Rot2& p2); + Matrix matrix() const; }; class Rot3 { + // Standard Constructors and Named Constructors Rot3(); Rot3(Matrix R); static gtsam::Rot3 Rx(double t); @@ -147,17 +217,29 @@ class Rot3 { static gtsam::Rot3 ypr(double y, double p, double r); static gtsam::Rot3 quaternion(double w, double x, double y, double z); static gtsam::Rot3 rodriguez(Vector v); + + // Testable void print(string s) const; bool equals(const gtsam::Rot3& rot, double tol) const; + + // Group static gtsam::Rot3 identity(); + gtsam::Rot3 inverse() const; gtsam::Rot3 compose(const gtsam::Rot3& p2) const; - gtsam::Rot3 inverse() const; gtsam::Rot3 between(const gtsam::Rot3& p2) const; + + // Manifold + static size_t Dim(); + size_t dim() const; + gtsam::Rot3 retractCayley(Vector v) const; + gtsam::Rot3 retract(Vector v) const; + Vector localCoordinates(const gtsam::Rot3& p) const; + + // Group Action on Point3 gtsam::Point3 rotate(const gtsam::Point3& p) const; gtsam::Point3 unrotate(const gtsam::Point3& p) const; - gtsam::Rot3 retractCayley(Vector v) const; - gtsam::Rot3 retract(Vector v) const; - Vector localCoordinates(const gtsam::Rot3& p) const; + + // Standard Interface static gtsam::Rot3 Expmap(Vector v); static Vector Logmap(const gtsam::Rot3& p); Matrix matrix() const; @@ -170,30 +252,53 @@ class Rot3 { double pitch() const; double yaw() const; // Vector toQuaternion() const; // FIXME: Can't cast to Vector properly + Matrix matrix() const; }; class Pose2 { + // Standard Constructor Pose2(); Pose2(double x, double y, double theta); Pose2(double theta, const gtsam::Point2& t); Pose2(const gtsam::Rot2& r, const gtsam::Point2& t); Pose2(Vector v); + + // Testable + void print(string s) const; + bool equals(const gtsam::Pose2& pose, double tol) const; + + // Group + static gtsam::Pose2 identity(); + gtsam::Pose2 inverse() const; + gtsam::Pose2 compose(const gtsam::Pose2& p2) const; + gtsam::Pose2 between(const gtsam::Pose2& p2) const; + + // Manifold + static size_t Dim(); + size_t dim() const; + gtsam::Pose2 retract(Vector v) const; + Vector localCoordinates(const gtsam::Pose2& p) const; + + // Lie Group static gtsam::Pose2 Expmap(Vector v); static Vector Logmap(const gtsam::Pose2& p); - void print(string s) const; - bool equals(const gtsam::Pose2& pose, double tol) const; + Matrix adjointMap() const; + Vector adjoint() const; + static Matrix wedge(); + + // Group Actions on Point2 + gtsam::Point2 transform_from(const gtsam::Point2& p) const; + gtsam::Point2 transform_to(const gtsam::Point2& p) const; + + // Standard Interface double x() const; double y() const; double theta() const; - size_t dim() const; - Vector localCoordinates(const gtsam::Pose2& p); - gtsam::Pose2 retract(Vector v); - gtsam::Pose2 compose(const gtsam::Pose2& p2); - gtsam::Pose2 between(const gtsam::Pose2& p2); - gtsam::Rot2 bearing(const gtsam::Point2& point); - double range(const gtsam::Point2& point); + gtsam::Rot2 bearing(const gtsam::Point2& point) const; + double range(const gtsam::Point2& point) const; gtsam::Point2 translation() const; gtsam::Rot2 rotation() const; + Matrix matrix() const; }; class Pose3 { @@ -210,15 +315,15 @@ class Pose3 { // Group static gtsam::Pose3 identity(); - gtsam::Pose3 inverse(); - gtsam::Pose3 compose(const gtsam::Pose3& p2); - gtsam::Pose3 between(const gtsam::Pose3& p2); + gtsam::Pose3 inverse() const; + gtsam::Pose3 compose(const gtsam::Pose3& p2) const; + gtsam::Pose3 between(const gtsam::Pose3& p2) const; // Manifold static size_t Dim(); size_t dim() const; - gtsam::Pose3 retract(Vector v); - gtsam::Pose3 retractFirstOrder(Vector v); + gtsam::Pose3 retract(Vector v) const; + gtsam::Pose3 retractFirstOrder(Vector v) const; Vector localCoordinates(const gtsam::Pose3& T2) const; // Lie Group @@ -245,41 +350,98 @@ class Pose3 { }; class Cal3_S2 { + // Standard Constructors Cal3_S2(); Cal3_S2(double fx, double fy, double s, double u0, double v0); + // Testable void print(string s) const; + bool equals(const gtsam::Cal3_S2& pose, double tol) const; + + // Manifold + static size_t Dim(); + size_t dim() const; + gtsam::Cal3_S2 retract(Vector v) const; + Vector localCoordinates(const gtsam::Cal3_S2& c) const; + + // Action on Point2 + gtsam::Point2 calibrate(const gtsam::Point2& p) const; + gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; + + // Standard Interface + double fx() const; + double fy() const; + double skew() const; + double px() const; + double py() const; + gtsam::Point2 principalPoint() const; + Vector vector() const; + Matrix matrix() const; + Matrix matrix_inverse() const; }; class Cal3_S2Stereo { + // Standard Constructors Cal3_S2Stereo(); Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b); + // Testable void print(string s) const; + bool equals(const gtsam::Cal3_S2Stereo& pose, double tol) const; + + // Manifold + static size_t Dim(); + size_t dim() const; + gtsam::Cal3_S2Stereo retract(Vector v) const; + Vector localCoordinates(const gtsam::Cal3_S2Stereo& c) const; + + // Action on Point2 + gtsam::Point2 calibrate(const gtsam::Point2& p) const; + gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; + + // Standard Interface + double fx() const; + double fy() const; + double skew() const; + double px() const; + double py() const; + gtsam::Point2 principalPoint() const; + Vector vector() const; + Matrix matrix() const; + Matrix matrix_inverse() const; + double baseline() const; }; class CalibratedCamera { - + // Standard Constructors and Named Constructors CalibratedCamera(); CalibratedCamera(const gtsam::Pose3& pose); CalibratedCamera(const Vector& v); + gtsam::CalibratedCamera level(const gtsam::Pose2& pose2, double height); + // Testable void print(string s) const; bool equals(const gtsam::CalibratedCamera& camera, double tol) const; + // Manifold + static size_t Dim(); + size_t dim() const; + gtsam::CalibratedCamera retract(const Vector& d) const; + Vector localCoordinates(const gtsam::CalibratedCamera& T2) const; + + // Group + gtsam::CalibratedCamera compose(const gtsam::CalibratedCamera& c) const; + gtsam::CalibratedCamera inverse() const; + + // Action on Point3 + gtsam::Point2 project(const gtsam::Point3& point) const; + static gtsam::Point2 project_to_camera(const gtsam::Point3& cameraPoint); + + // Standard Interface gtsam::Pose3 pose() const; - - gtsam::CalibratedCamera compose(const gtsam::CalibratedCamera& c) const; - gtsam::CalibratedCamera inverse() const; - gtsam::CalibratedCamera level(const gtsam::Pose2& pose2, double height); - gtsam::CalibratedCamera retract(const Vector& d) const; - Vector localCoordinates(const gtsam::CalibratedCamera& T2) const; - - gtsam::Point2 project(const gtsam::Point3& point) const; - static gtsam::Point2 project_to_camera(const gtsam::Point3& cameraPoint); + double range(const gtsam::Point3& p) const; // TODO: Other overloaded range methods }; - //************************************************************************* // inference //************************************************************************* diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index e39dbc803..fa02ecfb8 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -108,7 +108,7 @@ Vector Pose2::localCoordinates(const Pose2& p2) const { /* ************************************************************************* */ // Calculate Adjoint map // Ad_pose is 3*3 matrix that when applied to twist xi, returns Ad_pose(xi) -Matrix Pose2::AdjointMap() const { +Matrix Pose2::adjointMap() const { double c = r_.c(), s = r_.s(), x = t_.x(), y = t_.y(); return Matrix_(3,3, c, -s, y, @@ -119,7 +119,7 @@ Matrix Pose2::AdjointMap() const { /* ************************************************************************* */ Pose2 Pose2::inverse(boost::optional H1) const { - if (H1) *H1 = -AdjointMap(); + if (H1) *H1 = -adjointMap(); return Pose2(r_.inverse(), r_.unrotate(Point2(-t_.x(), -t_.y()))); } @@ -142,7 +142,7 @@ Point2 Pose2::transform_to(const Point2& point, Pose2 Pose2::compose(const Pose2& p2, boost::optional H1, boost::optional H2) const { // TODO: inline and reuse? - if(H1) *H1 = p2.inverse().AdjointMap(); + if(H1) *H1 = p2.inverse().adjointMap(); if(H2) *H2 = I3; return (*this)*p2; } diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index 877eb93a1..55fe0a9d4 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -158,10 +158,10 @@ public: * Calculate Adjoint map * Ad_pose is 3*3 matrix that when applied to twist xi, returns Ad_pose(xi) */ - Matrix AdjointMap() const; - inline Vector Adjoint(const Vector& xi) const { + Matrix adjointMap() const; + inline Vector adjoint(const Vector& xi) const { assert(xi.size() == 3); - return AdjointMap()*xi; + return adjointMap()*xi; } /** From e3922d7b34ad8e311a7b4357d1867280ab0432b2 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Mon, 4 Jun 2012 20:10:20 +0000 Subject: [PATCH 35/35] Missed rename --- gtsam/geometry/tests/testPose2.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index cdde3cd54..2a0f9ebec 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -426,7 +426,7 @@ TEST( Pose2, between ) EXPECT(assert_equal(expectedH1,actualH1)); EXPECT(assert_equal(numericalH1,actualH1)); // Assert H1 = -AdjointMap(between(p2,p1)) as in doc/math.lyx - EXPECT(assert_equal(-gT2.between(gT1).AdjointMap(),actualH1)); + EXPECT(assert_equal(-gT2.between(gT1).adjointMap(),actualH1)); Matrix expectedH2 = Matrix_(3,3, 1.0, 0.0, 0.0,