diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000000..d54a39d883 --- /dev/null +++ b/.clang-format @@ -0,0 +1,8 @@ +BasedOnStyle: Google + +BinPackArguments: false +BinPackParameters: false +ColumnLimit: 100 +DerivePointerAlignment: false +IncludeBlocks: Preserve +PointerAlignment: Left diff --git a/cmake/GtsamBuildTypes.cmake b/cmake/GtsamBuildTypes.cmake index 3e8cf7192d..b24be5f08c 100644 --- a/cmake/GtsamBuildTypes.cmake +++ b/cmake/GtsamBuildTypes.cmake @@ -150,7 +150,7 @@ if (NOT CMAKE_VERSION VERSION_LESS 3.8) set(CMAKE_CXX_EXTENSIONS OFF) if (MSVC) # NOTE(jlblanco): seems to be required in addition to the cxx_std_17 above? - list_append_cache(GTSAM_COMPILE_OPTIONS_PUBLIC /std:c++latest) + list_append_cache(GTSAM_COMPILE_OPTIONS_PUBLIC /std:c++17) endif() else() # Old cmake versions: diff --git a/gtsam/base/std_optional_serialization.h b/gtsam/base/std_optional_serialization.h index ec6eec56eb..5c250eab43 100644 --- a/gtsam/base/std_optional_serialization.h +++ b/gtsam/base/std_optional_serialization.h @@ -76,8 +76,7 @@ void save(Archive& ar, const std::optional& t, const unsigned int /*version*/ } template -void load(Archive& ar, std::optional& t, const unsigned int /*version*/ -) { +void load(Archive& ar, std::optional& t, const unsigned int /*version*/) { bool tflag; ar >> boost::serialization::make_nvp("initialized", tflag); if (!tflag) { diff --git a/gtsam/base/timing.cpp b/gtsam/base/timing.cpp index 5567ce35d5..154a564db6 100644 --- a/gtsam/base/timing.cpp +++ b/gtsam/base/timing.cpp @@ -272,20 +272,21 @@ void tic(size_t id, const char *labelC) { } /* ************************************************************************* */ -void toc(size_t id, const char *label) { +void toc(size_t id, const char *labelC) { // disable anything which refers to TimingOutline as well, for good measure #ifdef GTSAM_USE_BOOST_FEATURES + const std::string label(labelC); std::shared_ptr current(gCurrentTimer.lock()); if (id != current->id_) { gTimingRoot->print(); throw std::invalid_argument( - "gtsam timing: Mismatched tic/toc: gttoc(\"" + std::string(label) + + "gtsam timing: Mismatched tic/toc: gttoc(\"" + label + "\") called when last tic was \"" + current->label_ + "\"."); } if (!current->parent_.lock()) { gTimingRoot->print(); throw std::invalid_argument( - "gtsam timing: Mismatched tic/toc: extra gttoc(\"" + std::string(label) + + "gtsam timing: Mismatched tic/toc: extra gttoc(\"" + label + "\"), already at the root"); } current->toc(); diff --git a/gtsam/discrete/DecisionTreeFactor.cpp b/gtsam/discrete/DecisionTreeFactor.cpp index cb6c7761eb..5fb5ae2e61 100644 --- a/gtsam/discrete/DecisionTreeFactor.cpp +++ b/gtsam/discrete/DecisionTreeFactor.cpp @@ -94,7 +94,10 @@ namespace gtsam { for (Key j : f.keys()) cs[j] = f.cardinality(j); // Convert map into keys DiscreteKeys keys; - for (const std::pair& key : cs) keys.push_back(key); + keys.reserve(cs.size()); + for (const auto& key : cs) { + keys.emplace_back(key); + } // apply operand ADT result = ADT::apply(f, op); // Make a new factor diff --git a/gtsam/geometry/Line3.cpp b/gtsam/geometry/Line3.cpp index 9e7b2e13ee..f5cf344f51 100644 --- a/gtsam/geometry/Line3.cpp +++ b/gtsam/geometry/Line3.cpp @@ -111,8 +111,8 @@ Line3 transformTo(const Pose3 &wTc, const Line3 &wL, } if (Dline) { Dline->setIdentity(); - (*Dline)(0, 3) = -t[2]; - (*Dline)(1, 2) = t[2]; + (*Dline)(3, 0) = -t[2]; + (*Dline)(2, 1) = t[2]; } return Line3(cRl, c_ab[0], c_ab[1]); } diff --git a/gtsam/geometry/tests/testLine3.cpp b/gtsam/geometry/tests/testLine3.cpp index 09371bad4c..ae2a5e05de 100644 --- a/gtsam/geometry/tests/testLine3.cpp +++ b/gtsam/geometry/tests/testLine3.cpp @@ -123,10 +123,10 @@ TEST(Line3, localCoordinatesOfRetract) { // transform from world to camera test TEST(Line3, transformToExpressionJacobians) { Rot3 r = Rot3::Expmap(Vector3(0, M_PI / 3, 0)); - Vector3 t(0, 0, 0); + Vector3 t(-2.0, 2.0, 3.0); Pose3 p(r, t); - Line3 l_c(r.inverse(), 1, 1); + Line3 l_c(r.inverse(), 3, -1); Line3 l_w(Rot3(), 1, 1); EXPECT(l_c.equals(transformTo(p, l_w))); diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 897d562728..f0d28e9f54 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -248,7 +248,6 @@ hybridElimination(const HybridGaussianFactorGraph &factors, #ifdef HYBRID_TIMING tictoc_print_(); - tictoc_reset_(); #endif // Separate out decision tree into conditionals and remaining factors. @@ -416,9 +415,6 @@ EliminateHybrid(const HybridGaussianFactorGraph &factors, return continuousElimination(factors, frontalKeys); } else { // Case 3: We are now in the hybrid land! -#ifdef HYBRID_TIMING - tictoc_reset_(); -#endif return hybridElimination(factors, frontalKeys, continuousSeparator, discreteSeparatorSet); } diff --git a/gtsam/hybrid/HybridSmoother.cpp b/gtsam/hybrid/HybridSmoother.cpp index de26bad7ef..56c62cf191 100644 --- a/gtsam/hybrid/HybridSmoother.cpp +++ b/gtsam/hybrid/HybridSmoother.cpp @@ -57,8 +57,16 @@ Ordering HybridSmoother::getOrdering( /* ************************************************************************* */ void HybridSmoother::update(HybridGaussianFactorGraph graph, - const Ordering &ordering, - std::optional maxNrLeaves) { + std::optional maxNrLeaves, + const std::optional given_ordering) { + Ordering ordering; + // If no ordering provided, then we compute one + if (!given_ordering.has_value()) { + ordering = this->getOrdering(graph); + } else { + ordering = *given_ordering; + } + // Add the necessary conditionals from the previous timestep(s). std::tie(graph, hybridBayesNet_) = addConditionals(graph, hybridBayesNet_, ordering); diff --git a/gtsam/hybrid/HybridSmoother.h b/gtsam/hybrid/HybridSmoother.h index 0494834cd6..0767da12ff 100644 --- a/gtsam/hybrid/HybridSmoother.h +++ b/gtsam/hybrid/HybridSmoother.h @@ -44,13 +44,14 @@ class HybridSmoother { * corresponding to the pruned choices. * * @param graph The new factors, should be linear only - * @param ordering The ordering for elimination, only continuous vars are - * allowed * @param maxNrLeaves The maximum number of leaves in the new discrete factor, * if applicable + * @param given_ordering The (optional) ordering for elimination, only + * continuous variables are allowed */ - void update(HybridGaussianFactorGraph graph, const Ordering& ordering, - std::optional maxNrLeaves = {}); + void update(HybridGaussianFactorGraph graph, + std::optional maxNrLeaves = {}, + const std::optional given_ordering = {}); Ordering getOrdering(const HybridGaussianFactorGraph& newFactors); @@ -74,4 +75,4 @@ class HybridSmoother { const HybridBayesNet& hybridBayesNet() const; }; -}; // namespace gtsam +} // namespace gtsam diff --git a/gtsam/hybrid/tests/testHybridEstimation.cpp b/gtsam/hybrid/tests/testHybridEstimation.cpp index e74990fe60..b5f5244fa4 100644 --- a/gtsam/hybrid/tests/testHybridEstimation.cpp +++ b/gtsam/hybrid/tests/testHybridEstimation.cpp @@ -46,35 +46,6 @@ using namespace gtsam; using symbol_shorthand::X; using symbol_shorthand::Z; -Ordering getOrdering(HybridGaussianFactorGraph& factors, - const HybridGaussianFactorGraph& newFactors) { - factors.push_back(newFactors); - // Get all the discrete keys from the factors - KeySet allDiscrete = factors.discreteKeySet(); - - // Create KeyVector with continuous keys followed by discrete keys. - KeyVector newKeysDiscreteLast; - const KeySet newFactorKeys = newFactors.keys(); - // Insert continuous keys first. - for (auto& k : newFactorKeys) { - if (!allDiscrete.exists(k)) { - newKeysDiscreteLast.push_back(k); - } - } - - // Insert discrete keys at the end - std::copy(allDiscrete.begin(), allDiscrete.end(), - std::back_inserter(newKeysDiscreteLast)); - - const VariableIndex index(factors); - - // Get an ordering where the new keys are eliminated last - Ordering ordering = Ordering::ColamdConstrainedLast( - index, KeyVector(newKeysDiscreteLast.begin(), newKeysDiscreteLast.end()), - true); - return ordering; -} - TEST(HybridEstimation, Full) { size_t K = 6; std::vector measurements = {0, 1, 2, 2, 2, 3}; @@ -117,7 +88,7 @@ TEST(HybridEstimation, Full) { /****************************************************************************/ // Test approximate inference with an additional pruning step. -TEST(HybridEstimation, Incremental) { +TEST(HybridEstimation, IncrementalSmoother) { size_t K = 15; std::vector measurements = {0, 1, 2, 2, 2, 2, 3, 4, 5, 6, 6, 7, 8, 9, 9, 9, 10, 11, 11, 11, 11}; @@ -136,7 +107,6 @@ TEST(HybridEstimation, Incremental) { initial.insert(X(0), switching.linearizationPoint.at(X(0))); HybridGaussianFactorGraph linearized; - HybridGaussianFactorGraph bayesNet; for (size_t k = 1; k < K; k++) { // Motion Model @@ -146,11 +116,10 @@ TEST(HybridEstimation, Incremental) { initial.insert(X(k), switching.linearizationPoint.at(X(k))); - bayesNet = smoother.hybridBayesNet(); linearized = *graph.linearize(initial); - Ordering ordering = getOrdering(bayesNet, linearized); + Ordering ordering = smoother.getOrdering(linearized); - smoother.update(linearized, ordering, 3); + smoother.update(linearized, 3, ordering); graph.resize(0); } diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp index 4826544714..075e3b9ec7 100644 --- a/gtsam/linear/VectorValues.cpp +++ b/gtsam/linear/VectorValues.cpp @@ -41,7 +41,7 @@ namespace gtsam { /* ************************************************************************ */ VectorValues::VectorValues(const Vector& x, const Dims& dims) { size_t j = 0; - for (const auto& [key,n] : dims) { + for (const auto& [key, n] : dims) { #ifdef TBB_GREATER_EQUAL_2020 values_.emplace(key, x.segment(j, n)); #else @@ -68,7 +68,7 @@ namespace gtsam { VectorValues VectorValues::Zero(const VectorValues& other) { VectorValues result; - for(const auto& [key,value]: other) + for (const auto& [key, value] : other) #ifdef TBB_GREATER_EQUAL_2020 result.values_.emplace(key, Vector::Zero(value.size())); #else @@ -79,7 +79,7 @@ namespace gtsam { /* ************************************************************************ */ VectorValues::iterator VectorValues::insert(const std::pair& key_value) { - std::pair result = values_.insert(key_value); + const std::pair result = values_.insert(key_value); if(!result.second) throw std::invalid_argument( "Requested to insert variable '" + DefaultKeyFormatter(key_value.first) @@ -90,7 +90,7 @@ namespace gtsam { /* ************************************************************************ */ VectorValues& VectorValues::update(const VectorValues& values) { iterator hint = begin(); - for (const auto& [key,value] : values) { + for (const auto& [key, value] : values) { // Use this trick to find the value using a hint, since we are inserting // from another sorted map size_t oldSize = values_.size(); @@ -131,10 +131,10 @@ namespace gtsam { // Change print depending on whether we are using TBB #ifdef GTSAM_USE_TBB std::map sorted; - for (const auto& [key,value] : v) { + for (const auto& [key, value] : v) { sorted.emplace(key, value); } - for (const auto& [key,value] : sorted) + for (const auto& [key, value] : sorted) #else for (const auto& [key,value] : v) #endif @@ -344,14 +344,13 @@ namespace gtsam { } /* ************************************************************************ */ - VectorValues operator*(const double a, const VectorValues &v) - { + VectorValues operator*(const double a, const VectorValues& c) { VectorValues result; - for(const VectorValues::KeyValuePair& key_v: v) + for (const auto& [key, value] : c) #ifdef TBB_GREATER_EQUAL_2020 - result.values_.emplace(key_v.first, a * key_v.second); + result.values_.emplace(key, a * value); #else - result.values_.insert({key_v.first, a * key_v.second}); + result.values_.insert({key, a * value}); #endif return result; } diff --git a/gtsam/navigation/ConstantVelocityFactor.h b/gtsam/navigation/ConstantVelocityFactor.h index f75436ae34..9fe5bef85a 100644 --- a/gtsam/navigation/ConstantVelocityFactor.h +++ b/gtsam/navigation/ConstantVelocityFactor.h @@ -38,7 +38,7 @@ class ConstantVelocityFactor : public NoiseModelFactorN { public: ConstantVelocityFactor(Key i, Key j, double dt, const SharedNoiseModel &model) : NoiseModelFactorN(model, i, j), dt_(dt) {} - ~ConstantVelocityFactor() override{}; + ~ConstantVelocityFactor() override {} /** * @brief Caclulate error: (x2 - x1.update(dt))) diff --git a/gtsam/navigation/ManifoldPreintegration.cpp b/gtsam/navigation/ManifoldPreintegration.cpp index c0c917d9c9..278c44b904 100644 --- a/gtsam/navigation/ManifoldPreintegration.cpp +++ b/gtsam/navigation/ManifoldPreintegration.cpp @@ -67,9 +67,11 @@ void ManifoldPreintegration::update(const Vector3& measuredAcc, // Possibly correct for sensor pose Matrix3 D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega; - if (p().body_P_sensor) - std::tie(acc, omega) = correctMeasurementsBySensorPose(acc, omega, - D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega); + if (p().body_P_sensor) { + std::tie(acc, omega) = correctMeasurementsBySensorPose( + acc, omega, D_correctedAcc_acc, D_correctedAcc_omega, + D_correctedOmega_omega); + } // Save current rotation for updating Jacobians const Rot3 oldRij = deltaXij_.attitude(); diff --git a/gtsam/navigation/ManifoldPreintegration.h b/gtsam/navigation/ManifoldPreintegration.h index a8c97477b2..40691c445b 100644 --- a/gtsam/navigation/ManifoldPreintegration.h +++ b/gtsam/navigation/ManifoldPreintegration.h @@ -27,7 +27,7 @@ namespace gtsam { /** - * IMU pre-integration on NavSatet manifold. + * IMU pre-integration on NavState manifold. * This corresponds to the original RSS paper (with one difference: V is rotated) */ class GTSAM_EXPORT ManifoldPreintegration : public PreintegrationBase { diff --git a/gtsam/navigation/TangentPreintegration.cpp b/gtsam/navigation/TangentPreintegration.cpp index a472b2cfd5..52f730cbbe 100644 --- a/gtsam/navigation/TangentPreintegration.cpp +++ b/gtsam/navigation/TangentPreintegration.cpp @@ -111,9 +111,11 @@ void TangentPreintegration::update(const Vector3& measuredAcc, // Possibly correct for sensor pose by converting to body frame Matrix3 D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega; - if (p().body_P_sensor) - std::tie(acc, omega) = correctMeasurementsBySensorPose(acc, omega, - D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega); + if (p().body_P_sensor) { + std::tie(acc, omega) = correctMeasurementsBySensorPose( + acc, omega, D_correctedAcc_acc, D_correctedAcc_omega, + D_correctedOmega_omega); + } // Do update deltaTij_ += dt; diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 5241659729..2557da2372 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -198,9 +198,9 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON) "${GTSAM_UNSTABLE_MODULE_PATH}") # Hack to get python test files copied every time they are modified - file(GLOB GTSAM_UNSTABLE_PYTHON_TEST_FILES "${CMAKE_CURRENT_SOURCE_DIR}/gtsam_unstable/tests/*.py") + file(GLOB GTSAM_UNSTABLE_PYTHON_TEST_FILES RELATIVE "${CMAKE_CURRENT_SOURCE_DIR}/gtsam_unstable/" "${CMAKE_CURRENT_SOURCE_DIR}/gtsam_unstable/tests/*.py") foreach(test_file ${GTSAM_UNSTABLE_PYTHON_TEST_FILES}) - configure_file(${test_file} "${GTSAM_UNSTABLE_MODULE_PATH}/tests/${test_file}" COPYONLY) + configure_file("${CMAKE_CURRENT_SOURCE_DIR}/gtsam_unstable/${test_file}" "${GTSAM_UNSTABLE_MODULE_PATH}/${test_file}" COPYONLY) endforeach() # Add gtsam_unstable to the install target