3838
3939#include < algorithm>
4040#include < cassert>
41+ #include < functional>
42+ #include < iterator>
4143#include < ostream>
44+ #include < stdexcept>
4245#include < string>
46+ #include < utility>
4347#include < vector>
4448
49+ #include < fuse_core/ceres_macros.hpp>
4550#include < fuse_core/constraint.hpp>
4651#include < fuse_core/eigen.hpp>
47- #include < fuse_core/local_parameterization.hpp>
4852#include < fuse_core/fuse_macros.hpp>
53+ #include < fuse_core/local_parameterization.hpp>
54+ #include < fuse_core/manifold.hpp>
55+ #include < fuse_core/manifold_adapter.hpp>
4956#include < fuse_core/serialization.hpp>
5057#include < fuse_core/variable.hpp>
5158
@@ -125,13 +132,20 @@ class MarginalConstraint : public fuse_core::Constraint
125132 */
126133 const std::vector<fuse_core::VectorXd> & x_bar () const {return x_bar_;}
127134
135+ #if !CERES_SUPPORTS_MANIFOLDS
128136 /* *
129137 * @brief Read-only access to the variable local parameterizations
130138 */
131139 const std::vector<fuse_core::LocalParameterization::SharedPtr> & localParameterizations () const
132140 {
133141 return local_parameterizations_;
134142 }
143+ #else
144+ /* *
145+ * @brief Read-only access to the variable manifolds
146+ */
147+ const std::vector<fuse_core::Manifold::SharedPtr> & manifolds () const {return manifolds_;}
148+ #endif
135149
136150 /* *
137151 * @brief Print a human-readable description of the constraint to the provided stream.
@@ -155,32 +169,88 @@ class MarginalConstraint : public fuse_core::Constraint
155169protected:
156170 std::vector<fuse_core::MatrixXd> A_; // !< The A matrices of the marginal constraint
157171 fuse_core::VectorXd b_; // !< The b vector of the marginal constraint
158-
172+ # if !CERES_SUPPORTS_MANIFOLDS
159173 // !< The local parameterizations
160174 std::vector<fuse_core::LocalParameterization::SharedPtr> local_parameterizations_;
161-
175+ #else
176+ std::vector<fuse_core::Manifold::SharedPtr> manifolds_; // !< Manifolds
177+ #endif
162178 std::vector<fuse_core::VectorXd> x_bar_; // !< The linearization point of each involved variable
163179
164180private:
165181 // Allow Boost Serialization access to private methods
166182 friend class boost ::serialization::access;
167183
168184 /* *
169- * @brief The Boost Serialize method that serializes all of the data members in to/out of the
170- * archive
185+ * @brief The Boost Serialize method that serializes all of the data members in to the archive
171186 *
172- * @param[in/ out] archive - The archive object that holds the serialized class members
173- * @param[in] version - The version of the archive being read/ written. Generally unused .
187+ * @param[out] archive - The archive object into which class members will be serialized
188+ * @param[in] version - The version of the archive being written.
174189 */
175190 template <class Archive >
176- void serialize (Archive & archive, const unsigned int /* version */
191+ void save (Archive & archive, const unsigned int /* version */ const
177192 {
178- archive & boost::serialization::base_object<fuse_core::Constraint>(*this );
179- archive & A_;
180- archive & b_;
181- archive & local_parameterizations_;
182- archive & x_bar_;
193+ archive << boost::serialization::base_object<fuse_core::Constraint>(*this );
194+ archive << A_;
195+ archive << b_;
196+ #if !CERES_SUPPORTS_MANIFOLDS
197+ archive << local_parameterizations_;
198+ #else
199+ archive << manifolds_;
200+ #endif
201+ archive << x_bar_;
183202 }
203+
204+ /* *
205+ * @brief The Boost Serialize method that serializes all of the data members out of the archive
206+ *
207+ * @param[in] archive - The archive object that holds the serialized class members
208+ * @param[in] version - The version of the archive being read.
209+ */
210+ template <class Archive >
211+ void load (Archive & archive, const unsigned int version)
212+ {
213+ archive >> boost::serialization::base_object<fuse_core::Constraint>(*this );
214+ archive >> A_;
215+ archive >> b_;
216+ if (version == 0 ) {
217+ // Version 0 serialization files will contain a std::vector of LocalParameterization
218+ // shared pointers. If the current version of Ceres Solver does not support Manifolds,
219+ // then the serialized LocalParameterization pointers can be deserialized directly into
220+ // the class member. But if the current version of Ceres Solver supports manifolds, then
221+ // the serialized LocalParameterization pointers must be wrapped in a Manifold adapter first.
222+ #if !CERES_SUPPORTS_MANIFOLDS
223+ archive >> local_parameterizations_;
224+ #else
225+ auto local_parameterizations = std::vector<fuse_core::LocalParameterization::SharedPtr>();
226+ archive >> local_parameterizations;
227+ std::transform (
228+ std::make_move_iterator (local_parameterizations.begin ()),
229+ std::make_move_iterator (local_parameterizations.end ()),
230+ std::back_inserter (manifolds_),
231+ [](fuse_core::LocalParameterization::SharedPtr local_parameterization)
232+ {return fuse_core::ManifoldAdapter::make_shared (std::move (local_parameterization));});
233+ #endif
234+ } else { // (version >= 1)
235+ // Version 1 serialization files will contain a std::vector of Manifold shared pointers. If
236+ // the current version of Ceres Solver does not support Manifolds, then there is no way to
237+ // deserialize the requested data. But if the current version of Ceres Solver does support
238+ // manifolds, then the serialized Manifold pointers can be deserialized directly into the
239+ // class member.
240+ #if !CERES_SUPPORTS_MANIFOLDS
241+ throw std::runtime_error (
242+ " Attempting to deserialize an archive saved in Version "
243+ std::to_string (
244+ version) + " format. However, the current version of Ceres Solver ("
245+ CERES_VERSION_STRING + " ) does not support manifolds. Ceres Solver version 2.1.0 "
246+ " or later is required to load this file."
247+ #else
248+ archive >> manifolds_;
249+ #endif
250+ }
251+ archive >> x_bar_;
252+ }
253+ BOOST_SERIALIZATION_SPLIT_MEMBER ()
184254};
185255
186256namespace detail
@@ -202,14 +272,24 @@ inline const fuse_core::VectorXd getCurrentValue(const fuse_core::Variable & var
202272 return Eigen::Map<const fuse_core::VectorXd>(variable.data (), variable.size ());
203273}
204274
275+ #if !CERES_SUPPORTS_MANIFOLDS
205276/* *
206277 * @brief Return the local parameterization of the provided variable
207278 */
208- inline fuse_core::LocalParameterization::SharedPtr const getLocalParameterization (
279+ inline fuse_core::LocalParameterization::SharedPtr getLocalParameterization (
209280 const fuse_core::Variable & variable)
210281{
211282 return fuse_core::LocalParameterization::SharedPtr (variable.localParameterization ());
212283}
284+ #else
285+ /* *
286+ * @brief Return the manifold of the provided variable
287+ */
288+ inline fuse_core::Manifold::SharedPtr getManifold (const fuse_core::Variable & variable)
289+ {
290+ return fuse_core::Manifold::SharedPtr (variable.manifold ());
291+ }
292+ #endif
213293
214294} // namespace detail
215295
@@ -226,16 +306,26 @@ MarginalConstraint::MarginalConstraint(
226306 boost::make_transform_iterator(last_variable, &fuse_constraints::detail::getUuid)),
227307 A_(first_A, last_A),
228308 b_(b),
309+ #if !CERES_SUPPORTS_MANIFOLDS
229310 local_parameterizations_ (boost::make_transform_iterator(first_variable,
230311 &fuse_constraints::detail::getLocalParameterization),
231312 boost::make_transform_iterator(last_variable,
232313 &fuse_constraints::detail::getLocalParameterization)),
314+ #else
315+ manifolds_ (
316+ boost::make_transform_iterator (first_variable, &fuse_constraints::detail::getManifold),
317+ boost::make_transform_iterator(last_variable, &fuse_constraints::detail::getManifold)),
318+ #endif
233319 x_bar_ (boost::make_transform_iterator(first_variable, &fuse_constraints::detail::getCurrentValue),
234320 boost::make_transform_iterator(last_variable, &fuse_constraints::detail::getCurrentValue))
235321{
236322 assert (!A_.empty ());
237323 assert (A_.size () == x_bar_.size ());
324+ #if !CERES_SUPPORTS_MANIFOLDS
238325 assert (A_.size () == local_parameterizations_.size ());
326+ #else
327+ assert (A_.size () == manifolds_.size ());
328+ #endif
239329 assert (b_.rows () > 0 );
240330 assert (
241331 std::all_of (
@@ -255,5 +345,13 @@ MarginalConstraint::MarginalConstraint(
255345} // namespace fuse_constraints
256346
257347BOOST_CLASS_EXPORT_KEY (fuse_constraints::MarginalConstraint);
348+ // Since the contents of the serialized file will change depending on the CeresSolver version,
349+ // also set the Boost Serialization version to allow code reading serialized file to know what
350+ // data to expect.
351+ #if !CERES_SUPPORTS_MANIFOLDS
352+ BOOST_CLASS_VERSION (fuse_constraints::MarginalConstraint, 0 );
353+ #else
354+ BOOST_CLASS_VERSION (fuse_constraints::MarginalConstraint, 1 );
355+ #endif
258356
259357#endif // FUSE_CONSTRAINTS__MARGINAL_CONSTRAINT_HPP_
0 commit comments