From bb77ad71636f875889d2db43d5993cc31616e440 Mon Sep 17 00:00:00 2001 From: Jake McLaughlin Date: Sun, 20 Nov 2022 16:49:07 -0500 Subject: [PATCH 1/2] Implement check for adding constraints to marginalized variables --- fuse_optimizers/src/fixed_lag_smoother.cpp | 24 ++++++++++++++++++++++ 1 file changed, 24 insertions(+) diff --git a/fuse_optimizers/src/fixed_lag_smoother.cpp b/fuse_optimizers/src/fixed_lag_smoother.cpp index a91850825..1ea2d36b0 100644 --- a/fuse_optimizers/src/fixed_lag_smoother.cpp +++ b/fuse_optimizers/src/fixed_lag_smoother.cpp @@ -197,6 +197,30 @@ void FixedLagSmoother::optimizationLoop() { continue; } + // check if new transaction has added constraints to variables that are to be marginalized + std::vector faulty_constraints; + for(auto& c: new_transaction->addedConstraints()) + { + for(auto var_uuid: c.variables()) + { + for (auto marginal_uuid : marginal_transaction_.removedVariables()) + { + if (var_uuid == marginal_uuid) + { + faulty_constraints.push_back(c.uuid()); + break; + } + } + } + } + if(faulty_constraints.size() > 0) + { + ROS_WARN_STREAM("Removing invalid constraints."); + for(auto& faulty_constraint: faulty_constraints) + { + new_transaction->removeConstraint(faulty_constraint); + } + } // Prepare for selecting the marginal variables preprocessMarginalization(*new_transaction); // Combine the new transactions with any marginal transaction from the end of the last cycle From 6a0ea4e4e659ca67c1831caf0ec2d78350fa2ffa Mon Sep 17 00:00:00 2001 From: Jake McLaughlin Date: Tue, 17 Jan 2023 19:45:07 -0500 Subject: [PATCH 2/2] Add helper to landmakr variable --- fuse_optimizers/CMakeLists.txt | 1 + .../fuse_variables/point_3d_landmark.h | 197 +++++++++--------- 2 files changed, 102 insertions(+), 96 deletions(-) diff --git a/fuse_optimizers/CMakeLists.txt b/fuse_optimizers/CMakeLists.txt index 798787394..1c101fd6b 100644 --- a/fuse_optimizers/CMakeLists.txt +++ b/fuse_optimizers/CMakeLists.txt @@ -1,5 +1,6 @@ cmake_minimum_required(VERSION 3.0.2) project(fuse_optimizers) +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fopenmp") set(build_depends diagnostic_updater diff --git a/fuse_variables/include/fuse_variables/point_3d_landmark.h b/fuse_variables/include/fuse_variables/point_3d_landmark.h index 1661c11a1..598079695 100644 --- a/fuse_variables/include/fuse_variables/point_3d_landmark.h +++ b/fuse_variables/include/fuse_variables/point_3d_landmark.h @@ -49,107 +49,112 @@ namespace fuse_variables { -/** - * @brief Variable representing a 3D point landmark that exists across time. - * - * This is commonly used to represent locations of visual features. The UUID of this class is constant after - * construction and dependent on a user input database id. As such, the database id cannot be altered after - * construction. - */ -class Point3DLandmark : public FixedSizeVariable<3> -{ -public: - FUSE_VARIABLE_DEFINITIONS(Point3DLandmark); - - /** - * @brief Can be used to directly index variables in the data array - */ - enum : size_t - { - X = 0, - Y = 1, - Z = 2 - }; - - /** - * @brief Default constructor - */ - Point3DLandmark() = default; - /** - * @brief Construct a point 3D variable given a landmarks id + * @brief Variable representing a 3D point landmark that exists across time. * - * @param[in] landmark_id The id associated to a landmark - */ - explicit Point3DLandmark(const uint64_t& landmark_id); - - /** - * @brief Read-write access to the X-axis position. + * This is commonly used to represent locations of visual features. The UUID of this class is constant after + * construction and dependent on a user input database id. As such, the database id cannot be altered after + * construction. */ - double& x() { return data_[X]; } - - /** - * @brief Read-only access to the X-axis position. - */ - const double& x() const { return data_[X]; } - - /** - * @brief Read-write access to the Y-axis position. - */ - double& y() { return data_[Y]; } - - /** - * @brief Read-only access to the Y-axis position. - */ - const double& y() const { return data_[Y]; } - - /** - * @brief Read-write access to the Z-axis position. - */ - double& z() { return data_[Z]; } - - /** - * @brief Read-only access to the Z-axis position. - */ - const double& z() const { return data_[Z]; } - - /** - * @brief Read-only access to the id - */ - const uint64_t& id() const { return id_; } - - /** - * @brief Print a human-readable description of the variable to the provided - * stream. - * - * @param[out] stream The stream to write to. Defaults to stdout. - */ - void print(std::ostream& stream = std::cout) const override; - -private: - // Allow Boost Serialization access to private methods - friend class boost::serialization::access; - uint64_t id_ { 0 }; - - /** - * @brief The Boost Serialize method that serializes all of the data members - * in to/out of the archive - * - * @param[in/out] archive - The archive object that holds the serialized class - * members - * @param[in] version - The version of the archive being read/written. - * Generally unused. - */ - template - void serialize(Archive& archive, const unsigned int /* version */) + class Point3DLandmark : public FixedSizeVariable<3> { - archive& boost::serialization::base_object>(*this); - archive& id_; - } -}; + public: + FUSE_VARIABLE_DEFINITIONS(Point3DLandmark); + + /** + * @brief Can be used to directly index variables in the data array + */ + enum : size_t + { + X = 0, + Y = 1, + Z = 2 + }; + + /** + * @brief Default constructor + */ + Point3DLandmark() = default; + + /** + * @brief Construct a point 3D variable given a landmarks id + * + * @param[in] landmark_id The id associated to a landmark + */ + explicit Point3DLandmark(const uint64_t &landmark_id); + + /** + * @brief Read-write access to the X-axis position. + */ + double &x() { return data_[X]; } + + /** + * @brief Read-only access to the X-axis position. + */ + const double &x() const { return data_[X]; } + + /** + * @brief Read-write access to the Y-axis position. + */ + double &y() { return data_[Y]; } + + /** + * @brief Read-only access to the Y-axis position. + */ + const double &y() const { return data_[Y]; } + + /** + * @brief Read-write access to the Z-axis position. + */ + double &z() { return data_[Z]; } + + /** + * @brief Read-only access to the Z-axis position. + */ + const double &z() const { return data_[Z]; } + + /** + * @brief Read-only access to the id + */ + const uint64_t &id() const { return id_; } + + /** + * @brief Access to the point as a vector + */ + Eigen::Vector3d point() const { return Eigen::Vector3d(x(), y(), z()); } + + /** + * @brief Print a human-readable description of the variable to the provided + * stream. + * + * @param[out] stream The stream to write to. Defaults to stdout. + */ + void print(std::ostream &stream = std::cout) const override; + + private: + // Allow Boost Serialization access to private methods + friend class boost::serialization::access; + uint64_t id_{0}; + + /** + * @brief The Boost Serialize method that serializes all of the data members + * in to/out of the archive + * + * @param[in/out] archive - The archive object that holds the serialized class + * members + * @param[in] version - The version of the archive being read/written. + * Generally unused. + */ + template + void serialize(Archive &archive, const unsigned int /* version */) + { + archive &boost::serialization::base_object>(*this); + archive &id_; + } + }; -} // namespace fuse_variables +} // namespace fuse_variables BOOST_CLASS_EXPORT_KEY(fuse_variables::Point3DLandmark); -#endif // FUSE_VARIABLES_POINT_3D_LANDMARK_H +#endif // FUSE_VARIABLES_POINT_3D_LANDMARK_H