Merge branch 'borglab:develop' into patch-4
						commit
						5bf11de6cf
					
				|  | @ -0,0 +1,8 @@ | |||
| BasedOnStyle: Google | ||||
| 
 | ||||
| BinPackArguments: false | ||||
| BinPackParameters: false | ||||
| ColumnLimit: 100 | ||||
| DerivePointerAlignment: false | ||||
| IncludeBlocks: Preserve | ||||
| PointerAlignment: Left | ||||
|  | @ -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: | ||||
|  |  | |||
|  | @ -76,8 +76,7 @@ void save(Archive& ar, const std::optional<T>& t, const unsigned int /*version*/ | |||
| } | ||||
| 
 | ||||
| template <class Archive, class T> | ||||
| void load(Archive& ar, std::optional<T>& t, const unsigned int /*version*/ | ||||
| ) { | ||||
| void load(Archive& ar, std::optional<T>& t, const unsigned int /*version*/) { | ||||
|   bool tflag; | ||||
|   ar >> boost::serialization::make_nvp("initialized", tflag); | ||||
|   if (!tflag) { | ||||
|  |  | |||
|  | @ -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<TimingOutline> 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(); | ||||
|  |  | |||
|  | @ -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<const Key, size_t>& 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
 | ||||
|  |  | |||
|  | @ -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]); | ||||
| } | ||||
|  |  | |||
|  | @ -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))); | ||||
| 
 | ||||
|  |  | |||
|  | @ -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); | ||||
|   } | ||||
|  |  | |||
|  | @ -57,8 +57,16 @@ Ordering HybridSmoother::getOrdering( | |||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| void HybridSmoother::update(HybridGaussianFactorGraph graph, | ||||
|                             const Ordering &ordering, | ||||
|                             std::optional<size_t> maxNrLeaves) { | ||||
|                             std::optional<size_t> maxNrLeaves, | ||||
|                             const std::optional<Ordering> 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); | ||||
|  |  | |||
|  | @ -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<size_t> maxNrLeaves = {}); | ||||
|   void update(HybridGaussianFactorGraph graph, | ||||
|               std::optional<size_t> maxNrLeaves = {}, | ||||
|               const std::optional<Ordering> given_ordering = {}); | ||||
| 
 | ||||
|   Ordering getOrdering(const HybridGaussianFactorGraph& newFactors); | ||||
| 
 | ||||
|  | @ -74,4 +75,4 @@ class HybridSmoother { | |||
|   const HybridBayesNet& hybridBayesNet() const; | ||||
| }; | ||||
| 
 | ||||
| };  // namespace gtsam
 | ||||
| }  // namespace gtsam
 | ||||
|  |  | |||
|  | @ -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<double> 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<double> 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<double>(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<double>(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); | ||||
|   } | ||||
| 
 | ||||
|  |  | |||
|  | @ -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, Vector>& key_value) { | ||||
|     std::pair<iterator, bool> result = values_.insert(key_value); | ||||
|     const std::pair<iterator, bool> 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<Key, Vector> 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; | ||||
|   } | ||||
|  |  | |||
|  | @ -38,7 +38,7 @@ class ConstantVelocityFactor : public NoiseModelFactorN<NavState, NavState> { | |||
|    public: | ||||
|     ConstantVelocityFactor(Key i, Key j, double dt, const SharedNoiseModel &model) | ||||
|         : NoiseModelFactorN<NavState, NavState>(model, i, j), dt_(dt) {} | ||||
|     ~ConstantVelocityFactor() override{}; | ||||
|     ~ConstantVelocityFactor() override {} | ||||
| 
 | ||||
|     /**
 | ||||
|      * @brief Caclulate error: (x2 - x1.update(dt))) | ||||
|  |  | |||
|  | @ -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(); | ||||
|  |  | |||
|  | @ -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 { | ||||
|  |  | |||
|  | @ -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; | ||||
|  |  | |||
|  | @ -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 | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue