├── .clang-format
├── .gitignore
├── CMakeLists.txt
├── LICENSE
├── README.md
├── include
└── imesa
│ ├── biased_prior-inl.h
│ ├── biased_prior.h
│ ├── imesa.h
│ └── incremental_sam_agent.h
├── media
├── imesa_header_photo.png
├── len_5000_1_clip.gif
├── rpl.png
└── scale_10_0.gif
└── src
├── biased_prior.cpp
└── imesa.cpp
/.clang-format:
--------------------------------------------------------------------------------
1 | ---
2 | BasedOnStyle: Google
3 | ColumnLimit: 120
--------------------------------------------------------------------------------
/.gitignore:
--------------------------------------------------------------------------------
1 | build/
2 | .vscode/
3 | __pycache__/
--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
1 | project(imesa CXX C)
2 | cmake_minimum_required(VERSION 2.8.3)
3 | set(CMAKE_CXX_STANDARD 17)
4 |
5 | # Dependencies
6 | find_package(GTSAM REQUIRED)
7 | include_directories(${GTSAM_INCLUDE_DIR})
8 |
9 |
10 | message(STATUS "================ iMESA ======================")
11 | # Build the iMESA library
12 | file(GLOB_RECURSE imesa_srcs "*.cpp" "*.h")
13 | add_library(imesa ${imesa_srcs})
14 | target_link_libraries(imesa gtsam)
15 | target_include_directories(imesa PUBLIC "${CMAKE_CURRENT_SOURCE_DIR}/include")
--------------------------------------------------------------------------------
/LICENSE:
--------------------------------------------------------------------------------
1 | MIT License
2 |
3 | Copyright (c) 2024 Daniel McGann
4 |
5 | Permission is hereby granted, free of charge, to any person obtaining a copy
6 | of this software and associated documentation files (the "Software"), to deal
7 | in the Software without restriction, including without limitation the rights
8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
9 | copies of the Software, and to permit persons to whom the Software is
10 | furnished to do so, subject to the following conditions:
11 |
12 | The above copyright notice and this permission notice shall be included in all
13 | copies or substantial portions of the Software.
14 |
15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
21 | SOFTWARE.
22 |
--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
1 | # Incremental Manifold Edge-based Separable ADMM (iMESA)
2 | [](https://opensource.org/licenses/MIT) [
](https://rpl.ri.cmu.edu/)
3 |
4 |
5 | 
8 | Overview of the iMESA algorithm operating on a toy dataset
9 |
10 |
11 |
12 | iMESA is an incremental distributed back-end algorithm for Collaborative Simultaneous Localization and Mapping (C-SLAM). For real-world deployments, robotic teams require algorithms to compute a consistent state estimate accurately, within online runtime constraints, and with potentially limited communication. In this package we provide the original implementation of "Incremental Manifold Edge-based Separable ADMM" (iMESA) a fully distributed C-SLAM back-end algorithm that can provide a multi-robot team with accurate state estimates in real-time with only sparse pair-wise communication between robots. This algorithm is described in full in our paper "iMESA: Incremental Distributed Optimization for Collaborative Simultaneous Localization and Mapping" which can be accessed on [arXiv](https://arxiv.org/pdf/2406.07371). If you use this package please cite our paper as:
13 |
14 | ```
15 | @inproceedings{mcgann_imesa_2024,
16 | title = {{iMESA}: Incremental Distributed Optimization for Collaborative Simultaneous Localization and Mapping},
17 | author = {D. McGann and M. Kaess},
18 | fullauthor = {Daniel McGann and Michael Kaess},
19 | booktitle = {Proc. Robotics: Science and Systems (RSS)},
20 | year = 2024,
21 | pages = {n/a}, % will be added soon
22 | address = {Delft, {NL}}
23 | }
24 | ```
25 |
26 | Extensive evaluation on real and synthetic data demonstrates that iMESA is able to outperform comparable state-of-the-art C-SLAM back-ends. Below we have an examples of such performance on a long 5 robot dataset, and a shorter 10 robot dataset. In these animations color represents the magnitude of error for individual poses.
27 |
28 |
29 | 
32 | iMESA Compared to prior works on a long 5 robot synthetic dataset
33 |
34 |
35 |
36 | 
39 | iMESA Compared to prior works on a short 10 robot synthetic dataset
40 |
41 |
42 |
43 | ## Documentation
44 | We provide the implementation of iMESA as a simple C++ library. This library provides a class `IMESAAgent` that can be used on-board each robot in a multi-robot team to solve their collaborative state estimate using the iMESA algorithm. Practically, this class is used much like existing incremental SLAM solvers like `ISAM2` as provided by `gtsam`, with extensions to incorporate information from communications with other robots.
45 |
46 | See `include/imesa/imesa.h` for inline documentation on this class's interface. Additionally, see our [experiments repository](https://github.com/rpl-cmu/imesa-experiments) for an example of how this library can be used. The best place for documentation on the algorithm itself is the paper discussed above!
47 |
48 | We additionally provide a more generic interface (`IncrementalSAMAgent`) that can be inherited from, to implement other incremental C-SLAM solvers that use the same 2-stage communication procedure. See our implementation of DDF-SAM2 in our [experiments repository](https://github.com/rpl-cmu/imesa-experiments) for an example of how to write a custom algorithm using this interface.
49 |
50 | ### Variable Conventions
51 | iMESA uses uses the keys of variables to implicitly identify which variables are shared with other robots. The library assumes that all variable keys are represented according to the `gtsam::Symbol` convention and consist of a character that denotes the unique identifier of the robot that owns the variable and a integer representing that index of the variable. For example the 15th pose of robot "a" will be `a15`. The iMESA algorithm identifies shared variables as any variable with a key denoting that it is owned by another robot.
52 |
53 | This library also provides functionality to handle global variables that are variables in the SLAM optimization that do not necessarily belong to any singular robot (i.e. landmarks). Such variables are marked with a `#` character in their variable key. Handling these variables requires some additional information to be shared during communication (keys of global vars). This functionality was not explicitly discussed in the paper above, but has been tested and users are safe to use this functionality!
54 |
55 | ## Dependencies
56 | The only dependency for iMESA is [GTSAM](https://github.com/borglab/gtsam). iMESA is sensitive to the version of GTSAM used. We have tested iMESA with GTSAM version `4.2.0` and recommend its use.
57 |
58 | iMESA additionally needs some development functionality in GTSAM that unfortunately is not included in any tagged release. The details of these changes are summarized in this [pull request](https://github.com/borglab/gtsam/pull/1504). For convenience we provide these changes cherry-picked onto GTSAM `4.2.0` via our [fork](https://github.com/DanMcGann/gtsam) under branch `4.2.0-imesa`.
59 |
60 |
61 | ## Installation Instructions
62 |
63 | As this package is only a library it is most frequently will be used as a 3rd party dependency within your project. We encourage users to use `FetchContext` to access and make the iMESA library available within their own project.
64 |
65 | You can include iMESA as a dependency in your project by adding the following to your `CMakeLists.txt` file:
66 |
67 | ```
68 | include(FetchContent)
69 | FetchContent_Declare(
70 | imesa
71 | GIT_REPOSITORY https://github.com/rpl-cmu/imesa.git
72 | GIT_TAG main
73 | )
74 | FetchContent_MakeAvailable(imesa)
75 | ```
76 |
77 | The project in which iMESA is used will need to be build against the GTSAM version discussed above. An example of this in practice can be found in the [experiments repository](https://github.com/rpl-cmu/imesa-experiments).
78 |
79 | # Issues
80 | If you encounter issues with this library please fill out a bug report on github with details on how to reproduce the error!
--------------------------------------------------------------------------------
/include/imesa/biased_prior-inl.h:
--------------------------------------------------------------------------------
1 | #pragma once
2 | /** @brief Implementation of Split and Geodesic Biased Priors.
3 | *
4 | * @author Dan McGann
5 | * @date October, 6th 2023
6 | */
7 |
8 | #include "imesa/biased_prior.h"
9 |
10 | namespace biased_priors {
11 |
12 | /*********************************************************************************************************************/
13 | template
14 | gtsam::Vector GeodesicBiasedPrior::constraintError(const VAR_TYPE& x, const VAR_TYPE& z,
15 | boost::optional Hx) {
16 | return gtsam::traits::Local(z, x, boost::none, Hx);
17 | }
18 |
19 | /*********************************************************************************************************************/
20 | template
21 | gtsam::Vector GeodesicBiasedPrior::genericConstraintError(const gtsam::Value& x, const gtsam::Value& z,
22 | boost::optional Hx) {
23 | return constraintError(x.template cast(), z.template cast(), Hx);
24 | }
25 |
26 | /*********************************************************************************************************************/
27 | template
28 | gtsam::Vector GeodesicBiasedPrior::evaluateError(const VAR_TYPE& val,
29 | boost::optional H) const {
30 | gtsam::Vector err_vec =
31 | constraintError(val, this->vals_ptr_->shared_estimate_lin_point->template cast(), H);
32 | if (H) (*H) = sqrt(this->vals_ptr_->penalty) * (*H);
33 | return sqrt(this->vals_ptr_->penalty) * (err_vec + (this->vals_ptr_->dual_lin_point / this->vals_ptr_->penalty));
34 | }
35 |
36 | /*********************************************************************************************************************/
37 | template
38 | gtsam::Vector SplitBiasedPrior::constraintError(const VAR_TYPE& x, const VAR_TYPE& z,
39 | boost::optional Hx) {
40 | // Translation Error
41 | gtsam::Matrix dtrans_dx, dlocal_dtrans;
42 | gtsam::Vector translation_error = gtsam::traits::Local(
43 | z.translation(), x.translation(&dtrans_dx), boost::none, &dlocal_dtrans);
44 |
45 | // Rotation Error
46 | gtsam::Matrix drot_dx, dlocal_drot;
47 | gtsam::Vector rotation_error =
48 | gtsam::traits::Local(z.rotation(), x.rotation(&drot_dx), boost::none, &dlocal_drot);
49 |
50 | // Construct the result
51 | gtsam::Vector constraint_satisfaction_gap = gtsam::Vector::Zero(translation_error.size() + rotation_error.size());
52 | constraint_satisfaction_gap << translation_error, rotation_error;
53 | if (Hx) {
54 | gtsam::Matrix dtrans_err_dx = dlocal_dtrans * dtrans_dx;
55 | gtsam::Matrix drot_err_dx = dlocal_drot * drot_dx;
56 |
57 | // Vertical concatenation
58 | *Hx = gtsam::Matrix(dtrans_err_dx.rows() + drot_err_dx.rows(), dtrans_err_dx.cols());
59 | *Hx << dtrans_err_dx, drot_err_dx;
60 | }
61 |
62 | return constraint_satisfaction_gap;
63 | }
64 |
65 | /*********************************************************************************************************************/
66 | template
67 | gtsam::Vector SplitBiasedPrior::genericConstraintError(const gtsam::Value& x, const gtsam::Value& z,
68 | boost::optional Hx) {
69 | return constraintError(x.template cast(), z.template cast(), Hx);
70 | }
71 |
72 | /*********************************************************************************************************************/
73 | template
74 | gtsam::Vector SplitBiasedPrior::evaluateError(const VAR_TYPE& val, boost::optional H) const {
75 | gtsam::Vector err_vec =
76 | constraintError(val, this->vals_ptr_->shared_estimate_lin_point->template cast(), H);
77 | if (H) (*H) = sqrt(this->vals_ptr_->penalty) * (*H);
78 | return sqrt(this->vals_ptr_->penalty) * (err_vec + (this->vals_ptr_->dual_lin_point / this->vals_ptr_->penalty));
79 | }
80 |
81 | /*********************************************************************************************************************/
82 | template
83 | LIE_TYPE baseInterpolateSLERP(const LIE_TYPE& pa, const LIE_TYPE& pb, double alpha) {
84 | return gtsam::traits::Retract(pa, alpha * gtsam::traits::Local(pa, pb));
85 | }
86 |
87 | /*********************************************************************************************************************/
88 | template
89 | POSE_TYPE baseInterpolateSPLIT(const POSE_TYPE& start_pose, const POSE_TYPE& end_pose, double alpha) {
90 | typename POSE_TYPE::Rotation rs = start_pose.rotation();
91 | typename POSE_TYPE::Rotation re = end_pose.rotation();
92 | typename POSE_TYPE::Rotation interp_rot = rs.compose(gtsam::traits::Expmap(
93 | alpha * gtsam::traits::Logmap(rs.inverse().compose(re))));
94 | typename POSE_TYPE::Translation interp_trans =
95 | start_pose.translation() + (alpha * (end_pose.translation() - start_pose.translation()));
96 | return POSE_TYPE(interp_rot, interp_trans);
97 | }
98 |
99 | /*********************************************************************************************************************/
100 | /// @brief Constructs a biased prior factor templatized
101 | template typename BIASED_PRIOR>
102 | gtsam::NonlinearFactor::shared_ptr factory(const gtsam::Key& key, const BiasedPriorInfo::shared_ptr& vals_ptr,
103 | const std::optional>& biased_prior_noise_model_sigmas) {
104 | /** VECTOR **/
105 | if (typeid(*vals_ptr->shared_estimate) == typeid(gtsam::GenericValue)) {
106 | // Always use geodesic for linear variables
107 | return boost::make_shared>(
108 | key, vals_ptr, gtsam::noiseModel::Unit::Create(vals_ptr->shared_estimate->dim()));
109 | }
110 | /** POINT2 **/
111 | else if (typeid(*vals_ptr->shared_estimate) == typeid(gtsam::GenericValue)) {
112 | // Always use geodesic for linear variables
113 | return boost::make_shared>(
114 | key, vals_ptr, gtsam::noiseModel::Unit::Create(vals_ptr->shared_estimate->dim()));
115 | }
116 | /** POINT3 **/
117 | else if (typeid(*vals_ptr->shared_estimate) == typeid(gtsam::GenericValue)) {
118 | // Always use geodesic for linear variables
119 | return boost::make_shared>(
120 | key, vals_ptr, gtsam::noiseModel::Unit::Create(vals_ptr->shared_estimate->dim()));
121 | }
122 | /** POSE2 **/
123 | else if (typeid(*vals_ptr->shared_estimate) == typeid(gtsam::GenericValue)) {
124 | gtsam::SharedNoiseModel nm = gtsam::noiseModel::Unit::Create(vals_ptr->shared_estimate->dim());
125 | if (biased_prior_noise_model_sigmas) {
126 | auto [rs, ts] = *biased_prior_noise_model_sigmas;
127 | nm = gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(3) << ts, ts, rs).finished());
128 | }
129 | return boost::make_shared>(key, vals_ptr, nm);
130 | }
131 | /** POSE3 **/
132 | else if (typeid(*vals_ptr->shared_estimate) == typeid(gtsam::GenericValue)) {
133 | gtsam::SharedNoiseModel nm = gtsam::noiseModel::Unit::Create(vals_ptr->shared_estimate->dim());
134 | if (biased_prior_noise_model_sigmas) {
135 | auto [rs, ts] = *biased_prior_noise_model_sigmas;
136 | nm = gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << rs, rs, rs, ts, ts, ts).finished());
137 | }
138 | return boost::make_shared>(key, vals_ptr, nm);
139 | } else {
140 | throw std::runtime_error("Invalid shared_estimate type passed in vals_ptr to Biased Prior factory");
141 | }
142 | }
143 |
144 | } // namespace biased_priors
--------------------------------------------------------------------------------
/include/imesa/biased_prior.h:
--------------------------------------------------------------------------------
1 | #pragma once
2 | /** @brief Implementation of Biased Priors used by the iMESA algorithm to
3 | * enforce equality of shared variables between robots in a multi-robot team.
4 | *
5 | * In this module we implement the Split and Geodesic Biased Priors.
6 | * For details on these biased priors and other options see [1].
7 | *
8 | * [1] D. McGann, K. Lassak, and M. Kaess, “Asynchronous distributed smoothing and mapping via
9 | * on-manifold consensus ADMM,” Proc. IEEE Intl. Conf. on Robotics and Automation (ICRA), 2024
10 | *
11 | * @author Dan McGann
12 | * @date October, 6th 2023
13 | */
14 |
15 | #include
16 | #include
17 | #include
18 | #include
19 | #include
20 |
21 | #include
22 | #include
23 | #include
24 |
25 | namespace biased_priors {
26 | /**
27 | * ######## ### ###### ########
28 | * ## ## ## ## ## ## ##
29 | * ## ## ## ## ## ##
30 | * ######## ## ## ###### ######
31 | * ## ## ######### ## ##
32 | * ## ## ## ## ## ## ##
33 | * ######## ## ## ###### ########
34 | */
35 |
36 | /// @brief Type for a generic constraint function
37 | typedef std::function)>
38 | ConstraintFunction;
39 |
40 | /// @brief Type for a function that interpolates a new shared estimate
41 | typedef std::function(const gtsam::Value&, const gtsam::Value&)>
42 | SharedEstimateInterpolator;
43 |
44 | /// @brief The types of biased priors we can use
45 | enum BiasedPriorType { GEODESIC, SPLIT };
46 | /// @brief Converts string form of BiasedPriorType to enumeration type
47 | BiasedPriorType BPTypeFromString(const std::string& str);
48 | /// @brief Converts enumeration of BiasedPriorType to string representation
49 | std::string BPTypeToString(const BiasedPriorType& type);
50 |
51 | /** @brief The values required by the based prior
52 | * These values are modified externally (during robot communication) and therefore
53 | * they are stored in biased priors as a pointer and accessed during relinearlization.
54 | */
55 | struct BiasedPriorInfo {
56 | /** TYPES **/
57 | typedef std::shared_ptr shared_ptr;
58 |
59 | /** FIELDS **/
60 | /// @brief The current shared estimate for this biased prior ($z$)
61 | boost::shared_ptr shared_estimate;
62 | /// @brief The current shared estimate used for linearization for this biased prior ($z$)
63 | boost::shared_ptr shared_estimate_lin_point;
64 | /// @brief The dual term accumulated for the biased prior ($\lambda$)
65 | gtsam::Vector dual;
66 | /// @brief The dual term accumulated for linearization for the biased prior ($\lambda$)
67 | gtsam::Vector dual_lin_point;
68 | /// @brief The penalty term accumulated for the biased prior ($\beta$)
69 | double penalty;
70 |
71 | /** FUNCTIONS **/
72 | /// @brief Computes the shared estimate update for the biased prior $z = interpolate(x_i, x_j)$
73 | SharedEstimateInterpolator interpolate;
74 | /// @brief Computes the constraint function for the biased prior $q(x,z)$
75 | ConstraintFunction constraint_func;
76 |
77 | /** INTERFACE **/
78 | /// @brief default constructor
79 | BiasedPriorInfo() = default;
80 | /// @brief Constructor from initial est and initial penalty, dual and functions are auto-populated
81 | BiasedPriorInfo(const BiasedPriorType& type, const gtsam::Value& init_estimate, double init_penalty);
82 |
83 | /// @brief Output to string
84 | friend std::ostream& operator<<(std::ostream& o, BiasedPriorInfo const& bpi);
85 | };
86 |
87 | /** @brief A biased prior is the combination of Augmented Lagrangian Dual and Penalty Terms
88 | * We augment the factor-graph with these factors to enforce equality constraints.
89 | */
90 | template
91 | class BiasedPrior : public gtsam::NoiseModelFactor1 {
92 | protected:
93 | /// @brief Pointer to struct containing current edge and dual variables
94 | BiasedPriorInfo::shared_ptr vals_ptr_;
95 |
96 | public:
97 | /** @brief Constructor for base BiasedPrior
98 | * @param key: The key of the variable this biased prior affects
99 | * @param vals_ptr: Pointer to the structure containing the current edge and dual variables for this biased prior
100 | * @param noise_model: The noise model with which to weight this biased prior
101 | */
102 | BiasedPrior(const gtsam::Key& key, const BiasedPriorInfo::shared_ptr& vals_ptr,
103 | const gtsam::SharedNoiseModel& noise_model)
104 | : gtsam::NoiseModelFactor1(noise_model, key), vals_ptr_(vals_ptr) {}
105 |
106 | /// @brief The non-squared biased prior error $\sqrt{\beta} q(\theta) - \frac{\lambda}{\beta}$
107 | virtual gtsam::Vector evaluateError(const VAR_TYPE& val, boost::optional H = boost::none) const = 0;
108 | };
109 |
110 | /**
111 | * ###### ######## ####### ######## ######## ###### #### ######
112 | * ## ## ## ## ## ## ## ## ## ## ## ## ##
113 | * ## ## ## ## ## ## ## ## ## ##
114 | * ## #### ###### ## ## ## ## ###### ###### ## ##
115 | * ## ## ## ## ## ## ## ## ## ## ##
116 | * ## ## ## ## ## ## ## ## ## ## ## ## ##
117 | * ###### ######## ####### ######## ######## ###### #### ######
118 | */
119 | /// @brief Implementation of a Biased Prior using a Geodesic Constraint Function
120 | template
121 | class GeodesicBiasedPrior : public BiasedPrior {
122 | public:
123 | /// @brief Constructor for GeodesicBiasedPrior. For params see BiasedPrior
124 | GeodesicBiasedPrior(const gtsam::Key& key, const BiasedPriorInfo::shared_ptr& vals_ptr,
125 | const gtsam::SharedNoiseModel& noise_model)
126 | : BiasedPrior(key, vals_ptr, noise_model) {}
127 |
128 | /// @brief The non-squared biased prior error $\sqrt{\beta} q(\theta) - \frac{\lambda}{\beta}$
129 | gtsam::Vector evaluateError(const VAR_TYPE& val, boost::optional H = boost::none) const;
130 |
131 | /// @brief The constraint function error $q(x)$
132 | static gtsam::Vector constraintError(const VAR_TYPE& x, const VAR_TYPE& z,
133 | boost::optional Hx = boost::none);
134 | /// @brief The constraint function error $q(x)$ that accepts a generic value
135 | static gtsam::Vector genericConstraintError(const gtsam::Value& x, const gtsam::Value& z,
136 | boost::optional Hx = boost::none);
137 | };
138 |
139 | /**
140 | * ###### ######## ## #### ########
141 | * ## ## ## ## ## ## ##
142 | * ## ## ## ## ## ##
143 | * ###### ######## ## ## ##
144 | * ## ## ## ## ##
145 | * ## ## ## ## ## ##
146 | * ###### ## ######## #### ##
147 | */
148 | /// @brief Implementation of a Biased Prior using a SPLIT Constraint Function
149 | template
150 | class SplitBiasedPrior : public BiasedPrior {
151 | public:
152 | /// @brief Constructor for SplitBiasedPrior. For params see BiasedPrior
153 | SplitBiasedPrior(const gtsam::Key& key, const BiasedPriorInfo::shared_ptr& vals_ptr,
154 | const gtsam::SharedNoiseModel& noise_model)
155 | : BiasedPrior(key, vals_ptr, noise_model) {}
156 |
157 | /// @brief The non-squared biased prior error $\sqrt{\beta} q(\theta) - \frac{\lambda}{\beta}$
158 | gtsam::Vector evaluateError(const VAR_TYPE& val, boost::optional H = boost::none) const;
159 |
160 | /// @brief The constraint function error $q(x)$
161 | static gtsam::Vector constraintError(const VAR_TYPE& x, const VAR_TYPE& z,
162 | boost::optional Hx = boost::none);
163 | /// @brief The constraint function error $q(x)$ that accepts a generic value
164 | static gtsam::Vector genericConstraintError(const gtsam::Value& x, const gtsam::Value& z,
165 | boost::optional Hx = boost::none);
166 | };
167 |
168 | /**
169 | * #### ## ## ######## ######## ######## ######## ####### ## ### ######## ########
170 | * ## ### ## ## ## ## ## ## ## ## ## ## ## ## ## ##
171 | * ## #### ## ## ## ## ## ## ## ## ## ## ## ## ## ##
172 | * ## ## ## ## ## ###### ######## ######## ## ## ## ## ## ## ######
173 | * ## ## #### ## ## ## ## ## ## ## ## ######### ## ##
174 | * ## ## ### ## ## ## ## ## ## ## ## ## ## ## ##
175 | * #### ## ## ## ######## ## ## ## ####### ######## ## ## ## ########
176 | */
177 |
178 | /** @brief Interpolates a Lie Group Object spherically.
179 | * @param pa: The start object of interpolation
180 | * @param pb: The end object of interpolation
181 | * @param alpha: The interpolation factor
182 | * @returns The interpolated object
183 | */
184 | template
185 | LIE_TYPE baseInterpolateSLERP(const LIE_TYPE& pa, const LIE_TYPE& pb, double alpha);
186 |
187 | /** @brief Interpolates a POSE Object: linearly for its translation and spherically for its rotation
188 | * @param pa: The start pose of interpolation
189 | * @param pb: The end pose of interpolation
190 | * @param alpha: The interpolation factor
191 | * @returns The interpolated pose
192 | */
193 | template
194 | POSE_TYPE baseInterpolateSPLIT(const POSE_TYPE& start_pose, const POSE_TYPE& end_pose, double alpha);
195 |
196 | /** @brief Interpolates the generic values to compute a new shared estimate.
197 | * Actual implementation handled in template specialization functions
198 | * @param this_estimate: The current solution to the variable held by this agent
199 | * @param other_estimate: The current solution to the variable held by the other agent
200 | * @returns The new shared estimate (z) held jointly by both agents
201 | */
202 | boost::shared_ptr interpolate_shared_estimate_vector(const gtsam::Value& this_estimate,
203 | const gtsam::Value& other_estimate);
204 | boost::shared_ptr interpolate_shared_estimate_point2(const gtsam::Value& this_estimate,
205 | const gtsam::Value& other_estimate);
206 | boost::shared_ptr interpolate_shared_estimate_point3(const gtsam::Value& this_estimate,
207 | const gtsam::Value& other_estimate);
208 | boost::shared_ptr interpolate_shared_estimate_pose2(const gtsam::Value& this_estimate,
209 | const gtsam::Value& other_estimate);
210 | boost::shared_ptr interpolate_shared_estimate_pose3(const gtsam::Value& this_estimate,
211 | const gtsam::Value& other_estimate);
212 | /**
213 | * ######## ### ###### ######## ####### ######## ## ##
214 | * ## ## ## ## ## ## ## ## ## ## ## ##
215 | * ## ## ## ## ## ## ## ## ## ####
216 | * ###### ## ## ## ## ## ## ######## ##
217 | * ## ######### ## ## ## ## ## ## ##
218 | * ## ## ## ## ## ## ## ## ## ## ##
219 | * ## ## ## ###### ## ####### ## ## ##
220 | */
221 |
222 | /** @brief Constructs a biased prior factor.
223 | * Factors are templatized, but we want to construct biased priors without templatizing the agent class.
224 | * So that the class can handle generic shared measurements of any type. therefore we need to deduce the type at runtime
225 | * Note: no macro usage to make things easier to read (though much longer code wise)
226 | * @param biased_prior_noise_model_sigmas: (rotation, translation) the weights with which to construct the noise model
227 | * of the biased prior. If none the noise model defaults to identity. Also note the weights are only used for POSE
228 | * biased priors.
229 | */
230 | gtsam::NonlinearFactor::shared_ptr factory(
231 | const BiasedPriorType& type, const gtsam::Key& key, const BiasedPriorInfo::shared_ptr& vals_ptr,
232 | const std::optional>& biased_prior_noise_model_sigmas = std::nullopt);
233 |
234 | /// @brief Constructs a biased prior factor templatized
235 | template typename BIASED_PRIOR>
236 | gtsam::NonlinearFactor::shared_ptr factory(
237 | const gtsam::Key& key, const BiasedPriorInfo::shared_ptr& vals_ptr,
238 | const std::optional>& biased_prior_noise_model_sigmas = std::nullopt);
239 |
240 | /** @brief Constructs a shared estimate interpolator function
241 | * Each shared variable needs its own interpolator function as the interpolation depends on type so we use this factory
242 | * to construct the corresponding interpolator for the given biased prior
243 | */
244 | SharedEstimateInterpolator interpolator_factory(const boost::shared_ptr& val);
245 |
246 | /** @brief Constructs the appropriate constraint function for the value
247 | * To update the dual variable we need access to the constraint function q.
248 | */
249 | ConstraintFunction constraint_func_factory(const BiasedPriorType& type, const boost::shared_ptr& val);
250 |
251 | }; // namespace biased_priors
252 |
253 | #include "imesa/biased_prior-inl.h"
--------------------------------------------------------------------------------
/include/imesa/imesa.h:
--------------------------------------------------------------------------------
1 | #pragma once
2 | /** @brief Interface of the iMESA algorithm via an individual agent interface.
3 | * This solver is to be run on-board individual robots in the multi-robot team to
4 | * collaboratively solve the collaborative simultaneous localization and mapping problem.
5 | *
6 | * This algorithm is described in detail in
7 | *
8 | * [2] D. McGann and M. Kaess, "iMESA: Incremental Distributed Optimization for Collaborative
9 | * Simultaneous Localization and Mapping", Proc. Robotic Science and Systems (RSS), 2024
10 | *
11 | *
12 | * Note: The communication interface REQUIRES the following order of operations to complete
13 | * the two state communication process outlined in [2] - Alg. 3
14 | * 1. declareNewSharedVariables / receiveNewSharedVariables
15 | * 2. sendCommunication / receiveCommunication
16 | * If the two stage communication process is not completed in whole, the algorithm invariants will break
17 | * and the algorithm can exhibit bad performance.
18 | *
19 | * @author Dan McGann
20 | * @date October, 6th 2023
21 | */
22 |
23 | #include
24 | #include
25 | #include
26 | #include
27 | #include
28 | #include
29 |
30 | #include
31 |
32 | #include "imesa/biased_prior.h"
33 | #include "imesa/incremental_sam_agent.h"
34 |
35 | using namespace incremental_sam_agent;
36 |
37 | namespace imesa {
38 |
39 | /// @brief The data communicated between two iMESA agents
40 | struct IMESACommunicationData : public CommunicationData {
41 | typedef std::shared_ptr shared_ptr;
42 | /// @brief The current solution to all variables shared between robots
43 | gtsam::Values shared_var_solution;
44 | /// @brief The variables that this robot needs to initialize from the other robot
45 | gtsam::KeySet vars_requested_for_init;
46 | };
47 |
48 | /// @brief Configuration parameters for the iMESA algo, all agents MUST use the same params
49 | struct IMESAAgentParams {
50 | /// @brief The type of biased prior to use for pose variables
51 | /// Note: Using SPLIT was partially explored but not rigorously evaluated or discussed in [2]. Use with caution.
52 | biased_priors::BiasedPriorType pose_biased_prior_type{biased_priors::BiasedPriorType::GEODESIC};
53 | /// @brief The initial penalty parameter used before the shared variable is initialized (should be very small)
54 | double initial_penalty_param{0.0001};
55 | /// @brief The constant penalty parameter to use for biased priors of initialzed shared variables
56 | double penalty_param{1};
57 | /// @brief The threshold for a change in shared estimate or dual variable value required to force reelimination
58 | /// Note: Setting > 0.0 was partially explored, but not rigorously evaluated or discussed in [2]. Use with caution.
59 | double shared_var_wildfire_threshold{0.0};
60 | /// @brief The sigmas to use for biased prior noise models on poses (rotation_sigma, translation sigma)
61 | std::optional> biased_prior_noise_model_sigmas{std::nullopt};
62 | };
63 |
64 | /// @brief Implementation of the iMESA Algorithm
65 | class IMESAAgent : public IncrementalSAMAgent {
66 | /** STATIC MEMBERS **/
67 | public:
68 | inline static const std::string METHOD_NAME = "imesa";
69 | inline static const std::string SPLIT_METHOD_NAME = "imesa_split";
70 | inline static const std::string UNWEIGHTED_METHOD_NAME = "imesa_unweighted";
71 | inline static const std::string SPLIT_UNWEIGHTED_METHOD_NAME = "imesa_split_unweighted";
72 |
73 | /** FIELDS **/
74 | protected:
75 | /// @brief The hyper-parameters for the iMESA algorithm
76 | IMESAAgentParams params_;
77 | /// @brief The underlying incremental smoother instance
78 | gtsam::ISAM2 smoother_;
79 |
80 | /// @brief The set of all observed global variables
81 | gtsam::KeySet observed_global_variables_;
82 | /// @brief Mapping from shared variables to the agents with which they are shared
83 | std::map> shared_var_robots_;
84 | /// @brief Mapping from other agents to all variables shared between this and the other agent
85 | std::map robot_shared_vars_;
86 | /// @brief The Dual and Penalty terms for each shared variable's biased prior
87 | std::map>> biased_prior_values_;
88 | /// @brief Variables marked to be initialized from their owner robot
89 | std::map shared_initialization_variables_;
90 | /// @brief Mapping from other agents to all vars that need to be declared as shared with the other agent
91 | std::map undeclared_robot_shared_vars_;
92 | /// @brief Shared variables updated during the last communication that need to be reeliminated to account for their
93 | /// new shared estimates and dual variables
94 | gtsam::FastList reelim_keys_;
95 | /// @brief Aggregated biased priors declared during communication with other robots
96 | gtsam::NonlinearFactorGraph new_other_robot_declared_biased_priors_;
97 |
98 | /** INTERFACE **/
99 | public:
100 | /** @brief Constructor
101 | * @param rid: The robot id for this agent
102 | * @param params: The iMESA specific parameters
103 | * @param isam2_params: The parameters for the underlying iSAM2 solver
104 | * WARN: This class will override some isam2_params (relinearizeSkip, cacheLinearizedFactors) to ensure proper
105 | * functionality of the iMESA algorithm
106 | */
107 | IMESAAgent(const RobotId& rid, IMESAAgentParams params = IMESAAgentParams(),
108 | gtsam::ISAM2Params isam2_params = gtsam::ISAM2Params())
109 | : IncrementalSAMAgent(rid), params_(params) {
110 | // Note: to ensure proper behavior of iMESA we override some iSAM2 parameters
111 | isam2_params.relinearizeSkip = 1;
112 | isam2_params.cacheLinearizedFactors = false;
113 | smoother_ = gtsam::ISAM2(isam2_params);
114 | }
115 |
116 | /** @brief Update interface for new measurements. [2] - Alg. 2
117 | * @param new_factors: The new measurements. These measurements may be intra or inter robot measurements.
118 | * @param new_theta: Any new values affected by the new_factors that are not already part of the system.
119 | * @returns The ISAM2Result containing information on the update.
120 | */
121 | gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& new_factors = gtsam::NonlinearFactorGraph(),
122 | const gtsam::Values& new_theta = gtsam::Values()) override;
123 |
124 | /** @brief Interface for new shared variables which are the first thing communicated during inter-robot communication
125 | * @param other_robot: The identifier of the robot with which this agent is communicating
126 | * @returns Any new variables shared with the other robot since last communication, and all observed global variables
127 | * that may be shared
128 | */
129 | DeclaredVariables declareNewSharedVariables(const RobotId& other_robot) override;
130 |
131 | /** @brief Interface for receiving new shared variables from another robot. [2] - Alg. 3
132 | * @param other_robot: The identifier of the other robot with which this agent is communicating
133 | * @param declared_variables: The declared variables of the robot with whom we are communicating
134 | */
135 | void receiveNewSharedVariables(const RobotId& other_robot, const DeclaredVariables& declared_variables) override;
136 |
137 | /** @brief Interface to construct data to send to another robot during inter-robot communication, [2], Alg. 3
138 | * @param other_robot: The identifier of the robot with which this agent is communicating
139 | * @returns The data to send to the other_robot
140 | */
141 | CommunicationData::shared_ptr sendCommunication(const RobotId& other_robot) override;
142 |
143 | /** @brief Interface for receiving shared estimates from inter-robot communication. [2], Alg. 3
144 | * @param other_robot: The identifier of the other robot with which this agent is communicating
145 | * @param comm_data: The data received from the robot with whom we are communicating
146 | */
147 | void receiveCommunication(const RobotId& other_robot, const CommunicationData::shared_ptr& comm_data) override;
148 |
149 | /// @brief Returns the name of the method (note this is affected by parameters in IMESAAgentParams)
150 | std::string name() const override;
151 |
152 | /** HELPERS **/
153 | protected:
154 | /** @brief Adds new shared variables updating the internal book keeping and constructing biased priors. [2] - Alg. 1
155 | * This routine is called both during communication procedures, and during update procedures
156 | * @param new_shared_vars: New shared variables and the robot they are shared with induced by a inter-robot update
157 | * @param new_theta: Values containing initial estimates for all new shared variables
158 | * @param undeclared: Flag indicating that these shared variables are "undeclared" and not yet know to the other robot
159 | * @returns Biased priors for each new shared variable
160 | */
161 | gtsam::NonlinearFactorGraph addNewSharedVariables(const std::vector>& new_shared_vars,
162 | const gtsam::Values& new_theta, bool undeclared);
163 | };
164 |
165 | } // namespace imesa
--------------------------------------------------------------------------------
/include/imesa/incremental_sam_agent.h:
--------------------------------------------------------------------------------
1 | #pragma once
2 | /** @brief This module defines a generic interface for an incremental "Smoothing and Mapping" (SAM) agent.
3 | * Classes (like iMESA) that inherit from this interface are meant to solve incremental Collaborative Simultaneous
4 | * Localization and Mapping (C-SLAM) problems for multi-robot teams. In this context each robot is to have a unique
5 | * instance of an "agent" solver that works similar to existing incremental SLAM solvers (e.g. gtsam::ISAM2) but with
6 | * additional functionality to incorporate information from communications between robots.
7 | *
8 | * Note: As discussed in [2] any incremental C-SLAM solver requires either sharing entire states or requires a 2-stage
9 | * communication. This interface assumes that algorithms use the more efficient 2-stage approach.
10 | *
11 | * [2] D. McGann and M. Kaess, "iMESA: Incremental Distributed Optimization for Collaborative
12 | * Simultaneous Localization and Mapping", Proc. Robotic Science and Systems (RSS), 2024
13 | *
14 | * @author Dan McGann
15 | * @date October, 6th 2023
16 | */
17 | #include
18 | #include
19 | #include
20 | #include
21 |
22 | namespace incremental_sam_agent {
23 |
24 | /** TYPES**/
25 | using RobotId = char;
26 | inline static const char GLOBAL_ID = '#';
27 |
28 | /// @brief Base type for derived communication data
29 | struct CommunicationData {
30 | typedef std::shared_ptr shared_ptr;
31 | virtual ~CommunicationData() = default; // Force struct to be polymorphic
32 | };
33 |
34 | /// @brief Container for sharing new shared variables between robots
35 | struct DeclaredVariables {
36 | /// @brief Shared variables directly observed since last communication
37 | gtsam::KeySet new_shared_variables;
38 | /// @brief List of all global variables observed, which may be shared
39 | gtsam::KeySet observed_global_variables;
40 | };
41 |
42 | class IncrementalSAMAgent {
43 | /** FIELDS **/
44 | protected:
45 | /// @brief The unique identifier for this agent
46 | RobotId robot_id_;
47 | /// @brief The current solution of the smoother
48 | gtsam::Values current_estimate_;
49 |
50 | /** INTERFACE **/
51 | public:
52 | /// @brief Constructor populating the unique robot identifier
53 | IncrementalSAMAgent(const RobotId& rid) : robot_id_(rid) {}
54 |
55 | /** @brief Update interface for new measurements.
56 | * @param new_factors: The new measurements. These measurements may be intra or inter robot measurements.
57 | * @param new_theta: Any new values affected by the new_factors that are not already part of the system.
58 | * @returns The ISAM2Result containing information on the update.
59 | */
60 | virtual gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& new_factors = gtsam::NonlinearFactorGraph(),
61 | const gtsam::Values& new_theta = gtsam::Values()) = 0;
62 |
63 | /** @brief Interface for new shared variables which are the first thing communicated during inter-robot communication
64 | * @param other_robot: The identifier of the robot with which this agent is communicating
65 | * @returns Any new variables shared with the other robot since last communication, and all observed global variables
66 | * that may be shared
67 | */
68 | virtual DeclaredVariables declareNewSharedVariables(const RobotId& other_robot) = 0;
69 |
70 | /** @brief Interface for receiving new shared variables from another robot.
71 | * @param other_robot: The identifier of the other robot with which this agent is communicating
72 | * @param declared_variables: The declared variables of the robot with whom we are communicating
73 | */
74 | virtual void receiveNewSharedVariables(const RobotId& other_robot, const DeclaredVariables& declared_variables) = 0;
75 |
76 | /** @brief Interface to construct data to send to another robot during inter-robot communication,
77 | * @param other_robot: The identifier of the robot with which this agent is communicating
78 | * @returns The data to send to the other_robot
79 | */
80 | virtual CommunicationData::shared_ptr sendCommunication(const RobotId& other_robot) = 0;
81 |
82 | /** @brief Interface for receiving shared estimates from inter-robot communication.
83 | * @param other_robot: The identifier of the other robot with which this agent is communicating
84 | * @param comm_data: The data received from the robot with whom we are communicating
85 | */
86 | virtual void receiveCommunication(const RobotId& other_robot, const CommunicationData::shared_ptr& comm_data) = 0;
87 |
88 | /// @brief Returns the current solution of the distributed incremental smoother
89 | virtual gtsam::Values getEstimate() { return current_estimate_; }
90 |
91 | /// @brief Returns the name of the method of the agent
92 | virtual std::string name() const = 0;
93 | };
94 |
95 | } // namespace incremental_sam_agent
--------------------------------------------------------------------------------
/media/imesa_header_photo.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/rpl-cmu/imesa/693c1ad2e20d88f97b480060c4830f0545599f7b/media/imesa_header_photo.png
--------------------------------------------------------------------------------
/media/len_5000_1_clip.gif:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/rpl-cmu/imesa/693c1ad2e20d88f97b480060c4830f0545599f7b/media/len_5000_1_clip.gif
--------------------------------------------------------------------------------
/media/rpl.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/rpl-cmu/imesa/693c1ad2e20d88f97b480060c4830f0545599f7b/media/rpl.png
--------------------------------------------------------------------------------
/media/scale_10_0.gif:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/rpl-cmu/imesa/693c1ad2e20d88f97b480060c4830f0545599f7b/media/scale_10_0.gif
--------------------------------------------------------------------------------
/src/biased_prior.cpp:
--------------------------------------------------------------------------------
1 | /** @brief Implementation of Split and Geodesic Biased Priors.
2 | *
3 | * @author Dan McGann
4 | * @date October, 6th 2023
5 | */
6 |
7 | #include "imesa/biased_prior.h"
8 |
9 | namespace biased_priors {
10 | /*********************************************************************************************************************/
11 | BiasedPriorType BPTypeFromString(const std::string& str) {
12 | if (str == "GEODESIC") {
13 | return biased_priors::BiasedPriorType::GEODESIC;
14 | } else if (str == "SPLIT") {
15 | return biased_priors::BiasedPriorType::SPLIT;
16 | } else {
17 | throw std::runtime_error("BPTypeFromString: invalid pose_biased_prior_type");
18 | }
19 | }
20 | /*********************************************************************************************************************/
21 | std::string BPTypeToString(const BiasedPriorType& type) {
22 | if (type == BiasedPriorType::GEODESIC) {
23 | return "GEODESIC";
24 | } else if (type == BiasedPriorType::SPLIT) {
25 | return "SPLIT";
26 | } else {
27 | throw std::runtime_error("BPTypeToString: invalid pose_biased_prior_type");
28 | }
29 | }
30 |
31 | /*********************************************************************************************************************/
32 | BiasedPriorInfo::BiasedPriorInfo(const BiasedPriorType& type, const gtsam::Value& init_estimate, double init_penalty) {
33 | this->shared_estimate = init_estimate.clone();
34 | this->shared_estimate_lin_point = init_estimate.clone();
35 | this->dual = gtsam::Vector::Zero(init_estimate.dim());
36 | this->dual_lin_point = gtsam::Vector::Zero(init_estimate.dim());
37 | this->penalty = init_penalty;
38 | this->interpolate = interpolator_factory(this->shared_estimate);
39 | this->constraint_func = constraint_func_factory(type, this->shared_estimate);
40 | }
41 |
42 | /*********************************************************************************************************************/
43 | std::ostream& operator<<(std::ostream& o, BiasedPriorInfo const& bpi) {
44 | o << "Biased Prior Info | Dual: [" << bpi.dual(0) << ", " << bpi.dual(1) << ", " << bpi.dual(2)
45 | << "] Penalty: " << bpi.penalty;
46 | return o;
47 | }
48 |
49 | /*********************************************************************************************************************/
50 | boost::shared_ptr interpolate_shared_estimate_vector(const gtsam::Value& this_estimate,
51 | const gtsam::Value& other_estimate) {
52 | gtsam::Vector new_shared_estimate =
53 | baseInterpolateSLERP(this_estimate.cast(), other_estimate.cast(), 0.5);
54 | return gtsam::GenericValue(new_shared_estimate).clone();
55 | }
56 |
57 | boost::shared_ptr interpolate_shared_estimate_point2(const gtsam::Value& this_estimate,
58 | const gtsam::Value& other_estimate) {
59 | gtsam::Point2 new_shared_estimate =
60 | baseInterpolateSLERP(this_estimate.cast(), other_estimate.cast(), 0.5);
61 | return gtsam::GenericValue(new_shared_estimate).clone();
62 | }
63 |
64 | boost::shared_ptr interpolate_shared_estimate_point3(const gtsam::Value& this_estimate,
65 | const gtsam::Value& other_estimate) {
66 | gtsam::Point3 new_shared_estimate =
67 | baseInterpolateSLERP(this_estimate.cast(), other_estimate.cast(), 0.5);
68 | return gtsam::GenericValue(new_shared_estimate).clone();
69 | }
70 |
71 | boost::shared_ptr interpolate_shared_estimate_pose2(const gtsam::Value& this_estimate,
72 | const gtsam::Value& other_estimate) {
73 | gtsam::Pose2 new_shared_estimate =
74 | baseInterpolateSPLIT(this_estimate.cast(), other_estimate.cast(), 0.5);
75 | return gtsam::GenericValue(new_shared_estimate).clone();
76 | }
77 |
78 | boost::shared_ptr interpolate_shared_estimate_pose3(const gtsam::Value& this_estimate,
79 | const gtsam::Value& other_estimate) {
80 | gtsam::Pose3 new_shared_estimate =
81 | baseInterpolateSPLIT(this_estimate.cast(), other_estimate.cast(), 0.5);
82 | return gtsam::GenericValue(new_shared_estimate).clone();
83 | }
84 |
85 | /*********************************************************************************************************************/
86 | gtsam::NonlinearFactor::shared_ptr factory(
87 | const BiasedPriorType& type, const gtsam::Key& key, const BiasedPriorInfo::shared_ptr& vals_ptr,
88 | const std::optional>& biased_prior_noise_model_sigmas) {
89 | if (type == BiasedPriorType::GEODESIC) {
90 | return factory(key, vals_ptr, biased_prior_noise_model_sigmas);
91 | } else if (type == BiasedPriorType::SPLIT) {
92 | return factory(key, vals_ptr, biased_prior_noise_model_sigmas);
93 | } else {
94 | throw std::runtime_error("Invalid BiasedPriorType type passed to Biased Prior factory");
95 | }
96 | }
97 |
98 | /*********************************************************************************************************************/
99 | SharedEstimateInterpolator interpolator_factory(const boost::shared_ptr& val) {
100 | if (typeid(*val) == typeid(gtsam::GenericValue)) {
101 | return &interpolate_shared_estimate_vector;
102 | } else if (typeid(*val) == typeid(gtsam::GenericValue)) {
103 | return &interpolate_shared_estimate_point2;
104 | } else if (typeid(*val) == typeid(gtsam::GenericValue)) {
105 | return &interpolate_shared_estimate_point3;
106 | } else if (typeid(*val) == typeid(gtsam::GenericValue)) {
107 | return &interpolate_shared_estimate_pose2;
108 | } else if (typeid(*val) == typeid(gtsam::GenericValue)) {
109 | return &interpolate_shared_estimate_pose3;
110 | } else {
111 | throw std::runtime_error("Invalid type passed in vals_ptr to interpolator_factory");
112 | }
113 | }
114 |
115 | /*********************************************************************************************************************/
116 | ConstraintFunction constraint_func_factory(const BiasedPriorType& type, const boost::shared_ptr& val) {
117 | /** VECTOR **/
118 | if (typeid(*val) == typeid(gtsam::GenericValue)) {
119 | // Always use geodesic for linear variables
120 | return &GeodesicBiasedPrior::genericConstraintError;
121 | }
122 | /** POINT2 **/
123 | else if (typeid(*val) == typeid(gtsam::GenericValue)) {
124 | // Always use geodesic for linear variables
125 | return &GeodesicBiasedPrior::genericConstraintError;
126 | }
127 | /** POINT3 **/
128 | else if (typeid(*val) == typeid(gtsam::GenericValue)) {
129 | // Always use geodesic for linear variables
130 | return &GeodesicBiasedPrior::genericConstraintError;
131 | }
132 | /** POSE2 **/
133 | else if (typeid(*val) == typeid(gtsam::GenericValue)) {
134 | if (type == BiasedPriorType::GEODESIC) {
135 | return &GeodesicBiasedPrior::genericConstraintError;
136 | } else if (type == BiasedPriorType::SPLIT) {
137 | return &SplitBiasedPrior::genericConstraintError;
138 | }
139 | }
140 | /** POSE3 **/
141 | else if (typeid(*val) == typeid(gtsam::GenericValue)) {
142 | if (type == BiasedPriorType::GEODESIC) {
143 | return &GeodesicBiasedPrior::genericConstraintError;
144 | } else if (type == BiasedPriorType::SPLIT) {
145 | return &SplitBiasedPrior::genericConstraintError;
146 | }
147 | }
148 | throw std::runtime_error("Invalid type or value type passed in vals_ptr to constraint_func_factory");
149 | }
150 |
151 | } // namespace biased_priors
--------------------------------------------------------------------------------
/src/imesa.cpp:
--------------------------------------------------------------------------------
1 | /** @brief Implementation of the iMESA algorithm interface.
2 | *
3 | * @author Dan McGann
4 | * @date October, 6th 2023
5 | */
6 |
7 | #include "imesa/imesa.h"
8 |
9 | #include
10 | #include
11 |
12 | namespace imesa {
13 | /**
14 | * #### ## ## ######## ######## ######## ######## ### ###### ########
15 | * ## ### ## ## ## ## ## ## ## ## ## ## ##
16 | * ## #### ## ## ## ## ## ## ## ## ## ##
17 | * ## ## ## ## ## ###### ######## ###### ## ## ## ######
18 | * ## ## #### ## ## ## ## ## ######### ## ##
19 | * ## ## ### ## ## ## ## ## ## ## ## ## ##
20 | * #### ## ## ## ######## ## ## ## ## ## ###### ########
21 | */
22 |
23 | /*********************************************************************************************************************/
24 | gtsam::ISAM2Result IMESAAgent::update(const gtsam::NonlinearFactorGraph& new_factors, const gtsam::Values& new_theta) {
25 | // Determine if we observed new variables of other robots
26 | // We identify shared variables by the character of the variable key as it is used to signify the owner robot.
27 | std::vector> new_shared_variables;
28 | for (const auto& key : new_factors.keys()) {
29 | gtsam::Symbol sym(key);
30 | if (sym.chr() != robot_id_ && !current_estimate_.exists(key)) {
31 | if (sym.chr() == GLOBAL_ID) {
32 | observed_global_variables_.insert(key);
33 | } else {
34 | new_shared_variables.push_back(std::make_pair(sym.chr(), key));
35 | }
36 | }
37 | }
38 | // Create new biased priors for all newly observed other robot variables
39 | gtsam::NonlinearFactorGraph new_biased_priors(addNewSharedVariables(new_shared_variables, new_theta, true));
40 |
41 | // Augment the new factors with new biased priors and any biased priors for variables declared by other robots
42 | gtsam::NonlinearFactorGraph augmented_new_factors(new_factors);
43 | augmented_new_factors.push_back(new_biased_priors);
44 | augmented_new_factors.push_back(new_other_robot_declared_biased_priors_);
45 | new_other_robot_declared_biased_priors_ = gtsam::NonlinearFactorGraph();
46 |
47 | // Do the update
48 | gtsam::ISAM2UpdateParams update_params;
49 | update_params.force_relinearize = true;
50 | update_params.extraReelimKeys = reelim_keys_;
51 | gtsam::ISAM2Result result = smoother_.update(augmented_new_factors, new_theta, update_params);
52 | current_estimate_ = smoother_.calculateEstimate();
53 | reelim_keys_.clear();
54 | return result;
55 | }
56 |
57 | /*********************************************************************************************************************/
58 | DeclaredVariables IMESAAgent::declareNewSharedVariables(const RobotId& other_robot) {
59 | DeclaredVariables result;
60 | result.new_shared_variables = undeclared_robot_shared_vars_[other_robot];
61 | result.observed_global_variables = observed_global_variables_;
62 |
63 | undeclared_robot_shared_vars_[other_robot] = gtsam::KeySet();
64 | return result;
65 | }
66 |
67 | /*********************************************************************************************************************/
68 | void IMESAAgent::receiveNewSharedVariables(const RobotId& other_robot, const DeclaredVariables& declared_variables) {
69 | std::vector> new_shared_variables;
70 | for (const auto& key : declared_variables.new_shared_variables) {
71 | new_shared_variables.push_back(std::make_pair(other_robot, key));
72 | }
73 |
74 | for (const auto& key : declared_variables.observed_global_variables) {
75 | // If we have observed the key, but it is not marked as shared with the other_robot add it
76 | if (observed_global_variables_.count(key) != 0 && robot_shared_vars_[other_robot].count(key) == 0) {
77 | new_shared_variables.push_back(std::make_pair(other_robot, key));
78 | }
79 | }
80 |
81 | // Update book keeping and create biased priors, aggregate biased priors to be added on next update
82 | new_other_robot_declared_biased_priors_.push_back(
83 | addNewSharedVariables(new_shared_variables, current_estimate_, false));
84 | }
85 |
86 | /*********************************************************************************************************************/
87 | CommunicationData::shared_ptr IMESAAgent::sendCommunication(const RobotId& other_robot) {
88 | IMESACommunicationData::shared_ptr cd_ptr = std::make_shared();
89 | for (const gtsam::Key& key : robot_shared_vars_[other_robot]) {
90 | // Add the local solution to the shared variable
91 | cd_ptr->shared_var_solution.insert(key, current_estimate_.at(key));
92 |
93 | // Mark the variable if we need to initialize it from the other robot
94 | if (shared_initialization_variables_.count(key) && shared_initialization_variables_[key] == other_robot) {
95 | cd_ptr->vars_requested_for_init.insert(key);
96 | }
97 | }
98 | return cd_ptr;
99 | }
100 |
101 | /*********************************************************************************************************************/
102 | void IMESAAgent::receiveCommunication(const RobotId& other_robot, const CommunicationData::shared_ptr& comm_data) {
103 | std::shared_ptr imesa_comm_data =
104 | std::dynamic_pointer_cast(comm_data);
105 | // Update shared estimate, dual, and penalty terms for all shared variables
106 | for (const gtsam::Key& key : robot_shared_vars_[other_robot]) {
107 | biased_priors::BiasedPriorInfo::shared_ptr bpi = biased_prior_values_[other_robot][key];
108 |
109 | // Update our value for the variable if marked for shared initialization
110 | if (shared_initialization_variables_.count(key) && shared_initialization_variables_[key] == other_robot) {
111 | current_estimate_.update(key, imesa_comm_data->shared_var_solution.at(key));
112 | shared_initialization_variables_.erase(key);
113 | }
114 |
115 | // If the variable is marked for initialization the other robot will use our estimate
116 | boost::shared_ptr other_val = imesa_comm_data->vars_requested_for_init.count(key)
117 | ? current_estimate_.at(key).clone()
118 | : imesa_comm_data->shared_var_solution.at(key).clone();
119 |
120 | // Compute the new shared estimate and dual variable
121 | boost::shared_ptr new_shared_estimate = bpi->interpolate(current_estimate_.at(key), *other_val);
122 | gtsam::Vector new_dual_variable =
123 | bpi->dual + bpi->penalty * bpi->constraint_func(current_estimate_.at(key), *new_shared_estimate, boost::none);
124 |
125 | // Compute the change in the shared estimate and dual variable relative to the current linearization point
126 | double estimate_delta = new_shared_estimate->localCoordinates_(*bpi->shared_estimate_lin_point).norm();
127 | double dual_delta = (new_dual_variable - bpi->dual_lin_point).norm();
128 |
129 | // If new, or there is sufficient change, mark for reelim and update linearization points
130 | if ((estimate_delta + dual_delta) >= params_.shared_var_wildfire_threshold ||
131 | bpi->penalty == params_.initial_penalty_param) {
132 | reelim_keys_.push_back(key);
133 | bpi->shared_estimate_lin_point = new_shared_estimate->clone();
134 | bpi->dual_lin_point = gtsam::Vector(new_dual_variable);
135 | }
136 |
137 | // Update shared and dual estimates
138 | bpi->shared_estimate = new_shared_estimate;
139 | bpi->dual = new_dual_variable;
140 | if (bpi->penalty == params_.initial_penalty_param) bpi->penalty = params_.penalty_param;
141 | }
142 | }
143 |
144 | /*********************************************************************************************************************/
145 | std::string IMESAAgent::name() const {
146 | // Unweighted Variants
147 | if (!params_.biased_prior_noise_model_sigmas.has_value()) {
148 | if (params_.pose_biased_prior_type == biased_priors::BiasedPriorType::GEODESIC) {
149 | return UNWEIGHTED_METHOD_NAME;
150 | } else {
151 | return SPLIT_UNWEIGHTED_METHOD_NAME;
152 | }
153 | }
154 | // Standard Variants
155 | else {
156 | if (params_.pose_biased_prior_type == biased_priors::BiasedPriorType::GEODESIC) {
157 | return METHOD_NAME; // iMESA as presented in [2]
158 | } else {
159 | return SPLIT_METHOD_NAME;
160 | }
161 | }
162 | }
163 |
164 | /**
165 | * ## ## ######## ## ######## ######## ######## ######
166 | * ## ## ## ## ## ## ## ## ## ## ##
167 | * ## ## ## ## ## ## ## ## ## ##
168 | * ######### ###### ## ######## ###### ######## ######
169 | * ## ## ## ## ## ## ## ## ##
170 | * ## ## ## ## ## ## ## ## ## ##
171 | * ## ## ######## ######## ## ######## ## ## ######
172 | */
173 |
174 | /*********************************************************************************************************************/
175 | gtsam::NonlinearFactorGraph IMESAAgent::addNewSharedVariables(
176 | const std::vector>& new_shared_vars, const gtsam::Values& new_theta,
177 | bool undeclared) {
178 | gtsam::NonlinearFactorGraph new_biased_priors;
179 |
180 | // Iterate over all the new shared variables for each update bookkeeping
181 | for (const auto& rkp : new_shared_vars) {
182 | const RobotId rid = rkp.first;
183 | const gtsam::Key key = rkp.second;
184 |
185 | // Construct containers if first instance of a variable or a robot
186 | if (robot_shared_vars_.count(rid) == 0) robot_shared_vars_[rid] = gtsam::KeySet();
187 | if (shared_var_robots_.count(key) == 0) shared_var_robots_[key] = std::set();
188 |
189 | // Record this shared variable
190 | robot_shared_vars_[rid].insert(key);
191 | shared_var_robots_[key].insert(rid);
192 |
193 | // Mark for initialization if we dont already have the variable initialized
194 | if (!current_estimate_.exists(key) && gtsam::Symbol(key).chr() != robot_id_) {
195 | shared_initialization_variables_[key] = rid;
196 | }
197 |
198 | // If these new shared variables are undeclared we need to mark them for communication to other robots
199 | if (undeclared) {
200 | if (undeclared_robot_shared_vars_.count(rid) == 0) {
201 | undeclared_robot_shared_vars_[rid] = gtsam::KeySet();
202 | }
203 | undeclared_robot_shared_vars_[rid].insert(key);
204 | }
205 |
206 | // Construct the biased prior
207 | biased_priors::BiasedPriorInfo::shared_ptr bpi = std::make_shared(
208 | params_.pose_biased_prior_type, new_theta.at(key), params_.initial_penalty_param);
209 |
210 | // Book keep the new biased prior and its corresponding values
211 | new_biased_priors.push_back(
212 | biased_priors::factory(params_.pose_biased_prior_type, key, bpi, params_.biased_prior_noise_model_sigmas));
213 | biased_prior_values_[rid][key] = bpi;
214 | }
215 | return new_biased_priors;
216 | }
217 |
218 | } // namespace imesa
--------------------------------------------------------------------------------