diff --git a/CMakeLists.txt b/CMakeLists.txt index d82e31228..45a57827d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -111,17 +111,19 @@ set(CPACK_GENERATOR "TGZ" CACHE STRING "CPack Default Binary Generator") # BOOST_ROOT: path to install prefix for boost # Boost_NO_SYSTEM_PATHS: set to true to keep the find script from ignoring BOOST_ROOT -# If using Boost shared libs, disable auto linking if(MSVC) - # Some libraries, at least Boost Program Options, rely on this to export DLL symbols - # Disable autolinking + # By default, boost only builds static libraries on windows + set(Boost_USE_STATIC_LIBS ON) # only find static libs + # If we ever reset above on windows and, ... + # If we use Boost shared libs, disable auto linking. + # Some libraries, at least Boost Program Options, rely on this to export DLL symbols. if(NOT Boost_USE_STATIC_LIBS) add_definitions(-DBOOST_ALL_NO_LIB) add_definitions(-DBOOST_ALL_DYN_LINK) endif() endif() -find_package(Boost 1.43 COMPONENTS serialization system filesystem thread program_options date_time timer chrono) +find_package(Boost 1.43 COMPONENTS serialization system filesystem thread program_options date_time timer chrono regex) # Required components if(NOT Boost_SERIALIZATION_LIBRARY OR NOT Boost_SYSTEM_LIBRARY OR NOT Boost_FILESYSTEM_LIBRARY OR @@ -133,7 +135,7 @@ 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} - ${Boost_THREAD_LIBRARY} ${Boost_DATE_TIME_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) @@ -256,6 +258,14 @@ else() endif() +if (MSVC) + if (NOT GTSAM_BUILD_STATIC_LIBRARY) + # mute eigen static assert to avoid errors in shared lib + add_definitions(-DEIGEN_NO_STATIC_ASSERT) + endif() + add_definitions(/wd4244) # Disable loss of precision which is thrown all over our Eigen +endif() + ############################################################################### # Global compile options @@ -321,6 +331,7 @@ include_directories(BEFORE if(MSVC) add_definitions(-D_CRT_SECURE_NO_WARNINGS -D_SCL_SECURE_NO_WARNINGS) add_definitions(/wd4251 /wd4275 /wd4251 /wd4661 /wd4344 /wd4503) # Disable non-DLL-exported base class and other warnings + add_definitions(/bigobj) # Allow large object files for template-based code endif() # GCC 4.8+ complains about local typedefs which we use for shared_ptr etc. diff --git a/cmake/dllexport.h.in b/cmake/dllexport.h.in index 023f06f57..55683a496 100644 --- a/cmake/dllexport.h.in +++ b/cmake/dllexport.h.in @@ -26,21 +26,27 @@ // class __declspec(dllexport) MyClass { ... }; // When included while compiling other code against GTSAM: // class __declspec(dllimport) MyClass { ... }; + +#pragma once + +// Whether GTSAM is compiled as static or DLL in windows. +// This will be used to decide whether include __declspec(dllimport) or not in headers +#cmakedefine GTSAM_BUILD_STATIC_LIBRARY + #ifdef _WIN32 -# ifdef @library_name@_EXPORTS -# define @library_name@_EXPORT __declspec(dllexport) -# define @library_name@_EXTERN_EXPORT __declspec(dllexport) extern +# ifdef @library_name@_BUILD_STATIC_LIBRARY +# define @library_name@_EXPORT +# define @library_name@_EXTERN_EXPORT extern # else -# ifndef @library_name@_IMPORT_STATIC +# ifdef @library_name@_EXPORTS +# define @library_name@_EXPORT __declspec(dllexport) +# define @library_name@_EXTERN_EXPORT __declspec(dllexport) extern +# else # define @library_name@_EXPORT __declspec(dllimport) # define @library_name@_EXTERN_EXPORT __declspec(dllimport) -# else /* @library_name@_IMPORT_STATIC */ -# define @library_name@_EXPORT -# define @library_name@_EXTERN_EXPORT extern -# endif /* @library_name@_IMPORT_STATIC */ -# endif /* @library_name@_EXPORTS */ -#else /* _WIN32 */ +# endif +# endif +#else # define @library_name@_EXPORT # define @library_name@_EXTERN_EXPORT extern #endif - diff --git a/gtsam/base/GenericValue.h b/gtsam/base/GenericValue.h index 87ddf574c..67cc2646e 100644 --- a/gtsam/base/GenericValue.h +++ b/gtsam/base/GenericValue.h @@ -167,10 +167,10 @@ public: // implicit assignment operator for (const GenericValue& rhs) works fine here /// Assignment operator, protected because only the Value or DERIVED /// assignment operators should be used. - // DerivedValue& operator=(const DerivedValue& rhs) { - // // Nothing to do, do not call base class assignment operator - // return *this; - // } + GenericValue& operator=(const GenericValue& rhs) { + // Nothing to do, do not call base class assignment operator + return *this; + } private: @@ -187,13 +187,18 @@ public: ar & boost::serialization::make_nvp("GenericValue", boost::serialization::base_object(*this)); ar & boost::serialization::make_nvp("value", value_); - } + } + + + // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html + enum { NeedsToAlign = (sizeof(T) % 16) == 0 }; +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) +}; /// use this macro instead of BOOST_CLASS_EXPORT for GenericValues #define GTSAM_VALUE_EXPORT(Type) BOOST_CLASS_EXPORT(gtsam::GenericValue) -}; - // traits template struct traits > diff --git a/gtsam/base/testLie.h b/gtsam/base/testLie.h index 1906ec439..63eb8674d 100644 --- a/gtsam/base/testLie.h +++ b/gtsam/base/testLie.h @@ -10,8 +10,8 @@ * -------------------------------------------------------------------------- */ /* - * @file chartTesting.h - * @brief + * @file testLie.h + * @brief Test utilities for Lie groups * @date November, 2014 * @author Paul Furgale */ diff --git a/gtsam/discrete/DiscreteConditional.cpp b/gtsam/discrete/DiscreteConditional.cpp index 4a918ef02..f12cd2567 100644 --- a/gtsam/discrete/DiscreteConditional.cpp +++ b/gtsam/discrete/DiscreteConditional.cpp @@ -87,17 +87,19 @@ bool DiscreteConditional::equals(const DiscreteFactor& other, Potentials::ADT DiscreteConditional::choose(const Values& parentsValues) const { ADT pFS(*this); Key j; size_t value; - for(Key key: parents()) - try { - j = (key); - value = parentsValues.at(j); - pFS = pFS.choose(j, value); - } catch (exception&) { - cout << "Key: " << j << " Value: " << value << endl; - parentsValues.print("parentsValues: "); -// pFS.print("pFS: "); - throw runtime_error("DiscreteConditional::choose: parent value missing"); - }; + for(Key key: parents()) { + try { + j = (key); + value = parentsValues.at(j); + pFS = pFS.choose(j, value); + } catch (exception&) { + cout << "Key: " << j << " Value: " << value << endl; + parentsValues.print("parentsValues: "); + // pFS.print("pFS: "); + throw runtime_error("DiscreteConditional::choose: parent value missing"); + }; + } + return pFS; } diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h index 1f5d6f774..ecf9a572d 100644 --- a/gtsam/geometry/CameraSet.h +++ b/gtsam/geometry/CameraSet.h @@ -211,11 +211,11 @@ public: bool diagonalDamping = false) { if (E.cols() == 2) { Matrix2 P2; - ComputePointCovariance(P2, E, lambda, diagonalDamping); + ComputePointCovariance<2>(P2, E, lambda, diagonalDamping); return P2; } else { Matrix3 P3; - ComputePointCovariance(P3, E, lambda, diagonalDamping); + ComputePointCovariance<3>(P3, E, lambda, diagonalDamping); return P3; } } @@ -229,12 +229,12 @@ public: bool diagonalDamping = false) { if (E.cols() == 2) { Matrix2 P; - ComputePointCovariance(P, E, lambda, diagonalDamping); - return SchurComplement(Fblocks, E, P, b); + ComputePointCovariance<2>(P, E, lambda, diagonalDamping); + return SchurComplement<2>(Fblocks, E, P, b); } else { Matrix3 P; - ComputePointCovariance(P, E, lambda, diagonalDamping); - return SchurComplement(Fblocks, E, P, b); + ComputePointCovariance<3>(P, E, lambda, diagonalDamping); + return SchurComplement<3>(Fblocks, E, P, b); } } diff --git a/gtsam/geometry/tests/testCameraSet.cpp b/gtsam/geometry/tests/testCameraSet.cpp index f3feab741..60cee59ee 100644 --- a/gtsam/geometry/tests/testCameraSet.cpp +++ b/gtsam/geometry/tests/testCameraSet.cpp @@ -89,17 +89,17 @@ TEST(CameraSet, Pinhole) { Vector4 b = actualV; Vector v = Ft * (b - E * P * Et * b); schur << Ft * F - Ft * E * P * Et * F, v, v.transpose(), 30; - SymmetricBlockMatrix actualReduced = Set::SchurComplement(Fs, E, P, b); + SymmetricBlockMatrix actualReduced = Set::SchurComplement<3>(Fs, E, P, b); EXPECT(assert_equal(schur, actualReduced.selfadjointView())); // Check Schur complement update, same order, should just double KeyVector allKeys {1, 2}, keys {1, 2}; - Set::UpdateSchurComplement(Fs, E, P, b, allKeys, keys, actualReduced); + Set::UpdateSchurComplement<3>(Fs, E, P, b, allKeys, keys, actualReduced); EXPECT(assert_equal((Matrix )(2.0 * schur), actualReduced.selfadjointView())); // Check Schur complement update, keys reversed KeyVector keys2 {2, 1}; - Set::UpdateSchurComplement(Fs, E, P, b, allKeys, keys2, actualReduced); + Set::UpdateSchurComplement<3>(Fs, E, P, b, allKeys, keys2, actualReduced); Vector4 reverse_b; reverse_b << b.tail<2>(), b.head<2>(); Vector reverse_v = Ft * (reverse_b - E * P * Et * reverse_b); diff --git a/gtsam/geometry/tests/testOrientedPlane3.cpp b/gtsam/geometry/tests/testOrientedPlane3.cpp index b3d87f18c..a1531e07c 100644 --- a/gtsam/geometry/tests/testOrientedPlane3.cpp +++ b/gtsam/geometry/tests/testOrientedPlane3.cpp @@ -120,8 +120,6 @@ TEST(OrientedPlane3, localCoordinates_retract) { maxXiLimit << M_PI, M_PI, 10.0; for (size_t i = 0; i < numIterations; i++) { - sleep(0); - // Create a Plane OrientedPlane3 p1(randomVector(minPlaneLimit, maxPlaneLimit)); Vector v12 = randomVector(minXiLimit, maxXiLimit); diff --git a/gtsam/inference/Ordering.h b/gtsam/inference/Ordering.h index f37de12fe..f770c7da1 100644 --- a/gtsam/inference/Ordering.h +++ b/gtsam/inference/Ordering.h @@ -11,8 +11,10 @@ /** * @file Ordering.h + * @brief Variable ordering for the elimination algorithm * @author Richard Roberts * @author Andrew Melim + * @author Frank Dellaert * @date Sep 2, 2010 */ @@ -21,7 +23,6 @@ #include #include #include -#include #include #include @@ -77,8 +78,8 @@ public: /// Compute a fill-reducing ordering using COLAMD from a factor graph (see details for note on /// performance). This internally builds a VariableIndex so if you already have a VariableIndex, /// it is faster to use COLAMD(const VariableIndex&) - template - static Ordering Colamd(const FactorGraph& graph) { + template + static Ordering Colamd(const FACTOR_GRAPH& graph) { if (graph.empty()) return Ordering(); else @@ -96,8 +97,8 @@ public: /// constrainLast will be ordered in the same order specified in the KeyVector \c /// constrainLast. If \c forceOrder is false, the variables in \c constrainLast will be /// ordered after all the others, but will be rearranged by CCOLAMD to reduce fill-in as well. - template - static Ordering ColamdConstrainedLast(const FactorGraph& graph, + template + static Ordering ColamdConstrainedLast(const FACTOR_GRAPH& graph, const KeyVector& constrainLast, bool forceOrder = false) { if (graph.empty()) return Ordering(); @@ -123,8 +124,8 @@ public: /// constrainFirst will be ordered in the same order specified in the KeyVector \c /// constrainFirst. If \c forceOrder is false, the variables in \c constrainFirst will be /// ordered before all the others, but will be rearranged by CCOLAMD to reduce fill-in as well. - template - static Ordering ColamdConstrainedFirst(const FactorGraph& graph, + template + static Ordering ColamdConstrainedFirst(const FACTOR_GRAPH& graph, const KeyVector& constrainFirst, bool forceOrder = false) { if (graph.empty()) return Ordering(); @@ -152,8 +153,8 @@ public: /// arbitrary order. Any variables not present in \c groups will be assigned to group 0. This /// function simply fills the \c cmember argument to CCOLAMD with the supplied indices, see the /// CCOLAMD documentation for more information. - template - static Ordering ColamdConstrained(const FactorGraph& graph, + template + static Ordering ColamdConstrained(const FACTOR_GRAPH& graph, const FastMap& groups) { if (graph.empty()) return Ordering(); @@ -172,8 +173,8 @@ public: const VariableIndex& variableIndex, const FastMap& groups); /// Return a natural Ordering. Typically used by iterative solvers - template - static Ordering Natural(const FactorGraph &fg) { + template + static Ordering Natural(const FACTOR_GRAPH &fg) { KeySet src = fg.keys(); KeyVector keys(src.begin(), src.end()); std::stable_sort(keys.begin(), keys.end()); @@ -181,15 +182,15 @@ public: } /// METIS Formatting function - template + template static GTSAM_EXPORT void CSRFormat(std::vector& xadj, - std::vector& adj, const FactorGraph& graph); + std::vector& adj, const FACTOR_GRAPH& graph); /// Compute an ordering determined by METIS from a VariableIndex static GTSAM_EXPORT Ordering Metis(const MetisIndex& met); - template - static Ordering Metis(const FactorGraph& graph) { + template + static Ordering Metis(const FACTOR_GRAPH& graph) { return Metis(MetisIndex(graph)); } @@ -197,9 +198,9 @@ public: /// @name Named Constructors @{ - template + template static Ordering Create(OrderingType orderingType, - const FactorGraph& graph) { + const FACTOR_GRAPH& graph) { if (graph.empty()) return Ordering(); diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 61ce84fe9..cded6d2ee 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -486,7 +486,7 @@ boost::shared_ptr HessianFactor::eliminateCholesky(const Or // Erase the eliminated keys in this factor keys_.erase(begin(), begin() + nFrontals); - } catch (const CholeskyFailed& e) { + } catch (const CholeskyFailed&) { throw IndeterminantLinearSystemException(keys.front()); } diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index 5f29c3bdf..b9579661d 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -148,7 +148,7 @@ bool LevenbergMarquardtOptimizer::tryLambda(const GaussianFactorGraph& linear, // ============ Solve is where most computation happens !! ================= delta = solve(dampedSystem, params_); systemSolvedSuccessfully = true; - } catch (const IndeterminantLinearSystemException& e) { + } catch (const IndeterminantLinearSystemException&) { systemSolvedSuccessfully = false; } diff --git a/gtsam/nonlinear/internal/LevenbergMarquardtState.h b/gtsam/nonlinear/internal/LevenbergMarquardtState.h index bd5465dda..8ab2e7466 100644 --- a/gtsam/nonlinear/internal/LevenbergMarquardtState.h +++ b/gtsam/nonlinear/internal/LevenbergMarquardtState.h @@ -147,7 +147,7 @@ struct LevenbergMarquardtState : public NonlinearOptimizerState { CachedModel* item = getCachedModel(dim); item->A.diagonal() = sqrtHessianDiagonal.at(key); // use diag(hessian) damped += boost::make_shared(key, item->A, item->b, item->model); - } catch (const std::out_of_range& e) { + } catch (const std::out_of_range&) { continue; // Don't attempt any damping if no key found in diagonal } } diff --git a/gtsam/precompiled_header.h b/gtsam/precompiled_header.h index c1b159802..37ab01cf2 100644 --- a/gtsam/precompiled_header.h +++ b/gtsam/precompiled_header.h @@ -18,7 +18,10 @@ #pragma once -// All headers in base: +// All headers in base, except: +// treeTraversal-inst.h: very specific to only a few compilation units +// numericalDerivative.h : includes things in linear, nonlinear :-( +// testLie.h: includes numericalDerivative #include #include #include @@ -38,7 +41,6 @@ #include #include #include -#include #include #include #include @@ -46,10 +48,8 @@ #include #include #include -#include #include #include -#include #include #include #include