From 5f475df299af7a938b7de2ea03c89940d1056a9d Mon Sep 17 00:00:00 2001 From: Mitchell Date: Thu, 23 Oct 2025 16:52:31 -0400 Subject: [PATCH] Modify StateID to handle timesstamp rounding, modify StateCollection to add, remove, and query states by StateID --- examples/CMakeLists.txt | 2 +- examples/gps_imu_example.cpp | 2 +- examples/include/FactorGraphUtils.h | 48 ++--- examples/python/run_gps_imu_fusion.py | 4 +- include/lib/Covariance.h | 18 +- include/lib/FactorGraph.h | 78 ++++---- include/lib/StateCollection.h | 232 +++++++--------------- include/lib/StateId.h | 102 +++++----- src/lib/Covariance.cpp | 54 +++--- src/lib/FactorGraph.cpp | 219 +++++++-------------- src/lib/StateCollection.cpp | 246 +++++++++++------------ tests/CMakeLists.txt | 1 - tests/test_factor_graph.cpp | 69 ++++--- tests/test_state_collection.cpp | 269 +++++++++++++------------- 14 files changed, 588 insertions(+), 756 deletions(-) diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index ecf0383..4cbbc44 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -9,7 +9,7 @@ set(sources include/GPSIMUExampleUtils.h ) -# # Create executable +# Create executable add_executable(gps_imu_example ${sources}) target_include_directories(gps_imu_example PRIVATE include) diff --git a/examples/gps_imu_example.cpp b/examples/gps_imu_example.cpp index 54666be..791833e 100644 --- a/examples/gps_imu_example.cpp +++ b/examples/gps_imu_example.cpp @@ -126,7 +126,7 @@ void runSlidingWindowEstimator( R_gps, state_rep, keys); // If we've reached the window size, optimize the graph and marginalize the // oldest state - if (graph.getStates().getNumStatesForType(keys.nav_state_key) >= + if (graph.getStates().getNumberOfStatesForType(keys.nav_state_key) >= window_size) { graph.solve(options); ceres::Solver::Summary summary = graph.getSolverSummary(); diff --git a/examples/include/FactorGraphUtils.h b/examples/include/FactorGraphUtils.h index 37713d5..630443e 100644 --- a/examples/include/FactorGraphUtils.h +++ b/examples/include/FactorGraphUtils.h @@ -29,8 +29,7 @@ struct ProblemKeys { }; void addIMUState( - FactorGraph &graph, const IMUState &imu_state, - const LieDirection direction, + FactorGraph &graph, const IMUState &imu_state, const LieDirection direction, ExtendedPoseRepresentation state_rep = ExtendedPoseRepresentation::SE23, ProblemKeys keys = ProblemKeys()) { // Create a new ExtendedPoseParameterBlock for the IMU state @@ -40,12 +39,13 @@ void addIMUState( std::shared_ptr> bias_block = std::make_shared>(imu_state.bias()); - graph.addState(keys.nav_state_key, imu_state.timestamp(), nav_state_block); - graph.addState(keys.bias_state_key, imu_state.timestamp(), bias_block); + StateID nav_state_id(keys.nav_state_key, imu_state.timestamp()); + StateID bias_state_id(keys.bias_state_key, imu_state.timestamp()); + graph.addState(nav_state_id, nav_state_block); + graph.addState(bias_state_id, bias_block); }; -void addPriorFactor(FactorGraph &graph, - const IMUState prior_imu_state, +void addPriorFactor(FactorGraph &graph, const IMUState prior_imu_state, const Eigen::Matrix &prior_covariance, LieDirection direction, ExtendedPoseRepresentation state_rep, ProblemKeys keys) { @@ -66,11 +66,10 @@ void addPreintegrationFactor(ceres_nav::FactorGraph &graph, ProblemKeys keys = ProblemKeys()) { double start_stamp = imu_increment.start_stamp; double end_stamp = imu_increment.end_stamp; - std::vector state_ids = { - StateID(keys.nav_state_key, start_stamp), - StateID(keys.bias_state_key, start_stamp), - StateID(keys.nav_state_key, end_stamp), - StateID(keys.bias_state_key, end_stamp)}; + std::vector state_ids = {StateID(keys.nav_state_key, start_stamp), + StateID(keys.bias_state_key, start_stamp), + StateID(keys.nav_state_key, end_stamp), + StateID(keys.bias_state_key, end_stamp)}; auto *factor = new IMUPreintegrationFactor(imu_increment, false); graph.addFactor(state_ids, factor, start_stamp); @@ -97,11 +96,11 @@ void addGPSFactor( IMUState getIMUState(ceres_nav::FactorGraph &graph, double timestamp, ProblemKeys keys = ProblemKeys()) { std::shared_ptr nav_state = - graph.getStates().getState(keys.nav_state_key, - timestamp); + graph.getStates().getState( + StateID(keys.nav_state_key, timestamp)); std::shared_ptr> bias = - graph.getStates().getState>(keys.bias_state_key, - timestamp); + graph.getStates().getState>( + StateID(keys.bias_state_key, timestamp)); if (!nav_state || !bias) { throw std::runtime_error("IMU state not found for timestamp: " + std::to_string(timestamp)); @@ -115,8 +114,9 @@ Eigen::Matrix computeIMUCovariance(ceres_nav::FactorGraph &graph, double timestamp, ProblemKeys keys) { bool success_ext_pose = - graph.computeCovariance(keys.nav_state_key, timestamp); - bool success_bias = graph.computeCovariance(keys.bias_state_key, timestamp); + graph.computeCovariance(StateID(keys.nav_state_key, timestamp)); + bool success_bias = + graph.computeCovariance(StateID(keys.bias_state_key, timestamp)); if (!success_ext_pose || !success_bias) { return Eigen::Matrix::Identity(); @@ -125,13 +125,13 @@ computeIMUCovariance(ceres_nav::FactorGraph &graph, double timestamp, // Assemble covariance and return Eigen::Matrix covariance = Eigen::Matrix::Zero(); - covariance.block<9, 9>(0, 0) = - graph.getStates() - .getState(keys.nav_state_key, timestamp) - ->getCovariance(); + covariance.block<9, 9>(0, 0) = graph.getStates() + .getState( + StateID(keys.nav_state_key, timestamp)) + ->getCovariance(); covariance.block<6, 6>(9, 9) = graph.getStates() - .getState>(keys.bias_state_key, timestamp) + .getState>(StateID(keys.bias_state_key, timestamp)) ->getCovariance(); return covariance; } @@ -143,8 +143,8 @@ void marginalizeIMUState(ceres_nav::FactorGraph &graph, double timestamp_marg, StateID(keys.bias_state_key, timestamp_marg)}; graph.marginalizeStates(state_ids_marg); - // FactorGraph::LastMarginalizationInfo marg_info = graph.getLastMarginalizationInfo(); - // marg_info.print(); + // FactorGraph::LastMarginalizationInfo marg_info = + // graph.getLastMarginalizationInfo(); marg_info.print(); } } // namespace factor_graph_utils \ No newline at end of file diff --git a/examples/python/run_gps_imu_fusion.py b/examples/python/run_gps_imu_fusion.py index 90aa45f..78d1c74 100644 --- a/examples/python/run_gps_imu_fusion.py +++ b/examples/python/run_gps_imu_fusion.py @@ -444,8 +444,8 @@ def evaluate_imu_states( ) executable_path = os.path.join(cur_dir, "../../build/examples/gps_imu_example") - config.lie_direction = "right" - config.state_representation = "decoupled" + config.lie_direction = "left" # left or right + config.state_representation = "SE23" # SE23 or decoupled # Generate data an run the example data_fpaths = generate_and_save_data(config, save_dir) diff --git a/include/lib/Covariance.h b/include/lib/Covariance.h index 5cd9a84..ea6fe4e 100644 --- a/include/lib/Covariance.h +++ b/include/lib/Covariance.h @@ -2,13 +2,25 @@ #include -// Forward declarations +// Forward declarations class StateCollection; namespace ceres { - class Problem; +class Problem; } namespace ceres_nav { +class StateID; +} + +namespace ceres_nav { + +/** + * @brief Computes the covariance for a given state in the StateCollection + * using the provided Ceres Problem. + * + * It is assumed that the state exists in both the StateCollection and the Ceres + * problem. + */ bool calculateCovariance(ceres::Problem &graph, StateCollection &states, - const std::string &key, double timestamp); + const StateID &state_id); }; // namespace ceres_nav \ No newline at end of file diff --git a/include/lib/FactorGraph.h b/include/lib/FactorGraph.h index 3a0368d..895da01 100644 --- a/include/lib/FactorGraph.h +++ b/include/lib/FactorGraph.h @@ -16,6 +16,12 @@ namespace ceres_nav { +struct FactorInfo { + ceres::ResidualBlockId residual_block_id; + std::vector connected_states; + double timestamp; +}; + class FactorGraph { public: using StatePtr = std::shared_ptr; @@ -31,14 +37,11 @@ class FactorGraph { FactorGraph(ceres::Solver::Options solver_options); /** - * @brief Adds a state to the problem with a particular name and - * timestamp - */ - void addState(const std::string &name, double timestamp, - std::shared_ptr state); - - /** - * @brief Adds a state to the problem with a particular StateID + * @brief Adds a state to the problem with a particular StateID. + * + * @param state_id The StateID for the state to add + * @param state A shared pointer to the ParameterBlockBase object containing + * the estimate of the state. */ void addState(const StateID &state_id, std::shared_ptr state); @@ -56,7 +59,22 @@ class FactorGraph { ceres::LossFunction *loss_function = nullptr); /** - * @brief Solves the optimization problem using the current solver options. + * @brief Adds a factor to the problem, and additionally returns + * information about the added factor via the FactorInfo struct. + */ + bool addFactor(const std::vector &state_ids, + ceres::CostFunction *cost_function, double stamp, + FactorInfo &info, + ceres::LossFunction *loss_function = nullptr); + + /** + * @brief Removes a factor from the problem given its FactorInfo. + */ + bool removeFactor(const FactorInfo &info); + + /** + * @brief Solves the optimization problem using the current solver + * options. */ void solve(); @@ -65,7 +83,9 @@ class FactorGraph { */ void solve(ceres::Solver::Options Options); - /** Get information about the internal Ceres problem. */ + /** + * @brief Gets the + */ bool getStatePointers(const std::vector &StateIDs, std::vector &state_ptrs) const; /** @@ -86,30 +106,10 @@ class FactorGraph { std::vector &factors_m, std::vector &factors_r) const; - /** - * @brief Removes a timestamped state from the problem. - */ - void removeState(const std::string &name, double timestamp); - - /** - * @brief Removes a state from the problem given a StateID. - */ - void removeState(const StateID &state_id); - - /** - * @brief Sets a state as constant in the optimization problem. - */ - void setConstant(const std::string &name, double timestamp); - - /** - * @brief Checks if a state is constant in the optimization problem. - */ - bool isConstant(const std::string &name, double timestamp); - - /** - * @brief Sets a state as variable in the optimization problem. - */ - void setVariable(const std::string &name, double timestamp); + // Control whether a state is constant or variable + void setConstant(const StateID &state_id); + void setVariable(const StateID &state_id); + bool isConstant(const StateID &state_id) const; /** * @brief Marginalizes out a set of states from the problem @@ -126,11 +126,19 @@ class FactorGraph { std::vector states_m, const std::map &linearization_points); + /** + * @brief Directly removes a state from the problem given a StateID. + * + * WARNING: this does not properly marginalize out the state, and should be + * used with caution! + */ + void removeState(const StateID &state_id); + /** * @brief Computes the covariance of a state with a given name at * a particular timestamp. */ - bool computeCovariance(const std::string &name, double timestamp); + bool computeCovariance(const StateID &state_id); /** * @brief Gets the marginalization information for a set of states. diff --git a/include/lib/StateCollection.h b/include/lib/StateCollection.h index 3dda7d3..54d3b5d 100644 --- a/include/lib/StateCollection.h +++ b/include/lib/StateCollection.h @@ -5,7 +5,6 @@ #include #include "ParameterBlockBase.h" -#include namespace ceres_nav { struct StateID; @@ -14,124 +13,91 @@ struct StateID; namespace ceres_nav { /** - * @brief Holds a collection of states in time, accessible by a string key and a - * timestamp. + * @brief Holds a collection of states, allowing users to add, remove, and + * query states by a StateID. */ class StateCollection { public: StateCollection(){}; /** - * @brief Adds a state to the collection with a given name and timestamp. + * @brief Adds a parameter block using a StateID. */ - void addState(const std::string &name, double timestamp, - std::shared_ptr state); + bool addState(const StateID &state_id, + std::shared_ptr param_block); /** - * @brief Adds a non-timestamped (static) state to the collection + * @brief Removes a state from the collection using a StateID. */ - void addStaticState(const std::string &name, - std::shared_ptr state); + bool removeState(const StateID &state_id); /** - * @brief Retrieves a state for a given key and timestamp. - * - * @param key The key of the state to retrieve. - * @param timestamp The timestamp of the state to retrieve. - * @return A shared pointer to the state, or nullptr if not found. + * @brief Query single parameter block using a StateID. + * Returns nullptr if not found. */ - std::shared_ptr getState(const std::string &key, - double timestamp) const; + std::shared_ptr getState(const StateID &state_id) const; /** - * @brief Retrieves a static (non-timestamped) state for a given key. + * @brief Templated version of getState that returns a state of specific type. */ - std::shared_ptr - getStaticState(const std::string &key) const; + + template + std::shared_ptr getState(const StateID &state_id) const { + auto state = getState(state_id); + if (state) { + return std::dynamic_pointer_cast(state); + } + return nullptr; + } /** - * @brief Gets a state for a given StateID (which may be static or timestamped). - * Returns nullptr if not found. + * @brief Check if a state exists in the collection using a StateID. */ - std::shared_ptr getState(const StateID &state_id) const; + bool hasState(const StateID &state_id) const; + + /// Get some information about the timestamps for a given state type + bool getOldestStamp(const std::string &key, double ×tamp) const; + bool getLatestStamp(const std::string &key, double ×tamp) const; + bool getTimesForState(const std::string &key, + std::vector ×tamps) const; + + // Gets the oldest and latest states for a given key + std::shared_ptr + getOldestState(const std::string &key) const; + std::shared_ptr + getLatestState(const std::string &key) const; /** - * @brief Templated version of get state that returns a state of a specific - * type. - * - * @param key The key of the state to retrieve. - * @param timestamp The timestamp of the state to retrieve. - * @return A shared pointer to the state of type T, or nullptr if not found or - * if the downcast fails. + * @brief Templated version of getOldestState that returns a state of + * specific type. */ template - std::shared_ptr getState(const std::string &key, double timestamp) const { - int64_t timestamp_key = timestampToKey(timestamp); - - auto it1 = states_.find(key); - // If we've found the key - if (it1 != states_.end()) { - auto state_it = it1->second.find(timestamp_key); - if (state_it != it1->second.end()) { - // Attempt to downcast to the specific type T - auto casted_ptr = std::dynamic_pointer_cast(state_it->second); - if (casted_ptr) { - return casted_ptr; - } else { - // If downcast fails, return nullptr - LOG(ERROR) << "Failed to downcast state for key: " << key - << "at timestamp: " << timestamp; - return nullptr; - } - } + std::shared_ptr getOldestState(const std::string &key) const { + auto state = getOldestState(key); + if (state) { + return std::dynamic_pointer_cast(state); } - // Return nullptr if not found or downcast fails return nullptr; } /** - * @brief Templated version of getStaticState that returns a static state of - * a specific type. - * - * @param key The key of the state to retrieve. - * @return A shared pointer to the static state of type T, or nullptr if not - * found or if the downcast fails. + * @brief Templated version of getLatestState that returns a state of + * specific type. */ template - std::shared_ptr getStaticState(const std::string &key) const { - auto it = static_states_.find(key); - - if (it != static_states_.end()) { - auto casted_ptr = std::dynamic_pointer_cast(it->second); - if (casted_ptr) { - return casted_ptr; - } else { - LOG(ERROR) << "Failed to downcast static state for key: " << key; - return nullptr; - } + std::shared_ptr getLatestState(const std::string &key) const { + auto state = getLatestState(key); + if (state) { + return std::dynamic_pointer_cast(state); } - - LOG(ERROR) << "Static state not found for key: " << key; return nullptr; } - /** - * @brief Removes a state from the collection for a given key and timestamp. - * - * @param key The key of the state to remove. - * @param timestamp The timestamp of the state to remove. - */ - void removeState(const std::string &key, double timestamp); - - /** - * @brief Removes a static (non-timestamped) state from the collection. - */ - void removeStaticState(const std::string &key); - /** * @brief Retrieves a state by its estimate pointer. * - * This is useful for finding a state when you have a pointer to its estimate + * This is useful for finding a state when you have a pointer to its + estimate * (i.e., from Ceres.) */ std::shared_ptr @@ -146,100 +112,44 @@ class StateCollection { */ bool getStateIDByEstimatePointer(double *ptr, StateID &state_id) const; - // Check if a state exists at a given timestamp - bool hasState(const std::string &key, double timestamp) const; - bool hasStaticState(const std::string &key) const; - bool hasStateType(const std::string &key) const; - - /** - * @brief Get the number of different state types stored in the collection. - */ - size_t getNumStateTypes() const { return states_.size() + static_states_.size(); } - /** - * @brief Get the number of states for a given type. - * - * @param key The key of the state type to check. - * @return The number of states for the given type. + * @brief Clears all states from the collection. */ - size_t getNumStatesForType(const std::string &key) const { - auto it = states_.find(key); - if (it != states_.end()) { - return it->second.size(); - } - return 0; + void clear() { + time_varying_states_.clear(); + static_states_.clear(); } /** - * @brief Get the first timestamp for a given key. - */ - bool getOldestStamp(const std::string &key, double &stamp) const; - - /** - * @brief Gets the last timestamp for a given key. - */ - bool getLatestStamp(const std::string &key, double &stamp) const; - - /** - * @brief Gets all timestamps for a given key. + * @brief Get the total number of states stored in the collection. */ - bool getTimesForState(const std::string &key, - std::vector &stamps) const; - - // Get the oldest and latest states for a given key - // Returns a pointer to the base class. - std::shared_ptr - getOldestState(const std::string &key) const; - std::shared_ptr - getLatestState(const std::string &key) const; - - /** - * @brief Gets the oldest state of a specific type for a given key. - */ - template - std::shared_ptr getOldestState(const std::string &key) const { - auto state = getOldestState(key); - if (state) { - return std::dynamic_pointer_cast(state); + size_t size() const { + size_t total_size = static_states_.size(); + for (const auto &pair : time_varying_states_) { + total_size += pair.second.size(); } - return nullptr; + return total_size; } - /** - * @brief Templated version of getLatestState that returns a state of - * specific type. - */ - template - std::shared_ptr getLatestState(const std::string &key) const { - auto state = getLatestState(key); - if (state) { - return std::dynamic_pointer_cast(state); + size_t getNumberOfStatesForType(const std::string &key) const { + auto it = time_varying_states_.find(key); + if (it != time_varying_states_.end()) { + return it->second.size(); } - return nullptr; - } - -protected: - static constexpr double default_timestamp_precision = 1e-9; - double timestamp_precision_ = default_timestamp_precision; - - int64_t timestampToKey(double timestamp) const { - return static_cast(std::round(timestamp / timestamp_precision_)); + return 0; } - double keyToTimestamp(int64_t key) const { - return static_cast(key) * timestamp_precision_; - } + size_t staticSize() const { return static_states_.size(); } - // The states are stored in a map where the key corresponds to the name of the - // state, and the value is a map of timestamps to state pointers. This allows - // for retrieval of states +protected: + // Time-varying states stored as a map from string key to a map of timestamp + // to ParameterBlockBase pointers. std::unordered_map>> - states_; + std::map>> + time_varying_states_; - // Static states that do not change over time, accessible by a string key - // This is useful for parameters like landmarks in SLAM, or calibration - // parameters. + // Time-invariant states are stored in a separate map from + // string key to ParameterBlockBase pointers. std::unordered_map> static_states_; }; diff --git a/include/lib/StateId.h b/include/lib/StateId.h index a7d96bc..9aecefa 100644 --- a/include/lib/StateId.h +++ b/include/lib/StateId.h @@ -4,77 +4,75 @@ #include namespace ceres_nav { - -// struct StateID { -// StateID() { -// ID = ""; -// timestamp = 0.0; -// } -// StateID(const std::string &state_id, const double timestamp_) -// : ID(state_id), timestamp(timestamp_) {} +/** + * @brief Unique identifier for a state. + * + * Stores a string key, and an optional timestamp for time-varying states. + * The timestamp is automatically rounded to a specified precision to avoid + * floating point issues for retrieving states with a given timestamp. + */ +class StateID { +public: + // Round to nanosecond precision by default + inline static double DEFAULT_PRECISION = 1e-9; -// /** is required to compare keys */ -// bool operator==(const StateID &other) const { -// return ID == other.ID && timestamp == other.timestamp; -// } + StateID() = default; -// std::string ID; -// double timestamp; -// }; + // Construct for time-varying states (poses, velocities, biases, etc.) + StateID(const std::string &key, double timestamp, + double precision = DEFAULT_PRECISION) + : key_(key), timestamp_(roundToPrecision(timestamp, precision)) {} -struct StateID { - StateID() { ID = ""; } + // Constructor for time-invariant states (landmarks, calibration, + // etc.) + StateID(const std::string &key) : key_(key) {} - // Constructor for static states with no timestamp - StateID(const std::string &state_id) : ID(state_id) {} - - // Constructor for timestamped states - StateID(const std::string &state_id, double timestamp_) - : ID(state_id), timestamp(timestamp_) {} - - // Compare two StateID objects - bool operator==(const StateID &other) const { - return ID == other.ID && timestamp == other.timestamp; + static void setDefaultPrecision(double precision) { + DEFAULT_PRECISION = precision; } - // Comparison operator for map/set - bool operator<(const StateID &other) const { - if (ID != other.ID) { - return ID < other.ID; - } + // Accessors + const std::string key() const { return key_; } + std::optional timestamp() const { return timestamp_; } + bool hasTimestamp() const { return timestamp_.has_value(); } - // If IDs are equal, compare timestamps - // States without timestamps come before states with timestamps - if (!timestamp.has_value() && other.timestamp.has_value()) { - return true; + // Comparison operators for use in maps and sets + bool operator<(const StateID &other) const { + if (key_ != other.key_) { + return key_ < other.key_; } - if (timestamp.has_value() && !other.timestamp.has_value()) { - return false; + if (timestamp_.has_value() != other.timestamp_.has_value()) { + return !timestamp_.has_value(); } - // If both have timestamps, compare the values - if (timestamp.has_value() && other.timestamp.has_value()) { - return timestamp.value() < other.timestamp.value(); + if (timestamp_.has_value()) { + return timestamp_.value() < other.timestamp_.value(); } - // Both are static (no timestamp) and have same ID return false; } - bool isStatic() const { return !timestamp.has_value(); } + bool operator==(const StateID &other) const { + return key_ == other.key_ && timestamp_ == other.timestamp_; + } + + bool operator!=(const StateID &other) const { return !(*this == other); } - /** - * @brief converts a StateID to a string for printing - */ + // String representation for debuggin std::string toString() const { - if (timestamp.has_value()) { - return ID + " at time " + std::to_string(timestamp.value()); + if (timestamp_.has_value()) { + return key_ + " at time " + std::to_string(timestamp_.value()); } else { - return ID; + return key_; } } - std::string ID; - std::optional timestamp; -}; +private: + // Round timestamp to a specified precision + static double roundToPrecision(double value, double precision) { + return std::round(value / precision) * precision; + } + std::string key_; + std::optional timestamp_; +}; } // namespace ceres_nav \ No newline at end of file diff --git a/src/lib/Covariance.cpp b/src/lib/Covariance.cpp index ba2fcef..9aa0762 100644 --- a/src/lib/Covariance.cpp +++ b/src/lib/Covariance.cpp @@ -1,6 +1,7 @@ #include "lib/Covariance.h" - #include "lib/StateCollection.h" +#include "lib/StateId.h" + #include #include #include @@ -8,11 +9,11 @@ namespace ceres_nav { bool calculateCovariance(ceres::Problem &graph, StateCollection &states, - const std::string &key, double timestamp) { + const StateID &state_id) { // Check if the state exists in the collection - if (!states.hasState(key, timestamp)) { - LOG(ERROR) << "State with key: " << key << " and timestamp: " << timestamp + if (!states.hasState(state_id)) { + LOG(ERROR) << "State with ID: " << state_id.toString() << " does not exist in the state collection."; return false; } @@ -26,57 +27,56 @@ bool calculateCovariance(ceres::Problem &graph, StateCollection &states, ceres::Covariance covariance(cov_options); std::vector parameter_block_ptrs; - parameter_block_ptrs.push_back( - states.getState(key, timestamp)->estimatePointer()); + parameter_block_ptrs.push_back(states.getState(state_id)->estimatePointer()); // Try with sparse QR first if (covariance.Compute(parameter_block_ptrs, &graph)) { - if (states.getState(key, timestamp)->getLocalParameterizationPointer() == + if (states.getState(state_id)->getLocalParameterizationPointer() == nullptr) { covariance.GetCovarianceBlock( - states.getState(key, timestamp)->estimatePointer(), - states.getState(key, timestamp)->estimatePointer(), - states.getState(key, timestamp)->getCovariancePointer()); + states.getState(state_id)->estimatePointer(), + states.getState(state_id)->estimatePointer(), + states.getState(state_id)->getCovariancePointer()); } else { covariance.GetCovarianceBlockInTangentSpace( - states.getState(key, timestamp)->estimatePointer(), - states.getState(key, timestamp)->estimatePointer(), - states.getState(key, timestamp)->getCovariancePointer()); + states.getState(state_id)->estimatePointer(), + states.getState(state_id)->estimatePointer(), + states.getState(state_id)->getCovariancePointer()); } return true; } else { - LOG(ERROR) << "Sparse QR covariance computation failed for state: " << key - << " at timestamp: " << timestamp; + LOG(ERROR) << "Sparse QR covariance computation failed for state: " + << state_id.toString(); if (!graph.NumParameterBlocks() > 100) { - LOG(ERROR) << "Covariance computation of " << key + LOG(ERROR) << "Covariance computation of " << state_id.toString() << " failed. No covariance computed!"; return false; } - LOG(ERROR) << "Jacobian related to state " << key << " at timestamp " - << timestamp << " is not full rank. Computing with SVD..."; + LOG(ERROR) << "Jacobian related to state " << state_id.toString() + << " is not full rank. Computing with SVD..."; cov_options.algorithm_type = ceres::CovarianceAlgorithmType::DENSE_SVD; cov_options.null_space_rank = -1; ceres::Covariance covariance_svd(cov_options); // Try to compute again if (covariance_svd.Compute(parameter_block_ptrs, &graph)) { - if (states.getState(key, timestamp)->getLocalParameterizationPointer() == + if (states.getState(state_id)->getLocalParameterizationPointer() == nullptr) { covariance_svd.GetCovarianceBlock( - states.getState(key, timestamp)->estimatePointer(), - states.getState(key, timestamp)->estimatePointer(), - states.getState(key, timestamp)->getCovariancePointer()); + states.getState(state_id)->estimatePointer(), + states.getState(state_id)->estimatePointer(), + states.getState(state_id)->getCovariancePointer()); } else { covariance_svd.GetCovarianceBlockInTangentSpace( - states.getState(key, timestamp)->estimatePointer(), - states.getState(key, timestamp)->estimatePointer(), - states.getState(key, timestamp)->getCovariancePointer()); + states.getState(state_id)->estimatePointer(), + states.getState(state_id)->estimatePointer(), + states.getState(state_id)->getCovariancePointer()); } return true; } else { - LOG(ERROR) << "Failed to compute covariance for state: " << key - << " at timestamp: " << timestamp; + LOG(ERROR) << "Failed to compute covariance for state: " + << state_id.toString(); return false; } } diff --git a/src/lib/FactorGraph.cpp b/src/lib/FactorGraph.cpp index a8e8184..238bf6f 100644 --- a/src/lib/FactorGraph.cpp +++ b/src/lib/FactorGraph.cpp @@ -1,4 +1,6 @@ #include "lib/FactorGraph.h" + +#include "lib/ParameterBlockBase.h" #include "lib/Covariance.h" #include "lib/Marginalization.h" @@ -18,76 +20,32 @@ FactorGraph::FactorGraph() : problem_(default_problem_options_) {} FactorGraph::FactorGraph(ceres::Solver::Options solver_options) : problem_(default_problem_options_), solver_options_(solver_options) {} -void FactorGraph::addState(const std::string &name, double timestamp, - std::shared_ptr state) { - states_.addState(name, timestamp, state); - - // Get the mean pointer - double *estimate_ptr = state->estimatePointer(); - int size = state->dimension(); - - ceres::LocalParameterization *local_parameterization_ptr = - state->getLocalParameterizationPointer(); - - if (local_parameterization_ptr != nullptr) { - problem_.AddParameterBlock(estimate_ptr, state->dimension(), - local_parameterization_ptr); - } else { - problem_.AddParameterBlock(estimate_ptr, size); - } -} - void FactorGraph::addState(const StateID &state_id, std::shared_ptr state) { - if (state_id.isStatic()) { - states_.addStaticState(state_id.ID, state); - } else { - states_.addState(state_id.ID, state_id.timestamp.value(), state); - } + states_.addState(state_id, state); - // Common parameter block addition logic + // Add state to Ceres problem double *estimate_ptr = state->estimatePointer(); int size = state->dimension(); - ceres::LocalParameterization *local_parameterization_ptr = state->getLocalParameterizationPointer(); - - if (local_parameterization_ptr != nullptr) { - problem_.AddParameterBlock(estimate_ptr, state->dimension(), - local_parameterization_ptr); - } else { - problem_.AddParameterBlock(estimate_ptr, size); - } + problem_.AddParameterBlock(estimate_ptr, state->dimension(), + local_parameterization_ptr); } // Add a factor to the problem bool FactorGraph::addFactor(const std::vector &state_ids, ceres::CostFunction *cost_function, double stamp, + FactorInfo &info, ceres::LossFunction *loss_function) { // Build vector of state pointers std::vector state_ptrs; for (auto &state_id : state_ids) { - std::shared_ptr state; - // For static states, we don't have a timestamp - if (state_id.isStatic()) { - state = states_.getStaticState(state_id.ID); - if (!state) { - LOG(ERROR) - << "Trying to add a factor with static state that does not exist: " - << state_id.ID; - return false; - } - } else { - // For timestamped states - state = states_.getState(state_id.ID, state_id.timestamp.value()); - if (!state) { - LOG(ERROR) << "Trying to add a factor with state that does not exist: " - << state_id.ID - << " at timestamp: " << state_id.timestamp.value(); - return false; - } + std::shared_ptr state = states_.getState(state_id); + if (!state) { + LOG(ERROR) << "State not found in collection: " << state_id.toString(); + return false; } - state_ptrs.push_back(state->estimatePointer()); } @@ -97,6 +55,33 @@ bool FactorGraph::addFactor(const std::vector &state_ids, // Add to map residual_blocks_to_cost_function_map.insert({residual_id, cost_function}); + + info.residual_block_id = residual_id; + info.connected_states = state_ids; + info.timestamp = stamp; + + return true; +} + +bool FactorGraph::addFactor(const std::vector &state_ids, + ceres::CostFunction *cost_function, double stamp, + ceres::LossFunction *loss_function) { + FactorInfo factor_info; + return addFactor(state_ids, cost_function, stamp, factor_info, loss_function); +} + +bool FactorGraph::removeFactor(const FactorInfo &info) { + // Remove from the Ceres problem + problem_.RemoveResidualBlock(info.residual_block_id); + // Remove from the map + auto it = residual_blocks_to_cost_function_map.find(info.residual_block_id); + if (it != residual_blocks_to_cost_function_map.end()) { + residual_blocks_to_cost_function_map.erase(it); + } else { + LOG(WARNING) << "Trying to remove a factor that does not exist in the map"; + return false; + } + return true; } @@ -125,31 +110,18 @@ bool FactorGraph::getStatePointers(const std::vector &state_ids, std::vector &state_ptrs) const { // Get the pointers to the states for (auto &state_id : state_ids) { - std::shared_ptr state; - - if (state_id.isStatic()) { - state = states_.getStaticState(state_id.ID); - if (!state) { - LOG(ERROR) << "State not found in collection: " << state_id.ID; - return false; - } - } else { - state = states_.getState(state_id.ID, state_id.timestamp.value()); - if (!state) { - LOG(ERROR) << "State not found in collection: " << state_id.ID - << " at timestamp: " << state_id.timestamp.value(); - return false; - } + std::shared_ptr state = states_.getState(state_id); + if (!state) { + LOG(ERROR) << "State not found in collection: " << state_id.toString(); + return false; } - - double *state_ptr = state->estimatePointer(); - - if (!problem_.HasParameterBlock(state_ptr)) { - LOG(ERROR) << "State not found in Ceres problem: " << state_id.ID - << " at timestamp: " << state_id.timestamp.value(); + // Ensure that the state exists in the Ceres problem + if (!problem_.HasParameterBlock(state->estimatePointer())) { + LOG(ERROR) << "State not found in Ceres problem: " << state_id.toString(); return false; } - state_ptrs.push_back(state_ptr); + + state_ptrs.push_back(state->estimatePointer()); } return true; @@ -208,50 +180,10 @@ bool FactorGraph::getConnectedStatePointers( return true; } -void FactorGraph::removeState(const std::string &name, double timestamp) { - - if (!states_.hasState(name, timestamp)) { - LOG(ERROR) << "State not found in collection: " << name - << " at timestamp: " << timestamp; - return; - } - - // Remove relevant residuals from the cost function map - double *state_ptr = states_.getState(name, timestamp)->estimatePointer(); - std::vector residual_ids; - problem_.GetResidualBlocksForParameterBlock(state_ptr, &residual_ids); - - for (auto const &residual : residual_ids) { - if (residual_blocks_to_cost_function_map.find(residual) != - residual_blocks_to_cost_function_map.end()) { - residual_blocks_to_cost_function_map.erase(residual); - } else { - LOG(WARNING) << "Residual block not found in map: " << residual; - } - } - - // Remove from the Ceres problem - problem_.RemoveParameterBlock( - states_.getState(name, timestamp)->estimatePointer()); - - // Remove from the StateCollection - states_.removeState(name, timestamp); -} - void FactorGraph::removeState(const StateID &state_id) { - std::shared_ptr state; - - if (state_id.isStatic()) { - state = states_.getStaticState(state_id.ID); - } else { - state = states_.getState(state_id.ID, state_id.timestamp.value()); - } - + std::shared_ptr state = states_.getState(state_id); if (!state) { - LOG(ERROR) << "State not found in collection: " << state_id.ID; - if (!state_id.isStatic()) { - LOG(ERROR) << " at timestamp: " << state_id.timestamp.value(); - } + LOG(ERROR) << "State not found in collection: " << state_id.toString(); return; } @@ -271,48 +203,35 @@ void FactorGraph::removeState(const StateID &state_id) { // Remove from the Ceres problem problem_.RemoveParameterBlock(state->estimatePointer()); - // Remove from the StateCollection - if (state_id.isStatic()) { - states_.removeStaticState(state_id.ID); - } else { - states_.removeState(state_id.ID, state_id.timestamp.value()); - } + states_.removeState(state_id); } -void FactorGraph::setConstant(const std::string &name, double timestamp) { - if (states_.hasState(name, timestamp)) { - problem_.SetParameterBlockConstant( - states_.getState(name, timestamp)->estimatePointer()); - } else { - LOG(ERROR) << "State not found in collection: " << name - << " at timestamp: " << timestamp; +void FactorGraph::setConstant(const StateID &state_id) { + std::shared_ptr state = states_.getState(state_id); + if (!state) { + LOG(ERROR) << "State not found in collection: " << state_id.toString(); + return; } + + problem_.SetParameterBlockConstant(state->estimatePointer()); } -bool FactorGraph::isConstant(const std::string &name, double timestamp) { - if (states_.hasState(name, timestamp)) { - return problem_.IsParameterBlockConstant( - states_.getState(name, timestamp)->estimatePointer()); - } else { - LOG(ERROR) << "State not found in collection: " << name - << " at timestamp: " << timestamp; - return false; +void FactorGraph::setVariable(const StateID &state_id) { + std::shared_ptr state = states_.getState(state_id); + if (!state) { + LOG(ERROR) << "State not found in collection: " << state_id.toString(); } + + problem_.SetParameterBlockVariable(state->estimatePointer()); } -void FactorGraph::setVariable(const std::string &name, double timestamp) { - if (states_.hasState(name, timestamp)) { - problem_.SetParameterBlockVariable( - states_.getState(name, timestamp)->estimatePointer()); - } else { - LOG(ERROR) << "State not found in collection: " << name - << " at timestamp: " << timestamp; - } +bool FactorGraph::isConstant(const StateID &state_id) const { + std::shared_ptr state = states_.getState(state_id); + return problem_.IsParameterBlockConstant(state->estimatePointer()); } -bool FactorGraph::computeCovariance(const std::string &key, double timestamp) { - const bool success = calculateCovariance(problem_, states_, key, timestamp); - return success; +bool FactorGraph::computeCovariance(const StateID &state_id) { + return calculateCovariance(problem_, states_, state_id); } bool FactorGraph::marginalizeStates(std::vector states_m) { @@ -377,7 +296,7 @@ bool FactorGraph::marginalizeStates( if (lin_point.size() != state_info.param_ptr->dimension()) { LOG(ERROR) << "Linearization point size does not match state " "dimension for state: " - << state_id.ID; + << state_id.toString(); return false; } // Temporarily set the state to the linearization point! diff --git a/src/lib/StateCollection.cpp b/src/lib/StateCollection.cpp index c33efef..fe23d08 100644 --- a/src/lib/StateCollection.cpp +++ b/src/lib/StateCollection.cpp @@ -2,181 +2,175 @@ #include "lib/StateId.h" #include "utils/Utils.h" -namespace ceres_nav { +#include -void StateCollection::addState(const std::string &name, double timestamp, +namespace ceres_nav { +bool StateCollection::addState(const StateID &id, std::shared_ptr state) { - int64_t timestamp_key = timestampToKey(timestamp); - - auto it = states_.find(name); - // If we haven't found this state, create a new entry - if (it == states_.end()) { - states_.emplace(name, - std::map>()); - } - - // Check if a state already exists for this name and timestamp - if (states_.at(name).find(timestamp_key) != states_.at(name).end()) { - LOG(ERROR) << "State with name: " << name << " and timestamp: " << timestamp - << " already exists."; - } - - // Add the state to the map for this name - states_.at(name).emplace(timestamp_key, state); -} + // Add the time-varying state to the map + if (id.hasTimestamp()) { + + auto it = time_varying_states_.find(id.key()); + // If we haven't found this key yet, create a new amp for it + if (it == time_varying_states_.end()) { + time_varying_states_.emplace( + id.key(), std::map>()); + } -void StateCollection::addStaticState( - const std::string &name, std::shared_ptr state) { - auto it = static_states_.find(name); - if (it != static_states_.end()) { - LOG(ERROR) << "Static state with name: " << name << " already exists."; - return; - } - static_states_.emplace(name, state); -} + if (time_varying_states_.at(id.key()).find(id.timestamp().value()) != + time_varying_states_.at(id.key()).end()) { + LOG(ERROR) << "State already exists for key: " << id.key() + << " at timestamp: " << id.timestamp().value(); + return false; + } -std::shared_ptr -StateCollection::getState(const std::string &key, double timestamp) const { - int64_t timestamp_key = timestampToKey(timestamp); - auto it1 = states_.find(key); - if (it1 != states_.end()) { - auto it2 = it1->second.find(timestamp_key); - if (it2 != it1->second.end()) { - return it2->second; + time_varying_states_.at(id.key()).emplace(id.timestamp().value(), state); + } else { + // Static state + if (static_states_.find(id.key()) != static_states_.end()) { + LOG(ERROR) << "State already exists for key: " << id.key(); + return false; } + static_states_.emplace(id.key(), state); } - return nullptr; -} -std::shared_ptr -StateCollection::getStaticState(const std::string &key) const { - auto it = static_states_.find(key); - if (it != static_states_.end()) { - return it->second; - } - return nullptr; + return true; } -std::shared_ptr -StateCollection::getState(const StateID &state_id) const { - if (state_id.isStatic()) { - return getStaticState(state_id.ID); +bool StateCollection::removeState(const StateID &id) { + if (id.hasTimestamp()) { + auto it = time_varying_states_.find(id.key()); + if (it != time_varying_states_.end()) { + return it->second.erase(id.timestamp().value()) > 0; + } + return false; } else { - return getState(state_id.ID, state_id.timestamp.value()); + return static_states_.erase(id.key()) > 0; } } -bool StateCollection::hasState(const std::string &key, double timestamp) const { - int64_t timestamp_key = timestampToKey(timestamp); - auto it1 = states_.find(key); - if (it1 != states_.end()) { - return it1->second.find(timestamp_key) != it1->second.end(); - } - return false; -} - -bool StateCollection::hasStaticState(const std::string &key) const { - return static_states_.find(key) != static_states_.end(); -} - -bool StateCollection::hasStateType(const std::string &key) const { - bool exists = states_.find(key) != states_.end(); - bool static_exists = static_states_.find(key) != static_states_.end(); - return exists || static_exists; -} - -void StateCollection::removeState(const std::string &key, double timestamp) { - int64_t timestamp_key = timestampToKey(timestamp); - auto it1 = states_.find(key); - if (it1 != states_.end()) { - auto it2 = it1->second.find(timestamp_key); - if (it2 != it1->second.end()) { - it1->second.erase(it2); +std::shared_ptr +StateCollection::getState(const StateID &id) const { + if (id.hasTimestamp()) { + auto it = time_varying_states_.find(id.key()); + if (it != time_varying_states_.end()) { + auto state_it = it->second.find(id.timestamp().value()); + if (state_it != it->second.end()) { + return state_it->second; + } } - // If the map is empty, remove the key - if (it1->second.empty()) { - states_.erase(it1); + return nullptr; + } else { + auto it = static_states_.find(id.key()); + if (it != static_states_.end()) { + return it->second; } + return nullptr; } } -void StateCollection::removeStaticState(const std::string &key) { - auto it = static_states_.find(key); - if (it != static_states_.end()) { - static_states_.erase(it); +bool StateCollection::hasState(const StateID &state_id) const { + if (state_id.hasTimestamp()) { + auto it = time_varying_states_.find(state_id.key()); + if (it != time_varying_states_.end()) { + auto state_it = it->second.find(state_id.timestamp().value()); + return state_it != it->second.end(); + } + return false; + } else { + return static_states_.find(state_id.key()) != static_states_.end(); } } bool StateCollection::getOldestStamp(const std::string &key, double ×tamp) const { - auto it = states_.find(key); - if (it != states_.end() && !it->second.empty()) { - int64_t timestamp_key = it->second.begin()->first; - timestamp = keyToTimestamp(timestamp_key); + auto it = time_varying_states_.find(key); + if (it != time_varying_states_.end() && !it->second.empty()) { + timestamp = it->second.begin()->first; return true; } return false; } - bool StateCollection::getLatestStamp(const std::string &key, double ×tamp) const { - auto it = states_.find(key); - if (it != states_.end() && !it->second.empty()) { - int64_t timestamp_key = it->second.rbegin()->first; - timestamp = keyToTimestamp(timestamp_key); + auto it = time_varying_states_.find(key); + if (it != time_varying_states_.end() && !it->second.empty()) { + timestamp = it->second.rbegin()->first; return true; } return false; } bool StateCollection::getTimesForState(const std::string &key, - std::vector ×) const { - auto it = states_.find(key); - if (it != states_.end()) { - for (const auto &state : it->second) { - times.push_back(keyToTimestamp(state.first)); + std::vector ×tamps) const { + auto it = time_varying_states_.find(key); + if (it != time_varying_states_.end() && !it->second.empty()) { + timestamps.clear(); + for (const auto &pair : it->second) { + timestamps.push_back(pair.first); } return true; } return false; } +std::shared_ptr +StateCollection::getOldestState(const std::string &key) const { + auto it = time_varying_states_.find(key); + if (it != time_varying_states_.end() && !it->second.empty()) { + // Return the first state + return it->second.begin()->second; + } + return nullptr; +} + +std::shared_ptr +StateCollection::getLatestState(const std::string &key) const { + auto it = time_varying_states_.find(key); + if (it != time_varying_states_.end() && !it->second.empty()) { + // Return the last state + return it->second.rbegin()->second; + } + return nullptr; +} + std::shared_ptr StateCollection::getStateByEstimatePointer(double *ptr) const { - // Loop through all states and check if the pointer matches - for (auto const &state_map : states_) { - for (auto const &state : state_map.second) { - if (state.second->estimatePointer() == ptr) { - return state.second; + // Check time-varying states + for (const auto &type_pair : time_varying_states_) { + for (const auto &time_pair : type_pair.second) { + if (time_pair.second->estimatePointer() == ptr) { + return time_pair.second; } } } - // Loop through static states as well - for (auto const &static_state : static_states_) { - if (static_state.second->estimatePointer() == ptr) { - return static_state.second; + // Check static states + for (const auto &static_pair : static_states_) { + if (static_pair.second->estimatePointer() == ptr) { + return static_pair.second; } } + return nullptr; } bool StateCollection::getStateIDByEstimatePointer(double *ptr, StateID &state_id) const { - // Check all timestamped states first - for (auto const &state_map_ : states_) { - for (auto const &state : state_map_.second) { - if (state.second->estimatePointer() == ptr) { - state_id = StateID(state_map_.first, keyToTimestamp(state.first)); + // Check time-varying states + for (const auto &type_pair : time_varying_states_) { + for (const auto &time_pair : type_pair.second) { + if (time_pair.second->estimatePointer() == ptr) { + state_id = StateID(type_pair.first, time_pair.first); return true; } } } - // Check static states next - for (auto const &static_state : static_states_) { - if (static_state.second->estimatePointer() == ptr) { - state_id = StateID(static_state.first); + // Check static states + for (const auto &static_pair : static_states_) { + if (static_pair.second->estimatePointer() == ptr) { + state_id = StateID(static_pair.first); return true; } } @@ -184,24 +178,4 @@ bool StateCollection::getStateIDByEstimatePointer(double *ptr, return false; } -std::shared_ptr -StateCollection::getOldestState(const std::string &key) const { - auto it = states_.find(key); - if (it != states_.end() && !it->second.empty()) { - // Return the first state - return it->second.begin()->second; - } - return nullptr; -} - -std::shared_ptr -StateCollection::getLatestState(const std::string &key) const { - auto it = states_.find(key); - if (it != states_.end() && !it->second.empty()) { - // Return the last state - return it->second.rbegin()->second; - } - return nullptr; -} - } // namespace ceres_nav \ No newline at end of file diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index af6895d..013b56b 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -1,4 +1,3 @@ -# find_package(Catch2 REQUIRED) add_executable(test_factor_graph test_factor_graph.cpp) target_link_libraries(test_factor_graph PRIVATE ${PROJECT_NAME} Catch2::Catch2WithMain) diff --git a/tests/test_factor_graph.cpp b/tests/test_factor_graph.cpp index 1fa7287..2e69cc4 100644 --- a/tests/test_factor_graph.cpp +++ b/tests/test_factor_graph.cpp @@ -9,9 +9,9 @@ #include "factors/AbsolutePositionFactor.h" #include "factors/IMUPreintegrationFactor.h" +#include "factors/MarginalizationPrior.h" #include "factors/RelativeLandmarkFactor.h" #include "factors/RelativePoseFactor.h" -#include "factors/MarginalizationPrior.h" #include @@ -21,45 +21,47 @@ TEST_CASE("Test Add/Remove states from FactorGraph") { FactorGraph factor_graph; std::shared_ptr> state = std::make_shared>(Eigen::Vector3d(1.0, 2.0, 3.0)); - - factor_graph.addState("x", 0.0, state); - REQUIRE(factor_graph.getStates().hasState("x", 0.0)); + StateID state_id("x", 0.0); + factor_graph.addState(state_id, state); + REQUIRE(factor_graph.getStates().hasState(state_id)); REQUIRE(factor_graph.numParameterBlocks() == 1); // Get the state pointer for this state - std::vector state_ids = {StateID("x", 0.0)}; + std::vector state_ids = {state_id}; std::vector estimate_ptrs; factor_graph.getStatePointers(state_ids, estimate_ptrs); REQUIRE(estimate_ptrs.size() == 1); REQUIRE(estimate_ptrs[0] == state->estimatePointer()); - factor_graph.removeState("x", 0.0); - REQUIRE(!factor_graph.getStates().hasState("x", 0.0)); + factor_graph.removeState(state_id); + REQUIRE(!factor_graph.getStates().hasState(state_id)); REQUIRE(factor_graph.numParameterBlocks() == 0); // Now, add a non-timestamped state std::shared_ptr> static_state = std::make_shared>(Eigen::Vector3d(4.0, 5.0, 6.0)); - factor_graph.addState(StateID("static_state"), static_state); - REQUIRE(factor_graph.getStates().hasStaticState("static_state")); + StateID id2("x", 0.0); + factor_graph.addState(id2, static_state); + REQUIRE(factor_graph.getStates().hasState(id2)); REQUIRE(factor_graph.numParameterBlocks() == 1); // Remove the state - factor_graph.removeState(StateID("static_state")); - REQUIRE(!factor_graph.getStates().hasStaticState("static_state")); + factor_graph.removeState(id2); + REQUIRE(!factor_graph.getStates().hasState(id2)); REQUIRE(factor_graph.numParameterBlocks() == 0); } TEST_CASE("Test setting states constant") { - FactorGraph factor_graph; + FactorGraph graph; std::shared_ptr> state = std::make_shared>(Eigen::Vector3d(1.0, 2.0, 3.0)); - factor_graph.addState("x", 0.0, state); - factor_graph.setConstant("x", 0.0); - REQUIRE(factor_graph.isConstant("x", 0.0)); - - factor_graph.setVariable("x", 0.0); - REQUIRE(!factor_graph.isConstant("x", 0.0)); + StateID state_id("x", 0.0); + graph.addState(state_id, state); + ; + graph.setConstant(state_id); + REQUIRE(graph.isConstant(state_id)); + graph.setVariable(state_id); + REQUIRE(!graph.isConstant(state_id)); } TEST_CASE("Test adding a factor") { @@ -74,7 +76,7 @@ TEST_CASE("Test adding a factor") { // Add in the state to the factor graph auto state = std::make_shared(); - factor_graph.addState("x", 0.0, state); + factor_graph.addState(state_ids[0], state); // Try adding the factor again REQUIRE(factor_graph.addFactor(state_ids, cost_function, 0.0)); @@ -84,10 +86,12 @@ TEST_CASE("Test adding a factor") { /** * This test case creates a simple factor graph with two IMU states and one * landmark. Preintegration factors connect the IMU states, and landmark - * factor connects the IMU states to the landmark. - * - * The test then calls getMarkovBlanketInfo() to get the information for states connected - * to the first IMU state, and checks that the correct states and factors are returned. + * factor connects the IMU states to the landmark. + * + * The test then calls getMarkovBlanketInfo() to get the information for + states + * connected to the first IMU state, and checks that the correct states and + * factors are returned. */ TEST_CASE("Test MarkovBlanketInfo") { FactorGraph factor_graph; @@ -99,10 +103,14 @@ TEST_CASE("Test MarkovBlanketInfo") { auto b1 = std::make_shared>(Eigen::VectorXd::Zero(6)); // Add states to the factor graph - factor_graph.addState("X", 0.0, X0); - factor_graph.addState("b", 0.0, b0); - factor_graph.addState("X", 1.0, X1); - factor_graph.addState("b", 1.0, b1); + StateID x0_id("X", 0.0); + StateID b0_id("b", 0.0); + StateID x1_id("X", 1.0); + StateID b1_id("b", 1.0); + factor_graph.addState(x0_id, X0); + factor_graph.addState(b0_id, b0); + factor_graph.addState(x1_id, X1); + factor_graph.addState(b1_id, b1); // Create and add IMU preintegration factor between X0, b0 and X1, b1 IMUIncrement rmi0(Eigen::Matrix::Identity(), @@ -114,8 +122,7 @@ TEST_CASE("Test MarkovBlanketInfo") { ceres::CostFunction *preintegration_factor = new IMUPreintegrationFactor(rmi0, false); - std::vector state_ids = {StateID("X", 0.0), StateID("b", 0.0), - StateID("X", 1.0), StateID("b", 1.0)}; + std::vector state_ids = {x0_id, b0_id, x1_id, b1_id}; factor_graph.addFactor(state_ids, preintegration_factor, 1.0); REQUIRE(factor_graph.numResidualBlocks() == 1); @@ -150,8 +157,8 @@ TEST_CASE("Test MarkovBlanketInfo") { std::vector connected_states; std::vector factors_m; std::vector factors_r; - bool success = factor_graph.getMarkovBlanketInfo( - states_m, connected_states, factors_m, factors_r); + bool success = factor_graph.getMarkovBlanketInfo(states_m, connected_states, + factors_m, factors_r); REQUIRE(success); diff --git a/tests/test_state_collection.cpp b/tests/test_state_collection.cpp index 63baeeb..a204cb7 100644 --- a/tests/test_state_collection.cpp +++ b/tests/test_state_collection.cpp @@ -17,82 +17,127 @@ using namespace ceres_nav; -TEST_CASE("Test Add/Remove Operations") { - StateCollection state_collection; +TEST_CASE("State ID Comparisons") { + SECTION("Test timestamp rounding and equality") { + StateID id1("pose", 1.2344, 1e-3); + StateID id2("pose", 1.2338, 1e-3); + + REQUIRE(id1.timestamp().value() == 1.234); + REQUIRE(id2.timestamp().value() == 1.234); + REQUIRE(id1 == id2); + } - std::shared_ptr> state = - std::make_shared>(Eigen::Vector3d(1.0, 2.0, 3.0)); + SECTION("Different keys are not equal") { + StateID id3("landmark"); + StateID id4("landmark", 0.0); + REQUIRE(id3 != id4); + } + + SECTION("Change default precision") { + StateID::setDefaultPrecision(1e-9); + double test_time = 2.123456789; + StateID id5("x", test_time); + REQUIRE(id5.timestamp().value() == test_time); + StateID::setDefaultPrecision(1e-6); + } + + SECTION("StateID as map key") { + std::map state_map; + + StateID id1("x", 10.0); + StateID id2("x", 20.0); + StateID id3("x", 30.0); + StateID id4("y"); + StateID id5("z"); + + state_map[id1] = 1; + state_map[id2] = 2; + state_map[id3] = 3; + state_map[id4] = 4; + state_map[id5] = 5; + REQUIRE(state_map.size() == 5); + + REQUIRE(state_map[id1] == 1); + REQUIRE(state_map[id2] == 2); + REQUIRE(state_map[id3] == 3); + REQUIRE(state_map[id4] == 4); + REQUIRE(state_map[id5] == 5); + } +} - state_collection.addState("x", 0.0, state); - state_collection.addState("x", 1.0, state); - state_collection.addState("x", 2.0, state); - - REQUIRE(state_collection.hasState("x", 0.0)); - REQUIRE(state_collection.hasState("x", 1.0)); - REQUIRE(state_collection.hasState("x", 2.0)); - REQUIRE(!state_collection.hasState("x", 3.0)); - REQUIRE(state_collection.getNumStateTypes() == 1); - REQUIRE(state_collection.getNumStatesForType("x") == 3); - - // Now, try removing a state - state_collection.removeState("x", 1.0); - REQUIRE(state_collection.getNumStatesForType("x") == 2); - REQUIRE(!state_collection.hasState("x", 1.0)); - - // Remove the other two states - state_collection.removeState("x", 0.0); - state_collection.removeState("x", 2.0); - REQUIRE(state_collection.getNumStatesForType("x") == 0); +TEST_CASE("Add time-varying parameter block", "[StateCollection]") { + StateCollection collection; + auto block = + std::make_shared>(Eigen::Vector3d(1.0, 2.0, 3.0)); + StateID state_id("velocity", 10.0); + REQUIRE(collection.addState(state_id, block)); + REQUIRE(collection.hasState(state_id)); + REQUIRE(collection.size() == 1); + REQUIRE(collection.getNumberOfStatesForType("velocity") == 1); + + auto retrieved_block = collection.getState(state_id); + REQUIRE(retrieved_block == block); + + // Try adding the same state again - should fail + REQUIRE(!collection.addState(state_id, block)); + + // Now, remove the state + REQUIRE(collection.removeState(state_id)); + REQUIRE(!collection.hasState(state_id)); + REQUIRE(collection.size() == 0); + REQUIRE(collection.getState(state_id) == nullptr); + REQUIRE(collection.getNumberOfStatesForType("velocity") == 0); + + // Add in two states + StateID state_id2("velocity", 12.0); + REQUIRE(collection.addState(state_id, block)); + REQUIRE(collection.addState(state_id2, block)); + REQUIRE(collection.size() == 2); + REQUIRE(collection.getNumberOfStatesForType("velocity") == 2); } -TEST_CASE("Test Multiple State Types") { - StateCollection state_collection; - Eigen::Vector3d x0 = Eigen::Vector3d(1.0, 2.0, 3.0); - Eigen::Matrix x1 = {1.0, 2.0, 3.0, 4.0}; +TEST_CASE("Add static states", "[StateCollection]") { + StateCollection collection; + auto block = + std::make_shared>(Eigen::Vector3d(4.0, 5.0, 6.0)); + StateID id("landmark"); + REQUIRE(collection.addState(id, block)); + REQUIRE(collection.hasState(id)); + REQUIRE(collection.size() == 1); + auto retrieved_block = collection.getState(id); + REQUIRE(retrieved_block == block); + + // Try adding the same static state again - should fail + REQUIRE(!collection.addState(id, block)); + // Now, remove the static state + REQUIRE(collection.removeState(id)); + REQUIRE(!collection.hasState(id)); + REQUIRE(collection.size() == 0); + REQUIRE(collection.getState(id) == nullptr); +} - std::shared_ptr> state = - std::make_shared>(x0); - std::shared_ptr> state_2 = - std::make_shared>(x1); +TEST_CASE("Test multiple state types", "[StateCollection]") { + StateCollection collection; + auto block1 = + std::make_shared>(Eigen::Vector3d(1.0, 2.0, 3.0)); + auto block2 = + std::make_shared>(Eigen::Vector4d(4.0, 5.0, 6.0, 7.0)); + StateID id1("x", 0.0); + StateID id2("y", 0.0); - // Add the states to the collection - state_collection.addState("x1", 0.0, state); - state_collection.addState("x2", 0.0, state_2); + REQUIRE(collection.addState(id1, block1)); + REQUIRE(collection.addState(id2, block2)); - // Try retrieving the states - std::shared_ptr retrieved_state = - state_collection.getState("x1", 0.0); - std::shared_ptr retrieved_state_2 = - state_collection.getState("x2", 0.0); + // Try retrieving both states + auto retrieved_block1 = collection.getState(id1); + auto retrieved_block2 = collection.getState(id2); - REQUIRE(retrieved_state != nullptr); - REQUIRE(retrieved_state_2 != nullptr); - - // Check the estimates - Eigen::Vector3d estimate_x0 = retrieved_state->getEstimate(); - Eigen::Vector4d estimate_x1 = retrieved_state_2->getEstimate(); - REQUIRE(estimate_x0.isApprox(x0)); - REQUIRE(estimate_x1.isApprox(x1)); - - // Try adding a PoseParameterBlock - Eigen::Matrix4d pose = - SE3::fromComponents(SO3::expMap(Eigen::Vector3d(0.1, 0.2, 0.3)), - Eigen::Vector3d(1.0, 2.0, 3.0)); - std::shared_ptr pose_state = - std::make_shared(pose); - - state_collection.addState("pose", 0.0, pose_state); - - // Retrieve the pose state using the method that downcasts to - // PoseParameterBlock - std::shared_ptr retrieved_pose_state = - state_collection.getState("pose", 0.0); - REQUIRE(retrieved_pose_state != nullptr); - REQUIRE(retrieved_pose_state->pose().isApprox(pose)); + REQUIRE(retrieved_block1 == block1); + REQUIRE(retrieved_block2 == block2); } TEST_CASE("Test Timestamp Operations") { - StateCollection state_collection; + StateCollection collection; Eigen::Vector3d x(1.0, 2.0, 3.0); // Add states with timestamps and then try to retrieve them @@ -110,11 +155,10 @@ TEST_CASE("Test Timestamp Operations") { std::shared_ptr> state = std::make_shared>(x); - state_collection.addState("x", cur_stamp, state); - - std::shared_ptr retrieved_state = - state_collection.getState("x", cur_stamp); - REQUIRE(retrieved_state != nullptr); + StateID state_id("x", cur_stamp); + collection.addState(state_id, state); + REQUIRE(collection.getState(state_id) != nullptr); + REQUIRE(collection.getState(state_id)->getEstimate().isApprox(x)); state_values.push_back(x); timestamps.push_back(cur_stamp); @@ -123,8 +167,8 @@ TEST_CASE("Test Timestamp Operations") { // Test the oldest/newest state retrieval double oldest_stamp, latest_stamp; - REQUIRE(state_collection.getOldestStamp("x", oldest_stamp)); - REQUIRE(state_collection.getLatestStamp("x", latest_stamp)); + REQUIRE(collection.getOldestStamp("x", oldest_stamp)); + REQUIRE(collection.getLatestStamp("x", latest_stamp)); // Check that the oldest and latest timestamps approximately match REQUIRE_THAT(oldest_stamp, @@ -134,14 +178,14 @@ TEST_CASE("Test Timestamp Operations") { // Get the timestamps for the states std::vector timestamps_retrieved; - REQUIRE(state_collection.getTimesForState("x", timestamps_retrieved)); + REQUIRE(collection.getTimesForState("x", timestamps_retrieved)); REQUIRE(timestamps_retrieved.size() == num_states); // Get the oldest and latest states from the collection std::shared_ptr> oldest_state = - state_collection.getOldestState>("x"); + collection.getOldestState>("x"); std::shared_ptr> latest_state = - state_collection.getLatestState>("x"); + collection.getLatestState>("x"); REQUIRE(oldest_state != nullptr); REQUIRE(latest_state != nullptr); @@ -149,62 +193,23 @@ TEST_CASE("Test Timestamp Operations") { REQUIRE(latest_state->getEstimate().isApprox(state_values.back())); } -TEST_CASE("Test Static States") { - StateCollection state_collection; +TEST_CASE("Test estimate pointer functions", "[StateCollection]") { + StateCollection collection; - auto state_1 = std::make_shared>(Eigen::Vector3d(1.0, 2.0, 3.0)); - auto state_2 = std::make_shared>(Eigen::Vector3d(4.0, 5.0, 6.0)); - - state_collection.addStaticState("x", state_1); - state_collection.addState("y", 0.0, state_2); - - REQUIRE(state_collection.hasStaticState("x")); - REQUIRE(!state_collection.hasStaticState("y")); - REQUIRE(state_collection.hasStateType("x")); - REQUIRE(state_collection.hasState("y", 0.0)); - - auto static_state = state_collection.getStaticState>("x"); - REQUIRE(static_state != nullptr); - REQUIRE(static_state->getEstimate().isApprox(Eigen::Vector3d(1.0, 2.0, 3.0))); - - // Try getting a state by it's estimate pointer - double* state_ptr = state_1->estimatePointer(); - auto found_state = state_collection.getStateByEstimatePointer(state_ptr); - REQUIRE(found_state != nullptr); - REQUIRE(found_state->getEstimate().isApprox(Eigen::Vector3d(1.0, 2.0, 3.0))); - - double* state_ptr_2 = state_2->estimatePointer(); - auto found_state_2 = state_collection.getStateByEstimatePointer(state_ptr_2); - REQUIRE(found_state_2 != nullptr); - REQUIRE(found_state_2->getEstimate().isApprox(Eigen::Vector3d(4.0, 5.0, 6.0))); - - // Test retrieving a state by StateID - StateID static_id("x"); - auto retrieved_static_state = state_collection.getState(static_id); - REQUIRE(retrieved_static_state != nullptr); - REQUIRE(retrieved_static_state->estimatePointer() == state_ptr); - - // Remove the static state - state_collection.removeStaticState("x"); - REQUIRE(!state_collection.hasStaticState("x")); -} - -TEST_CASE("Test StateID") { - StateID id1("x", 0.1); - StateID id2("y"); - - REQUIRE(!id1.isStatic()); - REQUIRE(id2.isStatic()); - - // Test storing state IDs in a map - std::map state_map; - state_map[id1] = 1; - state_map[id2] = 2; - - REQUIRE(state_map.size() == 2); - - // Test retrieving values - REQUIRE(state_map.at(id1) == 1); - REQUIRE(state_map.at(id2) == 2); -} + Eigen::Vector3d x(1.0, 2.0, 3.0); + StateID state_id("x", 100.0); + std::shared_ptr> state = + std::make_shared>(x); + collection.addState(state_id, state); + auto retrieved_state = + collection.getStateByEstimatePointer(state->estimatePointer()); + REQUIRE(retrieved_state != nullptr); + REQUIRE(retrieved_state->getEstimate().isApprox(x)); + + // Now, try to get the StateID by the estimate pointer + StateID retrieved_id; + REQUIRE(collection.getStateIDByEstimatePointer(state->estimatePointer(), + retrieved_id)); + REQUIRE(retrieved_id == state_id); +} \ No newline at end of file