From 92bd5f18cf1ece302cc6aeaf3a4fd7627b02baee Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Mon, 1 Jul 2013 20:20:14 +0000 Subject: [PATCH 1/6] Formatting fixes --- CMakeLists.txt | 4 ++-- gtsam/CMakeLists.txt | 2 -- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index bdbd25abf..341082d17 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -274,9 +274,9 @@ print_config_flag(${GTSAM_BUILD_TESTS} "Build Tests if (DOXYGEN_FOUND) print_config_flag(${GTSAM_BUILD_DOCS} "Build Docs ") endif() +print_config_flag(${GTSAM_BUILD_SHARED_LIBRARY} "Build shared GTSAM Library ") +print_config_flag(${GTSAM_BUILD_STATIC_LIBRARY} "Build static GTSAM Library ") if(NOT MSVC) - print_config_flag(${GTSAM_BUILD_SHARED_LIBRARY} "Build shared GTSAM Library ") - print_config_flag(${GTSAM_BUILD_STATIC_LIBRARY} "Build static GTSAM Library ") print_config_flag(${GTSAM_BUILD_CONVENIENCE_LIBRARIES} "Build Convenience Libraries ") endif() print_config_flag(${GTSAM_BUILD_TYPE_POSTFIXES} "Put build-type in library name ") diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index 55a43f1dc..36b6d3509 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -70,8 +70,6 @@ foreach(subdir ${gtsam_subdirs}) add_subdirectory(${subdir}) endforeach(subdir) -message(STATUS "navigation_srcs: [${navigation_srcs}]") - # To add additional sources to gtsam when building the full library (static or shared) # Add the subfolder with _srcs appended to the end to this list set(gtsam_srcs From a897015a113674b525b8a7146b06855ec5f40ff7 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Tue, 2 Jul 2013 13:03:00 +0000 Subject: [PATCH 2/6] Fixed VS folder --- wrap/CMakeLists.txt | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/wrap/CMakeLists.txt b/wrap/CMakeLists.txt index 7226d8006..6c1cf8d4b 100644 --- a/wrap/CMakeLists.txt +++ b/wrap/CMakeLists.txt @@ -12,6 +12,10 @@ gtsam_assign_source_folders(${wrap_srcs} ${wrap_headers}) add_executable(wrap wrap.cpp) target_link_libraries(wrap wrap_lib ${WRAP_BOOST_LIBRARIES}) +# Set folder in Visual Studio +file(RELATIVE_PATH relative_path "${PROJECT_SOURCE_DIR}" "${CMAKE_CURRENT_SOURCE_DIR}") +set_target_properties(wrap_lib wrap PROPERTIES FOLDER "${relative_path}") + # Install wrap binary and export target if (GTSAM_INSTALL_WRAP) install(TARGETS wrap EXPORT GTSAM-exports DESTINATION bin) From 7f08cf6ba12b5887db2929412a4b22d63130dda7 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Wed, 3 Jul 2013 20:20:32 +0000 Subject: [PATCH 3/6] Fixed indexing problem in KalmanFilter - linear variable index was incremented, resulting in allocating larger and larger data structures with each step. Now shifting indices back to 0 each step. --- gtsam/linear/KalmanFilter.cpp | 22 +++++++++++----------- gtsam/linear/KalmanFilter.h | 5 ----- gtsam/linear/tests/testKalmanFilter.cpp | 2 -- 3 files changed, 11 insertions(+), 18 deletions(-) diff --git a/gtsam/linear/KalmanFilter.cpp b/gtsam/linear/KalmanFilter.cpp index c32621ca2..8c36cedc1 100644 --- a/gtsam/linear/KalmanFilter.cpp +++ b/gtsam/linear/KalmanFilter.cpp @@ -59,7 +59,12 @@ namespace gtsam { factorGraph.push_back(GaussianFactor::shared_ptr(newFactor)); // Eliminate graph in order x0, x1, to get Bayes net P(x0|x1)P(x1) - return solve(factorGraph, useQR); + GaussianDensity::shared_ptr result = solve(factorGraph, useQR); + + // Change key in result to 0 to start back at variable index 0 and return + assert(result->nrFrontals() == 1); + result->frontals().front() = 0; + return result; } /* ************************************************************************* */ @@ -94,8 +99,7 @@ namespace gtsam { // The factor related to the motion model is defined as // f2(x_{t},x_{t+1}) = (F*x_{t} + B*u - x_{t+1}) * Q^-1 * (F*x_{t} + B*u - x_{t+1})^T - Index k = step(p); - return fuse(p, new JacobianFactor(k, -F, k + 1, I_, B * u, model), useQR()); + return fuse(p, new JacobianFactor(0, -F, 1, I_, B * u, model), useQR()); } /* ************************************************************************* */ @@ -119,8 +123,7 @@ namespace gtsam { Matrix G12 = -Ft * M, G11 = -G12 * F, G22 = M; Vector b = B * u, g2 = M * b, g1 = -Ft * g2; double f = dot(b, g2); - Index k = step(p); - return fuse(p, new HessianFactor(k, k + 1, G11, G12, g1, G22, g2, f), + return fuse(p, new HessianFactor(0, 1, G11, G12, g1, G22, g2, f), useQR()); } @@ -129,8 +132,7 @@ namespace gtsam { const Matrix& A1, const Vector& b, const SharedDiagonal& model) { // Nhe factor related to the motion model is defined as // f2(x_{t},x_{t+1}) = |A0*x_{t} + A1*x_{t+1} - b|^2 - Index k = step(p); - return fuse(p, new JacobianFactor(k, A0, k + 1, A1, b, model), useQR()); + return fuse(p, new JacobianFactor(0, A0, 1, A1, b, model), useQR()); } /* ************************************************************************* */ @@ -139,19 +141,17 @@ namespace gtsam { // The factor related to the measurements would be defined as // f2 = (h(x_{t}) - z_{t}) * R^-1 * (h(x_{t}) - z_{t})^T // = (x_{t} - z_{t}) * R^-1 * (x_{t} - z_{t})^T - Index k = step(p); - return fuse(p, new JacobianFactor(k, H, z, model), useQR()); + return fuse(p, new JacobianFactor(0, H, z, model), useQR()); } /* ************************************************************************* */ KalmanFilter::State KalmanFilter::updateQ(const State& p, const Matrix& H, const Vector& z, const Matrix& Q) { - Index k = step(p); Matrix M = inverse(Q), Ht = trans(H); Matrix G = Ht * M * H; Vector g = Ht * M * z; double f = dot(z, M * z); - return fuse(p, new HessianFactor(k, G, g, f), useQR()); + return fuse(p, new HessianFactor(0, G, g, f), useQR()); } /* ************************************************************************* */ diff --git a/gtsam/linear/KalmanFilter.h b/gtsam/linear/KalmanFilter.h index 0089bd34d..6b062d696 100644 --- a/gtsam/linear/KalmanFilter.h +++ b/gtsam/linear/KalmanFilter.h @@ -86,11 +86,6 @@ namespace gtsam { /// print void print(const std::string& s = "") const; - /** Return step index k, starts at 0, incremented at each predict. */ - static Index step(const State& p) { - return p->firstFrontalKey(); - } - /** * Predict the state P(x_{t+1}|Z^t) * In Kalman Filter notation, this is x_{t+1|t} and P_{t+1|t} diff --git a/gtsam/linear/tests/testKalmanFilter.cpp b/gtsam/linear/tests/testKalmanFilter.cpp index 083e942c3..476846d79 100644 --- a/gtsam/linear/tests/testKalmanFilter.cpp +++ b/gtsam/linear/tests/testKalmanFilter.cpp @@ -124,11 +124,9 @@ TEST( KalmanFilter, linear1 ) { // Run iteration 3 KalmanFilter::State p3p = kf.predict(p2, F, B, u, modelQ); EXPECT(assert_equal(expected3, p3p->mean())); - LONGS_EQUAL(3, KalmanFilter::step(p3p)); KalmanFilter::State p3 = kf.update(p3p, H, z3, modelR); EXPECT(assert_equal(expected3, p3->mean())); - LONGS_EQUAL(3, KalmanFilter::step(p3)); } /* ************************************************************************* */ From 59d549dae2443b6513cc28308e1748f6c2131500 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Wed, 3 Jul 2013 21:55:30 +0000 Subject: [PATCH 4/6] Fixed indexing problem in KalmanFilter - linear variable index was incremented, resulting in allocating larger and larger data structures with each step. Now shifting indices back to 0 each step. --- gtsam.h | 1 - 1 file changed, 1 deletion(-) diff --git a/gtsam.h b/gtsam.h index ee069e0d3..ddcb5a368 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1545,7 +1545,6 @@ class KalmanFilter { // gtsam::GaussianDensity* init(Vector x0, const gtsam::SharedDiagonal& P0); gtsam::GaussianDensity* init(Vector x0, Matrix P0); void print(string s) const; - static size_t step(gtsam::GaussianDensity* p); gtsam::GaussianDensity* predict(gtsam::GaussianDensity* p, Matrix F, Matrix B, Vector u, const gtsam::noiseModel::Diagonal* modelQ); gtsam::GaussianDensity* predictQ(gtsam::GaussianDensity* p, Matrix F, From 4af8d3156df7aad2b4e26743128c67f553780ce6 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Fri, 5 Jul 2013 15:46:07 +0000 Subject: [PATCH 5/6] Fixed indexing problem in KalmanFilter in a different way - now just modify the indices before solving and put them back afterwards, so that the timestep is still available --- gtsam.h | 1 + gtsam/linear/KalmanFilter.cpp | 44 +++++++++++++++++-------- gtsam/linear/KalmanFilter.h | 5 +++ gtsam/linear/tests/testKalmanFilter.cpp | 2 ++ 4 files changed, 38 insertions(+), 14 deletions(-) diff --git a/gtsam.h b/gtsam.h index ddcb5a368..ee069e0d3 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1545,6 +1545,7 @@ class KalmanFilter { // gtsam::GaussianDensity* init(Vector x0, const gtsam::SharedDiagonal& P0); gtsam::GaussianDensity* init(Vector x0, Matrix P0); void print(string s) const; + static size_t step(gtsam::GaussianDensity* p); gtsam::GaussianDensity* predict(gtsam::GaussianDensity* p, Matrix F, Matrix B, Vector u, const gtsam::noiseModel::Diagonal* modelQ); gtsam::GaussianDensity* predictQ(gtsam::GaussianDensity* p, Matrix F, diff --git a/gtsam/linear/KalmanFilter.cpp b/gtsam/linear/KalmanFilter.cpp index 8c36cedc1..4a4730e60 100644 --- a/gtsam/linear/KalmanFilter.cpp +++ b/gtsam/linear/KalmanFilter.cpp @@ -24,6 +24,7 @@ #include #include #include +#include #include #include @@ -33,16 +34,31 @@ namespace gtsam { using namespace std; /// Auxiliary function to solve factor graph and return pointer to root conditional - KalmanFilter::State solve(const GaussianFactorGraph& factorGraph, - bool useQR) { + KalmanFilter::State solve(const GaussianFactorGraph& factorGraph, bool useQR) + { + // Start indices at zero + Index nVars = 0; + internal::Reduction remapping; + BOOST_FOREACH(const GaussianFactorGraph::sharedFactor& factor, factorGraph) + BOOST_FOREACH(Index j, *factor) + if(remapping.insert(make_pair(j, nVars)).second) + ++ nVars; + Permutation inverseRemapping = remapping.inverse(); + GaussianFactorGraph factorGraphOrdered(factorGraph); // NOTE this shares the factors with the original!! + factorGraphOrdered.reduceWithInverse(remapping); // Solve the factor graph - GaussianSequentialSolver solver(factorGraph, useQR); + GaussianSequentialSolver solver(factorGraphOrdered, useQR); GaussianBayesNet::shared_ptr bayesNet = solver.eliminate(); // As this is a filter, all we need is the posterior P(x_t), // so we just keep the root of the Bayes net GaussianConditional::shared_ptr conditional = bayesNet->back(); + + // Undo the remapping + factorGraphOrdered.permuteWithInverse(inverseRemapping); + conditional->permuteWithInverse(inverseRemapping); + // TODO: awful ! A copy constructor followed by ANOTHER copy constructor in make_shared? return boost::make_shared(*conditional); } @@ -59,12 +75,7 @@ namespace gtsam { factorGraph.push_back(GaussianFactor::shared_ptr(newFactor)); // Eliminate graph in order x0, x1, to get Bayes net P(x0|x1)P(x1) - GaussianDensity::shared_ptr result = solve(factorGraph, useQR); - - // Change key in result to 0 to start back at variable index 0 and return - assert(result->nrFrontals() == 1); - result->frontals().front() = 0; - return result; + return solve(factorGraph, useQR); } /* ************************************************************************* */ @@ -99,7 +110,8 @@ namespace gtsam { // The factor related to the motion model is defined as // f2(x_{t},x_{t+1}) = (F*x_{t} + B*u - x_{t+1}) * Q^-1 * (F*x_{t} + B*u - x_{t+1})^T - return fuse(p, new JacobianFactor(0, -F, 1, I_, B * u, model), useQR()); + Index k = step(p); + return fuse(p, new JacobianFactor(k, -F, k + 1, I_, B * u, model), useQR()); } /* ************************************************************************* */ @@ -123,7 +135,8 @@ namespace gtsam { Matrix G12 = -Ft * M, G11 = -G12 * F, G22 = M; Vector b = B * u, g2 = M * b, g1 = -Ft * g2; double f = dot(b, g2); - return fuse(p, new HessianFactor(0, 1, G11, G12, g1, G22, g2, f), + Index k = step(p); + return fuse(p, new HessianFactor(k, k + 1, G11, G12, g1, G22, g2, f), useQR()); } @@ -132,7 +145,8 @@ namespace gtsam { const Matrix& A1, const Vector& b, const SharedDiagonal& model) { // Nhe factor related to the motion model is defined as // f2(x_{t},x_{t+1}) = |A0*x_{t} + A1*x_{t+1} - b|^2 - return fuse(p, new JacobianFactor(0, A0, 1, A1, b, model), useQR()); + Index k = step(p); + return fuse(p, new JacobianFactor(k, A0, k + 1, A1, b, model), useQR()); } /* ************************************************************************* */ @@ -141,17 +155,19 @@ namespace gtsam { // The factor related to the measurements would be defined as // f2 = (h(x_{t}) - z_{t}) * R^-1 * (h(x_{t}) - z_{t})^T // = (x_{t} - z_{t}) * R^-1 * (x_{t} - z_{t})^T - return fuse(p, new JacobianFactor(0, H, z, model), useQR()); + Index k = step(p); + return fuse(p, new JacobianFactor(k, H, z, model), useQR()); } /* ************************************************************************* */ KalmanFilter::State KalmanFilter::updateQ(const State& p, const Matrix& H, const Vector& z, const Matrix& Q) { + Index k = step(p); Matrix M = inverse(Q), Ht = trans(H); Matrix G = Ht * M * H; Vector g = Ht * M * z; double f = dot(z, M * z); - return fuse(p, new HessianFactor(0, G, g, f), useQR()); + return fuse(p, new HessianFactor(k, G, g, f), useQR()); } /* ************************************************************************* */ diff --git a/gtsam/linear/KalmanFilter.h b/gtsam/linear/KalmanFilter.h index 6b062d696..0089bd34d 100644 --- a/gtsam/linear/KalmanFilter.h +++ b/gtsam/linear/KalmanFilter.h @@ -86,6 +86,11 @@ namespace gtsam { /// print void print(const std::string& s = "") const; + /** Return step index k, starts at 0, incremented at each predict. */ + static Index step(const State& p) { + return p->firstFrontalKey(); + } + /** * Predict the state P(x_{t+1}|Z^t) * In Kalman Filter notation, this is x_{t+1|t} and P_{t+1|t} diff --git a/gtsam/linear/tests/testKalmanFilter.cpp b/gtsam/linear/tests/testKalmanFilter.cpp index 476846d79..083e942c3 100644 --- a/gtsam/linear/tests/testKalmanFilter.cpp +++ b/gtsam/linear/tests/testKalmanFilter.cpp @@ -124,9 +124,11 @@ TEST( KalmanFilter, linear1 ) { // Run iteration 3 KalmanFilter::State p3p = kf.predict(p2, F, B, u, modelQ); EXPECT(assert_equal(expected3, p3p->mean())); + LONGS_EQUAL(3, KalmanFilter::step(p3p)); KalmanFilter::State p3 = kf.update(p3p, H, z3, modelR); EXPECT(assert_equal(expected3, p3->mean())); + LONGS_EQUAL(3, KalmanFilter::step(p3)); } /* ************************************************************************* */ From ecb24ebd0664d17995818f251138a916b4c6e4d2 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Tue, 9 Jul 2013 15:43:55 +0000 Subject: [PATCH 6/6] Fixes to include path ordering and boost libraries --- CMakeLists.txt | 16 ++++++++++------ gtsam_unstable/CMakeLists.txt | 2 +- 2 files changed, 11 insertions(+), 7 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 341082d17..b976d5008 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -114,13 +114,13 @@ endif() option(GTSAM_DISABLE_NEW_TIMERS "Disables using Boost.chrono for timing" OFF) # Allow for not using the timer libraries on boost < 1.48 (GTSAM timing code falls back to old timer library) -set(GTSAM_BOOST_LIBRARIES ${Boost_SERIALIZATION_LIBRARY} ${Boost_SYSTEM_LIBRARY} ${Boost_FILESYSTEM_LIBRARY}) +set(GTSAM_BOOST_LIBRARIES ${Boost_SERIALIZATION_LIBRARY} ${Boost_SYSTEM_LIBRARY} ${Boost_FILESYSTEM_LIBRARY} ${Boost_THREAD_LIBRARY} ${Boost_DATE_TIME_LIBRARY} ${Boost_REGEX_LIBRARY}) if (GTSAM_DISABLE_NEW_TIMERS) message("WARNING: GTSAM timing instrumentation manually disabled") add_definitions(-DGTSAM_DISABLE_NEW_TIMERS) else() if(Boost_TIMER_LIBRARY) - list(APPEND GTSAM_BOOST_LIBRARIES ${GTSAM_BOOST_LIBRARIES} ${Boost_TIMER_LIBRARY} ${Boost_CHRONO_LIBRARY}) + list(APPEND GTSAM_BOOST_LIBRARIES ${Boost_TIMER_LIBRARY} ${Boost_CHRONO_LIBRARY}) else() message("WARNING: Boost older than 1.48 was found, GTSAM timing instrumentation will use the older, less accurate, Boost timer library.") endif() @@ -158,15 +158,19 @@ install(FILES ${CMAKE_BINARY_DIR}/gtsam/3rdparty/gtsam_eigen_includes.h DESTINAT ############################################################################### # Global compile options -# General build settings +# Include boost - use 'BEFORE' so that a specific boost specified to CMake +# takes precedence over a system-installed one. +include_directories(BEFORE ${Boost_INCLUDE_DIR}) + +# Add includes for source directories 'BEFORE' boost and any system include +# paths so that the compiler uses GTSAM headers in our source directory instead +# of any previously installed GTSAM headers. include_directories(BEFORE gtsam/3rdparty/UFconfig gtsam/3rdparty/CCOLAMD/Include ${CMAKE_SOURCE_DIR} ${CMAKE_BINARY_DIR} # So we can include generated config header files - CppUnitLite - ${Boost_INCLUDE_DIR}) -link_directories(${Boost_LIBRARY_DIRS}) + CppUnitLite) if(MSVC) add_definitions(-D_CRT_SECURE_NO_WARNINGS -D_SCL_SECURE_NO_WARNINGS) diff --git a/gtsam_unstable/CMakeLists.txt b/gtsam_unstable/CMakeLists.txt index 39fcaccc1..f59208ef6 100644 --- a/gtsam_unstable/CMakeLists.txt +++ b/gtsam_unstable/CMakeLists.txt @@ -10,7 +10,7 @@ set (gtsam_unstable_subdirs slam ) -set(GTSAM_UNSTABLE_BOOST_LIBRARIES ${GTSAM_BOOST_LIBRARIES} ${Boost_THREAD_LIBRARY}) +set(GTSAM_UNSTABLE_BOOST_LIBRARIES ${GTSAM_BOOST_LIBRARIES}) add_custom_target(check.unstable COMMAND ${CMAKE_CTEST_COMMAND} --output-on-failure)