12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

slam folder. serialize std::optional

release/4.3a0
kartik arcot 2023-01-11 13:33:25 -08:00
parent 610a73cfe1
commit d338a7086b
19 changed files with 144 additions and 55 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

79
gtsam/base/std_optional_serialization.h Normal file
View File

@ -0,0 +1,79 @@
// A hack to serialize std::optional<T> to boost::archive
// Don't know if it will work. Trying to follow this:
// PR: https://github.com/boostorg/serialization/pull/148/files#
#include <optional>
#include <boost/config.hpp>
#include <boost/archive/detail/basic_iarchive.hpp>
#include <boost/move/utility_core.hpp>
#include <boost/serialization/item_version_type.hpp>
#include <boost/serialization/split_free.hpp>
#include <boost/serialization/level.hpp>
#include <boost/serialization/nvp.hpp>
#include <boost/serialization/version.hpp>
#include <boost/type_traits/is_pointer.hpp>
#include <boost/serialization/detail/stack_constructor.hpp>
#include <boost/serialization/detail/is_default_constructible.hpp>
// function specializations must be defined in the appropriate
// namespace - boost::serialization
namespace boost {
namespace serialization {
template <class Archive, class T>
void save(Archive& ar, const std::optional<T>& t, const unsigned int /*version*/
) {
// It is an inherent limitation to the serialization of optional.hpp
// that the underlying type must be either a pointer or must have a
// default constructor. It's possible that this could change sometime
// in the future, but for now, one will have to work around it. This can
// be done by serialization the optional<T> as optional<T *>
BOOST_STATIC_ASSERT(boost::serialization::detail::is_default_constructible<T>::value || boost::is_pointer<T>::value);
const bool tflag = t.has_value();
ar << boost::serialization::make_nvp("initialized", tflag);
if (tflag) {
const boost::serialization::item_version_type item_version(version<T>::value);
#if 0
const boost::archive::library_version_type library_version(
ar.get_library_version()
};
if(boost::archive::library_version_type(3) < library_version){
ar << BOOST_SERIALIZATION_NVP(item_version);
}
#else
ar << BOOST_SERIALIZATION_NVP(item_version);
#endif
ar << boost::serialization::make_nvp("value", *t);
}
}
template <class Archive, class T>
void load(Archive& ar, std::optional<T>& t, const unsigned int /*version*/
) {
bool tflag;
ar >> boost::serialization::make_nvp("initialized", tflag);
if (!tflag) {
t.reset();
return;
}
boost::serialization::item_version_type item_version(0);
boost::archive::library_version_type library_version(ar.get_library_version());
if (boost::archive::library_version_type(3) < library_version) {
ar >> BOOST_SERIALIZATION_NVP(item_version);
}
detail::stack_allocate<T> tp;
ar >> boost::serialization::make_nvp("value", tp.reference());
t = std::move(tp.reference());
// hack because std::optional does not have get_ptr fn
ar.reset_object_address(&(t.value()), &tp.reference());
}
template <class Archive, class T>
void serialize(Archive& ar, std::optional<T>& t, const unsigned int version) {
boost::serialization::split_free(ar, t, version);
}
} // namespace serialization
} // namespace boost

1
gtsam/base/tests/testSerializationBase.cpp
View File

@ -28,6 +28,7 @@
#include <gtsam/base/FastVector.h>
#include <gtsam/base/serializationTestHelpers.h>
#include <gtsam/base/std_optional_serialization.h>
#include <CppUnitLite/TestHarness.h>
using namespace std;

5
gtsam/slam/KarcherMeanFactor-inl.h
View File

@ -20,6 +20,7 @@
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/slam/KarcherMeanFactor.h>
#include <optional>
using namespace std;
@ -58,7 +59,7 @@ T FindKarcherMean(std::initializer_list<T>&& rotations) {
template <class T>
template <typename CONTAINER>
KarcherMeanFactor<T>::KarcherMeanFactor(const CONTAINER &keys, int d,
boost::optional<double> beta)
std::optional<double> beta)
: NonlinearFactor(keys), d_(static_cast<size_t>(d)) {
if (d <= 0) {
throw std::invalid_argument(
@ -75,4 +76,4 @@ KarcherMeanFactor<T>::KarcherMeanFactor(const CONTAINER &keys, int d,
whitenedJacobian_ =
boost::make_shared<JacobianFactor>(terms, Vector::Zero(d));
}
} // namespace gtsam
} // namespace gtsam

3
gtsam/slam/KarcherMeanFactor.h
View File

@ -22,6 +22,7 @@
#include <map>
#include <vector>
#include <optional>
namespace gtsam {
/**
@ -60,7 +61,7 @@ public:
*/
template <typename CONTAINER>
KarcherMeanFactor(const CONTAINER &keys, int d = D,
boost::optional<double> beta = boost::none);
std::optional<double> beta = {});
/// Destructor
~KarcherMeanFactor() override {}

10
gtsam/slam/ProjectionFactor.h
View File

@ -25,7 +25,7 @@
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <boost/optional.hpp>
#include <optional>
namespace gtsam {
@ -43,7 +43,7 @@ namespace gtsam {
// Keep a copy of measurement and calibration for I/O
Point2 measured_; ///< 2D measurement
boost::shared_ptr<CALIBRATION> K_; ///< shared pointer to calibration object
boost::optional<POSE> body_P_sensor_; ///< The pose of the sensor in the body frame
std::optional<POSE> body_P_sensor_; ///< The pose of the sensor in the body frame
// verbosity handling for Cheirality Exceptions
bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false)
@ -80,7 +80,7 @@ namespace gtsam {
*/
GenericProjectionFactor(const Point2& measured, const SharedNoiseModel& model,
Key poseKey, Key pointKey, const boost::shared_ptr<CALIBRATION>& K,
boost::optional<POSE> body_P_sensor = boost::none) :
std::optional<POSE> body_P_sensor = {}) :
Base(model, poseKey, pointKey), measured_(measured), K_(K), body_P_sensor_(body_P_sensor),
throwCheirality_(false), verboseCheirality_(false) {}
@ -99,7 +99,7 @@ namespace gtsam {
GenericProjectionFactor(const Point2& measured, const SharedNoiseModel& model,
Key poseKey, Key pointKey, const boost::shared_ptr<CALIBRATION>& K,
bool throwCheirality, bool verboseCheirality,
boost::optional<POSE> body_P_sensor = boost::none) :
std::optional<POSE> body_P_sensor = {}) :
Base(model, poseKey, pointKey), measured_(measured), K_(K), body_P_sensor_(body_P_sensor),
throwCheirality_(throwCheirality), verboseCheirality_(verboseCheirality) {}
@ -176,7 +176,7 @@ namespace gtsam {
}
/** return the (optional) sensor pose with respect to the vehicle frame */
const boost::optional<POSE>& body_P_sensor() const {
const std::optional<POSE>& body_P_sensor() const {
return body_P_sensor_;
}

5
gtsam/slam/SmartFactorBase.h
View File

@ -30,6 +30,7 @@
#include <gtsam/geometry/CameraSet.h>
#include <boost/optional.hpp>
#include <optional>
#include <boost/serialization/optional.hpp>
#include <boost/make_shared.hpp>
#include <vector>
@ -78,7 +79,7 @@ protected:
*/
ZVector measured_;
boost::optional<Pose3>
std::optional<Pose3>
body_P_sensor_; ///< Pose of the camera in the body frame
// Cache for Fblocks, to avoid a malloc ever time we re-linearize
@ -98,7 +99,7 @@ protected:
/// Construct with given noise model and optional arguments.
SmartFactorBase(const SharedNoiseModel& sharedNoiseModel,
boost::optional<Pose3> body_P_sensor = boost::none,
std::optional<Pose3> body_P_sensor = {},
size_t expectedNumberCameras = 10)
: body_P_sensor_(body_P_sensor), Fs(expectedNumberCameras) {

4
gtsam/slam/SmartProjectionFactor.h
View File

@ -26,7 +26,7 @@
#include <gtsam/inference/Symbol.h>
#include <gtsam/slam/dataset.h>
#include <boost/optional.hpp>
#include <optional>
#include <boost/make_shared.hpp>
#include <vector>
@ -410,7 +410,7 @@ protected:
* to transform it to \f$ (h(x)-z)^2/\sigma^2 \f$, and then multiply by 0.5.
*/
double totalReprojectionError(const Cameras& cameras,
boost::optional<Point3> externalPoint = boost::none) const {
std::optional<Point3> externalPoint = {}) const {
if (externalPoint)
result_ = TriangulationResult(*externalPoint);

2
gtsam/slam/SmartProjectionPoseFactor.h
View File

@ -86,7 +86,7 @@ public:
SmartProjectionPoseFactor(
const SharedNoiseModel& sharedNoiseModel,
const boost::shared_ptr<CALIBRATION> K,
const boost::optional<Pose3> body_P_sensor,
const std::optional<Pose3> body_P_sensor,
const SmartProjectionParams& params = SmartProjectionParams())
: SmartProjectionPoseFactor(sharedNoiseModel, K, params) {
this->body_P_sensor_ = body_P_sensor;

7
gtsam/slam/StereoFactor.h
View File

@ -18,6 +18,7 @@
#pragma once
#include <optional>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/geometry/StereoCamera.h>
@ -34,7 +35,7 @@ private:
// Keep a copy of measurement and calibration for I/O
StereoPoint2 measured_; ///< the measurement
Cal3_S2Stereo::shared_ptr K_; ///< shared pointer to calibration
boost::optional<POSE> body_P_sensor_; ///< The pose of the sensor in the body frame
std::optional<POSE> body_P_sensor_; ///< The pose of the sensor in the body frame
// verbosity handling for Cheirality Exceptions
bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false)
@ -68,7 +69,7 @@ public:
*/
GenericStereoFactor(const StereoPoint2& measured, const SharedNoiseModel& model,
Key poseKey, Key landmarkKey, const Cal3_S2Stereo::shared_ptr& K,
boost::optional<POSE> body_P_sensor = boost::none) :
std::optional<POSE> body_P_sensor = {}) :
Base(model, poseKey, landmarkKey), measured_(measured), K_(K), body_P_sensor_(body_P_sensor),
throwCheirality_(false), verboseCheirality_(false) {}
@ -86,7 +87,7 @@ public:
GenericStereoFactor(const StereoPoint2& measured, const SharedNoiseModel& model,
Key poseKey, Key landmarkKey, const Cal3_S2Stereo::shared_ptr& K,
bool throwCheirality, bool verboseCheirality,
boost::optional<POSE> body_P_sensor = boost::none) :
std::optional<POSE> body_P_sensor = {}) :
Base(model, poseKey, landmarkKey), measured_(measured), K_(K), body_P_sensor_(body_P_sensor),
throwCheirality_(throwCheirality), verboseCheirality_(verboseCheirality) {}

52
gtsam/slam/dataset.cpp
View File

@ -43,7 +43,7 @@
#include <boost/filesystem/operations.hpp>
#include <boost/filesystem/path.hpp>
#include <boost/optional.hpp>
#include <optional>
#include <cmath>
#include <fstream>
@ -113,7 +113,7 @@ string createRewrittenFileName(const string &name) {
// Type for parser functions used in parseLines below.
template <typename T>
using Parser =
std::function<boost::optional<T>(istream &is, const string &tag)>;
std::function<std::optional<T>(istream &is, const string &tag)>;
// Parse a file by calling the parse(is, tag) function for every line.
// The result of calling the function is ignored, so typically parse function
@ -141,7 +141,7 @@ map<size_t, T> parseToMap(const string &filename, Parser<pair<size_t, T>> parse,
if (!maxIndex || t->first <= maxIndex)
result.emplace(*t);
}
return boost::none;
return std::nullopt;
};
parseLines(filename, emplace);
return result;
@ -155,14 +155,14 @@ static vector<T> parseToVector(const string &filename, Parser<T> parse) {
Parser<T> add = [&result, parse](istream &is, const string &tag) {
if (auto t = parse(is, tag))
result.push_back(*t);
return boost::none;
return std::nullopt;
};
parseLines(filename, add);
return result;
}
/* ************************************************************************* */
boost::optional<IndexedPose> parseVertexPose(istream &is, const string &tag) {
std::optional<IndexedPose> parseVertexPose(istream &is, const string &tag) {
if ((tag == "VERTEX2") || (tag == "VERTEX_SE2") || (tag == "VERTEX")) {
size_t id;
double x, y, yaw;
@ -171,7 +171,7 @@ boost::optional<IndexedPose> parseVertexPose(istream &is, const string &tag) {
}
return IndexedPose(id, Pose2(x, y, yaw));
} else {
return boost::none;
return std::nullopt;
}
}
@ -182,7 +182,7 @@ GTSAM_EXPORT std::map<size_t, Pose2> parseVariables<Pose2>(
}
/* ************************************************************************* */
boost::optional<IndexedLandmark> parseVertexLandmark(istream &is,
std::optional<IndexedLandmark> parseVertexLandmark(istream &is,
const string &tag) {
if (tag == "VERTEX_XY") {
size_t id;
@ -193,7 +193,7 @@ boost::optional<IndexedLandmark> parseVertexLandmark(istream &is,
}
return IndexedLandmark(id, Point2(x, y));
} else {
return boost::none;
return std::nullopt;
}
}
@ -288,7 +288,7 @@ static SharedNoiseModel createNoiseModel(
}
/* ************************************************************************* */
boost::optional<IndexedEdge> parseEdge(istream &is, const string &tag) {
std::optional<IndexedEdge> parseEdge(istream &is, const string &tag) {
if ((tag == "EDGE2") || (tag == "EDGE") || (tag == "EDGE_SE2") ||
(tag == "ODOMETRY")) {
@ -299,7 +299,7 @@ boost::optional<IndexedEdge> parseEdge(istream &is, const string &tag) {
}
return IndexedEdge({id1, id2}, Pose2(x, y, yaw));
} else {
return boost::none;
return std::nullopt;
}
}
@ -315,13 +315,13 @@ template <typename T> struct ParseFactor : ParseMeasurement<T> {
: ParseMeasurement<T>(parent) {}
// We parse a measurement then convert
typename boost::optional<typename BetweenFactor<T>::shared_ptr>
typename std::optional<typename BetweenFactor<T>::shared_ptr>
operator()(istream &is, const string &tag) {
if (auto m = ParseMeasurement<T>::operator()(is, tag))
return boost::make_shared<BetweenFactor<T>>(
m->key1(), m->key2(), m->measured(), m->noiseModel());
else
return boost::none;
return std::nullopt;
}
};
@ -341,11 +341,11 @@ template <> struct ParseMeasurement<Pose2> {
SharedNoiseModel model;
// The actual parser
boost::optional<BinaryMeasurement<Pose2>> operator()(istream &is,
std::optional<BinaryMeasurement<Pose2>> operator()(istream &is,
const string &tag) {
auto edge = parseEdge(is, tag);
if (!edge)
return boost::none;
return std::nullopt;
// parse noise model
Vector6 v;
@ -355,7 +355,7 @@ template <> struct ParseMeasurement<Pose2> {
size_t id1, id2;
tie(id1, id2) = edge->first;
if (maxIndex && (id1 > maxIndex || id2 > maxIndex))
return boost::none;
return std::nullopt;
// Get pose and optionally add noise
Pose2 &pose = edge->second;
@ -446,10 +446,10 @@ template <> struct ParseMeasurement<BearingRange2D> {
size_t maxIndex;
// The actual parser
boost::optional<BinaryMeasurement<BearingRange2D>>
std::optional<BinaryMeasurement<BearingRange2D>>
operator()(istream &is, const string &tag) {
if (tag != "BR" && tag != "LANDMARK")
return boost::none;
return std::nullopt;
size_t id1, id2;
is >> id1 >> id2;
@ -485,7 +485,7 @@ template <> struct ParseMeasurement<BearingRange2D> {
// optional filter
if (maxIndex && id1 > maxIndex)
return boost::none;
return std::nullopt;
// Create noise model
auto measurementNoise = noiseModel::Diagonal::Sigmas(
@ -759,7 +759,7 @@ istream &operator>>(istream &is, Rot3 &R) {
}
/* ************************************************************************* */
boost::optional<pair<size_t, Pose3>> parseVertexPose3(istream &is,
std::optional<pair<size_t, Pose3>> parseVertexPose3(istream &is,
const string &tag) {
if (tag == "VERTEX3") {
size_t id;
@ -774,7 +774,7 @@ boost::optional<pair<size_t, Pose3>> parseVertexPose3(istream &is,
is >> id >> x >> y >> z >> q;
return make_pair(id, Pose3(q, {x, y, z}));
} else
return boost::none;
return std::nullopt;
}
template <>
@ -784,7 +784,7 @@ GTSAM_EXPORT std::map<size_t, Pose3> parseVariables<Pose3>(
}
/* ************************************************************************* */
boost::optional<pair<size_t, Point3>> parseVertexPoint3(istream &is,
std::optional<pair<size_t, Point3>> parseVertexPoint3(istream &is,
const string &tag) {
if (tag == "VERTEX_TRACKXYZ") {
size_t id;
@ -792,7 +792,7 @@ boost::optional<pair<size_t, Point3>> parseVertexPoint3(istream &is,
is >> id >> x >> y >> z;
return make_pair(id, Point3(x, y, z));
} else
return boost::none;
return std::nullopt;
}
template <>
@ -820,16 +820,16 @@ template <> struct ParseMeasurement<Pose3> {
size_t maxIndex;
// The actual parser
boost::optional<BinaryMeasurement<Pose3>> operator()(istream &is,
std::optional<BinaryMeasurement<Pose3>> operator()(istream &is,
const string &tag) {
if (tag != "EDGE3" && tag != "EDGE_SE3:QUAT")
return boost::none;
return std::nullopt;
// parse ids and optionally filter
size_t id1, id2;
is >> id1 >> id2;
if (maxIndex && (id1 > maxIndex || id2 > maxIndex))
return boost::none;
return std::nullopt;
Matrix6 m;
if (tag == "EDGE3") {
@ -864,7 +864,7 @@ template <> struct ParseMeasurement<Pose3> {
return BinaryMeasurement<Pose3>(
id1, id2, T12, noiseModel::Gaussian::Information(mgtsam));
} else
return boost::none;
return std::nullopt;
}
};

9
gtsam/slam/dataset.h
View File

@ -40,6 +40,7 @@
#include <vector>
#include <iosfwd>
#include <map>
#include <optional>
namespace gtsam {
@ -118,7 +119,7 @@ typedef std::pair<std::pair<size_t, size_t>, Pose2> IndexedEdge;
* @param is input stream
* @param tag string parsed from input stream, will only parse if vertex type
*/
GTSAM_EXPORT boost::optional<IndexedPose> parseVertexPose(std::istream& is,
GTSAM_EXPORT std::optional<IndexedPose> parseVertexPose(std::istream& is,
const std::string& tag);
/**
@ -126,7 +127,7 @@ GTSAM_EXPORT boost::optional<IndexedPose> parseVertexPose(std::istream& is,
* @param is input stream
* @param tag string parsed from input stream, will only parse if vertex type
*/
GTSAM_EXPORT boost::optional<IndexedLandmark> parseVertexLandmark(std::istream& is,
GTSAM_EXPORT std::optional<IndexedLandmark> parseVertexLandmark(std::istream& is,
const std::string& tag);
/**
@ -134,7 +135,7 @@ GTSAM_EXPORT boost::optional<IndexedLandmark> parseVertexLandmark(std::istream&
* @param is input stream
* @param tag string parsed from input stream, will only parse if edge type
*/
GTSAM_EXPORT boost::optional<IndexedEdge> parseEdge(std::istream& is,
GTSAM_EXPORT std::optional<IndexedEdge> parseEdge(std::istream& is,
const std::string& tag);
/// Return type for load functions, which return a graph and initial values. For
@ -227,7 +228,7 @@ using BinaryMeasurementsPoint3 = std::vector<BinaryMeasurement<Point3>>;
using BinaryMeasurementsRot3 = std::vector<BinaryMeasurement<Rot3>>;
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
inline boost::optional<IndexedPose> GTSAM_DEPRECATED
inline std::optional<IndexedPose> GTSAM_DEPRECATED
parseVertex(std::istream& is, const std::string& tag) {
return parseVertexPose(is, tag);
}

2
gtsam/slam/tests/PinholeFactor.h
View File

@ -35,7 +35,7 @@ class PinholeFactor : public SmartFactorBase<PinholeCamera<Cal3Bundler> > {
typedef SmartFactorBase<PinholeCamera<Cal3Bundler> > Base;
PinholeFactor() {}
PinholeFactor(const SharedNoiseModel& sharedNoiseModel,
boost::optional<Pose3> body_P_sensor = boost::none,
std::optional<Pose3> body_P_sensor = {},
size_t expectedNumberCameras = 10)
: Base(sharedNoiseModel, body_P_sensor, expectedNumberCameras) {}
double error(const Values& values) const override { return 0.0; }

1
gtsam/slam/tests/testSerializationInSlam.cpp
View File

@ -22,6 +22,7 @@
#include <gtsam/slam/SmartProjectionFactor.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/base/serializationTestHelpers.h>
#include <gtsam/base/std_optional_serialization.h>
#include <CppUnitLite/TestHarness.h>

4
gtsam/slam/tests/testSmartProjectionPoseFactor.cpp
View File

@ -138,7 +138,7 @@ TEST( SmartProjectionPoseFactor, noiseless ) {
factor.triangulateAndComputeE(actualE, values);
// get point
boost::optional<Point3> point = factor.point();
auto point = factor.point();
CHECK(point);
// calculate numerical derivative with triangulated point
@ -366,7 +366,7 @@ TEST( SmartProjectionPoseFactor, Factors ) {
CHECK(smartFactor1->triangulateSafe(cameras));
CHECK(!smartFactor1->isDegenerate());
CHECK(!smartFactor1->isPointBehindCamera());
boost::optional<Point3> p = smartFactor1->point();
auto p = smartFactor1->point();
CHECK(p);
EXPECT(assert_equal(landmark1, *p));

4
gtsam/slam/tests/testSmartProjectionRigFactor.cpp
View File

@ -166,7 +166,7 @@ TEST(SmartProjectionRigFactor, noiseless) {
factor.triangulateAndComputeE(actualE, values);
// get point
boost::optional<Point3> point = factor.point();
auto point = factor.point();
CHECK(point);
// calculate numerical derivative with triangulated point
@ -508,7 +508,7 @@ TEST(SmartProjectionRigFactor, Factors) {
CHECK(smartFactor1->triangulateSafe(cameras));
CHECK(!smartFactor1->isDegenerate());
CHECK(!smartFactor1->isPointBehindCamera());
boost::optional<Point3> p = smartFactor1->point();
auto p = smartFactor1->point();
CHECK(p);
EXPECT(assert_equal(landmark1, *p));

6
gtsam_unstable/slam/SmartStereoProjectionFactor.h
View File

@ -32,6 +32,8 @@
#include <boost/make_shared.hpp>
#include <boost/optional.hpp>
#include <boost/serialization/optional.hpp>
#include <optional>
#include <vector>
namespace gtsam {
@ -87,7 +89,7 @@ public:
*/
SmartStereoProjectionFactor(const SharedNoiseModel& sharedNoiseModel,
const SmartStereoProjectionParams& params = SmartStereoProjectionParams(),
const boost::optional<Pose3> body_P_sensor = boost::none) :
const std::optional<Pose3> body_P_sensor = {}) :
Base(sharedNoiseModel, body_P_sensor), //
params_(params), //
result_(TriangulationResult::Degenerate()) {
@ -415,7 +417,7 @@ public:
* to transform it to \f$ (h(x)-z)^2/\sigma^2 \f$, and then multiply by 0.5.
*/
double totalReprojectionError(const Cameras& cameras,
boost::optional<Point3> externalPoint = boost::none) const {
std::optional<Point3> externalPoint = {}) const {
if (externalPoint)
result_ = TriangulationResult(*externalPoint);

2
gtsam_unstable/slam/SmartStereoProjectionPoseFactor.cpp
View File

@ -26,7 +26,7 @@ namespace gtsam {
SmartStereoProjectionPoseFactor::SmartStereoProjectionPoseFactor(
const SharedNoiseModel& sharedNoiseModel,
const SmartStereoProjectionParams& params,
const boost::optional<Pose3>& body_P_sensor)
const std::optional<Pose3>& body_P_sensor)
: Base(sharedNoiseModel, params, body_P_sensor) {}
void SmartStereoProjectionPoseFactor::add(

2
gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h
View File

@ -69,7 +69,7 @@ class GTSAM_UNSTABLE_EXPORT SmartStereoProjectionPoseFactor
SmartStereoProjectionPoseFactor(
const SharedNoiseModel& sharedNoiseModel,
const SmartStereoProjectionParams& params = SmartStereoProjectionParams(),
const boost::optional<Pose3>& body_P_sensor = boost::none);
const std::optional<Pose3>& body_P_sensor = {});
/** Virtual destructor */
~SmartStereoProjectionPoseFactor() override = default;

1
tests/testSerializationSlam.cpp
View File

@ -50,6 +50,7 @@
#include <gtsam/linear/GaussianISAM.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/base/serializationTestHelpers.h>
#include <gtsam/base/std_optional_serialization.h>
#include <boost/archive/xml_iarchive.hpp>
#include <boost/serialization/export.hpp>