From 610a73cfe1dc1e95b4e85b9d12c2af5e88f53f96 Mon Sep 17 00:00:00 2001 From: kartik arcot Date: Wed, 4 Jan 2023 13:29:44 -0800 Subject: [PATCH 01/38] compile definitions for conditionally compiling --- CMakeLists.txt | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 9be0d89a9..21f8b28a1 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -6,7 +6,11 @@ if(NOT DEFINED CMAKE_MACOSX_RPATH) set(CMAKE_MACOSX_RPATH 0) endif() -set(CMAKE_CXX_STANDARD 17) +option(GTSAM_NO_BOOST_CPP17 "Require and use boost" ON) + +if (GTSAM_NO_BOOST_CPP17) + set(CMAKE_CXX_STANDARD 17) +endif() # Set the version number for the library set (GTSAM_VERSION_MAJOR 4) @@ -53,7 +57,11 @@ endif() include(cmake/HandleGeneralOptions.cmake) # CMake build options # Libraries: -include(cmake/HandleBoost.cmake) # Boost +if (GTSAM_NO_BOOST_CPP17) + include(cmake/HandleBoost.cmake) # Boost + add_definitions(-DNO_BOOST_CPP17) +endif() + include(cmake/HandleCCache.cmake) # ccache include(cmake/HandleCPack.cmake) # CPack include(cmake/HandleEigen.cmake) # Eigen3 From d338a7086bd41380f3d050e94cbae49909b9b24b Mon Sep 17 00:00:00 2001 From: kartik arcot Date: Wed, 11 Jan 2023 13:33:25 -0800 Subject: [PATCH 02/38] slam folder. serialize std::optional --- gtsam/base/std_optional_serialization.h | 79 +++++++++++++++++++ gtsam/base/tests/testSerializationBase.cpp | 1 + gtsam/slam/KarcherMeanFactor-inl.h | 5 +- gtsam/slam/KarcherMeanFactor.h | 3 +- gtsam/slam/ProjectionFactor.h | 10 +-- gtsam/slam/SmartFactorBase.h | 5 +- gtsam/slam/SmartProjectionFactor.h | 4 +- gtsam/slam/SmartProjectionPoseFactor.h | 2 +- gtsam/slam/StereoFactor.h | 7 +- gtsam/slam/dataset.cpp | 52 ++++++------ gtsam/slam/dataset.h | 9 ++- gtsam/slam/tests/PinholeFactor.h | 2 +- gtsam/slam/tests/testSerializationInSlam.cpp | 1 + .../tests/testSmartProjectionPoseFactor.cpp | 4 +- .../tests/testSmartProjectionRigFactor.cpp | 4 +- .../slam/SmartStereoProjectionFactor.h | 6 +- .../slam/SmartStereoProjectionPoseFactor.cpp | 2 +- .../slam/SmartStereoProjectionPoseFactor.h | 2 +- tests/testSerializationSlam.cpp | 1 + 19 files changed, 144 insertions(+), 55 deletions(-) create mode 100644 gtsam/base/std_optional_serialization.h diff --git a/gtsam/base/std_optional_serialization.h b/gtsam/base/std_optional_serialization.h new file mode 100644 index 000000000..3dac79284 --- /dev/null +++ b/gtsam/base/std_optional_serialization.h @@ -0,0 +1,79 @@ +// A hack to serialize std::optional to boost::archive +// Don't know if it will work. Trying to follow this: +// PR: https://github.com/boostorg/serialization/pull/148/files# +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +// function specializations must be defined in the appropriate +// namespace - boost::serialization +namespace boost { +namespace serialization { + +template +void save(Archive& ar, const std::optional& 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 as optional + BOOST_STATIC_ASSERT(boost::serialization::detail::is_default_constructible::value || boost::is_pointer::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::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 +void load(Archive& ar, std::optional& 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 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 +void serialize(Archive& ar, std::optional& t, const unsigned int version) { + boost::serialization::split_free(ar, t, version); +} + +} // namespace serialization +} // namespace boost + diff --git a/gtsam/base/tests/testSerializationBase.cpp b/gtsam/base/tests/testSerializationBase.cpp index d2f4d5f51..9387d9b59 100644 --- a/gtsam/base/tests/testSerializationBase.cpp +++ b/gtsam/base/tests/testSerializationBase.cpp @@ -28,6 +28,7 @@ #include #include +#include #include using namespace std; diff --git a/gtsam/slam/KarcherMeanFactor-inl.h b/gtsam/slam/KarcherMeanFactor-inl.h index 00f741705..e714f653b 100644 --- a/gtsam/slam/KarcherMeanFactor-inl.h +++ b/gtsam/slam/KarcherMeanFactor-inl.h @@ -20,6 +20,7 @@ #include #include #include +#include using namespace std; @@ -58,7 +59,7 @@ T FindKarcherMean(std::initializer_list&& rotations) { template template KarcherMeanFactor::KarcherMeanFactor(const CONTAINER &keys, int d, - boost::optional beta) + std::optional beta) : NonlinearFactor(keys), d_(static_cast(d)) { if (d <= 0) { throw std::invalid_argument( @@ -75,4 +76,4 @@ KarcherMeanFactor::KarcherMeanFactor(const CONTAINER &keys, int d, whitenedJacobian_ = boost::make_shared(terms, Vector::Zero(d)); } -} // namespace gtsam \ No newline at end of file +} // namespace gtsam diff --git a/gtsam/slam/KarcherMeanFactor.h b/gtsam/slam/KarcherMeanFactor.h index 964cd598f..2b230c2f2 100644 --- a/gtsam/slam/KarcherMeanFactor.h +++ b/gtsam/slam/KarcherMeanFactor.h @@ -22,6 +22,7 @@ #include #include +#include namespace gtsam { /** @@ -60,7 +61,7 @@ public: */ template KarcherMeanFactor(const CONTAINER &keys, int d = D, - boost::optional beta = boost::none); + std::optional beta = {}); /// Destructor ~KarcherMeanFactor() override {} diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h index b263b4de6..3b60c8a5a 100644 --- a/gtsam/slam/ProjectionFactor.h +++ b/gtsam/slam/ProjectionFactor.h @@ -25,7 +25,7 @@ #include #include #include -#include +#include 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 K_; ///< shared pointer to calibration object - boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame + std::optional 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& K, - boost::optional body_P_sensor = boost::none) : + std::optional 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& K, bool throwCheirality, bool verboseCheirality, - boost::optional body_P_sensor = boost::none) : + std::optional 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& body_P_sensor() const { + const std::optional& body_P_sensor() const { return body_P_sensor_; } diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index b53235ab3..09627a68f 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -30,6 +30,7 @@ #include #include +#include #include #include #include @@ -78,7 +79,7 @@ protected: */ ZVector measured_; - boost::optional + std::optional 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 body_P_sensor = boost::none, + std::optional body_P_sensor = {}, size_t expectedNumberCameras = 10) : body_P_sensor_(body_P_sensor), Fs(expectedNumberCameras) { diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index d09f42509..a38a8bd88 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -26,7 +26,7 @@ #include #include -#include +#include #include #include @@ -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 externalPoint = boost::none) const { + std::optional externalPoint = {}) const { if (externalPoint) result_ = TriangulationResult(*externalPoint); diff --git a/gtsam/slam/SmartProjectionPoseFactor.h b/gtsam/slam/SmartProjectionPoseFactor.h index e432cbfb1..ce84babed 100644 --- a/gtsam/slam/SmartProjectionPoseFactor.h +++ b/gtsam/slam/SmartProjectionPoseFactor.h @@ -86,7 +86,7 @@ public: SmartProjectionPoseFactor( const SharedNoiseModel& sharedNoiseModel, const boost::shared_ptr K, - const boost::optional body_P_sensor, + const std::optional body_P_sensor, const SmartProjectionParams& params = SmartProjectionParams()) : SmartProjectionPoseFactor(sharedNoiseModel, K, params) { this->body_P_sensor_ = body_P_sensor; diff --git a/gtsam/slam/StereoFactor.h b/gtsam/slam/StereoFactor.h index c0e3fd1b3..d52860776 100644 --- a/gtsam/slam/StereoFactor.h +++ b/gtsam/slam/StereoFactor.h @@ -18,6 +18,7 @@ #pragma once +#include #include #include @@ -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 body_P_sensor_; ///< The pose of the sensor in the body frame + std::optional 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 body_P_sensor = boost::none) : + std::optional 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 body_P_sensor = boost::none) : + std::optional body_P_sensor = {}) : Base(model, poseKey, landmarkKey), measured_(measured), K_(K), body_P_sensor_(body_P_sensor), throwCheirality_(throwCheirality), verboseCheirality_(verboseCheirality) {} diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index db79fcd3c..cfd7b8820 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -43,7 +43,7 @@ #include #include -#include +#include #include #include @@ -113,7 +113,7 @@ string createRewrittenFileName(const string &name) { // Type for parser functions used in parseLines below. template using Parser = - std::function(istream &is, const string &tag)>; + std::function(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 parseToMap(const string &filename, Parser> 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 parseToVector(const string &filename, Parser parse) { Parser 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 parseVertexPose(istream &is, const string &tag) { +std::optional 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 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 parseVariables( } /* ************************************************************************* */ -boost::optional parseVertexLandmark(istream &is, +std::optional parseVertexLandmark(istream &is, const string &tag) { if (tag == "VERTEX_XY") { size_t id; @@ -193,7 +193,7 @@ boost::optional 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 parseEdge(istream &is, const string &tag) { +std::optional parseEdge(istream &is, const string &tag) { if ((tag == "EDGE2") || (tag == "EDGE") || (tag == "EDGE_SE2") || (tag == "ODOMETRY")) { @@ -299,7 +299,7 @@ boost::optional 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 struct ParseFactor : ParseMeasurement { : ParseMeasurement(parent) {} // We parse a measurement then convert - typename boost::optional::shared_ptr> + typename std::optional::shared_ptr> operator()(istream &is, const string &tag) { if (auto m = ParseMeasurement::operator()(is, tag)) return boost::make_shared>( m->key1(), m->key2(), m->measured(), m->noiseModel()); else - return boost::none; + return std::nullopt; } }; @@ -341,11 +341,11 @@ template <> struct ParseMeasurement { SharedNoiseModel model; // The actual parser - boost::optional> operator()(istream &is, + std::optional> 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 { 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 { size_t maxIndex; // The actual parser - boost::optional> + std::optional> 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 { // 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> parseVertexPose3(istream &is, +std::optional> parseVertexPose3(istream &is, const string &tag) { if (tag == "VERTEX3") { size_t id; @@ -774,7 +774,7 @@ boost::optional> 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 parseVariables( } /* ************************************************************************* */ -boost::optional> parseVertexPoint3(istream &is, +std::optional> parseVertexPoint3(istream &is, const string &tag) { if (tag == "VERTEX_TRACKXYZ") { size_t id; @@ -792,7 +792,7 @@ boost::optional> 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 { size_t maxIndex; // The actual parser - boost::optional> operator()(istream &is, + std::optional> 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 { return BinaryMeasurement( id1, id2, T12, noiseModel::Gaussian::Information(mgtsam)); } else - return boost::none; + return std::nullopt; } }; diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 95e750674..875f23306 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -40,6 +40,7 @@ #include #include #include +#include namespace gtsam { @@ -118,7 +119,7 @@ typedef std::pair, Pose2> IndexedEdge; * @param is input stream * @param tag string parsed from input stream, will only parse if vertex type */ -GTSAM_EXPORT boost::optional parseVertexPose(std::istream& is, +GTSAM_EXPORT std::optional parseVertexPose(std::istream& is, const std::string& tag); /** @@ -126,7 +127,7 @@ GTSAM_EXPORT boost::optional 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 parseVertexLandmark(std::istream& is, +GTSAM_EXPORT std::optional parseVertexLandmark(std::istream& is, const std::string& tag); /** @@ -134,7 +135,7 @@ GTSAM_EXPORT boost::optional parseVertexLandmark(std::istream& * @param is input stream * @param tag string parsed from input stream, will only parse if edge type */ -GTSAM_EXPORT boost::optional parseEdge(std::istream& is, +GTSAM_EXPORT std::optional 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>; using BinaryMeasurementsRot3 = std::vector>; #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 -inline boost::optional GTSAM_DEPRECATED +inline std::optional GTSAM_DEPRECATED parseVertex(std::istream& is, const std::string& tag) { return parseVertexPose(is, tag); } diff --git a/gtsam/slam/tests/PinholeFactor.h b/gtsam/slam/tests/PinholeFactor.h index 35500ca35..7266a4aea 100644 --- a/gtsam/slam/tests/PinholeFactor.h +++ b/gtsam/slam/tests/PinholeFactor.h @@ -35,7 +35,7 @@ class PinholeFactor : public SmartFactorBase > { typedef SmartFactorBase > Base; PinholeFactor() {} PinholeFactor(const SharedNoiseModel& sharedNoiseModel, - boost::optional body_P_sensor = boost::none, + std::optional body_P_sensor = {}, size_t expectedNumberCameras = 10) : Base(sharedNoiseModel, body_P_sensor, expectedNumberCameras) {} double error(const Values& values) const override { return 0.0; } diff --git a/gtsam/slam/tests/testSerializationInSlam.cpp b/gtsam/slam/tests/testSerializationInSlam.cpp index 08fb80c0a..f729f8ed6 100644 --- a/gtsam/slam/tests/testSerializationInSlam.cpp +++ b/gtsam/slam/tests/testSerializationInSlam.cpp @@ -22,6 +22,7 @@ #include #include #include +#include #include diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index ba5a53bbb..2eb799663 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -138,7 +138,7 @@ TEST( SmartProjectionPoseFactor, noiseless ) { factor.triangulateAndComputeE(actualE, values); // get point - boost::optional 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 p = smartFactor1->point(); + auto p = smartFactor1->point(); CHECK(p); EXPECT(assert_equal(landmark1, *p)); diff --git a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp index 6d39eb5b0..2fa6e7290 100644 --- a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp @@ -166,7 +166,7 @@ TEST(SmartProjectionRigFactor, noiseless) { factor.triangulateAndComputeE(actualE, values); // get point - boost::optional 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 p = smartFactor1->point(); + auto p = smartFactor1->point(); CHECK(p); EXPECT(assert_equal(landmark1, *p)); diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index a9caf0c4c..9ebc9ba10 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -32,6 +32,8 @@ #include #include +#include +#include #include namespace gtsam { @@ -87,7 +89,7 @@ public: */ SmartStereoProjectionFactor(const SharedNoiseModel& sharedNoiseModel, const SmartStereoProjectionParams& params = SmartStereoProjectionParams(), - const boost::optional body_P_sensor = boost::none) : + const std::optional 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 externalPoint = boost::none) const { + std::optional externalPoint = {}) const { if (externalPoint) result_ = TriangulationResult(*externalPoint); diff --git a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.cpp b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.cpp index c28cb25a1..4f6ef18ab 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.cpp +++ b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.cpp @@ -26,7 +26,7 @@ namespace gtsam { SmartStereoProjectionPoseFactor::SmartStereoProjectionPoseFactor( const SharedNoiseModel& sharedNoiseModel, const SmartStereoProjectionParams& params, - const boost::optional& body_P_sensor) + const std::optional& body_P_sensor) : Base(sharedNoiseModel, params, body_P_sensor) {} void SmartStereoProjectionPoseFactor::add( diff --git a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h index ad09cd207..7295413ca 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h @@ -69,7 +69,7 @@ class GTSAM_UNSTABLE_EXPORT SmartStereoProjectionPoseFactor SmartStereoProjectionPoseFactor( const SharedNoiseModel& sharedNoiseModel, const SmartStereoProjectionParams& params = SmartStereoProjectionParams(), - const boost::optional& body_P_sensor = boost::none); + const std::optional& body_P_sensor = {}); /** Virtual destructor */ ~SmartStereoProjectionPoseFactor() override = default; diff --git a/tests/testSerializationSlam.cpp b/tests/testSerializationSlam.cpp index ea7038635..f986d812a 100644 --- a/tests/testSerializationSlam.cpp +++ b/tests/testSerializationSlam.cpp @@ -50,6 +50,7 @@ #include #include #include +#include #include #include From 0873ad628e399993bf1bc64b3c02dcd453d660c8 Mon Sep 17 00:00:00 2001 From: kartik arcot Date: Wed, 11 Jan 2023 13:45:45 -0800 Subject: [PATCH 03/38] hybrid folder --- gtsam/hybrid/HybridGaussianFactorGraph.h | 2 ++ gtsam/hybrid/HybridGaussianISAM.cpp | 8 ++++---- gtsam/hybrid/HybridGaussianISAM.h | 10 ++++++---- gtsam/hybrid/HybridNonlinearISAM.cpp | 6 +++--- gtsam/hybrid/HybridNonlinearISAM.h | 5 +++-- gtsam/hybrid/HybridSmoother.cpp | 2 +- gtsam/hybrid/HybridSmoother.h | 4 +++- gtsam/hybrid/tests/TinyHybridExample.h | 2 +- 8 files changed, 23 insertions(+), 16 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.h b/gtsam/hybrid/HybridGaussianFactorGraph.h index 0db4f734b..9724fccc5 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.h +++ b/gtsam/hybrid/HybridGaussianFactorGraph.h @@ -27,6 +27,8 @@ #include #include +#include + namespace gtsam { // Forward declarations diff --git a/gtsam/hybrid/HybridGaussianISAM.cpp b/gtsam/hybrid/HybridGaussianISAM.cpp index 3f63cb089..086127f16 100644 --- a/gtsam/hybrid/HybridGaussianISAM.cpp +++ b/gtsam/hybrid/HybridGaussianISAM.cpp @@ -71,8 +71,8 @@ Ordering HybridGaussianISAM::GetOrdering( void HybridGaussianISAM::updateInternal( const HybridGaussianFactorGraph& newFactors, HybridBayesTree::Cliques* orphans, - const boost::optional& maxNrLeaves, - const boost::optional& ordering, + const std::optional& maxNrLeaves, + const std::optional& ordering, const HybridBayesTree::Eliminate& function) { // Remove the contaminated part of the Bayes tree BayesNetType bn; @@ -116,8 +116,8 @@ void HybridGaussianISAM::updateInternal( /* ************************************************************************* */ void HybridGaussianISAM::update(const HybridGaussianFactorGraph& newFactors, - const boost::optional& maxNrLeaves, - const boost::optional& ordering, + const std::optional& maxNrLeaves, + const std::optional& ordering, const HybridBayesTree::Eliminate& function) { Cliques orphans; this->updateInternal(newFactors, &orphans, maxNrLeaves, ordering, function); diff --git a/gtsam/hybrid/HybridGaussianISAM.h b/gtsam/hybrid/HybridGaussianISAM.h index 5e936c47b..b353b26a3 100644 --- a/gtsam/hybrid/HybridGaussianISAM.h +++ b/gtsam/hybrid/HybridGaussianISAM.h @@ -24,6 +24,8 @@ #include #include +#include + namespace gtsam { /** @@ -53,8 +55,8 @@ class GTSAM_EXPORT HybridGaussianISAM : public ISAM { void updateInternal( const HybridGaussianFactorGraph& newFactors, HybridBayesTree::Cliques* orphans, - const boost::optional& maxNrLeaves = boost::none, - const boost::optional& ordering = boost::none, + const std::optional& maxNrLeaves = {}, + const std::optional& ordering = {}, const HybridBayesTree::Eliminate& function = HybridBayesTree::EliminationTraitsType::DefaultEliminate); @@ -68,8 +70,8 @@ class GTSAM_EXPORT HybridGaussianISAM : public ISAM { * @param function Elimination function. */ void update(const HybridGaussianFactorGraph& newFactors, - const boost::optional& maxNrLeaves = boost::none, - const boost::optional& ordering = boost::none, + const std::optional& maxNrLeaves = {}, + const std::optional& ordering = {}, const HybridBayesTree::Eliminate& function = HybridBayesTree::EliminationTraitsType::DefaultEliminate); diff --git a/gtsam/hybrid/HybridNonlinearISAM.cpp b/gtsam/hybrid/HybridNonlinearISAM.cpp index b7a5a8eeb..9a6d25ade 100644 --- a/gtsam/hybrid/HybridNonlinearISAM.cpp +++ b/gtsam/hybrid/HybridNonlinearISAM.cpp @@ -34,8 +34,8 @@ void HybridNonlinearISAM::saveGraph(const string& s, /* ************************************************************************* */ void HybridNonlinearISAM::update(const HybridNonlinearFactorGraph& newFactors, const Values& initialValues, - const boost::optional& maxNrLeaves, - const boost::optional& ordering) { + const std::optional& maxNrLeaves, + const std::optional& ordering) { if (newFactors.size() > 0) { // Reorder and relinearize every reorderInterval updates if (reorderInterval_ > 0 && ++reorderCounter_ >= reorderInterval_) { @@ -70,7 +70,7 @@ void HybridNonlinearISAM::reorder_relinearize() { // Just recreate the whole BayesTree // TODO: allow for constrained ordering here // TODO: decouple relinearization and reordering to avoid - isam_.update(*factors_.linearize(newLinPoint), boost::none, boost::none, + isam_.update(*factors_.linearize(newLinPoint), {}, {}, eliminationFunction_); // Update linearization point diff --git a/gtsam/hybrid/HybridNonlinearISAM.h b/gtsam/hybrid/HybridNonlinearISAM.h index 53bacb0ff..3372593be 100644 --- a/gtsam/hybrid/HybridNonlinearISAM.h +++ b/gtsam/hybrid/HybridNonlinearISAM.h @@ -19,6 +19,7 @@ #include #include +#include namespace gtsam { /** @@ -119,8 +120,8 @@ class GTSAM_EXPORT HybridNonlinearISAM { /** Add new factors along with their initial linearization points */ void update(const HybridNonlinearFactorGraph& newFactors, const Values& initialValues, - const boost::optional& maxNrLeaves = boost::none, - const boost::optional& ordering = boost::none); + const std::optional& maxNrLeaves = {}, + const std::optional& ordering = {}); /** Relinearization and reordering of variables */ void reorder_relinearize(); diff --git a/gtsam/hybrid/HybridSmoother.cpp b/gtsam/hybrid/HybridSmoother.cpp index 35dd5f88b..7e8a5f4d6 100644 --- a/gtsam/hybrid/HybridSmoother.cpp +++ b/gtsam/hybrid/HybridSmoother.cpp @@ -26,7 +26,7 @@ namespace gtsam { /* ************************************************************************* */ void HybridSmoother::update(HybridGaussianFactorGraph graph, const Ordering &ordering, - boost::optional maxNrLeaves) { + std::optional maxNrLeaves) { // Add the necessary conditionals from the previous timestep(s). std::tie(graph, hybridBayesNet_) = addConditionals(graph, hybridBayesNet_, ordering); diff --git a/gtsam/hybrid/HybridSmoother.h b/gtsam/hybrid/HybridSmoother.h index 7e90f9425..00dfc4685 100644 --- a/gtsam/hybrid/HybridSmoother.h +++ b/gtsam/hybrid/HybridSmoother.h @@ -20,6 +20,8 @@ #include #include +#include + namespace gtsam { class HybridSmoother { @@ -48,7 +50,7 @@ class HybridSmoother { * if applicable */ void update(HybridGaussianFactorGraph graph, const Ordering& ordering, - boost::optional maxNrLeaves = boost::none); + std::optional maxNrLeaves = {}); /** * @brief Add conditionals from previous timestep as part of liquefication. diff --git a/gtsam/hybrid/tests/TinyHybridExample.h b/gtsam/hybrid/tests/TinyHybridExample.h index 0ef8955c4..39a1a1a9e 100644 --- a/gtsam/hybrid/tests/TinyHybridExample.h +++ b/gtsam/hybrid/tests/TinyHybridExample.h @@ -71,7 +71,7 @@ inline HybridBayesNet createHybridBayesNet(size_t num_measurements = 1, */ inline HybridGaussianFactorGraph createHybridGaussianFactorGraph( size_t num_measurements = 1, - boost::optional measurements = boost::none, + std::optional measurements = {}, bool manyModes = false) { auto bayesNet = createHybridBayesNet(num_measurements, manyModes); if (measurements) { From 236c02eb52166508ece25f22b4b088fd3e7f5e40 Mon Sep 17 00:00:00 2001 From: kartik arcot Date: Wed, 11 Jan 2023 13:49:07 -0800 Subject: [PATCH 04/38] examples folder --- examples/ISAM2Example_SmartFactor.cpp | 2 +- examples/SFMExample_SmartFactor.cpp | 2 +- examples/TriangulationLOSTExample.cpp | 6 +++--- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/examples/ISAM2Example_SmartFactor.cpp b/examples/ISAM2Example_SmartFactor.cpp index 7dbd8323b..a874efc9a 100644 --- a/examples/ISAM2Example_SmartFactor.cpp +++ b/examples/ISAM2Example_SmartFactor.cpp @@ -105,7 +105,7 @@ int main(int argc, char* argv[]) { Values currentEstimate = isam.calculateEstimate(); currentEstimate.print("Current estimate: "); - boost::optional pointEstimate = smartFactor->point(currentEstimate); + auto pointEstimate = smartFactor->point(currentEstimate); if (pointEstimate) { cout << *pointEstimate << endl; } else { diff --git a/examples/SFMExample_SmartFactor.cpp b/examples/SFMExample_SmartFactor.cpp index 86e6b8ae9..9d90cd897 100644 --- a/examples/SFMExample_SmartFactor.cpp +++ b/examples/SFMExample_SmartFactor.cpp @@ -116,7 +116,7 @@ int main(int argc, char* argv[]) { if (smart) { // The output of point() is in boost::optional, as sometimes // the triangulation operation inside smart factor will encounter degeneracy. - boost::optional point = smart->point(result); + auto point = smart->point(result); if (point) // ignore if boost::optional return nullptr landmark_result.insert(j, *point); } diff --git a/examples/TriangulationLOSTExample.cpp b/examples/TriangulationLOSTExample.cpp index a862026e0..923606995 100644 --- a/examples/TriangulationLOSTExample.cpp +++ b/examples/TriangulationLOSTExample.cpp @@ -131,17 +131,17 @@ int main(int argc, char* argv[]) { AddNoiseToMeasurements(measurements, measurementSigma); auto lostStart = std::chrono::high_resolution_clock::now(); - boost::optional estimateLOST = triangulatePoint3( + auto estimateLOST = triangulatePoint3( cameras, noisyMeasurements, rank_tol, false, measurementNoise, true); durationLOST += std::chrono::high_resolution_clock::now() - lostStart; auto dltStart = std::chrono::high_resolution_clock::now(); - boost::optional estimateDLT = triangulatePoint3( + auto estimateDLT = triangulatePoint3( cameras, noisyMeasurements, rank_tol, false, measurementNoise, false); durationDLT += std::chrono::high_resolution_clock::now() - dltStart; auto dltOptStart = std::chrono::high_resolution_clock::now(); - boost::optional estimateDLTOpt = triangulatePoint3( + auto estimateDLTOpt = triangulatePoint3( cameras, noisyMeasurements, rank_tol, true, measurementNoise, false); durationDLTOpt += std::chrono::high_resolution_clock::now() - dltOptStart; From 18562e97aeb31e5503a14826558f800e180f0b8a Mon Sep 17 00:00:00 2001 From: kartik arcot Date: Wed, 11 Jan 2023 14:58:19 -0800 Subject: [PATCH 05/38] testable assertions work --- gtsam/base/TestableAssertions.h | 20 ++++++-------------- gtsam/nonlinear/LinearContainerFactor.cpp | 4 ++-- gtsam/nonlinear/LinearContainerFactor.h | 11 +++++++---- 3 files changed, 15 insertions(+), 20 deletions(-) diff --git a/gtsam/base/TestableAssertions.h b/gtsam/base/TestableAssertions.h index e5bd34d19..0bf3cf21f 100644 --- a/gtsam/base/TestableAssertions.h +++ b/gtsam/base/TestableAssertions.h @@ -20,7 +20,7 @@ #include #include -#include +#include #include #include #include @@ -40,15 +40,15 @@ inline bool assert_equal(const Key& expected, const Key& actual, double tol = 0. } /** - * Comparisons for boost.optional objects that checks whether objects exist + * Comparisons for std.optional objects that checks whether objects exist * before comparing their values. First version allows for both to be - * boost::none, but the second, with expected given rather than optional + * std::nullopt, but the second, with expected given rather than optional * * Concept requirement: V is testable */ template -bool assert_equal(const boost::optional& expected, - const boost::optional& actual, double tol = 1e-9) { +bool assert_equal(const std::optional& expected, + const std::optional& actual, double tol = 1e-9) { if (!expected && actual) { std::cout << "expected is boost::none, while actual is not" << std::endl; return false; @@ -63,7 +63,7 @@ bool assert_equal(const boost::optional& expected, } template -bool assert_equal(const V& expected, const boost::optional& actual, double tol = 1e-9) { +bool assert_equal(const V& expected, const std::optional& actual, double tol = 1e-9) { if (!actual) { std::cout << "actual is boost::none" << std::endl; return false; @@ -71,14 +71,6 @@ bool assert_equal(const V& expected, const boost::optional& actual, double to return assert_equal(expected, *actual, tol); } -template -bool assert_equal(const V& expected, const boost::optional& actual, double tol = 1e-9) { - if (!actual) { - std::cout << "actual is boost::none" << std::endl; - return false; - } - return assert_equal(expected, *actual, tol); -} #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 /** diff --git a/gtsam/nonlinear/LinearContainerFactor.cpp b/gtsam/nonlinear/LinearContainerFactor.cpp index 9ee0681d2..8b05bc2d9 100644 --- a/gtsam/nonlinear/LinearContainerFactor.cpp +++ b/gtsam/nonlinear/LinearContainerFactor.cpp @@ -21,13 +21,13 @@ void LinearContainerFactor::initializeLinearizationPoint(const Values& lineariza linearizationPoint_->insert(key, linearizationPoint.at(key)); } } else { - linearizationPoint_ = boost::none; + linearizationPoint_ = {}; } } /* ************************************************************************* */ LinearContainerFactor::LinearContainerFactor(const GaussianFactor::shared_ptr& factor, - const boost::optional& linearizationPoint) + const std::optional& linearizationPoint) : NonlinearFactor(factor->keys()), factor_(factor), linearizationPoint_(linearizationPoint) { } diff --git a/gtsam/nonlinear/LinearContainerFactor.h b/gtsam/nonlinear/LinearContainerFactor.h index 16094b67a..3a0555af6 100644 --- a/gtsam/nonlinear/LinearContainerFactor.h +++ b/gtsam/nonlinear/LinearContainerFactor.h @@ -10,6 +10,9 @@ #pragma once #include +#include + +#include namespace gtsam { @@ -27,10 +30,10 @@ class GTSAM_EXPORT LinearContainerFactor : public NonlinearFactor { protected: GaussianFactor::shared_ptr factor_; - boost::optional linearizationPoint_; + std::optional linearizationPoint_; /** direct copy constructor */ - LinearContainerFactor(const GaussianFactor::shared_ptr& factor, const boost::optional& linearizationPoint); + LinearContainerFactor(const GaussianFactor::shared_ptr& factor, const std::optional& linearizationPoint); // Some handy typedefs typedef NonlinearFactor Base; @@ -80,7 +83,7 @@ public: size_t dim() const override; /** Extract the linearization point used in recalculating error */ - const boost::optional& linearizationPoint() const { return linearizationPoint_; } + const std::optional& linearizationPoint() const { return linearizationPoint_; } /** * Linearize to a GaussianFactor, with method depending on the presence of a linearizationPoint @@ -135,7 +138,7 @@ public: NonlinearFactor::shared_ptr rekey(const KeyVector& new_keys) const override; /// Casting syntactic sugar - inline bool hasLinearizationPoint() const { return linearizationPoint_.is_initialized(); } + inline bool hasLinearizationPoint() const { return linearizationPoint_.has_value(); } /** * Simple checks whether this is a Jacobian or Hessian factor From 3ebada2dc45d592aac8f5ead0ae64532207c111f Mon Sep 17 00:00:00 2001 From: kartik arcot Date: Wed, 11 Jan 2023 15:07:25 -0800 Subject: [PATCH 06/38] discrete folder --- gtsam/base/ThreadsafeException.h | 3 ++- gtsam/discrete/DecisionTree-inl.h | 6 +++--- gtsam/discrete/Signature.cpp | 6 +++--- gtsam/discrete/Signature.h | 8 ++++---- 4 files changed, 12 insertions(+), 11 deletions(-) diff --git a/gtsam/base/ThreadsafeException.h b/gtsam/base/ThreadsafeException.h index 652dbd90f..ad228e590 100644 --- a/gtsam/base/ThreadsafeException.h +++ b/gtsam/base/ThreadsafeException.h @@ -26,6 +26,7 @@ #include #include #include +#include #ifdef GTSAM_USE_TBB #include @@ -53,7 +54,7 @@ protected: protected: bool dynamic_; ///< Whether this object was moved - mutable boost::optional description_; ///< Optional description + mutable std::optional description_; ///< Optional description /// Default constructor is protected - may only be created from derived classes ThreadsafeException() : diff --git a/gtsam/discrete/DecisionTree-inl.h b/gtsam/discrete/DecisionTree-inl.h index 9f3d5e8f9..0726da3dd 100644 --- a/gtsam/discrete/DecisionTree-inl.h +++ b/gtsam/discrete/DecisionTree-inl.h @@ -24,7 +24,6 @@ #include #include #include -#include #include #include @@ -34,6 +33,7 @@ #include #include #include +#include namespace gtsam { @@ -563,7 +563,7 @@ namespace gtsam { typename DecisionTree::NodePtr DecisionTree::compose( Iterator begin, Iterator end, const L& label) const { // find highest label among branches - boost::optional highestLabel; + std::optional highestLabel; size_t nrChoices = 0; for (Iterator it = begin; it != end; it++) { if (it->root_->isLeaf()) @@ -571,7 +571,7 @@ namespace gtsam { boost::shared_ptr c = boost::dynamic_pointer_cast(it->root_); if (!highestLabel || c->label() > *highestLabel) { - highestLabel.reset(c->label()); + highestLabel = c->label(); nrChoices = c->nrChoices(); } } diff --git a/gtsam/discrete/Signature.cpp b/gtsam/discrete/Signature.cpp index 146555898..bc045e8c2 100644 --- a/gtsam/discrete/Signature.cpp +++ b/gtsam/discrete/Signature.cpp @@ -137,14 +137,14 @@ namespace gtsam { } Signature& Signature::operator=(const string& spec) { - spec_.reset(spec); + spec_ = spec; Table table; parser::It f = spec.begin(), l = spec.end(); bool success = qi::phrase_parse(f, l, parser::grammar.table, qi::space, table); if (success) { for (Row& row : table) normalize(row); - table_.reset(table); + table_ = table; } return *this; } @@ -153,7 +153,7 @@ namespace gtsam { Table table = t; for(Row& row: table) normalize(row); - table_.reset(table); + table_ = table; return *this; } diff --git a/gtsam/discrete/Signature.h b/gtsam/discrete/Signature.h index 3138435a9..963843ab2 100644 --- a/gtsam/discrete/Signature.h +++ b/gtsam/discrete/Signature.h @@ -19,7 +19,7 @@ #pragma once #include #include -#include +#include #include namespace gtsam { @@ -68,10 +68,10 @@ namespace gtsam { DiscreteKeys parents_; // the given CPT specification string - boost::optional spec_; + std::optional spec_; // the CPT as parsed, if successful - boost::optional table_; + std::optional
table_; public: /** @@ -124,7 +124,7 @@ namespace gtsam { KeyVector indices() const; // the CPT as parsed, if successful - const boost::optional
& table() const { return table_; } + const std::optional
& table() const { return table_; } // the CPT as a vector of doubles, with key's values most rapidly changing std::vector cpt() const; From 02b5485c764e1001235514496ec2d3c5e82cafde Mon Sep 17 00:00:00 2001 From: kartik arcot Date: Wed, 11 Jan 2023 15:21:41 -0800 Subject: [PATCH 07/38] point2.h --- gtsam/base/std_optional_serialization.h | 1 + gtsam/geometry/Point2.cpp | 10 +++++----- gtsam/geometry/Point2.h | 7 +++++-- gtsam_unstable/slam/SmartRangeFactor.h | 5 +++-- 4 files changed, 14 insertions(+), 9 deletions(-) diff --git a/gtsam/base/std_optional_serialization.h b/gtsam/base/std_optional_serialization.h index 3dac79284..22a4369e8 100644 --- a/gtsam/base/std_optional_serialization.h +++ b/gtsam/base/std_optional_serialization.h @@ -1,6 +1,7 @@ // A hack to serialize std::optional to boost::archive // Don't know if it will work. Trying to follow this: // PR: https://github.com/boostorg/serialization/pull/148/files# +#pragma once #include #include diff --git a/gtsam/geometry/Point2.cpp b/gtsam/geometry/Point2.cpp index 06c32526b..54c94addb 100644 --- a/gtsam/geometry/Point2.cpp +++ b/gtsam/geometry/Point2.cpp @@ -52,7 +52,7 @@ double distance2(const Point2& p, const Point2& q, OptionalJacobian<1, 2> H1, /* ************************************************************************* */ // Math inspired by http://paulbourke.net/geometry/circlesphere/ -boost::optional circleCircleIntersection(double R_d, double r_d, +std::optional circleCircleIntersection(double R_d, double r_d, double tol) { double R2_d2 = R_d*R_d; // Yes, RD-D2 ! @@ -61,17 +61,17 @@ boost::optional circleCircleIntersection(double R_d, double r_d, // h^2<0 is equivalent to (d > (R + r) || d < (R - r)) // Hence, there are only solutions if >=0 - if (h2<-tol) return boost::none; // allow *slightly* negative + if (h2<-tol) return {}; // allow *slightly* negative else if (h2 circleCircleIntersection(Point2 c1, Point2 c2, - boost::optional fh) { + std::optional fh) { list solutions; - // If fh==boost::none, there are no solutions, i.e., d > (R + r) || d < (R - r) + // If fh==std::nullopt, there are no solutions, i.e., d > (R + r) || d < (R - r) if (fh) { // vector between circle centers Point2 c12 = c2-c1; @@ -107,7 +107,7 @@ list circleCircleIntersection(Point2 c1, double r1, Point2 c2, // Calculate f and h given normalized radii double _d = 1.0/d, R_d = r1*_d, r_d=r2*_d; - boost::optional fh = circleCircleIntersection(R_d,r_d); + std::optional fh = circleCircleIntersection(R_d,r_d); // Call version that takes fh return circleCircleIntersection(c1, c2, fh); diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index d8b6daca8..2b4f48197 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -18,8 +18,11 @@ #pragma once #include +#include #include +#include + namespace gtsam { /// As of GTSAM 4, in order to make GTSAM more lean, @@ -62,7 +65,7 @@ inline Point2 operator*(double s, const Point2& p) { * @param tol: absolute tolerance below which we consider touching circles * @return optional Point2 with f and h, boost::none if no solution. */ -GTSAM_EXPORT boost::optional circleCircleIntersection(double R_d, double r_d, double tol = 1e-9); +GTSAM_EXPORT std::optional circleCircleIntersection(double R_d, double r_d, double tol = 1e-9); /* * @brief Circle-circle intersection, from the normalized radii solution. @@ -70,7 +73,7 @@ GTSAM_EXPORT boost::optional circleCircleIntersection(double R_d, double * @param c2 center of second circle * @return list of solutions (0,1, or 2). Identical circles will return empty list, as well. */ -GTSAM_EXPORT std::list circleCircleIntersection(Point2 c1, Point2 c2, boost::optional fh); +GTSAM_EXPORT std::list circleCircleIntersection(Point2 c1, Point2 c2, std::optional fh); /// Calculate the two means of a set of Point2 pairs GTSAM_EXPORT Point2Pair means(const std::vector &abPointPairs); diff --git a/gtsam_unstable/slam/SmartRangeFactor.h b/gtsam_unstable/slam/SmartRangeFactor.h index 301508c0f..d512ed85a 100644 --- a/gtsam_unstable/slam/SmartRangeFactor.h +++ b/gtsam_unstable/slam/SmartRangeFactor.h @@ -19,6 +19,7 @@ #include #include #include +#include namespace gtsam { @@ -105,7 +106,7 @@ class SmartRangeFactor: public NoiseModelFactor { } Circle2 circle1 = circles.front(); - boost::optional best_fh; + std::optional best_fh; auto bestCircle2 = boost::make_optional(false, circle1); // fixes issue #38 // loop over all circles @@ -115,7 +116,7 @@ class SmartRangeFactor: public NoiseModelFactor { if (d < 1e-9) continue; // skip circles that are in the same location // Find f and h, the intersection points in normalized circles - boost::optional fh = circleCircleIntersection( + std::optional fh = circleCircleIntersection( circle1.radius / d, it.radius / d); // Check if this pair is better by checking h = fh->y() // if h is large, the intersections are well defined. From 273e2eb9ef98e5a2a35f629b7a76311896b2a5fa Mon Sep 17 00:00:00 2001 From: kartik arcot Date: Wed, 11 Jan 2023 15:27:26 -0800 Subject: [PATCH 08/38] pose2.h --- gtsam/geometry/Pose2.cpp | 8 +++---- gtsam/geometry/Pose2.h | 37 ++++++++++++++++-------------- gtsam/geometry/tests/testPose2.cpp | 12 +++++----- 3 files changed, 30 insertions(+), 27 deletions(-) diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index b37674b92..d7996f74f 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -327,10 +327,10 @@ double Pose2::range(const Pose2& pose, * as they also satisfy ca = t + R*cb, hence t = ca - R*cb */ -boost::optional Pose2::Align(const Point2Pairs &ab_pairs) { +std::optional Pose2::Align(const Point2Pairs &ab_pairs) { const size_t n = ab_pairs.size(); if (n < 2) { - return boost::none; // we need at least 2 pairs + return {}; // we need at least 2 pairs } // calculate centroids @@ -359,7 +359,7 @@ boost::optional Pose2::Align(const Point2Pairs &ab_pairs) { return Pose2(R, t); } -boost::optional Pose2::Align(const Matrix& a, const Matrix& b) { +std::optional Pose2::Align(const Matrix& a, const Matrix& b) { if (a.rows() != 2 || b.rows() != 2 || a.cols() != b.cols()) { throw std::invalid_argument( "Pose2:Align expects 2*N matrices of equal shape."); @@ -372,7 +372,7 @@ boost::optional Pose2::Align(const Matrix& a, const Matrix& b) { } #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 -boost::optional align(const Point2Pairs& ba_pairs) { +std::optional align(const Point2Pairs& ba_pairs) { Point2Pairs ab_pairs; for (const Point2Pair &baPair : ba_pairs) { ab_pairs.emplace_back(baPair.second, baPair.first); diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index 0ecd95e1c..7a6a846be 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -25,6 +25,9 @@ #include #include #include +#include + +#include namespace gtsam { @@ -99,10 +102,10 @@ public: * Note this allows for noise on the points but in that case the mapping * will not be exact. */ - static boost::optional Align(const Point2Pairs& abPointPairs); + static std::optional Align(const Point2Pairs& abPointPairs); // Version of Pose2::Align that takes 2 matrices. - static boost::optional Align(const Matrix& a, const Matrix& b); + static std::optional Align(const Matrix& a, const Matrix& b); /// @} /// @name Testable @@ -134,10 +137,10 @@ public: /// @{ ///Exponential map at identity - create a rotation from canonical coordinates \f$ [T_x,T_y,\theta] \f$ - GTSAM_EXPORT static Pose2 Expmap(const Vector3& xi, ChartJacobian H = boost::none); + GTSAM_EXPORT static Pose2 Expmap(const Vector3& xi, ChartJacobian H = {}); ///Log map at identity - return the canonical coordinates \f$ [T_x,T_y,\theta] \f$ of this rotation - GTSAM_EXPORT static Vector3 Logmap(const Pose2& p, ChartJacobian H = boost::none); + GTSAM_EXPORT static Vector3 Logmap(const Pose2& p, ChartJacobian H = {}); /** * Calculate Adjoint map @@ -196,8 +199,8 @@ public: // Chart at origin, depends on compile-time flag SLOW_BUT_CORRECT_EXPMAP struct ChartAtOrigin { - GTSAM_EXPORT static Pose2 Retract(const Vector3& v, ChartJacobian H = boost::none); - GTSAM_EXPORT static Vector3 Local(const Pose2& r, ChartJacobian H = boost::none); + GTSAM_EXPORT static Pose2 Retract(const Vector3& v, ChartJacobian H = {}); + GTSAM_EXPORT static Vector3 Local(const Pose2& r, ChartJacobian H = {}); }; using LieGroup::inverse; // version with derivative @@ -208,8 +211,8 @@ public: /** Return point coordinates in pose coordinate frame */ GTSAM_EXPORT Point2 transformTo(const Point2& point, - OptionalJacobian<2, 3> Dpose = boost::none, - OptionalJacobian<2, 2> Dpoint = boost::none) const; + OptionalJacobian<2, 3> Dpose = {}, + OptionalJacobian<2, 2> Dpoint = {}) const; /** * @brief transform many points in world coordinates and transform to Pose. @@ -220,8 +223,8 @@ public: /** Return point coordinates in global frame */ GTSAM_EXPORT Point2 transformFrom(const Point2& point, - OptionalJacobian<2, 3> Dpose = boost::none, - OptionalJacobian<2, 2> Dpoint = boost::none) const; + OptionalJacobian<2, 3> Dpose = {}, + OptionalJacobian<2, 2> Dpoint = {}) const; /** * @brief transform many points in Pose coordinates and transform to world. @@ -269,7 +272,7 @@ public: * @return 2D rotation \f$ \in SO(2) \f$ */ GTSAM_EXPORT Rot2 bearing(const Point2& point, - OptionalJacobian<1, 3> H1=boost::none, OptionalJacobian<1, 2> H2=boost::none) const; + OptionalJacobian<1, 3> H1={}, OptionalJacobian<1, 2> H2={}) const; /** * Calculate bearing to another pose @@ -277,7 +280,7 @@ public: * @return 2D rotation \f$ \in SO(2) \f$ */ GTSAM_EXPORT Rot2 bearing(const Pose2& pose, - OptionalJacobian<1, 3> H1=boost::none, OptionalJacobian<1, 3> H2=boost::none) const; + OptionalJacobian<1, 3> H1={}, OptionalJacobian<1, 3> H2={}) const; /** * Calculate range to a landmark @@ -285,8 +288,8 @@ public: * @return range (double) */ GTSAM_EXPORT double range(const Point2& point, - OptionalJacobian<1, 3> H1=boost::none, - OptionalJacobian<1, 2> H2=boost::none) const; + OptionalJacobian<1, 3> H1={}, + OptionalJacobian<1, 2> H2={}) const; /** * Calculate range to another pose @@ -294,8 +297,8 @@ public: * @return range (double) */ GTSAM_EXPORT double range(const Pose2& point, - OptionalJacobian<1, 3> H1=boost::none, - OptionalJacobian<1, 3> H2=boost::none) const; + OptionalJacobian<1, 3> H1={}, + OptionalJacobian<1, 3> H2={}) const; /// @} /// @name Advanced Interface @@ -349,7 +352,7 @@ inline Matrix wedge(const Vector& xi) { * Calculate pose between a vector of 2D point correspondences (p,q) * where q = Pose2::transformFrom(p) = t + R*p */ -GTSAM_EXPORT boost::optional +GTSAM_EXPORT std::optional GTSAM_DEPRECATED align(const Point2Pairs& pairs); #endif diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index 44dc55a81..34ce3932d 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -23,7 +23,7 @@ #include #include -#include +#include #include #include @@ -718,7 +718,7 @@ TEST(Pose2, align_1) { Pose2 expected(Rot2::fromAngle(0), Point2(10, 10)); Point2Pairs ab_pairs {{Point2(10, 10), Point2(0, 0)}, {Point2(30, 20), Point2(20, 10)}}; - boost::optional aTb = Pose2::Align(ab_pairs); + std::optional aTb = Pose2::Align(ab_pairs); EXPECT(assert_equal(expected, *aTb)); } @@ -731,7 +731,7 @@ TEST(Pose2, align_2) { Point2Pairs ab_pairs {{expected.transformFrom(b1), b1}, {expected.transformFrom(b2), b2}}; - boost::optional aTb = Pose2::Align(ab_pairs); + std::optional aTb = Pose2::Align(ab_pairs); EXPECT(assert_equal(expected, *aTb)); } @@ -752,7 +752,7 @@ TEST(Pose2, align_3) { Point2Pair ab3(make_pair(a3, b3)); const Point2Pairs ab_pairs{ab1, ab2, ab3}; - boost::optional aTb = Pose2::Align(ab_pairs); + std::optional aTb = Pose2::Align(ab_pairs); EXPECT(assert_equal(expected, *aTb)); } @@ -762,7 +762,7 @@ namespace { /* ************************************************************************* */ struct Triangle { size_t i_, j_, k_;}; - boost::optional align2(const Point2Vector& as, const Point2Vector& bs, + std::optional align2(const Point2Vector& as, const Point2Vector& bs, const pair& trianglePair) { const Triangle& t1 = trianglePair.first, t2 = trianglePair.second; Point2Pairs ab_pairs = {{as[t1.i_], bs[t2.i_]}, @@ -780,7 +780,7 @@ TEST(Pose2, align_4) { Triangle t1; t1.i_=0; t1.j_=1; t1.k_=2; Triangle t2; t2.i_=1; t2.j_=2; t2.k_=0; - boost::optional actual = align2(as, bs, {t1, t2}); + std::optional actual = align2(as, bs, {t1, t2}); EXPECT(assert_equal(expected, *actual)); } From a18902563d0a898565cf070c14f5f733bbc6e17c Mon Sep 17 00:00:00 2001 From: kartik arcot Date: Thu, 12 Jan 2023 08:20:25 -0800 Subject: [PATCH 09/38] pose3.h --- gtsam/geometry/Pose3.cpp | 10 ++--- gtsam/geometry/Pose3.h | 68 +++++++++++++++--------------- gtsam/geometry/tests/testPose3.cpp | 39 ++++++++--------- 3 files changed, 59 insertions(+), 58 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 2652fc073..3779d401a 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -439,14 +439,14 @@ Unit3 Pose3::bearing(const Pose3& pose, OptionalJacobian<2, 6> Hself, Hpose->setZero(); return bearing(pose.translation(), Hself, Hpose.cols<3>(3)); } - return bearing(pose.translation(), Hself, boost::none); + return bearing(pose.translation(), Hself, {}); } /* ************************************************************************* */ -boost::optional Pose3::Align(const Point3Pairs &abPointPairs) { +std::optional Pose3::Align(const Point3Pairs &abPointPairs) { const size_t n = abPointPairs.size(); if (n < 3) { - return boost::none; // we need at least three pairs + return {}; // we need at least three pairs } // calculate centroids @@ -466,7 +466,7 @@ boost::optional Pose3::Align(const Point3Pairs &abPointPairs) { return Pose3(aRb, aTb); } -boost::optional Pose3::Align(const Matrix& a, const Matrix& b) { +std::optional Pose3::Align(const Matrix& a, const Matrix& b) { if (a.rows() != 3 || b.rows() != 3 || a.cols() != b.cols()) { throw std::invalid_argument( "Pose3:Align expects 3*N matrices of equal shape."); @@ -479,7 +479,7 @@ boost::optional Pose3::Align(const Matrix& a, const Matrix& b) { } #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 -boost::optional align(const Point3Pairs &baPointPairs) { +std::optional align(const Point3Pairs &baPointPairs) { Point3Pairs abPointPairs; for (const Point3Pair &baPair : baPointPairs) { abPointPairs.emplace_back(baPair.second, baPair.first); diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 80741e1c3..3dd9e6b9f 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -75,18 +75,18 @@ public: /// Named constructor with derivatives static Pose3 Create(const Rot3& R, const Point3& t, - OptionalJacobian<6, 3> HR = boost::none, - OptionalJacobian<6, 3> Ht = boost::none); + OptionalJacobian<6, 3> HR = {}, + OptionalJacobian<6, 3> Ht = {}); /** * Create Pose3 by aligning two point pairs * A pose aTb is estimated between pairs (a_point, b_point) such that a_point = aTb * b_point * Note this allows for noise on the points but in that case the mapping will not be exact. */ - static boost::optional Align(const Point3Pairs& abPointPairs); + static std::optional Align(const Point3Pairs& abPointPairs); // Version of Pose3::Align that takes 2 matrices. - static boost::optional Align(const Matrix& a, const Matrix& b); + static std::optional Align(const Matrix& a, const Matrix& b); /// @} /// @name Testable @@ -139,10 +139,10 @@ public: /// @{ /// Exponential map at identity - create a rotation from canonical coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ - static Pose3 Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi = boost::none); + static Pose3 Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi = {}); /// Log map at identity - return the canonical coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ of this rotation - static Vector6 Logmap(const Pose3& pose, OptionalJacobian<6, 6> Hpose = boost::none); + static Vector6 Logmap(const Pose3& pose, OptionalJacobian<6, 6> Hpose = {}); /** * Calculate Adjoint map, transforming a twist in this pose's (i.e, body) frame to the world spatial frame @@ -157,13 +157,13 @@ public: * Note that H_xib = AdjointMap() */ Vector6 Adjoint(const Vector6& xi_b, - OptionalJacobian<6, 6> H_this = boost::none, - OptionalJacobian<6, 6> H_xib = boost::none) const; + OptionalJacobian<6, 6> H_this = {}, + OptionalJacobian<6, 6> H_xib = {}) const; /// The dual version of Adjoint Vector6 AdjointTranspose(const Vector6& x, - OptionalJacobian<6, 6> H_this = boost::none, - OptionalJacobian<6, 6> H_x = boost::none) const; + OptionalJacobian<6, 6> H_this = {}, + OptionalJacobian<6, 6> H_x = {}) const; /** * Compute the [ad(w,v)] operator as defined in [Kobilarov09siggraph], pg 11 @@ -186,8 +186,8 @@ public: * Action of the adjointMap on a Lie-algebra vector y, with optional derivatives */ static Vector6 adjoint(const Vector6& xi, const Vector6& y, - OptionalJacobian<6, 6> Hxi = boost::none, - OptionalJacobian<6, 6> H_y = boost::none); + OptionalJacobian<6, 6> Hxi = {}, + OptionalJacobian<6, 6> H_y = {}); // temporary fix for wrappers until case issue is resolved static Matrix6 adjointMap_(const Vector6 &xi) { return adjointMap(xi);} @@ -197,8 +197,8 @@ public: * The dual version of adjoint action, acting on the dual space of the Lie-algebra vector space. */ static Vector6 adjointTranspose(const Vector6& xi, const Vector6& y, - OptionalJacobian<6, 6> Hxi = boost::none, - OptionalJacobian<6, 6> H_y = boost::none); + OptionalJacobian<6, 6> Hxi = {}, + OptionalJacobian<6, 6> H_y = {}); /// Derivative of Expmap static Matrix6 ExpmapDerivative(const Vector6& xi); @@ -208,8 +208,8 @@ public: // Chart at origin, depends on compile-time flag GTSAM_POSE3_EXPMAP struct ChartAtOrigin { - static Pose3 Retract(const Vector6& xi, ChartJacobian Hxi = boost::none); - static Vector6 Local(const Pose3& pose, ChartJacobian Hpose = boost::none); + static Pose3 Retract(const Vector6& xi, ChartJacobian Hxi = {}); + static Vector6 Local(const Pose3& pose, ChartJacobian Hpose = {}); }; /** @@ -250,7 +250,7 @@ public: * @return point in world coordinates */ Point3 transformFrom(const Point3& point, OptionalJacobian<3, 6> Hself = - boost::none, OptionalJacobian<3, 3> Hpoint = boost::none) const; + {}, OptionalJacobian<3, 3> Hpoint = {}) const; /** * @brief transform many points in Pose coordinates and transform to world. @@ -272,7 +272,7 @@ public: * @return point in Pose coordinates */ Point3 transformTo(const Point3& point, OptionalJacobian<3, 6> Hself = - boost::none, OptionalJacobian<3, 3> Hpoint = boost::none) const; + {}, OptionalJacobian<3, 3> Hpoint = {}) const; /** * @brief transform many points in world coordinates and transform to Pose. @@ -286,10 +286,10 @@ public: /// @{ /// get rotation - const Rot3& rotation(OptionalJacobian<3, 6> Hself = boost::none) const; + const Rot3& rotation(OptionalJacobian<3, 6> Hself = {}) const; /// get translation - const Point3& translation(OptionalJacobian<3, 6> Hself = boost::none) const; + const Point3& translation(OptionalJacobian<3, 6> Hself = {}) const; /// get x double x() const { @@ -314,39 +314,39 @@ public: * and transforms it to world coordinates wTb = wTa * aTb. * This is identical to compose. */ - Pose3 transformPoseFrom(const Pose3& aTb, OptionalJacobian<6, 6> Hself = boost::none, - OptionalJacobian<6, 6> HaTb = boost::none) const; + Pose3 transformPoseFrom(const Pose3& aTb, OptionalJacobian<6, 6> Hself = {}, + OptionalJacobian<6, 6> HaTb = {}) const; /** * Assuming self == wTa, takes a pose wTb in world coordinates * and transforms it to local coordinates aTb = inv(wTa) * wTb */ - Pose3 transformPoseTo(const Pose3& wTb, OptionalJacobian<6, 6> Hself = boost::none, - OptionalJacobian<6, 6> HwTb = boost::none) const; + Pose3 transformPoseTo(const Pose3& wTb, OptionalJacobian<6, 6> Hself = {}, + OptionalJacobian<6, 6> HwTb = {}) const; /** * Calculate range to a landmark * @param point 3D location of landmark * @return range (double) */ - double range(const Point3& point, OptionalJacobian<1, 6> Hself = boost::none, - OptionalJacobian<1, 3> Hpoint = boost::none) const; + double range(const Point3& point, OptionalJacobian<1, 6> Hself = {}, + OptionalJacobian<1, 3> Hpoint = {}) const; /** * Calculate range to another pose * @param pose Other SO(3) pose * @return range (double) */ - double range(const Pose3& pose, OptionalJacobian<1, 6> Hself = boost::none, - OptionalJacobian<1, 6> Hpose = boost::none) const; + double range(const Pose3& pose, OptionalJacobian<1, 6> Hself = {}, + OptionalJacobian<1, 6> Hpose = {}) const; /** * Calculate bearing to a landmark * @param point 3D location of landmark * @return bearing (Unit3) */ - Unit3 bearing(const Point3& point, OptionalJacobian<2, 6> Hself = boost::none, - OptionalJacobian<2, 3> Hpoint = boost::none) const; + Unit3 bearing(const Point3& point, OptionalJacobian<2, 6> Hself = {}, + OptionalJacobian<2, 3> Hpoint = {}) const; /** * Calculate bearing to another pose @@ -354,8 +354,8 @@ public: * information is ignored. * @return bearing (Unit3) */ - Unit3 bearing(const Pose3& pose, OptionalJacobian<2, 6> Hself = boost::none, - OptionalJacobian<2, 6> Hpose = boost::none) const; + Unit3 bearing(const Pose3& pose, OptionalJacobian<2, 6> Hself = {}, + OptionalJacobian<2, 6> Hpose = {}) const; /// @} /// @name Advanced Interface @@ -384,8 +384,8 @@ public: * @param s a value between 0 and 1.5 * @param other final point of interpolation geodesic on manifold */ - Pose3 slerp(double t, const Pose3& other, OptionalJacobian<6, 6> Hx = boost::none, - OptionalJacobian<6, 6> Hy = boost::none) const; + Pose3 slerp(double t, const Pose3& other, OptionalJacobian<6, 6> Hx = {}, + OptionalJacobian<6, 6> Hy = {}) const; /// Output stream operator GTSAM_EXPORT diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index dd7da172e..93cf99972 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -23,6 +23,7 @@ #include #include +#include using namespace std; using namespace gtsam; @@ -286,8 +287,8 @@ TEST(Pose3, translation) { Matrix actualH; EXPECT(assert_equal(Point3(3.5, -8.2, 4.2), T.translation(actualH), 1e-8)); - Matrix numericalH = numericalDerivative11( - std::bind(&Pose3::translation, std::placeholders::_1, boost::none), T); + std::function f = [](const Pose3& T) { return T.translation(); }; + Matrix numericalH = numericalDerivative11(f, T); EXPECT(assert_equal(numericalH, actualH, 1e-6)); } @@ -297,8 +298,8 @@ TEST(Pose3, rotation) { Matrix actualH; EXPECT(assert_equal(R, T.rotation(actualH), 1e-8)); - Matrix numericalH = numericalDerivative11( - std::bind(&Pose3::rotation, std::placeholders::_1, boost::none), T); + std::function f = [](const Pose3& T) { return T.rotation(); }; + Matrix numericalH = numericalDerivative11(f, T); EXPECT(assert_equal(numericalH, actualH, 1e-6)); } @@ -396,7 +397,7 @@ Point3 transformFrom_(const Pose3& pose, const Point3& point) { } TEST(Pose3, Dtransform_from1_a) { Matrix actualDtransform_from1; - T.transformFrom(P, actualDtransform_from1, boost::none); + T.transformFrom(P, actualDtransform_from1, {}); Matrix numerical = numericalDerivative21(transformFrom_, T, P); EXPECT(assert_equal(numerical, actualDtransform_from1, 1e-8)); } @@ -404,7 +405,7 @@ TEST(Pose3, Dtransform_from1_a) { TEST(Pose3, Dtransform_from1_b) { Pose3 origin; Matrix actualDtransform_from1; - origin.transformFrom(P, actualDtransform_from1, boost::none); + origin.transformFrom(P, actualDtransform_from1, {}); Matrix numerical = numericalDerivative21(transformFrom_, origin, P); EXPECT(assert_equal(numerical, actualDtransform_from1, 1e-8)); } @@ -413,7 +414,7 @@ TEST(Pose3, Dtransform_from1_c) { Point3 origin(0, 0, 0); Pose3 T0(R, origin); Matrix actualDtransform_from1; - T0.transformFrom(P, actualDtransform_from1, boost::none); + T0.transformFrom(P, actualDtransform_from1, {}); Matrix numerical = numericalDerivative21(transformFrom_, T0, P); EXPECT(assert_equal(numerical, actualDtransform_from1, 1e-8)); } @@ -423,7 +424,7 @@ TEST(Pose3, Dtransform_from1_d) { Point3 t0(100, 0, 0); Pose3 T0(I, t0); Matrix actualDtransform_from1; - T0.transformFrom(P, actualDtransform_from1, boost::none); + T0.transformFrom(P, actualDtransform_from1, {}); // print(computed, "Dtransform_from1_d computed:"); Matrix numerical = numericalDerivative21(transformFrom_, T0, P); // print(numerical, "Dtransform_from1_d numerical:"); @@ -433,7 +434,7 @@ TEST(Pose3, Dtransform_from1_d) { /* ************************************************************************* */ TEST(Pose3, Dtransform_from2) { Matrix actualDtransform_from2; - T.transformFrom(P, boost::none, actualDtransform_from2); + T.transformFrom(P, {}, actualDtransform_from2); Matrix numerical = numericalDerivative22(transformFrom_, T, P); EXPECT(assert_equal(numerical, actualDtransform_from2, 1e-8)); } @@ -444,7 +445,7 @@ Point3 transform_to_(const Pose3& pose, const Point3& point) { } TEST(Pose3, Dtransform_to1) { Matrix computed; - T.transformTo(P, computed, boost::none); + T.transformTo(P, computed, {}); Matrix numerical = numericalDerivative21(transform_to_, T, P); EXPECT(assert_equal(numerical, computed, 1e-8)); } @@ -452,7 +453,7 @@ TEST(Pose3, Dtransform_to1) { /* ************************************************************************* */ TEST(Pose3, Dtransform_to2) { Matrix computed; - T.transformTo(P, boost::none, computed); + T.transformTo(P, {}, computed); Matrix numerical = numericalDerivative22(transform_to_, T, P); EXPECT(assert_equal(numerical, computed, 1e-8)); } @@ -812,7 +813,7 @@ TEST(Pose3, Align1) { Point3Pair ab3(Point3(20,30,0), Point3(10,20,0)); const vector correspondences{ab1, ab2, ab3}; - boost::optional actual = Pose3::Align(correspondences); + std::optional actual = Pose3::Align(correspondences); EXPECT(assert_equal(expected, *actual)); } @@ -829,7 +830,7 @@ TEST(Pose3, Align2) { const Point3Pair ab1{q1, p1}, ab2{q2, p2}, ab3{q3, p3}; const vector correspondences{ab1, ab2, ab3}; - boost::optional actual = Pose3::Align(correspondences); + std::optional actual = Pose3::Align(correspondences); EXPECT(assert_equal(expected, *actual, 1e-5)); } @@ -839,7 +840,7 @@ TEST( Pose3, ExpmapDerivative1) { Vector6 w; w << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0; Pose3::Expmap(w,actualH); Matrix expectedH = numericalDerivative21 >(&Pose3::Expmap, w, boost::none); + OptionalJacobian<6, 6> >(&Pose3::Expmap, w, {}); EXPECT(assert_equal(expectedH, actualH)); } @@ -884,7 +885,7 @@ TEST( Pose3, ExpmapDerivativeQr) { w.head<3>() = w.head<3>() * 0.9e-2; Matrix3 actualQr = Pose3::ComputeQforExpmapDerivative(w, 0.01); Matrix expectedH = numericalDerivative21 >(&Pose3::Expmap, w, boost::none); + OptionalJacobian<6, 6> >(&Pose3::Expmap, w, {}); Matrix3 expectedQr = expectedH.bottomLeftCorner<3, 3>(); EXPECT(assert_equal(expectedQr, actualQr, 1e-6)); } @@ -896,7 +897,7 @@ TEST( Pose3, LogmapDerivative) { Pose3 p = Pose3::Expmap(w); EXPECT(assert_equal(w, Pose3::Logmap(p,actualH), 1e-5)); Matrix expectedH = numericalDerivative21 >(&Pose3::Logmap, p, boost::none); + OptionalJacobian<6, 6> >(&Pose3::Logmap, p, {}); EXPECT(assert_equal(expectedH, actualH)); } @@ -1189,9 +1190,9 @@ TEST(Pose3, Create) { Matrix63 actualH1, actualH2; Pose3 actual = Pose3::Create(R, P2, actualH1, actualH2); EXPECT(assert_equal(T, actual)); - std::function create = - std::bind(Pose3::Create, std::placeholders::_1, std::placeholders::_2, - boost::none, boost::none); + std::function create = [](Rot3 R, Point3 t) { + return Pose3::Create(R, t); + }; EXPECT(assert_equal(numericalDerivative21(create, R, P2), actualH1, 1e-9)); EXPECT(assert_equal(numericalDerivative22(create, R, P2), actualH2, 1e-9)); } From 4495efe2335b87ac68f8a5ad5138600cc0b87edf Mon Sep 17 00:00:00 2001 From: kartik arcot Date: Thu, 12 Jan 2023 08:34:54 -0800 Subject: [PATCH 10/38] triangulation.h --- gtsam/geometry/tests/testTriangulation.cpp | 82 +++++++++---------- gtsam/geometry/triangulation.h | 6 +- ...martProjectionPoseFactorRollingShutter.cpp | 2 +- 3 files changed, 45 insertions(+), 45 deletions(-) diff --git a/gtsam/geometry/tests/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp index 95397d5b3..89b320e94 100644 --- a/gtsam/geometry/tests/testTriangulation.cpp +++ b/gtsam/geometry/tests/testTriangulation.cpp @@ -67,13 +67,13 @@ TEST(triangulation, twoPoses) { // 1. Test simple DLT, perfect in no noise situation bool optimize = false; - boost::optional actual1 = // + std::optional actual1 = // triangulatePoint3(kPoses, kSharedCal, measurements, rank_tol, optimize); EXPECT(assert_equal(kLandmark, *actual1, 1e-7)); // 2. test with optimization on, same answer optimize = true; - boost::optional actual2 = // + std::optional actual2 = // triangulatePoint3(kPoses, kSharedCal, measurements, rank_tol, optimize); EXPECT(assert_equal(kLandmark, *actual2, 1e-7)); @@ -82,13 +82,13 @@ TEST(triangulation, twoPoses) { measurements.at(0) += Point2(0.1, 0.5); measurements.at(1) += Point2(-0.2, 0.3); optimize = false; - boost::optional actual3 = // + std::optional actual3 = // triangulatePoint3(kPoses, kSharedCal, measurements, rank_tol, optimize); EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual3, 1e-4)); // 4. Now with optimization on optimize = true; - boost::optional actual4 = // + std::optional actual4 = // triangulatePoint3(kPoses, kSharedCal, measurements, rank_tol, optimize); EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual4, 1e-4)); } @@ -103,7 +103,7 @@ TEST(triangulation, twoCamerasUsingLOST) { double rank_tol = 1e-9; // 1. Test simple triangulation, perfect in no noise situation - boost::optional actual1 = // + std::optional actual1 = // triangulatePoint3>(cameras, measurements, rank_tol, /*optimize=*/false, measurementNoise, @@ -114,7 +114,7 @@ TEST(triangulation, twoCamerasUsingLOST) { // 0.499167, 1.19814) measurements[0] += Point2(0.1, 0.5); measurements[1] += Point2(-0.2, 0.3); - boost::optional actual2 = // + std::optional actual2 = // triangulatePoint3>( cameras, measurements, rank_tol, /*optimize=*/false, measurementNoise, @@ -140,7 +140,7 @@ TEST(triangulation, twoCamerasLOSTvsDLT) { const double rank_tol = 1e-9; SharedNoiseModel measurementNoise = noiseModel::Isotropic::Sigma(2, 1e-2); - boost::optional estimateLOST = // + std::optional estimateLOST = // triangulatePoint3>(cameras, measurements, rank_tol, /*optimize=*/false, measurementNoise, @@ -149,7 +149,7 @@ TEST(triangulation, twoCamerasLOSTvsDLT) { // These values are from a MATLAB implementation. EXPECT(assert_equal(Point3(0.007, 0.011, 0.945), *estimateLOST, 1e-3)); - boost::optional estimateDLT = + std::optional estimateDLT = triangulatePoint3(cameras, measurements, rank_tol, false); // The LOST estimate should have a smaller error. @@ -177,14 +177,14 @@ TEST(triangulation, twoPosesCal3DS2) { // 1. Test simple DLT, perfect in no noise situation bool optimize = false; - boost::optional actual1 = // + std::optional actual1 = // triangulatePoint3(kPoses, sharedDistortedCal, measurements, rank_tol, optimize); EXPECT(assert_equal(kLandmark, *actual1, 1e-7)); // 2. test with optimization on, same answer optimize = true; - boost::optional actual2 = // + std::optional actual2 = // triangulatePoint3(kPoses, sharedDistortedCal, measurements, rank_tol, optimize); EXPECT(assert_equal(kLandmark, *actual2, 1e-7)); @@ -194,14 +194,14 @@ TEST(triangulation, twoPosesCal3DS2) { measurements.at(0) += Point2(0.1, 0.5); measurements.at(1) += Point2(-0.2, 0.3); optimize = false; - boost::optional actual3 = // + std::optional actual3 = // triangulatePoint3(kPoses, sharedDistortedCal, measurements, rank_tol, optimize); EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual3, 1e-3)); // 4. Now with optimization on optimize = true; - boost::optional actual4 = // + std::optional actual4 = // triangulatePoint3(kPoses, sharedDistortedCal, measurements, rank_tol, optimize); EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual4, 1e-3)); @@ -230,14 +230,14 @@ TEST(triangulation, twoPosesFisheye) { // 1. Test simple DLT, perfect in no noise situation bool optimize = false; - boost::optional actual1 = // + std::optional actual1 = // triangulatePoint3(kPoses, sharedDistortedCal, measurements, rank_tol, optimize); EXPECT(assert_equal(kLandmark, *actual1, 1e-7)); // 2. test with optimization on, same answer optimize = true; - boost::optional actual2 = // + std::optional actual2 = // triangulatePoint3(kPoses, sharedDistortedCal, measurements, rank_tol, optimize); EXPECT(assert_equal(kLandmark, *actual2, 1e-7)); @@ -247,14 +247,14 @@ TEST(triangulation, twoPosesFisheye) { measurements.at(0) += Point2(0.1, 0.5); measurements.at(1) += Point2(-0.2, 0.3); optimize = false; - boost::optional actual3 = // + std::optional actual3 = // triangulatePoint3(kPoses, sharedDistortedCal, measurements, rank_tol, optimize); EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual3, 1e-3)); // 4. Now with optimization on optimize = true; - boost::optional actual4 = // + std::optional actual4 = // triangulatePoint3(kPoses, sharedDistortedCal, measurements, rank_tol, optimize); EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual4, 1e-3)); @@ -277,7 +277,7 @@ TEST(triangulation, twoPosesBundler) { bool optimize = true; double rank_tol = 1e-9; - boost::optional actual = // + std::optional actual = // triangulatePoint3(kPoses, bundlerCal, measurements, rank_tol, optimize); EXPECT(assert_equal(kLandmark, *actual, 1e-7)); @@ -286,7 +286,7 @@ TEST(triangulation, twoPosesBundler) { measurements.at(0) += Point2(0.1, 0.5); measurements.at(1) += Point2(-0.2, 0.3); - boost::optional actual2 = // + std::optional actual2 = // triangulatePoint3(kPoses, bundlerCal, measurements, rank_tol, optimize); EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19847), *actual2, 1e-3)); @@ -296,7 +296,7 @@ TEST(triangulation, twoPosesBundler) { TEST(triangulation, fourPoses) { Pose3Vector poses = kPoses; Point2Vector measurements = kMeasurements; - boost::optional actual = + std::optional actual = triangulatePoint3(poses, kSharedCal, measurements); EXPECT(assert_equal(kLandmark, *actual, 1e-2)); @@ -305,7 +305,7 @@ TEST(triangulation, fourPoses) { measurements.at(0) += Point2(0.1, 0.5); measurements.at(1) += Point2(-0.2, 0.3); - boost::optional actual2 = // + std::optional actual2 = // triangulatePoint3(poses, kSharedCal, measurements); EXPECT(assert_equal(kLandmark, *actual2, 1e-2)); @@ -317,12 +317,12 @@ TEST(triangulation, fourPoses) { poses.push_back(pose3); measurements.push_back(z3 + Point2(0.1, -0.1)); - boost::optional triangulated_3cameras = // + std::optional triangulated_3cameras = // triangulatePoint3(poses, kSharedCal, measurements); EXPECT(assert_equal(kLandmark, *triangulated_3cameras, 1e-2)); // Again with nonlinear optimization - boost::optional triangulated_3cameras_opt = + std::optional triangulated_3cameras_opt = triangulatePoint3(poses, kSharedCal, measurements, 1e-9, true); EXPECT(assert_equal(kLandmark, *triangulated_3cameras_opt, 1e-2)); @@ -352,7 +352,7 @@ TEST(triangulation, threePoses_robustNoiseModel) { Point2Vector measurements{kZ1, kZ2, z3}; // noise free, so should give exactly the landmark - boost::optional actual = + std::optional actual = triangulatePoint3(poses, kSharedCal, measurements); EXPECT(assert_equal(kLandmark, *actual, 1e-2)); @@ -360,14 +360,14 @@ TEST(triangulation, threePoses_robustNoiseModel) { measurements.at(0) += Point2(100, 120); // very large pixel noise! // now estimate does not match landmark - boost::optional actual2 = // + std::optional actual2 = // triangulatePoint3(poses, kSharedCal, measurements); // DLT is surprisingly robust, but still off (actual error is around 0.26m): EXPECT( (kLandmark - *actual2).norm() >= 0.2); EXPECT( (kLandmark - *actual2).norm() <= 0.5); // Again with nonlinear optimization - boost::optional actual3 = + std::optional actual3 = triangulatePoint3(poses, kSharedCal, measurements, 1e-9, true); // result from nonlinear (but non-robust optimization) is close to DLT and still off EXPECT(assert_equal(*actual2, *actual3, 0.1)); @@ -375,7 +375,7 @@ TEST(triangulation, threePoses_robustNoiseModel) { // Again with nonlinear optimization, this time with robust loss auto model = noiseModel::Robust::Create( noiseModel::mEstimator::Huber::Create(1.345), noiseModel::Unit::Create(2)); - boost::optional actual4 = triangulatePoint3( + std::optional actual4 = triangulatePoint3( poses, kSharedCal, measurements, 1e-9, true, model); // using the Huber loss we now have a quite small error!! nice! EXPECT(assert_equal(kLandmark, *actual4, 0.05)); @@ -393,7 +393,7 @@ TEST(triangulation, fourPoses_robustNoiseModel) { Point2Vector measurements{kZ1, kZ1, kZ2, z3}; // noise free, so should give exactly the landmark - boost::optional actual = + std::optional actual = triangulatePoint3(poses, kSharedCal, measurements); EXPECT(assert_equal(kLandmark, *actual, 1e-2)); @@ -405,14 +405,14 @@ TEST(triangulation, fourPoses_robustNoiseModel) { measurements.at(3) += Point2(0.3, 0.1); // now estimate does not match landmark - boost::optional actual2 = // + std::optional actual2 = // triangulatePoint3(poses, kSharedCal, measurements); // DLT is surprisingly robust, but still off (actual error is around 0.17m): EXPECT( (kLandmark - *actual2).norm() >= 0.1); EXPECT( (kLandmark - *actual2).norm() <= 0.5); // Again with nonlinear optimization - boost::optional actual3 = + std::optional actual3 = triangulatePoint3(poses, kSharedCal, measurements, 1e-9, true); // result from nonlinear (but non-robust optimization) is close to DLT and still off EXPECT(assert_equal(*actual2, *actual3, 0.1)); @@ -420,7 +420,7 @@ TEST(triangulation, fourPoses_robustNoiseModel) { // Again with nonlinear optimization, this time with robust loss auto model = noiseModel::Robust::Create( noiseModel::mEstimator::Huber::Create(1.345), noiseModel::Unit::Create(2)); - boost::optional actual4 = triangulatePoint3( + std::optional actual4 = triangulatePoint3( poses, kSharedCal, measurements, 1e-9, true, model); // using the Huber loss we now have a quite small error!! nice! EXPECT(assert_equal(kLandmark, *actual4, 0.05)); @@ -443,7 +443,7 @@ TEST(triangulation, fourPoses_distinct_Ks) { CameraSet> cameras{camera1, camera2}; Point2Vector measurements{z1, z2}; - boost::optional actual = // + std::optional actual = // triangulatePoint3(cameras, measurements); EXPECT(assert_equal(kLandmark, *actual, 1e-2)); @@ -452,7 +452,7 @@ TEST(triangulation, fourPoses_distinct_Ks) { measurements.at(0) += Point2(0.1, 0.5); measurements.at(1) += Point2(-0.2, 0.3); - boost::optional actual2 = // + std::optional actual2 = // triangulatePoint3(cameras, measurements); EXPECT(assert_equal(kLandmark, *actual2, 1e-2)); @@ -465,12 +465,12 @@ TEST(triangulation, fourPoses_distinct_Ks) { cameras.push_back(camera3); measurements.push_back(z3 + Point2(0.1, -0.1)); - boost::optional triangulated_3cameras = // + std::optional triangulated_3cameras = // triangulatePoint3(cameras, measurements); EXPECT(assert_equal(kLandmark, *triangulated_3cameras, 1e-2)); // Again with nonlinear optimization - boost::optional triangulated_3cameras_opt = + std::optional triangulated_3cameras_opt = triangulatePoint3(cameras, measurements, 1e-9, true); EXPECT(assert_equal(kLandmark, *triangulated_3cameras_opt, 1e-2)); @@ -506,7 +506,7 @@ TEST(triangulation, fourPoses_distinct_Ks_distortion) { const CameraSet> cameras{camera1, camera2}; const Point2Vector measurements{z1, z2}; - boost::optional actual = // + std::optional actual = // triangulatePoint3(cameras, measurements); EXPECT(assert_equal(kLandmark, *actual, 1e-2)); } @@ -727,14 +727,14 @@ TEST(triangulation, twoPoses_sphericalCamera) { // 3. Test simple DLT, now within triangulatePoint3 bool optimize = false; - boost::optional actual1 = // + std::optional actual1 = // triangulatePoint3(cameras, measurements, rank_tol, optimize); EXPECT(assert_equal(kLandmark, *actual1, 1e-7)); // 4. test with optimization on, same answer optimize = true; - boost::optional actual2 = // + std::optional actual2 = // triangulatePoint3(cameras, measurements, rank_tol, optimize); EXPECT(assert_equal(kLandmark, *actual2, 1e-7)); @@ -745,14 +745,14 @@ TEST(triangulation, twoPoses_sphericalCamera) { u1.retract(Vector2(0.01, 0.05)); // note: perturbation smaller for Unit3 measurements.at(1) = u2.retract(Vector2(-0.02, 0.03)); optimize = false; - boost::optional actual3 = // + std::optional actual3 = // triangulatePoint3(cameras, measurements, rank_tol, optimize); EXPECT(assert_equal(Point3(5.9432, 0.654319, 1.48192), *actual3, 1e-3)); // 6. Now with optimization on optimize = true; - boost::optional actual4 = // + std::optional actual4 = // triangulatePoint3(cameras, measurements, rank_tol, optimize); EXPECT(assert_equal(Point3(5.9432, 0.654334, 1.48192), *actual4, 1e-3)); @@ -795,7 +795,7 @@ TEST(triangulation, twoPoses_sphericalCamera_extremeFOV) { rank_tol, optimize), TriangulationCheiralityException); #else // otherwise project should not throw the exception - boost::optional actual1 = // + std::optional actual1 = // triangulatePoint3(cameras, measurements, rank_tol, optimize); EXPECT(assert_equal(landmarkL, *actual1, 1e-7)); @@ -809,7 +809,7 @@ TEST(triangulation, twoPoses_sphericalCamera_extremeFOV) { rank_tol, optimize), TriangulationCheiralityException); #else // otherwise project should not throw the exception - boost::optional actual1 = // + std::optional actual1 = // triangulatePoint3(cameras, measurements, rank_tol, optimize); EXPECT(assert_equal(landmarkL, *actual1, 1e-7)); diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index e0473f8c3..660d9fa3c 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -260,7 +260,7 @@ Cal3_S2 createPinholeCalibration(const CALIBRATION& cal) { template MEASUREMENT undistortMeasurementInternal( const CALIBRATION& cal, const MEASUREMENT& measurement, - boost::optional pinholeCal = boost::none) { + std::optional pinholeCal = {}) { if (!pinholeCal) { pinholeCal = createPinholeCalibration(cal); } @@ -623,7 +623,7 @@ private: * TriangulationResult is an optional point, along with the reasons why it is * invalid. */ -class TriangulationResult : public boost::optional { +class TriangulationResult : public std::optional { public: enum Status { VALID, DEGENERATE, BEHIND_CAMERA, OUTLIER, FAR_POINT }; Status status; @@ -640,7 +640,7 @@ class TriangulationResult : public boost::optional { /** * Constructor */ - TriangulationResult(const Point3& p) : status(VALID) { reset(p); } + TriangulationResult(const Point3& p) : status(VALID) { emplace(p); } static TriangulationResult Degenerate() { return TriangulationResult(DEGENERATE); } diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp index cad1feae1..098d33744 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -664,7 +664,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses) { EXPECT(smartFactor1->triangulateSafe(cameras)); EXPECT(!smartFactor1->isDegenerate()); EXPECT(!smartFactor1->isPointBehindCamera()); - boost::optional p = smartFactor1->point(); + std::optional p = smartFactor1->point(); EXPECT(p); EXPECT(assert_equal(landmark1, *p)); From 0bf269f85479b44ad09fbb01048755b3009e4815 Mon Sep 17 00:00:00 2001 From: kartik arcot Date: Thu, 12 Jan 2023 08:40:47 -0800 Subject: [PATCH 11/38] unit3 --- gtsam/geometry/Unit3.cpp | 6 +++--- gtsam/geometry/Unit3.h | 26 +++++++++++++------------- 2 files changed, 16 insertions(+), 16 deletions(-) diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index 6f70d840e..039b93a3a 100644 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -109,8 +109,8 @@ const Matrix32& Unit3::basis(OptionalJacobian<6, 2> H) const { jacobian.block<3, 2>(3, 0) = H_b2_n * H_n_p + H_b2_b1 * H_b1_p; // Cache the result and jacobian - H_B_.reset(jacobian); - B_.reset(B); + H_B_ = (jacobian); + B_ = (B); } // Return cached jacobian, possibly computed just above @@ -126,7 +126,7 @@ const Matrix32& Unit3::basis(OptionalJacobian<6, 2> H) const { const Point3 B1 = gtsam::cross(n, axis); B.col(0) = normalize(B1); B.col(1) = gtsam::cross(n, B.col(0)); - B_.reset(B); + B_ = (B); } return *B_; diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h index ebd5c63c9..8db13cddc 100644 --- a/gtsam/geometry/Unit3.h +++ b/gtsam/geometry/Unit3.h @@ -45,8 +45,8 @@ class GTSAM_EXPORT Unit3 { private: Vector3 p_; ///< The location of the point on the unit sphere - mutable boost::optional B_; ///< Cached basis - mutable boost::optional H_B_; ///< Cached basis derivative + mutable std::optional B_; ///< Cached basis + mutable std::optional H_B_; ///< Cached basis derivative #ifdef GTSAM_USE_TBB mutable std::mutex B_mutex_; ///< Mutex to protect the cached basis. @@ -98,7 +98,7 @@ public: /// Named constructor from Point3 with optional Jacobian static Unit3 FromPoint3(const Point3& point, // - OptionalJacobian<2, 3> H = boost::none); + OptionalJacobian<2, 3> H = {}); /** * Random direction, using boost::uniform_on_sphere @@ -133,16 +133,16 @@ public: * tangent to the sphere at the current direction. * Provides derivatives of the basis with the two basis vectors stacked up as a 6x1. */ - const Matrix32& basis(OptionalJacobian<6, 2> H = boost::none) const; + const Matrix32& basis(OptionalJacobian<6, 2> H = {}) const; /// Return skew-symmetric associated with 3D point on unit sphere Matrix3 skew() const; /// Return unit-norm Point3 - Point3 point3(OptionalJacobian<3, 2> H = boost::none) const; + Point3 point3(OptionalJacobian<3, 2> H = {}) const; /// Return unit-norm Vector - Vector3 unitVector(OptionalJacobian<3, 2> H = boost::none) const; + Vector3 unitVector(OptionalJacobian<3, 2> H = {}) const; /// Return scaled direction as Point3 friend Point3 operator*(double s, const Unit3& d) { @@ -150,20 +150,20 @@ public: } /// Return dot product with q - double dot(const Unit3& q, OptionalJacobian<1,2> H1 = boost::none, // - OptionalJacobian<1,2> H2 = boost::none) const; + double dot(const Unit3& q, OptionalJacobian<1,2> H1 = {}, // + OptionalJacobian<1,2> H2 = {}) const; /// Signed, vector-valued error between two directions /// @deprecated, errorVector has the proper derivatives, this confusingly has only the second. - Vector2 error(const Unit3& q, OptionalJacobian<2, 2> H_q = boost::none) const; + Vector2 error(const Unit3& q, OptionalJacobian<2, 2> H_q = {}) const; /// Signed, vector-valued error between two directions /// NOTE(hayk): This method has zero derivatives if this (p) and q are orthogonal. - Vector2 errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p = boost::none, // - OptionalJacobian<2, 2> H_q = boost::none) const; + Vector2 errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p = {}, // + OptionalJacobian<2, 2> H_q = {}) const; /// Distance between two directions - double distance(const Unit3& q, OptionalJacobian<1, 2> H = boost::none) const; + double distance(const Unit3& q, OptionalJacobian<1, 2> H = {}) const; /// Cross-product between two Unit3s Unit3 cross(const Unit3& q) const { @@ -196,7 +196,7 @@ public: }; /// The retract function - Unit3 retract(const Vector2& v, OptionalJacobian<2,2> H = boost::none) const; + Unit3 retract(const Vector2& v, OptionalJacobian<2,2> H = {}) const; /// The local coordinates function Vector2 localCoordinates(const Unit3& s) const; From 3bac1efd1f301d2d031a932512c949c97147b298 Mon Sep 17 00:00:00 2001 From: kartik arcot Date: Thu, 12 Jan 2023 09:16:58 -0800 Subject: [PATCH 12/38] inference --- gtsam/discrete/DiscreteFactorGraph.h | 6 +-- gtsam/inference/BayesTree-inst.h | 1 - gtsam/inference/BayesTreeCliqueBase-inst.h | 4 +- gtsam/inference/BayesTreeCliqueBase.h | 8 +-- gtsam/inference/DotWriter.cpp | 20 +++---- gtsam/inference/DotWriter.h | 9 ++-- gtsam/inference/EliminateableFactorGraph.h | 52 +++++++++---------- gtsam/inference/FactorGraph-inst.h | 2 +- gtsam/inference/VariableIndex.h | 1 + gtsam/nonlinear/GraphvizFormatting.cpp | 18 +++---- gtsam/nonlinear/GraphvizFormatting.h | 6 +-- gtsam/nonlinear/NonlinearFactorGraph.cpp | 2 +- .../symbolic/tests/testSymbolicBayesTree.cpp | 2 +- 13 files changed, 67 insertions(+), 64 deletions(-) diff --git a/gtsam/discrete/DiscreteFactorGraph.h b/gtsam/discrete/DiscreteFactorGraph.h index 3fbb64d50..8b30cb963 100644 --- a/gtsam/discrete/DiscreteFactorGraph.h +++ b/gtsam/discrete/DiscreteFactorGraph.h @@ -155,7 +155,7 @@ class GTSAM_EXPORT DiscreteFactorGraph * @return DiscreteBayesNet encoding posterior P(X|Z) */ DiscreteBayesNet sumProduct( - OptionalOrderingType orderingType = boost::none) const; + OptionalOrderingType orderingType = {}) const; /** * @brief Implement the sum-product algorithm @@ -172,7 +172,7 @@ class GTSAM_EXPORT DiscreteFactorGraph * @return DiscreteLookupDAG DAG with lookup tables */ DiscreteLookupDAG maxProduct( - OptionalOrderingType orderingType = boost::none) const; + OptionalOrderingType orderingType = {}) const; /** * @brief Implement the max-product algorithm @@ -189,7 +189,7 @@ class GTSAM_EXPORT DiscreteFactorGraph * @return DiscreteValues : MPE */ DiscreteValues optimize( - OptionalOrderingType orderingType = boost::none) const; + OptionalOrderingType orderingType = {}) const; /** * @brief Find the maximum probable explanation (MPE) by doing max-product. diff --git a/gtsam/inference/BayesTree-inst.h b/gtsam/inference/BayesTree-inst.h index 055971a82..65ba37889 100644 --- a/gtsam/inference/BayesTree-inst.h +++ b/gtsam/inference/BayesTree-inst.h @@ -25,7 +25,6 @@ #include #include -#include #include namespace gtsam { diff --git a/gtsam/inference/BayesTreeCliqueBase-inst.h b/gtsam/inference/BayesTreeCliqueBase-inst.h index 6da7d0fe4..37ed49055 100644 --- a/gtsam/inference/BayesTreeCliqueBase-inst.h +++ b/gtsam/inference/BayesTreeCliqueBase-inst.h @@ -178,7 +178,7 @@ namespace gtsam { this->conditional()->endParents()); auto separatorMarginal = p_Cp.marginalMultifrontalBayesNet(Ordering(indicesS), function); - cachedSeparatorMarginal_.reset(*separatorMarginal); + cachedSeparatorMarginal_ = *separatorMarginal; } } @@ -217,7 +217,7 @@ namespace gtsam { } //Delete CachedShortcut for this clique - cachedSeparatorMarginal_ = boost::none; + cachedSeparatorMarginal_ = {}; } } diff --git a/gtsam/inference/BayesTreeCliqueBase.h b/gtsam/inference/BayesTreeCliqueBase.h index c9bb6db94..4cb0941b9 100644 --- a/gtsam/inference/BayesTreeCliqueBase.h +++ b/gtsam/inference/BayesTreeCliqueBase.h @@ -21,10 +21,10 @@ #include #include #include -#include #include #include +#include namespace gtsam { @@ -102,7 +102,7 @@ namespace gtsam { /// @} /// This stores the Cached separator marginal P(S) - mutable boost::optional cachedSeparatorMarginal_; + mutable std::optional cachedSeparatorMarginal_; /// This protects Cached seperator marginal P(S) from concurrent read/writes /// as many the functions which access it are const (hence the mutable) /// leading to the false impression that these const functions are thread-safe @@ -174,7 +174,7 @@ namespace gtsam { */ void deleteCachedShortcuts(); - const boost::optional& cachedSeparatorMarginal() const { + const std::optional& cachedSeparatorMarginal() const { std::lock_guard marginalLock(cachedSeparatorMarginalMutex_); return cachedSeparatorMarginal_; } @@ -194,7 +194,7 @@ namespace gtsam { /** Non-recursive delete cached shortcuts and marginals - internal only. */ void deleteCachedShortcutsNonRecursive() { std::lock_guard marginalLock(cachedSeparatorMarginalMutex_); - cachedSeparatorMarginal_ = boost::none; + cachedSeparatorMarginal_ = {}; } private: diff --git a/gtsam/inference/DotWriter.cpp b/gtsam/inference/DotWriter.cpp index eac0c90f9..8047c281f 100644 --- a/gtsam/inference/DotWriter.cpp +++ b/gtsam/inference/DotWriter.cpp @@ -40,7 +40,7 @@ void DotWriter::digraphPreamble(ostream* os) const { } void DotWriter::drawVariable(Key key, const KeyFormatter& keyFormatter, - const boost::optional& position, + const std::optional& position, ostream* os) const { // Label the node with the label from the KeyFormatter *os << " var" << key << "[label=\"" << keyFormatter(key) @@ -54,7 +54,7 @@ void DotWriter::drawVariable(Key key, const KeyFormatter& keyFormatter, *os << "];\n"; } -void DotWriter::DrawFactor(size_t i, const boost::optional& position, +void DotWriter::DrawFactor(size_t i, const std::optional& position, ostream* os) { *os << " factor" << i << "[label=\"\", shape=point"; if (position) { @@ -76,26 +76,28 @@ static void ConnectVariableFactor(Key key, const KeyFormatter& keyFormatter, } /// Return variable position or none -boost::optional DotWriter::variablePos(Key key) const { - boost::optional result = boost::none; +std::optional DotWriter::variablePos(Key key) const { + std::optional result = {}; // Check position hint Symbol symbol(key); auto hint = positionHints.find(symbol.chr()); - if (hint != positionHints.end()) - result.reset(Vector2(symbol.index(), hint->second)); + if (hint != positionHints.end()) { + result = Vector2(symbol.index(), hint->second); + } // Override with explicit position, if given. auto pos = variablePositions.find(key); - if (pos != variablePositions.end()) - result.reset(pos->second); + if (pos != variablePositions.end()) { + result = pos->second; + } return result; } void DotWriter::processFactor(size_t i, const KeyVector& keys, const KeyFormatter& keyFormatter, - const boost::optional& position, + const std::optional& position, ostream* os) const { if (plotFactorPoints) { if (binaryEdges && keys.size() == 2) { diff --git a/gtsam/inference/DotWriter.h b/gtsam/inference/DotWriter.h index 4973f0e53..ff20f5fa0 100644 --- a/gtsam/inference/DotWriter.h +++ b/gtsam/inference/DotWriter.h @@ -25,6 +25,7 @@ #include #include #include +#include namespace gtsam { @@ -80,20 +81,20 @@ struct GTSAM_EXPORT DotWriter { /// Create a variable dot fragment. void drawVariable(Key key, const KeyFormatter& keyFormatter, - const boost::optional& position, + const std::optional& position, std::ostream* os) const; /// Create factor dot. - static void DrawFactor(size_t i, const boost::optional& position, + static void DrawFactor(size_t i, const std::optional& position, std::ostream* os); /// Return variable position or none - boost::optional variablePos(Key key) const; + std::optional variablePos(Key key) const; /// Draw a single factor, specified by its index i and its variable keys. void processFactor(size_t i, const KeyVector& keys, const KeyFormatter& keyFormatter, - const boost::optional& position, + const std::optional& position, std::ostream* os) const; }; diff --git a/gtsam/inference/EliminateableFactorGraph.h b/gtsam/inference/EliminateableFactorGraph.h index 900346f7f..c6cf56a11 100644 --- a/gtsam/inference/EliminateableFactorGraph.h +++ b/gtsam/inference/EliminateableFactorGraph.h @@ -20,8 +20,8 @@ #include #include +#include #include -#include #include #include @@ -92,7 +92,7 @@ namespace gtsam { typedef boost::optional OptionalVariableIndex; /// Typedef for an optional ordering type - typedef boost::optional OptionalOrderingType; + typedef std::optional OptionalOrderingType; /** Do sequential elimination of all variables to produce a Bayes net. If an ordering is not * provided, the ordering provided by COLAMD will be used. @@ -111,13 +111,13 @@ namespace gtsam { * \code * VariableIndex varIndex(graph); // Build variable index * Data data = otherFunctionUsingVariableIndex(graph, varIndex); // Other code that uses variable index - * boost::shared_ptr result = graph.eliminateSequential(EliminateQR, varIndex, boost::none); + * boost::shared_ptr result = graph.eliminateSequential(EliminateQR, varIndex, std::nullopt); * \endcode * */ boost::shared_ptr eliminateSequential( - OptionalOrderingType orderingType = boost::none, + OptionalOrderingType orderingType = {}, const Eliminate& function = EliminationTraitsType::DefaultEliminate, - OptionalVariableIndex variableIndex = boost::none) const; + OptionalVariableIndex variableIndex = {}) const; /** Do sequential elimination of all variables to produce a Bayes net. * @@ -130,13 +130,13 @@ namespace gtsam { * \code * VariableIndex varIndex(graph); // Build variable index * Data data = otherFunctionUsingVariableIndex(graph, varIndex); // Other code that uses variable index - * boost::shared_ptr result = graph.eliminateSequential(myOrdering, EliminateQR, varIndex, boost::none); + * boost::shared_ptr result = graph.eliminateSequential(myOrdering, EliminateQR, varIndex, std::nullopt); * \endcode * */ boost::shared_ptr eliminateSequential( const Ordering& ordering, const Eliminate& function = EliminationTraitsType::DefaultEliminate, - OptionalVariableIndex variableIndex = boost::none) const; + OptionalVariableIndex variableIndex = {}) const; /** Do multifrontal elimination of all variables to produce a Bayes tree. If an ordering is not * provided, the ordering will be computed using either COLAMD or METIS, dependeing on @@ -151,13 +151,13 @@ namespace gtsam { * \code * VariableIndex varIndex(graph); // Build variable index * Data data = otherFunctionUsingVariableIndex(graph, varIndex); // Other code that uses variable index - * boost::shared_ptr result = graph.eliminateMultifrontal(EliminateQR, boost::none, varIndex); + * boost::shared_ptr result = graph.eliminateMultifrontal(EliminateQR, {}, varIndex); * \endcode * */ boost::shared_ptr eliminateMultifrontal( - OptionalOrderingType orderingType = boost::none, + OptionalOrderingType orderingType = {}, const Eliminate& function = EliminationTraitsType::DefaultEliminate, - OptionalVariableIndex variableIndex = boost::none) const; + OptionalVariableIndex variableIndex = {}) const; /** Do multifrontal elimination of all variables to produce a Bayes tree. If an ordering is not * provided, the ordering will be computed using either COLAMD or METIS, dependeing on @@ -171,7 +171,7 @@ namespace gtsam { boost::shared_ptr eliminateMultifrontal( const Ordering& ordering, const Eliminate& function = EliminationTraitsType::DefaultEliminate, - OptionalVariableIndex variableIndex = boost::none) const; + OptionalVariableIndex variableIndex = {}) const; /** Do sequential elimination of some variables, in \c ordering provided, to produce a Bayes net * and a remaining factor graph. This computes the factorization \f$ p(X) = p(A|B) p(B) \f$, @@ -181,7 +181,7 @@ namespace gtsam { eliminatePartialSequential( const Ordering& ordering, const Eliminate& function = EliminationTraitsType::DefaultEliminate, - OptionalVariableIndex variableIndex = boost::none) const; + OptionalVariableIndex variableIndex = {}) const; /** Do sequential elimination of the given \c variables in an ordering computed by COLAMD to * produce a Bayes net and a remaining factor graph. This computes the factorization \f$ p(X) @@ -191,7 +191,7 @@ namespace gtsam { eliminatePartialSequential( const KeyVector& variables, const Eliminate& function = EliminationTraitsType::DefaultEliminate, - OptionalVariableIndex variableIndex = boost::none) const; + OptionalVariableIndex variableIndex = {}) const; /** Do multifrontal elimination of some variables, in \c ordering provided, to produce a Bayes * tree and a remaining factor graph. This computes the factorization \f$ p(X) = p(A|B) p(B) @@ -201,7 +201,7 @@ namespace gtsam { eliminatePartialMultifrontal( const Ordering& ordering, const Eliminate& function = EliminationTraitsType::DefaultEliminate, - OptionalVariableIndex variableIndex = boost::none) const; + OptionalVariableIndex variableIndex = {}) const; /** Do multifrontal elimination of the given \c variables in an ordering computed by COLAMD to * produce a Bayes tree and a remaining factor graph. This computes the factorization \f$ p(X) @@ -211,7 +211,7 @@ namespace gtsam { eliminatePartialMultifrontal( const KeyVector& variables, const Eliminate& function = EliminationTraitsType::DefaultEliminate, - OptionalVariableIndex variableIndex = boost::none) const; + OptionalVariableIndex variableIndex = {}) const; /** Compute the marginal of the requested variables and return the result as a Bayes net. Uses * COLAMD marginalization ordering by default @@ -225,7 +225,7 @@ namespace gtsam { boost::shared_ptr marginalMultifrontalBayesNet( boost::variant variables, const Eliminate& function = EliminationTraitsType::DefaultEliminate, - OptionalVariableIndex variableIndex = boost::none) const; + OptionalVariableIndex variableIndex = {}) const; /** Compute the marginal of the requested variables and return the result as a Bayes net. * @param variables Determines the variables whose marginal to compute, if provided as an @@ -241,7 +241,7 @@ namespace gtsam { boost::variant variables, const Ordering& marginalizedVariableOrdering, const Eliminate& function = EliminationTraitsType::DefaultEliminate, - OptionalVariableIndex variableIndex = boost::none) const; + OptionalVariableIndex variableIndex = {}) const; /** Compute the marginal of the requested variables and return the result as a Bayes tree. Uses * COLAMD marginalization order by default @@ -255,7 +255,7 @@ namespace gtsam { boost::shared_ptr marginalMultifrontalBayesTree( boost::variant variables, const Eliminate& function = EliminationTraitsType::DefaultEliminate, - OptionalVariableIndex variableIndex = boost::none) const; + OptionalVariableIndex variableIndex = {}) const; /** Compute the marginal of the requested variables and return the result as a Bayes tree. * @param variables Determines the variables whose marginal to compute, if provided as an @@ -271,13 +271,13 @@ namespace gtsam { boost::variant variables, const Ordering& marginalizedVariableOrdering, const Eliminate& function = EliminationTraitsType::DefaultEliminate, - OptionalVariableIndex variableIndex = boost::none) const; + OptionalVariableIndex variableIndex = {}) const; /** Compute the marginal factor graph of the requested variables. */ boost::shared_ptr marginal( const KeyVector& variables, const Eliminate& function = EliminationTraitsType::DefaultEliminate, - OptionalVariableIndex variableIndex = boost::none) const; + OptionalVariableIndex variableIndex = {}) const; private: @@ -301,8 +301,8 @@ namespace gtsam { /** @deprecated orderingType specified first for consistency */ boost::shared_ptr GTSAM_DEPRECATED eliminateSequential( const Eliminate& function, - OptionalVariableIndex variableIndex = boost::none, - OptionalOrderingType orderingType = boost::none) const { + OptionalVariableIndex variableIndex = {}, + OptionalOrderingType orderingType = {}) const { return eliminateSequential(orderingType, function, variableIndex); } @@ -318,8 +318,8 @@ namespace gtsam { /** @deprecated orderingType specified first for consistency */ boost::shared_ptr GTSAM_DEPRECATED eliminateMultifrontal( const Eliminate& function, - OptionalVariableIndex variableIndex = boost::none, - OptionalOrderingType orderingType = boost::none) const { + OptionalVariableIndex variableIndex = {}, + OptionalOrderingType orderingType = {}) const { return eliminateMultifrontal(orderingType, function, variableIndex); } @@ -328,7 +328,7 @@ namespace gtsam { boost::variant variables, boost::none_t, const Eliminate& function = EliminationTraitsType::DefaultEliminate, - OptionalVariableIndex variableIndex = boost::none) const { + OptionalVariableIndex variableIndex = {}) const { return marginalMultifrontalBayesNet(variables, function, variableIndex); } @@ -337,7 +337,7 @@ namespace gtsam { boost::variant variables, boost::none_t, const Eliminate& function = EliminationTraitsType::DefaultEliminate, - OptionalVariableIndex variableIndex = boost::none) const { + OptionalVariableIndex variableIndex = {}) const { return marginalMultifrontalBayesTree(variables, function, variableIndex); } #endif diff --git a/gtsam/inference/FactorGraph-inst.h b/gtsam/inference/FactorGraph-inst.h index 355fdf87e..8460bbe2c 100644 --- a/gtsam/inference/FactorGraph-inst.h +++ b/gtsam/inference/FactorGraph-inst.h @@ -155,7 +155,7 @@ void FactorGraph::dot(std::ostream& os, const auto& factor = at(i); if (factor) { const KeyVector& factorKeys = factor->keys(); - writer.processFactor(i, factorKeys, keyFormatter, boost::none, &os); + writer.processFactor(i, factorKeys, keyFormatter, {}, &os); } } diff --git a/gtsam/inference/VariableIndex.h b/gtsam/inference/VariableIndex.h index f934a72af..1357eacdd 100644 --- a/gtsam/inference/VariableIndex.h +++ b/gtsam/inference/VariableIndex.h @@ -28,6 +28,7 @@ #include #include +#include namespace gtsam { diff --git a/gtsam/nonlinear/GraphvizFormatting.cpp b/gtsam/nonlinear/GraphvizFormatting.cpp index ca3466b6a..95c6e7faa 100644 --- a/gtsam/nonlinear/GraphvizFormatting.cpp +++ b/gtsam/nonlinear/GraphvizFormatting.cpp @@ -34,7 +34,7 @@ Vector2 GraphvizFormatting::findBounds(const Values& values, min.y() = std::numeric_limits::infinity(); for (const Key& key : keys) { if (values.exists(key)) { - boost::optional xy = extractPosition(values.at(key)); + std::optional xy = extractPosition(values.at(key)); if (xy) { if (xy->x() < min.x()) min.x() = xy->x(); if (xy->y() < min.y()) min.y() = xy->y(); @@ -44,7 +44,7 @@ Vector2 GraphvizFormatting::findBounds(const Values& values, return min; } -boost::optional GraphvizFormatting::extractPosition( +std::optional GraphvizFormatting::extractPosition( const Value& value) const { Vector3 t; if (const GenericValue* p = @@ -62,7 +62,7 @@ boost::optional GraphvizFormatting::extractPosition( const Eigen::Ref p_3d(p->value()); t = p_3d; } else { - return boost::none; + return {}; } } else if (const GenericValue* p = dynamic_cast*>(&value)) { @@ -71,7 +71,7 @@ boost::optional GraphvizFormatting::extractPosition( dynamic_cast*>(&value)) { t = p->value(); } else { - return boost::none; + return {}; } double x, y; switch (paperHorizontalAxis) { @@ -121,11 +121,11 @@ boost::optional GraphvizFormatting::extractPosition( return Vector2(x, y); } -boost::optional GraphvizFormatting::variablePos(const Values& values, +std::optional GraphvizFormatting::variablePos(const Values& values, const Vector2& min, Key key) const { if (!values.exists(key)) return DotWriter::variablePos(key); - boost::optional xy = extractPosition(values.at(key)); + std::optional xy = extractPosition(values.at(key)); if (xy) { xy->x() = scale * (xy->x() - min.x()); xy->y() = scale * (xy->y() - min.y()); @@ -133,11 +133,11 @@ boost::optional GraphvizFormatting::variablePos(const Values& values, return xy; } -boost::optional GraphvizFormatting::factorPos(const Vector2& min, +std::optional GraphvizFormatting::factorPos(const Vector2& min, size_t i) const { - if (factorPositions.size() == 0) return boost::none; + if (factorPositions.size() == 0) return {}; auto it = factorPositions.find(i); - if (it == factorPositions.end()) return boost::none; + if (it == factorPositions.end()) return {}; auto pos = it->second; return Vector2(scale * (pos.x() - min.x()), scale * (pos.y() - min.y())); } diff --git a/gtsam/nonlinear/GraphvizFormatting.h b/gtsam/nonlinear/GraphvizFormatting.h index 03cdb3469..fc3cfc95e 100644 --- a/gtsam/nonlinear/GraphvizFormatting.h +++ b/gtsam/nonlinear/GraphvizFormatting.h @@ -53,14 +53,14 @@ struct GTSAM_EXPORT GraphvizFormatting : public DotWriter { Vector2 findBounds(const Values& values, const KeySet& keys) const; /// Extract a Vector2 from either Vector2, Pose2, Pose3, or Point3 - boost::optional extractPosition(const Value& value) const; + std::optional extractPosition(const Value& value) const; /// Return affinely transformed variable position if it exists. - boost::optional variablePos(const Values& values, const Vector2& min, + std::optional variablePos(const Values& values, const Vector2& min, Key key) const; /// Return affinely transformed factor position if it exists. - boost::optional factorPos(const Vector2& min, size_t i) const; + std::optional factorPos(const Vector2& min, size_t i) const; }; } // namespace gtsam diff --git a/gtsam/nonlinear/NonlinearFactorGraph.cpp b/gtsam/nonlinear/NonlinearFactorGraph.cpp index dfa54f26f..3b7efd790 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.cpp +++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp @@ -129,7 +129,7 @@ void NonlinearFactorGraph::dot(std::ostream& os, const Values& values, // Create factors and variable connections size_t i = 0; for (const KeyVector& factorKeys : structure) { - writer.processFactor(i++, factorKeys, keyFormatter, boost::none, &os); + writer.processFactor(i++, factorKeys, keyFormatter, {}, &os); } } else { // Create factors and variable connections diff --git a/gtsam/symbolic/tests/testSymbolicBayesTree.cpp b/gtsam/symbolic/tests/testSymbolicBayesTree.cpp index 0095c9293..f1cc6ee4f 100644 --- a/gtsam/symbolic/tests/testSymbolicBayesTree.cpp +++ b/gtsam/symbolic/tests/testSymbolicBayesTree.cpp @@ -213,7 +213,7 @@ TEST(BayesTree, shortcutCheck) { // Check if all the cached shortcuts are cleared rootClique->deleteCachedShortcuts(); for (SymbolicBayesTree::sharedClique& clique : allCliques) { - bool notCleared = clique->cachedSeparatorMarginal().is_initialized(); + bool notCleared = clique->cachedSeparatorMarginal().has_value(); CHECK(notCleared == false); } EXPECT_LONGS_EQUAL(0, (long)rootClique->numCachedSeparatorMarginals()); From ee65c85442d7c89beb36a4061f772da2be425ace Mon Sep 17 00:00:00 2001 From: kartik arcot Date: Thu, 12 Jan 2023 13:11:55 -0800 Subject: [PATCH 13/38] linear, navigation, std::optional serialization tests --- examples/SFMExample_SmartFactor.cpp | 4 +- examples/SFMExample_SmartFactorPCG.cpp | 4 +- examples/TriangulationLOSTExample.cpp | 7 +- gtsam/base/serializationTestHelpers.h | 1 + gtsam/base/std_optional_serialization.h | 30 ++--- .../tests/testStdOptionalSerialization.cpp | 110 ++++++++++++++++++ gtsam/linear/AcceleratedPowerMethod.h | 4 +- gtsam/linear/IterativeSolver.h | 2 +- gtsam/linear/JacobianFactor.cpp | 2 +- gtsam/linear/NoiseModel.cpp | 18 +-- gtsam/linear/NoiseModel.h | 10 +- gtsam/linear/PowerMethod.h | 5 +- gtsam/navigation/AHRSFactor.cpp | 4 +- gtsam/navigation/AHRSFactor.h | 6 +- gtsam/navigation/ImuBias.cpp | 2 +- gtsam/navigation/MagPoseFactor.h | 7 +- gtsam/navigation/NavState.cpp | 2 +- gtsam/navigation/NavState.h | 24 ++-- gtsam/navigation/PreintegratedRotation.cpp | 3 +- gtsam/navigation/PreintegratedRotation.h | 30 ++--- gtsam/navigation/PreintegrationBase.h | 20 ++-- gtsam/navigation/tests/testAHRSFactor.cpp | 7 +- gtsam/navigation/tests/testMagPoseFactor.cpp | 8 +- gtsam/sfm/ShonanAveraging.cpp | 2 +- 24 files changed, 208 insertions(+), 104 deletions(-) create mode 100644 gtsam/base/tests/testStdOptionalSerialization.cpp diff --git a/examples/SFMExample_SmartFactor.cpp b/examples/SFMExample_SmartFactor.cpp index 9d90cd897..c3400a0f9 100644 --- a/examples/SFMExample_SmartFactor.cpp +++ b/examples/SFMExample_SmartFactor.cpp @@ -114,10 +114,10 @@ int main(int argc, char* argv[]) { // The graph stores Factor shared_ptrs, so we cast back to a SmartFactor first SmartFactor::shared_ptr smart = boost::dynamic_pointer_cast(graph[j]); if (smart) { - // The output of point() is in boost::optional, as sometimes + // The output of point() is in std::optional, as sometimes // the triangulation operation inside smart factor will encounter degeneracy. auto point = smart->point(result); - if (point) // ignore if boost::optional return nullptr + if (point) // ignore if std::optional return nullptr landmark_result.insert(j, *point); } } diff --git a/examples/SFMExample_SmartFactorPCG.cpp b/examples/SFMExample_SmartFactorPCG.cpp index 3f553efc6..2f03a4ef0 100644 --- a/examples/SFMExample_SmartFactorPCG.cpp +++ b/examples/SFMExample_SmartFactorPCG.cpp @@ -110,8 +110,8 @@ int main(int argc, char* argv[]) { for (size_t j = 0; j < points.size(); ++j) { auto smart = boost::dynamic_pointer_cast(graph[j]); if (smart) { - boost::optional point = smart->point(result); - if (point) // ignore if boost::optional return nullptr + std::optional point = smart->point(result); + if (point) // ignore if std::optional return nullptr landmark_result.insert(j, *point); } } diff --git a/examples/TriangulationLOSTExample.cpp b/examples/TriangulationLOSTExample.cpp index 923606995..4bb280dd1 100644 --- a/examples/TriangulationLOSTExample.cpp +++ b/examples/TriangulationLOSTExample.cpp @@ -29,6 +29,7 @@ #include #include #include +#include using namespace std; using namespace gtsam; @@ -145,9 +146,9 @@ int main(int argc, char* argv[]) { cameras, noisyMeasurements, rank_tol, true, measurementNoise, false); durationDLTOpt += std::chrono::high_resolution_clock::now() - dltOptStart; - errorsLOST.row(i) = *estimateLOST - landmark; - errorsDLT.row(i) = *estimateDLT - landmark; - errorsDLTOpt.row(i) = *estimateDLTOpt - landmark; + errorsLOST.row(i) = estimateLOST - landmark; + errorsDLT.row(i) = estimateDLT - landmark; + errorsDLTOpt.row(i) = estimateDLTOpt - landmark; } PrintCovarianceStats(errorsLOST, "LOST"); PrintCovarianceStats(errorsDLT, "DLT"); diff --git a/gtsam/base/serializationTestHelpers.h b/gtsam/base/serializationTestHelpers.h index bb8574245..7aab743db 100644 --- a/gtsam/base/serializationTestHelpers.h +++ b/gtsam/base/serializationTestHelpers.h @@ -24,6 +24,7 @@ #include #include +#include #include #include diff --git a/gtsam/base/std_optional_serialization.h b/gtsam/base/std_optional_serialization.h index 22a4369e8..ae85907c8 100644 --- a/gtsam/base/std_optional_serialization.h +++ b/gtsam/base/std_optional_serialization.h @@ -26,24 +26,11 @@ void save(Archive& ar, const std::optional& 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 as optional + // default constructor. BOOST_STATIC_ASSERT(boost::serialization::detail::is_default_constructible::value || boost::is_pointer::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::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); } } @@ -58,16 +45,11 @@ void load(Archive& ar, std::optional& t, const unsigned int /*version*/ 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); + if (!t.has_value()) { + // Need to be default constructible + t.emplace(); } - detail::stack_allocate 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()); + ar >> boost::serialization::make_nvp("value", *t); } template @@ -75,6 +57,8 @@ void serialize(Archive& ar, std::optional& t, const unsigned int version) { boost::serialization::split_free(ar, t, version); } +// derive boost::xml_archive_impl for archiving std::optional with xml + } // namespace serialization } // namespace boost diff --git a/gtsam/base/tests/testStdOptionalSerialization.cpp b/gtsam/base/tests/testStdOptionalSerialization.cpp new file mode 100644 index 000000000..5affb9e69 --- /dev/null +++ b/gtsam/base/tests/testStdOptionalSerialization.cpp @@ -0,0 +1,110 @@ +#include + +#include +#include +#include +#include + +#include "gtsam/base/serializationTestHelpers.h" +#include "gtsam/base/std_optional_serialization.h" + +using namespace gtsam; + +// A test to check that the serialization of std::optional works with boost +TEST(StdOptionalSerialization, SerializeIntOptional) { + // Create an optional + std::optional opt(42); + + // Serialize it + std::stringstream ss; + boost::archive::text_oarchive oa(ss); + oa << opt; + + // Deserialize it + std::optional opt2; + boost::archive::text_iarchive ia(ss); + ia >> opt2; + + // Check that it worked + EXPECT(opt2.has_value()); + EXPECT(*opt2 == 42); +} + +// A test to check if we serialize an empty optional, it is deserialized as an +// empty optional +TEST(StdOptionalSerialization, SerializeEmptyOptional) { + // Create an optional + std::optional opt; + + // Serialize it + std::stringstream ss; + boost::archive::text_oarchive oa(ss); + oa << opt; + + // Deserialize it + std::optional opt2 = 43; + boost::archive::text_iarchive ia(ss); + ia >> opt2; + + // Check that it worked + EXPECT(!opt2.has_value()); +} + +/* ************************************************************************* */ +// Implement the equals trait for TestOptionalStruct +namespace gtsam { +// A struct which contains an optional +class TestOptionalStruct { +public: + std::optional opt; + TestOptionalStruct() = default; + TestOptionalStruct(const int& opt) + : opt(opt) {} + friend class boost::serialization::access; + template + void serialize(Archive& ar, const unsigned int /*version*/) { + ar& BOOST_SERIALIZATION_NVP(opt); + } +}; + +template <> +struct traits { + static bool Equals(const TestOptionalStruct& q1, const TestOptionalStruct& q2, double) { + // if both have values then compare the values + if (q1.opt.has_value() && q2.opt.has_value()) { + return *q1.opt == *q2.opt; + } + // otherwise check that both are empty + return q1.opt == q2.opt; + } + + static void Print(const TestOptionalStruct& m, const std::string& s = "") { + /* std::cout << s << "opt: " << m.opt << std::endl; */ + if (m.opt.has_value()) { + std::cout << s << "opt: " << *m.opt << std::endl; + } else { + std::cout << s << "opt: empty" << std::endl; + } + } +}; +} // namespace gtsam + +// Test for a struct with an initialized optional +TEST(StdOptionalSerialization, SerializTestOptionalStruct) { + TestOptionalStruct optStruct(10); + EXPECT(serializationTestHelpers::equalsObj(optStruct)); + EXPECT(serializationTestHelpers::equalsXML(optStruct)); + EXPECT(serializationTestHelpers::equalsBinary(optStruct)); +} + +// Test for a struct with an uninitialized optional +TEST(StdOptionalSerialization, SerializTestOptionalStructUninitialized) { + TestOptionalStruct optStruct; + EXPECT(serializationTestHelpers::equalsObj(optStruct)); + EXPECT(serializationTestHelpers::equalsXML(optStruct)); + EXPECT(serializationTestHelpers::equalsBinary(optStruct)); +} +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} diff --git a/gtsam/linear/AcceleratedPowerMethod.h b/gtsam/linear/AcceleratedPowerMethod.h index daa59fc1b..795a9f329 100644 --- a/gtsam/linear/AcceleratedPowerMethod.h +++ b/gtsam/linear/AcceleratedPowerMethod.h @@ -60,11 +60,11 @@ class AcceleratedPowerMethod : public PowerMethod { * vector as ritzVector */ explicit AcceleratedPowerMethod( - const Operator &A, const boost::optional initial = boost::none, + const Operator &A, const std::optional initial = {}, double initialBeta = 0.0) : PowerMethod(A, initial) { // initialize Ritz eigen vector and previous vector - this->ritzVector_ = initial ? initial.get() : Vector::Random(this->dim_); + this->ritzVector_ = initial ? *initial : Vector::Random(this->dim_); this->ritzVector_.normalize(); previousVector_ = Vector::Zero(this->dim_); diff --git a/gtsam/linear/IterativeSolver.h b/gtsam/linear/IterativeSolver.h index cd3f41659..743f072f2 100644 --- a/gtsam/linear/IterativeSolver.h +++ b/gtsam/linear/IterativeSolver.h @@ -23,11 +23,11 @@ #include #include -#include #include #include #include +#include namespace gtsam { diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 84083c4bc..6e1e7d359 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -264,7 +264,7 @@ void JacobianFactor::JacobianFactorHelper(const GaussianFactorGraph& graph, // Copy the RHS vectors and sigmas gttic(copy_vectors); bool anyConstrained = false; - boost::optional sigmas; + std::optional sigmas; // Loop over source jacobians DenseIndex nextRow = 0; for (size_t factorI = 0; factorI < jacobians.size(); ++factorI) { diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index 7108a7660..1cbbd1456 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -47,7 +47,7 @@ void updateAb(MATRIX& Ab, int j, const Vector& a, const Vector& rd) { /* ************************************************************************* */ // check *above the diagonal* for non-zero entries -boost::optional checkIfDiagonal(const Matrix& M) { +std::optional checkIfDiagonal(const Matrix& M) { size_t m = M.rows(), n = M.cols(); assert(m > 0); // check all non-diagonal entries @@ -61,7 +61,7 @@ boost::optional checkIfDiagonal(const Matrix& M) { break; } if (full) { - return boost::none; + return {}; } else { Vector diagonal(n); for (j = 0; j < n; j++) @@ -88,7 +88,7 @@ Gaussian::shared_ptr Gaussian::SqrtInformation(const Matrix& R, bool smart) { if (m != n) throw invalid_argument("Gaussian::SqrtInformation: R not square"); if (smart) { - boost::optional diagonal = checkIfDiagonal(R); + std::optional diagonal = checkIfDiagonal(R); if (diagonal) return Diagonal::Sigmas(diagonal->array().inverse(), true); } @@ -101,7 +101,7 @@ Gaussian::shared_ptr Gaussian::Information(const Matrix& information, bool smart size_t m = information.rows(), n = information.cols(); if (m != n) throw invalid_argument("Gaussian::Information: R not square"); - boost::optional diagonal = boost::none; + std::optional diagonal = {}; if (smart) diagonal = checkIfDiagonal(information); if (diagonal) @@ -119,7 +119,7 @@ Gaussian::shared_ptr Gaussian::Covariance(const Matrix& covariance, size_t m = covariance.rows(), n = covariance.cols(); if (m != n) throw invalid_argument("Gaussian::Covariance: covariance not square"); - boost::optional variances = boost::none; + std::optional variances = {}; if (smart) variances = checkIfDiagonal(covariance); if (variances) @@ -426,8 +426,8 @@ Constrained::shared_ptr Constrained::unit() const { // Check whether column a triggers a constraint and corresponding variable is deterministic // Return constraint_row with maximum element in case variable plays in multiple constraints template -boost::optional check_if_constraint(VECTOR a, const Vector& invsigmas, size_t m) { - boost::optional constraint_row; +std::optional check_if_constraint(VECTOR a, const Vector& invsigmas, size_t m) { + std::optional constraint_row; // not zero, so roundoff errors will not be counted // TODO(frank): that's a fairly crude way of dealing with roundoff errors :-( double max_element = 1e-9; @@ -437,7 +437,7 @@ boost::optional check_if_constraint(VECTOR a, const Vector& invsigmas, s double abs_ai = std::abs(a(i,0)); if (abs_ai > max_element) { max_element = abs_ai; - constraint_row.reset(i); + constraint_row = i; } } return constraint_row; @@ -468,7 +468,7 @@ SharedDiagonal Constrained::QR(Matrix& Ab) const { Eigen::Block a = Ab.block(0, j, m, 1); // Check whether we need to handle as a constraint - boost::optional constraint_row = check_if_constraint(a, invsigmas, m); + std::optional constraint_row = check_if_constraint(a, invsigmas, m); if (constraint_row) { // Handle this as a constraint, as the i^th row has zero sigma with non-zero entry A(i,j) diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 7bcf808e5..c23d35883 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -20,6 +20,7 @@ #include #include +#include #include #include @@ -27,7 +28,8 @@ #include #include #include -#include + +#include namespace gtsam { @@ -164,7 +166,7 @@ namespace gtsam { protected: /** Matrix square root of information matrix (R) */ - boost::optional sqrt_information_; + std::optional sqrt_information_; private: @@ -184,7 +186,7 @@ namespace gtsam { /** constructor takes square root information matrix */ Gaussian(size_t dim = 1, - const boost::optional& sqrt_information = boost::none) + const std::optional& sqrt_information = {}) : Base(dim), sqrt_information_(sqrt_information) {} ~Gaussian() override {} @@ -732,7 +734,7 @@ namespace gtsam { }; // Helper function - GTSAM_EXPORT boost::optional checkIfDiagonal(const Matrix& M); + GTSAM_EXPORT std::optional checkIfDiagonal(const Matrix& M); } // namespace noiseModel diff --git a/gtsam/linear/PowerMethod.h b/gtsam/linear/PowerMethod.h index 8834777cc..2c7027ebe 100644 --- a/gtsam/linear/PowerMethod.h +++ b/gtsam/linear/PowerMethod.h @@ -26,6 +26,7 @@ #include #include #include +#include namespace gtsam { @@ -75,10 +76,10 @@ class PowerMethod { /// Construct from the aim matrix and intial ritz vector explicit PowerMethod(const Operator &A, - const boost::optional initial = boost::none) + const std::optional initial = {}) : A_(A), dim_(A.rows()), nrIterations_(0) { Vector x0; - x0 = initial ? initial.get() : Vector::Random(dim_); + x0 = initial ? *initial : Vector::Random(dim_); x0.normalize(); // initialize Ritz eigen value diff --git a/gtsam/navigation/AHRSFactor.cpp b/gtsam/navigation/AHRSFactor.cpp index f00655902..aeaff58eb 100644 --- a/gtsam/navigation/AHRSFactor.cpp +++ b/gtsam/navigation/AHRSFactor.cpp @@ -185,7 +185,7 @@ Rot3 AHRSFactor::Predict(const Rot3& rot_i, const Vector3& bias, AHRSFactor::AHRSFactor(Key rot_i, Key rot_j, Key bias, const PreintegratedAhrsMeasurements& pim, const Vector3& omegaCoriolis, - const boost::optional& body_P_sensor) + const std::optional& body_P_sensor) : Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), rot_i, rot_j, bias), _PIM_(pim) { @@ -198,7 +198,7 @@ AHRSFactor::AHRSFactor(Key rot_i, Key rot_j, Key bias, Rot3 AHRSFactor::predict(const Rot3& rot_i, const Vector3& bias, const PreintegratedAhrsMeasurements& pim, const Vector3& omegaCoriolis, - const boost::optional& body_P_sensor) { + const std::optional& body_P_sensor) { auto p = boost::make_shared(pim.p()); p->omegaCoriolis = omegaCoriolis; p->body_P_sensor = body_P_sensor; diff --git a/gtsam/navigation/AHRSFactor.h b/gtsam/navigation/AHRSFactor.h index e19e53a1c..576874555 100644 --- a/gtsam/navigation/AHRSFactor.h +++ b/gtsam/navigation/AHRSFactor.h @@ -24,6 +24,8 @@ #include #include +#include + namespace gtsam { /** @@ -194,13 +196,13 @@ public: AHRSFactor(Key rot_i, Key rot_j, Key bias, const PreintegratedAhrsMeasurements& pim, const Vector3& omegaCoriolis, - const boost::optional& body_P_sensor = boost::none); + const std::optional& body_P_sensor = {}); /// @deprecated static function, but used in tests. static Rot3 predict( const Rot3& rot_i, const Vector3& bias, const PreintegratedAhrsMeasurements& pim, const Vector3& omegaCoriolis, - const boost::optional& body_P_sensor = boost::none); + const std::optional& body_P_sensor = {}); #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 /// @deprecated name diff --git a/gtsam/navigation/ImuBias.cpp b/gtsam/navigation/ImuBias.cpp index bc11f95f8..49a364ca0 100644 --- a/gtsam/navigation/ImuBias.cpp +++ b/gtsam/navigation/ImuBias.cpp @@ -38,7 +38,7 @@ namespace imuBias { // // H1: Jacobian w.r.t. IMUBias // // H2: Jacobian w.r.t. pose // Vector CorrectGyroWithEarthRotRate(Vector measurement, const Pose3& pose, const Vector& w_earth_rate_G, -// boost::optional H1=boost::none, boost::optional H2=boost::none) const { +// Matrix* H1=nullptr, Matrix* H2=nullptr) const { // // Matrix R_G_to_I( pose.rotation().matrix().transpose() ); // Vector w_earth_rate_I = R_G_to_I * w_earth_rate_G; diff --git a/gtsam/navigation/MagPoseFactor.h b/gtsam/navigation/MagPoseFactor.h index d69a0e0a4..67a312af9 100644 --- a/gtsam/navigation/MagPoseFactor.h +++ b/gtsam/navigation/MagPoseFactor.h @@ -11,9 +11,12 @@ #pragma once +#include #include #include +#include + namespace gtsam { /** @@ -35,7 +38,7 @@ class MagPoseFactor: public NoiseModelFactorN { const Point measured_; ///< The measured magnetometer data in the body frame. const Point nM_; ///< Local magnetic field (mag output units) in the nav frame. const Point bias_; ///< The bias vector (mag output units) in the body frame. - boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame. + std::optional body_P_sensor_; ///< The pose of the sensor in the body frame. static const int MeasDim = Point::RowsAtCompileTime; static const int PoseDim = traits::dimension; @@ -74,7 +77,7 @@ class MagPoseFactor: public NoiseModelFactorN { const Point& direction, const Point& bias, const SharedNoiseModel& model, - const boost::optional& body_P_sensor) + const std::optional& body_P_sensor) : Base(model, pose_key), measured_(body_P_sensor ? body_P_sensor->rotation() * measured : measured), nM_(scale * direction.normalized()), diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index f21a2f660..86e85a762 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -256,7 +256,7 @@ Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder, //------------------------------------------------------------------------------ Vector9 NavState::correctPIM(const Vector9& pim, double dt, - const Vector3& n_gravity, const boost::optional& omegaCoriolis, + const Vector3& n_gravity, const std::optional& omegaCoriolis, bool use2ndOrderCoriolis, OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const { const Rot3& nRb = R_; diff --git a/gtsam/navigation/NavState.h b/gtsam/navigation/NavState.h index f66e9d7a9..5e44e16c2 100644 --- a/gtsam/navigation/NavState.h +++ b/gtsam/navigation/NavState.h @@ -79,9 +79,9 @@ public: /// @name Component Access /// @{ - const Rot3& attitude(OptionalJacobian<3, 9> H = boost::none) const; - const Point3& position(OptionalJacobian<3, 9> H = boost::none) const; - const Velocity3& velocity(OptionalJacobian<3, 9> H = boost::none) const; + const Rot3& attitude(OptionalJacobian<3, 9> H = {}) const; + const Point3& position(OptionalJacobian<3, 9> H = {}) const; + const Velocity3& velocity(OptionalJacobian<3, 9> H = {}) const; const Pose3 pose() const { return Pose3(attitude(), position()); @@ -108,7 +108,7 @@ public: return v_; } // Return velocity in body frame - Velocity3 bodyVelocity(OptionalJacobian<3, 9> H = boost::none) const; + Velocity3 bodyVelocity(OptionalJacobian<3, 9> H = {}) const; /// Return matrix group representation, in MATLAB notation: /// nTb = [nRb 0 n_t; 0 nRb n_v; 0 0 1] @@ -156,13 +156,13 @@ public: /// retract with optional derivatives NavState retract(const Vector9& v, // - OptionalJacobian<9, 9> H1 = boost::none, OptionalJacobian<9, 9> H2 = - boost::none) const; + OptionalJacobian<9, 9> H1 = {}, OptionalJacobian<9, 9> H2 = + {}) const; /// localCoordinates with optional derivatives Vector9 localCoordinates(const NavState& g, // - OptionalJacobian<9, 9> H1 = boost::none, OptionalJacobian<9, 9> H2 = - boost::none) const; + OptionalJacobian<9, 9> H1 = {}, OptionalJacobian<9, 9> H2 = + {}) const; /// @} /// @name Dynamics @@ -176,14 +176,14 @@ public: /// Compute tangent space contribution due to Coriolis forces Vector9 coriolis(double dt, const Vector3& omega, bool secondOrder = false, - OptionalJacobian<9, 9> H = boost::none) const; + OptionalJacobian<9, 9> H = {}) const; /// Correct preintegrated tangent vector with our velocity and rotated gravity, /// taking into account Coriolis forces if omegaCoriolis is given. Vector9 correctPIM(const Vector9& pim, double dt, const Vector3& n_gravity, - const boost::optional& omegaCoriolis, bool use2ndOrderCoriolis = - false, OptionalJacobian<9, 9> H1 = boost::none, - OptionalJacobian<9, 9> H2 = boost::none) const; + const std::optional& omegaCoriolis, bool use2ndOrderCoriolis = + false, OptionalJacobian<9, 9> H1 = {}, + OptionalJacobian<9, 9> H2 = {}) const; /// @} diff --git a/gtsam/navigation/PreintegratedRotation.cpp b/gtsam/navigation/PreintegratedRotation.cpp index ee30bee9e..a9d4a28bb 100644 --- a/gtsam/navigation/PreintegratedRotation.cpp +++ b/gtsam/navigation/PreintegratedRotation.cpp @@ -115,8 +115,7 @@ void PreintegratedRotation::integrateMeasurement(const Vector3& measuredOmega, Rot3 PreintegratedRotation::biascorrectedDeltaRij(const Vector3& biasOmegaIncr, OptionalJacobian<3, 3> H) const { const Vector3 biasInducedOmega = delRdelBiasOmega_ * biasOmegaIncr; - const Rot3 deltaRij_biascorrected = deltaRij_.expmap(biasInducedOmega, - boost::none, H); + const Rot3 deltaRij_biascorrected = deltaRij_.expmap(biasInducedOmega,{}, H); if (H) (*H) *= delRdelBiasOmega_; return deltaRij_biascorrected; diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h index e52d28e1e..007deeeca 100644 --- a/gtsam/navigation/PreintegratedRotation.h +++ b/gtsam/navigation/PreintegratedRotation.h @@ -23,6 +23,7 @@ #include #include +#include namespace gtsam { @@ -32,16 +33,17 @@ struct GTSAM_EXPORT PreintegratedRotationParams { /// Continuous-time "Covariance" of gyroscope measurements /// The units for stddev are σ = rad/s/√Hz Matrix3 gyroscopeCovariance; - boost::optional omegaCoriolis; ///< Coriolis constant - boost::optional body_P_sensor; ///< The pose of the sensor in the body frame + std::optional omegaCoriolis; ///< Coriolis constant + std::optional body_P_sensor; ///< The pose of the sensor in the body frame PreintegratedRotationParams() : gyroscopeCovariance(I_3x3) {} PreintegratedRotationParams(const Matrix3& gyroscope_covariance, - boost::optional omega_coriolis) + std::optional omega_coriolis) : gyroscopeCovariance(gyroscope_covariance) { - if (omega_coriolis) - omegaCoriolis.reset(omega_coriolis.get()); + if (omega_coriolis) { + omegaCoriolis = *omega_coriolis; + } } virtual ~PreintegratedRotationParams() {} @@ -50,12 +52,12 @@ struct GTSAM_EXPORT PreintegratedRotationParams { virtual bool equals(const PreintegratedRotationParams& other, double tol=1e-9) const; void setGyroscopeCovariance(const Matrix3& cov) { gyroscopeCovariance = cov; } - void setOmegaCoriolis(const Vector3& omega) { omegaCoriolis.reset(omega); } - void setBodyPSensor(const Pose3& pose) { body_P_sensor.reset(pose); } + void setOmegaCoriolis(const Vector3& omega) { omegaCoriolis = omega; } + void setBodyPSensor(const Pose3& pose) { body_P_sensor = pose; } const Matrix3& getGyroscopeCovariance() const { return gyroscopeCovariance; } - boost::optional getOmegaCoriolis() const { return omegaCoriolis; } - boost::optional getBodyPSensor() const { return body_P_sensor; } + std::optional getOmegaCoriolis() const { return omegaCoriolis; } + std::optional getBodyPSensor() const { return body_P_sensor; } private: /** Serialization function */ @@ -66,8 +68,8 @@ struct GTSAM_EXPORT PreintegratedRotationParams { ar & BOOST_SERIALIZATION_NVP(gyroscopeCovariance); ar & BOOST_SERIALIZATION_NVP(body_P_sensor); - // Provide support for Eigen::Matrix in boost::optional - bool omegaCoriolisFlag = omegaCoriolis.is_initialized(); + // Provide support for Eigen::Matrix in std::optional + bool omegaCoriolisFlag = omegaCoriolis.has_value(); ar & boost::serialization::make_nvp("omegaCoriolisFlag", omegaCoriolisFlag); if (omegaCoriolisFlag) { ar & BOOST_SERIALIZATION_NVP(*omegaCoriolis); @@ -164,12 +166,12 @@ class GTSAM_EXPORT PreintegratedRotation { /// Calculate an incremental rotation given the gyro measurement and a time interval, /// and update both deltaTij_ and deltaRij_. void integrateMeasurement(const Vector3& measuredOmega, const Vector3& biasHat, double deltaT, - OptionalJacobian<3, 3> D_incrR_integratedOmega = boost::none, - OptionalJacobian<3, 3> F = boost::none); + OptionalJacobian<3, 3> D_incrR_integratedOmega = {}, + OptionalJacobian<3, 3> F = {}); /// Return a bias corrected version of the integrated rotation, with optional Jacobian Rot3 biascorrectedDeltaRij(const Vector3& biasOmegaIncr, - OptionalJacobian<3, 3> H = boost::none) const; + OptionalJacobian<3, 3> H = {}) const; /// Integrate coriolis correction in body frame rot_i Vector3 integrateCoriolis(const Rot3& rot_i) const; diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index e118a6232..a5e9e9391 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -129,9 +129,9 @@ class GTSAM_EXPORT PreintegrationBase { */ std::pair correctMeasurementsBySensorPose( const Vector3& unbiasedAcc, const Vector3& unbiasedOmega, - OptionalJacobian<3, 3> correctedAcc_H_unbiasedAcc = boost::none, - OptionalJacobian<3, 3> correctedAcc_H_unbiasedOmega = boost::none, - OptionalJacobian<3, 3> correctedOmega_H_unbiasedOmega = boost::none) const; + OptionalJacobian<3, 3> correctedAcc_H_unbiasedAcc = {}, + OptionalJacobian<3, 3> correctedAcc_H_unbiasedOmega = {}, + OptionalJacobian<3, 3> correctedOmega_H_unbiasedOmega = {}) const; /** * Update preintegrated measurements and get derivatives @@ -148,12 +148,12 @@ class GTSAM_EXPORT PreintegrationBase { /// Given the estimate of the bias, return a NavState tangent vector /// summarizing the preintegrated IMU measurements so far virtual Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i, - OptionalJacobian<9, 6> H = boost::none) const = 0; + OptionalJacobian<9, 6> H = {}) const = 0; /// Predict state at time j NavState predict(const NavState& state_i, const imuBias::ConstantBias& bias_i, - OptionalJacobian<9, 9> H1 = boost::none, - OptionalJacobian<9, 6> H2 = boost::none) const; + OptionalJacobian<9, 9> H1 = {}, + OptionalJacobian<9, 6> H2 = {}) const; /// Calculate error given navStates Vector9 computeError(const NavState& state_i, const NavState& state_j, @@ -167,10 +167,10 @@ class GTSAM_EXPORT PreintegrationBase { */ Vector9 computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, - const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H1 = - boost::none, OptionalJacobian<9, 3> H2 = boost::none, - OptionalJacobian<9, 6> H3 = boost::none, OptionalJacobian<9, 3> H4 = - boost::none, OptionalJacobian<9, 6> H5 = boost::none) const; + const imuBias::ConstantBias& bias_i, + OptionalJacobian<9, 6> H1 = {}, OptionalJacobian<9, 3> H2 = {}, + OptionalJacobian<9, 6> H3 = {}, OptionalJacobian<9, 3> H4 = {}, + OptionalJacobian<9, 6> H5 = {}) const; private: /** Serialization function */ diff --git a/gtsam/navigation/tests/testAHRSFactor.cpp b/gtsam/navigation/tests/testAHRSFactor.cpp index 779f6abcc..e6a246b9c 100644 --- a/gtsam/navigation/tests/testAHRSFactor.cpp +++ b/gtsam/navigation/tests/testAHRSFactor.cpp @@ -140,7 +140,7 @@ TEST( AHRSFactor, PreintegratedAhrsMeasurementsConstructor ) { EXPECT(assert_equal(gyroscopeCovariance, actualPim.p().getGyroscopeCovariance(), 1e-6)); EXPECT(assert_equal(omegaCoriolis, - actualPim.p().getOmegaCoriolis().get(), 1e-6)); + *(actualPim.p().getOmegaCoriolis()), 1e-6)); EXPECT(assert_equal(bias, actualPim.biasHat(), 1e-6)); DOUBLES_EQUAL(deltaTij, actualPim.deltaTij(), 1e-6); EXPECT(assert_equal(deltaRij, Rot3(actualPim.deltaRij()), 1e-6)); @@ -163,7 +163,7 @@ TEST(AHRSFactor, Error) { pim.integrateMeasurement(measuredOmega, deltaT); // Create factor - AHRSFactor factor(X(1), X(2), B(1), pim, kZeroOmegaCoriolis, boost::none); + AHRSFactor factor(X(1), X(2), B(1), pim, kZeroOmegaCoriolis, {}); Vector3 errorActual = factor.evaluateError(x1, x2, bias); @@ -458,8 +458,7 @@ TEST (AHRSFactor, predictTest) { // PreintegratedAhrsMeasurements::predict Matrix expectedH = numericalDerivative11( - std::bind(&PreintegratedAhrsMeasurements::predict, - &pim, std::placeholders::_1, boost::none), bias); + [&pim](const Vector3& b) { return pim.predict(b, {}); }, bias); // Actual Jacobians Matrix H; diff --git a/gtsam/navigation/tests/testMagPoseFactor.cpp b/gtsam/navigation/tests/testMagPoseFactor.cpp index 9fb81cd7f..cd9c2fa49 100644 --- a/gtsam/navigation/tests/testMagPoseFactor.cpp +++ b/gtsam/navigation/tests/testMagPoseFactor.cpp @@ -57,8 +57,8 @@ Pose3 body_P3_sensor(Rot3::RzRyRx(Vector3(1.5, 0.9, -1.15)), // ***************************************************************************** TEST(MagPoseFactor, Constructors) { - MagPoseFactor f2a(Symbol('X', 0), measured2, scale, dir2, bias2, model2, boost::none); - MagPoseFactor f3a(Symbol('X', 0), measured3, scale, dir3, bias3, model3, boost::none); + MagPoseFactor f2a(Symbol('X', 0), measured2, scale, dir2, bias2, model2, {}); + MagPoseFactor f3a(Symbol('X', 0), measured3, scale, dir3, bias3, model3, {}); // Try constructing with a body_P_sensor set. MagPoseFactor f2b = MagPoseFactor( @@ -75,7 +75,7 @@ TEST(MagPoseFactor, JacobianPose2) { Matrix H2; // Error should be zero at the groundtruth pose. - MagPoseFactor f(Symbol('X', 0), measured2, scale, dir2, bias2, model2, boost::none); + MagPoseFactor f(Symbol('X', 0), measured2, scale, dir2, bias2, model2, {}); CHECK(gtsam::assert_equal(Z_2x1, f.evaluateError(n_P2_b, H2), 1e-5)); CHECK(gtsam::assert_equal(gtsam::numericalDerivative11 // ([&f] (const Pose2& p) {return f.evaluateError(p);}, @@ -88,7 +88,7 @@ TEST(MagPoseFactor, JacobianPose3) { Matrix H3; // Error should be zero at the groundtruth pose. - MagPoseFactor f(Symbol('X', 0), measured3, scale, dir3, bias3, model3, boost::none); + MagPoseFactor f(Symbol('X', 0), measured3, scale, dir3, bias3, model3, {}); CHECK(gtsam::assert_equal(Z_3x1, f.evaluateError(n_P3_b, H3), 1e-5)); CHECK(gtsam::assert_equal(gtsam::numericalDerivative11 // ([&f] (const Pose3& p) {return f.evaluateError(p);}, diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp index ea4171238..9e5645a57 100644 --- a/gtsam/sfm/ShonanAveraging.cpp +++ b/gtsam/sfm/ShonanAveraging.cpp @@ -554,7 +554,7 @@ static bool PowerMinimumEigenValue( } const Sparse C = pmEigenValue * Matrix::Identity(A.rows(), A.cols()).sparseView() - A; - const boost::optional initial = perturb(S.row(0)); + const std::optional initial = perturb(S.row(0)); AcceleratedPowerMethod apmShiftedOperator(C, initial); const bool minConverged = apmShiftedOperator.compute( From 1f833a0bc3afc995883efba2518fed12ba9c3bb6 Mon Sep 17 00:00:00 2001 From: kartik arcot Date: Thu, 12 Jan 2023 13:35:48 -0800 Subject: [PATCH 14/38] nonlinear --- gtsam/nonlinear/ISAM2-impl.h | 6 +++--- gtsam/nonlinear/ISAM2.cpp | 6 +++--- gtsam/nonlinear/ISAM2.h | 8 ++++---- gtsam/nonlinear/ISAM2Result.h | 16 +++++++++++----- gtsam/nonlinear/ISAM2UpdateParams.h | 8 ++++---- gtsam/nonlinear/NonlinearOptimizerParams.h | 2 +- gtsam/nonlinear/internal/ExecutionTrace.h | 7 ++++--- .../nonlinear/ConcurrentIncrementalFilter.cpp | 10 +++++----- .../nonlinear/ConcurrentIncrementalFilter.h | 4 ++-- .../nonlinear/IncrementalFixedLagSmoother.cpp | 6 +++--- .../nonlinear/IncrementalFixedLagSmoother.h | 2 +- tests/testExpressionFactor.cpp | 6 +++--- 12 files changed, 44 insertions(+), 37 deletions(-) diff --git a/gtsam/nonlinear/ISAM2-impl.h b/gtsam/nonlinear/ISAM2-impl.h index eb2285b28..906d620cc 100644 --- a/gtsam/nonlinear/ISAM2-impl.h +++ b/gtsam/nonlinear/ISAM2-impl.h @@ -78,7 +78,7 @@ struct GTSAM_EXPORT DeltaImpl { size_t nFullSystemVars; enum { /*AS_ADDED,*/ COLAMD } algorithm; enum { NO_CONSTRAINT, CONSTRAIN_LAST } constrain; - boost::optional > constrainedKeys; + std::optional > constrainedKeys; }; /** @@ -195,9 +195,9 @@ struct GTSAM_EXPORT UpdateImpl { // Calculate nonlinear error void error(const NonlinearFactorGraph& nonlinearFactors, - const Values& estimate, boost::optional* result) const { + const Values& estimate, std::optional* result) const { gttic(error); - result->reset(nonlinearFactors.error(estimate)); + *result = nonlinearFactors.error(estimate); } // Mark linear update diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index d5e25cef8..de32aea9c 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -396,9 +396,9 @@ void ISAM2::removeVariables(const KeySet& unusedKeys) { ISAM2Result ISAM2::update( const NonlinearFactorGraph& newFactors, const Values& newTheta, const FactorIndices& removeFactorIndices, - const boost::optional >& constrainedKeys, - const boost::optional >& noRelinKeys, - const boost::optional >& extraReelimKeys, + const std::optional >& constrainedKeys, + const std::optional >& noRelinKeys, + const std::optional >& extraReelimKeys, bool force_relinearize) { ISAM2UpdateParams params; params.constrainedKeys = constrainedKeys; diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index dd246617a..8b48f2f7d 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -87,7 +87,7 @@ class GTSAM_EXPORT ISAM2 : public BayesTree { ISAM2Params params_; /** The current Dogleg Delta (trust region radius) */ - mutable boost::optional doglegDelta_; + mutable std::optional doglegDelta_; /** Set of variables that are involved with linear factors from marginalized * variables and thus cannot have their linearization points changed. */ @@ -152,9 +152,9 @@ class GTSAM_EXPORT ISAM2 : public BayesTree { const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(), const FactorIndices& removeFactorIndices = FactorIndices(), - const boost::optional >& constrainedKeys = boost::none, - const boost::optional >& noRelinKeys = boost::none, - const boost::optional >& extraReelimKeys = boost::none, + const std::optional >& constrainedKeys = {}, + const std::optional >& noRelinKeys = {}, + const std::optional >& extraReelimKeys = {}, bool force_relinearize = false); /** diff --git a/gtsam/nonlinear/ISAM2Result.h b/gtsam/nonlinear/ISAM2Result.h index 077174970..26049f9da 100644 --- a/gtsam/nonlinear/ISAM2Result.h +++ b/gtsam/nonlinear/ISAM2Result.h @@ -51,7 +51,7 @@ struct ISAM2Result { * ISAM2Params::evaluateNonlinearError is set to \c true, because there is * some cost to this computation. */ - boost::optional errorBefore; + std::optional errorBefore; /** The nonlinear error of all of the factors computed after the current * update, meaning that variables above the relinearization threshold @@ -63,7 +63,7 @@ struct ISAM2Result { * ISAM2Params::evaluateNonlinearError is set to \c true, because there is * some cost to this computation. */ - boost::optional errorAfter; + std::optional errorAfter; /** The number of variables that were relinearized because their linear * deltas exceeded the reslinearization threshold @@ -155,14 +155,20 @@ struct ISAM2Result { /** Detailed results, if enabled by ISAM2Params::enableDetailedResults. See * Detail for information about the results data stored here. */ - boost::optional detail; + std::optional detail; explicit ISAM2Result(bool enableDetailedResults = false) { - if (enableDetailedResults) detail.reset(DetailedResults()); + if (enableDetailedResults) detail = DetailedResults(); } /// Return pointer to detail, 0 if no detail requested - DetailedResults* details() { return detail.get_ptr(); } + DetailedResults* details() { + if (detail.has_value()) { + return &(*detail); + } else { + return nullptr; + } + } /// Print results void print(const std::string str = "") const { diff --git a/gtsam/nonlinear/ISAM2UpdateParams.h b/gtsam/nonlinear/ISAM2UpdateParams.h index 0a3be1daf..575c99be5 100644 --- a/gtsam/nonlinear/ISAM2UpdateParams.h +++ b/gtsam/nonlinear/ISAM2UpdateParams.h @@ -37,16 +37,16 @@ struct ISAM2UpdateParams { /** An optional map of keys to group labels, such that a variable can be * constrained to a particular grouping in the BayesTree */ - boost::optional> constrainedKeys{boost::none}; + std::optional> constrainedKeys{{}}; /** An optional set of nonlinear keys that iSAM2 will hold at a constant * linearization point, regardless of the size of the linear delta */ - boost::optional> noRelinKeys{boost::none}; + std::optional> noRelinKeys{{}}; /** An optional set of nonlinear keys that iSAM2 will re-eliminate, regardless * of the size of the linear delta. This allows the provided keys to be * reordered. */ - boost::optional> extraReelimKeys{boost::none}; + std::optional> extraReelimKeys{{}}; /** Relinearize any variables whose delta magnitude is sufficiently large * (Params::relinearizeThreshold), regardless of the relinearization @@ -63,7 +63,7 @@ struct ISAM2UpdateParams { * depend on Keys `X(2)`, `X(3)`. Next call to ISAM2::update() must include * its `newAffectedKeys` field with the map `13 -> {X(2), X(3)}`. */ - boost::optional> newAffectedKeys{boost::none}; + std::optional> newAffectedKeys{{}}; /** By default, iSAM2 uses a wildfire update scheme that stops updating when * the deltas become too small down in the tree. This flagg forces a full diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.h b/gtsam/nonlinear/NonlinearOptimizerParams.h index 218230421..6809137ef 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.h +++ b/gtsam/nonlinear/NonlinearOptimizerParams.h @@ -104,7 +104,7 @@ public: }; LinearSolverType linearSolverType = MULTIFRONTAL_CHOLESKY; ///< The type of linear solver to use in the nonlinear optimizer - boost::optional ordering; ///< The optional variable elimination ordering, or empty to use COLAMD (default: empty) + std::optional ordering; ///< The optional variable elimination ordering, or empty to use COLAMD (default: empty) IterativeOptimizationParameters::shared_ptr iterativeParams; ///< The container for iterativeOptimization parameters. used in CG Solvers. NonlinearOptimizerParams() = default; diff --git a/gtsam/nonlinear/internal/ExecutionTrace.h b/gtsam/nonlinear/internal/ExecutionTrace.h index 2943b5e68..17557ba3a 100644 --- a/gtsam/nonlinear/internal/ExecutionTrace.h +++ b/gtsam/nonlinear/internal/ExecutionTrace.h @@ -26,6 +26,7 @@ #include #include +#include namespace gtsam { namespace internal { @@ -132,12 +133,12 @@ class ExecutionTrace { /// Return record pointer, quite unsafe, used only for testing template - boost::optional record() { + std::optional record() { if (kind != Function) - return boost::none; + return {}; else { Record* p = dynamic_cast(content.ptr); - return p ? boost::optional(p) : boost::none; + return p ? std::optional(p) : std::nullopt; } } diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp index b9cf66a97..6ef4f9066 100644 --- a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp @@ -43,7 +43,7 @@ bool ConcurrentIncrementalFilter::equals(const ConcurrentFilter& rhs, double tol /* ************************************************************************* */ ConcurrentIncrementalFilter::Result ConcurrentIncrementalFilter::update(const NonlinearFactorGraph& newFactors, const Values& newTheta, - const boost::optional >& keysToMove, const boost::optional< FactorIndices >& removeFactorIndices) { + const std::optional >& keysToMove, const std::optional< FactorIndices >& removeFactorIndices) { gttic(update); @@ -63,7 +63,7 @@ ConcurrentIncrementalFilter::Result ConcurrentIncrementalFilter::update(const No } // Generate ordering constraints that force the 'keys to move' to the end - boost::optional > orderingConstraints = boost::none; + std::optional > orderingConstraints = {}; if(keysToMove && keysToMove->size() > 0) { orderingConstraints = FastMap(); int group = 1; @@ -86,10 +86,10 @@ ConcurrentIncrementalFilter::Result ConcurrentIncrementalFilter::update(const No // Create the set of linear keys that iSAM2 should hold constant // iSAM2 takes care of this for us; no need to specify additional noRelin keys - boost::optional > noRelinKeys = boost::none; + std::optional > noRelinKeys = {}; // Mark additional keys between the 'keys to move' and the leaves - boost::optional > additionalKeys = boost::none; + std::optional > additionalKeys = {}; if(keysToMove && keysToMove->size() > 0) { std::set markedKeys; for(Key key: *keysToMove) { @@ -211,7 +211,7 @@ void ConcurrentIncrementalFilter::synchronize(const NonlinearFactorGraph& smooth // Remove the old factors on the separator and insert the new ones FactorIndices removeFactors(currentSmootherSummarizationSlots_.begin(), currentSmootherSummarizationSlots_.end()); - ISAM2Result result = isam2_.update(currentSmootherSummarization, Values(), removeFactors, boost::none, noRelinKeys, boost::none, false); + ISAM2Result result = isam2_.update(currentSmootherSummarization, Values(), removeFactors, {}, noRelinKeys, {}, false); currentSmootherSummarizationSlots_ = result.newFactorsIndices; // Set the previous smoother summarization to the current smoother summarization and clear the smoother shortcut diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.h b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.h index eb1174749..8e1409523 100644 --- a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.h +++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.h @@ -123,8 +123,8 @@ public: * @param removeFactorIndices An optional set of indices corresponding to the factors you want to remove from the graph */ Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(), - const boost::optional >& keysToMove = boost::none, - const boost::optional< FactorIndices >& removeFactorIndices = boost::none); + const std::optional >& keysToMove = {}, + const std::optional< FactorIndices >& removeFactorIndices = {}); /** * Perform any required operations before the synchronization process starts. diff --git a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp index d29dfe8ca..52e56260d 100644 --- a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp @@ -76,7 +76,7 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update( } FastVector removedFactors; - boost::optional > constrainedKeys = boost::none; + std::optional > constrainedKeys = {}; // Update the Timestamps associated with the factor keys updateKeyTimestampMap(timestamps); @@ -126,7 +126,7 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update( // Update iSAM2 isamResult_ = isam_.update(newFactors, newTheta, - factorsToRemove, constrainedKeys, boost::none, additionalMarkedKeys); + factorsToRemove, constrainedKeys, {}, additionalMarkedKeys); if (debug) { PrintSymbolicTree(isam_, @@ -175,7 +175,7 @@ void IncrementalFixedLagSmoother::eraseKeysBefore(double timestamp) { /* ************************************************************************* */ void IncrementalFixedLagSmoother::createOrderingConstraints( const KeyVector& marginalizableKeys, - boost::optional >& constrainedKeys) const { + std::optional >& constrainedKeys) const { if (marginalizableKeys.size() > 0) { constrainedKeys = FastMap(); // Generate ordering constraints so that the marginalizable variables will be eliminated first diff --git a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h index 4079dbb23..b2910b139 100644 --- a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h +++ b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h @@ -137,7 +137,7 @@ protected: /** Fill in an iSAM2 ConstrainedKeys structure such that the provided keys are eliminated before all others */ void createOrderingConstraints(const KeyVector& marginalizableKeys, - boost::optional >& constrainedKeys) const; + std::optional >& constrainedKeys) const; private: /** Private methods for printing debug information */ diff --git a/tests/testExpressionFactor.cpp b/tests/testExpressionFactor.cpp index bb35ada19..5a3678392 100644 --- a/tests/testExpressionFactor.cpp +++ b/tests/testExpressionFactor.cpp @@ -204,7 +204,7 @@ TEST(ExpressionFactor, Binary) { expected22 << 1, 0, 0, 1; // Check matrices - boost::optional r = trace.record(); + std::optional r = trace.record(); CHECK(r); EXPECT(assert_equal(expected25, (Matrix ) (*r)->dTdA1, 1e-9)); EXPECT(assert_equal(expected22, (Matrix ) (*r)->dTdA2, 1e-9)); @@ -257,7 +257,7 @@ TEST(ExpressionFactor, Shallow) { expected23 << 1, 0, 0, 0, 1, 0; // Check matrices - boost::optional r = trace.record(); + std::optional r = trace.record(); CHECK(r); EXPECT(assert_equal(expected23, (Matrix)(*r)->dTdA1, 1e-9)); @@ -623,7 +623,7 @@ TEST(ExpressionFactor, MultiplyWithInverseFunction) { CHECK(assert_equal(A * b, Ab)); CHECK(assert_equal( numericalDerivative11( - std::bind(f, std::placeholders::_1, b, boost::none, boost::none), a), + [&](const Point2& a) { return f(a, b, {}, {}); }, a), H1)); Values values; From 51e60cc8e0ecc31f2a5a1108ba8887c054cf6fb3 Mon Sep 17 00:00:00 2001 From: kartik arcot Date: Thu, 12 Jan 2023 13:39:52 -0800 Subject: [PATCH 15/38] sfm --- gtsam/sfm/DsfTrackGenerator.h | 6 +++--- gtsam/sfm/SfmData.cpp | 4 ++-- gtsam/sfm/SfmData.h | 8 ++++---- gtsam/sfm/ShonanGaugeFactor.h | 4 ++-- 4 files changed, 11 insertions(+), 11 deletions(-) diff --git a/gtsam/sfm/DsfTrackGenerator.h b/gtsam/sfm/DsfTrackGenerator.h index 14ec2302d..518a5d10b 100644 --- a/gtsam/sfm/DsfTrackGenerator.h +++ b/gtsam/sfm/DsfTrackGenerator.h @@ -43,13 +43,13 @@ struct Keypoints { // Optional scale of the detections, of shape N. // Note: gtsam::Vector is typedef'd for Eigen::VectorXd. - boost::optional scales; + std::optional scales; /// Optional confidences/responses for each detection, of shape N. - boost::optional responses; + std::optional responses; Keypoints(const Eigen::MatrixX2d& coordinates) - : coordinates(coordinates){}; // boost::none + : coordinates(coordinates){}; // std::nullopt }; using KeypointsVector = std::vector; diff --git a/gtsam/sfm/SfmData.cpp b/gtsam/sfm/SfmData.cpp index 6c2676e48..1e27c332f 100644 --- a/gtsam/sfm/SfmData.cpp +++ b/gtsam/sfm/SfmData.cpp @@ -424,8 +424,8 @@ NonlinearFactorGraph SfmData::generalSfmFactors( /* ************************************************************************** */ NonlinearFactorGraph SfmData::sfmFactorGraph( - const SharedNoiseModel &model, boost::optional fixedCamera, - boost::optional fixedPoint) const { + const SharedNoiseModel &model, std::optional fixedCamera, + std::optional fixedPoint) const { NonlinearFactorGraph graph = generalSfmFactors(model); using noiseModel::Constrained; if (fixedCamera) { diff --git a/gtsam/sfm/SfmData.h b/gtsam/sfm/SfmData.h index f445d74da..6b69523a9 100644 --- a/gtsam/sfm/SfmData.h +++ b/gtsam/sfm/SfmData.h @@ -102,14 +102,14 @@ struct GTSAM_EXPORT SfmData { * Note: pose keys are simply integer indices, points use Symbol('p', j). * * @param model a noise model for projection errors - * @param fixedCamera which camera to fix, if any (use boost::none if none) - * @param fixedPoint which point to fix, if any (use boost::none if none) + * @param fixedCamera which camera to fix, if any (use std::nullopt if none) + * @param fixedPoint which point to fix, if any (use std::nullopt if none) * @return NonlinearFactorGraph */ NonlinearFactorGraph sfmFactorGraph( const SharedNoiseModel& model = noiseModel::Isotropic::Sigma(2, 1.0), - boost::optional fixedCamera = 0, - boost::optional fixedPoint = 0) const; + std::optional fixedCamera = 0, + std::optional fixedPoint = 0) const; /// @} /// @name Testable diff --git a/gtsam/sfm/ShonanGaugeFactor.h b/gtsam/sfm/ShonanGaugeFactor.h index d814aafa1..3152ad9d0 100644 --- a/gtsam/sfm/ShonanGaugeFactor.h +++ b/gtsam/sfm/ShonanGaugeFactor.h @@ -56,7 +56,7 @@ public: * the Jacobian will be multiplied with 1/sigma = sqrt(gamma). */ ShonanGaugeFactor(Key key, size_t p, size_t d = 3, - boost::optional gamma = boost::none) + std::optional gamma = {}) : NonlinearFactor(KeyVector{key}) { if (p < d) { throw std::invalid_argument("ShonanGaugeFactor must have p>=d."); @@ -103,4 +103,4 @@ public: }; // \ShonanGaugeFactor -} // namespace gtsam \ No newline at end of file +} // namespace gtsam From 0ae90f6262398cc62ffb038530702837b23ae991 Mon Sep 17 00:00:00 2001 From: kartik arcot Date: Thu, 12 Jan 2023 13:43:29 -0800 Subject: [PATCH 16/38] gtsam_unstable/discrete --- gtsam_unstable/discrete/AllDiff.cpp | 3 ++- gtsam_unstable/discrete/Domain.cpp | 4 ++-- gtsam_unstable/discrete/Domain.h | 3 ++- gtsam_unstable/discrete/Scheduler.cpp | 4 ++-- gtsam_unstable/discrete/Scheduler.h | 4 ++-- 5 files changed, 10 insertions(+), 8 deletions(-) diff --git a/gtsam_unstable/discrete/AllDiff.cpp b/gtsam_unstable/discrete/AllDiff.cpp index bff524bc2..cf598b3f4 100644 --- a/gtsam_unstable/discrete/AllDiff.cpp +++ b/gtsam_unstable/discrete/AllDiff.cpp @@ -10,6 +10,7 @@ #include #include +#include namespace gtsam { @@ -64,7 +65,7 @@ bool AllDiff::ensureArcConsistency(Key j, Domains* domains) const { // a value in domains->at(j) that does not occur in any other connected domain. // If found, we make this a singleton... // TODO: make a new constraint where this really is true - boost::optional maybeChanged = Dj.checkAllDiff(keys_, *domains); + std::optional maybeChanged = Dj.checkAllDiff(keys_, *domains); if (maybeChanged) { Dj = *maybeChanged; return true; diff --git a/gtsam_unstable/discrete/Domain.cpp b/gtsam_unstable/discrete/Domain.cpp index cf0da42e7..f2b7d9f95 100644 --- a/gtsam_unstable/discrete/Domain.cpp +++ b/gtsam_unstable/discrete/Domain.cpp @@ -61,7 +61,7 @@ bool Domain::ensureArcConsistency(Key j, Domains* domains) const { } /* ************************************************************************* */ -boost::optional Domain::checkAllDiff(const KeyVector keys, +std::optional Domain::checkAllDiff(const KeyVector keys, const Domains& domains) const { Key j = key(); // for all values in this domain @@ -74,7 +74,7 @@ boost::optional Domain::checkAllDiff(const KeyVector keys, return Domain(this->discreteKey(), value); found:; } - return boost::none; // we did not change it + return {}; // we did not change it } /* ************************************************************************* */ diff --git a/gtsam_unstable/discrete/Domain.h b/gtsam_unstable/discrete/Domain.h index 1047101c5..5d96f0dc9 100644 --- a/gtsam_unstable/discrete/Domain.h +++ b/gtsam_unstable/discrete/Domain.h @@ -9,6 +9,7 @@ #include #include +#include namespace gtsam { @@ -100,7 +101,7 @@ class GTSAM_UNSTABLE_EXPORT Domain : public Constraint { * @param keys connected domains through alldiff * @param keys other domains */ - boost::optional checkAllDiff(const KeyVector keys, + std::optional checkAllDiff(const KeyVector keys, const Domains& domains) const; /// Partially apply known values diff --git a/gtsam_unstable/discrete/Scheduler.cpp b/gtsam_unstable/discrete/Scheduler.cpp index b86df6c29..9543ab7cf 100644 --- a/gtsam_unstable/discrete/Scheduler.cpp +++ b/gtsam_unstable/discrete/Scheduler.cpp @@ -79,7 +79,7 @@ void Scheduler::addStudent(const string& studentName, const string& area1, /** get key for student and area, 0 is time slot itself */ const DiscreteKey& Scheduler::key(size_t s, - boost::optional area) const { + std::optional area) const { return area ? students_[s].keys_[*area] : students_[s].key_; } @@ -100,7 +100,7 @@ const string& Scheduler::studentArea(size_t i, size_t area) const { /** Add student-specific constraints to the graph */ void Scheduler::addStudentSpecificConstraints(size_t i, - boost::optional slot) { + std::optional slot) { bool debug = ISDEBUG("Scheduler::buildGraph"); assert(i < nrStudents()); diff --git a/gtsam_unstable/discrete/Scheduler.h b/gtsam_unstable/discrete/Scheduler.h index 8d269e81a..f7b45d54a 100644 --- a/gtsam_unstable/discrete/Scheduler.h +++ b/gtsam_unstable/discrete/Scheduler.h @@ -108,7 +108,7 @@ class GTSAM_UNSTABLE_EXPORT Scheduler : public CSP { /** get key for student and area, 0 is time slot itself */ const DiscreteKey& key(size_t s, - boost::optional area = boost::none) const; + std::optional area = {}) const; /** addStudent has to be called after adding slots and faculty */ void addStudent(const std::string& studentName, const std::string& area1, @@ -124,7 +124,7 @@ class GTSAM_UNSTABLE_EXPORT Scheduler : public CSP { /** Add student-specific constraints to the graph */ void addStudentSpecificConstraints( - size_t i, boost::optional slot = boost::none); + size_t i, std::optional slot = {}); /** Main routine that builds factor graph */ void buildGraph(size_t mutexBound = 7); From 4bc27952997bbc7ea1f50b8f2d48ce5aa43e7ae0 Mon Sep 17 00:00:00 2001 From: kartik arcot Date: Thu, 12 Jan 2023 13:48:16 -0800 Subject: [PATCH 17/38] gtsam_unstable/nonlinear --- gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp | 4 ++-- gtsam_unstable/nonlinear/ConcurrentBatchFilter.h | 4 ++-- gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp | 2 +- gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h | 2 +- gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp | 2 +- gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.h | 2 +- 6 files changed, 8 insertions(+), 8 deletions(-) diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp index 83d0ab719..d6456cd51 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp @@ -119,7 +119,7 @@ bool ConcurrentBatchFilter::equals(const ConcurrentFilter& rhs, double tol) cons /* ************************************************************************* */ ConcurrentBatchFilter::Result ConcurrentBatchFilter::update(const NonlinearFactorGraph& newFactors, const Values& newTheta, - const boost::optional >& keysToMove, const boost::optional< std::vector >& removeFactorIndices) { + const std::optional >& keysToMove, const std::optional< std::vector >& removeFactorIndices) { gttic(update); @@ -358,7 +358,7 @@ void ConcurrentBatchFilter::removeFactors(const std::vector& slots) { } /* ************************************************************************* */ -void ConcurrentBatchFilter::reorder(const boost::optional >& keysToMove) { +void ConcurrentBatchFilter::reorder(const std::optional >& keysToMove) { // COLAMD groups will be used to place marginalize keys in Group 0, and everything else in Group 1 if(keysToMove && keysToMove->size() > 0) { diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h index 7e35476b9..b88d5c739 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h +++ b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h @@ -124,7 +124,7 @@ public: * @param removeFactorIndices An optional set of indices corresponding to the factors you want to remove from the graph */ virtual Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(), - const boost::optional >& keysToMove = boost::none, const boost::optional< std::vector >& removeFactorIndices = boost::none); + const std::optional >& keysToMove = {}, const std::optional< std::vector >& removeFactorIndices = {}); /** * Perform any required operations before the synchronization process starts. @@ -200,7 +200,7 @@ private: void removeFactors(const std::vector& slots); /** Use colamd to update into an efficient ordering */ - void reorder(const boost::optional >& keysToMove = boost::none); + void reorder(const std::optional >& keysToMove = {}); /** Marginalize out the set of requested variables from the filter, caching them for the smoother * This effectively moves the separator. diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp index 75d491bde..816917eb5 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp @@ -47,7 +47,7 @@ bool ConcurrentBatchSmoother::equals(const ConcurrentSmoother& rhs, double tol) /* ************************************************************************* */ ConcurrentBatchSmoother::Result ConcurrentBatchSmoother::update(const NonlinearFactorGraph& newFactors, const Values& newTheta, - const boost::optional< std::vector >& removeFactorIndices) { + const std::optional< std::vector >& removeFactorIndices) { gttic(update); diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h index 51d3284a4..38b39594b 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h +++ b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h @@ -118,7 +118,7 @@ public: * and additionally, variables that were already in the system must not be included here. */ virtual Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(), - const boost::optional< std::vector >& removeFactorIndices = boost::none); + const std::optional< std::vector >& removeFactorIndices = {}); /** * Perform any required operations before the synchronization process starts. diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp b/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp index 3886d0e42..5c08eced5 100644 --- a/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp @@ -45,7 +45,7 @@ bool ConcurrentIncrementalSmoother::equals(const ConcurrentSmoother& rhs, double /* ************************************************************************* */ ConcurrentIncrementalSmoother::Result ConcurrentIncrementalSmoother::update(const NonlinearFactorGraph& newFactors, const Values& newTheta, - const boost::optional& removeFactorIndices) { + const std::optional& removeFactorIndices) { gttic(update); diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.h b/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.h index a27751561..92d8c462a 100644 --- a/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.h +++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.h @@ -109,7 +109,7 @@ public: * and additionally, variables that were already in the system must not be included here. */ Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(), - const boost::optional& removeFactorIndices = boost::none); + const std::optional& removeFactorIndices = {}); /** * Perform any required operations before the synchronization process starts. From 4e2f0cc36b814e7b08412feefae4c69517b593f4 Mon Sep 17 00:00:00 2001 From: kartik arcot Date: Thu, 12 Jan 2023 13:51:47 -0800 Subject: [PATCH 18/38] gtsam_unstable/partition --- gtsam_unstable/partition/FindSeparator-inl.h | 23 ++++++++++--------- gtsam_unstable/partition/FindSeparator.h | 4 ++-- .../partition/tests/testFindSeparator.cpp | 20 ++++++++-------- 3 files changed, 24 insertions(+), 23 deletions(-) diff --git a/gtsam_unstable/partition/FindSeparator-inl.h b/gtsam_unstable/partition/FindSeparator-inl.h index 0e4950b79..45868e78e 100644 --- a/gtsam_unstable/partition/FindSeparator-inl.h +++ b/gtsam_unstable/partition/FindSeparator-inl.h @@ -13,6 +13,7 @@ #include #include #include +#include #include #include @@ -237,7 +238,7 @@ namespace gtsam { namespace partition { /* ************************************************************************* */ template - boost::optional separatorPartitionByMetis(const GenericGraph& graph, + std::optional separatorPartitionByMetis(const GenericGraph& graph, const std::vector& keys, WorkSpace& workspace, bool verbose) { // create a metis graph size_t numKeys = keys.size(); @@ -252,7 +253,7 @@ namespace gtsam { namespace partition { size_t sepsize; sharedInts part; boost::tie(sepsize, part) = separatorMetis(numKeys, xadj, adjncy, adjwgt, verbose); - if (!sepsize) return boost::optional(); + if (!sepsize) return std::optional(); // convert the 0-1-2 from Metis to 1-2-0, so that the separator is 0, as later // we will have more submaps @@ -291,7 +292,7 @@ namespace gtsam { namespace partition { /* *************************************************************************/ template - boost::optional edgePartitionByMetis(const GenericGraph& graph, + std::optional edgePartitionByMetis(const GenericGraph& graph, const std::vector& keys, WorkSpace& workspace, bool verbose) { // a small hack for handling the camera1-camera2 case used in the unit tests @@ -443,15 +444,15 @@ namespace gtsam { namespace partition { /* ************************************************************************* */ template - boost::optional findPartitoning(const GenericGraph& graph, const std::vector& keys, + std::optional findPartitoning(const GenericGraph& graph, const std::vector& keys, WorkSpace& workspace, bool verbose, - const boost::optional >& int2symbol, const bool reduceGraph) { - boost::optional result; + const std::optional >& int2symbol, const bool reduceGraph) { + std::optional result; GenericGraph reducedGraph; std::vector keyToPartition; std::vector cameraKeys, landmarkKeys; if (reduceGraph) { - if (!int2symbol.is_initialized()) + if (!int2symbol.has_value()) throw std::invalid_argument("findSeparator: int2symbol must be valid!"); // find out all the landmark keys, which are to be eliminated @@ -474,9 +475,9 @@ namespace gtsam { namespace partition { } else // call Metis to partition the graph to A, B, C result = separatorPartitionByMetis(graph, keys, workspace, verbose); - if (!result.is_initialized()) { + if (!result.has_value()) { std::cout << "metis failed!" << std::endl; - return boost::none; + return {}; } if (reduceGraph) { @@ -491,10 +492,10 @@ namespace gtsam { namespace partition { template int findSeparator(const GenericGraph& graph, const std::vector& keys, const int minNodesPerMap, WorkSpace& workspace, bool verbose, - const boost::optional >& int2symbol, const bool reduceGraph, + const std::optional >& int2symbol, const bool reduceGraph, const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark) { - boost::optional result = findPartitoning(graph, keys, workspace, + std::optional result = findPartitoning(graph, keys, workspace, verbose, int2symbol, reduceGraph); // find the island in A and B, and make them separated submaps diff --git a/gtsam_unstable/partition/FindSeparator.h b/gtsam_unstable/partition/FindSeparator.h index f4342695b..9a6c61800 100644 --- a/gtsam_unstable/partition/FindSeparator.h +++ b/gtsam_unstable/partition/FindSeparator.h @@ -31,7 +31,7 @@ namespace gtsam { namespace partition { * the size of dictionary mush be equal to the number of variables in the original graph (the largest one) */ template - boost::optional separatorPartitionByMetis(const GenericGraph& graph, const std::vector& keys, + std::optional separatorPartitionByMetis(const GenericGraph& graph, const std::vector& keys, WorkSpace& workspace, bool verbose); /** @@ -40,7 +40,7 @@ namespace gtsam { namespace partition { */ template int findSeparator(const GenericGraph& graph, const std::vector& keys, - const int minNodesPerMap, WorkSpace& workspace, bool verbose, const boost::optional >& int2symbol, + const int minNodesPerMap, WorkSpace& workspace, bool verbose, const std::optional >& int2symbol, const bool reduceGraph, const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark); }} //namespace diff --git a/gtsam_unstable/partition/tests/testFindSeparator.cpp b/gtsam_unstable/partition/tests/testFindSeparator.cpp index 49796f80d..b086be7ff 100644 --- a/gtsam_unstable/partition/tests/testFindSeparator.cpp +++ b/gtsam_unstable/partition/tests/testFindSeparator.cpp @@ -29,10 +29,10 @@ TEST ( Partition, separatorPartitionByMetis ) std::vector keys{0, 1, 2, 3, 4}; WorkSpace workspace(5); - boost::optional actual = separatorPartitionByMetis(graph, keys, + std::optional actual = separatorPartitionByMetis(graph, keys, workspace, true); - CHECK(actual.is_initialized()); + CHECK(actual.has_value()); vector A_expected{0, 3}; // frontal vector B_expected{2, 4}; // frontal vector C_expected{1}; // separator @@ -54,10 +54,10 @@ TEST ( Partition, separatorPartitionByMetis2 ) std::vector keys{1, 2, 3, 5, 6}; WorkSpace workspace(8); - boost::optional actual = separatorPartitionByMetis(graph, keys, + std::optional actual = separatorPartitionByMetis(graph, keys, workspace, true); - CHECK(actual.is_initialized()); + CHECK(actual.has_value()); vector A_expected{1, 5}; // frontal vector B_expected{3, 6}; // frontal vector C_expected{2}; // separator @@ -77,10 +77,10 @@ TEST ( Partition, edgePartitionByMetis ) std::vector keys{0, 1, 2, 3}; WorkSpace workspace(6); - boost::optional actual = edgePartitionByMetis(graph, keys, + std::optional actual = edgePartitionByMetis(graph, keys, workspace, true); - CHECK(actual.is_initialized()); + CHECK(actual.has_value()); vector A_expected{0, 1}; // frontal vector B_expected{2, 3}; // frontal vector C_expected; // separator @@ -108,9 +108,9 @@ TEST ( Partition, edgePartitionByMetis2 ) std::vector keys{0, 1, 2, 3, 4}; WorkSpace workspace(6); - boost::optional actual = edgePartitionByMetis(graph, keys, + std::optional actual = edgePartitionByMetis(graph, keys, workspace, true); - CHECK(actual.is_initialized()); + CHECK(actual.has_value()); vector A_expected{0, 1}; // frontal vector B_expected{2, 3, 4}; // frontal vector C_expected; // separator @@ -135,7 +135,7 @@ TEST ( Partition, findSeparator ) int minNodesPerMap = -1; bool reduceGraph = false; int numSubmaps = findSeparator(graph, keys, minNodesPerMap, workspace, - false, boost::none, reduceGraph, 0, 0); + false, {}, reduceGraph, 0, 0); LONGS_EQUAL(2, numSubmaps); LONGS_EQUAL(5, workspace.partitionTable.size()); LONGS_EQUAL(1, workspace.partitionTable[0]); @@ -161,7 +161,7 @@ TEST ( Partition, findSeparator2 ) int minNodesPerMap = -1; bool reduceGraph = false; int numSubmaps = findSeparator(graph, keys, minNodesPerMap, workspace, - false, boost::none, reduceGraph, 0, 0); + false, {}, reduceGraph, 0, 0); LONGS_EQUAL(2, numSubmaps); LONGS_EQUAL(8, workspace.partitionTable.size()); LONGS_EQUAL(-1,workspace.partitionTable[0]); From d7f60353c9789590e5801759bf73c8e4dfd3583f Mon Sep 17 00:00:00 2001 From: kartik arcot Date: Thu, 12 Jan 2023 14:02:31 -0800 Subject: [PATCH 19/38] unstable/slam --- .../slam/EquivInertialNavFactor_GlobalVel.h | 14 +++++++------- .../slam/EquivInertialNavFactor_GlobalVel_NoBias.h | 6 +++--- .../slam/InertialNavFactor_GlobalVelocity.h | 4 ++-- gtsam_unstable/slam/LocalOrientedPlane3Factor.cpp | 2 +- gtsam_unstable/slam/MultiProjectionFactor.h | 6 +++--- gtsam_unstable/slam/PoseBetweenFactor.h | 4 ++-- gtsam_unstable/slam/PosePriorFactor.h | 4 ++-- gtsam_unstable/slam/ProjectionFactorPPP.h | 4 ++-- .../slam/ProjectionFactorRollingShutter.cpp | 4 ++-- .../slam/ProjectionFactorRollingShutter.h | 6 +++--- .../slam/TransformBtwRobotsUnaryFactor.h | 4 ++-- .../tests/testProjectionFactorRollingShutter.cpp | 12 ++++++------ 12 files changed, 35 insertions(+), 35 deletions(-) diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h index 529af878d..f6189cc5a 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h @@ -106,8 +106,8 @@ private: Matrix Jacobian_wrt_t0_Overall_; - boost::optional Bias_initial_; // Bias used when pre-integrating IMU measurements - boost::optional body_P_sensor_; // The pose of the sensor in the body frame + std::optional Bias_initial_; // Bias used when pre-integrating IMU measurements + std::optional body_P_sensor_; // The pose of the sensor in the body frame public: @@ -126,7 +126,7 @@ public: double dt12, const Vector world_g, const Vector world_rho, const Vector& world_omega_earth, const noiseModel::Gaussian::shared_ptr& model_equivalent, const Matrix& Jacobian_wrt_t0_Overall, - boost::optional Bias_initial = boost::none, boost::optional body_P_sensor = boost::none) : + std::optional Bias_initial = {}, std::optional body_P_sensor = {}) : Base(model_equivalent, Pose1, Vel1, IMUBias1, Pose2, Vel2), delta_pos_in_t0_(delta_pos_in_t0), delta_vel_in_t0_(delta_vel_in_t0), delta_angles_(delta_angles), dt12_(dt12), world_g_(world_g), world_rho_(world_rho), world_omega_earth_(world_omega_earth), Jacobian_wrt_t0_Overall_(Jacobian_wrt_t0_Overall), @@ -385,7 +385,7 @@ public: const Vector& delta_pos_in_t0, const Vector3& delta_angles, double dt12, const Vector world_g, const Vector world_rho, const Vector& world_omega_earth, const Matrix& Jacobian_wrt_t0_Overall, - const boost::optional& Bias_initial = boost::none) { + const std::optional& Bias_initial = {}) { // Correct delta_pos_in_t0_ using (Bias1 - Bias_t0) @@ -414,7 +414,7 @@ public: static inline VELOCITY PredictVelocityFromPreIntegration(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1, const Vector& delta_vel_in_t0, double dt12, const Vector world_g, const Vector world_rho, const Vector& world_omega_earth, const Matrix& Jacobian_wrt_t0_Overall, - const boost::optional& Bias_initial = boost::none) { + const std::optional& Bias_initial = {}) { // Correct delta_vel_in_t0_ using (Bias1 - Bias_t0) Vector delta_BiasAcc = Bias1.accelerometer(); @@ -436,7 +436,7 @@ public: const Vector& delta_pos_in_t0, const Vector& delta_vel_in_t0, const Vector3& delta_angles, double dt12, const Vector world_g, const Vector world_rho, const Vector& world_omega_earth, const Matrix& Jacobian_wrt_t0_Overall, - const boost::optional& Bias_initial = boost::none) { + const std::optional& Bias_initial = {}) { Pose2 = PredictPoseFromPreIntegration(Pose1, Vel1, Bias1, delta_pos_in_t0, delta_angles, dt12, world_g, world_rho, world_omega_earth, Jacobian_wrt_t0_Overall, Bias_initial); Vel2 = PredictVelocityFromPreIntegration(Pose1, Vel1, Bias1, delta_vel_in_t0, dt12, world_g, world_rho, world_omega_earth, Jacobian_wrt_t0_Overall, Bias_initial); @@ -447,7 +447,7 @@ public: Vector& delta_pos_in_t0, Vector3& delta_angles, Vector& delta_vel_in_t0, double& delta_t, const noiseModel::Gaussian::shared_ptr& model_continuous_overall, Matrix& EquivCov_Overall, Matrix& Jacobian_wrt_t0_Overall, const IMUBIAS Bias_t0 = IMUBIAS(), - boost::optional p_body_P_sensor = boost::none){ + std::optional p_body_P_sensor = {}){ // Note: all delta terms refer to an IMU\sensor system at t0 // Note: Earth-related terms are not accounted here but are incorporated in predict functions. diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h index f3e27f7e5..5d251094e 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h @@ -105,7 +105,7 @@ private: Matrix Jacobian_wrt_t0_Overall_; - boost::optional body_P_sensor_; // The pose of the sensor in the body frame + std::optional body_P_sensor_; // The pose of the sensor in the body frame public: @@ -124,7 +124,7 @@ public: double dt12, const Vector world_g, const Vector world_rho, const Vector& world_omega_earth, const noiseModel::Gaussian::shared_ptr& model_equivalent, const Matrix& Jacobian_wrt_t0_Overall, - boost::optional body_P_sensor = boost::none) : + std::optional body_P_sensor = {}) : Base(model_equivalent, Pose1, Vel1, Pose2, Vel2), delta_pos_in_t0_(delta_pos_in_t0), delta_vel_in_t0_(delta_vel_in_t0), delta_angles_(delta_angles), dt12_(dt12), world_g_(world_g), world_rho_(world_rho), world_omega_earth_(world_omega_earth), Jacobian_wrt_t0_Overall_(Jacobian_wrt_t0_Overall), @@ -352,7 +352,7 @@ public: Vector& delta_pos_in_t0, Vector3& delta_angles, Vector& delta_vel_in_t0, double& delta_t, const noiseModel::Gaussian::shared_ptr& model_continuous_overall, Matrix& EquivCov_Overall, Matrix& Jacobian_wrt_t0_Overall, - boost::optional p_body_P_sensor = boost::none){ + std::optional p_body_P_sensor = {}){ // Note: all delta terms refer to an IMU\sensor system at t0 // Note: Earth-related terms are not accounted here but are incorporated in predict functions. diff --git a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h index 40fc9da25..a107f84b5 100644 --- a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h +++ b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h @@ -92,7 +92,7 @@ private: Vector world_rho_; Vector world_omega_earth_; - boost::optional body_P_sensor_; // The pose of the sensor in the body frame + std::optional body_P_sensor_; // The pose of the sensor in the body frame public: @@ -108,7 +108,7 @@ public: /** Constructor */ InertialNavFactor_GlobalVelocity(const Key& Pose1, const Key& Vel1, const Key& IMUBias1, const Key& Pose2, const Key& Vel2, const Vector& measurement_acc, const Vector& measurement_gyro, const double measurement_dt, const Vector world_g, const Vector world_rho, - const Vector& world_omega_earth, const noiseModel::Gaussian::shared_ptr& model_continuous, boost::optional body_P_sensor = boost::none) : + const Vector& world_omega_earth, const noiseModel::Gaussian::shared_ptr& model_continuous, std::optional body_P_sensor = {}) : Base(calc_descrete_noise_model(model_continuous, measurement_dt ), Pose1, Vel1, IMUBias1, Pose2, Vel2), measurement_acc_(measurement_acc), measurement_gyro_(measurement_gyro), dt_(measurement_dt), world_g_(world_g), world_rho_(world_rho), world_omega_earth_(world_omega_earth), body_P_sensor_(body_P_sensor) { } diff --git a/gtsam_unstable/slam/LocalOrientedPlane3Factor.cpp b/gtsam_unstable/slam/LocalOrientedPlane3Factor.cpp index 8854df5c7..010e62218 100644 --- a/gtsam_unstable/slam/LocalOrientedPlane3Factor.cpp +++ b/gtsam_unstable/slam/LocalOrientedPlane3Factor.cpp @@ -43,7 +43,7 @@ Vector LocalOrientedPlane3Factor::evaluateError(const Pose3& wTwi, // Calculate the error between measured and estimated planes in sensor frame. const Vector3 err = measured_p_.errorVector(i_plane, - boost::none, (H1 || H2 || H3) ? &error_H_predicted : nullptr); + {}, (H1 || H2 || H3) ? &error_H_predicted : nullptr); // Apply the chain rule to calculate the derivatives. if (H1) { diff --git a/gtsam_unstable/slam/MultiProjectionFactor.h b/gtsam_unstable/slam/MultiProjectionFactor.h index f64568118..d7cebcffc 100644 --- a/gtsam_unstable/slam/MultiProjectionFactor.h +++ b/gtsam_unstable/slam/MultiProjectionFactor.h @@ -39,7 +39,7 @@ namespace gtsam { // Keep a copy of measurement and calibration for I/O Vector measured_; ///< 2D measurement for each of the n views boost::shared_ptr K_; ///< shared pointer to calibration object - boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame + std::optional body_P_sensor_; ///< The pose of the sensor in the body frame // verbosity handling for Cheirality Exceptions @@ -72,7 +72,7 @@ namespace gtsam { */ MultiProjectionFactor(const Vector& measured, const SharedNoiseModel& model, KeySet poseKeys, Key pointKey, const boost::shared_ptr& K, - boost::optional body_P_sensor = boost::none) : + std::optional body_P_sensor = {}) : Base(model), measured_(measured), K_(K), body_P_sensor_(body_P_sensor), throwCheirality_(false), verboseCheirality_(false) { keys_.assign(poseKeys.begin(), poseKeys.end()); @@ -94,7 +94,7 @@ namespace gtsam { MultiProjectionFactor(const Vector& measured, const SharedNoiseModel& model, KeySet poseKeys, Key pointKey, const boost::shared_ptr& K, bool throwCheirality, bool verboseCheirality, - boost::optional body_P_sensor = boost::none) : + std::optional body_P_sensor = {}) : Base(model), measured_(measured), K_(K), body_P_sensor_(body_P_sensor), throwCheirality_(throwCheirality), verboseCheirality_(verboseCheirality) {} diff --git a/gtsam_unstable/slam/PoseBetweenFactor.h b/gtsam_unstable/slam/PoseBetweenFactor.h index 14a18d938..9e275116b 100644 --- a/gtsam_unstable/slam/PoseBetweenFactor.h +++ b/gtsam_unstable/slam/PoseBetweenFactor.h @@ -37,7 +37,7 @@ namespace gtsam { typedef NoiseModelFactorN Base; POSE measured_; /** The measurement */ - boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame + std::optional body_P_sensor_; ///< The pose of the sensor in the body frame /** concept check by type */ GTSAM_CONCEPT_TESTABLE_TYPE(POSE) @@ -55,7 +55,7 @@ namespace gtsam { /** Constructor */ PoseBetweenFactor(Key key1, Key key2, const POSE& measured, - const SharedNoiseModel& model, boost::optional body_P_sensor = boost::none) : + const SharedNoiseModel& model, std::optional body_P_sensor = {}) : Base(model, key1, key2), measured_(measured), body_P_sensor_(body_P_sensor) { } diff --git a/gtsam_unstable/slam/PosePriorFactor.h b/gtsam_unstable/slam/PosePriorFactor.h index ac199588b..4e55bd649 100644 --- a/gtsam_unstable/slam/PosePriorFactor.h +++ b/gtsam_unstable/slam/PosePriorFactor.h @@ -34,7 +34,7 @@ namespace gtsam { typedef NoiseModelFactorN Base; POSE prior_; /** The measurement */ - boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame + std::optional body_P_sensor_; ///< The pose of the sensor in the body frame /** concept check by type */ GTSAM_CONCEPT_TESTABLE_TYPE(POSE) @@ -54,7 +54,7 @@ namespace gtsam { /** Constructor */ PosePriorFactor(Key key, const POSE& prior, const SharedNoiseModel& model, - boost::optional body_P_sensor = boost::none) : + std::optional body_P_sensor = {}) : Base(model, key), prior_(prior), body_P_sensor_(body_P_sensor) { } diff --git a/gtsam_unstable/slam/ProjectionFactorPPP.h b/gtsam_unstable/slam/ProjectionFactorPPP.h index 9ef11f022..2b799ac1d 100644 --- a/gtsam_unstable/slam/ProjectionFactorPPP.h +++ b/gtsam_unstable/slam/ProjectionFactorPPP.h @@ -130,13 +130,13 @@ namespace gtsam { if(H1 || H2 || H3) { Matrix H0, H02; PinholeCamera camera(pose.compose(transform, H0, H02), *K_); - Point2 reprojectionError(camera.project(point, H1, H3, boost::none) - measured_); + Point2 reprojectionError(camera.project(point, H1, H3, {}) - measured_); *H2 = *H1 * H02; *H1 = *H1 * H0; return reprojectionError; } else { PinholeCamera camera(pose.compose(transform), *K_); - return camera.project(point, H1, H3, boost::none) - measured_; + return camera.project(point, H1, H3, {}) - measured_; } } catch( CheiralityException& e) { if (H1) *H1 = Matrix::Zero(2,6); diff --git a/gtsam_unstable/slam/ProjectionFactorRollingShutter.cpp b/gtsam_unstable/slam/ProjectionFactorRollingShutter.cpp index 003f2f921..beeafecf4 100644 --- a/gtsam_unstable/slam/ProjectionFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/ProjectionFactorRollingShutter.cpp @@ -31,7 +31,7 @@ Vector ProjectionFactorRollingShutter::evaluateError( gtsam::Matrix HbodySensor; PinholeCamera camera( pose.compose(*body_P_sensor_, HbodySensor), *K_); - Point2 reprojectionError(camera.project(point, Hprj, H3, boost::none) - + Point2 reprojectionError(camera.project(point, Hprj, H3, {}) - measured_); if (H1) *H1 = Hprj * HbodySensor * (*H1); if (H2) *H2 = Hprj * HbodySensor * (*H2); @@ -42,7 +42,7 @@ Vector ProjectionFactorRollingShutter::evaluateError( } } else { PinholeCamera camera(pose, *K_); - Point2 reprojectionError(camera.project(point, Hprj, H3, boost::none) - + Point2 reprojectionError(camera.project(point, Hprj, H3, {}) - measured_); if (H1) *H1 = Hprj * (*H1); if (H2) *H2 = Hprj * (*H2); diff --git a/gtsam_unstable/slam/ProjectionFactorRollingShutter.h b/gtsam_unstable/slam/ProjectionFactorRollingShutter.h index bbf108ecc..e6c0fd16a 100644 --- a/gtsam_unstable/slam/ProjectionFactorRollingShutter.h +++ b/gtsam_unstable/slam/ProjectionFactorRollingShutter.h @@ -49,7 +49,7 @@ class GTSAM_UNSTABLE_EXPORT ProjectionFactorRollingShutter double alpha_; ///< interpolation parameter in [0,1] corresponding to the ///< point2 measurement boost::shared_ptr K_; ///< shared pointer to calibration object - boost::optional + std::optional body_P_sensor_; ///< The pose of the sensor in the body frame // verbosity handling for Cheirality Exceptions @@ -96,7 +96,7 @@ class GTSAM_UNSTABLE_EXPORT ProjectionFactorRollingShutter const Point2& measured, double alpha, const SharedNoiseModel& model, Key poseKey_a, Key poseKey_b, Key pointKey, const boost::shared_ptr& K, - boost::optional body_P_sensor = boost::none) + std::optional body_P_sensor = {}) : Base(model, poseKey_a, poseKey_b, pointKey), measured_(measured), alpha_(alpha), @@ -127,7 +127,7 @@ class GTSAM_UNSTABLE_EXPORT ProjectionFactorRollingShutter Key poseKey_a, Key poseKey_b, Key pointKey, const boost::shared_ptr& K, bool throwCheirality, bool verboseCheirality, - boost::optional body_P_sensor = boost::none) + std::optional body_P_sensor = {}) : Base(model, poseKey_a, poseKey_b, pointKey), measured_(measured), alpha_(alpha), diff --git a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h index 5745536e0..d2d28ef9b 100644 --- a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h +++ b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h @@ -165,8 +165,8 @@ namespace gtsam { T currA_T_currB_pred; if (H) { Matrix H_compose, H_between1; - T orgA_T_currB = orgA_T_orgB.compose(orgB_T_currB, H_compose, boost::none); - currA_T_currB_pred = orgA_T_currA.between(orgA_T_currB, boost::none, H_between1); + T orgA_T_currB = orgA_T_orgB.compose(orgB_T_currB, H_compose, {}); + currA_T_currB_pred = orgA_T_currA.between(orgA_T_currB, {}, H_between1); (*H)[0] = H_compose * H_between1; } else { diff --git a/gtsam_unstable/slam/tests/testProjectionFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testProjectionFactorRollingShutter.cpp index 3c7c39f48..31a385695 100644 --- a/gtsam_unstable/slam/tests/testProjectionFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testProjectionFactorRollingShutter.cpp @@ -342,24 +342,24 @@ TEST(ProjectionFactorRollingShutter, cheirality) { std::function( std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3, boost::none, boost::none, - boost::none)), + std::placeholders::_3, {}, {}, + {})), pose1, pose2, point); Matrix H2Expected = numericalDerivative32( std::function( std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3, boost::none, boost::none, - boost::none)), + std::placeholders::_3, {}, {}, + {})), pose1, pose2, point); Matrix H3Expected = numericalDerivative33( std::function( std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3, boost::none, boost::none, - boost::none)), + std::placeholders::_3, {}, {}, + {})), pose1, pose2, point); CHECK(assert_equal(H1Expected, H1Actual, 1e-5)); From 5880471136eb3ae37f2bf415ccfbb80e190db0d3 Mon Sep 17 00:00:00 2001 From: kartik arcot Date: Fri, 13 Jan 2023 12:31:12 -0800 Subject: [PATCH 20/38] eliminateable fg --- gtsam/discrete/DiscreteFactorGraph.h | 4 +- gtsam/hybrid/HybridGaussianFactorGraph.h | 2 +- gtsam/hybrid/HybridGaussianISAM.cpp | 2 +- .../inference/EliminateableFactorGraph-inst.h | 40 +++++++++---------- gtsam/inference/EliminateableFactorGraph.h | 2 +- gtsam/inference/ISAM-inst.h | 2 +- gtsam/linear/GaussianFactorGraph.h | 4 +- gtsam/symbolic/SymbolicFactorGraph.h | 4 +- 8 files changed, 30 insertions(+), 30 deletions(-) diff --git a/gtsam/discrete/DiscreteFactorGraph.h b/gtsam/discrete/DiscreteFactorGraph.h index 8b30cb963..fdfc2bc0d 100644 --- a/gtsam/discrete/DiscreteFactorGraph.h +++ b/gtsam/discrete/DiscreteFactorGraph.h @@ -70,8 +70,8 @@ template<> struct EliminationTraits /// The default ordering generation function static Ordering DefaultOrderingFunc( const FactorGraphType& graph, - boost::optional variableIndex) { - return Ordering::Colamd(*variableIndex); + const VariableIndex& variableIndex) { + return Ordering::Colamd(variableIndex); } }; diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.h b/gtsam/hybrid/HybridGaussianFactorGraph.h index 9724fccc5..432bf7c07 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.h +++ b/gtsam/hybrid/HybridGaussianFactorGraph.h @@ -87,7 +87,7 @@ struct EliminationTraits { /// The default ordering generation function static Ordering DefaultOrderingFunc( const FactorGraphType& graph, - boost::optional variableIndex) { + const VariableIndex&) { return HybridOrdering(graph); } }; diff --git a/gtsam/hybrid/HybridGaussianISAM.cpp b/gtsam/hybrid/HybridGaussianISAM.cpp index 086127f16..b6534df70 100644 --- a/gtsam/hybrid/HybridGaussianISAM.cpp +++ b/gtsam/hybrid/HybridGaussianISAM.cpp @@ -102,7 +102,7 @@ void HybridGaussianISAM::updateInternal( // eliminate all factors (top, added, orphans) into a new Bayes tree HybridBayesTree::shared_ptr bayesTree = - factors.eliminateMultifrontal(elimination_ordering, function, index); + factors.eliminateMultifrontal(elimination_ordering, function, &index); if (maxNrLeaves) { bayesTree->prune(*maxNrLeaves); diff --git a/gtsam/inference/EliminateableFactorGraph-inst.h b/gtsam/inference/EliminateableFactorGraph-inst.h index bebce14cd..a0847dbdd 100644 --- a/gtsam/inference/EliminateableFactorGraph-inst.h +++ b/gtsam/inference/EliminateableFactorGraph-inst.h @@ -36,7 +36,7 @@ namespace gtsam { // no Ordering is provided. When removing optional from VariableIndex, create VariableIndex // before creating ordering. VariableIndex computedVariableIndex(asDerived()); - return eliminateSequential(orderingType, function, computedVariableIndex); + return eliminateSequential(orderingType, function, &computedVariableIndex); } else { // Compute an ordering and call this function again. We are guaranteed to have a @@ -52,7 +52,7 @@ namespace gtsam { return eliminateSequential(computedOrdering, function, variableIndex); } else { Ordering computedOrdering = EliminationTraitsType::DefaultOrderingFunc( - asDerived(), variableIndex); + asDerived(), *variableIndex); return eliminateSequential(computedOrdering, function, variableIndex); } } @@ -68,7 +68,7 @@ namespace gtsam { if(!variableIndex) { // If no VariableIndex provided, compute one and call this function again VariableIndex computedVariableIndex(asDerived()); - return eliminateSequential(ordering, function, computedVariableIndex); + return eliminateSequential(ordering, function, &computedVariableIndex); } else { gttic(eliminateSequential); // Do elimination @@ -99,7 +99,7 @@ namespace gtsam { // creating ordering. VariableIndex computedVariableIndex(asDerived()); return eliminateMultifrontal(orderingType, function, - computedVariableIndex); + &computedVariableIndex); } else { // Compute an ordering and call this function again. We are guaranteed to // have a VariableIndex already here because we computed one if needed in @@ -115,7 +115,7 @@ namespace gtsam { return eliminateMultifrontal(computedOrdering, function, variableIndex); } else { Ordering computedOrdering = EliminationTraitsType::DefaultOrderingFunc( - asDerived(), variableIndex); + asDerived(), *variableIndex); return eliminateMultifrontal(computedOrdering, function, variableIndex); } } @@ -131,7 +131,7 @@ namespace gtsam { if(!variableIndex) { // If no VariableIndex provided, compute one and call this function again VariableIndex computedVariableIndex(asDerived()); - return eliminateMultifrontal(ordering, function, computedVariableIndex); + return eliminateMultifrontal(ordering, function, &computedVariableIndex); } else { gttic(eliminateMultifrontal); // Do elimination with given ordering @@ -162,7 +162,7 @@ namespace gtsam { } else { // If no variable index is provided, compute one and call this function again VariableIndex computedVariableIndex(asDerived()); - return eliminatePartialSequential(ordering, function, computedVariableIndex); + return eliminatePartialSequential(ordering, function, &computedVariableIndex); } } @@ -183,7 +183,7 @@ namespace gtsam { } else { // If no variable index is provided, compute one and call this function again VariableIndex computedVariableIndex(asDerived()); - return eliminatePartialSequential(variables, function, computedVariableIndex); + return eliminatePartialSequential(variables, function, &computedVariableIndex); } } @@ -202,7 +202,7 @@ namespace gtsam { } else { // If no variable index is provided, compute one and call this function again VariableIndex computedVariableIndex(asDerived()); - return eliminatePartialMultifrontal(ordering, function, computedVariableIndex); + return eliminatePartialMultifrontal(ordering, function, &computedVariableIndex); } } @@ -223,7 +223,7 @@ namespace gtsam { } else { // If no variable index is provided, compute one and call this function again VariableIndex computedVariableIndex(asDerived()); - return eliminatePartialMultifrontal(variables, function, computedVariableIndex); + return eliminatePartialMultifrontal(variables, function, &computedVariableIndex); } } @@ -237,7 +237,7 @@ namespace gtsam { if(!variableIndex) { // If no variable index is provided, compute one and call this function again VariableIndex index(asDerived()); - return marginalMultifrontalBayesNet(variables, function, index); + return marginalMultifrontalBayesNet(variables, function, &index); } else { // No ordering was provided for the marginalized variables, so order them using constrained // COLAMD. @@ -255,7 +255,7 @@ namespace gtsam { Ordering marginalVarsOrdering(totalOrdering.end() - nVars, totalOrdering.end()); // Call this function again with the computed orderings - return marginalMultifrontalBayesNet(marginalVarsOrdering, marginalizationOrdering, function, *variableIndex); + return marginalMultifrontalBayesNet(marginalVarsOrdering, marginalizationOrdering, function, variableIndex); } } @@ -270,7 +270,7 @@ namespace gtsam { if(!variableIndex) { // If no variable index is provided, compute one and call this function again VariableIndex index(asDerived()); - return marginalMultifrontalBayesNet(variables, marginalizedVariableOrdering, function, index); + return marginalMultifrontalBayesNet(variables, marginalizedVariableOrdering, function, &index); } else { gttic(marginalMultifrontalBayesNet); // An ordering was provided for the marginalized variables, so we can first eliminate them @@ -278,7 +278,7 @@ namespace gtsam { boost::shared_ptr bayesTree; boost::shared_ptr factorGraph; boost::tie(bayesTree,factorGraph) = - eliminatePartialMultifrontal(marginalizedVariableOrdering, function, *variableIndex); + eliminatePartialMultifrontal(marginalizedVariableOrdering, function, variableIndex); if(const Ordering* varsAsOrdering = boost::get(&variables)) { @@ -304,7 +304,7 @@ namespace gtsam { if(!variableIndex) { // If no variable index is provided, compute one and call this function again VariableIndex computedVariableIndex(asDerived()); - return marginalMultifrontalBayesTree(variables, function, computedVariableIndex); + return marginalMultifrontalBayesTree(variables, function, &computedVariableIndex); } else { // No ordering was provided for the marginalized variables, so order them using constrained // COLAMD. @@ -322,7 +322,7 @@ namespace gtsam { Ordering marginalVarsOrdering(totalOrdering.end() - nVars, totalOrdering.end()); // Call this function again with the computed orderings - return marginalMultifrontalBayesTree(marginalVarsOrdering, marginalizationOrdering, function, *variableIndex); + return marginalMultifrontalBayesTree(marginalVarsOrdering, marginalizationOrdering, function, variableIndex); } } @@ -337,7 +337,7 @@ namespace gtsam { if(!variableIndex) { // If no variable index is provided, compute one and call this function again VariableIndex computedVariableIndex(asDerived()); - return marginalMultifrontalBayesTree(variables, marginalizedVariableOrdering, function, computedVariableIndex); + return marginalMultifrontalBayesTree(variables, marginalizedVariableOrdering, function, &computedVariableIndex); } else { gttic(marginalMultifrontalBayesTree); // An ordering was provided for the marginalized variables, so we can first eliminate them @@ -345,7 +345,7 @@ namespace gtsam { boost::shared_ptr bayesTree; boost::shared_ptr factorGraph; boost::tie(bayesTree,factorGraph) = - eliminatePartialMultifrontal(marginalizedVariableOrdering, function, *variableIndex); + eliminatePartialMultifrontal(marginalizedVariableOrdering, function, variableIndex); if(const Ordering* varsAsOrdering = boost::get(&variables)) { @@ -377,13 +377,13 @@ namespace gtsam { Ordering marginalizationOrdering(totalOrdering.begin(), totalOrdering.end() - variables.size()); // Eliminate and return the remaining factor graph - return eliminatePartialMultifrontal(marginalizationOrdering, function, *variableIndex).second; + return eliminatePartialMultifrontal(marginalizationOrdering, function, variableIndex).second; } else { // If no variable index is provided, compute one and call this function again VariableIndex computedVariableIndex(asDerived()); - return marginal(variables, function, computedVariableIndex); + return marginal(variables, function, &computedVariableIndex); } } diff --git a/gtsam/inference/EliminateableFactorGraph.h b/gtsam/inference/EliminateableFactorGraph.h index c6cf56a11..1ee990807 100644 --- a/gtsam/inference/EliminateableFactorGraph.h +++ b/gtsam/inference/EliminateableFactorGraph.h @@ -89,7 +89,7 @@ namespace gtsam { typedef std::function Eliminate; /// Typedef for an optional variable index as an argument to elimination functions - typedef boost::optional OptionalVariableIndex; + typedef const VariableIndex* OptionalVariableIndex; /// Typedef for an optional ordering type typedef std::optional OptionalOrderingType; diff --git a/gtsam/inference/ISAM-inst.h b/gtsam/inference/ISAM-inst.h index 39d840906..5dd06702c 100644 --- a/gtsam/inference/ISAM-inst.h +++ b/gtsam/inference/ISAM-inst.h @@ -49,7 +49,7 @@ void ISAM::updateInternal(const FactorGraphType& newFactors, KeyVector(newFactorKeys.begin(), newFactorKeys.end())); // eliminate all factors (top, added, orphans) into a new Bayes tree - auto bayesTree = factors.eliminateMultifrontal(ordering, function, index); + auto bayesTree = factors.eliminateMultifrontal(ordering, function, &index); // Re-add into Bayes tree data structures this->roots_.insert(this->roots_.end(), bayesTree->roots().begin(), diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index fb5e1ea36..8b268558a 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -57,8 +57,8 @@ namespace gtsam { /// The default ordering generation function static Ordering DefaultOrderingFunc( const FactorGraphType& graph, - boost::optional variableIndex) { - return Ordering::Colamd(*variableIndex); + const VariableIndex& variableIndex) { + return Ordering::Colamd(variableIndex); } }; diff --git a/gtsam/symbolic/SymbolicFactorGraph.h b/gtsam/symbolic/SymbolicFactorGraph.h index 1d4292cbf..ecb7c222b 100644 --- a/gtsam/symbolic/SymbolicFactorGraph.h +++ b/gtsam/symbolic/SymbolicFactorGraph.h @@ -49,8 +49,8 @@ namespace gtsam { /// The default ordering generation function static Ordering DefaultOrderingFunc( const FactorGraphType& graph, - boost::optional variableIndex) { - return Ordering::Colamd(*variableIndex); + const VariableIndex& variableIndex) { + return Ordering::Colamd(variableIndex); } }; From 9329bddd8a3ab134527a005f9957a8b73506da01 Mon Sep 17 00:00:00 2001 From: kartik arcot Date: Fri, 13 Jan 2023 14:11:03 -0800 Subject: [PATCH 21/38] OptionalJacobian --- gtsam/base/Lie.h | 30 +++++----- gtsam/base/Matrix.h | 12 ++-- gtsam/base/OptionalJacobian.h | 53 ++++------------ gtsam/base/ProductLieGroup.h | 14 ++--- gtsam/base/TestableAssertions.h | 6 +- gtsam/base/Value.h | 1 + gtsam/base/VectorSpace.h | 60 +++++++++---------- gtsam/base/numericalDerivative.h | 4 +- gtsam/base/tests/testOptionalJacobian.cpp | 13 ++-- gtsam/basis/Basis.h | 28 ++++----- gtsam/basis/tests/testChebyshev2.cpp | 9 +-- gtsam/geometry/BearingRange.h | 12 ++-- gtsam/geometry/Cal3.h | 4 +- gtsam/geometry/Cal3Bundler.cpp | 4 +- gtsam/geometry/Cal3Bundler.h | 8 +-- gtsam/geometry/Cal3DS2_Base.h | 8 +-- gtsam/geometry/Cal3Fisheye.cpp | 2 +- gtsam/geometry/Cal3Fisheye.h | 8 +-- gtsam/geometry/Cal3Unified.h | 8 +-- gtsam/geometry/Cal3_S2.h | 12 ++-- gtsam/geometry/Cal3_S2Stereo.h | 8 +-- gtsam/geometry/CalibratedCamera.cpp | 2 +- gtsam/geometry/CalibratedCamera.h | 36 +++++------ gtsam/geometry/EssentialMatrix.h | 14 ++--- gtsam/geometry/Line3.h | 14 ++--- gtsam/geometry/OrientedPlane3.h | 14 ++--- gtsam/geometry/PinholeCamera.h | 22 +++---- gtsam/geometry/PinholePose.h | 48 +++++++-------- gtsam/geometry/Point2.h | 14 ++--- gtsam/geometry/Point3.h | 20 +++---- gtsam/geometry/Quaternion.h | 14 ++--- gtsam/geometry/Rot2.h | 18 +++--- gtsam/geometry/Rot3.h | 56 ++++++++--------- gtsam/geometry/SO3.h | 10 ++-- gtsam/geometry/SO4.h | 4 +- gtsam/geometry/SOn.h | 10 ++-- gtsam/geometry/Similarity2.h | 8 +-- gtsam/geometry/Similarity3.h | 12 ++-- gtsam/geometry/SphericalCamera.cpp | 2 +- gtsam/geometry/SphericalCamera.h | 16 ++--- gtsam/geometry/StereoCamera.h | 10 ++-- gtsam/geometry/tests/testCal3DS2.cpp | 4 +- gtsam/geometry/tests/testCal3Unified.cpp | 4 +- gtsam/geometry/tests/testCal3_S2.cpp | 8 +-- gtsam/geometry/tests/testCalibratedCamera.cpp | 2 +- gtsam/geometry/tests/testEssentialMatrix.cpp | 36 ++++++----- gtsam/geometry/tests/testOrientedPlane3.cpp | 28 +++++---- gtsam/geometry/tests/testPinholeCamera.cpp | 4 +- gtsam/geometry/tests/testPinholePose.cpp | 2 +- gtsam/geometry/tests/testPoint3.cpp | 12 ++-- gtsam/geometry/tests/testPose2.cpp | 8 +-- gtsam/geometry/tests/testRot3.cpp | 2 +- gtsam/geometry/tests/testRot3Q.cpp | 2 +- gtsam/geometry/tests/testSO3.cpp | 10 ++-- gtsam/geometry/tests/testSimpleCamera.cpp | 2 +- gtsam/geometry/tests/testStereoCamera.cpp | 4 +- gtsam/geometry/tests/testUnit3.cpp | 42 ++++++------- gtsam/inference/EliminateableFactorGraph.h | 5 +- gtsam/linear/GaussianFactorGraph.h | 3 +- gtsam/navigation/AHRSFactor.h | 2 +- gtsam/navigation/AttitudeFactor.h | 2 +- gtsam/navigation/ImuBias.h | 8 +-- gtsam/navigation/MagFactor.h | 2 +- gtsam/navigation/ManifoldPreintegration.cpp | 2 +- gtsam/navigation/ManifoldPreintegration.h | 2 +- gtsam/navigation/TangentPreintegration.h | 12 ++-- gtsam/navigation/tests/testImuBias.cpp | 4 +- gtsam/navigation/tests/testImuFactor.cpp | 20 +++---- .../tests/testManifoldPreintegration.cpp | 4 +- gtsam/navigation/tests/testNavState.cpp | 20 +++---- .../tests/testTangentPreintegration.cpp | 4 +- gtsam/nonlinear/AdaptAutoDiff.h | 4 +- gtsam/nonlinear/Expression-inl.h | 6 +- gtsam/nonlinear/FunctorizedFactor.h | 2 +- gtsam/nonlinear/NonlinearFactorGraph.h | 5 +- gtsam/nonlinear/internal/ExpressionNode.h | 10 ++-- gtsam/nonlinear/tests/testExpression.cpp | 2 +- gtsam/nonlinear/tests/testFactorTesting.cpp | 6 +- .../nonlinear/tests/testFunctorizedFactor.cpp | 12 ++-- gtsam/nonlinear/tests/testValues.cpp | 8 +-- gtsam/slam/ProjectionFactor.h | 6 +- gtsam/slam/TriangulationFactor.h | 2 +- gtsam/slam/tests/testTriangulationFactor.cpp | 2 +- gtsam_unstable/dynamics/PoseRTV.h | 8 +-- gtsam_unstable/dynamics/SimpleHelicopter.h | 2 +- gtsam_unstable/geometry/Event.h | 6 +- gtsam_unstable/geometry/InvDepthCamera3.h | 2 +- gtsam_unstable/geometry/Pose3Upright.h | 10 ++-- gtsam_unstable/geometry/tests/testEvent.cpp | 4 +- gtsam_unstable/gtsam_unstable.i | 2 +- tests/testSimulated3D.cpp | 6 +- 91 files changed, 496 insertions(+), 517 deletions(-) diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index c4eba5deb..1b574816a 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.h @@ -54,14 +54,14 @@ struct LieGroup { } Class compose(const Class& g, ChartJacobian H1, - ChartJacobian H2 = boost::none) const { + ChartJacobian H2 = {}) const { if (H1) *H1 = g.inverse().AdjointMap(); if (H2) *H2 = Eigen::Matrix::Identity(); return derived() * g; } Class between(const Class& g, ChartJacobian H1, - ChartJacobian H2 = boost::none) const { + ChartJacobian H2 = {}) const { Class result = derived().inverse() * g; if (H1) *H1 = - result.inverse().AdjointMap(); if (H2) *H2 = Eigen::Matrix::Identity(); @@ -87,7 +87,7 @@ struct LieGroup { /// expmap with optional derivatives Class expmap(const TangentVector& v, // - ChartJacobian H1, ChartJacobian H2 = boost::none) const { + ChartJacobian H1, ChartJacobian H2 = {}) const { Jacobian D_g_v; Class g = Class::Expmap(v,H2 ? &D_g_v : 0); Class h = compose(g); // derivatives inlined below @@ -98,7 +98,7 @@ struct LieGroup { /// logmap with optional derivatives TangentVector logmap(const Class& g, // - ChartJacobian H1, ChartJacobian H2 = boost::none) const { + ChartJacobian H1, ChartJacobian H2 = {}) const { Class h = between(g); // derivatives inlined below Jacobian D_v_h; TangentVector v = Class::Logmap(h, (H1 || H2) ? &D_v_h : 0); @@ -139,7 +139,7 @@ struct LieGroup { /// retract with optional derivatives Class retract(const TangentVector& v, // - ChartJacobian H1, ChartJacobian H2 = boost::none) const { + ChartJacobian H1, ChartJacobian H2 = {}) const { Jacobian D_g_v; Class g = Class::ChartAtOrigin::Retract(v, H2 ? &D_g_v : 0); Class h = compose(g); // derivatives inlined below @@ -150,7 +150,7 @@ struct LieGroup { /// localCoordinates with optional derivatives TangentVector localCoordinates(const Class& g, // - ChartJacobian H1, ChartJacobian H2 = boost::none) const { + ChartJacobian H1, ChartJacobian H2 = {}) const { Class h = between(g); // derivatives inlined below Jacobian D_v_h; TangentVector v = Class::ChartAtOrigin::Local(h, (H1 || H2) ? &D_v_h : 0); @@ -188,38 +188,38 @@ struct LieGroupTraits: GetDimensionImpl { typedef OptionalJacobian ChartJacobian; static TangentVector Local(const Class& origin, const Class& other, - ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) { + ChartJacobian Horigin = {}, ChartJacobian Hother = {}) { return origin.localCoordinates(other, Horigin, Hother); } static Class Retract(const Class& origin, const TangentVector& v, - ChartJacobian Horigin = boost::none, ChartJacobian Hv = boost::none) { + ChartJacobian Horigin = {}, ChartJacobian Hv = {}) { return origin.retract(v, Horigin, Hv); } /// @} /// @name Lie Group /// @{ - static TangentVector Logmap(const Class& m, ChartJacobian Hm = boost::none) { + static TangentVector Logmap(const Class& m, ChartJacobian Hm = {}) { return Class::Logmap(m, Hm); } - static Class Expmap(const TangentVector& v, ChartJacobian Hv = boost::none) { + static Class Expmap(const TangentVector& v, ChartJacobian Hv = {}) { return Class::Expmap(v, Hv); } static Class Compose(const Class& m1, const Class& m2, // - ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { + ChartJacobian H1 = {}, ChartJacobian H2 = {}) { return m1.compose(m2, H1, H2); } static Class Between(const Class& m1, const Class& m2, // - ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { + ChartJacobian H1 = {}, ChartJacobian H2 = {}) { return m1.between(m2, H1, H2); } static Class Inverse(const Class& m, // - ChartJacobian H = boost::none) { + ChartJacobian H = {}) { return m.inverse(H); } /// @} @@ -325,8 +325,8 @@ T expm(const Vector& x, int K=7) { */ template T interpolate(const T& X, const T& Y, double t, - typename MakeOptionalJacobian::type Hx = boost::none, - typename MakeOptionalJacobian::type Hy = boost::none) { + typename MakeOptionalJacobian::type Hx = {}, + typename MakeOptionalJacobian::type Hy = {}) { if (Hx || Hy) { typename MakeJacobian::type between_H_x, log_H, exp_H, compose_H_x; const T between = diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index cfedf6d8c..88714d6f6 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -459,8 +459,8 @@ struct MultiplyWithInverse { /// A.inverse() * b, with optional derivatives VectorN operator()(const MatrixN& A, const VectorN& b, - OptionalJacobian H1 = boost::none, - OptionalJacobian H2 = boost::none) const { + OptionalJacobian H1 = {}, + OptionalJacobian H2 = {}) const { const MatrixN invA = A.inverse(); const VectorN c = invA * b; // The derivative in A is just -[c[0]*invA c[1]*invA ... c[N-1]*invA] @@ -495,16 +495,16 @@ struct MultiplyWithInverseFunction { /// f(a).inverse() * b, with optional derivatives VectorN operator()(const T& a, const VectorN& b, - OptionalJacobian H1 = boost::none, - OptionalJacobian H2 = boost::none) const { + OptionalJacobian H1 = {}, + OptionalJacobian H2 = {}) const { MatrixN A; - phi_(a, b, boost::none, A); // get A = f(a) by calling f once + phi_(a, b, {}, A); // get A = f(a) by calling f once const MatrixN invA = A.inverse(); const VectorN c = invA * b; if (H1) { Eigen::Matrix H; - phi_(a, c, H, boost::none); // get derivative H of forward mapping + phi_(a, c, H, {}); // get derivative H of forward mapping *H1 = -invA* H; } if (H2) *H2 = invA; diff --git a/gtsam/base/OptionalJacobian.h b/gtsam/base/OptionalJacobian.h index 4bde4e5e7..3d5af97fd 100644 --- a/gtsam/base/OptionalJacobian.h +++ b/gtsam/base/OptionalJacobian.h @@ -18,23 +18,18 @@ */ #pragma once +#include #include // Configuration from CMake #include #include #include -#ifndef OPTIONALJACOBIAN_NOBOOST -#include -#endif - namespace gtsam { /** * OptionalJacobian is an Eigen::Ref like class that can take be constructed using * either a fixed size or dynamic Eigen matrix. In the latter case, the dynamic - * matrix will be resized. Finally, there is a constructor that takes - * boost::none, the default constructor acts like boost::none, and - * boost::optional is also supported for backwards compatibility. + * matrix will be resized. * Below this class, a dynamic version is also implemented. */ template @@ -66,11 +61,18 @@ private: public: - /// Default constructor acts like boost::none + /// Default constructor OptionalJacobian() : map_(nullptr) { } + /// Default constructor with nullptr_t + /// To guide the compiler when nullptr + /// is passed to args of the type OptionalJacobian + OptionalJacobian(std::nullptr_t /*unused*/) : + map_(nullptr) { + } + /// Constructor that will usurp data of a fixed-size matrix OptionalJacobian(Jacobian& fixed) : map_(nullptr) { @@ -118,24 +120,6 @@ public: } } -#ifndef OPTIONALJACOBIAN_NOBOOST - - /// Constructor with boost::none just makes empty - OptionalJacobian(boost::none_t /*none*/) : - map_(nullptr) { - } - - /// Constructor compatible with old-style derivatives - OptionalJacobian(const boost::optional optional) : - map_(nullptr) { - if (optional) { - optional->resize(Rows, Cols); - usurp(optional->data()); - } - } - -#endif - /// Constructor that will usurp data of a block expression /// TODO(frank): unfortunately using a Map makes usurping non-contiguous memory impossible // template @@ -200,7 +184,7 @@ private: public: - /// Default constructor acts like boost::none + /// Default constructor acts like OptionalJacobian() : pointer_(nullptr) { } @@ -211,21 +195,6 @@ public: /// Construct from refrence to dynamic matrix OptionalJacobian(Jacobian& dynamic) : pointer_(&dynamic) {} -#ifndef OPTIONALJACOBIAN_NOBOOST - - /// Constructor with boost::none just makes empty - OptionalJacobian(boost::none_t /*none*/) : - pointer_(nullptr) { - } - - /// Constructor compatible with old-style derivatives - OptionalJacobian(const boost::optional optional) : - pointer_(nullptr) { - if (optional) pointer_ = &(*optional); - } - -#endif - /// Return true if allocated, false if default constructor was used operator bool() const { return pointer_!=nullptr; diff --git a/gtsam/base/ProductLieGroup.h b/gtsam/base/ProductLieGroup.h index 6689c11d6..831968bc8 100644 --- a/gtsam/base/ProductLieGroup.h +++ b/gtsam/base/ProductLieGroup.h @@ -75,14 +75,14 @@ public: typedef OptionalJacobian ChartJacobian; ProductLieGroup retract(const TangentVector& v, // - ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) const { + ChartJacobian H1 = {}, ChartJacobian H2 = {}) const { if (H1||H2) throw std::runtime_error("ProductLieGroup::retract derivatives not implemented yet"); G g = traits::Retract(this->first, v.template head()); H h = traits::Retract(this->second, v.template tail()); return ProductLieGroup(g,h); } TangentVector localCoordinates(const ProductLieGroup& g, // - ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) const { + ChartJacobian H1 = {}, ChartJacobian H2 = {}) const { if (H1||H2) throw std::runtime_error("ProductLieGroup::localCoordinates derivatives not implemented yet"); typename traits::TangentVector v1 = traits::Local(this->first, g.first); typename traits::TangentVector v2 = traits::Local(this->second, g.second); @@ -101,7 +101,7 @@ protected: public: ProductLieGroup compose(const ProductLieGroup& other, ChartJacobian H1, - ChartJacobian H2 = boost::none) const { + ChartJacobian H2 = {}) const { Jacobian1 D_g_first; Jacobian2 D_h_second; G g = traits::Compose(this->first,other.first, H1 ? &D_g_first : 0); H h = traits::Compose(this->second,other.second, H1 ? &D_h_second : 0); @@ -114,7 +114,7 @@ public: return ProductLieGroup(g,h); } ProductLieGroup between(const ProductLieGroup& other, ChartJacobian H1, - ChartJacobian H2 = boost::none) const { + ChartJacobian H2 = {}) const { Jacobian1 D_g_first; Jacobian2 D_h_second; G g = traits::Between(this->first,other.first, H1 ? &D_g_first : 0); H h = traits::Between(this->second,other.second, H1 ? &D_h_second : 0); @@ -137,7 +137,7 @@ public: } return ProductLieGroup(g,h); } - static ProductLieGroup Expmap(const TangentVector& v, ChartJacobian Hv = boost::none) { + static ProductLieGroup Expmap(const TangentVector& v, ChartJacobian Hv = {}) { Jacobian1 D_g_first; Jacobian2 D_h_second; G g = traits::Expmap(v.template head(), Hv ? &D_g_first : 0); H h = traits::Expmap(v.template tail(), Hv ? &D_h_second : 0); @@ -148,7 +148,7 @@ public: } return ProductLieGroup(g,h); } - static TangentVector Logmap(const ProductLieGroup& p, ChartJacobian Hp = boost::none) { + static TangentVector Logmap(const ProductLieGroup& p, ChartJacobian Hp = {}) { Jacobian1 D_g_first; Jacobian2 D_h_second; typename traits::TangentVector v1 = traits::Logmap(p.first, Hp ? &D_g_first : 0); typename traits::TangentVector v2 = traits::Logmap(p.second, Hp ? &D_h_second : 0); @@ -161,7 +161,7 @@ public: } return v; } - static TangentVector LocalCoordinates(const ProductLieGroup& p, ChartJacobian Hp = boost::none) { + static TangentVector LocalCoordinates(const ProductLieGroup& p, ChartJacobian Hp = {}) { return Logmap(p, Hp); } ProductLieGroup expmap(const TangentVector& v) const { diff --git a/gtsam/base/TestableAssertions.h b/gtsam/base/TestableAssertions.h index 0bf3cf21f..3ade1cc2e 100644 --- a/gtsam/base/TestableAssertions.h +++ b/gtsam/base/TestableAssertions.h @@ -50,11 +50,11 @@ template bool assert_equal(const std::optional& expected, const std::optional& actual, double tol = 1e-9) { if (!expected && actual) { - std::cout << "expected is boost::none, while actual is not" << std::endl; + std::cout << "expected is {}, while actual is not" << std::endl; return false; } if (expected && !actual) { - std::cout << "actual is boost::none, while expected is not" << std::endl; + std::cout << "actual is {}, while expected is not" << std::endl; return false; } if (!expected && !actual) @@ -65,7 +65,7 @@ bool assert_equal(const std::optional& expected, template bool assert_equal(const V& expected, const std::optional& actual, double tol = 1e-9) { if (!actual) { - std::cout << "actual is boost::none" << std::endl; + std::cout << "actual is {}" << std::endl; return false; } return assert_equal(expected, *actual, tol); diff --git a/gtsam/base/Value.h b/gtsam/base/Value.h index 697c4f3be..0d2c601cd 100644 --- a/gtsam/base/Value.h +++ b/gtsam/base/Value.h @@ -21,6 +21,7 @@ #include // Configuration from CMake #include +#include #include #include #include diff --git a/gtsam/base/VectorSpace.h b/gtsam/base/VectorSpace.h index f7809f596..9fbd581bb 100644 --- a/gtsam/base/VectorSpace.h +++ b/gtsam/base/VectorSpace.h @@ -32,7 +32,7 @@ struct VectorSpaceImpl { static int GetDimension(const Class&) { return N;} static TangentVector Local(const Class& origin, const Class& other, - ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { + ChartJacobian H1 = {}, ChartJacobian H2 = {}) { if (H1) *H1 = - Jacobian::Identity(); if (H2) *H2 = Jacobian::Identity(); Class v = other-origin; @@ -40,7 +40,7 @@ struct VectorSpaceImpl { } static Class Retract(const Class& origin, const TangentVector& v, - ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { + ChartJacobian H1 = {}, ChartJacobian H2 = {}) { if (H1) *H1 = Jacobian::Identity(); if (H2) *H2 = Jacobian::Identity(); return origin + v; @@ -51,31 +51,31 @@ struct VectorSpaceImpl { /// @name Lie Group /// @{ - static TangentVector Logmap(const Class& m, ChartJacobian Hm = boost::none) { + static TangentVector Logmap(const Class& m, ChartJacobian Hm = {}) { if (Hm) *Hm = Jacobian::Identity(); return m.vector(); } - static Class Expmap(const TangentVector& v, ChartJacobian Hv = boost::none) { + static Class Expmap(const TangentVector& v, ChartJacobian Hv = {}) { if (Hv) *Hv = Jacobian::Identity(); return Class(v); } - static Class Compose(const Class& v1, const Class& v2, ChartJacobian H1 = boost::none, - ChartJacobian H2 = boost::none) { + static Class Compose(const Class& v1, const Class& v2, ChartJacobian H1 = {}, + ChartJacobian H2 = {}) { if (H1) *H1 = Jacobian::Identity(); if (H2) *H2 = Jacobian::Identity(); return v1 + v2; } - static Class Between(const Class& v1, const Class& v2, ChartJacobian H1 = boost::none, - ChartJacobian H2 = boost::none) { + static Class Between(const Class& v1, const Class& v2, ChartJacobian H1 = {}, + ChartJacobian H2 = {}) { if (H1) *H1 = - Jacobian::Identity(); if (H2) *H2 = Jacobian::Identity(); return v2 - v1; } - static Class Inverse(const Class& v, ChartJacobian H = boost::none) { + static Class Inverse(const Class& v, ChartJacobian H = {}) { if (H) *H = - Jacobian::Identity(); return -v; } @@ -106,7 +106,7 @@ struct VectorSpaceImpl { } static TangentVector Local(const Class& origin, const Class& other, - ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { + ChartJacobian H1 = {}, ChartJacobian H2 = {}) { if (H1) *H1 = - Eye(origin); if (H2) *H2 = Eye(other); Class v = other-origin; @@ -114,7 +114,7 @@ struct VectorSpaceImpl { } static Class Retract(const Class& origin, const TangentVector& v, - ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { + ChartJacobian H1 = {}, ChartJacobian H2 = {}) { if (H1) *H1 = Eye(origin); if (H2) *H2 = Eye(origin); return origin + v; @@ -125,12 +125,12 @@ struct VectorSpaceImpl { /// @name Lie Group /// @{ - static TangentVector Logmap(const Class& m, ChartJacobian Hm = boost::none) { + static TangentVector Logmap(const Class& m, ChartJacobian Hm = {}) { if (Hm) *Hm = Eye(m); return m.vector(); } - static Class Expmap(const TangentVector& v, ChartJacobian Hv = boost::none) { + static Class Expmap(const TangentVector& v, ChartJacobian Hv = {}) { Class result(v); if (Hv) *Hv = Eye(v); @@ -138,14 +138,14 @@ struct VectorSpaceImpl { } static Class Compose(const Class& v1, const Class& v2, ChartJacobian H1, - ChartJacobian H2 = boost::none) { + ChartJacobian H2 = {}) { if (H1) *H1 = Eye(v1); if (H2) *H2 = Eye(v2); return v1 + v2; } static Class Between(const Class& v1, const Class& v2, ChartJacobian H1, - ChartJacobian H2 = boost::none) { + ChartJacobian H2 = {}) { if (H1) *H1 = - Eye(v1); if (H2) *H2 = Eye(v2); return v2 - v1; @@ -237,7 +237,7 @@ struct ScalarTraits : VectorSpaceImpl { typedef OptionalJacobian<1, 1> ChartJacobian; static TangentVector Local(Scalar origin, Scalar other, - ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { + ChartJacobian H1 = {}, ChartJacobian H2 = {}) { if (H1) (*H1)[0] = -1.0; if (H2) (*H2)[0] = 1.0; TangentVector result; @@ -246,7 +246,7 @@ struct ScalarTraits : VectorSpaceImpl { } static Scalar Retract(Scalar origin, const TangentVector& v, - ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { + ChartJacobian H1 = {}, ChartJacobian H2 = {}) { if (H1) (*H1)[0] = 1.0; if (H2) (*H2)[0] = 1.0; return origin + v[0]; @@ -255,12 +255,12 @@ struct ScalarTraits : VectorSpaceImpl { /// @name Lie Group /// @{ - static TangentVector Logmap(Scalar m, ChartJacobian H = boost::none) { + static TangentVector Logmap(Scalar m, ChartJacobian H = {}) { if (H) (*H)[0] = 1.0; return Local(0, m); } - static Scalar Expmap(const TangentVector& v, ChartJacobian H = boost::none) { + static Scalar Expmap(const TangentVector& v, ChartJacobian H = {}) { if (H) (*H)[0] = 1.0; return v[0]; } @@ -312,7 +312,7 @@ struct traits > : typedef OptionalJacobian ChartJacobian; static TangentVector Local(const Fixed& origin, const Fixed& other, - ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { + ChartJacobian H1 = {}, ChartJacobian H2 = {}) { if (H1) (*H1) = -Jacobian::Identity(); if (H2) (*H2) = Jacobian::Identity(); TangentVector result; @@ -321,7 +321,7 @@ struct traits > : } static Fixed Retract(const Fixed& origin, const TangentVector& v, - ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { + ChartJacobian H1 = {}, ChartJacobian H2 = {}) { if (H1) (*H1) = Jacobian::Identity(); if (H2) (*H2) = Jacobian::Identity(); return origin + Eigen::Map(v.data()); @@ -330,14 +330,14 @@ struct traits > : /// @name Lie Group /// @{ - static TangentVector Logmap(const Fixed& m, ChartJacobian H = boost::none) { + static TangentVector Logmap(const Fixed& m, ChartJacobian H = {}) { if (H) *H = Jacobian::Identity(); TangentVector result; Eigen::Map(result.data()) = m; return result; } - static Fixed Expmap(const TangentVector& v, ChartJacobian H = boost::none) { + static Fixed Expmap(const TangentVector& v, ChartJacobian H = {}) { Fixed m; m.setZero(); if (H) *H = Jacobian::Identity(); @@ -393,7 +393,7 @@ struct DynamicTraits { } static TangentVector Local(const Dynamic& m, const Dynamic& other, // - ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { + ChartJacobian H1 = {}, ChartJacobian H2 = {}) { if (H1) *H1 = -Eye(m); if (H2) *H2 = Eye(m); TangentVector v(GetDimension(m)); @@ -402,7 +402,7 @@ struct DynamicTraits { } static Dynamic Retract(const Dynamic& m, const TangentVector& v, // - ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { + ChartJacobian H1 = {}, ChartJacobian H2 = {}) { if (H1) *H1 = Eye(m); if (H2) *H2 = Eye(m); return m + Eigen::Map(v.data(), m.rows(), m.cols()); @@ -411,32 +411,32 @@ struct DynamicTraits { /// @name Lie Group /// @{ - static TangentVector Logmap(const Dynamic& m, ChartJacobian H = boost::none) { + static TangentVector Logmap(const Dynamic& m, ChartJacobian H = {}) { if (H) *H = Eye(m); TangentVector result(GetDimension(m)); Eigen::Map(result.data(), m.cols(), m.rows()) = m; return result; } - static Dynamic Expmap(const TangentVector& /*v*/, ChartJacobian H = boost::none) { + static Dynamic Expmap(const TangentVector& /*v*/, ChartJacobian H = {}) { static_cast(H); throw std::runtime_error("Expmap not defined for dynamic types"); } - static Dynamic Inverse(const Dynamic& m, ChartJacobian H = boost::none) { + static Dynamic Inverse(const Dynamic& m, ChartJacobian H = {}) { if (H) *H = -Eye(m); return -m; } static Dynamic Compose(const Dynamic& v1, const Dynamic& v2, - ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { + ChartJacobian H1 = {}, ChartJacobian H2 = {}) { if (H1) *H1 = Eye(v1); if (H2) *H2 = Eye(v1); return v1 + v2; } static Dynamic Between(const Dynamic& v1, const Dynamic& v2, - ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { + ChartJacobian H1 = {}, ChartJacobian H2 = {}) { if (H1) *H1 = -Eye(v1); if (H2) *H2 = Eye(v1); return v2 - v1; diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h index e4255bc63..8c9934d77 100644 --- a/gtsam/base/numericalDerivative.h +++ b/gtsam/base/numericalDerivative.h @@ -37,8 +37,8 @@ namespace gtsam { * for a function with one relevant param and an optional derivative: * Foo bar(const Obj& a, OptionalMatrixType H1) * Use boost.bind to restructure: - * std::bind(bar, std::placeholders::_1, boost::none) - * This syntax will fix the optional argument to boost::none, while using the first argument provided + * std::bind(bar, std::placeholders::_1, {}) + * This syntax will fix the optional argument to {}, while using the first argument provided * * For member functions, such as below, with an instantiated copy instanceOfSomeClass * Foo SomeClass::bar(const Obj& a) diff --git a/gtsam/base/tests/testOptionalJacobian.cpp b/gtsam/base/tests/testOptionalJacobian.cpp index ae91642f4..7f1197584 100644 --- a/gtsam/base/tests/testOptionalJacobian.cpp +++ b/gtsam/base/tests/testOptionalJacobian.cpp @@ -32,7 +32,6 @@ using namespace gtsam; TEST( OptionalJacobian, Constructors ) { Matrix23 fixed; Matrix dynamic; - boost::optional optional(dynamic); OptionalJacobian<2, 3> H; EXPECT(!H); @@ -41,22 +40,18 @@ TEST( OptionalJacobian, Constructors ) { TEST_CONSTRUCTOR(2, 3, &fixed, true); TEST_CONSTRUCTOR(2, 3, dynamic, true); TEST_CONSTRUCTOR(2, 3, &dynamic, true); - TEST_CONSTRUCTOR(2, 3, boost::none, false); - TEST_CONSTRUCTOR(2, 3, optional, true); // Test dynamic OptionalJacobian<-1, -1> H7; EXPECT(!H7); TEST_CONSTRUCTOR(-1, -1, dynamic, true); - TEST_CONSTRUCTOR(-1, -1, boost::none, false); - TEST_CONSTRUCTOR(-1, -1, optional, true); } //****************************************************************************** Matrix kTestMatrix = (Matrix23() << 11,12,13,21,22,23).finished(); -void test(OptionalJacobian<2, 3> H = boost::none) { +void test(OptionalJacobian<2, 3> H = {}) { if (H) *H = kTestMatrix; } @@ -116,7 +111,7 @@ TEST( OptionalJacobian, Fixed) { } //****************************************************************************** -void test2(OptionalJacobian<-1,-1> H = boost::none) { +void test2(OptionalJacobian<-1,-1> H = {}) { if (H) *H = kTestMatrix; // resizes } @@ -145,12 +140,12 @@ TEST( OptionalJacobian, Dynamic) { } //****************************************************************************** -void test3(double add, OptionalJacobian<2,1> H = boost::none) { +void test3(double add, OptionalJacobian<2,1> H = {}) { if (H) *H << add + 10, add + 20; } // This function calls the above function three times, one for each column -void test4(OptionalJacobian<2, 3> H = boost::none) { +void test4(OptionalJacobian<2, 3> H = {}) { if (H) { test3(1, H.cols<1>(0)); test3(2, H.cols<1>(1)); diff --git a/gtsam/basis/Basis.h b/gtsam/basis/Basis.h index d517078f1..97704dab4 100644 --- a/gtsam/basis/Basis.h +++ b/gtsam/basis/Basis.h @@ -152,14 +152,14 @@ class Basis { /// Regular 1D evaluation double apply(const typename DERIVED::Parameters& p, - OptionalJacobian<-1, -1> H = boost::none) const { + OptionalJacobian<-1, -1> H = {}) const { if (H) *H = weights_; return (weights_ * p)(0); } /// c++ sugar double operator()(const typename DERIVED::Parameters& p, - OptionalJacobian<-1, -1> H = boost::none) const { + OptionalJacobian<-1, -1> H = {}) const { return apply(p, H); // might call apply in derived } @@ -212,14 +212,14 @@ class Basis { /// M-dimensional evaluation VectorM apply(const ParameterMatrix& P, - OptionalJacobian H = boost::none) const { + OptionalJacobian H = {}) const { if (H) *H = H_; return P.matrix() * this->weights_.transpose(); } /// c++ sugar VectorM operator()(const ParameterMatrix& P, - OptionalJacobian H = boost::none) const { + OptionalJacobian H = {}) const { return apply(P, H); } }; @@ -271,14 +271,14 @@ class Basis { /// Calculate component of component rowIndex_ of P double apply(const ParameterMatrix& P, - OptionalJacobian H = boost::none) const { + OptionalJacobian H = {}) const { if (H) *H = H_; return P.row(rowIndex_) * EvaluationFunctor::weights_.transpose(); } /// c++ sugar double operator()(const ParameterMatrix& P, - OptionalJacobian H = boost::none) const { + OptionalJacobian H = {}) const { return apply(P, H); } }; @@ -315,7 +315,7 @@ class Basis { /// Manifold evaluation T apply(const ParameterMatrix& P, - OptionalJacobian H = boost::none) const { + OptionalJacobian H = {}) const { // Interpolate the M-dimensional vector to yield a vector in tangent space Eigen::Matrix xi = Base::operator()(P, H); @@ -334,7 +334,7 @@ class Basis { /// c++ sugar T operator()(const ParameterMatrix& P, - OptionalJacobian H = boost::none) const { + OptionalJacobian H = {}) const { return apply(P, H); // might call apply in derived } }; @@ -377,13 +377,13 @@ class Basis { : DerivativeFunctorBase(N, x, a, b) {} double apply(const typename DERIVED::Parameters& p, - OptionalJacobian H = boost::none) const { + OptionalJacobian H = {}) const { if (H) *H = this->weights_; return (this->weights_ * p)(0); } /// c++ sugar double operator()(const typename DERIVED::Parameters& p, - OptionalJacobian H = boost::none) const { + OptionalJacobian H = {}) const { return apply(p, H); // might call apply in derived } }; @@ -433,14 +433,14 @@ class Basis { } VectorM apply(const ParameterMatrix& P, - OptionalJacobian H = boost::none) const { + OptionalJacobian H = {}) const { if (H) *H = H_; return P.matrix() * this->weights_.transpose(); } /// c++ sugar VectorM operator()( const ParameterMatrix& P, - OptionalJacobian H = boost::none) const { + OptionalJacobian H = {}) const { return apply(P, H); } }; @@ -491,13 +491,13 @@ class Basis { } /// Calculate derivative of component rowIndex_ of F double apply(const ParameterMatrix& P, - OptionalJacobian H = boost::none) const { + OptionalJacobian H = {}) const { if (H) *H = H_; return P.row(rowIndex_) * this->weights_.transpose(); } /// c++ sugar double operator()(const ParameterMatrix& P, - OptionalJacobian H = boost::none) const { + OptionalJacobian H = {}) const { return apply(P, H); } }; diff --git a/gtsam/basis/tests/testChebyshev2.cpp b/gtsam/basis/tests/testChebyshev2.cpp index 73339e0f1..177ea5b49 100644 --- a/gtsam/basis/tests/testChebyshev2.cpp +++ b/gtsam/basis/tests/testChebyshev2.cpp @@ -17,6 +17,7 @@ * methods */ +#include #include #include #include @@ -119,7 +120,7 @@ TEST(Chebyshev2, InterpolateVector) { // Check derivative std::function)> f = boost::bind( - &Chebyshev2::VectorEvaluationFunctor<2>::operator(), fx, _1, boost::none); + &Chebyshev2::VectorEvaluationFunctor<2>::operator(), fx, _1, nullptr); Matrix numericalH = numericalDerivative11, 2 * N>(f, X); EXPECT(assert_equal(numericalH, actualH, 1e-9)); @@ -377,7 +378,7 @@ TEST(Chebyshev2, VectorDerivativeFunctor) { // Test Jacobian Matrix expectedH = numericalDerivative11, M * N>( - boost::bind(&VecD::operator(), fx, _1, boost::none), X); + boost::bind(&VecD::operator(), fx, _1, nullptr), X); EXPECT(assert_equal(expectedH, actualH, 1e-7)); } @@ -409,7 +410,7 @@ TEST(Chebyshev2, VectorDerivativeFunctor2) { VecD vecd(N, points(0), 0, T); vecd(X, actualH); Matrix expectedH = numericalDerivative11, M * N>( - boost::bind(&VecD::operator(), vecd, _1, boost::none), X); + boost::bind(&VecD::operator(), vecd, _1, nullptr), X); EXPECT(assert_equal(expectedH, actualH, 1e-6)); } @@ -426,7 +427,7 @@ TEST(Chebyshev2, ComponentDerivativeFunctor) { EXPECT_DOUBLES_EQUAL(0, fx(X, actualH), 1e-8); Matrix expectedH = numericalDerivative11, M * N>( - boost::bind(&CompFunc::operator(), fx, _1, boost::none), X); + boost::bind(&CompFunc::operator(), fx, _1, nullptr), X); EXPECT(assert_equal(expectedH, actualH, 1e-7)); } diff --git a/gtsam/geometry/BearingRange.h b/gtsam/geometry/BearingRange.h index 95b0232f0..ca767c1f1 100644 --- a/gtsam/geometry/BearingRange.h +++ b/gtsam/geometry/BearingRange.h @@ -77,8 +77,8 @@ public: /// Prediction function that stacks measurements static BearingRange Measure( const A1& a1, const A2& a2, - OptionalJacobian::dimension> H1 = boost::none, - OptionalJacobian::dimension> H2 = boost::none) { + OptionalJacobian::dimension> H1 = {}, + OptionalJacobian::dimension> H2 = {}) { typename MakeJacobian::type HB1; typename MakeJacobian::type HB2; typename MakeJacobian::type HR1; @@ -181,8 +181,8 @@ struct HasBearing { typedef RT result_type; RT operator()( const A1& a1, const A2& a2, - OptionalJacobian::dimension, traits::dimension> H1=boost::none, - OptionalJacobian::dimension, traits::dimension> H2=boost::none) { + OptionalJacobian::dimension, traits::dimension> H1={}, + OptionalJacobian::dimension, traits::dimension> H2={}) { return a1.bearing(a2, H1, H2); } }; @@ -195,8 +195,8 @@ struct HasRange { typedef RT result_type; RT operator()( const A1& a1, const A2& a2, - OptionalJacobian::dimension, traits::dimension> H1=boost::none, - OptionalJacobian::dimension, traits::dimension> H2=boost::none) { + OptionalJacobian::dimension, traits::dimension> H1={}, + OptionalJacobian::dimension, traits::dimension> H2={}) { return a1.range(a2, H1, H2); } }; diff --git a/gtsam/geometry/Cal3.h b/gtsam/geometry/Cal3.h index 25acc11f8..6f446c8b3 100644 --- a/gtsam/geometry/Cal3.h +++ b/gtsam/geometry/Cal3.h @@ -45,8 +45,8 @@ namespace gtsam { */ template void calibrateJacobians(const Cal& calibration, const Point2& pn, - OptionalJacobian<2, Dim> Dcal = boost::none, - OptionalJacobian<2, 2> Dp = boost::none) { + OptionalJacobian<2, Dim> Dcal = {}, + OptionalJacobian<2, 2> Dp = {}) { if (Dcal || Dp) { Eigen::Matrix H_uncal_K; Matrix22 H_uncal_pn, H_uncal_pn_inv; diff --git a/gtsam/geometry/Cal3Bundler.cpp b/gtsam/geometry/Cal3Bundler.cpp index 3ae624bbc..cfaea61a9 100644 --- a/gtsam/geometry/Cal3Bundler.cpp +++ b/gtsam/geometry/Cal3Bundler.cpp @@ -131,14 +131,14 @@ Point2 Cal3Bundler::calibrate(const Point2& pi, OptionalJacobian<2, 3> Dcal, /* ************************************************************************* */ Matrix2 Cal3Bundler::D2d_intrinsic(const Point2& p) const { Matrix2 Dp; - uncalibrate(p, boost::none, Dp); + uncalibrate(p, {}, Dp); return Dp; } /* ************************************************************************* */ Matrix23 Cal3Bundler::D2d_calibration(const Point2& p) const { Matrix23 Dcal; - uncalibrate(p, Dcal, boost::none); + uncalibrate(p, Dcal, {}); return Dcal; } diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h index c8e0fe381..f333c2f02 100644 --- a/gtsam/geometry/Cal3Bundler.h +++ b/gtsam/geometry/Cal3Bundler.h @@ -116,8 +116,8 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 { * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates * @return point in image coordinates */ - Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 3> Dcal = boost::none, - OptionalJacobian<2, 2> Dp = boost::none) const; + Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 3> Dcal = {}, + OptionalJacobian<2, 2> Dp = {}) const; /** * Convert a pixel coordinate to ideal coordinate xy @@ -127,8 +127,8 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 { * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates * @return point in intrinsic coordinates */ - Point2 calibrate(const Point2& pi, OptionalJacobian<2, 3> Dcal = boost::none, - OptionalJacobian<2, 2> Dp = boost::none) const; + Point2 calibrate(const Point2& pi, OptionalJacobian<2, 3> Dcal = {}, + OptionalJacobian<2, 2> Dp = {}) const; /// @deprecated might be removed in next release, use uncalibrate Matrix2 D2d_intrinsic(const Point2& p) const; diff --git a/gtsam/geometry/Cal3DS2_Base.h b/gtsam/geometry/Cal3DS2_Base.h index f901de9a9..c8c76dcb9 100644 --- a/gtsam/geometry/Cal3DS2_Base.h +++ b/gtsam/geometry/Cal3DS2_Base.h @@ -122,12 +122,12 @@ class GTSAM_EXPORT Cal3DS2_Base : public Cal3 { * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates * @return point in (distorted) image coordinates */ - Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = boost::none, - OptionalJacobian<2, 2> Dp = boost::none) const; + Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = {}, + OptionalJacobian<2, 2> Dp = {}) const; /// Convert (distorted) image coordinates uv to intrinsic coordinates xy - Point2 calibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = boost::none, - OptionalJacobian<2, 2> Dp = boost::none) const; + Point2 calibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = {}, + OptionalJacobian<2, 2> Dp = {}) const; /// Derivative of uncalibrate wrpt intrinsic coordinates Matrix2 D2d_intrinsic(const Point2& p) const; diff --git a/gtsam/geometry/Cal3Fisheye.cpp b/gtsam/geometry/Cal3Fisheye.cpp index fd2c7ab65..fac09e465 100644 --- a/gtsam/geometry/Cal3Fisheye.cpp +++ b/gtsam/geometry/Cal3Fisheye.cpp @@ -134,7 +134,7 @@ Point2 Cal3Fisheye::calibrate(const Point2& uv, OptionalJacobian<2, 9> Dcal, Matrix2 jac; // Calculate the current estimate (uv_hat) and the jacobian - const Point2 uv_hat = uncalibrate(pi, boost::none, jac); + const Point2 uv_hat = uncalibrate(pi, {}, jac); // Test convergence if ((uv_hat - uv).norm() < tol_) break; diff --git a/gtsam/geometry/Cal3Fisheye.h b/gtsam/geometry/Cal3Fisheye.h index a314acb5e..2530231c5 100644 --- a/gtsam/geometry/Cal3Fisheye.h +++ b/gtsam/geometry/Cal3Fisheye.h @@ -121,8 +121,8 @@ class GTSAM_EXPORT Cal3Fisheye : public Cal3 { * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates (xi, yi) * @return point in (distorted) image coordinates */ - Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = boost::none, - OptionalJacobian<2, 2> Dp = boost::none) const; + Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = {}, + OptionalJacobian<2, 2> Dp = {}) const; /** * Convert (distorted) image coordinates [u;v] to intrinsic coordinates [x_i, @@ -132,8 +132,8 @@ class GTSAM_EXPORT Cal3Fisheye : public Cal3 { * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates (xi, yi) * @return point in intrinsic coordinates */ - Point2 calibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = boost::none, - OptionalJacobian<2, 2> Dp = boost::none) const; + Point2 calibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = {}, + OptionalJacobian<2, 2> Dp = {}) const; /// @} /// @name Testable diff --git a/gtsam/geometry/Cal3Unified.h b/gtsam/geometry/Cal3Unified.h index 83024c0ce..cf25c8f00 100644 --- a/gtsam/geometry/Cal3Unified.h +++ b/gtsam/geometry/Cal3Unified.h @@ -106,12 +106,12 @@ class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base { * @return point in image coordinates */ Point2 uncalibrate(const Point2& p, - OptionalJacobian<2, 10> Dcal = boost::none, - OptionalJacobian<2, 2> Dp = boost::none) const; + OptionalJacobian<2, 10> Dcal = {}, + OptionalJacobian<2, 2> Dp = {}) const; /// Conver a pixel coordinate to ideal coordinate - Point2 calibrate(const Point2& p, OptionalJacobian<2, 10> Dcal = boost::none, - OptionalJacobian<2, 2> Dp = boost::none) const; + Point2 calibrate(const Point2& p, OptionalJacobian<2, 10> Dcal = {}, + OptionalJacobian<2, 2> Dp = {}) const; /// Convert a 3D point to normalized unit plane Point2 spaceToNPlane(const Point2& p) const; diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index 10061fae8..b8d776538 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/gtsam/geometry/Cal3_S2.h @@ -66,8 +66,8 @@ class GTSAM_EXPORT Cal3_S2 : public Cal3 { * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates * @return point in image coordinates */ - Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 5> Dcal = boost::none, - OptionalJacobian<2, 2> Dp = boost::none) const; + Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 5> Dcal = {}, + OptionalJacobian<2, 2> Dp = {}) const; /** * Convert image coordinates uv to intrinsic coordinates xy @@ -76,8 +76,8 @@ class GTSAM_EXPORT Cal3_S2 : public Cal3 { * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates * @return point in intrinsic coordinates */ - Point2 calibrate(const Point2& p, OptionalJacobian<2, 5> Dcal = boost::none, - OptionalJacobian<2, 2> Dp = boost::none) const; + Point2 calibrate(const Point2& p, OptionalJacobian<2, 5> Dcal = {}, + OptionalJacobian<2, 2> Dp = {}) const; /** * Convert homogeneous image coordinates to intrinsic coordinates @@ -102,8 +102,8 @@ class GTSAM_EXPORT Cal3_S2 : public Cal3 { /// "Between", subtracts calibrations. between(p,q) == compose(inverse(p),q) inline Cal3_S2 between(const Cal3_S2& q, - OptionalJacobian<5, 5> H1 = boost::none, - OptionalJacobian<5, 5> H2 = boost::none) const { + OptionalJacobian<5, 5> H1 = {}, + OptionalJacobian<5, 5> H2 = {}) const { if (H1) *H1 = -I_5x5; if (H2) *H2 = I_5x5; return Cal3_S2(q.fx_ - fx_, q.fy_ - fy_, q.s_ - s_, q.u0_ - u0_, diff --git a/gtsam/geometry/Cal3_S2Stereo.h b/gtsam/geometry/Cal3_S2Stereo.h index ddd96bf17..574b5d7b5 100644 --- a/gtsam/geometry/Cal3_S2Stereo.h +++ b/gtsam/geometry/Cal3_S2Stereo.h @@ -64,8 +64,8 @@ class GTSAM_EXPORT Cal3_S2Stereo : public Cal3_S2 { * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates * @return point in image coordinates */ - Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 6> Dcal = boost::none, - OptionalJacobian<2, 2> Dp = boost::none) const; + Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 6> Dcal = {}, + OptionalJacobian<2, 2> Dp = {}) const; /** * Convert image coordinates uv to intrinsic coordinates xy @@ -74,8 +74,8 @@ class GTSAM_EXPORT Cal3_S2Stereo : public Cal3_S2 { * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates * @return point in intrinsic coordinates */ - Point2 calibrate(const Point2& p, OptionalJacobian<2, 6> Dcal = boost::none, - OptionalJacobian<2, 2> Dp = boost::none) const; + Point2 calibrate(const Point2& p, OptionalJacobian<2, 6> Dcal = {}, + OptionalJacobian<2, 2> Dp = {}) const; /** * Convert homogeneous image coordinates to intrinsic coordinates diff --git a/gtsam/geometry/CalibratedCamera.cpp b/gtsam/geometry/CalibratedCamera.cpp index dd88f9f69..34deb5c84 100644 --- a/gtsam/geometry/CalibratedCamera.cpp +++ b/gtsam/geometry/CalibratedCamera.cpp @@ -117,7 +117,7 @@ Point2 PinholeBase::project2(const Point3& point, OptionalJacobian<2, 6> Dpose, OptionalJacobian<2, 3> Dpoint) const { Matrix3 Rt; // calculated by transformTo if needed - const Point3 q = pose().transformTo(point, boost::none, Dpoint ? &Rt : 0); + const Point3 q = pose().transformTo(point, {}, Dpoint ? &Rt : 0); #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION if (q.z() <= 0) throw CheiralityException(); diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index 941ed3ffc..c8e27444e 100644 --- a/gtsam/geometry/CalibratedCamera.h +++ b/gtsam/geometry/CalibratedCamera.h @@ -176,7 +176,7 @@ public: * @param pc point in camera coordinates */ static Point2 Project(const Point3& pc, // - OptionalJacobian<2, 3> Dpoint = boost::none); + OptionalJacobian<2, 3> Dpoint = {}); /** * Project from 3D point at infinity in camera coordinates into image @@ -184,7 +184,7 @@ public: * @param pc point in camera coordinates */ static Point2 Project(const Unit3& pc, // - OptionalJacobian<2, 2> Dpoint = boost::none); + OptionalJacobian<2, 2> Dpoint = {}); /// Project a point into the image and check depth std::pair projectSafe(const Point3& pw) const; @@ -195,7 +195,7 @@ public: * @return the intrinsic coordinates of the projected point */ Point2 project2(const Point3& point, OptionalJacobian<2, 6> Dpose = - boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const; + {}, OptionalJacobian<2, 3> Dpoint = {}) const; /** Project point at infinity into the image * Throws a CheiralityException if point behind image plane iff GTSAM_THROW_CHEIRALITY_EXCEPTION @@ -203,13 +203,13 @@ public: * @return the intrinsic coordinates of the projected point */ Point2 project2(const Unit3& point, - OptionalJacobian<2, 6> Dpose = boost::none, - OptionalJacobian<2, 2> Dpoint = boost::none) const; + OptionalJacobian<2, 6> Dpose = {}, + OptionalJacobian<2, 2> Dpoint = {}) const; /// backproject a 2-dimensional point to a 3-dimensional point at given depth static Point3 BackprojectFromCamera(const Point2& p, const double depth, - OptionalJacobian<3, 2> Dpoint = boost::none, - OptionalJacobian<3, 1> Ddepth = boost::none); + OptionalJacobian<3, 2> Dpoint = {}, + OptionalJacobian<3, 1> Ddepth = {}); /// @} /// @name Advanced interface @@ -270,7 +270,7 @@ public: // Create CalibratedCamera, with derivatives static CalibratedCamera Create(const Pose3& pose, - OptionalJacobian H1 = boost::none) { + OptionalJacobian H1 = {}) { if (H1) *H1 << I_6x6; return CalibratedCamera(pose); @@ -346,13 +346,13 @@ public: * Use project2, which is more consistently named across Pinhole cameras */ Point2 project(const Point3& point, OptionalJacobian<2, 6> Dcamera = - boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const; + {}, OptionalJacobian<2, 3> Dpoint = {}) const; /// backproject a 2-dimensional point to a 3-dimensional point at given depth Point3 backproject(const Point2& pn, double depth, - OptionalJacobian<3, 6> Dresult_dpose = boost::none, - OptionalJacobian<3, 2> Dresult_dp = boost::none, - OptionalJacobian<3, 1> Dresult_ddepth = boost::none) const { + OptionalJacobian<3, 6> Dresult_dpose = {}, + OptionalJacobian<3, 2> Dresult_dp = {}, + OptionalJacobian<3, 1> Dresult_ddepth = {}) const { Matrix32 Dpoint_dpn; Matrix31 Dpoint_ddepth; @@ -379,8 +379,8 @@ public: * @return range (double) */ double range(const Point3& point, - OptionalJacobian<1, 6> Dcamera = boost::none, - OptionalJacobian<1, 3> Dpoint = boost::none) const { + OptionalJacobian<1, 6> Dcamera = {}, + OptionalJacobian<1, 3> Dpoint = {}) const { return pose().range(point, Dcamera, Dpoint); } @@ -389,8 +389,8 @@ public: * @param pose Other SO(3) pose * @return range (double) */ - double range(const Pose3& pose, OptionalJacobian<1, 6> Dcamera = boost::none, - OptionalJacobian<1, 6> Dpose = boost::none) const { + double range(const Pose3& pose, OptionalJacobian<1, 6> Dcamera = {}, + OptionalJacobian<1, 6> Dpose = {}) const { return this->pose().range(pose, Dcamera, Dpose); } @@ -400,8 +400,8 @@ public: * @return range (double) */ double range(const CalibratedCamera& camera, // - OptionalJacobian<1, 6> H1 = boost::none, // - OptionalJacobian<1, 6> H2 = boost::none) const { + OptionalJacobian<1, 6> H1 = {}, // + OptionalJacobian<1, 6> H2 = {}) const { return pose().range(camera.pose(), H1, H2); } diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h index 909576aa0..ea980d667 100644 --- a/gtsam/geometry/EssentialMatrix.h +++ b/gtsam/geometry/EssentialMatrix.h @@ -49,12 +49,12 @@ class EssentialMatrix { /// Named constructor with derivatives GTSAM_EXPORT static EssentialMatrix FromRotationAndDirection(const Rot3& aRb, const Unit3& aTb, - OptionalJacobian<5, 3> H1 = boost::none, - OptionalJacobian<5, 2> H2 = boost::none); + OptionalJacobian<5, 3> H1 = {}, + OptionalJacobian<5, 2> H2 = {}); /// Named constructor converting a Pose3 with scale to EssentialMatrix (no scale) GTSAM_EXPORT static EssentialMatrix FromPose3(const Pose3& _1P2_, - OptionalJacobian<5, 6> H = boost::none); + OptionalJacobian<5, 6> H = {}); /// Random, using Rot3::Random and Unit3::Random template @@ -139,8 +139,8 @@ class EssentialMatrix { * @return point in pose coordinates */ GTSAM_EXPORT Point3 transformTo(const Point3& p, - OptionalJacobian<3, 5> DE = boost::none, - OptionalJacobian<3, 3> Dpoint = boost::none) const; + OptionalJacobian<3, 5> DE = {}, + OptionalJacobian<3, 3> Dpoint = {}) const; /** * Given essential matrix E in camera frame B, convert to body frame C @@ -148,7 +148,7 @@ class EssentialMatrix { * @param E essential matrix E in camera frame C */ GTSAM_EXPORT EssentialMatrix rotate(const Rot3& cRb, OptionalJacobian<5, 5> HE = - boost::none, OptionalJacobian<5, 3> HR = boost::none) const; + {}, OptionalJacobian<5, 3> HR = {}) const; /** * Given essential matrix E in camera frame B, convert to body frame C @@ -161,7 +161,7 @@ class EssentialMatrix { /// epipolar error, algebraic GTSAM_EXPORT double error(const Vector3& vA, const Vector3& vB, - OptionalJacobian<1, 5> H = boost::none) const; + OptionalJacobian<1, 5> H = {}) const; /// @} diff --git a/gtsam/geometry/Line3.h b/gtsam/geometry/Line3.h index 08b81eaef..b1a1e4f6e 100644 --- a/gtsam/geometry/Line3.h +++ b/gtsam/geometry/Line3.h @@ -32,8 +32,8 @@ class Line3; * @return Transformed line in camera frame */ GTSAM_EXPORT Line3 transformTo(const Pose3 &wTc, const Line3 &wL, - OptionalJacobian<4, 6> Dpose = boost::none, - OptionalJacobian<4, 4> Dline = boost::none); + OptionalJacobian<4, 6> Dpose = {}, + OptionalJacobian<4, 4> Dline = {}); /** @@ -69,8 +69,8 @@ class GTSAM_EXPORT Line3 { * @return q: resulting line after adding the increment and mapping to the manifold */ Line3 retract(const Vector4 &v, - OptionalJacobian<4, 4> Dp = boost::none, - OptionalJacobian<4, 4> Dv = boost::none) const; + OptionalJacobian<4, 4> Dp = {}, + OptionalJacobian<4, 4> Dv = {}) const; /** * The localCoordinates method is the inverse of retract and finds the difference @@ -82,8 +82,8 @@ class GTSAM_EXPORT Line3 { * @return v: difference in the tangent space */ Vector4 localCoordinates(const Line3 &q, - OptionalJacobian<4, 4> Dp = boost::none, - OptionalJacobian<4, 4> Dq = boost::none) const; + OptionalJacobian<4, 4> Dp = {}, + OptionalJacobian<4, 4> Dq = {}) const; /** * Print R, a, b @@ -105,7 +105,7 @@ class GTSAM_EXPORT Line3 { * @return Unit3 - projected line in image plane, in homogenous coordinates. * We use Unit3 since it is a manifold with the right dimension. */ - Unit3 project(OptionalJacobian<2, 4> Dline = boost::none) const; + Unit3 project(OptionalJacobian<2, 4> Dline = {}) const; /** * Returns point on the line that is at a certain distance starting from the diff --git a/gtsam/geometry/OrientedPlane3.h b/gtsam/geometry/OrientedPlane3.h index d0d912ee9..07c8445fe 100644 --- a/gtsam/geometry/OrientedPlane3.h +++ b/gtsam/geometry/OrientedPlane3.h @@ -87,8 +87,8 @@ public: * @return the transformed plane */ OrientedPlane3 transform(const Pose3& xr, - OptionalJacobian<3, 3> Hp = boost::none, - OptionalJacobian<3, 6> Hr = boost::none) const; + OptionalJacobian<3, 3> Hp = {}, + OptionalJacobian<3, 6> Hr = {}) const; /** Computes the error between the two planes, with derivatives. * This uses Unit3::errorVector, as opposed to the other .error() in this @@ -98,8 +98,8 @@ public: * @param other the other plane */ Vector3 errorVector(const OrientedPlane3& other, - OptionalJacobian<3, 3> H1 = boost::none, - OptionalJacobian<3, 3> H2 = boost::none) const; + OptionalJacobian<3, 3> H1 = {}, + OptionalJacobian<3, 3> H2 = {}) const; /// Dimensionality of tangent space = 3 DOF inline static size_t Dim() { @@ -113,7 +113,7 @@ public: /// The retract function OrientedPlane3 retract(const Vector3& v, - OptionalJacobian<3, 3> H = boost::none) const; + OptionalJacobian<3, 3> H = {}) const; /// The local coordinates function Vector3 localCoordinates(const OrientedPlane3& s) const; @@ -125,13 +125,13 @@ public: } /// Return the normal - inline Unit3 normal(OptionalJacobian<2, 3> H = boost::none) const { + inline Unit3 normal(OptionalJacobian<2, 3> H = {}) const { if (H) *H << I_2x2, Z_2x1; return n_; } /// Return the perpendicular distance to the origin - inline double distance(OptionalJacobian<1, 3> H = boost::none) const { + inline double distance(OptionalJacobian<1, 3> H = {}) const { if (H) *H << 0,0,1; return d_; } diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index 48233c1ed..8f299f854 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -109,8 +109,8 @@ public: // Create PinholeCamera, with derivatives static PinholeCamera Create(const Pose3& pose, const Calibration &K, - OptionalJacobian H1 = boost::none, // - OptionalJacobian H2 = boost::none) { + OptionalJacobian H1 = {}, // + OptionalJacobian H2 = {}) { typedef Eigen::Matrix MatrixK6; if (H1) *H1 << I_6x6, MatrixK6::Zero(); @@ -237,19 +237,19 @@ public: *Dcamera << Dpose, Dcal; return pi; } else { - return Base::project(pw, boost::none, Dpoint, boost::none); + return Base::project(pw, {}, Dpoint, {}); } } /// project a 3D point from world coordinates into the image Point2 project2(const Point3& pw, OptionalJacobian<2, dimension> Dcamera = - boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const { + {}, OptionalJacobian<2, 3> Dpoint = {}) const { return _project2(pw, Dcamera, Dpoint); } /// project a point at infinity from world coordinates into the image Point2 project2(const Unit3& pw, OptionalJacobian<2, dimension> Dcamera = - boost::none, OptionalJacobian<2, 2> Dpoint = boost::none) const { + {}, OptionalJacobian<2, 2> Dpoint = {}) const { return _project2(pw, Dcamera, Dpoint); } @@ -259,7 +259,7 @@ public: * @return range (double) */ double range(const Point3& point, OptionalJacobian<1, dimension> Dcamera = - boost::none, OptionalJacobian<1, 3> Dpoint = boost::none) const { + {}, OptionalJacobian<1, 3> Dpoint = {}) const { Matrix16 Dpose_; double result = this->pose().range(point, Dcamera ? &Dpose_ : 0, Dpoint); if (Dcamera) @@ -273,7 +273,7 @@ public: * @return range (double) */ double range(const Pose3& pose, OptionalJacobian<1, dimension> Dcamera = - boost::none, OptionalJacobian<1, 6> Dpose = boost::none) const { + {}, OptionalJacobian<1, 6> Dpose = {}) const { Matrix16 Dpose_; double result = this->pose().range(pose, Dcamera ? &Dpose_ : 0, Dpose); if (Dcamera) @@ -288,8 +288,8 @@ public: */ template double range(const PinholeCamera& camera, - OptionalJacobian<1, dimension> Dcamera = boost::none, - OptionalJacobian<1, 6 + CalibrationB::dimension> Dother = boost::none) const { + OptionalJacobian<1, dimension> Dcamera = {}, + OptionalJacobian<1, 6 + CalibrationB::dimension> Dother = {}) const { Matrix16 Dcamera_, Dother_; double result = this->pose().range(camera.pose(), Dcamera ? &Dcamera_ : 0, Dother ? &Dother_ : 0); @@ -309,8 +309,8 @@ public: * @return range (double) */ double range(const CalibratedCamera& camera, - OptionalJacobian<1, dimension> Dcamera = boost::none, - OptionalJacobian<1, 6> Dother = boost::none) const { + OptionalJacobian<1, dimension> Dcamera = {}, + OptionalJacobian<1, 6> Dother = {}) const { return range(camera.pose(), Dcamera, Dother); } diff --git a/gtsam/geometry/PinholePose.h b/gtsam/geometry/PinholePose.h index b2de4fb11..e9d3859a1 100644 --- a/gtsam/geometry/PinholePose.h +++ b/gtsam/geometry/PinholePose.h @@ -115,32 +115,32 @@ public: } /// project a 3D point from world coordinates into the image - Point2 project(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none, - OptionalJacobian<2, 3> Dpoint = boost::none, - OptionalJacobian<2, DimK> Dcal = boost::none) const { + Point2 project(const Point3& pw, OptionalJacobian<2, 6> Dpose = {}, + OptionalJacobian<2, 3> Dpoint = {}, + OptionalJacobian<2, DimK> Dcal = {}) const { return _project(pw, Dpose, Dpoint, Dcal); } /// project a 3D point from world coordinates into the image - Point2 reprojectionError(const Point3& pw, const Point2& measured, OptionalJacobian<2, 6> Dpose = boost::none, - OptionalJacobian<2, 3> Dpoint = boost::none, - OptionalJacobian<2, DimK> Dcal = boost::none) const { + Point2 reprojectionError(const Point3& pw, const Point2& measured, OptionalJacobian<2, 6> Dpose = {}, + OptionalJacobian<2, 3> Dpoint = {}, + OptionalJacobian<2, DimK> Dcal = {}) const { return Point2(_project(pw, Dpose, Dpoint, Dcal) - measured); } /// project a point at infinity from world coordinates into the image - Point2 project(const Unit3& pw, OptionalJacobian<2, 6> Dpose = boost::none, - OptionalJacobian<2, 2> Dpoint = boost::none, - OptionalJacobian<2, DimK> Dcal = boost::none) const { + Point2 project(const Unit3& pw, OptionalJacobian<2, 6> Dpose = {}, + OptionalJacobian<2, 2> Dpoint = {}, + OptionalJacobian<2, DimK> Dcal = {}) const { return _project(pw, Dpose, Dpoint, Dcal); } /// backproject a 2-dimensional point to a 3-dimensional point at given depth Point3 backproject(const Point2& p, double depth, - OptionalJacobian<3, 6> Dresult_dpose = boost::none, - OptionalJacobian<3, 2> Dresult_dp = boost::none, - OptionalJacobian<3, 1> Dresult_ddepth = boost::none, - OptionalJacobian<3, DimK> Dresult_dcal = boost::none) const { + OptionalJacobian<3, 6> Dresult_dpose = {}, + OptionalJacobian<3, 2> Dresult_dp = {}, + OptionalJacobian<3, 1> Dresult_ddepth = {}, + OptionalJacobian<3, DimK> Dresult_dcal = {}) const { typedef Eigen::Matrix Matrix2K; Matrix2K Dpn_dcal; Matrix22 Dpn_dp; @@ -179,8 +179,8 @@ public: * @return range (double) */ double range(const Point3& point, - OptionalJacobian<1, 6> Dcamera = boost::none, - OptionalJacobian<1, 3> Dpoint = boost::none) const { + OptionalJacobian<1, 6> Dcamera = {}, + OptionalJacobian<1, 3> Dpoint = {}) const { return pose().range(point, Dcamera, Dpoint); } @@ -189,8 +189,8 @@ public: * @param pose Other SO(3) pose * @return range (double) */ - double range(const Pose3& pose, OptionalJacobian<1, 6> Dcamera = boost::none, - OptionalJacobian<1, 6> Dpose = boost::none) const { + double range(const Pose3& pose, OptionalJacobian<1, 6> Dcamera = {}, + OptionalJacobian<1, 6> Dpose = {}) const { return this->pose().range(pose, Dcamera, Dpose); } @@ -200,7 +200,7 @@ public: * @return range (double) */ double range(const CalibratedCamera& camera, OptionalJacobian<1, 6> Dcamera = - boost::none, OptionalJacobian<1, 6> Dother = boost::none) const { + {}, OptionalJacobian<1, 6> Dother = {}) const { return pose().range(camera.pose(), Dcamera, Dother); } @@ -211,8 +211,8 @@ public: */ template double range(const PinholeBaseK& camera, - OptionalJacobian<1, 6> Dcamera = boost::none, - OptionalJacobian<1, 6> Dother = boost::none) const { + OptionalJacobian<1, 6> Dcamera = {}, + OptionalJacobian<1, 6> Dother = {}) const { return pose().range(camera.pose(), Dcamera, Dother); } @@ -376,14 +376,14 @@ public: * @param Dpose is the Jacobian w.r.t. the whole camera (really only the pose) * @param Dpoint is the Jacobian w.r.t. point3 */ - Point2 project2(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none, - OptionalJacobian<2, 3> Dpoint = boost::none) const { + Point2 project2(const Point3& pw, OptionalJacobian<2, 6> Dpose = {}, + OptionalJacobian<2, 3> Dpoint = {}) const { return Base::project(pw, Dpose, Dpoint); } /// project2 version for point at infinity - Point2 project2(const Unit3& pw, OptionalJacobian<2, 6> Dpose = boost::none, - OptionalJacobian<2, 2> Dpoint = boost::none) const { + Point2 project2(const Unit3& pw, OptionalJacobian<2, 6> Dpose = {}, + OptionalJacobian<2, 2> Dpoint = {}) const { return Base::project(pw, Dpose, Dpoint); } diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index 2b4f48197..6d5940584 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -36,12 +36,12 @@ GTSAM_EXPORT std::ostream &operator<<(std::ostream &os, const gtsam::Point2Pair using Point2Pairs = std::vector; /// Distance of the point from the origin, with Jacobian -GTSAM_EXPORT double norm2(const Point2& p, OptionalJacobian<1, 2> H = boost::none); +GTSAM_EXPORT double norm2(const Point2& p, OptionalJacobian<1, 2> H = {}); /// distance between two points GTSAM_EXPORT double distance2(const Point2& p1, const Point2& q, - OptionalJacobian<1, 2> H1 = boost::none, - OptionalJacobian<1, 2> H2 = boost::none); + OptionalJacobian<1, 2> H1 = {}, + OptionalJacobian<1, 2> H2 = {}); // For MATLAB wrapper typedef std::vector > Point2Vector; @@ -56,14 +56,14 @@ inline Point2 operator*(double s, const Point2& p) { * Calculate f and h, respectively the parallel and perpendicular distance of * the intersections of two circles along and from the line connecting the centers. * Both are dimensionless fractions of the distance d between the circle centers. - * If the circles do not intersect or they are identical, returns boost::none. + * If the circles do not intersect or they are identical, returns {}. * If one solution (touching circles, as determined by tol), h will be exactly zero. * h is a good measure for how accurate the intersection will be, as when circles touch * or nearly touch, the intersection is ill-defined with noisy radius measurements. * @param R_d : R/d, ratio of radius of first circle to distance between centers * @param r_d : r/d, ratio of radius of second circle to distance between centers * @param tol: absolute tolerance below which we consider touching circles - * @return optional Point2 with f and h, boost::none if no solution. + * @return optional Point2 with f and h, {} if no solution. */ GTSAM_EXPORT std::optional circleCircleIntersection(double R_d, double r_d, double tol = 1e-9); @@ -97,8 +97,8 @@ template <> struct Range { typedef double result_type; double operator()(const Point2& p, const Point2& q, - OptionalJacobian<1, 2> H1 = boost::none, - OptionalJacobian<1, 2> H2 = boost::none) { + OptionalJacobian<1, 2> H1 = {}, + OptionalJacobian<1, 2> H2 = {}) { return distance2(p, q, H1, H2); } }; diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 13a435c24..b1fd26bb2 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -44,24 +44,24 @@ using Point3Pairs = std::vector; /// distance between two points GTSAM_EXPORT double distance3(const Point3& p1, const Point3& q, - OptionalJacobian<1, 3> H1 = boost::none, - OptionalJacobian<1, 3> H2 = boost::none); + OptionalJacobian<1, 3> H1 = {}, + OptionalJacobian<1, 3> H2 = {}); /// Distance of the point from the origin, with Jacobian -GTSAM_EXPORT double norm3(const Point3& p, OptionalJacobian<1, 3> H = boost::none); +GTSAM_EXPORT double norm3(const Point3& p, OptionalJacobian<1, 3> H = {}); /// normalize, with optional Jacobian -GTSAM_EXPORT Point3 normalize(const Point3& p, OptionalJacobian<3, 3> H = boost::none); +GTSAM_EXPORT Point3 normalize(const Point3& p, OptionalJacobian<3, 3> H = {}); /// cross product @return this x q GTSAM_EXPORT Point3 cross(const Point3& p, const Point3& q, - OptionalJacobian<3, 3> H_p = boost::none, - OptionalJacobian<3, 3> H_q = boost::none); + OptionalJacobian<3, 3> H_p = {}, + OptionalJacobian<3, 3> H_q = {}); /// dot product GTSAM_EXPORT double dot(const Point3& p, const Point3& q, - OptionalJacobian<1, 3> H_p = boost::none, - OptionalJacobian<1, 3> H_q = boost::none); + OptionalJacobian<1, 3> H_p = {}, + OptionalJacobian<1, 3> H_q = {}); /// mean template @@ -82,8 +82,8 @@ template <> struct Range { typedef double result_type; double operator()(const Point3& p, const Point3& q, - OptionalJacobian<1, 3> H1 = boost::none, - OptionalJacobian<1, 3> H2 = boost::none) { + OptionalJacobian<1, 3> H1 = {}, + OptionalJacobian<1, 3> H2 = {}) { return distance3(p, q, H1, H2); } }; diff --git a/gtsam/geometry/Quaternion.h b/gtsam/geometry/Quaternion.h index 2ef47d58e..f9ed2a383 100644 --- a/gtsam/geometry/Quaternion.h +++ b/gtsam/geometry/Quaternion.h @@ -55,14 +55,14 @@ struct traits { /// @name Lie group traits /// @{ static Q Compose(const Q &g, const Q & h, - ChartJacobian Hg = boost::none, ChartJacobian Hh = boost::none) { + ChartJacobian Hg = {}, ChartJacobian Hh = {}) { if (Hg) *Hg = h.toRotationMatrix().transpose(); if (Hh) *Hh = I_3x3; return g * h; } static Q Between(const Q &g, const Q & h, - ChartJacobian Hg = boost::none, ChartJacobian Hh = boost::none) { + ChartJacobian Hg = {}, ChartJacobian Hh = {}) { Q d = g.inverse() * h; if (Hg) *Hg = -d.toRotationMatrix().transpose(); if (Hh) *Hh = I_3x3; @@ -70,14 +70,14 @@ struct traits { } static Q Inverse(const Q &g, - ChartJacobian H = boost::none) { + ChartJacobian H = {}) { if (H) *H = -g.toRotationMatrix(); return g.inverse(); } /// Exponential map, using the inlined code from Eigen's conversion from axis/angle static Q Expmap(const Eigen::Ref& omega, - ChartJacobian H = boost::none) { + ChartJacobian H = {}) { using std::cos; using std::sin; if (H) *H = SO3::ExpmapDerivative(omega.template cast()); @@ -95,7 +95,7 @@ struct traits { } /// We use our own Logmap, as there is a slight bug in Eigen - static TangentVector Logmap(const Q& q, ChartJacobian H = boost::none) { + static TangentVector Logmap(const Q& q, ChartJacobian H = {}) { using std::acos; using std::sqrt; @@ -145,7 +145,7 @@ struct traits { /// @{ static TangentVector Local(const Q& g, const Q& h, - ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { + ChartJacobian H1 = {}, ChartJacobian H2 = {}) { Q b = Between(g, h, H1, H2); Matrix3 D_v_b; TangentVector v = Logmap(b, (H1 || H2) ? &D_v_b : 0); @@ -155,7 +155,7 @@ struct traits { } static Q Retract(const Q& g, const TangentVector& v, - ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { + ChartJacobian H1 = {}, ChartJacobian H2 = {}) { Matrix3 D_h_v; Q b = Expmap(v,H2 ? &D_h_v : 0); Q h = Compose(g, b, H1, H2); diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index 2e9e91cd8..fbf2d82e4 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -80,7 +80,7 @@ namespace gtsam { * @return 2D rotation \f$ \in SO(2) \f$ */ static Rot2 relativeBearing(const Point2& d, OptionalJacobian<1,2> H = - boost::none); + {}); /** Named constructor that behaves as atan2, i.e., y,x order (!) and normalizes */ static Rot2 atan2(double y, double x); @@ -123,10 +123,10 @@ namespace gtsam { /// @{ /// Exponential map at identity - create a rotation from canonical coordinates - static Rot2 Expmap(const Vector1& v, ChartJacobian H = boost::none); + static Rot2 Expmap(const Vector1& v, ChartJacobian H = {}); /// Log map at identity - return the canonical coordinates of this rotation - static Vector1 Logmap(const Rot2& r, ChartJacobian H = boost::none); + static Vector1 Logmap(const Rot2& r, ChartJacobian H = {}); /** Calculate Adjoint map */ Matrix1 AdjointMap() const { return I_1x1; } @@ -143,10 +143,10 @@ namespace gtsam { // Chart at origin simply uses exponential map and its inverse struct ChartAtOrigin { - static Rot2 Retract(const Vector1& v, ChartJacobian H = boost::none) { + static Rot2 Retract(const Vector1& v, ChartJacobian H = {}) { return Expmap(v, H); } - static Vector1 Local(const Rot2& r, ChartJacobian H = boost::none) { + static Vector1 Local(const Rot2& r, ChartJacobian H = {}) { return Logmap(r, H); } }; @@ -160,8 +160,8 @@ namespace gtsam { /** * rotate point from rotated coordinate frame to world \f$ p^w = R_c^w p^c \f$ */ - Point2 rotate(const Point2& p, OptionalJacobian<2, 1> H1 = boost::none, - OptionalJacobian<2, 2> H2 = boost::none) const; + Point2 rotate(const Point2& p, OptionalJacobian<2, 1> H1 = {}, + OptionalJacobian<2, 2> H2 = {}) const; /** syntactic sugar for rotate */ inline Point2 operator*(const Point2& p) const { @@ -171,8 +171,8 @@ namespace gtsam { /** * rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$ */ - Point2 unrotate(const Point2& p, OptionalJacobian<2, 1> H1 = boost::none, - OptionalJacobian<2, 2> H2 = boost::none) const; + Point2 unrotate(const Point2& p, OptionalJacobian<2, 1> H1 = {}, + OptionalJacobian<2, 2> H2 = {}) const; /// @} /// @name Standard Interface diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 65f3d4e5f..00beeb515 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -152,13 +152,13 @@ class GTSAM_EXPORT Rot3 : public LieGroup { /// Rotations around Z, Y, then X axes as in http://en.wikipedia.org/wiki/Rotation_matrix, counterclockwise when looking from unchanging axis. static Rot3 RzRyRx(double x, double y, double z, - OptionalJacobian<3, 1> Hx = boost::none, - OptionalJacobian<3, 1> Hy = boost::none, - OptionalJacobian<3, 1> Hz = boost::none); + OptionalJacobian<3, 1> Hx = {}, + OptionalJacobian<3, 1> Hy = {}, + OptionalJacobian<3, 1> Hz = {}); /// Rotations around Z, Y, then X axes as in http://en.wikipedia.org/wiki/Rotation_matrix, counterclockwise when looking from unchanging axis. inline static Rot3 RzRyRx(const Vector& xyz, - OptionalJacobian<3, 3> H = boost::none) { + OptionalJacobian<3, 3> H = {}) { assert(xyz.size() == 3); Rot3 out; if (H) { @@ -194,9 +194,9 @@ class GTSAM_EXPORT Rot3 : public LieGroup { * Positive roll is to right (decreasing yaw in aircraft). */ static Rot3 Ypr(double y, double p, double r, - OptionalJacobian<3, 1> Hy = boost::none, - OptionalJacobian<3, 1> Hp = boost::none, - OptionalJacobian<3, 1> Hr = boost::none) { + OptionalJacobian<3, 1> Hy = {}, + OptionalJacobian<3, 1> Hp = {}, + OptionalJacobian<3, 1> Hr = {}) { return RzRyRx(r, p, y, Hr, Hp, Hy); } @@ -347,8 +347,8 @@ class GTSAM_EXPORT Rot3 : public LieGroup { // Cayley chart around origin struct CayleyChart { - static Rot3 Retract(const Vector3& v, OptionalJacobian<3, 3> H = boost::none); - static Vector3 Local(const Rot3& r, OptionalJacobian<3, 3> H = boost::none); + static Rot3 Retract(const Vector3& v, OptionalJacobian<3, 3> H = {}); + static Vector3 Local(const Rot3& r, OptionalJacobian<3, 3> H = {}); }; /// Retraction from R^3 to Rot3 manifold using the Cayley transform @@ -371,7 +371,7 @@ class GTSAM_EXPORT Rot3 : public LieGroup { * Exponential map at identity - create a rotation from canonical coordinates * \f$ [R_x,R_y,R_z] \f$ using Rodrigues' formula */ - static Rot3 Expmap(const Vector3& v, OptionalJacobian<3,3> H = boost::none) { + static Rot3 Expmap(const Vector3& v, OptionalJacobian<3,3> H = {}) { if(H) *H = Rot3::ExpmapDerivative(v); #ifdef GTSAM_USE_QUATERNIONS return traits::Expmap(v); @@ -384,7 +384,7 @@ class GTSAM_EXPORT Rot3 : public LieGroup { * Log map at identity - returns the canonical coordinates * \f$ [R_x,R_y,R_z] \f$ of this rotation */ - static Vector3 Logmap(const Rot3& R, OptionalJacobian<3,3> H = boost::none); + static Vector3 Logmap(const Rot3& R, OptionalJacobian<3,3> H = {}); /// Derivative of Expmap static Matrix3 ExpmapDerivative(const Vector3& x); @@ -397,8 +397,8 @@ class GTSAM_EXPORT Rot3 : public LieGroup { // Chart at origin, depends on compile-time flag ROT3_DEFAULT_COORDINATES_MODE struct ChartAtOrigin { - static Rot3 Retract(const Vector3& v, ChartJacobian H = boost::none); - static Vector3 Local(const Rot3& r, ChartJacobian H = boost::none); + static Rot3 Retract(const Vector3& v, ChartJacobian H = {}); + static Vector3 Local(const Rot3& r, ChartJacobian H = {}); }; using LieGroup::inverse; // version with derivative @@ -410,27 +410,27 @@ class GTSAM_EXPORT Rot3 : public LieGroup { /** * rotate point from rotated coordinate frame to world \f$ p^w = R_c^w p^c \f$ */ - Point3 rotate(const Point3& p, OptionalJacobian<3,3> H1 = boost::none, - OptionalJacobian<3,3> H2 = boost::none) const; + Point3 rotate(const Point3& p, OptionalJacobian<3,3> H1 = {}, + OptionalJacobian<3,3> H2 = {}) const; /// rotate point from rotated coordinate frame to world = R*p Point3 operator*(const Point3& p) const; /// rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$ - Point3 unrotate(const Point3& p, OptionalJacobian<3,3> H1 = boost::none, - OptionalJacobian<3,3> H2=boost::none) const; + Point3 unrotate(const Point3& p, OptionalJacobian<3,3> H1 = {}, + OptionalJacobian<3,3> H2={}) const; /// @} /// @name Group Action on Unit3 /// @{ /// rotate 3D direction from rotated coordinate frame to world frame - Unit3 rotate(const Unit3& p, OptionalJacobian<2,3> HR = boost::none, - OptionalJacobian<2,2> Hp = boost::none) const; + Unit3 rotate(const Unit3& p, OptionalJacobian<2,3> HR = {}, + OptionalJacobian<2,2> Hp = {}) const; /// unrotate 3D direction from world frame to rotated coordinate frame - Unit3 unrotate(const Unit3& p, OptionalJacobian<2,3> HR = boost::none, - OptionalJacobian<2,2> Hp = boost::none) const; + Unit3 unrotate(const Unit3& p, OptionalJacobian<2,3> HR = {}, + OptionalJacobian<2,2> Hp = {}) const; /// rotate 3D direction from rotated coordinate frame to world frame Unit3 operator*(const Unit3& p) const; @@ -458,19 +458,19 @@ class GTSAM_EXPORT Rot3 : public LieGroup { * Use RQ to calculate xyz angle representation * @return a vector containing x,y,z s.t. R = Rot3::RzRyRx(x,y,z) */ - Vector3 xyz(OptionalJacobian<3, 3> H = boost::none) const; + Vector3 xyz(OptionalJacobian<3, 3> H = {}) const; /** * Use RQ to calculate yaw-pitch-roll angle representation * @return a vector containing ypr s.t. R = Rot3::Ypr(y,p,r) */ - Vector3 ypr(OptionalJacobian<3, 3> H = boost::none) const; + Vector3 ypr(OptionalJacobian<3, 3> H = {}) const; /** * Use RQ to calculate roll-pitch-yaw angle representation * @return a vector containing rpy s.t. R = Rot3::Ypr(y,p,r) */ - Vector3 rpy(OptionalJacobian<3, 3> H = boost::none) const; + Vector3 rpy(OptionalJacobian<3, 3> H = {}) const; /** * Accessor to get to component of angle representations @@ -478,7 +478,7 @@ class GTSAM_EXPORT Rot3 : public LieGroup { * you should instead use xyz() or ypr() * TODO: make this more efficient */ - double roll(OptionalJacobian<1, 3> H = boost::none) const; + double roll(OptionalJacobian<1, 3> H = {}) const; /** * Accessor to get to component of angle representations @@ -486,7 +486,7 @@ class GTSAM_EXPORT Rot3 : public LieGroup { * you should instead use xyz() or ypr() * TODO: make this more efficient */ - double pitch(OptionalJacobian<1, 3> H = boost::none) const; + double pitch(OptionalJacobian<1, 3> H = {}) const; /** * Accessor to get to component of angle representations @@ -494,7 +494,7 @@ class GTSAM_EXPORT Rot3 : public LieGroup { * you should instead use xyz() or ypr() * TODO: make this more efficient */ - double yaw(OptionalJacobian<1, 3> H = boost::none) const; + double yaw(OptionalJacobian<1, 3> H = {}) const; /// @} /// @name Advanced Interface @@ -583,7 +583,7 @@ class GTSAM_EXPORT Rot3 : public LieGroup { * @return a vector [thetax, thetay, thetaz] in radians. */ GTSAM_EXPORT std::pair RQ( - const Matrix3& A, OptionalJacobian<3, 9> H = boost::none); + const Matrix3& A, OptionalJacobian<3, 9> H = {}); template<> struct traits : public internal::LieGroup {}; diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h index 0463bc2e8..6f78bed4e 100644 --- a/gtsam/geometry/SO3.h +++ b/gtsam/geometry/SO3.h @@ -121,7 +121,7 @@ namespace so3 { * We only provide the 9*9 derivative in the first argument M. */ GTSAM_EXPORT Matrix3 compose(const Matrix3& M, const SO3& R, - OptionalJacobian<9, 9> H = boost::none); + OptionalJacobian<9, 9> H = {}); /// (constant) Jacobian of compose wrpt M GTSAM_EXPORT Matrix99 Dcompose(const SO3& R); @@ -170,13 +170,13 @@ class DexpFunctor : public ExpmapFunctor { const Matrix3& dexp() const { return dexp_; } /// Multiplies with dexp(), with optional derivatives - GTSAM_EXPORT Vector3 applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1 = boost::none, - OptionalJacobian<3, 3> H2 = boost::none) const; + GTSAM_EXPORT Vector3 applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1 = {}, + OptionalJacobian<3, 3> H2 = {}) const; /// Multiplies with dexp().inverse(), with optional derivatives GTSAM_EXPORT Vector3 applyInvDexp(const Vector3& v, - OptionalJacobian<3, 3> H1 = boost::none, - OptionalJacobian<3, 3> H2 = boost::none) const; + OptionalJacobian<3, 3> H1 = {}, + OptionalJacobian<3, 3> H2 = {}) const; }; } // namespace so3 diff --git a/gtsam/geometry/SO4.h b/gtsam/geometry/SO4.h index 7503b012b..529935006 100644 --- a/gtsam/geometry/SO4.h +++ b/gtsam/geometry/SO4.h @@ -70,13 +70,13 @@ Vector6 SO4::ChartAtOrigin::Local(const SO4 &Q, ChartJacobian H); /** * Project to top-left 3*3 matrix. Note this is *not* in general \in SO(3). */ -GTSAM_EXPORT Matrix3 topLeft(const SO4 &Q, OptionalJacobian<9, 6> H = boost::none); +GTSAM_EXPORT Matrix3 topLeft(const SO4 &Q, OptionalJacobian<9, 6> H = {}); /** * Project to Stiefel manifold of 4*3 orthonormal 3-frames in R^4, i.e., pi(Q) * -> \f$ S \in St(3,4) \f$. */ -GTSAM_EXPORT Matrix43 stiefel(const SO4 &Q, OptionalJacobian<12, 6> H = boost::none); +GTSAM_EXPORT Matrix43 stiefel(const SO4 &Q, OptionalJacobian<12, 6> H = {}); /** Serialization function */ template diff --git a/gtsam/geometry/SOn.h b/gtsam/geometry/SOn.h index 2c8261668..b8b9deb3b 100644 --- a/gtsam/geometry/SOn.h +++ b/gtsam/geometry/SOn.h @@ -242,12 +242,12 @@ class SO : public LieGroup, internal::DimensionSO(N)> { * Retract uses Cayley map. See note about xi element order in Hat. * Deafault implementation has no Jacobian implemented */ - static SO Retract(const TangentVector& xi, ChartJacobian H = boost::none); + static SO Retract(const TangentVector& xi, ChartJacobian H = {}); /** * Inverse of Retract. See note about xi element order in Hat. */ - static TangentVector Local(const SO& R, ChartJacobian H = boost::none); + static TangentVector Local(const SO& R, ChartJacobian H = {}); }; // Return dynamic identity DxD Jacobian for given SO(n) @@ -267,7 +267,7 @@ class SO : public LieGroup, internal::DimensionSO(N)> { /** * Exponential map at identity - create a rotation from canonical coordinates */ - static SO Expmap(const TangentVector& omega, ChartJacobian H = boost::none); + static SO Expmap(const TangentVector& omega, ChartJacobian H = {}); /// Derivative of Expmap, currently only defined for SO3 static MatrixDD ExpmapDerivative(const TangentVector& omega); @@ -275,7 +275,7 @@ class SO : public LieGroup, internal::DimensionSO(N)> { /** * Log map at identity - returns the canonical coordinates of this rotation */ - static TangentVector Logmap(const SO& R, ChartJacobian H = boost::none); + static TangentVector Logmap(const SO& R, ChartJacobian H = {}); /// Derivative of Logmap, currently only defined for SO3 static MatrixDD LogmapDerivative(const TangentVector& omega); @@ -293,7 +293,7 @@ class SO : public LieGroup, internal::DimensionSO(N)> { * X and fixed-size Jacobian if dimension is known at compile time. * */ VectorN2 vec(OptionalJacobian H = - boost::none) const; + {}) const; /// Calculate N^2 x dim matrix of vectorized Lie algebra generators for SO(N) template > diff --git a/gtsam/geometry/Similarity2.h b/gtsam/geometry/Similarity2.h index ad4b82e09..47281b383 100644 --- a/gtsam/geometry/Similarity2.h +++ b/gtsam/geometry/Similarity2.h @@ -143,20 +143,20 @@ class GTSAM_EXPORT Similarity2 : public LieGroup { * \f$ [t_x, t_y, \delta, \lambda] \f$ */ static Vector4 Logmap(const Similarity2& S, // - OptionalJacobian<4, 4> Hm = boost::none); + OptionalJacobian<4, 4> Hm = {}); /// Exponential map at the identity static Similarity2 Expmap(const Vector4& v, // - OptionalJacobian<4, 4> Hm = boost::none); + OptionalJacobian<4, 4> Hm = {}); /// Chart at the origin struct ChartAtOrigin { static Similarity2 Retract(const Vector4& v, - ChartJacobian H = boost::none) { + ChartJacobian H = {}) { return Similarity2::Expmap(v, H); } static Vector4 Local(const Similarity2& other, - ChartJacobian H = boost::none) { + ChartJacobian H = {}) { return Similarity2::Logmap(other, H); } }; diff --git a/gtsam/geometry/Similarity3.h b/gtsam/geometry/Similarity3.h index 41d9d9965..aa0f3a62a 100644 --- a/gtsam/geometry/Similarity3.h +++ b/gtsam/geometry/Similarity3.h @@ -98,8 +98,8 @@ class GTSAM_EXPORT Similarity3 : public LieGroup { /// Action on a point p is s*(R*p+t) Point3 transformFrom(const Point3& p, // - OptionalJacobian<3, 7> H1 = boost::none, // - OptionalJacobian<3, 3> H2 = boost::none) const; + OptionalJacobian<3, 7> H1 = {}, // + OptionalJacobian<3, 3> H2 = {}) const; /** * Action on a pose T. @@ -142,21 +142,21 @@ class GTSAM_EXPORT Similarity3 : public LieGroup { * \f$ [R_x,R_y,R_z, t_x, t_y, t_z, \lambda] \f$ */ static Vector7 Logmap(const Similarity3& s, // - OptionalJacobian<7, 7> Hm = boost::none); + OptionalJacobian<7, 7> Hm = {}); /** Exponential map at the identity */ static Similarity3 Expmap(const Vector7& v, // - OptionalJacobian<7, 7> Hm = boost::none); + OptionalJacobian<7, 7> Hm = {}); /// Chart at the origin struct ChartAtOrigin { static Similarity3 Retract(const Vector7& v, - ChartJacobian H = boost::none) { + ChartJacobian H = {}) { return Similarity3::Expmap(v, H); } static Vector7 Local(const Similarity3& other, - ChartJacobian H = boost::none) { + ChartJacobian H = {}) { return Similarity3::Logmap(other, H); } }; diff --git a/gtsam/geometry/SphericalCamera.cpp b/gtsam/geometry/SphericalCamera.cpp index 58a29dc09..5c6a2b3a4 100644 --- a/gtsam/geometry/SphericalCamera.cpp +++ b/gtsam/geometry/SphericalCamera.cpp @@ -96,7 +96,7 @@ Vector2 SphericalCamera::reprojectionError( Matrix23 H_project_point; Matrix22 H_error; Unit3 projected = project2(point, H_project_pose, H_project_point); - Vector2 error = measured.errorVector(projected, boost::none, H_error); + Vector2 error = measured.errorVector(projected, {}, H_error); if (Dpose) *Dpose = H_error * H_project_pose; if (Dpoint) *Dpoint = H_error * H_project_point; return error; diff --git a/gtsam/geometry/SphericalCamera.h b/gtsam/geometry/SphericalCamera.h index 53b67241f..dd3bc7420 100644 --- a/gtsam/geometry/SphericalCamera.h +++ b/gtsam/geometry/SphericalCamera.h @@ -152,16 +152,16 @@ class GTSAM_EXPORT SphericalCamera { * @param point 3D point in world coordinates * @return the intrinsic coordinates of the projected point */ - Unit3 project2(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none, - OptionalJacobian<2, 3> Dpoint = boost::none) const; + Unit3 project2(const Point3& pw, OptionalJacobian<2, 6> Dpose = {}, + OptionalJacobian<2, 3> Dpoint = {}) const; /** Project point into the image * (note: there is no CheiralityException for a spherical camera) * @param point 3D direction in world coordinates * @return the intrinsic coordinates of the projected point */ - Unit3 project2(const Unit3& pwu, OptionalJacobian<2, 6> Dpose = boost::none, - OptionalJacobian<2, 2> Dpoint = boost::none) const; + Unit3 project2(const Unit3& pwu, OptionalJacobian<2, 6> Dpose = {}, + OptionalJacobian<2, 2> Dpoint = {}) const; /// backproject a 2-dimensional point to a 3-dimensional point at given depth Point3 backproject(const Unit3& p, const double depth) const; @@ -174,16 +174,16 @@ class GTSAM_EXPORT SphericalCamera { * @param point 3D point in world coordinates * @return the intrinsic coordinates of the projected point */ - Unit3 project(const Point3& point, OptionalJacobian<2, 6> Dpose = boost::none, - OptionalJacobian<2, 3> Dpoint = boost::none) const; + Unit3 project(const Point3& point, OptionalJacobian<2, 6> Dpose = {}, + OptionalJacobian<2, 3> Dpoint = {}) const; /** Compute reprojection error for a given 3D point in world coordinates * @param point 3D point in world coordinates * @return the tangent space error between the projection and the measurement */ Vector2 reprojectionError(const Point3& point, const Unit3& measured, - OptionalJacobian<2, 6> Dpose = boost::none, - OptionalJacobian<2, 3> Dpoint = boost::none) const; + OptionalJacobian<2, 6> Dpose = {}, + OptionalJacobian<2, 3> Dpoint = {}) const; /// @} /// move a cameras according to d diff --git a/gtsam/geometry/StereoCamera.h b/gtsam/geometry/StereoCamera.h index 603476278..6ccebd8c2 100644 --- a/gtsam/geometry/StereoCamera.h +++ b/gtsam/geometry/StereoCamera.h @@ -143,7 +143,7 @@ public: * @param H2 derivative with respect to point */ StereoPoint2 project2(const Point3& point, OptionalJacobian<3, 6> H1 = - boost::none, OptionalJacobian<3, 3> H2 = boost::none) const; + {}, OptionalJacobian<3, 3> H2 = {}) const; /// back-project a measurement Point3 backproject(const StereoPoint2& z) const; @@ -153,8 +153,8 @@ public: * @param H2 derivative with respect to point */ Point3 backproject2(const StereoPoint2& z, - OptionalJacobian<3, 6> H1 = boost::none, - OptionalJacobian<3, 3> H2 = boost::none) const; + OptionalJacobian<3, 6> H1 = {}, + OptionalJacobian<3, 3> H2 = {}) const; /// @} /// @name Deprecated @@ -167,8 +167,8 @@ public: * @param H3 IGNORED (for calibration) */ StereoPoint2 project(const Point3& point, OptionalJacobian<3, 6> H1, - OptionalJacobian<3, 3> H2 = boost::none, OptionalJacobian<3, 0> H3 = - boost::none) const; + OptionalJacobian<3, 3> H2 = {}, OptionalJacobian<3, 0> H3 = + {}) const; /// for Nonlinear Triangulation Vector defaultErrorWhenTriangulatingBehindCamera() const { diff --git a/gtsam/geometry/tests/testCal3DS2.cpp b/gtsam/geometry/tests/testCal3DS2.cpp index 7ef6e5001..e9ee39836 100644 --- a/gtsam/geometry/tests/testCal3DS2.cpp +++ b/gtsam/geometry/tests/testCal3DS2.cpp @@ -57,7 +57,7 @@ Point2 uncalibrate_(const Cal3DS2& k, const Point2& pt) { /* ************************************************************************* */ TEST(Cal3DS2, Duncalibrate1) { Matrix computed; - K.uncalibrate(p, computed, boost::none); + K.uncalibrate(p, computed, {}); Matrix numerical = numericalDerivative21(uncalibrate_, K, p, 1e-7); CHECK(assert_equal(numerical, computed, 1e-5)); Matrix separate = K.D2d_calibration(p); @@ -67,7 +67,7 @@ TEST(Cal3DS2, Duncalibrate1) { /* ************************************************************************* */ TEST(Cal3DS2, Duncalibrate2) { Matrix computed; - K.uncalibrate(p, boost::none, computed); + K.uncalibrate(p, {}, computed); Matrix numerical = numericalDerivative22(uncalibrate_, K, p, 1e-7); CHECK(assert_equal(numerical, computed, 1e-5)); Matrix separate = K.D2d_intrinsic(p); diff --git a/gtsam/geometry/tests/testCal3Unified.cpp b/gtsam/geometry/tests/testCal3Unified.cpp index 648bb358c..0f58bd82a 100644 --- a/gtsam/geometry/tests/testCal3Unified.cpp +++ b/gtsam/geometry/tests/testCal3Unified.cpp @@ -68,7 +68,7 @@ Point2 uncalibrate_(const Cal3Unified& k, const Point2& pt) { /* ************************************************************************* */ TEST(Cal3Unified, Duncalibrate1) { Matrix computed; - K.uncalibrate(p, computed, boost::none); + K.uncalibrate(p, computed, {}); Matrix numerical = numericalDerivative21(uncalibrate_, K, p, 1e-7); CHECK(assert_equal(numerical, computed, 1e-6)); } @@ -76,7 +76,7 @@ TEST(Cal3Unified, Duncalibrate1) { /* ************************************************************************* */ TEST(Cal3Unified, Duncalibrate2) { Matrix computed; - K.uncalibrate(p, boost::none, computed); + K.uncalibrate(p, {}, computed); Matrix numerical = numericalDerivative22(uncalibrate_, K, p, 1e-7); CHECK(assert_equal(numerical, computed, 1e-6)); } diff --git a/gtsam/geometry/tests/testCal3_S2.cpp b/gtsam/geometry/tests/testCal3_S2.cpp index 41be5ea8e..68d5e238b 100644 --- a/gtsam/geometry/tests/testCal3_S2.cpp +++ b/gtsam/geometry/tests/testCal3_S2.cpp @@ -64,7 +64,7 @@ Point2 uncalibrate_(const Cal3_S2& k, const Point2& pt) { TEST(Cal3_S2, Duncalibrate1) { Matrix25 computed; - K.uncalibrate(p, computed, boost::none); + K.uncalibrate(p, computed, {}); Matrix numerical = numericalDerivative21(uncalibrate_, K, p); CHECK(assert_equal(numerical, computed, 1e-8)); } @@ -72,7 +72,7 @@ TEST(Cal3_S2, Duncalibrate1) { /* ************************************************************************* */ TEST(Cal3_S2, Duncalibrate2) { Matrix computed; - K.uncalibrate(p, boost::none, computed); + K.uncalibrate(p, {}, computed); Matrix numerical = numericalDerivative22(uncalibrate_, K, p); CHECK(assert_equal(numerical, computed, 1e-9)); } @@ -84,7 +84,7 @@ Point2 calibrate_(const Cal3_S2& k, const Point2& pt) { /* ************************************************************************* */ TEST(Cal3_S2, Dcalibrate1) { Matrix computed; - Point2 expected = K.calibrate(p_uv, computed, boost::none); + Point2 expected = K.calibrate(p_uv, computed, {}); Matrix numerical = numericalDerivative21(calibrate_, K, p_uv); CHECK(assert_equal(expected, p_xy, 1e-8)); CHECK(assert_equal(numerical, computed, 1e-8)); @@ -93,7 +93,7 @@ TEST(Cal3_S2, Dcalibrate1) { /* ************************************************************************* */ TEST(Cal3_S2, Dcalibrate2) { Matrix computed; - Point2 expected = K.calibrate(p_uv, boost::none, computed); + Point2 expected = K.calibrate(p_uv, {}, computed); Matrix numerical = numericalDerivative22(calibrate_, K, p_uv); CHECK(assert_equal(expected, p_xy, 1e-8)); CHECK(assert_equal(numerical, computed, 1e-8)); diff --git a/gtsam/geometry/tests/testCalibratedCamera.cpp b/gtsam/geometry/tests/testCalibratedCamera.cpp index 0fb0754fe..27f882ff5 100644 --- a/gtsam/geometry/tests/testCalibratedCamera.cpp +++ b/gtsam/geometry/tests/testCalibratedCamera.cpp @@ -54,7 +54,7 @@ TEST(CalibratedCamera, Create) { // Check derivative std::function f = // - std::bind(CalibratedCamera::Create, std::placeholders::_1, boost::none); + std::bind(CalibratedCamera::Create, std::placeholders::_1, nullptr); Matrix numericalH = numericalDerivative11(f, kDefaultPose); EXPECT(assert_equal(numericalH, actualH, 1e-9)); } diff --git a/gtsam/geometry/tests/testEssentialMatrix.cpp b/gtsam/geometry/tests/testEssentialMatrix.cpp index ff8c61f35..bd4568118 100644 --- a/gtsam/geometry/tests/testEssentialMatrix.cpp +++ b/gtsam/geometry/tests/testEssentialMatrix.cpp @@ -40,17 +40,21 @@ TEST(EssentialMatrix, FromRotationAndDirection) { trueE, EssentialMatrix::FromRotationAndDirection(trueRotation, trueDirection, actualH1, actualH2), 1e-8)); - Matrix expectedH1 = numericalDerivative11( - std::bind(EssentialMatrix::FromRotationAndDirection, - std::placeholders::_1, trueDirection, boost::none, boost::none), - trueRotation); - EXPECT(assert_equal(expectedH1, actualH1, 1e-7)); + { + std::function fn = [](const Rot3& R) { + return EssentialMatrix::FromRotationAndDirection(R, trueDirection); + }; + Matrix expectedH1 = numericalDerivative11(fn, trueRotation); + EXPECT(assert_equal(expectedH1, actualH1, 1e-7)); + } - Matrix expectedH2 = numericalDerivative11( - std::bind(EssentialMatrix::FromRotationAndDirection, trueRotation, - std::placeholders::_1, boost::none, boost::none), - trueDirection); - EXPECT(assert_equal(expectedH2, actualH2, 1e-7)); + { + std::function fn = [](const Unit3& t) { + return EssentialMatrix::FromRotationAndDirection(trueRotation, t); + }; + Matrix expectedH2 = numericalDerivative11(fn, trueDirection); + EXPECT(assert_equal(expectedH2, actualH2, 1e-7)); + } } //************************************************************************* @@ -174,8 +178,10 @@ TEST (EssentialMatrix, FromPose3_a) { Matrix actualH; Pose3 pose(trueRotation, trueTranslation); // Pose between two cameras EXPECT(assert_equal(trueE, EssentialMatrix::FromPose3(pose, actualH), 1e-8)); - Matrix expectedH = numericalDerivative11( - std::bind(EssentialMatrix::FromPose3, std::placeholders::_1, boost::none), pose); + std::function fn = [](const Pose3& pose) { + return EssentialMatrix::FromPose3(pose); + }; + Matrix expectedH = numericalDerivative11(fn, pose); EXPECT(assert_equal(expectedH, actualH, 1e-7)); } @@ -187,8 +193,10 @@ TEST (EssentialMatrix, FromPose3_b) { EssentialMatrix E(c1Rc2, Unit3(c1Tc2)); Pose3 pose(c1Rc2, c1Tc2); // Pose between two cameras EXPECT(assert_equal(E, EssentialMatrix::FromPose3(pose, actualH), 1e-8)); - Matrix expectedH = numericalDerivative11( - std::bind(EssentialMatrix::FromPose3, std::placeholders::_1, boost::none), pose); + std::function fn = [](const Pose3& pose) { + return EssentialMatrix::FromPose3(pose); + }; + Matrix expectedH = numericalDerivative11(fn, pose); EXPECT(assert_equal(expectedH, actualH, 1e-5)); } diff --git a/gtsam/geometry/tests/testOrientedPlane3.cpp b/gtsam/geometry/tests/testOrientedPlane3.cpp index 41b152557..f27b5070b 100644 --- a/gtsam/geometry/tests/testOrientedPlane3.cpp +++ b/gtsam/geometry/tests/testOrientedPlane3.cpp @@ -23,7 +23,6 @@ using namespace std::placeholders; using namespace gtsam; -using boost::none; GTSAM_CONCEPT_TESTABLE_INST(OrientedPlane3) GTSAM_CONCEPT_MANIFOLD_INST(OrientedPlane3) @@ -57,17 +56,17 @@ TEST(OrientedPlane3, transform) { gtsam::Point3(2.0, 3.0, 4.0)); OrientedPlane3 plane(-1, 0, 0, 5); OrientedPlane3 expectedPlane(-sqrt(2.0) / 2.0, -sqrt(2.0) / 2.0, 0.0, 3); - OrientedPlane3 transformedPlane = plane.transform(pose, none, none); + OrientedPlane3 transformedPlane = plane.transform(pose, {}, {}); EXPECT(assert_equal(expectedPlane, transformedPlane, 1e-5)); // Test the jacobians of transform Matrix actualH1, expectedH1, actualH2, expectedH2; expectedH1 = numericalDerivative21(transform_, plane, pose); - plane.transform(pose, actualH1, none); + plane.transform(pose, actualH1, {}); EXPECT(assert_equal(expectedH1, actualH1, 1e-5)); expectedH2 = numericalDerivative22(transform_, plane, pose); - plane.transform(pose, none, actualH2); + plane.transform(pose, {}, actualH2); EXPECT(assert_equal(expectedH2, actualH2, 1e-5)); } @@ -135,8 +134,9 @@ TEST(OrientedPlane3, errorVector) { EXPECT(assert_equal(plane1.distance() - plane2.distance(), actual[2])); std::function f = - std::bind(&OrientedPlane3::errorVector, std::placeholders::_1, - std::placeholders::_2, boost::none, boost::none); + [](const OrientedPlane3& p1, const OrientedPlane3& p2) { + return p1.errorVector(p2); + }; expectedH1 = numericalDerivative21(f, plane1, plane2); expectedH2 = numericalDerivative22(f, plane1, plane2); EXPECT(assert_equal(expectedH1, actualH1, 1e-5)); @@ -147,8 +147,10 @@ TEST(OrientedPlane3, errorVector) { TEST(OrientedPlane3, jacobian_retract) { OrientedPlane3 plane(-1, 0.1, 0.2, 5); Matrix33 H_actual; - std::function f = std::bind( - &OrientedPlane3::retract, plane, std::placeholders::_1, boost::none); + std::function f = [&plane](const Vector3& v) { + return plane.retract(v); + }; + { Vector3 v(-0.1, 0.2, 0.3); plane.retract(v, H_actual); @@ -168,8 +170,9 @@ TEST(OrientedPlane3, jacobian_normal) { Matrix23 H_actual, H_expected; OrientedPlane3 plane(-1, 0.1, 0.2, 5); - std::function f = std::bind( - &OrientedPlane3::normal, std::placeholders::_1, boost::none); + std::function f = [](const OrientedPlane3& p) { + return p.normal(); + }; H_expected = numericalDerivative11(f, plane); plane.normal(H_actual); @@ -181,8 +184,9 @@ TEST(OrientedPlane3, jacobian_distance) { Matrix13 H_actual, H_expected; OrientedPlane3 plane(-1, 0.1, 0.2, 5); - std::function f = std::bind( - &OrientedPlane3::distance, std::placeholders::_1, boost::none); + std::function f = [](const OrientedPlane3& p) { + return p.distance(); + }; H_expected = numericalDerivative11(f, plane); plane.distance(H_actual); diff --git a/gtsam/geometry/tests/testPinholeCamera.cpp b/gtsam/geometry/tests/testPinholeCamera.cpp index 0679a4609..6b5d93627 100644 --- a/gtsam/geometry/tests/testPinholeCamera.cpp +++ b/gtsam/geometry/tests/testPinholeCamera.cpp @@ -67,7 +67,7 @@ TEST(PinholeCamera, Create) { // Check derivative std::function f = // std::bind(Camera::Create, std::placeholders::_1, std::placeholders::_2, - boost::none, boost::none); + nullptr, nullptr); Matrix numericalH1 = numericalDerivative21(f,pose,K); EXPECT(assert_equal(numericalH1, actualH1, 1e-9)); Matrix numericalH2 = numericalDerivative22(f,pose,K); @@ -82,7 +82,7 @@ TEST(PinholeCamera, Pose) { // Check derivative std::function f = // - std::bind(&Camera::getPose, std::placeholders::_1, boost::none); + std::bind(&Camera::getPose, std::placeholders::_1, nullptr); Matrix numericalH = numericalDerivative11(f,camera); EXPECT(assert_equal(numericalH, actualH, 1e-9)); } diff --git a/gtsam/geometry/tests/testPinholePose.cpp b/gtsam/geometry/tests/testPinholePose.cpp index acfcd9f39..4fccc18a1 100644 --- a/gtsam/geometry/tests/testPinholePose.cpp +++ b/gtsam/geometry/tests/testPinholePose.cpp @@ -66,7 +66,7 @@ TEST(PinholeCamera, Pose) { // Check derivative std::function f = // - std::bind(&Camera::getPose,_1,boost::none); + std::bind(&Camera::getPose,_1,{}); Matrix numericalH = numericalDerivative11(f,camera); EXPECT(assert_equal(numericalH, actualH, 1e-9)); } diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp index e373e1d52..ad9f29620 100644 --- a/gtsam/geometry/tests/testPoint3.cpp +++ b/gtsam/geometry/tests/testPoint3.cpp @@ -121,9 +121,8 @@ TEST( Point3, dot) { /* ************************************************************************* */ TEST(Point3, cross) { Matrix aH1, aH2; - std::function f = - std::bind(>sam::cross, std::placeholders::_1, std::placeholders::_2, - boost::none, boost::none); + std::function f = + [](const Point3& p, const Point3& q) { return gtsam::cross(p, q); }; const Point3 omega(0, 1, 0), theta(4, 6, 8); cross(omega, theta, aH1, aH2); EXPECT(assert_equal(numericalDerivative21(f, omega, theta), aH1)); @@ -142,8 +141,7 @@ TEST( Point3, cross2) { // Use numerical derivatives to calculate the expected Jacobians Matrix H1, H2; std::function f = - std::bind(>sam::cross, std::placeholders::_1, std::placeholders::_2, // - boost::none, boost::none); + [](const Point3& p, const Point3& q) { return gtsam::cross(p, q); }; { gtsam::cross(p, q, H1, H2); EXPECT(assert_equal(numericalDerivative21(f, p, q), H1, 1e-9)); @@ -162,8 +160,8 @@ TEST (Point3, normalize) { Point3 point(1, -2, 3); // arbitrary point Point3 expected(point / sqrt(14.0)); EXPECT(assert_equal(expected, normalize(point, actualH), 1e-8)); - Matrix expectedH = numericalDerivative11( - std::bind(gtsam::normalize, std::placeholders::_1, boost::none), point); + std::function fn = [](const Point3& p) { return normalize(p); }; + Matrix expectedH = numericalDerivative11(fn, point); EXPECT(assert_equal(expectedH, actualH, 1e-8)); } diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index 34ce3932d..e38bfbd75 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -228,7 +228,7 @@ TEST( Pose2, ExpmapDerivative1) { Vector3 w(0.1, 0.27, -0.3); Pose2::Expmap(w,actualH); Matrix3 expectedH = numericalDerivative21 >(&Pose2::Expmap, w, boost::none, 1e-2); + OptionalJacobian<3, 3> >(&Pose2::Expmap, w, {}, 1e-2); EXPECT(assert_equal(expectedH, actualH, 1e-5)); } @@ -238,7 +238,7 @@ TEST( Pose2, ExpmapDerivative2) { Vector3 w0(0.1, 0.27, 0.0); // alpha = 0 Pose2::Expmap(w0,actualH); Matrix3 expectedH = numericalDerivative21 >(&Pose2::Expmap, w0, boost::none, 1e-2); + OptionalJacobian<3, 3> >(&Pose2::Expmap, w0, {}, 1e-2); EXPECT(assert_equal(expectedH, actualH, 1e-5)); } @@ -249,7 +249,7 @@ TEST( Pose2, LogmapDerivative1) { Pose2 p = Pose2::Expmap(w); EXPECT(assert_equal(w, Pose2::Logmap(p,actualH), 1e-5)); Matrix3 expectedH = numericalDerivative21 >(&Pose2::Logmap, p, boost::none, 1e-2); + OptionalJacobian<3, 3> >(&Pose2::Logmap, p, {}, 1e-2); EXPECT(assert_equal(expectedH, actualH, 1e-5)); } @@ -260,7 +260,7 @@ TEST( Pose2, LogmapDerivative2) { Pose2 p = Pose2::Expmap(w0); EXPECT(assert_equal(w0, Pose2::Logmap(p,actualH), 1e-5)); Matrix3 expectedH = numericalDerivative21 >(&Pose2::Logmap, p, boost::none, 1e-2); + OptionalJacobian<3, 3> >(&Pose2::Logmap, p, {}, 1e-2); EXPECT(assert_equal(expectedH, actualH, 1e-5)); } diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 1df342d57..0e7967715 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -360,7 +360,7 @@ TEST( Rot3, rotate_derivatives) { Matrix actualDrotate1a, actualDrotate1b, actualDrotate2; R.rotate(P, actualDrotate1a, actualDrotate2); - R.inverse().rotate(P, actualDrotate1b, boost::none); + R.inverse().rotate(P, actualDrotate1b, {}); Matrix numerical1 = numericalDerivative21(testing::rotate, R, P); Matrix numerical2 = numericalDerivative21(testing::rotate, R.inverse(), P); Matrix numerical3 = numericalDerivative22(testing::rotate, R, P); diff --git a/gtsam/geometry/tests/testRot3Q.cpp b/gtsam/geometry/tests/testRot3Q.cpp index be159ed58..bd2ad4562 100644 --- a/gtsam/geometry/tests/testRot3Q.cpp +++ b/gtsam/geometry/tests/testRot3Q.cpp @@ -30,7 +30,7 @@ using namespace gtsam; //****************************************************************************** TEST(Rot3Q , Compare) { - using boost::none; + using {}; // We set up expected values with quaternions typedef Quaternion Q; diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp index 96c1cce32..1afe9ff21 100644 --- a/gtsam/geometry/tests/testSO3.cpp +++ b/gtsam/geometry/tests/testSO3.cpp @@ -210,7 +210,7 @@ TEST(SO3, ExpmapDerivative) { TEST(SO3, ExpmapDerivative2) { const Vector3 theta(0.1, 0, 0.1); const Matrix Jexpected = numericalDerivative11( - std::bind(&SO3::Expmap, std::placeholders::_1, boost::none), theta); + std::bind(&SO3::Expmap, std::placeholders::_1, nullptr), theta); CHECK(assert_equal(Jexpected, SO3::ExpmapDerivative(theta))); CHECK(assert_equal(Matrix3(Jexpected.transpose()), @@ -221,7 +221,7 @@ TEST(SO3, ExpmapDerivative2) { TEST(SO3, ExpmapDerivative3) { const Vector3 theta(10, 20, 30); const Matrix Jexpected = numericalDerivative11( - std::bind(&SO3::Expmap, std::placeholders::_1, boost::none), theta); + std::bind(&SO3::Expmap, std::placeholders::_1, nullptr), theta); CHECK(assert_equal(Jexpected, SO3::ExpmapDerivative(theta))); CHECK(assert_equal(Matrix3(Jexpected.transpose()), @@ -276,7 +276,7 @@ TEST(SO3, ExpmapDerivative5) { TEST(SO3, ExpmapDerivative6) { const Vector3 thetahat(0.1, 0, 0.1); const Matrix Jexpected = numericalDerivative11( - std::bind(&SO3::Expmap, std::placeholders::_1, boost::none), thetahat); + std::bind(&SO3::Expmap, std::placeholders::_1, nullptr), thetahat); Matrix3 Jactual; SO3::Expmap(thetahat, Jactual); EXPECT(assert_equal(Jexpected, Jactual)); @@ -287,7 +287,7 @@ TEST(SO3, LogmapDerivative) { const Vector3 thetahat(0.1, 0, 0.1); const SO3 R = SO3::Expmap(thetahat); // some rotation const Matrix Jexpected = numericalDerivative11( - std::bind(&SO3::Logmap, std::placeholders::_1, boost::none), R); + std::bind(&SO3::Logmap, std::placeholders::_1, nullptr), R); const Matrix3 Jactual = SO3::LogmapDerivative(thetahat); EXPECT(assert_equal(Jexpected, Jactual)); } @@ -297,7 +297,7 @@ TEST(SO3, JacobianLogmap) { const Vector3 thetahat(0.1, 0, 0.1); const SO3 R = SO3::Expmap(thetahat); // some rotation const Matrix Jexpected = numericalDerivative11( - std::bind(&SO3::Logmap, std::placeholders::_1, boost::none), R); + std::bind(&SO3::Logmap, std::placeholders::_1, nullptr), R); Matrix3 Jactual; SO3::Logmap(R, Jactual); EXPECT(assert_equal(Jexpected, Jactual)); diff --git a/gtsam/geometry/tests/testSimpleCamera.cpp b/gtsam/geometry/tests/testSimpleCamera.cpp index 173ccf05b..f4f69a137 100644 --- a/gtsam/geometry/tests/testSimpleCamera.cpp +++ b/gtsam/geometry/tests/testSimpleCamera.cpp @@ -123,7 +123,7 @@ static Point2 project2(const Pose3& pose, const Point3& point) { TEST( SimpleCamera, Dproject_point_pose) { Matrix Dpose, Dpoint; - Point2 result = camera.project(point1, Dpose, Dpoint, boost::none); + Point2 result = camera.project(point1, Dpose, Dpoint, {}); Matrix numerical_pose = numericalDerivative21(project2, pose1, point1); Matrix numerical_point = numericalDerivative22(project2, pose1, point1); CHECK(assert_equal(result, Point2(-100, 100) )); diff --git a/gtsam/geometry/tests/testStereoCamera.cpp b/gtsam/geometry/tests/testStereoCamera.cpp index bc432cad3..d23b3c740 100644 --- a/gtsam/geometry/tests/testStereoCamera.cpp +++ b/gtsam/geometry/tests/testStereoCamera.cpp @@ -96,11 +96,11 @@ static StereoPoint2 project3(const Pose3& pose, const Point3& point, const Cal3_ TEST( StereoCamera, Dproject) { Matrix expected1 = numericalDerivative31(project3, camPose, landmark, *K); - Matrix actual1; stereoCam.project2(landmark, actual1, boost::none); + Matrix actual1; stereoCam.project2(landmark, actual1, {}); CHECK(assert_equal(expected1,actual1,1e-7)); Matrix expected2 = numericalDerivative32(project3, camPose, landmark, *K); - Matrix actual2; stereoCam.project2(landmark, boost::none, actual2); + Matrix actual2; stereoCam.project2(landmark, {}, actual2); CHECK(assert_equal(expected2,actual2,1e-7)); } diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp index 96a5522fb..66801971f 100644 --- a/gtsam/geometry/tests/testUnit3.cpp +++ b/gtsam/geometry/tests/testUnit3.cpp @@ -74,12 +74,12 @@ TEST(Unit3, rotate) { // Use numerical derivatives to calculate the expected Jacobian { expectedH = numericalDerivative21(rotate_, R, p); - R.rotate(p, actualH, boost::none); + R.rotate(p, actualH, nullptr); EXPECT(assert_equal(expectedH, actualH, 1e-5)); } { expectedH = numericalDerivative22(rotate_, R, p); - R.rotate(p, boost::none, actualH); + R.rotate(p, nullptr, actualH); EXPECT(assert_equal(expectedH, actualH, 1e-5)); } } @@ -100,12 +100,12 @@ TEST(Unit3, unrotate) { // Use numerical derivatives to calculate the expected Jacobian { expectedH = numericalDerivative21(unrotate_, R, p); - R.unrotate(p, actualH, boost::none); + R.unrotate(p, actualH, nullptr); EXPECT(assert_equal(expectedH, actualH, 1e-5)); } { expectedH = numericalDerivative22(unrotate_, R, p); - R.unrotate(p, boost::none, actualH); + R.unrotate(p, nullptr, actualH); EXPECT(assert_equal(expectedH, actualH, 1e-5)); } } @@ -124,7 +124,7 @@ TEST(Unit3, dot) { Matrix H1, H2; std::function f = std::bind(&Unit3::dot, std::placeholders::_1, std::placeholders::_2, // - boost::none, boost::none); + nullptr, nullptr); { p.dot(q, H1, H2); EXPECT(assert_equal(numericalDerivative21(f, p, q), H1, 1e-5)); @@ -154,13 +154,13 @@ TEST(Unit3, error) { // Use numerical derivatives to calculate the expected Jacobian { expected = numericalDerivative11( - std::bind(&Unit3::error, &p, std::placeholders::_1, boost::none), q); + std::bind(&Unit3::error, &p, std::placeholders::_1, nullptr), q); p.error(q, actual); EXPECT(assert_equal(expected.transpose(), actual, 1e-5)); } { expected = numericalDerivative11( - std::bind(&Unit3::error, &p, std::placeholders::_1, boost::none), r); + std::bind(&Unit3::error, &p, std::placeholders::_1, nullptr), r); p.error(r, actual); EXPECT(assert_equal(expected.transpose(), actual, 1e-5)); } @@ -182,33 +182,33 @@ TEST(Unit3, error2) { { expected = numericalDerivative21( std::bind(&Unit3::errorVector, std::placeholders::_1, - std::placeholders::_2, boost::none, boost::none), + std::placeholders::_2, nullptr, nullptr), p, q); - p.errorVector(q, actual, boost::none); + p.errorVector(q, actual, nullptr); EXPECT(assert_equal(expected, actual, 1e-5)); } { expected = numericalDerivative21( std::bind(&Unit3::errorVector, std::placeholders::_1, - std::placeholders::_2, boost::none, boost::none), + std::placeholders::_2, nullptr, nullptr), p, r); - p.errorVector(r, actual, boost::none); + p.errorVector(r, actual, nullptr); EXPECT(assert_equal(expected, actual, 1e-5)); } { expected = numericalDerivative22( std::bind(&Unit3::errorVector, std::placeholders::_1, - std::placeholders::_2, boost::none, boost::none), + std::placeholders::_2, nullptr, nullptr), p, q); - p.errorVector(q, boost::none, actual); + p.errorVector(q, nullptr, actual); EXPECT(assert_equal(expected, actual, 1e-5)); } { expected = numericalDerivative22( std::bind(&Unit3::errorVector, std::placeholders::_1, - std::placeholders::_2, boost::none, boost::none), + std::placeholders::_2, nullptr, nullptr), p, r); - p.errorVector(r, boost::none, actual); + p.errorVector(r, nullptr, actual); EXPECT(assert_equal(expected, actual, 1e-5)); } } @@ -225,13 +225,13 @@ TEST(Unit3, distance) { // Use numerical derivatives to calculate the expected Jacobian { expected = numericalGradient( - std::bind(&Unit3::distance, &p, std::placeholders::_1, boost::none), q); + std::bind(&Unit3::distance, &p, std::placeholders::_1, nullptr), q); p.distance(q, actual); EXPECT(assert_equal(expected.transpose(), actual, 1e-5)); } { expected = numericalGradient( - std::bind(&Unit3::distance, &p, std::placeholders::_1, boost::none), r); + std::bind(&Unit3::distance, &p, std::placeholders::_1, nullptr), r); p.distance(r, actual); EXPECT(assert_equal(expected.transpose(), actual, 1e-5)); } @@ -323,7 +323,7 @@ TEST(Unit3, basis) { Matrix62 actualH; Matrix62 expectedH = numericalDerivative11( - std::bind(BasisTest, std::placeholders::_1, boost::none), p); + std::bind(BasisTest, std::placeholders::_1, nullptr), p); // without H, first time EXPECT(assert_equal(expected, p.basis(), 1e-6)); @@ -352,7 +352,7 @@ TEST(Unit3, basis_derivatives) { p.basis(actualH); Matrix62 expectedH = numericalDerivative11( - std::bind(BasisTest, std::placeholders::_1, boost::none), p); + std::bind(BasisTest, std::placeholders::_1, nullptr), p); EXPECT(assert_equal(expectedH, actualH, 1e-5)); } } @@ -381,7 +381,7 @@ TEST (Unit3, jacobian_retract) { Matrix22 H; Unit3 p; std::function f = - std::bind(&Unit3::retract, p, std::placeholders::_1, boost::none); + std::bind(&Unit3::retract, p, std::placeholders::_1, nullptr); { Vector2 v (-0.2, 0.1); p.retract(v, H); @@ -444,7 +444,7 @@ TEST (Unit3, FromPoint3) { Unit3 expected(point); EXPECT(assert_equal(expected, Unit3::FromPoint3(point, actualH), 1e-5)); Matrix expectedH = numericalDerivative11( - std::bind(Unit3::FromPoint3, std::placeholders::_1, boost::none), point); + std::bind(Unit3::FromPoint3, std::placeholders::_1, nullptr), point); EXPECT(assert_equal(expectedH, actualH, 1e-5)); } diff --git a/gtsam/inference/EliminateableFactorGraph.h b/gtsam/inference/EliminateableFactorGraph.h index 1ee990807..9aaab8278 100644 --- a/gtsam/inference/EliminateableFactorGraph.h +++ b/gtsam/inference/EliminateableFactorGraph.h @@ -19,6 +19,7 @@ #pragma once #include +#include #include #include #include @@ -326,7 +327,7 @@ namespace gtsam { /** @deprecated */ boost::shared_ptr GTSAM_DEPRECATED marginalMultifrontalBayesNet( boost::variant variables, - boost::none_t, + std::nullptr_t, const Eliminate& function = EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex = {}) const { return marginalMultifrontalBayesNet(variables, function, variableIndex); @@ -335,7 +336,7 @@ namespace gtsam { /** @deprecated */ boost::shared_ptr GTSAM_DEPRECATED marginalMultifrontalBayesTree( boost::variant variables, - boost::none_t, + std::nullptr_t, const Eliminate& function = EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex = {}) const { return marginalMultifrontalBayesTree(variables, function, variableIndex); diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index 8b268558a..4f3a89226 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -21,6 +21,7 @@ #pragma once +#include #include #include #include // Included here instead of fw-declared so we can use Errors::iterator @@ -414,7 +415,7 @@ namespace gtsam { #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 /** @deprecated */ VectorValues GTSAM_DEPRECATED - optimize(boost::none_t, const Eliminate& function = + optimize(std::nullptr_t, const Eliminate& function = EliminationTraitsType::DefaultEliminate) const { return optimize(function); } diff --git a/gtsam/navigation/AHRSFactor.h b/gtsam/navigation/AHRSFactor.h index 576874555..e99fa7ae5 100644 --- a/gtsam/navigation/AHRSFactor.h +++ b/gtsam/navigation/AHRSFactor.h @@ -103,7 +103,7 @@ class GTSAM_EXPORT PreintegratedAhrsMeasurements : public PreintegratedRotation /// Predict bias-corrected incremental rotation /// TODO: The matrix Hbias is the derivative of predict? Unit-test? - Vector3 predict(const Vector3& bias, OptionalJacobian<3,3> H = boost::none) const; + Vector3 predict(const Vector3& bias, OptionalJacobian<3,3> H = {}) const; // This function is only used for test purposes // (compare numerical derivatives wrt analytic ones) diff --git a/gtsam/navigation/AttitudeFactor.h b/gtsam/navigation/AttitudeFactor.h index e353def6a..4dc5ecc0a 100644 --- a/gtsam/navigation/AttitudeFactor.h +++ b/gtsam/navigation/AttitudeFactor.h @@ -54,7 +54,7 @@ public: /** vector of errors */ Vector attitudeError(const Rot3& p, - OptionalJacobian<2,3> H = boost::none) const; + OptionalJacobian<2,3> H = {}) const; const Unit3& nZ() const { return nZ_; diff --git a/gtsam/navigation/ImuBias.h b/gtsam/navigation/ImuBias.h index d9080eb27..ab210a62a 100644 --- a/gtsam/navigation/ImuBias.h +++ b/gtsam/navigation/ImuBias.h @@ -72,8 +72,8 @@ public: /** Correct an accelerometer measurement using this bias model, and optionally compute Jacobians */ Vector3 correctAccelerometer(const Vector3& measurement, - OptionalJacobian<3, 6> H1 = boost::none, - OptionalJacobian<3, 3> H2 = boost::none) const { + OptionalJacobian<3, 6> H1 = {}, + OptionalJacobian<3, 3> H2 = {}) const { if (H1) (*H1) << -I_3x3, Z_3x3; if (H2) (*H2) << I_3x3; return measurement - biasAcc_; @@ -81,8 +81,8 @@ public: /** Correct a gyroscope measurement using this bias model, and optionally compute Jacobians */ Vector3 correctGyroscope(const Vector3& measurement, - OptionalJacobian<3, 6> H1 = boost::none, - OptionalJacobian<3, 3> H2 = boost::none) const { + OptionalJacobian<3, 6> H1 = {}, + OptionalJacobian<3, 3> H2 = {}) const { if (H1) (*H1) << Z_3x3, -I_3x3; if (H2) (*H2) << I_3x3; return measurement - biasGyro_; diff --git a/gtsam/navigation/MagFactor.h b/gtsam/navigation/MagFactor.h index 142d5a957..d0c62d6c4 100644 --- a/gtsam/navigation/MagFactor.h +++ b/gtsam/navigation/MagFactor.h @@ -65,7 +65,7 @@ public: static Point3 unrotate(const Rot2& R, const Point3& p, OptionalMatrixType HR = OptionalNone) { - Point3 q = Rot3::Yaw(R.theta()).unrotate(p, HR, boost::none); + Point3 q = Rot3::Yaw(R.theta()).unrotate(p, HR, {}); if (HR) { // assign to temporary first to avoid error in Win-Debug mode Matrix H = HR->col(2); diff --git a/gtsam/navigation/ManifoldPreintegration.cpp b/gtsam/navigation/ManifoldPreintegration.cpp index 165acdaf0..baddb73e3 100644 --- a/gtsam/navigation/ManifoldPreintegration.cpp +++ b/gtsam/navigation/ManifoldPreintegration.cpp @@ -113,7 +113,7 @@ Vector9 ManifoldPreintegration::biasCorrectedDelta( const imuBias::ConstantBias biasIncr = bias_i - biasHat_; Matrix3 D_correctedRij_bias; const Vector3 biasInducedOmega = delRdelBiasOmega_ * biasIncr.gyroscope(); - const Rot3 correctedRij = deltaRij().expmap(biasInducedOmega, boost::none, + const Rot3 correctedRij = deltaRij().expmap(biasInducedOmega, {}, H ? &D_correctedRij_bias : 0); if (H) D_correctedRij_bias *= delRdelBiasOmega_; diff --git a/gtsam/navigation/ManifoldPreintegration.h b/gtsam/navigation/ManifoldPreintegration.h index a290972e4..b9135e3e4 100644 --- a/gtsam/navigation/ManifoldPreintegration.h +++ b/gtsam/navigation/ManifoldPreintegration.h @@ -103,7 +103,7 @@ public: /// summarizing the preintegrated IMU measurements so far /// NOTE(frank): implementation is different in two versions Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i, - OptionalJacobian<9, 6> H = boost::none) const override; + OptionalJacobian<9, 6> H = {}) const override; /** Dummy clone for MATLAB */ virtual boost::shared_ptr clone() const { diff --git a/gtsam/navigation/TangentPreintegration.h b/gtsam/navigation/TangentPreintegration.h index 56b0a1c75..80a39e0f2 100644 --- a/gtsam/navigation/TangentPreintegration.h +++ b/gtsam/navigation/TangentPreintegration.h @@ -91,9 +91,9 @@ public: static Vector9 UpdatePreintegrated(const Vector3& a_body, const Vector3& w_body, const double dt, const Vector9& preintegrated, - OptionalJacobian<9, 9> A = boost::none, - OptionalJacobian<9, 3> B = boost::none, - OptionalJacobian<9, 3> C = boost::none); + OptionalJacobian<9, 9> A = {}, + OptionalJacobian<9, 3> B = {}, + OptionalJacobian<9, 3> C = {}); /// Update preintegrated measurements and get derivatives /// It takes measured quantities in the j frame @@ -106,13 +106,13 @@ public: /// summarizing the preintegrated IMU measurements so far /// NOTE(frank): implementation is different in two versions Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i, - OptionalJacobian<9, 6> H = boost::none) const override; + OptionalJacobian<9, 6> H = {}) const override; // Compose the two pre-integrated 9D-vectors zeta01 and zeta02, with derivatives static Vector9 Compose(const Vector9& zeta01, const Vector9& zeta12, double deltaT12, - OptionalJacobian<9, 9> H1 = boost::none, - OptionalJacobian<9, 9> H2 = boost::none); + OptionalJacobian<9, 9> H1 = {}, + OptionalJacobian<9, 9> H2 = {}); /// Merge in a different set of measurements and update bias derivatives accordingly /// The derivatives apply to the preintegrated Vector9 diff --git a/gtsam/navigation/tests/testImuBias.cpp b/gtsam/navigation/tests/testImuBias.cpp index 81a1a2ceb..f654a8ed9 100644 --- a/gtsam/navigation/tests/testImuBias.cpp +++ b/gtsam/navigation/tests/testImuBias.cpp @@ -125,7 +125,7 @@ TEST(ImuBias, Correct1) { const Vector3 measurement(1, 2, 3); std::function f = std::bind(&Bias::correctAccelerometer, std::placeholders::_1, - std::placeholders::_2, boost::none, boost::none); + std::placeholders::_2, nullptr, nullptr); bias1.correctAccelerometer(measurement, aH1, aH2); EXPECT(assert_equal(numericalDerivative21(f, bias1, measurement), aH1)); EXPECT(assert_equal(numericalDerivative22(f, bias1, measurement), aH2)); @@ -137,7 +137,7 @@ TEST(ImuBias, Correct2) { const Vector3 measurement(1, 2, 3); std::function f = std::bind(&Bias::correctGyroscope, std::placeholders::_1, - std::placeholders::_2, boost::none, boost::none); + std::placeholders::_2, nullptr, nullptr); bias1.correctGyroscope(measurement, aH1, aH2); EXPECT(assert_equal(numericalDerivative21(f, bias1, measurement), aH1)); EXPECT(assert_equal(numericalDerivative22(f, bias1, measurement), aH2)); diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 585da38b1..923d2b93f 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -149,7 +149,7 @@ TEST(ImuFactor, PreintegratedMeasurements) { std::function f = std::bind(&PreintegrationBase::computeError, actual, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, - boost::none, boost::none, boost::none); + nullptr, nullptr, nullptr); EXPECT(assert_equal(numericalDerivative31(f, x1, x2, bias), aH1, 1e-9)); EXPECT(assert_equal(numericalDerivative32(f, x1, x2, bias), aH2, 1e-9)); EXPECT(assert_equal(numericalDerivative33(f, x1, x2, bias), aH3, 1e-9)); @@ -205,7 +205,7 @@ TEST(ImuFactor, PreintegrationBaseMethods) { pim.biasCorrectedDelta(kZeroBias, actualH); Matrix expectedH = numericalDerivative11( std::bind(&PreintegrationBase::biasCorrectedDelta, pim, - std::placeholders::_1, boost::none), kZeroBias); + std::placeholders::_1, nullptr), kZeroBias); EXPECT(assert_equal(expectedH, actualH)); Matrix9 aH1; @@ -213,11 +213,11 @@ TEST(ImuFactor, PreintegrationBaseMethods) { NavState predictedState = pim.predict(state1, kZeroBias, aH1, aH2); Matrix eH1 = numericalDerivative11( std::bind(&PreintegrationBase::predict, pim, std::placeholders::_1, - kZeroBias, boost::none, boost::none), state1); + kZeroBias, nullptr, nullptr), state1); EXPECT(assert_equal(eH1, aH1)); Matrix eH2 = numericalDerivative11( std::bind(&PreintegrationBase::predict, pim, state1, - std::placeholders::_1, boost::none, boost::none), kZeroBias); + std::placeholders::_1, nullptr, nullptr), kZeroBias); EXPECT(assert_equal(eH2, aH2)); } @@ -334,7 +334,7 @@ TEST(ImuFactor, ErrorAndJacobianWithBiases) { pim.biasCorrectedDelta(bias, actualH); Matrix expectedH = numericalDerivative11( std::bind(&PreintegrationBase::biasCorrectedDelta, pim, - std::placeholders::_1, boost::none), bias); + std::placeholders::_1, nullptr), bias); EXPECT(assert_equal(expectedH, actualH)); // Create factor @@ -520,7 +520,7 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) { // Check updatedDeltaXij derivatives Matrix3 D_correctedAcc_measuredOmega = Z_3x3; pim.correctMeasurementsBySensorPose(measuredAcc, measuredOmega, - boost::none, D_correctedAcc_measuredOmega, boost::none); + nullptr, D_correctedAcc_measuredOmega, nullptr); Matrix3 expectedD = numericalDerivative11( std::bind(correctedAcc, pim, measuredAcc, std::placeholders::_1), measuredOmega, 1e-6); @@ -531,19 +531,19 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) { // TODO(frank): revive derivative tests // Matrix93 G1, G2; // Vector9 preint = -// pim.updatedDeltaXij(measuredAcc, measuredOmega, dt, boost::none, G1, G2); +// pim.updatedDeltaXij(measuredAcc, measuredOmega, dt, {}, G1, G2); // // Matrix93 expectedG1 = numericalDerivative21( // std::bind(&PreintegratedImuMeasurements::updatedDeltaXij, pim, // std::placeholders::_1, std::placeholders::_2, -// dt, boost::none, boost::none, boost::none), measuredAcc, +// dt, {}, {}, {}), measuredAcc, // measuredOmega, 1e-6); // EXPECT(assert_equal(expectedG1, G1, 1e-5)); // // Matrix93 expectedG2 = numericalDerivative22( // std::bind(&PreintegratedImuMeasurements::updatedDeltaXij, pim, // std::placeholders::_1, std::placeholders::_2, -// dt, boost::none, boost::none, boost::none), measuredAcc, +// dt, {}, {}, {}), measuredAcc, // measuredOmega, 1e-6); // EXPECT(assert_equal(expectedG2, G2, 1e-5)); @@ -658,7 +658,7 @@ TEST(ImuFactor, PredictArbitrary) { p->integrationCovariance = Z_3x3; // MonteCarlo does not sample integration noise PreintegratedImuMeasurements pim(p, biasHat); Bias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); -// EXPECT(MonteCarlo(pim, NavState(x1, v1), bias, 0.1, boost::none, measuredAcc, measuredOmega, +// EXPECT(MonteCarlo(pim, NavState(x1, v1), bias, 0.1, {}, measuredAcc, measuredOmega, // Vector3::Constant(accNoiseVar), Vector3::Constant(omegaNoiseVar), 100000)); double dt = 0.001; diff --git a/gtsam/navigation/tests/testManifoldPreintegration.cpp b/gtsam/navigation/tests/testManifoldPreintegration.cpp index 7796ccbda..ba92d8e92 100644 --- a/gtsam/navigation/tests/testManifoldPreintegration.cpp +++ b/gtsam/navigation/tests/testManifoldPreintegration.cpp @@ -101,8 +101,8 @@ TEST(ManifoldPreintegration, computeError) { const imuBias::ConstantBias&)> f = std::bind(&ManifoldPreintegration::computeError, pim, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3, boost::none, boost::none, - boost::none); + std::placeholders::_3, nullptr, nullptr, + nullptr); // NOTE(frank): tolerance of 1e-3 on H1 because approximate away from 0 EXPECT(assert_equal(numericalDerivative31(f, x1, x2, bias), aH1, 1e-9)); EXPECT(assert_equal(numericalDerivative32(f, x1, x2, bias), aH2, 1e-9)); diff --git a/gtsam/navigation/tests/testNavState.cpp b/gtsam/navigation/tests/testNavState.cpp index e7adb923d..e658639b0 100644 --- a/gtsam/navigation/tests/testNavState.cpp +++ b/gtsam/navigation/tests/testNavState.cpp @@ -41,7 +41,7 @@ static const Vector9 kZeroXi = Vector9::Zero(); TEST(NavState, Constructor) { std::function create = std::bind(&NavState::Create, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3, boost::none, boost::none, boost::none); + std::placeholders::_3, nullptr, nullptr, nullptr); Matrix aH1, aH2, aH3; EXPECT( assert_equal(kState1, @@ -61,7 +61,7 @@ TEST(NavState, Constructor) { TEST(NavState, Constructor2) { std::function construct = std::bind(&NavState::FromPoseVelocity, std::placeholders::_1, - std::placeholders::_2, boost::none, boost::none); + std::placeholders::_2, nullptr, nullptr); Matrix aH1, aH2; EXPECT( assert_equal(kState1, @@ -76,7 +76,7 @@ TEST( NavState, Attitude) { Rot3 actual = kState1.attitude(aH); EXPECT(assert_equal(actual, kAttitude)); eH = numericalDerivative11( - std::bind(&NavState::attitude, std::placeholders::_1, boost::none), kState1); + std::bind(&NavState::attitude, std::placeholders::_1, nullptr), kState1); EXPECT(assert_equal((Matrix )eH, aH)); } @@ -86,7 +86,7 @@ TEST( NavState, Position) { Point3 actual = kState1.position(aH); EXPECT(assert_equal(actual, kPosition)); eH = numericalDerivative11( - std::bind(&NavState::position, std::placeholders::_1, boost::none), + std::bind(&NavState::position, std::placeholders::_1, nullptr), kState1); EXPECT(assert_equal((Matrix )eH, aH)); } @@ -97,7 +97,7 @@ TEST( NavState, Velocity) { Velocity3 actual = kState1.velocity(aH); EXPECT(assert_equal(actual, kVelocity)); eH = numericalDerivative11( - std::bind(&NavState::velocity, std::placeholders::_1, boost::none), + std::bind(&NavState::velocity, std::placeholders::_1, nullptr), kState1); EXPECT(assert_equal((Matrix )eH, aH)); } @@ -108,7 +108,7 @@ TEST( NavState, BodyVelocity) { Velocity3 actual = kState1.bodyVelocity(aH); EXPECT(assert_equal(actual, kAttitude.unrotate(kVelocity))); eH = numericalDerivative11( - std::bind(&NavState::bodyVelocity, std::placeholders::_1, boost::none), + std::bind(&NavState::bodyVelocity, std::placeholders::_1, nullptr), kState1); EXPECT(assert_equal((Matrix )eH, aH)); } @@ -142,7 +142,7 @@ TEST( NavState, Manifold ) { kState1.retract(xi, aH1, aH2); std::function retract = std::bind(&NavState::retract, std::placeholders::_1, - std::placeholders::_2, boost::none, boost::none); + std::placeholders::_2, nullptr, nullptr); EXPECT(assert_equal(numericalDerivative21(retract, kState1, xi), aH1)); EXPECT(assert_equal(numericalDerivative22(retract, kState1, xi), aH2)); @@ -155,7 +155,7 @@ TEST( NavState, Manifold ) { // Check localCoordinates derivatives std::function local = std::bind(&NavState::localCoordinates, std::placeholders::_1, - std::placeholders::_2, boost::none, boost::none); + std::placeholders::_2, nullptr, nullptr); // from state1 to state2 kState1.localCoordinates(state2, aH1, aH2); EXPECT(assert_equal(numericalDerivative21(local, kState1, state2), aH1)); @@ -174,7 +174,7 @@ TEST( NavState, Manifold ) { static const double dt = 2.0; std::function coriolis = std::bind(&NavState::coriolis, std::placeholders::_1, dt, kOmegaCoriolis, - std::placeholders::_2, boost::none); + std::placeholders::_2, nullptr); TEST(NavState, Coriolis) { Matrix9 aH; @@ -252,7 +252,7 @@ TEST(NavState, CorrectPIM) { std::function correctPIM = std::bind(&NavState::correctPIM, std::placeholders::_1, std::placeholders::_2, dt, kGravity, kOmegaCoriolis, false, - boost::none, boost::none); + nullptr, nullptr); kState1.correctPIM(xi, dt, kGravity, kOmegaCoriolis, false, aH1, aH2); EXPECT(assert_equal(numericalDerivative21(correctPIM, kState1, xi), aH1)); EXPECT(assert_equal(numericalDerivative22(correctPIM, kState1, xi), aH2)); diff --git a/gtsam/navigation/tests/testTangentPreintegration.cpp b/gtsam/navigation/tests/testTangentPreintegration.cpp index ada059094..2f6678b97 100644 --- a/gtsam/navigation/tests/testTangentPreintegration.cpp +++ b/gtsam/navigation/tests/testTangentPreintegration.cpp @@ -108,8 +108,8 @@ TEST(TangentPreintegration, computeError) { const imuBias::ConstantBias&)> f = std::bind(&TangentPreintegration::computeError, pim, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3, boost::none, boost::none, - boost::none); + std::placeholders::_3, nullptr, nullptr, + nullptr); // NOTE(frank): tolerance of 1e-3 on H1 because approximate away from 0 EXPECT(assert_equal(numericalDerivative31(f, x1, x2, bias), aH1, 1e-9)); EXPECT(assert_equal(numericalDerivative32(f, x1, x2, bias), aH2, 1e-9)); diff --git a/gtsam/nonlinear/AdaptAutoDiff.h b/gtsam/nonlinear/AdaptAutoDiff.h index 682cca29a..b616a0e63 100644 --- a/gtsam/nonlinear/AdaptAutoDiff.h +++ b/gtsam/nonlinear/AdaptAutoDiff.h @@ -47,8 +47,8 @@ class AdaptAutoDiff { public: VectorT operator()(const Vector1& v1, const Vector2& v2, - OptionalJacobian H1 = boost::none, - OptionalJacobian H2 = boost::none) { + OptionalJacobian H1 = {}, + OptionalJacobian H2 = {}) { using ceres::internal::AutoDiff; bool success; diff --git a/gtsam/nonlinear/Expression-inl.h b/gtsam/nonlinear/Expression-inl.h index 06088c79b..7f7600774 100644 --- a/gtsam/nonlinear/Expression-inl.h +++ b/gtsam/nonlinear/Expression-inl.h @@ -241,7 +241,7 @@ struct apply_compose { typedef T result_type; static const int Dim = traits::dimension; T operator()(const T& x, const T& y, OptionalJacobian H1 = - boost::none, OptionalJacobian H2 = boost::none) const { + {}, OptionalJacobian H2 = {}) const { return x.compose(y, H1, H2); } }; @@ -249,8 +249,8 @@ struct apply_compose { template <> struct apply_compose { double operator()(const double& x, const double& y, - OptionalJacobian<1, 1> H1 = boost::none, - OptionalJacobian<1, 1> H2 = boost::none) const { + OptionalJacobian<1, 1> H1 = {}, + OptionalJacobian<1, 1> H2 = {}) const { if (H1) H1->setConstant(y); if (H2) H2->setConstant(x); return x * y; diff --git a/gtsam/nonlinear/FunctorizedFactor.h b/gtsam/nonlinear/FunctorizedFactor.h index 4053891f1..ee927327f 100644 --- a/gtsam/nonlinear/FunctorizedFactor.h +++ b/gtsam/nonlinear/FunctorizedFactor.h @@ -42,7 +42,7 @@ namespace gtsam { * public: * MultiplyFunctor(double m) : m_(m) {} * Matrix operator()(const Matrix &X, - * OptionalJacobian<-1, -1> H = boost::none) const { + * OptionalJacobian<-1, -1> H = {}) const { * if (H) * *H = m_ * Matrix::Identity(X.rows()*X.cols(), X.rows()*X.cols()); * return m_ * X; diff --git a/gtsam/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h index 3237d7c1e..d20e7dbb8 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.h +++ b/gtsam/nonlinear/NonlinearFactorGraph.h @@ -21,6 +21,7 @@ #pragma once +#include #include #include #include @@ -268,11 +269,11 @@ namespace gtsam { /// @{ /** @deprecated */ boost::shared_ptr GTSAM_DEPRECATED linearizeToHessianFactor( - const Values& values, boost::none_t, const Dampen& dampen = nullptr) const + const Values& values, std::nullptr_t, const Dampen& dampen = nullptr) const {return linearizeToHessianFactor(values, dampen);} /** @deprecated */ - Values GTSAM_DEPRECATED updateCholesky(const Values& values, boost::none_t, + Values GTSAM_DEPRECATED updateCholesky(const Values& values, std::nullptr_t, const Dampen& dampen = nullptr) const {return updateCholesky(values, dampen);} diff --git a/gtsam/nonlinear/internal/ExpressionNode.h b/gtsam/nonlinear/internal/ExpressionNode.h index f6afb287e..4c34a1777 100644 --- a/gtsam/nonlinear/internal/ExpressionNode.h +++ b/gtsam/nonlinear/internal/ExpressionNode.h @@ -255,7 +255,7 @@ public: /// Return value T value(const Values& values) const override { - return function_(expression1_->value(values), boost::none); + return function_(expression1_->value(values), {}); } /// Return keys that play in this expression @@ -365,9 +365,9 @@ public: /// Return value T value(const Values& values) const override { - using boost::none; + using std::nullopt; return function_(expression1_->value(values), expression2_->value(values), - none, none); + {}, {}); } /// Return keys that play in this expression @@ -473,9 +473,9 @@ public: /// Return value T value(const Values& values) const override { - using boost::none; + using std::nullopt; return function_(expression1_->value(values), expression2_->value(values), - expression3_->value(values), none, none, none); + expression3_->value(values), {}, {}, {}); } /// Return keys that play in this expression diff --git a/gtsam/nonlinear/tests/testExpression.cpp b/gtsam/nonlinear/tests/testExpression.cpp index a93f8a0e1..06ad16dfd 100644 --- a/gtsam/nonlinear/tests/testExpression.cpp +++ b/gtsam/nonlinear/tests/testExpression.cpp @@ -119,7 +119,7 @@ class Class : public Point3 { using Point3::Point3; const Vector3& vector() const { return *this; } inline static Class Identity() { return Class(0,0,0); } - double norm(OptionalJacobian<1,3> H = boost::none) const { + double norm(OptionalJacobian<1,3> H = {}) const { return norm3(*this, H); } bool equals(const Class &q, double tol) const { diff --git a/gtsam/nonlinear/tests/testFactorTesting.cpp b/gtsam/nonlinear/tests/testFactorTesting.cpp index 53111e1af..48f61a971 100644 --- a/gtsam/nonlinear/tests/testFactorTesting.cpp +++ b/gtsam/nonlinear/tests/testFactorTesting.cpp @@ -28,8 +28,8 @@ using namespace gtsam; /* ************************************************************************* */ Vector3 bodyVelocity(const Pose3& w_t_b, const Vector3& vec_w, - OptionalJacobian<3, 6> Hpose = boost::none, - OptionalJacobian<3, 3> Hvel = boost::none) { + OptionalJacobian<3, 6> Hpose = {}, + OptionalJacobian<3, 3> Hvel = {}) { Matrix36 Hrot__pose; Rot3 w_R_b = w_t_b.rotation(Hrot__pose); Matrix33 Hvel__rot; @@ -51,7 +51,7 @@ class ScaledVelocityFunctor { // and velocity scale factor. Also computes the corresponding jacobian // (w.r.t. the velocity scale). Vector3 operator()(double vscale, - OptionalJacobian<3, 1> H = boost::none) const { + OptionalJacobian<3, 1> H = {}) const { // The velocity scale factor value we are optimizing for is centered around // 0, so we need to add 1 to it before scaling the velocity. const Vector3 scaled_velocity = (vscale + 1.0) * measured_velocity_; diff --git a/gtsam/nonlinear/tests/testFunctorizedFactor.cpp b/gtsam/nonlinear/tests/testFunctorizedFactor.cpp index dbc22c079..f4ed70885 100644 --- a/gtsam/nonlinear/tests/testFunctorizedFactor.cpp +++ b/gtsam/nonlinear/tests/testFunctorizedFactor.cpp @@ -47,7 +47,7 @@ class MultiplyFunctor { MultiplyFunctor(double m) : m_(m) {} Matrix operator()(const Matrix &X, - OptionalJacobian<-1, -1> H = boost::none) const { + OptionalJacobian<-1, -1> H = {}) const { if (H) *H = m_ * Matrix::Identity(X.rows() * X.cols(), X.rows() * X.cols()); return m_ * X; } @@ -57,8 +57,8 @@ class MultiplyFunctor { class ProjectionFunctor { public: Vector operator()(const Matrix &A, const Vector &x, - OptionalJacobian<-1, -1> H1 = boost::none, - OptionalJacobian<-1, -1> H2 = boost::none) const { + OptionalJacobian<-1, -1> H1 = {}, + OptionalJacobian<-1, -1> H2 = {}) const { if (H1) { H1->resize(x.size(), A.size()); *H1 << I_3x3, I_3x3, I_3x3; @@ -178,7 +178,7 @@ TEST(FunctorizedFactor, Lambda) { Matrix measurement = multiplier * Matrix::Identity(3, 3); auto lambda = [multiplier](const Matrix &X, - OptionalJacobian<-1, -1> H = boost::none) { + OptionalJacobian<-1, -1> H = {}) { if (H) *H = multiplier * Matrix::Identity(X.rows() * X.cols(), X.rows() * X.cols()); @@ -251,8 +251,8 @@ TEST(FunctorizedFactor, Lambda2) { Matrix measurement = A * x; auto lambda = [](const Matrix &A, const Vector &x, - OptionalJacobian<-1, -1> H1 = boost::none, - OptionalJacobian<-1, -1> H2 = boost::none) { + OptionalJacobian<-1, -1> H1 = {}, + OptionalJacobian<-1, -1> H2 = {}) { if (H1) { H1->resize(x.size(), A.size()); *H1 << I_3x3, I_3x3, I_3x3; diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index ee7fcd189..9d22c0daf 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -60,13 +60,13 @@ public: bool equals(const TestValue& other, double tol = 1e-9) const { return true; } size_t dim() const { return 0; } TestValue retract(const Vector&, - OptionalJacobian H1=boost::none, - OptionalJacobian H2=boost::none) const { + OptionalJacobian H1={}, + OptionalJacobian H2={}) const { return TestValue(); } Vector localCoordinates(const TestValue&, - OptionalJacobian H1=boost::none, - OptionalJacobian H2=boost::none) const { + OptionalJacobian H1={}, + OptionalJacobian H2={}) const { return Vector(); } }; diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h index 3b60c8a5a..e8824d22b 100644 --- a/gtsam/slam/ProjectionFactor.h +++ b/gtsam/slam/ProjectionFactor.h @@ -142,16 +142,16 @@ namespace gtsam { if(H1) { gtsam::Matrix H0; PinholeCamera camera(pose.compose(*body_P_sensor_, H0), *K_); - Point2 reprojectionError(camera.project(point, H1, H2, boost::none) - measured_); + Point2 reprojectionError(camera.project(point, H1, H2, {}) - measured_); *H1 = *H1 * H0; return reprojectionError; } else { PinholeCamera camera(pose.compose(*body_P_sensor_), *K_); - return camera.project(point, H1, H2, boost::none) - measured_; + return camera.project(point, H1, H2, {}) - measured_; } } else { PinholeCamera camera(pose, *K_); - return camera.project(point, H1, H2, boost::none) - measured_; + return camera.project(point, H1, H2, {}) - measured_; } } catch( CheiralityException& e) { if (H1) *H1 = Matrix::Zero(2,6); diff --git a/gtsam/slam/TriangulationFactor.h b/gtsam/slam/TriangulationFactor.h index 6fac062b9..bd8354435 100644 --- a/gtsam/slam/TriangulationFactor.h +++ b/gtsam/slam/TriangulationFactor.h @@ -163,7 +163,7 @@ public: // Would be even better if we could pass blocks to project const Point3& point = x.at(key()); - b = traits::Local(camera_.project2(point, boost::none, A), measured_); + b = traits::Local(camera_.project2(point, {}, A), measured_); if (noiseModel_) this->noiseModel_->WhitenSystem(A, b); diff --git a/gtsam/slam/tests/testTriangulationFactor.cpp b/gtsam/slam/tests/testTriangulationFactor.cpp index 42df4d8fe..bdfade5f2 100644 --- a/gtsam/slam/tests/testTriangulationFactor.cpp +++ b/gtsam/slam/tests/testTriangulationFactor.cpp @@ -91,7 +91,7 @@ TEST( triangulation, TriangulationFactorStereo ) { // compare same problem against expression factor Expression::UnaryFunction::type f = std::bind(&StereoCamera::project2, camera2, std::placeholders::_1, - boost::none, std::placeholders::_2); + nullptr, std::placeholders::_2); Expression point_(pointKey); Expression project2_(f, point_); diff --git a/gtsam_unstable/dynamics/PoseRTV.h b/gtsam_unstable/dynamics/PoseRTV.h index c573de2b6..89ae59f9c 100644 --- a/gtsam_unstable/dynamics/PoseRTV.h +++ b/gtsam_unstable/dynamics/PoseRTV.h @@ -88,8 +88,8 @@ public: /** range between translations */ double range(const PoseRTV& other, - OptionalJacobian<1,9> H1=boost::none, - OptionalJacobian<1,9> H2=boost::none) const; + OptionalJacobian<1,9> H1={}, + OptionalJacobian<1,9> H2={}) const; /// @} /// @name IMU-specific @@ -138,8 +138,8 @@ public: * Note: the transform jacobian convention is flipped */ PoseRTV transformed_from(const Pose3& trans, - ChartJacobian Dglobal = boost::none, - OptionalJacobian<9, 6> Dtrans = boost::none) const; + ChartJacobian Dglobal = {}, + OptionalJacobian<9, 6> Dtrans = {}) const; /// @} /// @name Utility functions diff --git a/gtsam_unstable/dynamics/SimpleHelicopter.h b/gtsam_unstable/dynamics/SimpleHelicopter.h index 3e4ad0d9f..9a536fba0 100644 --- a/gtsam_unstable/dynamics/SimpleHelicopter.h +++ b/gtsam_unstable/dynamics/SimpleHelicopter.h @@ -130,7 +130,7 @@ public: Vector pk_1 = muk_1 - 0.5*Pose3::adjointTranspose(-h_*xik_1, muk_1, D_adjThxik1_muk1); Matrix D_gravityBody_gk; - Point3 gravityBody = gk.rotation().unrotate(Point3(0.0, 0.0, -9.81*m_), D_gravityBody_gk, boost::none); + Point3 gravityBody = gk.rotation().unrotate(Point3(0.0, 0.0, -9.81*m_), D_gravityBody_gk, {}); Vector f_ext = (Vector(6) << 0.0, 0.0, 0.0, gravityBody.x(), gravityBody.y(), gravityBody.z()).finished(); Vector hx = pk - pk_1 - h_*Fu_ - h_*f_ext; diff --git a/gtsam_unstable/geometry/Event.h b/gtsam_unstable/geometry/Event.h index ddeda180e..a4055d038 100644 --- a/gtsam_unstable/geometry/Event.h +++ b/gtsam_unstable/geometry/Event.h @@ -55,7 +55,7 @@ class Event { Point3 location() const { return location_; } // TODO(frank) we really have to think of a better way to do linear arguments - double height(OptionalJacobian<1, 4> H = boost::none) const { + double height(OptionalJacobian<1, 4> H = {}) const { static const Matrix14 JacobianZ = (Matrix14() << 0, 0, 0, 1).finished(); if (H) *H = JacobianZ; return location_.z(); @@ -101,8 +101,8 @@ class TimeOfArrival { /// Calculate time of arrival, with derivatives double operator()(const Event& event, const Point3& sensor, // - OptionalJacobian<1, 4> H1 = boost::none, // - OptionalJacobian<1, 3> H2 = boost::none) const { + OptionalJacobian<1, 4> H1 = {}, // + OptionalJacobian<1, 3> H2 = {}) const { Matrix13 D1, D2; double distance = gtsam::distance3(event.location(), sensor, D1, D2); if (H1) diff --git a/gtsam_unstable/geometry/InvDepthCamera3.h b/gtsam_unstable/geometry/InvDepthCamera3.h index 7e070ca72..643be4e3e 100644 --- a/gtsam_unstable/geometry/InvDepthCamera3.h +++ b/gtsam_unstable/geometry/InvDepthCamera3.h @@ -101,7 +101,7 @@ public: } else { gtsam::Matrix J2; - gtsam::Point2 uv= camera.project(landmark,H1, J2, boost::none); + gtsam::Point2 uv= camera.project(landmark,H1, J2, {}); if (H1) { *H1 = (*H1) * I_6x6; } diff --git a/gtsam_unstable/geometry/Pose3Upright.h b/gtsam_unstable/geometry/Pose3Upright.h index 321eba934..1c205c7fb 100644 --- a/gtsam_unstable/geometry/Pose3Upright.h +++ b/gtsam_unstable/geometry/Pose3Upright.h @@ -98,12 +98,12 @@ public: static Pose3Upright Identity() { return Pose3Upright(); } /// inverse transformation with derivatives - Pose3Upright inverse(OptionalJacobian<4,4> H1=boost::none) const; + Pose3Upright inverse(OptionalJacobian<4,4> H1={}) const; ///compose this transformation onto another (first *this and then p2) Pose3Upright compose(const Pose3Upright& p2, - OptionalJacobian<4,4> H1=boost::none, - OptionalJacobian<4,4> H2=boost::none) const; + OptionalJacobian<4,4> H1={}, + OptionalJacobian<4,4> H2={}) const; /// compose syntactic sugar inline Pose3Upright operator*(const Pose3Upright& T) const { return compose(T); } @@ -113,8 +113,8 @@ public: * as well as optionally the derivatives */ Pose3Upright between(const Pose3Upright& p2, - OptionalJacobian<4,4> H1=boost::none, - OptionalJacobian<4,4> H2=boost::none) const; + OptionalJacobian<4,4> H1={}, + OptionalJacobian<4,4> H2={}) const; /// @} /// @name Lie Group diff --git a/gtsam_unstable/geometry/tests/testEvent.cpp b/gtsam_unstable/geometry/tests/testEvent.cpp index f1bbc3970..25a05793d 100644 --- a/gtsam_unstable/geometry/tests/testEvent.cpp +++ b/gtsam_unstable/geometry/tests/testEvent.cpp @@ -65,11 +65,11 @@ TEST(Event, Derivatives) { Matrix13 actualH2; kToa(exampleEvent, microphoneAt0, actualH1, actualH2); Matrix expectedH1 = numericalDerivative11( - std::bind(kToa, std::placeholders::_1, microphoneAt0, boost::none, boost::none), + std::bind(kToa, std::placeholders::_1, microphoneAt0, nullptr, nullptr), exampleEvent); EXPECT(assert_equal(expectedH1, actualH1, 1e-8)); Matrix expectedH2 = numericalDerivative11( - std::bind(kToa, exampleEvent, std::placeholders::_1, boost::none, boost::none), + std::bind(kToa, exampleEvent, std::placeholders::_1, nullptr, nullptr), microphoneAt0); EXPECT(assert_equal(expectedH2, actualH2, 1e-8)); } diff --git a/gtsam_unstable/gtsam_unstable.i b/gtsam_unstable/gtsam_unstable.i index 6ce7be20a..d338b34aa 100644 --- a/gtsam_unstable/gtsam_unstable.i +++ b/gtsam_unstable/gtsam_unstable.i @@ -179,7 +179,7 @@ class SimWall2D { gtsam::Point2 midpoint() const; bool intersects(const gtsam::SimWall2D& wall) const; - // bool intersects(const gtsam::SimWall2D& wall, boost::optional pt=boost::none) const; + // bool intersects(const gtsam::SimWall2D& wall, boost::optional pt={}) const; gtsam::Point2 norm() const; gtsam::Rot2 reflection(const gtsam::Point2& init, const gtsam::Point2& intersection) const; diff --git a/tests/testSimulated3D.cpp b/tests/testSimulated3D.cpp index 2bc381f7a..cfe00856a 100644 --- a/tests/testSimulated3D.cpp +++ b/tests/testSimulated3D.cpp @@ -46,7 +46,7 @@ TEST( simulated3D, Values ) TEST( simulated3D, Dprior ) { Point3 x(1,-9, 7); - Matrix numerical = numericalDerivative11(std::bind(simulated3D::prior, std::placeholders::_1, boost::none),x); + Matrix numerical = numericalDerivative11(std::bind(simulated3D::prior, std::placeholders::_1, nullptr),x); Matrix computed; simulated3D::prior(x,computed); EXPECT(assert_equal(numerical,computed,1e-9)); @@ -60,12 +60,12 @@ TEST( simulated3D, DOdo ) simulated3D::odo(x1, x2, H1, H2); Matrix A1 = numericalDerivative21( std::bind(simulated3D::odo, std::placeholders::_1, std::placeholders::_2, - boost::none, boost::none), + nullptr, nullptr), x1, x2); EXPECT(assert_equal(A1, H1, 1e-9)); Matrix A2 = numericalDerivative22( std::bind(simulated3D::odo, std::placeholders::_1, std::placeholders::_2, - boost::none, boost::none), + nullptr, nullptr), x1, x2); EXPECT(assert_equal(A2, H2, 1e-9)); } From 607a30a08efd275e03a556376a5b650afa1b80c1 Mon Sep 17 00:00:00 2001 From: kartik arcot Date: Fri, 13 Jan 2023 14:12:15 -0800 Subject: [PATCH 22/38] .i files --- gtsam/geometry/geometry.i | 8 ++++---- gtsam/navigation/navigation.i | 4 ++-- gtsam/nonlinear/nonlinear.i | 2 +- gtsam_unstable/gtsam_unstable.i | 2 +- 4 files changed, 8 insertions(+), 8 deletions(-) diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 2f1810dbb..e9929227a 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -376,8 +376,8 @@ class Pose2 { Pose2(const gtsam::Rot2& r, const gtsam::Point2& t); Pose2(Vector v); - static boost::optional Align(const gtsam::Point2Pairs& abPointPairs); - static boost::optional Align(const gtsam::Matrix& a, const gtsam::Matrix& b); + static std::optional Align(const gtsam::Point2Pairs& abPointPairs); + static std::optional Align(const gtsam::Matrix& a, const gtsam::Matrix& b); // Testable void print(string s = "") const; @@ -440,8 +440,8 @@ class Pose3 { Pose3(const gtsam::Pose2& pose2); Pose3(Matrix mat); - static boost::optional Align(const gtsam::Point3Pairs& abPointPairs); - static boost::optional Align(const gtsam::Matrix& a, const gtsam::Matrix& b); + static std::optional Align(const gtsam::Point3Pairs& abPointPairs); + static std::optional Align(const gtsam::Matrix& a, const gtsam::Matrix& b); // Testable void print(string s = "") const; diff --git a/gtsam/navigation/navigation.i b/gtsam/navigation/navigation.i index 7bbef9fc5..8e6090e06 100644 --- a/gtsam/navigation/navigation.i +++ b/gtsam/navigation/navigation.i @@ -75,8 +75,8 @@ virtual class PreintegratedRotationParams { Matrix getGyroscopeCovariance() const; - boost::optional getOmegaCoriolis() const; - boost::optional getBodyPSensor() const; + std::optional getOmegaCoriolis() const; + std::optional getBodyPSensor() const; }; #include diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index 80e87a5fa..3e3373f61 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -392,7 +392,7 @@ virtual class LinearContainerFactor : gtsam::NonlinearFactor { LinearContainerFactor(gtsam::GaussianFactor* factor); gtsam::GaussianFactor* factor() const; - // const boost::optional& linearizationPoint() const; + // const std::optional& linearizationPoint() const; bool isJacobian() const; gtsam::JacobianFactor* toJacobian() const; diff --git a/gtsam_unstable/gtsam_unstable.i b/gtsam_unstable/gtsam_unstable.i index d338b34aa..5cd7e0da2 100644 --- a/gtsam_unstable/gtsam_unstable.i +++ b/gtsam_unstable/gtsam_unstable.i @@ -179,7 +179,7 @@ class SimWall2D { gtsam::Point2 midpoint() const; bool intersects(const gtsam::SimWall2D& wall) const; - // bool intersects(const gtsam::SimWall2D& wall, boost::optional pt={}) const; + // bool intersects(const gtsam::SimWall2D& wall, gtsam::Point2* pt = nullptr) const; gtsam::Point2 norm() const; gtsam::Rot2 reflection(const gtsam::Point2& init, const gtsam::Point2& intersection) const; From d886e78afabeff659d6d2d12bdd71d3ca5239110 Mon Sep 17 00:00:00 2001 From: kartik arcot Date: Tue, 17 Jan 2023 12:00:39 -0800 Subject: [PATCH 23/38] disabled two python tests --- gtsam/geometry/triangulation.h | 7 +++++++ python/gtsam/preamble/geometry.h | 7 ------- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 660d9fa3c..f0810105f 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -20,6 +20,7 @@ #pragma once +#include "gtsam/geometry/Point3.h" #include #include #include @@ -33,6 +34,8 @@ #include #include +#include + namespace gtsam { /// Exception thrown by triangulateDLT when SVD returns rank < 3 @@ -656,6 +659,10 @@ class TriangulationResult : public std::optional { bool outlier() const { return status == OUTLIER; } bool farPoint() const { return status == FAR_POINT; } bool behindCamera() const { return status == BEHIND_CAMERA; } + const gtsam::Point3& get() const { + if (!has_value()) throw std::runtime_error("TriangulationResult has no value"); + return value(); + } // stream to output friend std::ostream& operator<<(std::ostream& os, const TriangulationResult& result) { diff --git a/python/gtsam/preamble/geometry.h b/python/gtsam/preamble/geometry.h index bd0441d06..f36221d38 100644 --- a/python/gtsam/preamble/geometry.h +++ b/python/gtsam/preamble/geometry.h @@ -12,13 +12,6 @@ */ #include -// Support for binding boost::optional types in C++11. -// https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html -namespace pybind11 { namespace detail { - template - struct type_caster> : optional_caster> {}; -}} - PYBIND11_MAKE_OPAQUE( std::vector>); PYBIND11_MAKE_OPAQUE(gtsam::Point2Pairs); From 8181b50de3e82fb6ad7ff866ce79c34d7bfe81e7 Mon Sep 17 00:00:00 2001 From: kartik arcot Date: Wed, 18 Jan 2023 10:08:59 -0800 Subject: [PATCH 24/38] remove optional headers --- gtsam/base/ThreadsafeException.h | 1 - gtsam/base/Vector.cpp | 1 - gtsam/geometry/PinholeSet.h | 1 - gtsam/geometry/Rot2.h | 1 - gtsam/geometry/Unit3.h | 1 - gtsam/geometry/concepts.h | 1 - gtsam/inference/VariableIndex.h | 1 - gtsam/nonlinear/ISAM2UpdateParams.h | 1 - gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp | 1 - gtsam/nonlinear/NonlinearOptimizerParams.h | 1 - gtsam/slam/GeneralSFMFactor.h | 1 - gtsam/slam/SmartFactorBase.h | 1 - gtsam_unstable/discrete/examples/schedulingExample.cpp | 1 - gtsam_unstable/discrete/examples/schedulingQuals12.cpp | 1 - gtsam_unstable/discrete/examples/schedulingQuals13.cpp | 1 - gtsam_unstable/discrete/tests/testScheduler.cpp | 1 - gtsam_unstable/geometry/InvDepthCamera3.h | 1 - gtsam_unstable/partition/FindSeparator.h | 1 - gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h | 1 - gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h | 1 - gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h | 1 - gtsam_unstable/slam/MultiProjectionFactor.h | 1 - gtsam_unstable/slam/ProjectionFactorPPP.h | 1 - gtsam_unstable/slam/ProjectionFactorPPPC.h | 1 - gtsam_unstable/slam/ProjectionFactorRollingShutter.h | 1 - gtsam_unstable/slam/SmartStereoProjectionFactor.h | 1 - 26 files changed, 26 deletions(-) diff --git a/gtsam/base/ThreadsafeException.h b/gtsam/base/ThreadsafeException.h index ad228e590..57e37237f 100644 --- a/gtsam/base/ThreadsafeException.h +++ b/gtsam/base/ThreadsafeException.h @@ -21,7 +21,6 @@ #include // for GTSAM_USE_TBB -#include #include #include #include diff --git a/gtsam/base/Vector.cpp b/gtsam/base/Vector.cpp index 16d913a96..ce7aeba7b 100644 --- a/gtsam/base/Vector.cpp +++ b/gtsam/base/Vector.cpp @@ -18,7 +18,6 @@ */ #include -#include #include #include #include diff --git a/gtsam/geometry/PinholeSet.h b/gtsam/geometry/PinholeSet.h index 7e3dae56f..7541f886b 100644 --- a/gtsam/geometry/PinholeSet.h +++ b/gtsam/geometry/PinholeSet.h @@ -19,7 +19,6 @@ #include #include -#include namespace gtsam { diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index fbf2d82e4..26e6e6b7a 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -21,7 +21,6 @@ #include #include -#include #include diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h index 8db13cddc..8f629786d 100644 --- a/gtsam/geometry/Unit3.h +++ b/gtsam/geometry/Unit3.h @@ -28,7 +28,6 @@ #include #include -#include #include #include diff --git a/gtsam/geometry/concepts.h b/gtsam/geometry/concepts.h index bafb62418..30d87ac53 100644 --- a/gtsam/geometry/concepts.h +++ b/gtsam/geometry/concepts.h @@ -25,7 +25,6 @@ #pragma once #include -#include namespace gtsam { diff --git a/gtsam/inference/VariableIndex.h b/gtsam/inference/VariableIndex.h index 1357eacdd..fa9944e87 100644 --- a/gtsam/inference/VariableIndex.h +++ b/gtsam/inference/VariableIndex.h @@ -23,7 +23,6 @@ #include #include -#include #include #include diff --git a/gtsam/nonlinear/ISAM2UpdateParams.h b/gtsam/nonlinear/ISAM2UpdateParams.h index 575c99be5..34e4d353a 100644 --- a/gtsam/nonlinear/ISAM2UpdateParams.h +++ b/gtsam/nonlinear/ISAM2UpdateParams.h @@ -20,7 +20,6 @@ #include // GTSAM_EXPORT #include // Key, KeySet #include //FactorIndices -#include namespace gtsam { diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index 9b894db25..5cbed2a1f 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -29,7 +29,6 @@ #include #include -#include #include #include diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.h b/gtsam/nonlinear/NonlinearOptimizerParams.h index 6809137ef..45240ecfc 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.h +++ b/gtsam/nonlinear/NonlinearOptimizerParams.h @@ -23,7 +23,6 @@ #include #include -#include #include namespace gtsam { diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index 3f405f985..78fdb189a 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -37,7 +37,6 @@ #include #include -#include #include #include #include diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index 09627a68f..1eba3c953 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -29,7 +29,6 @@ #include #include -#include #include #include #include diff --git a/gtsam_unstable/discrete/examples/schedulingExample.cpp b/gtsam_unstable/discrete/examples/schedulingExample.cpp index bf5f2f25c..dd92b295d 100644 --- a/gtsam_unstable/discrete/examples/schedulingExample.cpp +++ b/gtsam_unstable/discrete/examples/schedulingExample.cpp @@ -12,7 +12,6 @@ #include #include -#include #include #include diff --git a/gtsam_unstable/discrete/examples/schedulingQuals12.cpp b/gtsam_unstable/discrete/examples/schedulingQuals12.cpp index a15b6f922..8c4c0d728 100644 --- a/gtsam_unstable/discrete/examples/schedulingQuals12.cpp +++ b/gtsam_unstable/discrete/examples/schedulingQuals12.cpp @@ -12,7 +12,6 @@ #include #include -#include #include #include diff --git a/gtsam_unstable/discrete/examples/schedulingQuals13.cpp b/gtsam_unstable/discrete/examples/schedulingQuals13.cpp index e888875a4..06276c516 100644 --- a/gtsam_unstable/discrete/examples/schedulingQuals13.cpp +++ b/gtsam_unstable/discrete/examples/schedulingQuals13.cpp @@ -12,7 +12,6 @@ #include #include -#include #include #include diff --git a/gtsam_unstable/discrete/tests/testScheduler.cpp b/gtsam_unstable/discrete/tests/testScheduler.cpp index 3b0528360..f868abb5e 100644 --- a/gtsam_unstable/discrete/tests/testScheduler.cpp +++ b/gtsam_unstable/discrete/tests/testScheduler.cpp @@ -10,7 +10,6 @@ #include #include -#include using namespace std; using namespace gtsam; diff --git a/gtsam_unstable/geometry/InvDepthCamera3.h b/gtsam_unstable/geometry/InvDepthCamera3.h index 643be4e3e..8b2797938 100644 --- a/gtsam_unstable/geometry/InvDepthCamera3.h +++ b/gtsam_unstable/geometry/InvDepthCamera3.h @@ -12,7 +12,6 @@ #pragma once #include -#include #include #include #include diff --git a/gtsam_unstable/partition/FindSeparator.h b/gtsam_unstable/partition/FindSeparator.h index 9a6c61800..3093fae4d 100644 --- a/gtsam_unstable/partition/FindSeparator.h +++ b/gtsam_unstable/partition/FindSeparator.h @@ -10,7 +10,6 @@ #include #include -#include #include #include diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h index f6189cc5a..c6c960ac1 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h @@ -28,7 +28,6 @@ #include #include -#include #include diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h index 5d251094e..f4c20a2ba 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h @@ -27,7 +27,6 @@ // Using numerical derivative to calculate d(Pose3::Expmap)/dw #include -#include #include diff --git a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h index a107f84b5..508f1f294 100644 --- a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h +++ b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h @@ -27,7 +27,6 @@ #include #include -#include #include diff --git a/gtsam_unstable/slam/MultiProjectionFactor.h b/gtsam_unstable/slam/MultiProjectionFactor.h index d7cebcffc..5ba25abd2 100644 --- a/gtsam_unstable/slam/MultiProjectionFactor.h +++ b/gtsam_unstable/slam/MultiProjectionFactor.h @@ -22,7 +22,6 @@ #include #include -#include #include "gtsam/geometry/Cal3_S2.h" namespace gtsam { diff --git a/gtsam_unstable/slam/ProjectionFactorPPP.h b/gtsam_unstable/slam/ProjectionFactorPPP.h index 2b799ac1d..9c09257d3 100644 --- a/gtsam_unstable/slam/ProjectionFactorPPP.h +++ b/gtsam_unstable/slam/ProjectionFactorPPP.h @@ -21,7 +21,6 @@ #include #include #include -#include namespace gtsam { diff --git a/gtsam_unstable/slam/ProjectionFactorPPPC.h b/gtsam_unstable/slam/ProjectionFactorPPPC.h index 7159cd5e0..b4288d7a0 100644 --- a/gtsam_unstable/slam/ProjectionFactorPPPC.h +++ b/gtsam_unstable/slam/ProjectionFactorPPPC.h @@ -23,7 +23,6 @@ #include #include -#include namespace gtsam { diff --git a/gtsam_unstable/slam/ProjectionFactorRollingShutter.h b/gtsam_unstable/slam/ProjectionFactorRollingShutter.h index e6c0fd16a..2d2c2c6c2 100644 --- a/gtsam_unstable/slam/ProjectionFactorRollingShutter.h +++ b/gtsam_unstable/slam/ProjectionFactorRollingShutter.h @@ -23,7 +23,6 @@ #include #include -#include namespace gtsam { diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index 9ebc9ba10..1a2a648e3 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -31,7 +31,6 @@ #include #include -#include #include #include #include From 12e35b1367fb93e107827fbd2043d6df3f592a23 Mon Sep 17 00:00:00 2001 From: kartik arcot Date: Wed, 18 Jan 2023 15:20:45 -0800 Subject: [PATCH 25/38] make temp variable std optional --- gtsam_unstable/slam/SmartRangeFactor.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam_unstable/slam/SmartRangeFactor.h b/gtsam_unstable/slam/SmartRangeFactor.h index d512ed85a..17270f392 100644 --- a/gtsam_unstable/slam/SmartRangeFactor.h +++ b/gtsam_unstable/slam/SmartRangeFactor.h @@ -107,7 +107,7 @@ class SmartRangeFactor: public NoiseModelFactor { Circle2 circle1 = circles.front(); std::optional best_fh; - auto bestCircle2 = boost::make_optional(false, circle1); // fixes issue #38 + std::optional bestCircle2 = std::nullopt; // fixes issue #38 // loop over all circles for (const Circle2& it : circles) { From 9bc7f3c6ddf72cbd383d3c72ccf80804b80648a9 Mon Sep 17 00:00:00 2001 From: kartik arcot Date: Sat, 21 Jan 2023 10:20:15 -0800 Subject: [PATCH 26/38] fix ci issue --- gtsam/nonlinear/ISAM2UpdateParams.h | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/gtsam/nonlinear/ISAM2UpdateParams.h b/gtsam/nonlinear/ISAM2UpdateParams.h index 34e4d353a..5c3cec63f 100644 --- a/gtsam/nonlinear/ISAM2UpdateParams.h +++ b/gtsam/nonlinear/ISAM2UpdateParams.h @@ -20,6 +20,7 @@ #include // GTSAM_EXPORT #include // Key, KeySet #include //FactorIndices +#include namespace gtsam { @@ -36,16 +37,16 @@ struct ISAM2UpdateParams { /** An optional map of keys to group labels, such that a variable can be * constrained to a particular grouping in the BayesTree */ - std::optional> constrainedKeys{{}}; + std::optional> constrainedKeys{std::nullopt}; /** An optional set of nonlinear keys that iSAM2 will hold at a constant * linearization point, regardless of the size of the linear delta */ - std::optional> noRelinKeys{{}}; + std::optional> noRelinKeys{std::nullopt}; /** An optional set of nonlinear keys that iSAM2 will re-eliminate, regardless * of the size of the linear delta. This allows the provided keys to be * reordered. */ - std::optional> extraReelimKeys{{}}; + std::optional> extraReelimKeys{std::nullopt}; /** Relinearize any variables whose delta magnitude is sufficiently large * (Params::relinearizeThreshold), regardless of the relinearization @@ -62,7 +63,7 @@ struct ISAM2UpdateParams { * depend on Keys `X(2)`, `X(3)`. Next call to ISAM2::update() must include * its `newAffectedKeys` field with the map `13 -> {X(2), X(3)}`. */ - std::optional> newAffectedKeys{{}}; + std::optional> newAffectedKeys{std::nullopt}; /** By default, iSAM2 uses a wildfire update scheme that stops updating when * the deltas become too small down in the tree. This flagg forces a full From 1756114986756f8571fee15a6039037369d9873e Mon Sep 17 00:00:00 2001 From: kartik arcot Date: Sat, 21 Jan 2023 11:20:01 -0800 Subject: [PATCH 27/38] optional jacobian with matrix references --- gtsam/base/OptionalJacobian.h | 28 ++++++++++++++++++++++- gtsam/base/tests/testOptionalJacobian.cpp | 9 ++++++++ 2 files changed, 36 insertions(+), 1 deletion(-) diff --git a/gtsam/base/OptionalJacobian.h b/gtsam/base/OptionalJacobian.h index 3d5af97fd..22a17e131 100644 --- a/gtsam/base/OptionalJacobian.h +++ b/gtsam/base/OptionalJacobian.h @@ -19,8 +19,10 @@ #pragma once #include +#include #include // Configuration from CMake #include +#include #include #include @@ -118,8 +120,21 @@ public: "Expected: ") + "(" + std::to_string(Rows) + ", " + std::to_string(Cols) + ")"); } + } + + /// Constructor with std::nullopt just makes empty + OptionalJacobian(std::nullopt_t /*none*/) : + map_(nullptr) { } + /// Constructor compatible with old-style derivatives + OptionalJacobian(const std::optional> optional) : + map_(nullptr) { + if (optional) { + optional->get().resize(Rows, Cols); + usurp(optional->get().data()); + } + } /// Constructor that will usurp data of a block expression /// TODO(frank): unfortunately using a Map makes usurping non-contiguous memory impossible // template @@ -184,7 +199,7 @@ private: public: - /// Default constructor acts like + /// Default constructor OptionalJacobian() : pointer_(nullptr) { } @@ -195,6 +210,17 @@ public: /// Construct from refrence to dynamic matrix OptionalJacobian(Jacobian& dynamic) : pointer_(&dynamic) {} + /// Constructor with std::nullopt just makes empty + OptionalJacobian(std::nullopt_t /*none*/) : + pointer_(nullptr) { + } + + /// Constructor for optional matrix reference + OptionalJacobian(const std::optional> optional) : + pointer_(nullptr) { + if (optional) pointer_ = &((*optional).get()); + } + /// Return true if allocated, false if default constructor was used operator bool() const { return pointer_!=nullptr; diff --git a/gtsam/base/tests/testOptionalJacobian.cpp b/gtsam/base/tests/testOptionalJacobian.cpp index 7f1197584..4c775af75 100644 --- a/gtsam/base/tests/testOptionalJacobian.cpp +++ b/gtsam/base/tests/testOptionalJacobian.cpp @@ -20,6 +20,9 @@ #include #include +#include +#include + using namespace std; using namespace gtsam; @@ -32,6 +35,7 @@ using namespace gtsam; TEST( OptionalJacobian, Constructors ) { Matrix23 fixed; Matrix dynamic; + std::optional> optionalRef(std::ref(dynamic)); OptionalJacobian<2, 3> H; EXPECT(!H); @@ -40,12 +44,17 @@ TEST( OptionalJacobian, Constructors ) { TEST_CONSTRUCTOR(2, 3, &fixed, true); TEST_CONSTRUCTOR(2, 3, dynamic, true); TEST_CONSTRUCTOR(2, 3, &dynamic, true); + TEST_CONSTRUCTOR(2, 3, std::nullopt, false); + TEST_CONSTRUCTOR(2, 3, optionalRef, true); // Test dynamic OptionalJacobian<-1, -1> H7; EXPECT(!H7); TEST_CONSTRUCTOR(-1, -1, dynamic, true); + TEST_CONSTRUCTOR(-1, -1, std::nullopt, false); + TEST_CONSTRUCTOR(-1, -1, optionalRef, true); + } //****************************************************************************** From 4f7151c320996afe55a67cd3b9f2806619bc1b03 Mon Sep 17 00:00:00 2001 From: Kartik Arcot Date: Sat, 21 Jan 2023 12:06:04 -0800 Subject: [PATCH 28/38] use ref wrapper in eliminateable factor graph --- gtsam/hybrid/HybridGaussianISAM.cpp | 2 +- .../inference/EliminateableFactorGraph-inst.h | 52 +++++++++---------- gtsam/inference/EliminateableFactorGraph.h | 3 +- gtsam/inference/ISAM-inst.h | 2 +- 4 files changed, 30 insertions(+), 29 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianISAM.cpp b/gtsam/hybrid/HybridGaussianISAM.cpp index b6534df70..dc7746209 100644 --- a/gtsam/hybrid/HybridGaussianISAM.cpp +++ b/gtsam/hybrid/HybridGaussianISAM.cpp @@ -102,7 +102,7 @@ void HybridGaussianISAM::updateInternal( // eliminate all factors (top, added, orphans) into a new Bayes tree HybridBayesTree::shared_ptr bayesTree = - factors.eliminateMultifrontal(elimination_ordering, function, &index); + factors.eliminateMultifrontal(elimination_ordering, function, std::cref(index)); if (maxNrLeaves) { bayesTree->prune(*maxNrLeaves); diff --git a/gtsam/inference/EliminateableFactorGraph-inst.h b/gtsam/inference/EliminateableFactorGraph-inst.h index a0847dbdd..b7aa52bab 100644 --- a/gtsam/inference/EliminateableFactorGraph-inst.h +++ b/gtsam/inference/EliminateableFactorGraph-inst.h @@ -36,7 +36,7 @@ namespace gtsam { // no Ordering is provided. When removing optional from VariableIndex, create VariableIndex // before creating ordering. VariableIndex computedVariableIndex(asDerived()); - return eliminateSequential(orderingType, function, &computedVariableIndex); + return eliminateSequential(orderingType, function, std::cref(computedVariableIndex)); } else { // Compute an ordering and call this function again. We are guaranteed to have a @@ -45,14 +45,14 @@ namespace gtsam { Ordering computedOrdering = Ordering::Metis(asDerived()); return eliminateSequential(computedOrdering, function, variableIndex); } else if (orderingType == Ordering::COLAMD) { - Ordering computedOrdering = Ordering::Colamd(*variableIndex); + Ordering computedOrdering = Ordering::Colamd((*variableIndex).get()); return eliminateSequential(computedOrdering, function, variableIndex); } else if (orderingType == Ordering::NATURAL) { Ordering computedOrdering = Ordering::Natural(asDerived()); return eliminateSequential(computedOrdering, function, variableIndex); } else { Ordering computedOrdering = EliminationTraitsType::DefaultOrderingFunc( - asDerived(), *variableIndex); + asDerived(), (*variableIndex).get()); return eliminateSequential(computedOrdering, function, variableIndex); } } @@ -68,11 +68,11 @@ namespace gtsam { if(!variableIndex) { // If no VariableIndex provided, compute one and call this function again VariableIndex computedVariableIndex(asDerived()); - return eliminateSequential(ordering, function, &computedVariableIndex); + return eliminateSequential(ordering, function, std::cref(computedVariableIndex)); } else { gttic(eliminateSequential); // Do elimination - EliminationTreeType etree(asDerived(), *variableIndex, ordering); + EliminationTreeType etree(asDerived(), (*variableIndex).get(), ordering); boost::shared_ptr bayesNet; boost::shared_ptr factorGraph; boost::tie(bayesNet,factorGraph) = etree.eliminate(function); @@ -99,7 +99,7 @@ namespace gtsam { // creating ordering. VariableIndex computedVariableIndex(asDerived()); return eliminateMultifrontal(orderingType, function, - &computedVariableIndex); + std::cref(computedVariableIndex)); } else { // Compute an ordering and call this function again. We are guaranteed to // have a VariableIndex already here because we computed one if needed in @@ -108,14 +108,14 @@ namespace gtsam { Ordering computedOrdering = Ordering::Metis(asDerived()); return eliminateMultifrontal(computedOrdering, function, variableIndex); } else if (orderingType == Ordering::COLAMD) { - Ordering computedOrdering = Ordering::Colamd(*variableIndex); + Ordering computedOrdering = Ordering::Colamd((*variableIndex).get()); return eliminateMultifrontal(computedOrdering, function, variableIndex); } else if (orderingType == Ordering::NATURAL) { Ordering computedOrdering = Ordering::Natural(asDerived()); return eliminateMultifrontal(computedOrdering, function, variableIndex); } else { Ordering computedOrdering = EliminationTraitsType::DefaultOrderingFunc( - asDerived(), *variableIndex); + asDerived(), (*variableIndex).get()); return eliminateMultifrontal(computedOrdering, function, variableIndex); } } @@ -131,11 +131,11 @@ namespace gtsam { if(!variableIndex) { // If no VariableIndex provided, compute one and call this function again VariableIndex computedVariableIndex(asDerived()); - return eliminateMultifrontal(ordering, function, &computedVariableIndex); + return eliminateMultifrontal(ordering, function, std::cref(computedVariableIndex)); } else { gttic(eliminateMultifrontal); // Do elimination with given ordering - EliminationTreeType etree(asDerived(), *variableIndex, ordering); + EliminationTreeType etree(asDerived(), (*variableIndex).get(), ordering); JunctionTreeType junctionTree(etree); boost::shared_ptr bayesTree; boost::shared_ptr factorGraph; @@ -157,12 +157,12 @@ namespace gtsam { if(variableIndex) { gttic(eliminatePartialSequential); // Do elimination - EliminationTreeType etree(asDerived(), *variableIndex, ordering); + EliminationTreeType etree(asDerived(), (*variableIndex).get(), ordering); return etree.eliminate(function); } else { // If no variable index is provided, compute one and call this function again VariableIndex computedVariableIndex(asDerived()); - return eliminatePartialSequential(ordering, function, &computedVariableIndex); + return eliminatePartialSequential(ordering, function, std::cref(computedVariableIndex)); } } @@ -175,7 +175,7 @@ namespace gtsam { if(variableIndex) { gttic(eliminatePartialSequential); // Compute full ordering - Ordering fullOrdering = Ordering::ColamdConstrainedFirst(*variableIndex, variables); + Ordering fullOrdering = Ordering::ColamdConstrainedFirst((*variableIndex).get(), variables); // Split off the part of the ordering for the variables being eliminated Ordering ordering(fullOrdering.begin(), fullOrdering.begin() + variables.size()); @@ -183,7 +183,7 @@ namespace gtsam { } else { // If no variable index is provided, compute one and call this function again VariableIndex computedVariableIndex(asDerived()); - return eliminatePartialSequential(variables, function, &computedVariableIndex); + return eliminatePartialSequential(variables, function, std::cref(computedVariableIndex)); } } @@ -196,13 +196,13 @@ namespace gtsam { if(variableIndex) { gttic(eliminatePartialMultifrontal); // Do elimination - EliminationTreeType etree(asDerived(), *variableIndex, ordering); + EliminationTreeType etree(asDerived(), (*variableIndex).get(), ordering); JunctionTreeType junctionTree(etree); return junctionTree.eliminate(function); } else { // If no variable index is provided, compute one and call this function again VariableIndex computedVariableIndex(asDerived()); - return eliminatePartialMultifrontal(ordering, function, &computedVariableIndex); + return eliminatePartialMultifrontal(ordering, function, std::cref(computedVariableIndex)); } } @@ -215,7 +215,7 @@ namespace gtsam { if(variableIndex) { gttic(eliminatePartialMultifrontal); // Compute full ordering - Ordering fullOrdering = Ordering::ColamdConstrainedFirst(*variableIndex, variables); + Ordering fullOrdering = Ordering::ColamdConstrainedFirst((*variableIndex).get(), variables); // Split off the part of the ordering for the variables being eliminated Ordering ordering(fullOrdering.begin(), fullOrdering.begin() + variables.size()); @@ -223,7 +223,7 @@ namespace gtsam { } else { // If no variable index is provided, compute one and call this function again VariableIndex computedVariableIndex(asDerived()); - return eliminatePartialMultifrontal(variables, function, &computedVariableIndex); + return eliminatePartialMultifrontal(variables, function, std::cref(computedVariableIndex)); } } @@ -237,7 +237,7 @@ namespace gtsam { if(!variableIndex) { // If no variable index is provided, compute one and call this function again VariableIndex index(asDerived()); - return marginalMultifrontalBayesNet(variables, function, &index); + return marginalMultifrontalBayesNet(variables, function, std::cref(index)); } else { // No ordering was provided for the marginalized variables, so order them using constrained // COLAMD. @@ -247,7 +247,7 @@ namespace gtsam { boost::get(&variables) : boost::get(&variables); Ordering totalOrdering = - Ordering::ColamdConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered); + Ordering::ColamdConstrainedLast((*variableIndex).get(), *variablesOrOrdering, unmarginalizedAreOrdered); // Split up ordering const size_t nVars = variablesOrOrdering->size(); @@ -270,7 +270,7 @@ namespace gtsam { if(!variableIndex) { // If no variable index is provided, compute one and call this function again VariableIndex index(asDerived()); - return marginalMultifrontalBayesNet(variables, marginalizedVariableOrdering, function, &index); + return marginalMultifrontalBayesNet(variables, marginalizedVariableOrdering, function, index); } else { gttic(marginalMultifrontalBayesNet); // An ordering was provided for the marginalized variables, so we can first eliminate them @@ -304,7 +304,7 @@ namespace gtsam { if(!variableIndex) { // If no variable index is provided, compute one and call this function again VariableIndex computedVariableIndex(asDerived()); - return marginalMultifrontalBayesTree(variables, function, &computedVariableIndex); + return marginalMultifrontalBayesTree(variables, function, std::cref(computedVariableIndex)); } else { // No ordering was provided for the marginalized variables, so order them using constrained // COLAMD. @@ -314,7 +314,7 @@ namespace gtsam { boost::get(&variables) : boost::get(&variables); Ordering totalOrdering = - Ordering::ColamdConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered); + Ordering::ColamdConstrainedLast((*variableIndex).get(), *variablesOrOrdering, unmarginalizedAreOrdered); // Split up ordering const size_t nVars = variablesOrOrdering->size(); @@ -337,7 +337,7 @@ namespace gtsam { if(!variableIndex) { // If no variable index is provided, compute one and call this function again VariableIndex computedVariableIndex(asDerived()); - return marginalMultifrontalBayesTree(variables, marginalizedVariableOrdering, function, &computedVariableIndex); + return marginalMultifrontalBayesTree(variables, marginalizedVariableOrdering, function, std::cref(computedVariableIndex)); } else { gttic(marginalMultifrontalBayesTree); // An ordering was provided for the marginalized variables, so we can first eliminate them @@ -371,7 +371,7 @@ namespace gtsam { if(variableIndex) { // Compute a total ordering for all variables - Ordering totalOrdering = Ordering::ColamdConstrainedLast(*variableIndex, variables); + Ordering totalOrdering = Ordering::ColamdConstrainedLast((*variableIndex).get(), variables); // Split out the part for the marginalized variables Ordering marginalizationOrdering(totalOrdering.begin(), totalOrdering.end() - variables.size()); @@ -383,7 +383,7 @@ namespace gtsam { { // If no variable index is provided, compute one and call this function again VariableIndex computedVariableIndex(asDerived()); - return marginal(variables, function, &computedVariableIndex); + return marginal(variables, function, std::cref(computedVariableIndex)); } } diff --git a/gtsam/inference/EliminateableFactorGraph.h b/gtsam/inference/EliminateableFactorGraph.h index 599baa7da..8cdf9abca 100644 --- a/gtsam/inference/EliminateableFactorGraph.h +++ b/gtsam/inference/EliminateableFactorGraph.h @@ -90,7 +90,8 @@ namespace gtsam { typedef std::function Eliminate; /// Typedef for an optional variable index as an argument to elimination functions - typedef const VariableIndex* OptionalVariableIndex; + /// It is an optional to a constant reference + typedef std::optional> OptionalVariableIndex; /// Typedef for an optional ordering type typedef std::optional OptionalOrderingType; diff --git a/gtsam/inference/ISAM-inst.h b/gtsam/inference/ISAM-inst.h index 5dd06702c..6026e052a 100644 --- a/gtsam/inference/ISAM-inst.h +++ b/gtsam/inference/ISAM-inst.h @@ -49,7 +49,7 @@ void ISAM::updateInternal(const FactorGraphType& newFactors, KeyVector(newFactorKeys.begin(), newFactorKeys.end())); // eliminate all factors (top, added, orphans) into a new Bayes tree - auto bayesTree = factors.eliminateMultifrontal(ordering, function, &index); + auto bayesTree = factors.eliminateMultifrontal(ordering, function, std::cref(index)); // Re-add into Bayes tree data structures this->roots_.insert(this->roots_.end(), bayesTree->roots().begin(), From 9e466345b67085b3f2f7e97d3322791aa0f84740 Mon Sep 17 00:00:00 2001 From: Kartik Arcot Date: Sat, 21 Jan 2023 12:09:13 -0800 Subject: [PATCH 29/38] remove set cxx17 --- CMakeLists.txt | 2 -- 1 file changed, 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 9be0d89a9..52b34101f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -6,8 +6,6 @@ if(NOT DEFINED CMAKE_MACOSX_RPATH) set(CMAKE_MACOSX_RPATH 0) endif() -set(CMAKE_CXX_STANDARD 17) - # Set the version number for the library set (GTSAM_VERSION_MAJOR 4) set (GTSAM_VERSION_MINOR 3) From e4e43d5d16b9acb071cf970a82d3c40d4b135614 Mon Sep 17 00:00:00 2001 From: Kartik Arcot Date: Sat, 21 Jan 2023 12:32:25 -0800 Subject: [PATCH 30/38] fix defaultorderingfunc --- gtsam/discrete/DiscreteFactorGraph.h | 4 ++-- gtsam/hybrid/HybridGaussianFactorGraph.h | 3 ++- gtsam/inference/EliminateableFactorGraph-inst.h | 4 ++-- gtsam/linear/GaussianFactorGraph.h | 4 ++-- gtsam/symbolic/SymbolicFactorGraph.h | 4 ++-- 5 files changed, 10 insertions(+), 9 deletions(-) diff --git a/gtsam/discrete/DiscreteFactorGraph.h b/gtsam/discrete/DiscreteFactorGraph.h index fdfc2bc0d..4441b0df4 100644 --- a/gtsam/discrete/DiscreteFactorGraph.h +++ b/gtsam/discrete/DiscreteFactorGraph.h @@ -70,8 +70,8 @@ template<> struct EliminationTraits /// The default ordering generation function static Ordering DefaultOrderingFunc( const FactorGraphType& graph, - const VariableIndex& variableIndex) { - return Ordering::Colamd(variableIndex); + std::optional> variableIndex) { + return Ordering::Colamd((*variableIndex).get()); } }; diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.h b/gtsam/hybrid/HybridGaussianFactorGraph.h index 432bf7c07..b516d0adc 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.h +++ b/gtsam/hybrid/HybridGaussianFactorGraph.h @@ -27,6 +27,7 @@ #include #include +#include #include namespace gtsam { @@ -87,7 +88,7 @@ struct EliminationTraits { /// The default ordering generation function static Ordering DefaultOrderingFunc( const FactorGraphType& graph, - const VariableIndex&) { + std::optional>) { return HybridOrdering(graph); } }; diff --git a/gtsam/inference/EliminateableFactorGraph-inst.h b/gtsam/inference/EliminateableFactorGraph-inst.h index b7aa52bab..eb6d57067 100644 --- a/gtsam/inference/EliminateableFactorGraph-inst.h +++ b/gtsam/inference/EliminateableFactorGraph-inst.h @@ -52,7 +52,7 @@ namespace gtsam { return eliminateSequential(computedOrdering, function, variableIndex); } else { Ordering computedOrdering = EliminationTraitsType::DefaultOrderingFunc( - asDerived(), (*variableIndex).get()); + asDerived(), *variableIndex); return eliminateSequential(computedOrdering, function, variableIndex); } } @@ -115,7 +115,7 @@ namespace gtsam { return eliminateMultifrontal(computedOrdering, function, variableIndex); } else { Ordering computedOrdering = EliminationTraitsType::DefaultOrderingFunc( - asDerived(), (*variableIndex).get()); + asDerived(), *variableIndex); return eliminateMultifrontal(computedOrdering, function, variableIndex); } } diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index 65c22cd04..cacb95119 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -58,8 +58,8 @@ namespace gtsam { /// The default ordering generation function static Ordering DefaultOrderingFunc( const FactorGraphType& graph, - const VariableIndex& variableIndex) { - return Ordering::Colamd(variableIndex); + std::optional> variableIndex) { + return Ordering::Colamd((*variableIndex).get()); } }; diff --git a/gtsam/symbolic/SymbolicFactorGraph.h b/gtsam/symbolic/SymbolicFactorGraph.h index ecb7c222b..219ba7bf5 100644 --- a/gtsam/symbolic/SymbolicFactorGraph.h +++ b/gtsam/symbolic/SymbolicFactorGraph.h @@ -49,8 +49,8 @@ namespace gtsam { /// The default ordering generation function static Ordering DefaultOrderingFunc( const FactorGraphType& graph, - const VariableIndex& variableIndex) { - return Ordering::Colamd(variableIndex); + std::optional> variableIndex) { + return Ordering::Colamd((*variableIndex).get()); } }; From 3cdbebbf6306df054e67276241d91df2d29686ae Mon Sep 17 00:00:00 2001 From: Kartik Arcot Date: Sat, 21 Jan 2023 12:53:59 -0800 Subject: [PATCH 31/38] more tests on optional serialization --- gtsam/base/std_optional_serialization.h | 5 ++- .../tests/testStdOptionalSerialization.cpp | 45 +++++++++++++++++++ 2 files changed, 48 insertions(+), 2 deletions(-) diff --git a/gtsam/base/std_optional_serialization.h b/gtsam/base/std_optional_serialization.h index ae85907c8..f285ae931 100644 --- a/gtsam/base/std_optional_serialization.h +++ b/gtsam/base/std_optional_serialization.h @@ -1,6 +1,7 @@ -// A hack to serialize std::optional to boost::archive -// Don't know if it will work. Trying to follow this: +// Functionality to serialize std::optional to boost::archive +// Following this: // PR: https://github.com/boostorg/serialization/pull/148/files# + #pragma once #include #include diff --git a/gtsam/base/tests/testStdOptionalSerialization.cpp b/gtsam/base/tests/testStdOptionalSerialization.cpp index 5affb9e69..a5aa815cd 100644 --- a/gtsam/base/tests/testStdOptionalSerialization.cpp +++ b/gtsam/base/tests/testStdOptionalSerialization.cpp @@ -60,6 +60,10 @@ public: TestOptionalStruct() = default; TestOptionalStruct(const int& opt) : opt(opt) {} + bool operator==(const TestOptionalStruct& other) const { + // check the values are equal + return *opt == *other.opt; + } friend class boost::serialization::access; template void serialize(Archive& ar, const unsigned int /*version*/) { @@ -104,6 +108,47 @@ TEST(StdOptionalSerialization, SerializTestOptionalStructUninitialized) { EXPECT(serializationTestHelpers::equalsXML(optStruct)); EXPECT(serializationTestHelpers::equalsBinary(optStruct)); } + +// Test for serialization of std::optional +TEST(StdOptionalSerialization, SerializTestOptionalStructPointer) { + // Create an optional + std::optional opt(TestOptionalStruct(42)); + + // Serialize it + std::stringstream ss; + boost::archive::text_oarchive oa(ss); + oa << opt; + + // Deserialize it + std::optional opt2; + boost::archive::text_iarchive ia(ss); + ia >> opt2; + + // Check that it worked + EXPECT(opt2.has_value()); + EXPECT(*opt2 == TestOptionalStruct(42)); +} + +// Test for serialization of std::optional +TEST(StdOptionalSerialization, SerializTestOptionalStructPointerPointer) { + // Create an optional + std::optional opt(new TestOptionalStruct(42)); + + // Serialize it + std::stringstream ss; + boost::archive::text_oarchive oa(ss); + oa << opt; + + // Deserialize it + std::optional opt2; + boost::archive::text_iarchive ia(ss); + ia >> opt2; + + // Check that it worked + EXPECT(opt2.has_value()); + EXPECT(**opt2 == TestOptionalStruct(42)); +} + int main() { TestResult tr; return TestRegistry::runAllTests(tr); From 9847eea40c57b9ffb72e2211b5e5b88f4657e2fa Mon Sep 17 00:00:00 2001 From: Kartik Arcot Date: Sat, 21 Jan 2023 13:11:40 -0800 Subject: [PATCH 32/38] fixed MR comments. Brought back a testable assertion feature. Added a test to test that assertion --- gtsam/base/TestableAssertions.h | 10 ++++++++++ gtsam/base/tests/testMatrix.cpp | 13 +++++++++++++ gtsam/nonlinear/ISAM2UpdateParams.h | 8 ++++---- gtsam/nonlinear/NonlinearFactorGraph.h | 1 - gtsam/nonlinear/NonlinearOptimizerParams.h | 2 ++ 5 files changed, 29 insertions(+), 5 deletions(-) diff --git a/gtsam/base/TestableAssertions.h b/gtsam/base/TestableAssertions.h index 020a5691c..b5068ad95 100644 --- a/gtsam/base/TestableAssertions.h +++ b/gtsam/base/TestableAssertions.h @@ -20,6 +20,7 @@ #include #include +#include #include #include #include @@ -71,6 +72,15 @@ bool assert_equal(const V& expected, const std::optional& actual, double tol return assert_equal(expected, *actual, tol); } +template +bool assert_equal(const V& expected, + const std::optional>& actual, double tol = 1e-9) { + if (!actual) { + std::cout << "actual is std::nullopt" << std::endl; + return false; + } + return assert_equal(expected, *actual.get(), tol); +} /** * Function for comparing maps of testable->testable diff --git a/gtsam/base/tests/testMatrix.cpp b/gtsam/base/tests/testMatrix.cpp index 7802f27e1..fb3bd948c 100644 --- a/gtsam/base/tests/testMatrix.cpp +++ b/gtsam/base/tests/testMatrix.cpp @@ -23,6 +23,8 @@ #include #include #include +#include +#include using namespace std; using namespace gtsam; @@ -1176,6 +1178,17 @@ TEST(Matrix, AbsoluteError) { EXPECT(isEqual); } +// A test to check if a matrix and an optional reference_wrapper to +// a matrix are equal. +TEST(Matrix, MatrixRef) { + Matrix A = Matrix::Random(3, 3); + Matrix B = Matrix::Random(3, 3); + + EXPECT(assert_equal(A, A)); + EXPECT(assert_equal(A, std::cref(A))); + EXPECT(!assert_equal(A, std::cref(B))); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/nonlinear/ISAM2UpdateParams.h b/gtsam/nonlinear/ISAM2UpdateParams.h index 5c3cec63f..fff047851 100644 --- a/gtsam/nonlinear/ISAM2UpdateParams.h +++ b/gtsam/nonlinear/ISAM2UpdateParams.h @@ -37,16 +37,16 @@ struct ISAM2UpdateParams { /** An optional map of keys to group labels, such that a variable can be * constrained to a particular grouping in the BayesTree */ - std::optional> constrainedKeys{std::nullopt}; + std::optional> constrainedKeys; /** An optional set of nonlinear keys that iSAM2 will hold at a constant * linearization point, regardless of the size of the linear delta */ - std::optional> noRelinKeys{std::nullopt}; + std::optional> noRelinKeys; /** An optional set of nonlinear keys that iSAM2 will re-eliminate, regardless * of the size of the linear delta. This allows the provided keys to be * reordered. */ - std::optional> extraReelimKeys{std::nullopt}; + std::optional> extraReelimKeys; /** Relinearize any variables whose delta magnitude is sufficiently large * (Params::relinearizeThreshold), regardless of the relinearization @@ -63,7 +63,7 @@ struct ISAM2UpdateParams { * depend on Keys `X(2)`, `X(3)`. Next call to ISAM2::update() must include * its `newAffectedKeys` field with the map `13 -> {X(2), X(3)}`. */ - std::optional> newAffectedKeys{std::nullopt}; + std::optional> newAffectedKeys; /** By default, iSAM2 uses a wildfire update scheme that stops updating when * the deltas become too small down in the tree. This flagg forces a full diff --git a/gtsam/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h index 0618f4eb1..cf0f5aa7f 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.h +++ b/gtsam/nonlinear/NonlinearFactorGraph.h @@ -21,7 +21,6 @@ #pragma once -#include #include #include #include diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.h b/gtsam/nonlinear/NonlinearOptimizerParams.h index 45240ecfc..7184b3dfc 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.h +++ b/gtsam/nonlinear/NonlinearOptimizerParams.h @@ -23,7 +23,9 @@ #include #include + #include +#include namespace gtsam { From 5cb31b3ae458882f91a8e5bec5475014554dcb19 Mon Sep 17 00:00:00 2001 From: Kartik Arcot Date: Sat, 21 Jan 2023 13:13:09 -0800 Subject: [PATCH 33/38] remove deprecated file --- gtsam/geometry/tests/testSimpleCamera.cpp | 160 ---------------------- 1 file changed, 160 deletions(-) delete mode 100644 gtsam/geometry/tests/testSimpleCamera.cpp diff --git a/gtsam/geometry/tests/testSimpleCamera.cpp b/gtsam/geometry/tests/testSimpleCamera.cpp deleted file mode 100644 index f4f69a137..000000000 --- a/gtsam/geometry/tests/testSimpleCamera.cpp +++ /dev/null @@ -1,160 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file testSimpleCamera.cpp - * @author Frank Dellaert - * @brief test SimpleCamera class - */ - -#include -#include -#include -#include -#include -#include -#include - -using namespace std; -using namespace gtsam; - -#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 - -static const Cal3_S2 K(625, 625, 0, 0, 0); - -static const Pose3 pose1(Rot3(Vector3(1, -1, -1).asDiagonal()), - Point3(0, 0, 0.5)); - -static const SimpleCamera camera(pose1, K); - -static const Point3 point1(-0.08,-0.08, 0.0); -static const Point3 point2(-0.08, 0.08, 0.0); -static const Point3 point3( 0.08, 0.08, 0.0); -static const Point3 point4( 0.08,-0.08, 0.0); - -/* ************************************************************************* */ -TEST( SimpleCamera, constructor) -{ - CHECK(assert_equal( camera.calibration(), K)); - CHECK(assert_equal( camera.pose(), pose1)); -} - -/* ************************************************************************* */ -TEST( SimpleCamera, level2) -{ - // Create a level camera, looking in Y-direction - Pose2 pose2(0.4,0.3,M_PI/2.0); - SimpleCamera camera = SimpleCamera::Level(K, pose2, 0.1); - - // expected - Point3 x(1,0,0),y(0,0,-1),z(0,1,0); - Rot3 wRc(x,y,z); - Pose3 expected(wRc,Point3(0.4,0.3,0.1)); - CHECK(assert_equal( camera.pose(), expected)); -} - -/* ************************************************************************* */ -TEST( SimpleCamera, lookat) -{ - // Create a level camera, looking in Y-direction - Point3 C(10,0,0); - SimpleCamera camera = SimpleCamera::Lookat(C, Point3(0,0,0), Point3(0,0,1)); - - // expected - Point3 xc(0,1,0),yc(0,0,-1),zc(-1,0,0); - Pose3 expected(Rot3(xc,yc,zc),C); - CHECK(assert_equal( camera.pose(), expected)); - - Point3 C2(30,0,10); - SimpleCamera camera2 = SimpleCamera::Lookat(C2, Point3(0,0,0), Point3(0,0,1)); - - Matrix R = camera2.pose().rotation().matrix(); - Matrix I = trans(R)*R; - CHECK(assert_equal(I, I_3x3)); -} - -/* ************************************************************************* */ -TEST( SimpleCamera, project) -{ - CHECK(assert_equal( camera.project(point1), Point2(-100, 100) )); - CHECK(assert_equal( camera.project(point2), Point2(-100, -100) )); - CHECK(assert_equal( camera.project(point3), Point2( 100, -100) )); - CHECK(assert_equal( camera.project(point4), Point2( 100, 100) )); -} - -/* ************************************************************************* */ -TEST( SimpleCamera, backproject) -{ - CHECK(assert_equal( camera.backproject(Point2(-100, 100), 0.5), point1)); - CHECK(assert_equal( camera.backproject(Point2(-100, -100), 0.5), point2)); - CHECK(assert_equal( camera.backproject(Point2( 100, -100), 0.5), point3)); - CHECK(assert_equal( camera.backproject(Point2( 100, 100), 0.5), point4)); -} - -/* ************************************************************************* */ -TEST( SimpleCamera, backproject2) -{ - Point3 origin(0,0,0); - Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera looking down - SimpleCamera camera(Pose3(rot, origin), K); - - Point3 actual = camera.backproject(Point2(0,0), 1.); - Point3 expected(0., 1., 0.); - pair x = camera.projectSafe(expected); - - CHECK(assert_equal(expected, actual)); - CHECK(assert_equal(Point2(0,0), x.first)); - CHECK(x.second); -} - -/* ************************************************************************* */ -static Point2 project2(const Pose3& pose, const Point3& point) { - return SimpleCamera(pose,K).project(point); -} - -TEST( SimpleCamera, Dproject_point_pose) -{ - Matrix Dpose, Dpoint; - Point2 result = camera.project(point1, Dpose, Dpoint, {}); - Matrix numerical_pose = numericalDerivative21(project2, pose1, point1); - Matrix numerical_point = numericalDerivative22(project2, pose1, point1); - CHECK(assert_equal(result, Point2(-100, 100) )); - CHECK(assert_equal(Dpose, numerical_pose, 1e-7)); - CHECK(assert_equal(Dpoint, numerical_point,1e-7)); -} - -/* ************************************************************************* */ -TEST( SimpleCamera, simpleCamera) -{ - Cal3_S2 K(468.2,427.2,91.2,300,200); - Rot3 R( - 0.41380,0.90915,0.04708, - -0.57338,0.22011,0.78917, - 0.70711,-0.35355,0.61237); - Point3 T(1000,2000,1500); - SimpleCamera expected(Pose3(R.inverse(),T),K); - // H&Z example, 2nd edition, page 163 - Matrix P = (Matrix(3,4) << - 3.53553e2, 3.39645e2, 2.77744e2, -1.44946e6, - -1.03528e2, 2.33212e1, 4.59607e2, -6.32525e5, - 7.07107e-1, -3.53553e-1,6.12372e-1, -9.18559e2).finished(); - SimpleCamera actual = simpleCamera(P); - // Note precision of numbers given in book - CHECK(assert_equal(expected, actual,1e-1)); -} - -#endif - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */ - - From 97750a136a386ed05ce97a0e8d3316e6cefbeec0 Mon Sep 17 00:00:00 2001 From: Kartik Arcot Date: Sat, 21 Jan 2023 13:52:47 -0800 Subject: [PATCH 34/38] use {} --- gtsam/geometry/tests/testUnit3.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp index 66801971f..29d4aee5e 100644 --- a/gtsam/geometry/tests/testUnit3.cpp +++ b/gtsam/geometry/tests/testUnit3.cpp @@ -74,12 +74,12 @@ TEST(Unit3, rotate) { // Use numerical derivatives to calculate the expected Jacobian { expectedH = numericalDerivative21(rotate_, R, p); - R.rotate(p, actualH, nullptr); + R.rotate(p, actualH, {}); EXPECT(assert_equal(expectedH, actualH, 1e-5)); } { expectedH = numericalDerivative22(rotate_, R, p); - R.rotate(p, nullptr, actualH); + R.rotate(p, {}, actualH); EXPECT(assert_equal(expectedH, actualH, 1e-5)); } } @@ -100,12 +100,12 @@ TEST(Unit3, unrotate) { // Use numerical derivatives to calculate the expected Jacobian { expectedH = numericalDerivative21(unrotate_, R, p); - R.unrotate(p, actualH, nullptr); + R.unrotate(p, actualH, {}); EXPECT(assert_equal(expectedH, actualH, 1e-5)); } { expectedH = numericalDerivative22(unrotate_, R, p); - R.unrotate(p, nullptr, actualH); + R.unrotate(p, {}, actualH); EXPECT(assert_equal(expectedH, actualH, 1e-5)); } } @@ -184,7 +184,7 @@ TEST(Unit3, error2) { std::bind(&Unit3::errorVector, std::placeholders::_1, std::placeholders::_2, nullptr, nullptr), p, q); - p.errorVector(q, actual, nullptr); + p.errorVector(q, actual, {}); EXPECT(assert_equal(expected, actual, 1e-5)); } { @@ -192,7 +192,7 @@ TEST(Unit3, error2) { std::bind(&Unit3::errorVector, std::placeholders::_1, std::placeholders::_2, nullptr, nullptr), p, r); - p.errorVector(r, actual, nullptr); + p.errorVector(r, actual, {}); EXPECT(assert_equal(expected, actual, 1e-5)); } { @@ -200,7 +200,7 @@ TEST(Unit3, error2) { std::bind(&Unit3::errorVector, std::placeholders::_1, std::placeholders::_2, nullptr, nullptr), p, q); - p.errorVector(q, nullptr, actual); + p.errorVector(q, {}, actual); EXPECT(assert_equal(expected, actual, 1e-5)); } { @@ -208,7 +208,7 @@ TEST(Unit3, error2) { std::bind(&Unit3::errorVector, std::placeholders::_1, std::placeholders::_2, nullptr, nullptr), p, r); - p.errorVector(r, nullptr, actual); + p.errorVector(r, {}, actual); EXPECT(assert_equal(expected, actual, 1e-5)); } } From 00e7cd1e0266c818a5e6dc78a5c27dda1ede3af3 Mon Sep 17 00:00:00 2001 From: Kartik Arcot Date: Sat, 21 Jan 2023 14:08:56 -0800 Subject: [PATCH 35/38] hope this resolves ci issue --- gtsam/base/tests/testStdOptionalSerialization.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/gtsam/base/tests/testStdOptionalSerialization.cpp b/gtsam/base/tests/testStdOptionalSerialization.cpp index a5aa815cd..dd99b0f12 100644 --- a/gtsam/base/tests/testStdOptionalSerialization.cpp +++ b/gtsam/base/tests/testStdOptionalSerialization.cpp @@ -60,6 +60,8 @@ public: TestOptionalStruct() = default; TestOptionalStruct(const int& opt) : opt(opt) {} + // A copy constructor is needed for serialization + TestOptionalStruct(const TestOptionalStruct& other) = default; bool operator==(const TestOptionalStruct& other) const { // check the values are equal return *opt == *other.opt; From 0517eee084485e09b8e8b2cd7e617d83f7c4d2c5 Mon Sep 17 00:00:00 2001 From: Kartik Arcot Date: Sat, 21 Jan 2023 20:52:20 -0800 Subject: [PATCH 36/38] risky code added --- gtsam/base/std_optional_serialization.h | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/gtsam/base/std_optional_serialization.h b/gtsam/base/std_optional_serialization.h index f285ae931..f8c351b9c 100644 --- a/gtsam/base/std_optional_serialization.h +++ b/gtsam/base/std_optional_serialization.h @@ -17,6 +17,19 @@ #include #include #include + +//!!!!!!!!! I don't completely understand or know if this is correct but compilation works!!!!!!!!!!! +#ifdef __GNUC__ +#if __GNUC__ >= 7 && __cplusplus >= 201703L +namespace boost { namespace serialization { struct U; } } +namespace std { template<> struct is_trivially_default_constructible : std::false_type {}; } +namespace std { template<> struct is_trivially_copy_constructible : std::false_type {}; } +namespace std { template<> struct is_trivially_move_constructible : std::false_type {}; } +#endif +#endif +//!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! + + // function specializations must be defined in the appropriate // namespace - boost::serialization namespace boost { From c4c15facbab206a35e65769d98fb2ed987af4e66 Mon Sep 17 00:00:00 2001 From: Kartik Arcot Date: Sat, 21 Jan 2023 22:29:44 -0800 Subject: [PATCH 37/38] better documentation --- gtsam/base/std_optional_serialization.h | 21 +++++++++++++++++++-- 1 file changed, 19 insertions(+), 2 deletions(-) diff --git a/gtsam/base/std_optional_serialization.h b/gtsam/base/std_optional_serialization.h index f8c351b9c..d4ed4f53d 100644 --- a/gtsam/base/std_optional_serialization.h +++ b/gtsam/base/std_optional_serialization.h @@ -18,7 +18,25 @@ #include #include -//!!!!!!!!! I don't completely understand or know if this is correct but compilation works!!!!!!!!!!! +/** A bunch of declarations to deal with gcc bug + * The compiler has a difficult time distinguisihing between: + * + * template