diff --git a/doc/CodingGuidelines.lyx b/doc/CodingGuidelines.lyx index 6d0b6eb1e..ce083df6c 100644 --- a/doc/CodingGuidelines.lyx +++ b/doc/CodingGuidelines.lyx @@ -271,7 +271,7 @@ color{red}{// Make 'private' any typedefs that must be redefined in derived \begin_layout Plain Layout - typedef boost::shared_ptr shared_ptr; ///< Shared pointer to + typedef std::shared_ptr shared_ptr; ///< Shared pointer to this \end_layout @@ -304,7 +304,7 @@ color{red}{// Make 'public' the typedefs that will be valid in the derived \begin_layout Plain Layout - typedef boost::shared_ptr sharedFactor; ///< Shared pointer + typedef std::shared_ptr sharedFactor; ///< Shared pointer to a factor \end_layout diff --git a/examples/CameraResectioning.cpp b/examples/CameraResectioning.cpp index 3e5e40374..dc451cfe3 100644 --- a/examples/CameraResectioning.cpp +++ b/examples/CameraResectioning.cpp @@ -20,7 +20,6 @@ #include #include #include -#include using namespace gtsam; using namespace gtsam::noiseModel; @@ -70,7 +69,7 @@ int main(int argc, char* argv[]) { /* 2. add factors to the graph */ // add measurement factors SharedDiagonal measurementNoise = Diagonal::Sigmas(Vector2(0.5, 0.5)); - boost::shared_ptr factor; + std::shared_ptr factor; graph.emplace_shared(measurementNoise, X(1), calib, Point2(55, 45), Point3(10, 10, 0)); graph.emplace_shared(measurementNoise, X(1), calib, diff --git a/examples/CombinedImuFactorsExample.cpp b/examples/CombinedImuFactorsExample.cpp index e0396ee81..6ce4f76fa 100644 --- a/examples/CombinedImuFactorsExample.cpp +++ b/examples/CombinedImuFactorsExample.cpp @@ -95,7 +95,7 @@ Vector10 readInitialState(ifstream& file) { return initial_state; } -boost::shared_ptr imuParams() { +std::shared_ptr imuParams() { // We use the sensor specs to build the noise model for the IMU factor. double accel_noise_sigma = 0.0003924; double gyro_noise_sigma = 0.000205689024915; diff --git a/examples/FisheyeExample.cpp b/examples/FisheyeExample.cpp index fc0aed0d7..12c787625 100644 --- a/examples/FisheyeExample.cpp +++ b/examples/FisheyeExample.cpp @@ -61,7 +61,7 @@ using symbol_shorthand::X; // for poses /* ************************************************************************* */ int main(int argc, char *argv[]) { // Define the camera calibration parameters - auto K = boost::make_shared( + auto K = std::make_shared( 278.66, 278.48, 0.0, 319.75, 241.96, -0.013721808247486035, 0.020727425669427896, -0.012786476702685545, 0.0025242267320687625); diff --git a/examples/ImuFactorsExample.cpp b/examples/ImuFactorsExample.cpp index ba4d5c974..342f1f220 100644 --- a/examples/ImuFactorsExample.cpp +++ b/examples/ImuFactorsExample.cpp @@ -82,7 +82,7 @@ po::variables_map parseOptions(int argc, char* argv[]) { return vm; } -boost::shared_ptr imuParams() { +std::shared_ptr imuParams() { // We use the sensor specs to build the noise model for the IMU factor. double accel_noise_sigma = 0.0003924; double gyro_noise_sigma = 0.000205689024915; diff --git a/examples/ImuFactorsExample2.cpp b/examples/ImuFactorsExample2.cpp index 0031acbe8..742b28e4e 100644 --- a/examples/ImuFactorsExample2.cpp +++ b/examples/ImuFactorsExample2.cpp @@ -115,7 +115,7 @@ int main(int argc, char* argv[]) { Vector6 covvec; covvec << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1; auto cov = noiseModel::Diagonal::Variances(covvec); - auto f = boost::make_shared >( + auto f = std::make_shared >( b1, b2, imuBias::ConstantBias(), cov); newgraph.add(f); initialEstimate.insert(biasKey, imuBias::ConstantBias()); diff --git a/examples/LocalizationExample.cpp b/examples/LocalizationExample.cpp index be13af210..ebe7d7f8c 100644 --- a/examples/LocalizationExample.cpp +++ b/examples/LocalizationExample.cpp @@ -76,7 +76,7 @@ class UnaryFactor: public NoiseModelFactorN { using NoiseModelFactor1::evaluateError; /// shorthand for a smart pointer to a factor - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; // The constructor requires the variable key, the (X, Y) measurement value, and the noise model UnaryFactor(Key j, double x, double y, const SharedNoiseModel& model): @@ -105,7 +105,7 @@ class UnaryFactor: public NoiseModelFactorN { // circumstances, the following code that employs the default copy constructor should // work fine. gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new UnaryFactor(*this))); } // Additionally, we encourage you the use of unit testing your custom factors, diff --git a/examples/Pose2SLAMwSPCG.cpp b/examples/Pose2SLAMwSPCG.cpp index cd74b2c07..37e8ec8a8 100644 --- a/examples/Pose2SLAMwSPCG.cpp +++ b/examples/Pose2SLAMwSPCG.cpp @@ -69,7 +69,7 @@ int main(int argc, char** argv) { // addition, the *type* of the iterativeParams decides on the type of // iterative solver, in this case the SPCG (subgraph PCG) parameters.linearSolverType = NonlinearOptimizerParams::Iterative; - parameters.iterativeParams = boost::make_shared(); + parameters.iterativeParams = std::make_shared(); LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, parameters); Values result = optimizer.optimize(); diff --git a/examples/Pose3SLAMExample_changeKeys.cpp b/examples/Pose3SLAMExample_changeKeys.cpp index 7da83d211..392b1c417 100644 --- a/examples/Pose3SLAMExample_changeKeys.cpp +++ b/examples/Pose3SLAMExample_changeKeys.cpp @@ -65,9 +65,9 @@ int main(const int argc, const char *argv[]) { simpleInitial.insert(key, initial->at(k)); } NonlinearFactorGraph simpleGraph; - for(const boost::shared_ptr& factor: *graph) { - boost::shared_ptr > pose3Between = - boost::dynamic_pointer_cast >(factor); + for(const std::shared_ptr& factor: *graph) { + std::shared_ptr > pose3Between = + std::dynamic_pointer_cast >(factor); if (pose3Between){ Key key1, key2; if(add){ diff --git a/examples/SFMExample_SmartFactor.cpp b/examples/SFMExample_SmartFactor.cpp index c3400a0f9..0cd2e77bf 100644 --- a/examples/SFMExample_SmartFactor.cpp +++ b/examples/SFMExample_SmartFactor.cpp @@ -112,7 +112,7 @@ int main(int argc, char* argv[]) { for (size_t j = 0; j < points.size(); ++j) { // The graph stores Factor shared_ptrs, so we cast back to a SmartFactor first - SmartFactor::shared_ptr smart = boost::dynamic_pointer_cast(graph[j]); + SmartFactor::shared_ptr smart = std::dynamic_pointer_cast(graph[j]); if (smart) { // The output of point() is in std::optional, as sometimes // the triangulation operation inside smart factor will encounter degeneracy. diff --git a/examples/SFMExample_SmartFactorPCG.cpp b/examples/SFMExample_SmartFactorPCG.cpp index 2f03a4ef0..a913d32ad 100644 --- a/examples/SFMExample_SmartFactorPCG.cpp +++ b/examples/SFMExample_SmartFactorPCG.cpp @@ -93,9 +93,9 @@ int main(int argc, char* argv[]) { parameters.relativeErrorTol = 1e-10; parameters.maxIterations = 500; PCGSolverParameters::shared_ptr pcg = - boost::make_shared(); + std::make_shared(); pcg->preconditioner_ = - boost::make_shared(); + std::make_shared(); // Following is crucial: pcg->setEpsilon_abs(1e-10); pcg->setEpsilon_rel(1e-10); @@ -108,7 +108,7 @@ int main(int argc, char* argv[]) { result.print("Final results:\n"); Values landmark_result; for (size_t j = 0; j < points.size(); ++j) { - auto smart = boost::dynamic_pointer_cast(graph[j]); + auto smart = std::dynamic_pointer_cast(graph[j]); if (smart) { std::optional point = smart->point(result); if (point) // ignore if std::optional return nullptr diff --git a/examples/SolverComparer.cpp b/examples/SolverComparer.cpp index 3fffc31d0..4cbaaf617 100644 --- a/examples/SolverComparer.cpp +++ b/examples/SolverComparer.cpp @@ -79,7 +79,7 @@ double chi2_red(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& c // the factor graph already includes a factor for the prior/equality constraint. // double dof = graph.size() - config.size(); int graph_dim = 0; - for(const boost::shared_ptr& nlf: graph) { + for(const std::shared_ptr& nlf: graph) { graph_dim += (int)nlf->dim(); } double dof = double(graph_dim) - double(config.dim()); // kaess: changed to dim @@ -259,7 +259,7 @@ void runIncremental() while(nextMeasurement < datasetMeasurements.size()) { if(BetweenFactor::shared_ptr factor = - boost::dynamic_pointer_cast >(datasetMeasurements[nextMeasurement])) + std::dynamic_pointer_cast >(datasetMeasurements[nextMeasurement])) { Key key1 = factor->key<1>(), key2 = factor->key<2>(); if(((int)key1 >= firstStep && key1 < key2) || ((int)key2 >= firstStep && key2 < key1)) { @@ -310,7 +310,7 @@ void runIncremental() NonlinearFactor::shared_ptr measurementf = datasetMeasurements[nextMeasurement]; if(BetweenFactor::shared_ptr factor = - boost::dynamic_pointer_cast >(measurementf)) + std::dynamic_pointer_cast >(measurementf)) { // Stop collecting measurements that are for future steps if(factor->key<1>() > step || factor->key<2>() > step) @@ -346,7 +346,7 @@ void runIncremental() } } else if(BearingRangeFactor::shared_ptr factor = - boost::dynamic_pointer_cast >(measurementf)) + std::dynamic_pointer_cast >(measurementf)) { Key poseKey = factor->keys()[0], lmKey = factor->keys()[1]; diff --git a/examples/TriangulationLOSTExample.cpp b/examples/TriangulationLOSTExample.cpp index 4bb280dd1..417c37c45 100644 --- a/examples/TriangulationLOSTExample.cpp +++ b/examples/TriangulationLOSTExample.cpp @@ -122,7 +122,7 @@ int main(int argc, char* argv[]) { Matrix errorsDLTOpt = Matrix::Zero(nrTrials, 3); double rank_tol = 1e-9; - boost::shared_ptr calib = boost::make_shared(); + std::shared_ptr calib = std::make_shared(); std::chrono::nanoseconds durationDLT; std::chrono::nanoseconds durationDLTOpt; std::chrono::nanoseconds durationLOST; diff --git a/gtsam/base/DSFVector.cpp b/gtsam/base/DSFVector.cpp index 95e8832e0..75a4ef3d7 100644 --- a/gtsam/base/DSFVector.cpp +++ b/gtsam/base/DSFVector.cpp @@ -17,8 +17,8 @@ */ #include -#include #include +#include using namespace std; @@ -26,14 +26,14 @@ namespace gtsam { /* ************************************************************************* */ DSFBase::DSFBase(const size_t numNodes) { - v_ = boost::make_shared < V > (numNodes); + v_ = std::make_shared < V > (numNodes); int index = 0; for (V::iterator it = v_->begin(); it != v_->end(); it++, index++) *it = index; } /* ************************************************************************* */ -DSFBase::DSFBase(const boost::shared_ptr& v_in) { +DSFBase::DSFBase(const std::shared_ptr& v_in) { v_ = v_in; int index = 0; for (V::iterator it = v_->begin(); it != v_->end(); it++, index++) @@ -69,7 +69,7 @@ DSFVector::DSFVector(const std::vector& keys) : } /* ************************************************************************* */ -DSFVector::DSFVector(const boost::shared_ptr& v_in, +DSFVector::DSFVector(const std::shared_ptr& v_in, const std::vector& keys) : DSFBase(v_in), keys_(keys) { assert(*(std::max_element(keys.begin(), keys.end()))size()); diff --git a/gtsam/base/DSFVector.h b/gtsam/base/DSFVector.h index 9c22e012e..a8b12764f 100644 --- a/gtsam/base/DSFVector.h +++ b/gtsam/base/DSFVector.h @@ -21,7 +21,7 @@ #include #include -#include +#include #include #include @@ -41,14 +41,14 @@ public: typedef std::vector V; ///< Vector of ints private: - boost::shared_ptr v_;///< Stores parent pointers, representative iff v[i]==i + std::shared_ptr v_;///< Stores parent pointers, representative iff v[i]==i public: /// Constructor that allocates new memory, allows for keys 0...numNodes-1. DSFBase(const size_t numNodes); /// Constructor that uses an existing, pre-allocated vector. - DSFBase(const boost::shared_ptr& v_in); + DSFBase(const std::shared_ptr& v_in); /// Find the label of the set in which {key} lives. size_t find(size_t key) const; @@ -74,7 +74,7 @@ public: DSFVector(const std::vector& keys); /// Constructor that uses existing vectors. - DSFVector(const boost::shared_ptr& v_in, const std::vector& keys); + DSFVector(const std::shared_ptr& v_in, const std::vector& keys); // All operations below loop over all keys and hence are *at least* O(n) diff --git a/gtsam/base/GenericValue.h b/gtsam/base/GenericValue.h index e3d6ae39c..f83a067fb 100644 --- a/gtsam/base/GenericValue.h +++ b/gtsam/base/GenericValue.h @@ -23,7 +23,6 @@ #include #include -#include #include #include @@ -114,8 +113,8 @@ public: /** * Clone this value (normal clone on the heap, delete with 'delete' operator) */ - boost::shared_ptr clone() const override { - return boost::allocate_shared(Eigen::aligned_allocator(), *this); + std::shared_ptr clone() const override { + return std::allocate_shared(Eigen::aligned_allocator(), *this); } /// Generic Value interface version of retract diff --git a/gtsam/base/Value.h b/gtsam/base/Value.h index 0d2c601cd..f824b388e 100644 --- a/gtsam/base/Value.h +++ b/gtsam/base/Value.h @@ -21,7 +21,6 @@ #include // Configuration from CMake #include -#include #include #include #include @@ -45,7 +44,7 @@ namespace gtsam { virtual void deallocate_() const = 0; /** Clone this value (normal clone on the heap, delete with 'delete' operator) */ - virtual boost::shared_ptr clone() const = 0; + virtual std::shared_ptr clone() const = 0; /** Compare this Value with another for equality. */ virtual bool equals_(const Value& other, double tol = 1e-9) const = 0; diff --git a/gtsam/base/make_shared.h b/gtsam/base/make_shared.h index 164bae77a..98a4a7ef7 100644 --- a/gtsam/base/make_shared.h +++ b/gtsam/base/make_shared.h @@ -21,7 +21,6 @@ #include -#include #include @@ -34,7 +33,7 @@ namespace gtsam { namespace gtsam { /** - * Add our own `make_shared` as a layer of wrapping on `boost::make_shared` + * Add our own `make_shared` as a layer of wrapping on `std::make_shared` * This solves the problem with the stock `make_shared` that custom alignment is not respected, causing SEGFAULTs * at runtime, which is notoriously hard to debug. * @@ -46,22 +45,22 @@ namespace gtsam { * * This function declaration will only be taken when the above condition is true, so if some object does not need to * be aligned, `gtsam::make_shared` will fall back to the next definition, which is a simple wrapper for - * `boost::make_shared`. + * `std::make_shared`. * * @tparam T The type of object being constructed * @tparam Args Type of the arguments of the constructor * @param args Arguments of the constructor - * @return The object created as a boost::shared_ptr + * @return The object created as a std::shared_ptr */ template - gtsam::enable_if_t::value, boost::shared_ptr> make_shared(Args &&... args) { - return boost::allocate_shared(Eigen::aligned_allocator(), std::forward(args)...); + gtsam::enable_if_t::value, std::shared_ptr> make_shared(Args &&... args) { + return std::allocate_shared(Eigen::aligned_allocator(), std::forward(args)...); } /// Fall back to the boost version if no need for alignment template - gtsam::enable_if_t::value, boost::shared_ptr> make_shared(Args &&... args) { - return boost::make_shared(std::forward(args)...); + gtsam::enable_if_t::value, std::shared_ptr> make_shared(Args &&... args) { + return std::make_shared(std::forward(args)...); } } diff --git a/gtsam/base/std_optional_serialization.h b/gtsam/base/std_optional_serialization.h index d4ed4f53d..f59c71cd3 100644 --- a/gtsam/base/std_optional_serialization.h +++ b/gtsam/base/std_optional_serialization.h @@ -1,6 +1,13 @@ -// Functionality to serialize std::optional to boost::archive -// Following this: -// PR: https://github.com/boostorg/serialization/pull/148/files# +/* ---------------------------------------------------------------------------- +* Use, modification and distribution is subject to the Boost Software +* License, Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at +* http://www.boost.org/LICENSE_1_0.txt) + +* See http://www.boost.org for updates, documentation, and revision history. + +* Functionality to serialize std::optional to boost::archive +* Inspired from this PR: https://github.com/boostorg/serialization/pull/163 +* ---------------------------------------------------------------------------- */ #pragma once #include @@ -88,8 +95,6 @@ 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/testDSFVector.cpp b/gtsam/base/tests/testDSFVector.cpp index b50ddde33..c626f941c 100644 --- a/gtsam/base/tests/testDSFVector.cpp +++ b/gtsam/base/tests/testDSFVector.cpp @@ -20,7 +20,6 @@ #include -#include #include #include @@ -85,7 +84,7 @@ TEST(DSFBase, mergePairwiseMatches) { /* ************************************************************************* */ TEST(DSFVector, merge2) { - boost::shared_ptr v = boost::make_shared(5); + std::shared_ptr v = std::make_shared(5); const std::vector keys {1, 3}; DSFVector dsf(v, keys); dsf.merge(1,3); diff --git a/gtsam/base/tests/testTreeTraversal.cpp b/gtsam/base/tests/testTreeTraversal.cpp index 147f4f030..74660bb17 100644 --- a/gtsam/base/tests/testTreeTraversal.cpp +++ b/gtsam/base/tests/testTreeTraversal.cpp @@ -21,13 +21,12 @@ #include #include -#include -#include +#include using namespace gtsam; struct TestNode { - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; int data; std::vector children; TestNode() : data(-1) {} @@ -48,11 +47,11 @@ TestForest makeTestForest() { // | // 4 TestForest forest; - forest.roots_.push_back(boost::make_shared(0)); - forest.roots_.push_back(boost::make_shared(1)); - forest.roots_[0]->children.push_back(boost::make_shared(2)); - forest.roots_[0]->children.push_back(boost::make_shared(3)); - forest.roots_[0]->children[1]->children.push_back(boost::make_shared(4)); + forest.roots_.push_back(std::make_shared(0)); + forest.roots_.push_back(std::make_shared(1)); + forest.roots_[0]->children.push_back(std::make_shared(2)); + forest.roots_[0]->children.push_back(std::make_shared(3)); + forest.roots_[0]->children[1]->children.push_back(std::make_shared(4)); return forest; } diff --git a/gtsam/base/timing.cpp b/gtsam/base/timing.cpp index 4b41ea937..3291415b8 100644 --- a/gtsam/base/timing.cpp +++ b/gtsam/base/timing.cpp @@ -34,9 +34,9 @@ namespace gtsam { namespace internal { -GTSAM_EXPORT boost::shared_ptr gTimingRoot( +GTSAM_EXPORT std::shared_ptr gTimingRoot( new TimingOutline("Total", getTicTocID("Total"))); -GTSAM_EXPORT boost::weak_ptr gCurrentTimer(gTimingRoot); +GTSAM_EXPORT std::weak_ptr gCurrentTimer(gTimingRoot); /* ************************************************************************* */ // Implementation of TimingOutline @@ -83,7 +83,7 @@ void TimingOutline::print(const std::string& outline) const { << n_ << " times, " << wall() << " wall, " << secs() << " children, min: " << min() << " max: " << max() << ")\n"; // Order children - typedef FastMap > ChildOrder; + typedef FastMap > ChildOrder; ChildOrder childOrder; for(const ChildMap::value_type& child: children_) { childOrder[child.second->myOrder_] = child.second; @@ -141,10 +141,10 @@ void TimingOutline::print2(const std::string& outline, } /* ************************************************************************* */ -const boost::shared_ptr& TimingOutline::child(size_t child, - const std::string& label, const boost::weak_ptr& thisPtr) { +const std::shared_ptr& TimingOutline::child(size_t child, + const std::string& label, const std::weak_ptr& thisPtr) { assert(thisPtr.lock().get() == this); - boost::shared_ptr& result = children_[child]; + std::shared_ptr& result = children_[child]; if (!result) { // Create child if necessary result.reset(new TimingOutline(label, child)); @@ -236,7 +236,7 @@ size_t getTicTocID(const char *descriptionC) { /* ************************************************************************* */ void tic(size_t id, const char *labelC) { const std::string label(labelC); - boost::shared_ptr node = // + std::shared_ptr node = // gCurrentTimer.lock()->child(id, label, gCurrentTimer); gCurrentTimer = node; node->tic(); @@ -244,7 +244,7 @@ void tic(size_t id, const char *labelC) { /* ************************************************************************* */ void toc(size_t id, const char *label) { - boost::shared_ptr current(gCurrentTimer.lock()); + std::shared_ptr current(gCurrentTimer.lock()); if (id != current->id_) { gTimingRoot->print(); throw std::invalid_argument( diff --git a/gtsam/base/timing.h b/gtsam/base/timing.h index db0060bfb..33236d8e1 100644 --- a/gtsam/base/timing.h +++ b/gtsam/base/timing.h @@ -21,8 +21,6 @@ #include #include // for GTSAM_USE_TBB -#include -#include #include #include @@ -157,8 +155,8 @@ namespace gtsam { std::string label_; // Tree structure - boost::weak_ptr parent_; ///< parent pointer - typedef FastMap > ChildMap; + std::weak_ptr parent_; ///< parent pointer + typedef FastMap > ChildMap; ChildMap children_; ///< subtrees #ifdef GTSAM_USING_NEW_BOOST_TIMERS @@ -184,8 +182,8 @@ namespace gtsam { double mean() const { return self() / double(n_); } ///< mean self time, in seconds GTSAM_EXPORT void print(const std::string& outline = "") const; GTSAM_EXPORT void print2(const std::string& outline = "", const double parentTotal = -1.0) const; - GTSAM_EXPORT const boost::shared_ptr& - child(size_t child, const std::string& label, const boost::weak_ptr& thisPtr); + GTSAM_EXPORT const std::shared_ptr& + child(size_t child, const std::string& label, const std::weak_ptr& thisPtr); GTSAM_EXPORT void tic(); GTSAM_EXPORT void toc(); GTSAM_EXPORT void finishedIteration(); @@ -216,8 +214,8 @@ namespace gtsam { } }; - GTSAM_EXTERN_EXPORT boost::shared_ptr gTimingRoot; - GTSAM_EXTERN_EXPORT boost::weak_ptr gCurrentTimer; + GTSAM_EXTERN_EXPORT std::shared_ptr gTimingRoot; + GTSAM_EXTERN_EXPORT std::weak_ptr gCurrentTimer; } // Tic and toc functions that are always active (whether or not ENABLE_TIMING is defined) @@ -260,7 +258,7 @@ inline void tictoc_print2_() { // get a node by label and assign it to variable #define tictoc_getNode(variable, label) \ static const size_t label##_id_getnode = ::gtsam::internal::getTicTocID(#label); \ - const boost::shared_ptr variable = \ + const std::shared_ptr variable = \ ::gtsam::internal::gCurrentTimer.lock()->child(label##_id_getnode, #label, ::gtsam::internal::gCurrentTimer); // reset diff --git a/gtsam/base/treeTraversal-inst.h b/gtsam/base/treeTraversal-inst.h index be45a248e..4c6d74806 100644 --- a/gtsam/base/treeTraversal-inst.h +++ b/gtsam/base/treeTraversal-inst.h @@ -27,8 +27,7 @@ #include #include #include -#include -#include +#include namespace gtsam { @@ -41,10 +40,10 @@ namespace { template struct TraversalNode { bool expanded; - const boost::shared_ptr& treeNode; + const std::shared_ptr& treeNode; DATA& parentData; typename FastList::iterator dataPointer; - TraversalNode(const boost::shared_ptr& _treeNode, DATA& _parentData) : + TraversalNode(const std::shared_ptr& _treeNode, DATA& _parentData) : expanded(false), treeNode(_treeNode), parentData(_parentData) { } }; @@ -52,7 +51,7 @@ struct TraversalNode { // Do nothing - default argument for post-visitor for tree traversal struct no_op { template - void operator()(const boost::shared_ptr& node, const DATA& data) { + void operator()(const std::shared_ptr& node, const DATA& data) { } }; @@ -78,7 +77,7 @@ void DepthFirstForest(FOREST& forest, DATA& rootData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost) { // Typedefs typedef typename FOREST::Node Node; - typedef boost::shared_ptr sharedNode; + typedef std::shared_ptr sharedNode; // Depth first traversal stack typedef TraversalNode TraversalNode; @@ -169,29 +168,29 @@ void DepthFirstForestParallel(FOREST& forest, DATA& rootData, /** Traversal function for CloneForest */ namespace { template -boost::shared_ptr CloneForestVisitorPre( - const boost::shared_ptr& node, - const boost::shared_ptr& parentPointer) { +std::shared_ptr CloneForestVisitorPre( + const std::shared_ptr& node, + const std::shared_ptr& parentPointer) { // Clone the current node and add it to its cloned parent - boost::shared_ptr clone = boost::make_shared(*node); + std::shared_ptr clone = std::make_shared(*node); clone->children.clear(); parentPointer->children.push_back(clone); return clone; } } -/** Clone a tree, copy-constructing new nodes (calling boost::make_shared) and setting up child +/** Clone a tree, copy-constructing new nodes (calling std::make_shared) and setting up child * pointers for a clone of the original tree. * @param forest The forest of trees to clone. The method \c forest.roots() should exist and * return a collection of shared pointers to \c FOREST::Node. * @return The new collection of roots. */ template -FastVector > CloneForest( +FastVector > CloneForest( const FOREST& forest) { typedef typename FOREST::Node Node; - boost::shared_ptr rootContainer = boost::make_shared(); + std::shared_ptr rootContainer = std::make_shared(); DepthFirstForest(forest, rootContainer, CloneForestVisitorPre); - return FastVector >(rootContainer->children.begin(), + return FastVector >(rootContainer->children.begin(), rootContainer->children.end()); } @@ -204,7 +203,7 @@ struct PrintForestVisitorPre { formatter(formatter) { } template std::string operator()( - const boost::shared_ptr& node, const std::string& parentString) { + const std::shared_ptr& node, const std::string& parentString) { // Print the current node node->print(parentString + "-", formatter); // Increment the indentation diff --git a/gtsam/base/treeTraversal/parallelTraversalTasks.h b/gtsam/base/treeTraversal/parallelTraversalTasks.h index dc1b45906..41996a350 100644 --- a/gtsam/base/treeTraversal/parallelTraversalTasks.h +++ b/gtsam/base/treeTraversal/parallelTraversalTasks.h @@ -18,8 +18,7 @@ #include -#include -#include +#include #ifdef GTSAM_USE_TBB #include // tbb::task_group @@ -37,8 +36,8 @@ namespace gtsam { class PreOrderTask { public: - const boost::shared_ptr& treeNode; - boost::shared_ptr myData; + const std::shared_ptr& treeNode; + std::shared_ptr myData; VISITOR_PRE& visitorPre; VISITOR_POST& visitorPost; int problemSizeThreshold; @@ -48,7 +47,7 @@ namespace gtsam { // Keep track of order phase across multiple calls to the same functor mutable bool isPostOrderPhase; - PreOrderTask(const boost::shared_ptr& treeNode, const boost::shared_ptr& myData, + PreOrderTask(const std::shared_ptr& treeNode, const std::shared_ptr& myData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, int problemSizeThreshold, tbb::task_group& tg, bool makeNewTasks = true) : treeNode(treeNode), @@ -77,12 +76,12 @@ namespace gtsam { // If we have child tasks, start subtasks and wait for them to complete tbb::task_group ctg; - for(const boost::shared_ptr& child: treeNode->children) + for(const std::shared_ptr& child: treeNode->children) { // Process child in a subtask. Important: Run visitorPre before calling // allocate_child so that if visitorPre throws an exception, we will not have // allocated an extra child, this causes a TBB error. - boost::shared_ptr childData = boost::allocate_shared( + std::shared_ptr childData = std::allocate_shared( tbb::scalable_allocator(), visitorPre(child, *myData)); ctg.run(PreOrderTask(child, childData, visitorPre, visitorPost, problemSizeThreshold, ctg, overThreshold)); @@ -107,9 +106,9 @@ namespace gtsam { } } - void processNodeRecursively(const boost::shared_ptr& node, DATA& myData) const + void processNodeRecursively(const std::shared_ptr& node, DATA& myData) const { - for(const boost::shared_ptr& child: node->children) + for(const std::shared_ptr& child: node->children) { DATA childData = visitorPre(child, myData); processNodeRecursively(child, childData); @@ -140,9 +139,9 @@ namespace gtsam { { typedef PreOrderTask PreOrderTask; // Create data and tasks for our children - for(const boost::shared_ptr& root: roots) + for(const std::shared_ptr& root: roots) { - boost::shared_ptr rootData = boost::allocate_shared(tbb::scalable_allocator(), visitorPre(root, myData)); + std::shared_ptr rootData = std::allocate_shared(tbb::scalable_allocator(), visitorPre(root, myData)); tg.run(PreOrderTask(root, rootData, visitorPre, visitorPost, problemSizeThreshold, tg)); } } diff --git a/gtsam/base/treeTraversal/statistics.h b/gtsam/base/treeTraversal/statistics.h index 7a5c7d256..3ab0c9489 100644 --- a/gtsam/base/treeTraversal/statistics.h +++ b/gtsam/base/treeTraversal/statistics.h @@ -55,7 +55,7 @@ namespace gtsam { /* ************************************************************************* */ namespace internal { template - ForestStatistics* statisticsVisitor(const boost::shared_ptr& node, ForestStatistics* stats) + ForestStatistics* statisticsVisitor(const std::shared_ptr& node, ForestStatistics* stats) { (*stats->problemSizeHistogram[node->problemSize()]) ++; (*stats->numberOfChildrenHistogram[(int)node->children.size()]) ++; @@ -63,7 +63,7 @@ namespace gtsam { { int largestProblemSize = 0; int secondLargestProblemSize = 0; - for(const boost::shared_ptr& child: node->children) + for(const std::shared_ptr& child: node->children) { if (child->problemSize() > largestProblemSize) { diff --git a/gtsam/base/types.h b/gtsam/base/types.h index b9f4b0a3e..5b6f053cb 100644 --- a/gtsam/base/types.h +++ b/gtsam/base/types.h @@ -279,7 +279,7 @@ namespace gtsam { /** * A SFINAE trait to mark classes that need special alignment. * - * This is required to make boost::make_shared and etc respect alignment, which is essential for the Python + * This is required to make std::make_shared and etc respect alignment, which is essential for the Python * wrappers to work properly. * * Explanation diff --git a/gtsam/basis/tests/testFourier.cpp b/gtsam/basis/tests/testFourier.cpp index 7a53cfcc9..3a147a9f6 100644 --- a/gtsam/basis/tests/testFourier.cpp +++ b/gtsam/basis/tests/testFourier.cpp @@ -94,7 +94,7 @@ TEST(Basis, Manual) { auto linearizedFactor = predictFactor.linearize(values); auto linearizedJacobianFactor = - boost::dynamic_pointer_cast(linearizedFactor); + std::dynamic_pointer_cast(linearizedFactor); CHECK(linearizedJacobianFactor); // makes sure it's indeed a JacobianFactor EXPECT(assert_equal(linearFactor, *linearizedJacobianFactor, 1e-9)); } diff --git a/gtsam/discrete/DecisionTree-inl.h b/gtsam/discrete/DecisionTree-inl.h index 0726da3dd..da72bf71f 100644 --- a/gtsam/discrete/DecisionTree-inl.h +++ b/gtsam/discrete/DecisionTree-inl.h @@ -23,7 +23,6 @@ #include #include -#include #include #include @@ -183,7 +182,7 @@ namespace gtsam { */ size_t allSame_; - using ChoicePtr = boost::shared_ptr; + using ChoicePtr = std::shared_ptr; public: /// Default constructor for serialization. @@ -207,10 +206,10 @@ namespace gtsam { for(auto branch: f->branches()) { assert(branch->isLeaf()); nrAssignments += - boost::dynamic_pointer_cast(branch)->nrAssignments(); + std::dynamic_pointer_cast(branch)->nrAssignments(); } NodePtr newLeaf( - new Leaf(boost::dynamic_pointer_cast(f0)->constant(), + new Leaf(std::dynamic_pointer_cast(f0)->constant(), nrAssignments)); return newLeaf; } else @@ -387,14 +386,14 @@ namespace gtsam { /// apply unary operator. NodePtr apply(const Unary& op) const override { - auto r = boost::make_shared(label_, *this, op); + auto r = std::make_shared(label_, *this, op); return Unique(r); } /// Apply unary operator with assignment NodePtr apply(const UnaryAssignment& op, const Assignment& assignment) const override { - auto r = boost::make_shared(label_, *this, op, assignment); + auto r = std::make_shared(label_, *this, op, assignment); return Unique(r); } @@ -409,7 +408,7 @@ namespace gtsam { // If second argument of binary op is Leaf node, recurse on branches NodePtr apply_g_op_fL(const Leaf& fL, const Binary& op) const override { - auto h = boost::make_shared(label(), nrChoices()); + auto h = std::make_shared(label(), nrChoices()); for (auto&& branch : branches_) h->push_back(fL.apply_f_op_g(*branch, op)); return Unique(h); @@ -417,14 +416,14 @@ namespace gtsam { // If second argument of binary op is Choice, call constructor NodePtr apply_g_op_fC(const Choice& fC, const Binary& op) const override { - auto h = boost::make_shared(fC, *this, op); + auto h = std::make_shared(fC, *this, op); return Unique(h); } // If second argument of binary op is Leaf template NodePtr apply_fC_op_gL(const Leaf& gL, OP op) const { - auto h = boost::make_shared(label(), nrChoices()); + auto h = std::make_shared(label(), nrChoices()); for (auto&& branch : branches_) h->push_back(branch->apply_f_op_g(gL, op)); return Unique(h); @@ -435,7 +434,7 @@ namespace gtsam { if (label_ == label) return branches_[index]; // choose branch // second case, not label of interest, just recurse - auto r = boost::make_shared(label_, branches_.size()); + auto r = std::make_shared(label_, branches_.size()); for (auto&& branch : branches_) r->push_back(branch->choose(label, index)); return Unique(r); @@ -476,7 +475,7 @@ namespace gtsam { /****************************************************************************/ template DecisionTree::DecisionTree(const L& label, const Y& y1, const Y& y2) { - auto a = boost::make_shared(label, 2); + auto a = std::make_shared(label, 2); NodePtr l1(new Leaf(y1)), l2(new Leaf(y2)); a->push_back(l1); a->push_back(l2); @@ -489,7 +488,7 @@ namespace gtsam { const Y& y2) { if (labelC.second != 2) throw std::invalid_argument( "DecisionTree: binary constructor called with non-binary label"); - auto a = boost::make_shared(labelC.first, 2); + auto a = std::make_shared(labelC.first, 2); NodePtr l1(new Leaf(y1)), l2(new Leaf(y2)); a->push_back(l1); a->push_back(l2); @@ -568,8 +567,8 @@ namespace gtsam { for (Iterator it = begin; it != end; it++) { if (it->root_->isLeaf()) continue; - boost::shared_ptr c = - boost::dynamic_pointer_cast(it->root_); + std::shared_ptr c = + std::dynamic_pointer_cast(it->root_); if (!highestLabel || c->label() > *highestLabel) { highestLabel = c->label(); nrChoices = c->nrChoices(); @@ -578,14 +577,14 @@ namespace gtsam { // if label is already in correct order, just put together a choice on label if (!nrChoices || !highestLabel || label > *highestLabel) { - auto choiceOnLabel = boost::make_shared(label, end - begin); + auto choiceOnLabel = std::make_shared(label, end - begin); for (Iterator it = begin; it != end; it++) choiceOnLabel->push_back(it->root_); return Choice::Unique(choiceOnLabel); } else { // Set up a new choice on the highest label auto choiceOnHighestLabel = - boost::make_shared(*highestLabel, nrChoices); + std::make_shared(*highestLabel, nrChoices); // now, for all possible values of highestLabel for (size_t index = 0; index < nrChoices; index++) { // make a new set of functions for composing by iterating over the given @@ -647,7 +646,7 @@ namespace gtsam { << std::endl; throw std::invalid_argument("DecisionTree::create invalid argument"); } - auto choice = boost::make_shared(begin->first, endY - beginY); + auto choice = std::make_shared(begin->first, endY - beginY); for (ValueIt y = beginY; y != endY; y++) choice->push_back(NodePtr(new Leaf(*y))); return Choice::Unique(choice); @@ -678,13 +677,13 @@ namespace gtsam { // functions. // If leaf, apply unary conversion "op" and create a unique leaf. using MXLeaf = typename DecisionTree::Leaf; - if (auto leaf = boost::dynamic_pointer_cast(f)) { + if (auto leaf = std::dynamic_pointer_cast(f)) { return NodePtr(new Leaf(Y_of_X(leaf->constant()), leaf->nrAssignments())); } // Check if Choice using MXChoice = typename DecisionTree::Choice; - auto choice = boost::dynamic_pointer_cast(f); + auto choice = std::dynamic_pointer_cast(f); if (!choice) throw std::invalid_argument( "DecisionTree::convertFrom: Invalid NodePtr"); @@ -720,11 +719,11 @@ namespace gtsam { /// Do a depth-first visit on the tree rooted at node. void operator()(const typename DecisionTree::NodePtr& node) const { using Leaf = typename DecisionTree::Leaf; - if (auto leaf = boost::dynamic_pointer_cast(node)) + if (auto leaf = std::dynamic_pointer_cast(node)) return f(leaf->constant()); using Choice = typename DecisionTree::Choice; - auto choice = boost::dynamic_pointer_cast(node); + auto choice = std::dynamic_pointer_cast(node); if (!choice) throw std::invalid_argument("DecisionTree::Visit: Invalid NodePtr"); for (auto&& branch : choice->branches()) (*this)(branch); // recurse! @@ -757,11 +756,11 @@ namespace gtsam { /// Do a depth-first visit on the tree rooted at node. void operator()(const typename DecisionTree::NodePtr& node) const { using Leaf = typename DecisionTree::Leaf; - if (auto leaf = boost::dynamic_pointer_cast(node)) + if (auto leaf = std::dynamic_pointer_cast(node)) return f(*leaf); using Choice = typename DecisionTree::Choice; - auto choice = boost::dynamic_pointer_cast(node); + auto choice = std::dynamic_pointer_cast(node); if (!choice) throw std::invalid_argument("DecisionTree::VisitLeaf: Invalid NodePtr"); for (auto&& branch : choice->branches()) (*this)(branch); // recurse! @@ -792,11 +791,11 @@ namespace gtsam { /// Do a depth-first visit on the tree rooted at node. void operator()(const typename DecisionTree::NodePtr& node) { using Leaf = typename DecisionTree::Leaf; - if (auto leaf = boost::dynamic_pointer_cast(node)) + if (auto leaf = std::dynamic_pointer_cast(node)) return f(assignment, leaf->constant()); using Choice = typename DecisionTree::Choice; - auto choice = boost::dynamic_pointer_cast(node); + auto choice = std::dynamic_pointer_cast(node); if (!choice) throw std::invalid_argument("DecisionTree::VisitWith: Invalid NodePtr"); for (size_t i = 0; i < choice->nrChoices(); i++) { diff --git a/gtsam/discrete/DecisionTree.h b/gtsam/discrete/DecisionTree.h index a8764a98f..af6101296 100644 --- a/gtsam/discrete/DecisionTree.h +++ b/gtsam/discrete/DecisionTree.h @@ -24,7 +24,7 @@ #include #include -#include +#include #include #include #include @@ -70,7 +70,7 @@ namespace gtsam { /** ------------------------ Node base class --------------------------- */ struct Node { - using Ptr = boost::shared_ptr; + using Ptr = std::shared_ptr; #ifdef DT_DEBUG_MEMORY static int nrNodes; diff --git a/gtsam/discrete/DecisionTreeFactor.cpp b/gtsam/discrete/DecisionTreeFactor.cpp index 14a24b6e6..399216b83 100644 --- a/gtsam/discrete/DecisionTreeFactor.cpp +++ b/gtsam/discrete/DecisionTreeFactor.cpp @@ -22,7 +22,6 @@ #include #include -#include #include #include @@ -127,7 +126,7 @@ namespace gtsam { Key j = keys()[i]; dkeys.push_back(DiscreteKey(j, cardinality(j))); } - return boost::make_shared(dkeys, result); + return std::make_shared(dkeys, result); } /* ************************************************************************ */ @@ -160,7 +159,7 @@ namespace gtsam { continue; dkeys.push_back(DiscreteKey(j, cardinality(j))); } - return boost::make_shared(dkeys, result); + return std::make_shared(dkeys, result); } /* ************************************************************************ */ diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h index dd292cae8..9150c459a 100644 --- a/gtsam/discrete/DecisionTreeFactor.h +++ b/gtsam/discrete/DecisionTreeFactor.h @@ -24,7 +24,7 @@ #include #include -#include +#include #include #include #include @@ -47,7 +47,7 @@ namespace gtsam { // typedefs needed to play nice with gtsam typedef DecisionTreeFactor This; typedef DiscreteFactor Base; ///< Typedef to base class - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; typedef AlgebraicDecisionTree ADT; protected: diff --git a/gtsam/discrete/DiscreteBayesNet.cpp b/gtsam/discrete/DiscreteBayesNet.cpp index 97734c3bb..451f5f07b 100644 --- a/gtsam/discrete/DiscreteBayesNet.cpp +++ b/gtsam/discrete/DiscreteBayesNet.cpp @@ -20,7 +20,6 @@ #include #include -#include #include namespace gtsam { diff --git a/gtsam/discrete/DiscreteBayesNet.h b/gtsam/discrete/DiscreteBayesNet.h index f3b4a58f5..469bfc768 100644 --- a/gtsam/discrete/DiscreteBayesNet.h +++ b/gtsam/discrete/DiscreteBayesNet.h @@ -23,7 +23,7 @@ #include #include -#include +#include #include #include #include @@ -40,8 +40,8 @@ class GTSAM_EXPORT DiscreteBayesNet: public BayesNet { typedef BayesNet Base; typedef DiscreteBayesNet This; typedef DiscreteConditional ConditionalType; - typedef boost::shared_ptr shared_ptr; - typedef boost::shared_ptr sharedConditional; + typedef std::shared_ptr shared_ptr; + typedef std::shared_ptr sharedConditional; /// @name Standard Constructors /// @{ diff --git a/gtsam/discrete/DiscreteBayesTree.h b/gtsam/discrete/DiscreteBayesTree.h index 03a9a2fc7..f65508a1a 100644 --- a/gtsam/discrete/DiscreteBayesTree.h +++ b/gtsam/discrete/DiscreteBayesTree.h @@ -42,12 +42,12 @@ class GTSAM_EXPORT DiscreteBayesTreeClique typedef DiscreteBayesTreeClique This; typedef BayesTreeCliqueBase Base; - typedef boost::shared_ptr shared_ptr; - typedef boost::weak_ptr weak_ptr; + typedef std::shared_ptr shared_ptr; + typedef std::weak_ptr weak_ptr; DiscreteBayesTreeClique() {} virtual ~DiscreteBayesTreeClique() {} DiscreteBayesTreeClique( - const boost::shared_ptr& conditional) + const std::shared_ptr& conditional) : Base(conditional) {} /// print index signature only @@ -73,7 +73,7 @@ class GTSAM_EXPORT DiscreteBayesTree public: typedef DiscreteBayesTree This; - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; /// @name Standard interface /// @{ diff --git a/gtsam/discrete/DiscreteConditional.cpp b/gtsam/discrete/DiscreteConditional.cpp index b4f58a5bd..c40573ee8 100644 --- a/gtsam/discrete/DiscreteConditional.cpp +++ b/gtsam/discrete/DiscreteConditional.cpp @@ -23,7 +23,6 @@ #include #include -#include #include #include #include @@ -195,7 +194,7 @@ DiscreteConditional::shared_ptr DiscreteConditional::choose( dKeys.emplace_back(j, this->cardinality(j)); } } - return boost::make_shared(nrFrontals(), dKeys, adt); + return std::make_shared(nrFrontals(), dKeys, adt); } /* ************************************************************************** */ @@ -220,7 +219,7 @@ DecisionTreeFactor::shared_ptr DiscreteConditional::likelihood( for (Key j : parents()) { discreteKeys.emplace_back(j, this->cardinality(j)); } - return boost::make_shared(discreteKeys, adt); + return std::make_shared(discreteKeys, adt); } /* ****************************************************************************/ diff --git a/gtsam/discrete/DiscreteConditional.h b/gtsam/discrete/DiscreteConditional.h index 03c44649f..854f5f43d 100644 --- a/gtsam/discrete/DiscreteConditional.h +++ b/gtsam/discrete/DiscreteConditional.h @@ -22,8 +22,7 @@ #include #include -#include -#include +#include #include #include @@ -41,7 +40,7 @@ class GTSAM_EXPORT DiscreteConditional public: // typedefs needed to play nice with gtsam typedef DiscreteConditional This; ///< Typedef to this class - typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class + typedef std::shared_ptr shared_ptr; ///< shared_ptr to this class typedef DecisionTreeFactor BaseFactor; ///< Typedef to our factor base class typedef Conditional BaseConditional; ///< Typedef to our conditional base class diff --git a/gtsam/discrete/DiscreteEliminationTree.h b/gtsam/discrete/DiscreteEliminationTree.h index a28054615..54e4fca44 100644 --- a/gtsam/discrete/DiscreteEliminationTree.h +++ b/gtsam/discrete/DiscreteEliminationTree.h @@ -34,7 +34,7 @@ namespace gtsam { public: typedef EliminationTree Base; ///< Base class typedef DiscreteEliminationTree This; ///< This class - typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class + typedef std::shared_ptr shared_ptr; ///< Shared pointer to this class /** * Build the elimination tree of a factor graph using pre-computed column structure. diff --git a/gtsam/discrete/DiscreteFactor.h b/gtsam/discrete/DiscreteFactor.h index 712e1ff75..b026b1a3a 100644 --- a/gtsam/discrete/DiscreteFactor.h +++ b/gtsam/discrete/DiscreteFactor.h @@ -41,7 +41,7 @@ public: // typedefs needed to play nice with gtsam typedef DiscreteFactor This; ///< This class - typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class + typedef std::shared_ptr shared_ptr; ///< shared_ptr to this class typedef Factor Base; ///< Our base class using Values = DiscreteValues; ///< backwards compatibility diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp index ebcac445c..0501cd12e 100644 --- a/gtsam/discrete/DiscreteFactorGraph.cpp +++ b/gtsam/discrete/DiscreteFactorGraph.cpp @@ -54,7 +54,7 @@ namespace gtsam { DiscreteKeys DiscreteFactorGraph::discreteKeys() const { DiscreteKeys result; for (auto&& factor : *this) { - if (auto p = boost::dynamic_pointer_cast(factor)) { + if (auto p = std::dynamic_pointer_cast(factor)) { DiscreteKeys factor_keys = p->discreteKeys(); result.insert(result.end(), factor_keys.begin(), factor_keys.end()); } @@ -136,12 +136,12 @@ namespace gtsam { // Make lookup with product gttic(lookup); size_t nrFrontals = frontalKeys.size(); - auto lookup = boost::make_shared(nrFrontals, + auto lookup = std::make_shared(nrFrontals, orderedKeys, product); gttoc(lookup); return std::make_pair( - boost::dynamic_pointer_cast(lookup), max); + std::dynamic_pointer_cast(lookup), max); } /* ************************************************************************ */ @@ -220,7 +220,7 @@ namespace gtsam { // now divide product/sum to get conditional gttic(divide); auto conditional = - boost::make_shared(product, *sum, orderedKeys); + std::make_shared(product, *sum, orderedKeys); gttoc(divide); return std::make_pair(conditional, sum); diff --git a/gtsam/discrete/DiscreteFactorGraph.h b/gtsam/discrete/DiscreteFactorGraph.h index 4441b0df4..d698a00c6 100644 --- a/gtsam/discrete/DiscreteFactorGraph.h +++ b/gtsam/discrete/DiscreteFactorGraph.h @@ -25,7 +25,6 @@ #include #include -#include #include #include #include @@ -48,7 +47,7 @@ class DiscreteJunctionTree; * @return GTSAM_EXPORT * @ingroup discrete */ -GTSAM_EXPORT std::pair, DecisionTreeFactor::shared_ptr> +GTSAM_EXPORT std::pair, DecisionTreeFactor::shared_ptr> EliminateDiscrete(const DiscreteFactorGraph& factors, const Ordering& keys); /* ************************************************************************* */ @@ -62,8 +61,8 @@ template<> struct EliminationTraits typedef DiscreteBayesTree BayesTreeType; ///< Type of Bayes tree typedef DiscreteJunctionTree JunctionTreeType; ///< Type of Junction tree /// The default dense elimination function - static std::pair, - boost::shared_ptr > + static std::pair, + std::shared_ptr > DefaultEliminate(const FactorGraphType& factors, const Ordering& keys) { return EliminateDiscrete(factors, keys); } @@ -89,7 +88,7 @@ class GTSAM_EXPORT DiscreteFactorGraph using Base = FactorGraph; ///< base factor graph type using BaseEliminateable = EliminateableFactorGraph; ///< for elimination - using shared_ptr = boost::shared_ptr; ///< shared_ptr to This + using shared_ptr = std::shared_ptr; ///< shared_ptr to This using Values = DiscreteValues; ///< backwards compatibility diff --git a/gtsam/discrete/DiscreteJunctionTree.h b/gtsam/discrete/DiscreteJunctionTree.h index f417cf6fa..385c659e4 100644 --- a/gtsam/discrete/DiscreteJunctionTree.h +++ b/gtsam/discrete/DiscreteJunctionTree.h @@ -53,7 +53,7 @@ namespace gtsam { public: typedef JunctionTree Base; ///< Base class typedef DiscreteJunctionTree This; ///< This class - typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class + typedef std::shared_ptr shared_ptr; ///< Shared pointer to this class /** * Build the elimination tree of a factor graph using precomputed column structure. diff --git a/gtsam/discrete/DiscreteLookupDAG.cpp b/gtsam/discrete/DiscreteLookupDAG.cpp index d96b38b0e..bdc77031c 100644 --- a/gtsam/discrete/DiscreteLookupDAG.cpp +++ b/gtsam/discrete/DiscreteLookupDAG.cpp @@ -106,7 +106,7 @@ DiscreteLookupDAG DiscreteLookupDAG::FromBayesNet( DiscreteLookupDAG dag; for (auto&& conditional : bayesNet) { if (auto lookupTable = - boost::dynamic_pointer_cast(conditional)) { + std::dynamic_pointer_cast(conditional)) { dag.push_back(lookupTable); } else { throw std::runtime_error( diff --git a/gtsam/discrete/DiscreteLookupDAG.h b/gtsam/discrete/DiscreteLookupDAG.h index b7f900073..3b8e6d6db 100644 --- a/gtsam/discrete/DiscreteLookupDAG.h +++ b/gtsam/discrete/DiscreteLookupDAG.h @@ -21,7 +21,7 @@ #include #include -#include +#include #include #include #include @@ -40,7 +40,7 @@ class DiscreteBayesNet; class GTSAM_EXPORT DiscreteLookupTable : public DiscreteConditional { public: using This = DiscreteLookupTable; - using shared_ptr = boost::shared_ptr; + using shared_ptr = std::shared_ptr; using BaseConditional = Conditional; /** @@ -78,7 +78,7 @@ class GTSAM_EXPORT DiscreteLookupDAG : public BayesNet { public: using Base = BayesNet; using This = DiscreteLookupDAG; - using shared_ptr = boost::shared_ptr; + using shared_ptr = std::shared_ptr; /// @name Standard Constructors /// @{ diff --git a/gtsam/discrete/tests/testDecisionTree.cpp b/gtsam/discrete/tests/testDecisionTree.cpp index 2f385263c..3bf12cec1 100644 --- a/gtsam/discrete/tests/testDecisionTree.cpp +++ b/gtsam/discrete/tests/testDecisionTree.cpp @@ -326,7 +326,7 @@ TEST(DecisionTree, NrAssignments) { const std::pair A("A", 2), B("B", 2), C("C", 2); DT tree({A, B, C}, "1 1 1 1 1 1 1 1"); EXPECT(tree.root_->isLeaf()); - auto leaf = boost::dynamic_pointer_cast(tree.root_); + auto leaf = std::dynamic_pointer_cast(tree.root_); EXPECT_LONGS_EQUAL(8, leaf->nrAssignments()); DT tree2({C, B, A}, "1 1 1 2 3 4 5 5"); @@ -344,20 +344,20 @@ TEST(DecisionTree, NrAssignments) { 1 1 Leaf 5 */ - auto root = boost::dynamic_pointer_cast(tree2.root_); + auto root = std::dynamic_pointer_cast(tree2.root_); CHECK(root); - auto choice0 = boost::dynamic_pointer_cast(root->branches()[0]); + auto choice0 = std::dynamic_pointer_cast(root->branches()[0]); CHECK(choice0); EXPECT(choice0->branches()[0]->isLeaf()); - auto choice00 = boost::dynamic_pointer_cast(choice0->branches()[0]); + auto choice00 = std::dynamic_pointer_cast(choice0->branches()[0]); CHECK(choice00); EXPECT_LONGS_EQUAL(2, choice00->nrAssignments()); - auto choice1 = boost::dynamic_pointer_cast(root->branches()[1]); + auto choice1 = std::dynamic_pointer_cast(root->branches()[1]); CHECK(choice1); - auto choice10 = boost::dynamic_pointer_cast(choice1->branches()[0]); + auto choice10 = std::dynamic_pointer_cast(choice1->branches()[0]); CHECK(choice10); - auto choice11 = boost::dynamic_pointer_cast(choice1->branches()[1]); + auto choice11 = std::dynamic_pointer_cast(choice1->branches()[1]); CHECK(choice11); EXPECT(choice11->isLeaf()); EXPECT_LONGS_EQUAL(2, choice11->nrAssignments()); diff --git a/gtsam/discrete/tests/testDiscreteBayesNet.cpp b/gtsam/discrete/tests/testDiscreteBayesNet.cpp index 0f1fb23cd..a17a20d41 100644 --- a/gtsam/discrete/tests/testDiscreteBayesNet.cpp +++ b/gtsam/discrete/tests/testDiscreteBayesNet.cpp @@ -42,13 +42,13 @@ TEST(DiscreteBayesNet, bayesNet) { DiscreteBayesNet bayesNet; DiscreteKey Parent(0, 2), Child(1, 2); - auto prior = boost::make_shared(Parent % "6/4"); + auto prior = std::make_shared(Parent % "6/4"); CHECK(assert_equal(ADT({Parent}, "0.6 0.4"), (ADT)*prior)); bayesNet.push_back(prior); auto conditional = - boost::make_shared(Child | Parent = "7/3 8/2"); + std::make_shared(Child | Parent = "7/3 8/2"); EXPECT_LONGS_EQUAL(1, *(conditional->beginFrontals())); ADT expected(Child & Parent, "0.7 0.8 0.3 0.2"); CHECK(assert_equal(expected, (ADT)*conditional)); diff --git a/gtsam/discrete/tests/testDiscreteBayesTree.cpp b/gtsam/discrete/tests/testDiscreteBayesTree.cpp index 0a7dc72f4..99038566e 100644 --- a/gtsam/discrete/tests/testDiscreteBayesTree.cpp +++ b/gtsam/discrete/tests/testDiscreteBayesTree.cpp @@ -34,7 +34,7 @@ static constexpr bool debug = false; struct TestFixture { vector keys; DiscreteBayesNet bayesNet; - boost::shared_ptr bayesTree; + std::shared_ptr bayesTree; /** * Create a thin-tree Bayesnet, a la Jean-Guillaume Durand (former student), diff --git a/gtsam/discrete/tests/testDiscreteConditional.cpp b/gtsam/discrete/tests/testDiscreteConditional.cpp index 6e73cfc6e..aa393d74c 100644 --- a/gtsam/discrete/tests/testDiscreteConditional.cpp +++ b/gtsam/discrete/tests/testDiscreteConditional.cpp @@ -23,7 +23,6 @@ #include #include -#include using namespace std; using namespace gtsam; diff --git a/gtsam/discrete/tests/testDiscreteMarginals.cpp b/gtsam/discrete/tests/testDiscreteMarginals.cpp index ffca8a481..ad9b1b191 100644 --- a/gtsam/discrete/tests/testDiscreteMarginals.cpp +++ b/gtsam/discrete/tests/testDiscreteMarginals.cpp @@ -119,11 +119,11 @@ TEST_UNSAFE( DiscreteMarginals, truss ) { // bayesTree->print("Bayes Tree"); typedef DiscreteBayesTreeClique Clique; - Clique expected0(boost::make_shared((key[0] | key[2], key[4]) = "2/1 2/1 2/1 2/1")); + Clique expected0(std::make_shared((key[0] | key[2], key[4]) = "2/1 2/1 2/1 2/1")); Clique::shared_ptr actual0 = (*bayesTree)[0]; // EXPECT(assert_equal(expected0, *actual0)); // TODO, correct but fails - Clique expected1(boost::make_shared((key[1] | key[3], key[4]) = "1/2 1/2 1/2 1/2")); + Clique expected1(std::make_shared((key[1] | key[3], key[4]) = "1/2 1/2 1/2 1/2")); Clique::shared_ptr actual1 = (*bayesTree)[1]; // EXPECT(assert_equal(expected1, *actual1)); // TODO, correct but fails @@ -133,12 +133,12 @@ TEST_UNSAFE( DiscreteMarginals, truss ) { // test 0 DecisionTreeFactor expectedM0(key[0],"0.666667 0.333333"); DiscreteFactor::shared_ptr actualM0 = marginals(0); - EXPECT(assert_equal(expectedM0, *boost::dynamic_pointer_cast(actualM0),1e-5)); + EXPECT(assert_equal(expectedM0, *std::dynamic_pointer_cast(actualM0),1e-5)); // test 1 DecisionTreeFactor expectedM1(key[1],"0.333333 0.666667"); DiscreteFactor::shared_ptr actualM1 = marginals(1); - EXPECT(assert_equal(expectedM1, *boost::dynamic_pointer_cast(actualM1),1e-5)); + EXPECT(assert_equal(expectedM1, *std::dynamic_pointer_cast(actualM1),1e-5)); } /* ************************************************************************* */ @@ -187,7 +187,7 @@ TEST_UNSAFE(DiscreteMarginals, truss2) { DecisionTreeFactor expectedM(key[j], table); DiscreteFactor::shared_ptr actualM = marginals(j); EXPECT(assert_equal( - expectedM, *boost::dynamic_pointer_cast(actualM))); + expectedM, *std::dynamic_pointer_cast(actualM))); } } diff --git a/gtsam/geometry/Cal3.h b/gtsam/geometry/Cal3.h index eb7d1ff27..a55010ce0 100644 --- a/gtsam/geometry/Cal3.h +++ b/gtsam/geometry/Cal3.h @@ -75,7 +75,7 @@ class GTSAM_EXPORT Cal3 { public: enum { dimension = 5 }; ///< shared pointer to calibration object - using shared_ptr = boost::shared_ptr; + using shared_ptr = std::shared_ptr; /// @name Standard Constructors /// @{ diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h index 2a32f6297..c12f5448f 100644 --- a/gtsam/geometry/Cal3Bundler.h +++ b/gtsam/geometry/Cal3Bundler.h @@ -42,7 +42,7 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 { enum { dimension = 3 }; ///< shared pointer to stereo calibration object - using shared_ptr = boost::shared_ptr; + using shared_ptr = std::shared_ptr; /// @name Standard Constructors /// @{ diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h index cc406d331..7252f15dc 100644 --- a/gtsam/geometry/Cal3DS2.h +++ b/gtsam/geometry/Cal3DS2.h @@ -21,7 +21,7 @@ #pragma once #include -#include +#include namespace gtsam { @@ -39,7 +39,7 @@ class GTSAM_EXPORT Cal3DS2 : public Cal3DS2_Base { enum { dimension = 9 }; ///< shared pointer to stereo calibration object - using shared_ptr = boost::shared_ptr; + using shared_ptr = std::shared_ptr; /// @name Standard Constructors /// @{ @@ -94,8 +94,8 @@ class GTSAM_EXPORT Cal3DS2 : public Cal3DS2_Base { /// @{ /// @return a deep copy of this object - boost::shared_ptr clone() const override { - return boost::shared_ptr(new Cal3DS2(*this)); + std::shared_ptr clone() const override { + return std::shared_ptr(new Cal3DS2(*this)); } /// @} diff --git a/gtsam/geometry/Cal3DS2_Base.h b/gtsam/geometry/Cal3DS2_Base.h index c8c76dcb9..185521b77 100644 --- a/gtsam/geometry/Cal3DS2_Base.h +++ b/gtsam/geometry/Cal3DS2_Base.h @@ -21,7 +21,7 @@ #include #include -#include +#include namespace gtsam { @@ -49,7 +49,7 @@ class GTSAM_EXPORT Cal3DS2_Base : public Cal3 { enum { dimension = 9 }; ///< shared pointer to stereo calibration object - using shared_ptr = boost::shared_ptr; + using shared_ptr = std::shared_ptr; /// @name Standard Constructors /// @{ @@ -146,8 +146,8 @@ class GTSAM_EXPORT Cal3DS2_Base : public Cal3 { /// @{ /// @return a deep copy of this object - virtual boost::shared_ptr clone() const { - return boost::shared_ptr(new Cal3DS2_Base(*this)); + virtual std::shared_ptr clone() const { + return std::shared_ptr(new Cal3DS2_Base(*this)); } /// @} diff --git a/gtsam/geometry/Cal3Fisheye.h b/gtsam/geometry/Cal3Fisheye.h index 2530231c5..d3faf3a92 100644 --- a/gtsam/geometry/Cal3Fisheye.h +++ b/gtsam/geometry/Cal3Fisheye.h @@ -22,7 +22,7 @@ #include #include -#include +#include #include @@ -57,7 +57,7 @@ class GTSAM_EXPORT Cal3Fisheye : public Cal3 { public: enum { dimension = 9 }; ///< shared pointer to fisheye calibration object - using shared_ptr = boost::shared_ptr; + using shared_ptr = std::shared_ptr; /// @name Standard Constructors /// @{ @@ -174,8 +174,8 @@ class GTSAM_EXPORT Cal3Fisheye : public Cal3 { /// @{ /// @return a deep copy of this object - virtual boost::shared_ptr clone() const { - return boost::shared_ptr(new Cal3Fisheye(*this)); + virtual std::shared_ptr clone() const { + return std::shared_ptr(new Cal3Fisheye(*this)); } /// @} diff --git a/gtsam/geometry/Cal3Unified.h b/gtsam/geometry/Cal3Unified.h index cf25c8f00..de292b3b7 100644 --- a/gtsam/geometry/Cal3Unified.h +++ b/gtsam/geometry/Cal3Unified.h @@ -53,7 +53,7 @@ class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base { enum { dimension = 10 }; ///< shared pointer to stereo calibration object - using shared_ptr = boost::shared_ptr; + using shared_ptr = std::shared_ptr; /// @name Standard Constructors /// @{ diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index b8d776538..ed62c5dd1 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/gtsam/geometry/Cal3_S2.h @@ -36,7 +36,7 @@ class GTSAM_EXPORT Cal3_S2 : public Cal3 { enum { dimension = 5 }; ///< shared pointer to calibration object - using shared_ptr = boost::shared_ptr; + using shared_ptr = std::shared_ptr; /// @name Standard Constructors /// @{ diff --git a/gtsam/geometry/Cal3_S2Stereo.h b/gtsam/geometry/Cal3_S2Stereo.h index 574b5d7b5..39ca1b099 100644 --- a/gtsam/geometry/Cal3_S2Stereo.h +++ b/gtsam/geometry/Cal3_S2Stereo.h @@ -35,7 +35,7 @@ class GTSAM_EXPORT Cal3_S2Stereo : public Cal3_S2 { enum { dimension = 6 }; ///< shared pointer to stereo calibration object - using shared_ptr = boost::shared_ptr; + using shared_ptr = std::shared_ptr; /// @name Standard Constructors /// @{ diff --git a/gtsam/geometry/PinholePose.h b/gtsam/geometry/PinholePose.h index e9d3859a1..813f04aaa 100644 --- a/gtsam/geometry/PinholePose.h +++ b/gtsam/geometry/PinholePose.h @@ -21,7 +21,6 @@ #include #include -#include namespace gtsam { @@ -245,7 +244,7 @@ class PinholePose: public PinholeBaseK { private: typedef PinholeBaseK Base; ///< base class has 3D pose as private member - boost::shared_ptr K_; ///< shared pointer to fixed calibration + std::shared_ptr K_; ///< shared pointer to fixed calibration public: @@ -266,7 +265,7 @@ public: } /** constructor with pose and calibration */ - PinholePose(const Pose3& pose, const boost::shared_ptr& K) : + PinholePose(const Pose3& pose, const std::shared_ptr& K) : Base(pose), K_(K) { } @@ -281,14 +280,14 @@ public: * (theta 0 = looking in direction of positive X axis) * @param height camera height */ - static PinholePose Level(const boost::shared_ptr& K, + static PinholePose Level(const std::shared_ptr& K, const Pose2& pose2, double height) { return PinholePose(Base::LevelPose(pose2, height), K); } /// PinholePose::level with default calibration static PinholePose Level(const Pose2& pose2, double height) { - return PinholePose::Level(boost::make_shared(), pose2, height); + return PinholePose::Level(std::make_shared(), pose2, height); } /** @@ -301,8 +300,8 @@ public: * @param K optional calibration parameter */ static PinholePose Lookat(const Point3& eye, const Point3& target, - const Point3& upVector, const boost::shared_ptr& K = - boost::make_shared()) { + const Point3& upVector, const std::shared_ptr& K = + std::make_shared()) { return PinholePose(Base::LookatPose(eye, target, upVector), K); } @@ -362,7 +361,7 @@ public: } /// return shared pointer to calibration - const boost::shared_ptr& sharedCalibration() const { + const std::shared_ptr& sharedCalibration() const { return K_; } diff --git a/gtsam/geometry/SphericalCamera.h b/gtsam/geometry/SphericalCamera.h index dd3bc7420..202aefa9f 100644 --- a/gtsam/geometry/SphericalCamera.h +++ b/gtsam/geometry/SphericalCamera.h @@ -42,7 +42,7 @@ class GTSAM_EXPORT EmptyCal { enum { dimension = 0 }; EmptyCal() {} virtual ~EmptyCal() = default; - using shared_ptr = boost::shared_ptr; + using shared_ptr = std::shared_ptr; /// return DOF, dimensionality of tangent space inline static size_t Dim() { return dimension; } @@ -87,11 +87,11 @@ class GTSAM_EXPORT SphericalCamera { /// Default constructor SphericalCamera() - : pose_(Pose3()), emptyCal_(boost::make_shared()) {} + : pose_(Pose3()), emptyCal_(std::make_shared()) {} /// Constructor with pose explicit SphericalCamera(const Pose3& pose) - : pose_(pose), emptyCal_(boost::make_shared()) {} + : pose_(pose), emptyCal_(std::make_shared()) {} /// Constructor with empty intrinsics (needed for smart factors) explicit SphericalCamera(const Pose3& pose, diff --git a/gtsam/geometry/tests/testPinholePose.cpp b/gtsam/geometry/tests/testPinholePose.cpp index 4fccc18a1..7d795db86 100644 --- a/gtsam/geometry/tests/testPinholePose.cpp +++ b/gtsam/geometry/tests/testPinholePose.cpp @@ -33,7 +33,7 @@ using namespace gtsam; typedef PinholePose Camera; -static const Cal3_S2::shared_ptr K = boost::make_shared(625, 625, 0, 0, 0); +static const Cal3_S2::shared_ptr K = std::make_shared(625, 625, 0, 0, 0); static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5)); static const Camera camera(pose, K); @@ -263,8 +263,8 @@ TEST( PinholePose, range1) { /* ************************************************************************* */ typedef PinholePose Camera2; -static const boost::shared_ptr K2 = - boost::make_shared(625, 1e-3, 1e-3); +static const std::shared_ptr K2 = + std::make_shared(625, 1e-3, 1e-3); static const Camera2 camera2(pose1, K2); static double range2(const Camera& camera, const Camera2& camera2) { return camera.range(camera2); diff --git a/gtsam/geometry/tests/testStereoCamera.cpp b/gtsam/geometry/tests/testStereoCamera.cpp index d23b3c740..df275c96a 100644 --- a/gtsam/geometry/tests/testStereoCamera.cpp +++ b/gtsam/geometry/tests/testStereoCamera.cpp @@ -89,7 +89,7 @@ static Point3 landmark(0, 0, 5); /* ************************************************************************* */ static StereoPoint2 project3(const Pose3& pose, const Point3& point, const Cal3_S2Stereo& K) { - return StereoCamera(pose, boost::make_shared(K)).project(point); + return StereoCamera(pose, std::make_shared(K)).project(point); } /* ************************************************************************* */ @@ -150,7 +150,7 @@ TEST( StereoCamera, backproject_case2) } static Point3 backproject3(const Pose3& pose, const StereoPoint2& point, const Cal3_S2Stereo& K) { - return StereoCamera(pose, boost::make_shared(K)).backproject(point); + return StereoCamera(pose, std::make_shared(K)).backproject(point); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp index 56f8e3db8..e7812e527 100644 --- a/gtsam/geometry/tests/testTriangulation.cpp +++ b/gtsam/geometry/tests/testTriangulation.cpp @@ -35,8 +35,8 @@ using namespace gtsam; // Some common constants -static const boost::shared_ptr kSharedCal = // - boost::make_shared(1500, 1200, 0.1, 640, 480); +static const std::shared_ptr kSharedCal = // + std::make_shared(1500, 1200, 0.1, 640, 480); // Looking along X-axis, 1 meter above ground plane (x-y) static const Rot3 upright = Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2); @@ -159,8 +159,8 @@ TEST(triangulation, twoCamerasLOSTvsDLT) { //****************************************************************************** // Simple test with a well-behaved two camera situation with Cal3DS2 calibration. TEST(triangulation, twoPosesCal3DS2) { - static const boost::shared_ptr sharedDistortedCal = // - boost::make_shared(1500, 1200, 0, 640, 480, -.3, 0.1, 0.0001, + static const std::shared_ptr sharedDistortedCal = // + std::make_shared(1500, 1200, 0, 640, 480, -.3, 0.1, 0.0001, -0.0003); PinholeCamera camera1Distorted(kPose1, *sharedDistortedCal); @@ -212,8 +212,8 @@ TEST(triangulation, twoPosesCal3DS2) { // calibration. TEST(triangulation, twoPosesFisheye) { using Calibration = Cal3Fisheye; - static const boost::shared_ptr sharedDistortedCal = // - boost::make_shared(1500, 1200, .1, 640, 480, -.3, 0.1, + static const std::shared_ptr sharedDistortedCal = // + std::make_shared(1500, 1200, .1, 640, 480, -.3, 0.1, 0.0001, -0.0003); PinholeCamera camera1Distorted(kPose1, *sharedDistortedCal); @@ -263,8 +263,8 @@ TEST(triangulation, twoPosesFisheye) { //****************************************************************************** // Similar, but now with Bundler calibration TEST(triangulation, twoPosesBundler) { - boost::shared_ptr bundlerCal = // - boost::make_shared(1500, 0.1, 0.2, 640, 480); + std::shared_ptr bundlerCal = // + std::make_shared(1500, 0.1, 0.2, 640, 480); PinholeCamera camera1(kPose1, *bundlerCal); PinholeCamera camera2(kPose2, *bundlerCal); @@ -597,7 +597,7 @@ TEST(triangulation, onePose) { //****************************************************************************** TEST(triangulation, StereoTriangulateNonlinear) { - auto stereoK = boost::make_shared(1733.75, 1733.75, 0, 689.645, + auto stereoK = std::make_shared(1733.75, 1733.75, 0, 689.645, 508.835, 0.0699612); // two camera kPoses m1, m2 diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index f0810105f..160ab0480 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -123,7 +123,7 @@ GTSAM_EXPORT Point3 triangulateLOST(const std::vector& poses, */ template std::pair triangulationGraph( - const std::vector& poses, boost::shared_ptr sharedCal, + const std::vector& poses, std::shared_ptr sharedCal, const Point2Vector& measurements, Key landmarkKey, const Point3& initialEstimate, const SharedNoiseModel& model = noiseModel::Unit::Create(2)) { @@ -188,7 +188,7 @@ GTSAM_EXPORT Point3 optimize(const NonlinearFactorGraph& graph, */ template Point3 triangulateNonlinear(const std::vector& poses, - boost::shared_ptr sharedCal, + std::shared_ptr sharedCal, const Point2Vector& measurements, const Point3& initialEstimate, const SharedNoiseModel& model = nullptr) { @@ -236,7 +236,7 @@ projectionMatricesFromCameras(const CameraSet &cameras) { // overload, assuming pinholePose template std::vector> projectionMatricesFromPoses( - const std::vector &poses, boost::shared_ptr sharedCal) { + const std::vector &poses, std::shared_ptr sharedCal) { std::vector> projection_matrices; for (size_t i = 0; i < poses.size(); i++) { PinholePose camera(poses.at(i), sharedCal); @@ -422,7 +422,7 @@ inline Point3Vector calibrateMeasurements( */ template Point3 triangulatePoint3(const std::vector& poses, - boost::shared_ptr sharedCal, + std::shared_ptr sharedCal, const Point2Vector& measurements, double rank_tol = 1e-9, bool optimize = false, const SharedNoiseModel& model = nullptr, diff --git a/gtsam/hybrid/GaussianMixture.cpp b/gtsam/hybrid/GaussianMixture.cpp index cbf3bc376..546d0200b 100644 --- a/gtsam/hybrid/GaussianMixture.cpp +++ b/gtsam/hybrid/GaussianMixture.cpp @@ -106,7 +106,7 @@ GaussianConditional::shared_ptr GaussianMixture::operator()( const DiscreteValues &discreteValues) const { auto &ptr = conditionals_(discreteValues); if (!ptr) return nullptr; - auto conditional = boost::dynamic_pointer_cast(ptr); + auto conditional = std::dynamic_pointer_cast(ptr); if (conditional) return conditional; else @@ -185,7 +185,7 @@ bool GaussianMixture::allFrontalsGiven(const VectorValues &given) const { } /* ************************************************************************* */ -boost::shared_ptr GaussianMixture::likelihood( +std::shared_ptr GaussianMixture::likelihood( const VectorValues &given) const { if (!allFrontalsGiven(given)) { throw std::runtime_error( @@ -208,12 +208,12 @@ boost::shared_ptr GaussianMixture::likelihood( gfg.push_back(likelihood_m); Vector c(1); c << std::sqrt(2.0 * Cgm_Kgcm); - auto constantFactor = boost::make_shared(c); + auto constantFactor = std::make_shared(c); gfg.push_back(constantFactor); - return boost::make_shared(gfg); + return std::make_shared(gfg); } }); - return boost::make_shared( + return std::make_shared( continuousParentKeys, discreteParentKeys, likelihoods); } @@ -252,7 +252,7 @@ GaussianMixture::prunerFunc(const DecisionTreeFactor &decisionTree) { if (gaussianMixtureKeySet == decisionTreeKeySet) { if (decisionTree(values) == 0.0) { // empty aka null pointer - boost::shared_ptr null; + std::shared_ptr null; return null; } else { return conditional; diff --git a/gtsam/hybrid/GaussianMixture.h b/gtsam/hybrid/GaussianMixture.h index eef0419a2..5b4bc0f74 100644 --- a/gtsam/hybrid/GaussianMixture.h +++ b/gtsam/hybrid/GaussianMixture.h @@ -55,7 +55,7 @@ class GTSAM_EXPORT GaussianMixture public Conditional { public: using This = GaussianMixture; - using shared_ptr = boost::shared_ptr; + using shared_ptr = std::shared_ptr; using BaseFactor = HybridFactor; using BaseConditional = Conditional; @@ -164,7 +164,7 @@ class GTSAM_EXPORT GaussianMixture * Create a likelihood factor for a Gaussian mixture, return a Mixture factor * on the parents. */ - boost::shared_ptr likelihood( + std::shared_ptr likelihood( const VectorValues &given) const; /// Getter for the underlying Conditionals DecisionTree diff --git a/gtsam/hybrid/GaussianMixtureFactor.h b/gtsam/hybrid/GaussianMixtureFactor.h index aa8f2a199..30899ca7b 100644 --- a/gtsam/hybrid/GaussianMixtureFactor.h +++ b/gtsam/hybrid/GaussianMixtureFactor.h @@ -48,9 +48,9 @@ class GTSAM_EXPORT GaussianMixtureFactor : public HybridFactor { public: using Base = HybridFactor; using This = GaussianMixtureFactor; - using shared_ptr = boost::shared_ptr; + using shared_ptr = std::shared_ptr; - using sharedFactor = boost::shared_ptr; + using sharedFactor = std::shared_ptr; /// typedef for Decision Tree of Gaussian factors and log-constant. using Factors = DecisionTree; diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index 7c9023e58..68129bc27 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -52,7 +52,7 @@ DecisionTreeFactor::shared_ptr HybridBayesNet::discreteConditionals() const { dtFactor = dtFactor * f; } } - return boost::make_shared(dtFactor); + return std::make_shared(dtFactor); } /* ************************************************************************* */ @@ -155,16 +155,16 @@ void HybridBayesNet::updateDiscreteConditionals( // Apply prunerFunc to the underlying AlgebraicDecisionTree auto discreteTree = - boost::dynamic_pointer_cast(discrete); + std::dynamic_pointer_cast(discrete); DecisionTreeFactor::ADT prunedDiscreteTree = discreteTree->apply(prunerFunc(prunedDecisionTree, *conditional)); // Create the new (hybrid) conditional KeyVector frontals(discrete->frontals().begin(), discrete->frontals().end()); - auto prunedDiscrete = boost::make_shared( + auto prunedDiscrete = std::make_shared( frontals.size(), conditional->discreteKeys(), prunedDiscreteTree); - conditional = boost::make_shared(prunedDiscrete); + conditional = std::make_shared(prunedDiscrete); // Add it back to the BayesNet this->at(i) = conditional; @@ -194,7 +194,7 @@ HybridBayesNet HybridBayesNet::prune(size_t maxNrLeaves) { for (auto &&conditional : *this) { if (auto gm = conditional->asMixture()) { // Make a copy of the Gaussian mixture and prune it! - auto prunedGaussianMixture = boost::make_shared(*gm); + auto prunedGaussianMixture = std::make_shared(*gm); prunedGaussianMixture->prune(decisionTree); // imperative :-( // Type-erase and add to the pruned Bayes Net fragment. diff --git a/gtsam/hybrid/HybridBayesNet.h b/gtsam/hybrid/HybridBayesNet.h index 46a2b4f77..ef35ae29d 100644 --- a/gtsam/hybrid/HybridBayesNet.h +++ b/gtsam/hybrid/HybridBayesNet.h @@ -37,8 +37,8 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet { using Base = BayesNet; using This = HybridBayesNet; using ConditionalType = HybridConditional; - using shared_ptr = boost::shared_ptr; - using sharedConditional = boost::shared_ptr; + using shared_ptr = std::shared_ptr; + using sharedConditional = std::shared_ptr; /// @name Standard Constructors /// @{ @@ -66,7 +66,7 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet { * * This is the "native" push back, as this class stores hybrid conditionals. */ - void push_back(boost::shared_ptr conditional) { + void push_back(std::shared_ptr conditional) { factors_.push_back(conditional); } @@ -80,8 +80,8 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet { */ template void emplace_back(Conditional *conditional) { - factors_.push_back(boost::make_shared( - boost::shared_ptr(conditional))); + factors_.push_back(std::make_shared( + std::shared_ptr(conditional))); } /** @@ -93,12 +93,12 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet { * * Example: * auto shared_ptr_to_a_conditional = - * boost::make_shared(...); + * std::make_shared(...); * hbn.push_back(shared_ptr_to_a_conditional); */ void push_back(HybridConditional &&conditional) { factors_.push_back( - boost::make_shared(std::move(conditional))); + std::make_shared(std::move(conditional))); } /** diff --git a/gtsam/hybrid/HybridBayesTree.cpp b/gtsam/hybrid/HybridBayesTree.cpp index df2367cb5..b252e613e 100644 --- a/gtsam/hybrid/HybridBayesTree.cpp +++ b/gtsam/hybrid/HybridBayesTree.cpp @@ -114,13 +114,13 @@ struct HybridAssignmentData { conditional = hybrid_conditional->asGaussian(); } else { // Discrete only conditional, so we set to empty gaussian conditional - conditional = boost::make_shared(); + conditional = std::make_shared(); } GaussianBayesTree::sharedNode clique; if (conditional) { // Create the GaussianClique for the current node - clique = boost::make_shared(conditional); + clique = std::make_shared(conditional); // Add the current clique to the GaussianBayesTree. parentData.gaussianbayesTree_->addClique(clique, parentData.parentClique_); diff --git a/gtsam/hybrid/HybridBayesTree.h b/gtsam/hybrid/HybridBayesTree.h index 7240edaac..236ebf8b7 100644 --- a/gtsam/hybrid/HybridBayesTree.h +++ b/gtsam/hybrid/HybridBayesTree.h @@ -48,10 +48,10 @@ class GTSAM_EXPORT HybridBayesTreeClique typedef HybridBayesTreeClique This; typedef BayesTreeCliqueBase Base; - typedef boost::shared_ptr shared_ptr; - typedef boost::weak_ptr weak_ptr; + typedef std::shared_ptr shared_ptr; + typedef std::weak_ptr weak_ptr; HybridBayesTreeClique() {} - HybridBayesTreeClique(const boost::shared_ptr& conditional) + HybridBayesTreeClique(const std::shared_ptr& conditional) : Base(conditional) {} ///< Copy constructor HybridBayesTreeClique(const HybridBayesTreeClique& clique) : Base(clique) {} @@ -67,7 +67,7 @@ class GTSAM_EXPORT HybridBayesTree : public BayesTree { public: typedef HybridBayesTree This; - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; /// @name Standard interface /// @{ @@ -142,14 +142,14 @@ class BayesTreeOrphanWrapper : public HybridConditional { typedef HybridBayesTreeClique CliqueType; typedef HybridConditional Base; - boost::shared_ptr clique; + std::shared_ptr clique; /** * @brief Construct a new Bayes Tree Orphan Wrapper object. * * @param clique Bayes tree clique. */ - BayesTreeOrphanWrapper(const boost::shared_ptr& clique) + BayesTreeOrphanWrapper(const std::shared_ptr& clique) : clique(clique) { // Store parent keys in our base type factor so that eliminating those // parent keys will pull this subtree into the elimination. diff --git a/gtsam/hybrid/HybridConditional.cpp b/gtsam/hybrid/HybridConditional.cpp index 24f61a85f..6c4893476 100644 --- a/gtsam/hybrid/HybridConditional.cpp +++ b/gtsam/hybrid/HybridConditional.cpp @@ -39,7 +39,7 @@ HybridConditional::HybridConditional(const KeyVector &continuousFrontals, /* ************************************************************************ */ HybridConditional::HybridConditional( - const boost::shared_ptr &continuousConditional) + const std::shared_ptr &continuousConditional) : HybridConditional(continuousConditional->keys(), {}, continuousConditional->nrFrontals()) { inner_ = continuousConditional; @@ -47,7 +47,7 @@ HybridConditional::HybridConditional( /* ************************************************************************ */ HybridConditional::HybridConditional( - const boost::shared_ptr &discreteConditional) + const std::shared_ptr &discreteConditional) : HybridConditional({}, discreteConditional->discreteKeys(), discreteConditional->nrFrontals()) { inner_ = discreteConditional; @@ -55,7 +55,7 @@ HybridConditional::HybridConditional( /* ************************************************************************ */ HybridConditional::HybridConditional( - const boost::shared_ptr &gaussianMixture) + const std::shared_ptr &gaussianMixture) : BaseFactor(KeyVector(gaussianMixture->keys().begin(), gaussianMixture->keys().begin() + gaussianMixture->nrContinuous()), diff --git a/gtsam/hybrid/HybridConditional.h b/gtsam/hybrid/HybridConditional.h index c8cb968df..b38942bf5 100644 --- a/gtsam/hybrid/HybridConditional.h +++ b/gtsam/hybrid/HybridConditional.h @@ -25,8 +25,7 @@ #include #include -#include -#include +#include #include #include #include @@ -63,14 +62,14 @@ class GTSAM_EXPORT HybridConditional public: // typedefs needed to play nice with gtsam typedef HybridConditional This; ///< Typedef to this class - typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class + typedef std::shared_ptr shared_ptr; ///< shared_ptr to this class typedef HybridFactor BaseFactor; ///< Typedef to our factor base class typedef Conditional BaseConditional; ///< Typedef to our conditional base class protected: /// Type-erased pointer to the inner type - boost::shared_ptr inner_; + std::shared_ptr inner_; public: /// @name Standard Constructors @@ -111,7 +110,7 @@ class GTSAM_EXPORT HybridConditional * HybridConditional. */ HybridConditional( - const boost::shared_ptr& continuousConditional); + const std::shared_ptr& continuousConditional); /** * @brief Construct a new Hybrid Conditional object @@ -120,7 +119,7 @@ class GTSAM_EXPORT HybridConditional * HybridConditional. */ HybridConditional( - const boost::shared_ptr& discreteConditional); + const std::shared_ptr& discreteConditional); /** * @brief Construct a new Hybrid Conditional object @@ -128,7 +127,7 @@ class GTSAM_EXPORT HybridConditional * @param gaussianMixture Gaussian Mixture Conditional used to create the * HybridConditional. */ - HybridConditional(const boost::shared_ptr& gaussianMixture); + HybridConditional(const std::shared_ptr& gaussianMixture); /// @} /// @name Testable @@ -152,7 +151,7 @@ class GTSAM_EXPORT HybridConditional * @return GaussianMixture::shared_ptr otherwise */ GaussianMixture::shared_ptr asMixture() const { - return boost::dynamic_pointer_cast(inner_); + return std::dynamic_pointer_cast(inner_); } /** @@ -161,7 +160,7 @@ class GTSAM_EXPORT HybridConditional * @return GaussianConditional::shared_ptr otherwise */ GaussianConditional::shared_ptr asGaussian() const { - return boost::dynamic_pointer_cast(inner_); + return std::dynamic_pointer_cast(inner_); } /** @@ -170,11 +169,11 @@ class GTSAM_EXPORT HybridConditional * @return DiscreteConditional::shared_ptr */ DiscreteConditional::shared_ptr asDiscrete() const { - return boost::dynamic_pointer_cast(inner_); + return std::dynamic_pointer_cast(inner_); } /// Get the type-erased pointer to the inner type - boost::shared_ptr inner() const { return inner_; } + std::shared_ptr inner() const { return inner_; } /// Return the error of the underlying conditional. double error(const HybridValues& values) const override; diff --git a/gtsam/hybrid/HybridEliminationTree.h b/gtsam/hybrid/HybridEliminationTree.h index 941fefa5a..e1eb1764a 100644 --- a/gtsam/hybrid/HybridEliminationTree.h +++ b/gtsam/hybrid/HybridEliminationTree.h @@ -37,7 +37,7 @@ class GTSAM_EXPORT HybridEliminationTree typedef EliminationTree Base; ///< Base class typedef HybridEliminationTree This; ///< This class - typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class + typedef std::shared_ptr shared_ptr; ///< Shared pointer to this class /// @name Constructors /// @{ diff --git a/gtsam/hybrid/HybridFactor.h b/gtsam/hybrid/HybridFactor.h index 0fa352191..ebd1d289a 100644 --- a/gtsam/hybrid/HybridFactor.h +++ b/gtsam/hybrid/HybridFactor.h @@ -64,7 +64,7 @@ class GTSAM_EXPORT HybridFactor : public Factor { public: // typedefs needed to play nice with gtsam typedef HybridFactor This; ///< This class - typedef boost::shared_ptr + typedef std::shared_ptr shared_ptr; ///< shared_ptr to this class typedef Factor Base; ///< Our base class diff --git a/gtsam/hybrid/HybridFactorGraph.cpp b/gtsam/hybrid/HybridFactorGraph.cpp index 7fb280116..631f8d22f 100644 --- a/gtsam/hybrid/HybridFactorGraph.cpp +++ b/gtsam/hybrid/HybridFactorGraph.cpp @@ -28,12 +28,12 @@ namespace gtsam { std::set HybridFactorGraph::discreteKeys() const { std::set keys; for (auto& factor : factors_) { - if (auto p = boost::dynamic_pointer_cast(factor)) { + if (auto p = std::dynamic_pointer_cast(factor)) { for (const DiscreteKey& key : p->discreteKeys()) { keys.insert(key); } } - if (auto p = boost::dynamic_pointer_cast(factor)) { + if (auto p = std::dynamic_pointer_cast(factor)) { for (const DiscreteKey& key : p->discreteKeys()) { keys.insert(key); } @@ -65,7 +65,7 @@ std::unordered_map HybridFactorGraph::discreteKeyMap() const { const KeySet HybridFactorGraph::continuousKeySet() const { KeySet keys; for (auto& factor : factors_) { - if (auto p = boost::dynamic_pointer_cast(factor)) { + if (auto p = std::dynamic_pointer_cast(factor)) { for (const Key& key : p->continuousKeys()) { keys.insert(key); } diff --git a/gtsam/hybrid/HybridFactorGraph.h b/gtsam/hybrid/HybridFactorGraph.h index a02d4a212..af0cea746 100644 --- a/gtsam/hybrid/HybridFactorGraph.h +++ b/gtsam/hybrid/HybridFactorGraph.h @@ -30,7 +30,7 @@ namespace gtsam { class DiscreteFactor; class Ordering; -using SharedFactor = boost::shared_ptr; +using SharedFactor = std::shared_ptr; /** * Hybrid Factor Graph @@ -40,7 +40,7 @@ class HybridFactorGraph : public FactorGraph { public: using Base = FactorGraph; using This = HybridFactorGraph; ///< this class - using shared_ptr = boost::shared_ptr; ///< shared_ptr to This + using shared_ptr = std::shared_ptr; ///< shared_ptr to This using Values = gtsam::Values; ///< backwards compatibility using Indices = KeyVector; ///> map from keys to values diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index c912a74fc..346f2141a 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -57,12 +57,12 @@ template class EliminateableFactorGraph; using OrphanWrapper = BayesTreeOrphanWrapper; -using boost::dynamic_pointer_cast; +using std::dynamic_pointer_cast; /* ************************************************************************ */ // Throw a runtime exception for method specified in string s, and factor f: static void throwRuntimeError(const std::string &s, - const boost::shared_ptr &f) { + const std::shared_ptr &f) { auto &fr = *f; throw std::runtime_error(s + " not implemented for factor type " + demangle(typeid(fr).name()) + "."); @@ -135,7 +135,7 @@ GaussianFactorGraphTree HybridGaussianFactorGraph::assembleGraphTree() const { } /* ************************************************************************ */ -static std::pair> +static std::pair> continuousElimination(const HybridGaussianFactorGraph &factors, const Ordering &frontalKeys) { GaussianFactorGraph gfg; @@ -155,11 +155,11 @@ continuousElimination(const HybridGaussianFactorGraph &factors, } auto result = EliminatePreferCholesky(gfg, frontalKeys); - return {boost::make_shared(result.first), result.second}; + return {std::make_shared(result.first), result.second}; } /* ************************************************************************ */ -static std::pair> +static std::pair> discreteElimination(const HybridGaussianFactorGraph &factors, const Ordering &frontalKeys) { DiscreteFactorGraph dfg; @@ -182,7 +182,7 @@ discreteElimination(const HybridGaussianFactorGraph &factors, // NOTE: This does sum-product. For max-product, use EliminateForMPE. auto result = EliminateDiscrete(dfg, frontalKeys); - return {boost::make_shared(result.first), result.second}; + return {std::make_shared(result.first), result.second}; } /* ************************************************************************ */ @@ -201,7 +201,7 @@ GaussianFactorGraphTree removeEmpty(const GaussianFactorGraphTree &sum) { } /* ************************************************************************ */ -static std::pair> +static std::pair> hybridElimination(const HybridGaussianFactorGraph &factors, const Ordering &frontalKeys, const KeyVector &continuousSeparator, @@ -220,7 +220,7 @@ hybridElimination(const HybridGaussianFactorGraph &factors, // FG has a nullptr as we're looping over the factors. factorGraphTree = removeEmpty(factorGraphTree); - using Result = std::pair, + using Result = std::pair, GaussianMixtureFactor::sharedFactor>; // This is the elimination method on the leaf nodes @@ -256,7 +256,7 @@ hybridElimination(const HybridGaussianFactorGraph &factors, std::tie(conditionals, newFactors) = unzip(eliminationResults); // Create the GaussianMixture from the conditionals - auto gaussianMixture = boost::make_shared( + auto gaussianMixture = std::make_shared( frontalKeys, continuousSeparator, discreteSeparator, conditionals); if (continuousSeparator.empty()) { @@ -275,8 +275,8 @@ hybridElimination(const HybridGaussianFactorGraph &factors, }; DecisionTree probabilities(eliminationResults, probability); - return {boost::make_shared(gaussianMixture), - boost::make_shared(discreteSeparator, + return {std::make_shared(gaussianMixture), + std::make_shared(discreteSeparator, probabilities)}; } else { // Otherwise, we create a resulting GaussianMixtureFactor on the separator, @@ -286,7 +286,7 @@ hybridElimination(const HybridGaussianFactorGraph &factors, auto correct = [&](const Result &pair) -> GaussianFactor::shared_ptr { const auto &factor = pair.second; if (!factor) return factor; // TODO(dellaert): not loving this. - auto hf = boost::dynamic_pointer_cast(factor); + auto hf = std::dynamic_pointer_cast(factor); if (!hf) throw std::runtime_error("Expected HessianFactor!"); hf->constantTerm() += 2.0 * pair.first->logNormalizationConstant(); return hf; @@ -294,10 +294,10 @@ hybridElimination(const HybridGaussianFactorGraph &factors, GaussianMixtureFactor::Factors correctedFactors(eliminationResults, correct); - const auto mixtureFactor = boost::make_shared( + const auto mixtureFactor = std::make_shared( continuousSeparator, discreteSeparator, newFactors); - return {boost::make_shared(gaussianMixture), + return {std::make_shared(gaussianMixture), mixtureFactor}; } } @@ -316,7 +316,7 @@ hybridElimination(const HybridGaussianFactorGraph &factors, * eliminate a discrete variable (as specified in the ordering), the result will * be INCORRECT and there will be NO error raised. */ -std::pair> // +std::pair> // EliminateHybrid(const HybridGaussianFactorGraph &factors, const Ordering &frontalKeys) { // NOTE: Because we are in the Conditional Gaussian regime there are only diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.h b/gtsam/hybrid/HybridGaussianFactorGraph.h index b516d0adc..f911b135b 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.h +++ b/gtsam/hybrid/HybridGaussianFactorGraph.h @@ -52,7 +52,7 @@ class HybridValues; * @ingroup hybrid */ GTSAM_EXPORT -std::pair, boost::shared_ptr> +std::pair, std::shared_ptr> EliminateHybrid(const HybridGaussianFactorGraph& factors, const Ordering& keys); /** @@ -80,8 +80,8 @@ struct EliminationTraits { typedef HybridBayesTree BayesTreeType; ///< Type of Bayes tree typedef HybridJunctionTree JunctionTreeType; ///< Type of Junction tree /// The default dense elimination function - static std::pair, - boost::shared_ptr> + static std::pair, + std::shared_ptr> DefaultEliminate(const FactorGraphType& factors, const Ordering& keys) { return EliminateHybrid(factors, keys); } @@ -114,7 +114,7 @@ class GTSAM_EXPORT HybridGaussianFactorGraph using This = HybridGaussianFactorGraph; ///< this class using BaseEliminateable = EliminateableFactorGraph; ///< for elimination - using shared_ptr = boost::shared_ptr; ///< shared_ptr to This + using shared_ptr = std::shared_ptr; ///< shared_ptr to This using Values = gtsam::Values; ///< backwards compatibility using Indices = KeyVector; ///< map from keys to values diff --git a/gtsam/hybrid/HybridGaussianISAM.cpp b/gtsam/hybrid/HybridGaussianISAM.cpp index dc7746209..3e5a4f1d1 100644 --- a/gtsam/hybrid/HybridGaussianISAM.cpp +++ b/gtsam/hybrid/HybridGaussianISAM.cpp @@ -89,7 +89,7 @@ void HybridGaussianISAM::updateInternal( // Add the orphaned subtrees for (const sharedClique& orphan : *orphans) { - factors += boost::make_shared>(orphan); + factors += std::make_shared>(orphan); } const VariableIndex index(factors); diff --git a/gtsam/hybrid/HybridGaussianISAM.h b/gtsam/hybrid/HybridGaussianISAM.h index b353b26a3..8578f3dc5 100644 --- a/gtsam/hybrid/HybridGaussianISAM.h +++ b/gtsam/hybrid/HybridGaussianISAM.h @@ -37,7 +37,7 @@ class GTSAM_EXPORT HybridGaussianISAM : public ISAM { public: typedef ISAM Base; typedef HybridGaussianISAM This; - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; /// @name Standard Constructors /// @{ diff --git a/gtsam/hybrid/HybridJunctionTree.cpp b/gtsam/hybrid/HybridJunctionTree.cpp index a463c625f..6c0e62ac9 100644 --- a/gtsam/hybrid/HybridJunctionTree.cpp +++ b/gtsam/hybrid/HybridJunctionTree.cpp @@ -50,23 +50,23 @@ struct HybridConstructorTraversalData { // Pre-order visitor function static HybridConstructorTraversalData ConstructorTraversalVisitorPre( - const boost::shared_ptr& node, + const std::shared_ptr& node, HybridConstructorTraversalData& parentData) { // On the pre-order pass, before children have been visited, we just set up // a traversal data structure with its own JT node, and create a child // pointer in its parent. HybridConstructorTraversalData data = HybridConstructorTraversalData(&parentData); - data.junctionTreeNode = boost::make_shared(node->key, node->factors); + data.junctionTreeNode = std::make_shared(node->key, node->factors); parentData.junctionTreeNode->addChild(data.junctionTreeNode); // Add all the discrete keys in the hybrid factors to the current data for (const auto& f : node->factors) { - if (auto hf = boost::dynamic_pointer_cast(f)) { + if (auto hf = std::dynamic_pointer_cast(f)) { for (auto& k : hf->discreteKeys()) { data.discreteKeys.insert(k.first); } - } else if (auto hf = boost::dynamic_pointer_cast(f)) { + } else if (auto hf = std::dynamic_pointer_cast(f)) { for (auto& k : hf->discreteKeys()) { data.discreteKeys.insert(k.first); } @@ -78,7 +78,7 @@ struct HybridConstructorTraversalData { // Post-order visitor function static void ConstructorTraversalVisitorPost( - const boost::shared_ptr& node, + const std::shared_ptr& node, const HybridConstructorTraversalData& data) { // In this post-order visitor, we combine the symbolic elimination results // from the elimination tree children and symbolically eliminate the current @@ -162,7 +162,7 @@ HybridJunctionTree::HybridJunctionTree( typedef HybridConstructorTraversalData Data; Data rootData(0); rootData.junctionTreeNode = - boost::make_shared(); // Make a dummy node to gather + std::make_shared(); // Make a dummy node to gather // the junction tree roots treeTraversal::DepthFirstForest(eliminationTree, rootData, Data::ConstructorTraversalVisitorPre, diff --git a/gtsam/hybrid/HybridJunctionTree.h b/gtsam/hybrid/HybridJunctionTree.h index 29fa24809..a197e6e3c 100644 --- a/gtsam/hybrid/HybridJunctionTree.h +++ b/gtsam/hybrid/HybridJunctionTree.h @@ -56,7 +56,7 @@ class GTSAM_EXPORT HybridJunctionTree typedef JunctionTree Base; ///< Base class typedef HybridJunctionTree This; ///< This class - typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class + typedef std::shared_ptr shared_ptr; ///< Shared pointer to this class /** * Build the elimination tree of a factor graph using precomputed column diff --git a/gtsam/hybrid/HybridNonlinearFactorGraph.cpp b/gtsam/hybrid/HybridNonlinearFactorGraph.cpp index 71b064eb6..b0a85de5d 100644 --- a/gtsam/hybrid/HybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/HybridNonlinearFactorGraph.cpp @@ -43,10 +43,10 @@ void HybridNonlinearFactorGraph::print(const std::string& s, /* ************************************************************************* */ HybridGaussianFactorGraph::shared_ptr HybridNonlinearFactorGraph::linearize( const Values& continuousValues) const { - using boost::dynamic_pointer_cast; + using std::dynamic_pointer_cast; // create an empty linear FG - auto linearFG = boost::make_shared(); + auto linearFG = std::make_shared(); linearFG->reserve(size()); diff --git a/gtsam/hybrid/HybridNonlinearFactorGraph.h b/gtsam/hybrid/HybridNonlinearFactorGraph.h index 0cc87d504..89b4fb391 100644 --- a/gtsam/hybrid/HybridNonlinearFactorGraph.h +++ b/gtsam/hybrid/HybridNonlinearFactorGraph.h @@ -35,7 +35,7 @@ class GTSAM_EXPORT HybridNonlinearFactorGraph : public HybridFactorGraph { public: using Base = HybridFactorGraph; using This = HybridNonlinearFactorGraph; ///< this class - using shared_ptr = boost::shared_ptr; ///< shared_ptr to This + using shared_ptr = std::shared_ptr; ///< shared_ptr to This using Values = gtsam::Values; ///< backwards compatibility using Indices = KeyVector; ///> map from keys to values @@ -74,7 +74,7 @@ class GTSAM_EXPORT HybridNonlinearFactorGraph : public HybridFactorGraph { * @param continuousValues: Dictionary of continuous values. * @return HybridGaussianFactorGraph::shared_ptr */ - boost::shared_ptr linearize( + std::shared_ptr linearize( const Values& continuousValues) const; /// @} }; diff --git a/gtsam/hybrid/HybridNonlinearISAM.cpp b/gtsam/hybrid/HybridNonlinearISAM.cpp index 9a6d25ade..ec6ebf4d0 100644 --- a/gtsam/hybrid/HybridNonlinearISAM.cpp +++ b/gtsam/hybrid/HybridNonlinearISAM.cpp @@ -50,7 +50,7 @@ void HybridNonlinearISAM::update(const HybridNonlinearFactorGraph& newFactors, // TODO: optimize for whole config? linPoint_.insert(initialValues); - boost::shared_ptr linearizedNewFactors = + std::shared_ptr linearizedNewFactors = newFactors.linearize(linPoint_); // Update ISAM diff --git a/gtsam/hybrid/HybridSmoother.cpp b/gtsam/hybrid/HybridSmoother.cpp index 7e8a5f4d6..3fe41a4c7 100644 --- a/gtsam/hybrid/HybridSmoother.cpp +++ b/gtsam/hybrid/HybridSmoother.cpp @@ -42,7 +42,7 @@ void HybridSmoother::update(HybridGaussianFactorGraph graph, bayesNetFragment->prune(*maxNrLeaves); // Set the bayes net fragment to the pruned version bayesNetFragment = - boost::make_shared(prunedBayesNetFragment); + std::make_shared(prunedBayesNetFragment); } // Add the partial bayes net to the posterior bayes net. diff --git a/gtsam/hybrid/MixtureFactor.h b/gtsam/hybrid/MixtureFactor.h index 220f1bdbf..cbc949404 100644 --- a/gtsam/hybrid/MixtureFactor.h +++ b/gtsam/hybrid/MixtureFactor.h @@ -49,8 +49,8 @@ class MixtureFactor : public HybridFactor { public: using Base = HybridFactor; using This = MixtureFactor; - using shared_ptr = boost::shared_ptr; - using sharedFactor = boost::shared_ptr; + using shared_ptr = std::shared_ptr; + using sharedFactor = std::shared_ptr; /** * @brief typedef for DecisionTree which has Keys as node labels and @@ -97,7 +97,7 @@ class MixtureFactor : public HybridFactor { */ template MixtureFactor(const KeyVector& keys, const DiscreteKeys& discreteKeys, - const std::vector>& factors, + const std::vector>& factors, bool normalized = false) : Base(keys, discreteKeys), normalized_(normalized) { std::vector nonlinear_factors; @@ -108,7 +108,7 @@ class MixtureFactor : public HybridFactor { std::copy(f->keys().begin(), f->keys().end(), std::inserter(factor_keys_set, factor_keys_set.end())); - if (auto nf = boost::dynamic_pointer_cast(f)) { + if (auto nf = std::dynamic_pointer_cast(f)) { nonlinear_factors.push_back(nf); } else { throw std::runtime_error( @@ -237,7 +237,7 @@ class MixtureFactor : public HybridFactor { } /// Linearize all the continuous factors to get a GaussianMixtureFactor. - boost::shared_ptr linearize( + std::shared_ptr linearize( const Values& continuousValues) const { // functional to linearize each factor in the decision tree auto linearizeDT = [continuousValues](const sharedFactor& factor) { @@ -247,7 +247,7 @@ class MixtureFactor : public HybridFactor { DecisionTree linearized_factors( factors_, linearizeDT); - return boost::make_shared( + return std::make_shared( continuousKeys_, discreteKeys_, linearized_factors); } @@ -266,13 +266,13 @@ class MixtureFactor : public HybridFactor { // If this is a NoiseModelFactor, we'll use its noiseModel to // otherwise noiseModelFactor will be nullptr if (auto noiseModelFactor = - boost::dynamic_pointer_cast(factor)) { + std::dynamic_pointer_cast(factor)) { // If dynamic cast to NoiseModelFactor succeeded, see if the noise model // is Gaussian auto noiseModel = noiseModelFactor->noiseModel(); auto gaussianNoiseModel = - boost::dynamic_pointer_cast(noiseModel); + std::dynamic_pointer_cast(noiseModel); if (gaussianNoiseModel) { // If the noise model is Gaussian, retrieve the information matrix infoMat = gaussianNoiseModel->information(); diff --git a/gtsam/hybrid/tests/Switching.h b/gtsam/hybrid/tests/Switching.h index ab7a9a84f..5842e1f1a 100644 --- a/gtsam/hybrid/tests/Switching.h +++ b/gtsam/hybrid/tests/Switching.h @@ -59,9 +59,9 @@ inline HybridGaussianFactorGraph::shared_ptr makeSwitchingChain( for (size_t t = 1; t < n; t++) { hfg.add(GaussianMixtureFactor( {keyFunc(t), keyFunc(t + 1)}, {{dKeyFunc(t), 2}}, - {boost::make_shared(keyFunc(t), I_3x3, keyFunc(t + 1), + {std::make_shared(keyFunc(t), I_3x3, keyFunc(t + 1), I_3x3, Z_3x1), - boost::make_shared(keyFunc(t), I_3x3, keyFunc(t + 1), + std::make_shared(keyFunc(t), I_3x3, keyFunc(t + 1), I_3x3, Vector3::Ones())})); if (t > 1) { @@ -70,7 +70,7 @@ inline HybridGaussianFactorGraph::shared_ptr makeSwitchingChain( } } - return boost::make_shared(std::move(hfg)); + return std::make_shared(std::move(hfg)); } /** @@ -161,7 +161,7 @@ struct Switching { auto motion_models = motionModels(k, between_sigma); std::vector components; for (auto &&f : motion_models) { - components.push_back(boost::dynamic_pointer_cast(f)); + components.push_back(std::dynamic_pointer_cast(f)); } nonlinearFactorGraph.emplace_shared( keys, DiscreteKeys{modes[k]}, components); @@ -192,9 +192,9 @@ struct Switching { double sigma = 1.0) { auto noise_model = noiseModel::Isotropic::Sigma(1, sigma); auto still = - boost::make_shared(X(k), X(k + 1), 0.0, noise_model), + std::make_shared(X(k), X(k + 1), 0.0, noise_model), moving = - boost::make_shared(X(k), X(k + 1), 1.0, noise_model); + std::make_shared(X(k), X(k + 1), 1.0, noise_model); return {still, moving}; } diff --git a/gtsam/hybrid/tests/testGaussianMixture.cpp b/gtsam/hybrid/tests/testGaussianMixture.cpp index 7a4439e21..f15c06165 100644 --- a/gtsam/hybrid/tests/testGaussianMixture.cpp +++ b/gtsam/hybrid/tests/testGaussianMixture.cpp @@ -185,7 +185,7 @@ TEST(GaussianMixture, Likelihood2) { { // We have a JacobianFactor const auto gf1 = (*likelihood)(assignment1); - const auto jf1 = boost::dynamic_pointer_cast(gf1); + const auto jf1 = std::dynamic_pointer_cast(gf1); CHECK(jf1); // It has 2 rows, not 1! diff --git a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp index 962d238a8..c60b95d4b 100644 --- a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp +++ b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp @@ -58,11 +58,11 @@ TEST(GaussianMixtureFactor, Sum) { sigmas << 1, 2; auto model = noiseModel::Diagonal::Sigmas(sigmas, true); - auto f10 = boost::make_shared(X(1), A1, X(2), A2, b); - auto f11 = boost::make_shared(X(1), A1, X(2), A2, b); - auto f20 = boost::make_shared(X(1), A1, X(3), A3, b); - auto f21 = boost::make_shared(X(1), A1, X(3), A3, b); - auto f22 = boost::make_shared(X(1), A1, X(3), A3, b); + auto f10 = std::make_shared(X(1), A1, X(2), A2, b); + auto f11 = std::make_shared(X(1), A1, X(2), A2, b); + auto f20 = std::make_shared(X(1), A1, X(3), A3, b); + auto f21 = std::make_shared(X(1), A1, X(3), A3, b); + auto f22 = std::make_shared(X(1), A1, X(3), A3, b); std::vector factorsA{f10, f11}; std::vector factorsB{f20, f21, f22}; @@ -98,8 +98,8 @@ TEST(GaussianMixtureFactor, Printing) { auto A1 = Matrix::Zero(2, 1); auto A2 = Matrix::Zero(2, 2); auto b = Matrix::Zero(2, 1); - auto f10 = boost::make_shared(X(1), A1, X(2), A2, b); - auto f11 = boost::make_shared(X(1), A1, X(2), A2, b); + auto f10 = std::make_shared(X(1), A1, X(2), A2, b); + auto f11 = std::make_shared(X(1), A1, X(2), A2, b); std::vector factors{f10, f11}; GaussianMixtureFactor mixtureFactor({X(1), X(2)}, {m1}, factors); @@ -145,7 +145,7 @@ TEST(GaussianMixtureFactor, GaussianMixture) { dKeys.emplace_back(M(0), 2); dKeys.emplace_back(M(1), 2); - auto gaussians = boost::make_shared(); + auto gaussians = std::make_shared(); GaussianMixture::Conditionals conditionals(gaussians); GaussianMixture gm({}, keys, dKeys, conditionals); @@ -165,8 +165,8 @@ TEST(GaussianMixtureFactor, Error) { auto b = Vector2::Zero(); - auto f0 = boost::make_shared(X(1), A01, X(2), A02, b); - auto f1 = boost::make_shared(X(1), A11, X(2), A12, b); + auto f0 = std::make_shared(X(1), A01, X(2), A02, b); + auto f1 = std::make_shared(X(1), A11, X(2), A12, b); std::vector factors{f0, f1}; GaussianMixtureFactor mixtureFactor({X(1), X(2)}, {m1}, factors); diff --git a/gtsam/hybrid/tests/testHybridBayesNet.cpp b/gtsam/hybrid/tests/testHybridBayesNet.cpp index 425e93415..c4bac1df2 100644 --- a/gtsam/hybrid/tests/testHybridBayesNet.cpp +++ b/gtsam/hybrid/tests/testHybridBayesNet.cpp @@ -99,9 +99,9 @@ TEST(HybridBayesNet, evaluateHybrid) { const SharedDiagonal model0 = noiseModel::Diagonal::Sigmas(Vector1(2.0)), model1 = noiseModel::Diagonal::Sigmas(Vector1(3.0)); - const auto conditional0 = boost::make_shared( + const auto conditional0 = std::make_shared( X(1), Vector1::Constant(5), I_1x1, model0), - conditional1 = boost::make_shared( + conditional1 = std::make_shared( X(1), Vector1::Constant(2), I_1x1, model1); // Create hybrid Bayes net. @@ -289,7 +289,7 @@ TEST(HybridBayesNet, UpdateDiscreteConditionals) { size_t maxNrLeaves = 3; auto discreteConditionals = posterior->discreteConditionals(); const DecisionTreeFactor::shared_ptr prunedDecisionTree = - boost::make_shared( + std::make_shared( discreteConditionals->prune(maxNrLeaves)); EXPECT_LONGS_EQUAL(maxNrLeaves + 2 /*2 zero leaves*/, @@ -317,7 +317,7 @@ TEST(HybridBayesNet, UpdateDiscreteConditionals) { // Get the pruned discrete conditionals as an AlgebraicDecisionTree auto pruned_discrete_conditionals = posterior->at(4)->asDiscrete(); auto discrete_conditional_tree = - boost::dynamic_pointer_cast( + std::dynamic_pointer_cast( pruned_discrete_conditionals); // The checker functor verifies the values for us. @@ -331,9 +331,9 @@ TEST(HybridBayesNet, Sampling) { auto noise_model = noiseModel::Diagonal::Sigmas(Vector1(1.0)); auto zero_motion = - boost::make_shared>(X(0), X(1), 0, noise_model); + std::make_shared>(X(0), X(1), 0, noise_model); auto one_motion = - boost::make_shared>(X(0), X(1), 1, noise_model); + std::make_shared>(X(0), X(1), 1, noise_model); std::vector factors = {zero_motion, one_motion}; nfg.emplace_shared>(X(0), 0.0, noise_model); nfg.emplace_shared( diff --git a/gtsam/hybrid/tests/testHybridEstimation.cpp b/gtsam/hybrid/tests/testHybridEstimation.cpp index a0195d0c5..55d2bc248 100644 --- a/gtsam/hybrid/tests/testHybridEstimation.cpp +++ b/gtsam/hybrid/tests/testHybridEstimation.cpp @@ -200,7 +200,7 @@ GaussianFactorGraph::shared_ptr specificModesFactorGraph( // Add "motion models". auto motion_noise_model = noiseModel::Isotropic::Sigma(1, between_sigma); for (size_t k = 0; k < K - 1; k++) { - auto motion_model = boost::make_shared( + auto motion_model = std::make_shared( X(k), X(k + 1), discrete_seq.at(k), motion_noise_model); graph.push_back(motion_model); } @@ -256,7 +256,7 @@ AlgebraicDecisionTree getProbPrimeTree( vector vector_values; for (const DiscreteValues& assignment : assignments) { VectorValues values = bayesNet->optimize(assignment); - vector_values.push_back(boost::make_shared(values)); + vector_values.push_back(std::make_shared(values)); } DecisionTree delta_tree(discrete_keys, vector_values); @@ -413,9 +413,9 @@ static HybridNonlinearFactorGraph createHybridNonlinearFactorGraph() { // Add mixture factor: DiscreteKey m(M(0), 2); const auto zero_motion = - boost::make_shared>(X(0), X(1), 0, noise_model); + std::make_shared>(X(0), X(1), 0, noise_model); const auto one_motion = - boost::make_shared>(X(0), X(1), 1, noise_model); + std::make_shared>(X(0), X(1), 1, noise_model); nfg.emplace_shared( KeyVector{X(0), X(1)}, DiscreteKeys{m}, std::vector{zero_motion, one_motion}); diff --git a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp index e5c11bf0c..4c8c5518e 100644 --- a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp @@ -73,9 +73,9 @@ TEST(HybridGaussianFactorGraph, Creation) { GaussianMixture gm({X(0)}, {X(1)}, DiscreteKeys(DiscreteKey{M(0), 2}), GaussianMixture::Conditionals( M(0), - boost::make_shared( + std::make_shared( X(0), Z_3x1, I_3x3, X(1), I_3x3), - boost::make_shared( + std::make_shared( X(0), Vector3::Ones(), I_3x3, X(1), I_3x3))); hfg.add(gm); @@ -126,8 +126,8 @@ TEST(HybridGaussianFactorGraph, eliminateFullSequentialEqualChance) { // Add a gaussian mixture factor Ï•(x1, c1) DiscreteKey m1(M(1), 2); DecisionTree dt( - M(1), boost::make_shared(X(1), I_3x3, Z_3x1), - boost::make_shared(X(1), I_3x3, Vector3::Ones())); + M(1), std::make_shared(X(1), I_3x3, Z_3x1), + std::make_shared(X(1), I_3x3, Vector3::Ones())); hfg.add(GaussianMixtureFactor({X(1)}, {m1}, dt)); auto result = hfg.eliminateSequential(); @@ -152,8 +152,8 @@ TEST(HybridGaussianFactorGraph, eliminateFullSequentialSimple) { hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1)); std::vector factors = { - boost::make_shared(X(1), I_3x3, Z_3x1), - boost::make_shared(X(1), I_3x3, Vector3::Ones())}; + std::make_shared(X(1), I_3x3, Z_3x1), + std::make_shared(X(1), I_3x3, Vector3::Ones())}; hfg.add(GaussianMixtureFactor({X(1)}, {m1}, factors)); // Discrete probability table for c1 @@ -178,8 +178,8 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalSimple) { hfg.add(GaussianMixtureFactor( {X(1)}, {{M(1), 2}}, - {boost::make_shared(X(1), I_3x3, Z_3x1), - boost::make_shared(X(1), I_3x3, Vector3::Ones())})); + {std::make_shared(X(1), I_3x3, Z_3x1), + std::make_shared(X(1), I_3x3, Vector3::Ones())})); hfg.add(DecisionTreeFactor(m1, {2, 8})); // TODO(Varun) Adding extra discrete variable not connected to continuous @@ -207,8 +207,8 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalCLG) { // Decision tree with different modes on x1 DecisionTree dt( - M(1), boost::make_shared(X(1), I_3x3, Z_3x1), - boost::make_shared(X(1), I_3x3, Vector3::Ones())); + M(1), std::make_shared(X(1), I_3x3, Z_3x1), + std::make_shared(X(1), I_3x3, Vector3::Ones())); // Hybrid factor P(x1|c1) hfg.add(GaussianMixtureFactor({X(1)}, {m}, dt)); @@ -238,12 +238,12 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalTwoClique) { { hfg.add(GaussianMixtureFactor( {X(0)}, {{M(0), 2}}, - {boost::make_shared(X(0), I_3x3, Z_3x1), - boost::make_shared(X(0), I_3x3, Vector3::Ones())})); + {std::make_shared(X(0), I_3x3, Z_3x1), + std::make_shared(X(0), I_3x3, Vector3::Ones())})); DecisionTree dt1( - M(1), boost::make_shared(X(2), I_3x3, Z_3x1), - boost::make_shared(X(2), I_3x3, Vector3::Ones())); + M(1), std::make_shared(X(2), I_3x3, Z_3x1), + std::make_shared(X(2), I_3x3, Vector3::Ones())); hfg.add(GaussianMixtureFactor({X(2)}, {{M(1), 2}}, dt1)); } @@ -255,14 +255,14 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalTwoClique) { { DecisionTree dt( - M(3), boost::make_shared(X(3), I_3x3, Z_3x1), - boost::make_shared(X(3), I_3x3, Vector3::Ones())); + M(3), std::make_shared(X(3), I_3x3, Z_3x1), + std::make_shared(X(3), I_3x3, Vector3::Ones())); hfg.add(GaussianMixtureFactor({X(3)}, {{M(3), 2}}, dt)); DecisionTree dt1( - M(2), boost::make_shared(X(5), I_3x3, Z_3x1), - boost::make_shared(X(5), I_3x3, Vector3::Ones())); + M(2), std::make_shared(X(5), I_3x3, Z_3x1), + std::make_shared(X(5), I_3x3, Vector3::Ones())); hfg.add(GaussianMixtureFactor({X(5)}, {{M(2), 2}}, dt1)); } @@ -510,8 +510,8 @@ TEST(HybridGaussianFactorGraph, optimize) { hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1)); DecisionTree dt( - C(1), boost::make_shared(X(1), I_3x3, Z_3x1), - boost::make_shared(X(1), I_3x3, Vector3::Ones())); + C(1), std::make_shared(X(1), I_3x3, Z_3x1), + std::make_shared(X(1), I_3x3, Vector3::Ones())); hfg.add(GaussianMixtureFactor({X(1)}, {c1}, dt)); @@ -624,11 +624,11 @@ TEST(HybridGaussianFactorGraph, assembleGraphTree) { // Create expected decision tree with two factor graphs: // Get mixture factor: - auto mixture = boost::dynamic_pointer_cast(fg.at(0)); + auto mixture = std::dynamic_pointer_cast(fg.at(0)); CHECK(mixture); // Get prior factor: - const auto gf = boost::dynamic_pointer_cast(fg.at(1)); + const auto gf = std::dynamic_pointer_cast(fg.at(1)); CHECK(gf); using GF = GaussianFactor::shared_ptr; const GF prior = gf->asGaussian(); @@ -709,9 +709,9 @@ TEST(HybridGaussianFactorGraph, EliminateTiny1) { // Create Gaussian mixture on X(0). using tiny::mode; // regression, but mean checked to be 5.0 in both cases: - const auto conditional0 = boost::make_shared( + const auto conditional0 = std::make_shared( X(0), Vector1(14.1421), I_1x1 * 2.82843), - conditional1 = boost::make_shared( + conditional1 = std::make_shared( X(0), Vector1(10.1379), I_1x1 * 2.02759); expectedBayesNet.emplace_back( new GaussianMixture({X(0)}, {}, {mode}, {conditional0, conditional1})); @@ -743,9 +743,9 @@ TEST(HybridGaussianFactorGraph, EliminateTiny2) { // Create Gaussian mixture on X(0). using tiny::mode; // regression, but mean checked to be 5.0 in both cases: - const auto conditional0 = boost::make_shared( + const auto conditional0 = std::make_shared( X(0), Vector1(17.3205), I_1x1 * 3.4641), - conditional1 = boost::make_shared( + conditional1 = std::make_shared( X(0), Vector1(10.274), I_1x1 * 2.0548); expectedBayesNet.emplace_back( new GaussianMixture({X(0)}, {}, {mode}, {conditional0, conditional1})); diff --git a/gtsam/hybrid/tests/testHybridGaussianISAM.cpp b/gtsam/hybrid/tests/testHybridGaussianISAM.cpp index 4f135f84f..573fbc671 100644 --- a/gtsam/hybrid/tests/testHybridGaussianISAM.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianISAM.cpp @@ -201,7 +201,7 @@ TEST(HybridGaussianElimination, IncrementalInference) { DiscreteKeys discrete_keys = {{M(0), 2}, {M(1), 2}}; vector probs = {0.095292197, 0.31417524, 0.28275772, 0.30777485}; auto expectedConditional = - boost::make_shared(discrete_keys, probs); + std::make_shared(discrete_keys, probs); EXPECT(assert_equal(*expectedConditional, *actualConditional, 1e-6)); } @@ -422,12 +422,12 @@ TEST(HybridGaussianISAM, NonTrivial) { Pose2 odometry(1.0, 0.0, 0.0); KeyVector contKeys = {W(0), W(1)}; auto noise_model = noiseModel::Isotropic::Sigma(3, 1.0); - auto still = boost::make_shared(W(0), W(1), Pose2(0, 0, 0), + auto still = std::make_shared(W(0), W(1), Pose2(0, 0, 0), noise_model), - moving = boost::make_shared(W(0), W(1), odometry, + moving = std::make_shared(W(0), W(1), odometry, noise_model); std::vector components = {moving, still}; - auto mixtureFactor = boost::make_shared( + auto mixtureFactor = std::make_shared( contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components); fg.push_back(mixtureFactor); @@ -462,12 +462,12 @@ TEST(HybridGaussianISAM, NonTrivial) { /*************** Run Round 3 ***************/ // Add odometry factor with discrete modes. contKeys = {W(1), W(2)}; - still = boost::make_shared(W(1), W(2), Pose2(0, 0, 0), + still = std::make_shared(W(1), W(2), Pose2(0, 0, 0), noise_model); moving = - boost::make_shared(W(1), W(2), odometry, noise_model); + std::make_shared(W(1), W(2), odometry, noise_model); components = {moving, still}; - mixtureFactor = boost::make_shared( + mixtureFactor = std::make_shared( contKeys, DiscreteKeys{gtsam::DiscreteKey(M(2), 2)}, components); fg.push_back(mixtureFactor); @@ -505,12 +505,12 @@ TEST(HybridGaussianISAM, NonTrivial) { /*************** Run Round 4 ***************/ // Add odometry factor with discrete modes. contKeys = {W(2), W(3)}; - still = boost::make_shared(W(2), W(3), Pose2(0, 0, 0), + still = std::make_shared(W(2), W(3), Pose2(0, 0, 0), noise_model); moving = - boost::make_shared(W(2), W(3), odometry, noise_model); + std::make_shared(W(2), W(3), odometry, noise_model); components = {moving, still}; - mixtureFactor = boost::make_shared( + mixtureFactor = std::make_shared( contKeys, DiscreteKeys{gtsam::DiscreteKey(M(3), 2)}, components); fg.push_back(mixtureFactor); diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp index 5b8b32b21..73f969ff2 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp @@ -80,12 +80,12 @@ TEST(HybridNonlinearFactorGraph, Equals) { // Test empty factor graphs EXPECT(assert_equal(graph1, graph2)); - auto f0 = boost::make_shared>( + auto f0 = std::make_shared>( 1, Pose2(), noiseModel::Isotropic::Sigma(3, 0.001)); graph1.push_back(f0); graph2.push_back(f0); - auto f1 = boost::make_shared>( + auto f1 = std::make_shared>( 1, 2, Pose2(), noiseModel::Isotropic::Sigma(3, 0.1)); graph1.push_back(f1); graph2.push_back(f1); @@ -99,13 +99,13 @@ TEST(HybridNonlinearFactorGraph, Equals) { */ TEST(HybridNonlinearFactorGraph, Resize) { HybridNonlinearFactorGraph fg; - auto nonlinearFactor = boost::make_shared>(); + auto nonlinearFactor = std::make_shared>(); fg.push_back(nonlinearFactor); - auto discreteFactor = boost::make_shared(); + auto discreteFactor = std::make_shared(); fg.push_back(discreteFactor); - auto dcFactor = boost::make_shared(); + auto dcFactor = std::make_shared(); fg.push_back(dcFactor); EXPECT_LONGS_EQUAL(fg.size(), 3); @@ -120,19 +120,19 @@ TEST(HybridNonlinearFactorGraph, Resize) { */ TEST(HybridGaussianFactorGraph, Resize) { HybridNonlinearFactorGraph nhfg; - auto nonlinearFactor = boost::make_shared>( + auto nonlinearFactor = std::make_shared>( X(0), X(1), 0.0, Isotropic::Sigma(1, 0.1)); nhfg.push_back(nonlinearFactor); - auto discreteFactor = boost::make_shared(); + auto discreteFactor = std::make_shared(); nhfg.push_back(discreteFactor); KeyVector contKeys = {X(0), X(1)}; auto noise_model = noiseModel::Isotropic::Sigma(1, 1.0); - auto still = boost::make_shared(X(0), X(1), 0.0, noise_model), - moving = boost::make_shared(X(0), X(1), 1.0, noise_model); + auto still = std::make_shared(X(0), X(1), 0.0, noise_model), + moving = std::make_shared(X(0), X(1), 1.0, noise_model); std::vector components = {still, moving}; - auto dcFactor = boost::make_shared( + auto dcFactor = std::make_shared( contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components); nhfg.push_back(dcFactor); @@ -154,24 +154,24 @@ TEST(HybridGaussianFactorGraph, Resize) { * keys provided do not match the keys in the factors. */ TEST(HybridGaussianFactorGraph, MixtureFactor) { - auto nonlinearFactor = boost::make_shared>( + auto nonlinearFactor = std::make_shared>( X(0), X(1), 0.0, Isotropic::Sigma(1, 0.1)); - auto discreteFactor = boost::make_shared(); + auto discreteFactor = std::make_shared(); auto noise_model = noiseModel::Isotropic::Sigma(1, 1.0); - auto still = boost::make_shared(X(0), X(1), 0.0, noise_model), - moving = boost::make_shared(X(0), X(1), 1.0, noise_model); + auto still = std::make_shared(X(0), X(1), 0.0, noise_model), + moving = std::make_shared(X(0), X(1), 1.0, noise_model); std::vector components = {still, moving}; // Check for exception when number of continuous keys are under-specified. KeyVector contKeys = {X(0)}; - THROWS_EXCEPTION(boost::make_shared( + THROWS_EXCEPTION(std::make_shared( contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components)); // Check for exception when number of continuous keys are too many. contKeys = {X(0), X(1), X(2)}; - THROWS_EXCEPTION(boost::make_shared( + THROWS_EXCEPTION(std::make_shared( contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components)); } @@ -181,21 +181,21 @@ TEST(HybridGaussianFactorGraph, MixtureFactor) { TEST(HybridFactorGraph, PushBack) { HybridNonlinearFactorGraph fg; - auto nonlinearFactor = boost::make_shared>(); + auto nonlinearFactor = std::make_shared>(); fg.push_back(nonlinearFactor); EXPECT_LONGS_EQUAL(fg.size(), 1); fg = HybridNonlinearFactorGraph(); - auto discreteFactor = boost::make_shared(); + auto discreteFactor = std::make_shared(); fg.push_back(discreteFactor); EXPECT_LONGS_EQUAL(fg.size(), 1); fg = HybridNonlinearFactorGraph(); - auto dcFactor = boost::make_shared(); + auto dcFactor = std::make_shared(); fg.push_back(dcFactor); EXPECT_LONGS_EQUAL(fg.size(), 1); @@ -203,7 +203,7 @@ TEST(HybridFactorGraph, PushBack) { // Now do the same with HybridGaussianFactorGraph HybridGaussianFactorGraph ghfg; - auto gaussianFactor = boost::make_shared(); + auto gaussianFactor = std::make_shared(); ghfg.push_back(gaussianFactor); EXPECT_LONGS_EQUAL(ghfg.size(), 1); @@ -329,7 +329,7 @@ GaussianFactorGraph::shared_ptr batchGFG(double between, NonlinearFactorGraph graph; graph.addPrior(X(0), 0, Isotropic::Sigma(1, 0.1)); - auto between_x0_x1 = boost::make_shared( + auto between_x0_x1 = std::make_shared( X(0), X(1), between, Isotropic::Sigma(1, 1.0)); graph.push_back(between_x0_x1); @@ -351,7 +351,7 @@ TEST(HybridGaussianElimination, EliminateHybrid_2_Variable) { ordering += X(1); HybridConditional::shared_ptr hybridConditionalMixture; - boost::shared_ptr factorOnModes; + std::shared_ptr factorOnModes; std::tie(hybridConditionalMixture, factorOnModes) = EliminateHybrid(factors, ordering); @@ -684,9 +684,9 @@ TEST(HybridFactorGraph, DefaultDecisionTree) { Pose2 odometry(2.0, 0.0, 0.0); KeyVector contKeys = {X(0), X(1)}; auto noise_model = noiseModel::Isotropic::Sigma(3, 1.0); - auto still = boost::make_shared(X(0), X(1), Pose2(0, 0, 0), + auto still = std::make_shared(X(0), X(1), Pose2(0, 0, 0), noise_model), - moving = boost::make_shared(X(0), X(1), odometry, + moving = std::make_shared(X(0), X(1), odometry, noise_model); std::vector motion_models = {still, moving}; fg.emplace_shared( diff --git a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp index d89909122..0e7010595 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp @@ -218,7 +218,7 @@ TEST(HybridNonlinearISAM, IncrementalInference) { DiscreteKeys discrete_keys = {{M(0), 2}, {M(1), 2}}; vector probs = {0.095292197, 0.31417524, 0.28275772, 0.30777485}; auto expectedConditional = - boost::make_shared(discrete_keys, probs); + std::make_shared(discrete_keys, probs); EXPECT(assert_equal(*expectedConditional, *actualConditional, 1e-6)); } @@ -441,12 +441,12 @@ TEST(HybridNonlinearISAM, NonTrivial) { Pose2 odometry(1.0, 0.0, 0.0); KeyVector contKeys = {W(0), W(1)}; auto noise_model = noiseModel::Isotropic::Sigma(3, 1.0); - auto still = boost::make_shared(W(0), W(1), Pose2(0, 0, 0), + auto still = std::make_shared(W(0), W(1), Pose2(0, 0, 0), noise_model), - moving = boost::make_shared(W(0), W(1), odometry, + moving = std::make_shared(W(0), W(1), odometry, noise_model); std::vector components = {moving, still}; - auto mixtureFactor = boost::make_shared( + auto mixtureFactor = std::make_shared( contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components); fg.push_back(mixtureFactor); @@ -481,12 +481,12 @@ TEST(HybridNonlinearISAM, NonTrivial) { /*************** Run Round 3 ***************/ // Add odometry factor with discrete modes. contKeys = {W(1), W(2)}; - still = boost::make_shared(W(1), W(2), Pose2(0, 0, 0), + still = std::make_shared(W(1), W(2), Pose2(0, 0, 0), noise_model); moving = - boost::make_shared(W(1), W(2), odometry, noise_model); + std::make_shared(W(1), W(2), odometry, noise_model); components = {moving, still}; - mixtureFactor = boost::make_shared( + mixtureFactor = std::make_shared( contKeys, DiscreteKeys{gtsam::DiscreteKey(M(2), 2)}, components); fg.push_back(mixtureFactor); @@ -524,12 +524,12 @@ TEST(HybridNonlinearISAM, NonTrivial) { /*************** Run Round 4 ***************/ // Add odometry factor with discrete modes. contKeys = {W(2), W(3)}; - still = boost::make_shared(W(2), W(3), Pose2(0, 0, 0), + still = std::make_shared(W(2), W(3), Pose2(0, 0, 0), noise_model); moving = - boost::make_shared(W(2), W(3), odometry, noise_model); + std::make_shared(W(2), W(3), odometry, noise_model); components = {moving, still}; - mixtureFactor = boost::make_shared( + mixtureFactor = std::make_shared( contKeys, DiscreteKeys{gtsam::DiscreteKey(M(3), 2)}, components); fg.push_back(mixtureFactor); diff --git a/gtsam/hybrid/tests/testMixtureFactor.cpp b/gtsam/hybrid/tests/testMixtureFactor.cpp index 9e4d66bf2..67a7fd8ae 100644 --- a/gtsam/hybrid/tests/testMixtureFactor.cpp +++ b/gtsam/hybrid/tests/testMixtureFactor.cpp @@ -52,9 +52,9 @@ TEST(MixtureFactor, Printing) { auto model = noiseModel::Diagonal::Sigmas(sigmas, false); auto f0 = - boost::make_shared>(X(1), X(2), between0, model); + std::make_shared>(X(1), X(2), between0, model); auto f1 = - boost::make_shared>(X(1), X(2), between1, model); + std::make_shared>(X(1), X(2), between1, model); std::vector factors{f0, f1}; MixtureFactor mixtureFactor({X(1), X(2)}, {m1}, factors); @@ -80,9 +80,9 @@ static MixtureFactor getMixtureFactor() { auto model = noiseModel::Diagonal::Sigmas(sigmas, false); auto f0 = - boost::make_shared>(X(1), X(2), between0, model); + std::make_shared>(X(1), X(2), between0, model); auto f1 = - boost::make_shared>(X(1), X(2), between1, model); + std::make_shared>(X(1), X(2), between1, model); std::vector factors{f0, f1}; return MixtureFactor({X(1), X(2)}, {m1}, factors); diff --git a/gtsam/hybrid/tests/testSerializationHybrid.cpp b/gtsam/hybrid/tests/testSerializationHybrid.cpp index bfa8b7514..961618626 100644 --- a/gtsam/hybrid/tests/testSerializationHybrid.cpp +++ b/gtsam/hybrid/tests/testSerializationHybrid.cpp @@ -80,8 +80,8 @@ TEST(HybridSerialization, GaussianMixtureFactor) { auto A = Matrix::Zero(2, 1); auto b0 = Matrix::Zero(2, 1); auto b1 = Matrix::Ones(2, 1); - auto f0 = boost::make_shared(X(0), A, b0); - auto f1 = boost::make_shared(X(0), A, b1); + auto f0 = std::make_shared(X(0), A, b0); + auto f1 = std::make_shared(X(0), A, b1); std::vector factors{f0, f1}; const GaussianMixtureFactor factor(continuousKeys, discreteKeys, factors); @@ -96,7 +96,7 @@ TEST(HybridSerialization, GaussianMixtureFactor) { TEST(HybridSerialization, HybridConditional) { const DiscreteKey mode(M(0), 2); Matrix1 I = Matrix1::Identity(); - const auto conditional = boost::make_shared( + const auto conditional = std::make_shared( GaussianConditional::FromMeanAndStddev(Z(0), I, X(0), Vector1(0), 0.5)); const HybridConditional hc(conditional); @@ -110,9 +110,9 @@ TEST(HybridSerialization, HybridConditional) { TEST(HybridSerialization, GaussianMixture) { const DiscreteKey mode(M(0), 2); Matrix1 I = Matrix1::Identity(); - const auto conditional0 = boost::make_shared( + const auto conditional0 = std::make_shared( GaussianConditional::FromMeanAndStddev(Z(0), I, X(0), Vector1(0), 0.5)); - const auto conditional1 = boost::make_shared( + const auto conditional1 = std::make_shared( GaussianConditional::FromMeanAndStddev(Z(0), I, X(0), Vector1(0), 3)); const GaussianMixture gm({Z(0)}, {X(0)}, {mode}, {conditional0, conditional1}); diff --git a/gtsam/inference/BayesNet.h b/gtsam/inference/BayesNet.h index 3e6d55281..87f5ec6fa 100644 --- a/gtsam/inference/BayesNet.h +++ b/gtsam/inference/BayesNet.h @@ -20,7 +20,7 @@ #include -#include +#include #include namespace gtsam { @@ -37,7 +37,7 @@ class BayesNet : public FactorGraph { typedef FactorGraph Base; public: - typedef typename boost::shared_ptr + typedef typename std::shared_ptr sharedConditional; ///< A shared pointer to a conditional protected: diff --git a/gtsam/inference/BayesTree-inst.h b/gtsam/inference/BayesTree-inst.h index 65ba37889..8e771708f 100644 --- a/gtsam/inference/BayesTree-inst.h +++ b/gtsam/inference/BayesTree-inst.h @@ -155,7 +155,7 @@ namespace gtsam { struct _pushCliqueFunctor { _pushCliqueFunctor(FactorGraph* graph_) : graph(graph_) {} FactorGraph* graph; - int operator()(const boost::shared_ptr& clique, int dummy) { + int operator()(const std::shared_ptr& clique, int dummy) { graph->push_back(clique->conditional_); return 0; } @@ -181,11 +181,11 @@ namespace gtsam { /* ************************************************************************* */ namespace { template - boost::shared_ptr - BayesTreeCloneForestVisitorPre(const boost::shared_ptr& node, const boost::shared_ptr& parentPointer) + std::shared_ptr + BayesTreeCloneForestVisitorPre(const std::shared_ptr& node, const std::shared_ptr& parentPointer) { // Clone the current node and add it to its cloned parent - boost::shared_ptr clone = boost::make_shared(*node); + std::shared_ptr clone = std::make_shared(*node); clone->children.clear(); clone->parent_ = parentPointer; parentPointer->children.push_back(clone); @@ -197,7 +197,7 @@ namespace gtsam { template BayesTree& BayesTree::operator=(const This& other) { this->clear(); - boost::shared_ptr rootContainer = boost::make_shared(); + std::shared_ptr rootContainer = std::make_shared(); treeTraversal::DepthFirstForest(other, rootContainer, BayesTreeCloneForestVisitorPre); for(const sharedClique& root: rootContainer->children) { root->parent_ = typename Clique::weak_ptr(); // Reset the parent since it's set to the dummy clique @@ -292,7 +292,7 @@ namespace gtsam { BayesTree::joint(Key j1, Key j2, const Eliminate& function) const { gttic(BayesTree_joint); - return boost::make_shared(*jointBayesNet(j1, j2, function)); + return std::make_shared(*jointBayesNet(j1, j2, function)); } /* ************************************************************************* */ @@ -352,7 +352,7 @@ namespace gtsam { // Factor the shortcuts to be conditioned on the full root // Get the set of variables to eliminate, which is C1\B. gttic(Full_root_factoring); - boost::shared_ptr p_C1_B; { + std::shared_ptr p_C1_B; { KeyVector C1_minus_B; { KeySet C1_minus_B_set(C1->conditional()->beginParents(), C1->conditional()->endParents()); for(const Key j: *B->conditional()) { @@ -364,7 +364,7 @@ namespace gtsam { boost::tie(p_C1_B, temp_remaining) = FactorGraphType(p_C1_Bred).eliminatePartialMultifrontal(Ordering(C1_minus_B), function); } - boost::shared_ptr p_C2_B; { + std::shared_ptr p_C2_B; { KeyVector C2_minus_B; { KeySet C2_minus_B_set(C2->conditional()->beginParents(), C2->conditional()->endParents()); for(const Key j: *B->conditional()) { diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h index b9bffe13f..d9450e107 100644 --- a/gtsam/inference/BayesTree.h +++ b/gtsam/inference/BayesTree.h @@ -19,7 +19,7 @@ #pragma once -#include +#include #include #include @@ -67,21 +67,21 @@ namespace gtsam { { protected: typedef BayesTree This; - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; public: typedef CLIQUE Clique; ///< The clique type, normally BayesTreeClique - typedef boost::shared_ptr sharedClique; ///< Shared pointer to a clique + typedef std::shared_ptr sharedClique; ///< Shared pointer to a clique typedef Clique Node; ///< Synonym for Clique (TODO: remove) typedef sharedClique sharedNode; ///< Synonym for sharedClique (TODO: remove) typedef typename CLIQUE::ConditionalType ConditionalType; - typedef boost::shared_ptr sharedConditional; + typedef std::shared_ptr sharedConditional; typedef typename CLIQUE::BayesNetType BayesNetType; - typedef boost::shared_ptr sharedBayesNet; + typedef std::shared_ptr sharedBayesNet; typedef typename CLIQUE::FactorType FactorType; - typedef boost::shared_ptr sharedFactor; + typedef std::shared_ptr sharedFactor; typedef typename CLIQUE::FactorGraphType FactorGraphType; - typedef boost::shared_ptr sharedFactorGraph; + typedef std::shared_ptr sharedFactorGraph; typedef typename FactorGraphType::Eliminate Eliminate; typedef typename CLIQUE::EliminationTraitsType EliminationTraitsType; @@ -278,7 +278,7 @@ namespace gtsam { typedef CLIQUE CliqueType; typedef typename CLIQUE::ConditionalType Base; - boost::shared_ptr clique; + std::shared_ptr clique; /** * @brief Construct a new Bayes Tree Orphan Wrapper object @@ -290,7 +290,7 @@ namespace gtsam { * @param clique Orphan clique to add for further consideration in * elimination. */ - BayesTreeOrphanWrapper(const boost::shared_ptr& clique) + BayesTreeOrphanWrapper(const std::shared_ptr& clique) : clique(clique) { this->keys_.assign(clique->conditional()->beginParents(), clique->conditional()->endParents()); diff --git a/gtsam/inference/BayesTreeCliqueBase-inst.h b/gtsam/inference/BayesTreeCliqueBase-inst.h index 37ed49055..62ed0b90f 100644 --- a/gtsam/inference/BayesTreeCliqueBase-inst.h +++ b/gtsam/inference/BayesTreeCliqueBase-inst.h @@ -129,7 +129,7 @@ namespace gtsam { KeyVector keep = shortcut_indices(B, p_Cp_B); // Marginalize out everything except S union B - boost::shared_ptr p_S_B = p_Cp_B.marginal(keep, function); + std::shared_ptr p_S_B = p_Cp_B.marginal(keep, function); return *p_S_B->eliminatePartialSequential(S_setminus_B, function).first; } else @@ -198,7 +198,7 @@ namespace gtsam { // initialize with separator marginal P(S) FactorGraphType p_C = this->separatorMarginal(function); // add the conditional P(F|S) - p_C += boost::shared_ptr(this->conditional_); + p_C += std::shared_ptr(this->conditional_); return p_C; } diff --git a/gtsam/inference/BayesTreeCliqueBase.h b/gtsam/inference/BayesTreeCliqueBase.h index 4cb0941b9..a50c4bec9 100644 --- a/gtsam/inference/BayesTreeCliqueBase.h +++ b/gtsam/inference/BayesTreeCliqueBase.h @@ -52,16 +52,16 @@ namespace gtsam { typedef BayesTreeCliqueBase This; typedef DERIVED DerivedType; typedef EliminationTraits EliminationTraitsType; - typedef boost::shared_ptr shared_ptr; - typedef boost::weak_ptr weak_ptr; - typedef boost::shared_ptr derived_ptr; - typedef boost::weak_ptr derived_weak_ptr; + typedef std::shared_ptr shared_ptr; + typedef std::weak_ptr weak_ptr; + typedef std::shared_ptr derived_ptr; + typedef std::weak_ptr derived_weak_ptr; public: typedef FACTORGRAPH FactorGraphType; typedef typename EliminationTraitsType::BayesNetType BayesNetType; typedef typename BayesNetType::ConditionalType ConditionalType; - typedef boost::shared_ptr sharedConditional; + typedef std::shared_ptr sharedConditional; typedef typename FactorGraphType::FactorType FactorType; typedef typename FactorGraphType::Eliminate Eliminate; diff --git a/gtsam/inference/ClusterTree-inst.h b/gtsam/inference/ClusterTree-inst.h index 6295549c4..4d6dfb22e 100644 --- a/gtsam/inference/ClusterTree-inst.h +++ b/gtsam/inference/ClusterTree-inst.h @@ -41,7 +41,7 @@ std::vector ClusterTree::Cluster::nrFrontalsOfChildren() const { /* ************************************************************************* */ template -void ClusterTree::Cluster::merge(const boost::shared_ptr& cluster) { +void ClusterTree::Cluster::merge(const std::shared_ptr& cluster) { // Merge keys. For efficiency, we add keys in reverse order at end, calling reverse after.. orderedFrontalKeys.insert(orderedFrontalKeys.end(), cluster->orderedFrontalKeys.rbegin(), cluster->orderedFrontalKeys.rend()); @@ -123,15 +123,15 @@ struct EliminationData { EliminationData* const parentData; size_t myIndexInParent; FastVector childFactors; - boost::shared_ptr bayesTreeNode; + std::shared_ptr bayesTreeNode; #ifdef GTSAM_USE_TBB - boost::shared_ptr writeLock; + std::shared_ptr writeLock; #endif EliminationData(EliminationData* _parentData, size_t nChildren) : - parentData(_parentData), bayesTreeNode(boost::make_shared()) + parentData(_parentData), bayesTreeNode(std::make_shared()) #ifdef GTSAM_USE_TBB - , writeLock(boost::make_shared()) + , writeLock(std::make_shared()) #endif { if (parentData) { @@ -241,13 +241,13 @@ EliminatableClusterTree& EliminatableClusterTree -std::pair, boost::shared_ptr > +std::pair, std::shared_ptr > EliminatableClusterTree::eliminate(const Eliminate& function) const { gttic(ClusterTree_eliminate); // Do elimination (depth-first traversal). The rootsContainer stores a 'dummy' BayesTree node // that contains all of the roots as its children. rootsContainer also stores the remaining // un-eliminated factors passed up from the roots. - boost::shared_ptr result = boost::make_shared(); + std::shared_ptr result = std::make_shared(); typedef EliminationData Data; Data rootsContainer(0, this->nrRoots()); @@ -264,7 +264,7 @@ EliminatableClusterTree::eliminate(const Eliminate& function) rootsContainer.bayesTreeNode->children.end()); // Add remaining factors that were not involved with eliminated variables - boost::shared_ptr remaining = boost::make_shared(); + std::shared_ptr remaining = std::make_shared(); remaining->reserve(remainingFactors_.size() + rootsContainer.childFactors.size()); remaining->push_back(remainingFactors_.begin(), remainingFactors_.end()); for (const sharedFactor& factor : rootsContainer.childFactors) { diff --git a/gtsam/inference/ClusterTree.h b/gtsam/inference/ClusterTree.h index 26c853a7b..98a86b3f8 100644 --- a/gtsam/inference/ClusterTree.h +++ b/gtsam/inference/ClusterTree.h @@ -26,15 +26,15 @@ class ClusterTree { public: typedef GRAPH FactorGraphType; ///< The factor graph type typedef ClusterTree This; ///< This class - typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class + typedef std::shared_ptr shared_ptr; ///< Shared pointer to this class typedef typename GRAPH::FactorType FactorType; ///< The type of factors - typedef boost::shared_ptr sharedFactor; ///< Shared pointer to a factor + typedef std::shared_ptr sharedFactor; ///< Shared pointer to a factor /// A Cluster is just a collection of factors // TODO(frank): re-factor JunctionTree so we can make members private struct Cluster { - typedef FastVector > Children; + typedef FastVector > Children; Children children; ///< sub-trees typedef Ordering Keys; @@ -68,7 +68,7 @@ class ClusterTree { } /// Add a child cluster - void addChild(const boost::shared_ptr& cluster) { + void addChild(const std::shared_ptr& cluster) { children.push_back(cluster); problemSize_ = std::max(problemSize_, cluster->problemSize_); } @@ -97,13 +97,13 @@ class ClusterTree { std::vector nrFrontalsOfChildren() const; /// Merge in given cluster - void merge(const boost::shared_ptr& cluster); + void merge(const std::shared_ptr& cluster); /// Merge all children for which bit is set into this node void mergeChildren(const std::vector& merge); }; - typedef boost::shared_ptr sharedCluster; ///< Shared pointer to Cluster + typedef std::shared_ptr sharedCluster; ///< Shared pointer to Cluster // Define Node=Cluster for compatibility with tree traversal functions typedef Cluster Node; @@ -142,11 +142,11 @@ class ClusterTree { /// @name Advanced Interface /// @{ - void addRoot(const boost::shared_ptr& cluster) { + void addRoot(const std::shared_ptr& cluster) { roots_.push_back(cluster); } - void addChildrenAsRoots(const boost::shared_ptr& cluster) { + void addChildrenAsRoots(const std::shared_ptr& cluster) { for (auto child : cluster->children) this->addRoot(child); } @@ -186,15 +186,15 @@ class EliminatableClusterTree : public ClusterTree { typedef BAYESTREE BayesTreeType; ///< The BayesTree type produced by elimination typedef GRAPH FactorGraphType; ///< The factor graph type typedef EliminatableClusterTree This; ///< This class - typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class + typedef std::shared_ptr shared_ptr; ///< Shared pointer to this class typedef typename BAYESTREE::ConditionalType ConditionalType; ///< The type of conditionals - typedef boost::shared_ptr + typedef std::shared_ptr sharedConditional; ///< Shared pointer to a conditional typedef typename GRAPH::Eliminate Eliminate; ///< Typedef for an eliminate subroutine typedef typename GRAPH::FactorType FactorType; ///< The type of factors - typedef boost::shared_ptr sharedFactor; ///< Shared pointer to a factor + typedef std::shared_ptr sharedFactor; ///< Shared pointer to a factor protected: FastVector remainingFactors_; @@ -219,7 +219,7 @@ class EliminatableClusterTree : public ClusterTree { * in GaussianFactorGraph.h * @return The Bayes tree and factor graph resulting from elimination */ - std::pair, boost::shared_ptr > eliminate( + std::pair, std::shared_ptr > eliminate( const Eliminate& function) const; /// @} diff --git a/gtsam/inference/EliminateableFactorGraph-inst.h b/gtsam/inference/EliminateableFactorGraph-inst.h index eb6d57067..558acbfe8 100644 --- a/gtsam/inference/EliminateableFactorGraph-inst.h +++ b/gtsam/inference/EliminateableFactorGraph-inst.h @@ -26,7 +26,7 @@ namespace gtsam { /* ************************************************************************* */ template - boost::shared_ptr::BayesNetType> + std::shared_ptr::BayesNetType> EliminateableFactorGraph::eliminateSequential( OptionalOrderingType orderingType, const Eliminate& function, OptionalVariableIndex variableIndex) const { @@ -60,7 +60,7 @@ namespace gtsam { /* ************************************************************************* */ template - boost::shared_ptr::BayesNetType> + std::shared_ptr::BayesNetType> EliminateableFactorGraph::eliminateSequential( const Ordering& ordering, const Eliminate& function, OptionalVariableIndex variableIndex) const @@ -73,8 +73,8 @@ namespace gtsam { gttic(eliminateSequential); // Do elimination EliminationTreeType etree(asDerived(), (*variableIndex).get(), ordering); - boost::shared_ptr bayesNet; - boost::shared_ptr factorGraph; + std::shared_ptr bayesNet; + std::shared_ptr factorGraph; boost::tie(bayesNet,factorGraph) = etree.eliminate(function); // If any factors are remaining, the ordering was incomplete if(!factorGraph->empty()) @@ -86,7 +86,7 @@ namespace gtsam { /* ************************************************************************* */ template - boost::shared_ptr< + std::shared_ptr< typename EliminateableFactorGraph::BayesTreeType> EliminateableFactorGraph::eliminateMultifrontal( OptionalOrderingType orderingType, const Eliminate& function, @@ -123,7 +123,7 @@ namespace gtsam { /* ************************************************************************* */ template - boost::shared_ptr::BayesTreeType> + std::shared_ptr::BayesTreeType> EliminateableFactorGraph::eliminateMultifrontal( const Ordering& ordering, const Eliminate& function, OptionalVariableIndex variableIndex) const @@ -137,8 +137,8 @@ namespace gtsam { // Do elimination with given ordering EliminationTreeType etree(asDerived(), (*variableIndex).get(), ordering); JunctionTreeType junctionTree(etree); - boost::shared_ptr bayesTree; - boost::shared_ptr factorGraph; + std::shared_ptr bayesTree; + std::shared_ptr factorGraph; boost::tie(bayesTree,factorGraph) = junctionTree.eliminate(function); // If any factors are remaining, the ordering was incomplete if(!factorGraph->empty()) @@ -150,7 +150,7 @@ namespace gtsam { /* ************************************************************************* */ template - std::pair::BayesNetType>, boost::shared_ptr > + std::pair::BayesNetType>, std::shared_ptr > EliminateableFactorGraph::eliminatePartialSequential( const Ordering& ordering, const Eliminate& function, OptionalVariableIndex variableIndex) const { @@ -168,7 +168,7 @@ namespace gtsam { /* ************************************************************************* */ template - std::pair::BayesNetType>, boost::shared_ptr > + std::pair::BayesNetType>, std::shared_ptr > EliminateableFactorGraph::eliminatePartialSequential( const KeyVector& variables, const Eliminate& function, OptionalVariableIndex variableIndex) const { @@ -189,7 +189,7 @@ namespace gtsam { /* ************************************************************************* */ template - std::pair::BayesTreeType>, boost::shared_ptr > + std::pair::BayesTreeType>, std::shared_ptr > EliminateableFactorGraph::eliminatePartialMultifrontal( const Ordering& ordering, const Eliminate& function, OptionalVariableIndex variableIndex) const { @@ -208,7 +208,7 @@ namespace gtsam { /* ************************************************************************* */ template - std::pair::BayesTreeType>, boost::shared_ptr > + std::pair::BayesTreeType>, std::shared_ptr > EliminateableFactorGraph::eliminatePartialMultifrontal( const KeyVector& variables, const Eliminate& function, OptionalVariableIndex variableIndex) const { @@ -229,7 +229,7 @@ namespace gtsam { /* ************************************************************************* */ template - boost::shared_ptr::BayesNetType> + std::shared_ptr::BayesNetType> EliminateableFactorGraph::marginalMultifrontalBayesNet( boost::variant variables, const Eliminate& function, OptionalVariableIndex variableIndex) const @@ -261,7 +261,7 @@ namespace gtsam { /* ************************************************************************* */ template - boost::shared_ptr::BayesNetType> + std::shared_ptr::BayesNetType> EliminateableFactorGraph::marginalMultifrontalBayesNet( boost::variant variables, const Ordering& marginalizedVariableOrdering, @@ -275,8 +275,8 @@ namespace gtsam { gttic(marginalMultifrontalBayesNet); // An ordering was provided for the marginalized variables, so we can first eliminate them // in the order requested. - boost::shared_ptr bayesTree; - boost::shared_ptr factorGraph; + std::shared_ptr bayesTree; + std::shared_ptr factorGraph; boost::tie(bayesTree,factorGraph) = eliminatePartialMultifrontal(marginalizedVariableOrdering, function, variableIndex); @@ -296,7 +296,7 @@ namespace gtsam { /* ************************************************************************* */ template - boost::shared_ptr::BayesTreeType> + std::shared_ptr::BayesTreeType> EliminateableFactorGraph::marginalMultifrontalBayesTree( boost::variant variables, const Eliminate& function, OptionalVariableIndex variableIndex) const @@ -328,7 +328,7 @@ namespace gtsam { /* ************************************************************************* */ template - boost::shared_ptr::BayesTreeType> + std::shared_ptr::BayesTreeType> EliminateableFactorGraph::marginalMultifrontalBayesTree( boost::variant variables, const Ordering& marginalizedVariableOrdering, @@ -342,8 +342,8 @@ namespace gtsam { gttic(marginalMultifrontalBayesTree); // An ordering was provided for the marginalized variables, so we can first eliminate them // in the order requested. - boost::shared_ptr bayesTree; - boost::shared_ptr factorGraph; + std::shared_ptr bayesTree; + std::shared_ptr factorGraph; boost::tie(bayesTree,factorGraph) = eliminatePartialMultifrontal(marginalizedVariableOrdering, function, variableIndex); @@ -363,7 +363,7 @@ namespace gtsam { /* ************************************************************************* */ template - boost::shared_ptr + std::shared_ptr EliminateableFactorGraph::marginal( const KeyVector& variables, const Eliminate& function, OptionalVariableIndex variableIndex) const diff --git a/gtsam/inference/EliminateableFactorGraph.h b/gtsam/inference/EliminateableFactorGraph.h index 8cdf9abca..561c478ff 100644 --- a/gtsam/inference/EliminateableFactorGraph.h +++ b/gtsam/inference/EliminateableFactorGraph.h @@ -18,7 +18,7 @@ #pragma once -#include +#include #include #include #include @@ -84,7 +84,7 @@ namespace gtsam { /// The pair of conditional and remaining factor produced by a single dense elimination step on /// a subgraph. - typedef std::pair, boost::shared_ptr<_FactorType> > EliminationResult; + typedef std::pair, std::shared_ptr<_FactorType> > EliminationResult; /// The function type that does a single dense elimination step on a subgraph. typedef std::function Eliminate; @@ -101,22 +101,22 @@ namespace gtsam { * * Example - Full Cholesky elimination in COLAMD order: * \code - * boost::shared_ptr result = graph.eliminateSequential(EliminateCholesky); + * std::shared_ptr result = graph.eliminateSequential(EliminateCholesky); * \endcode * * Example - METIS ordering for elimination * \code - * boost::shared_ptr result = graph.eliminateSequential(OrderingType::METIS); + * std::shared_ptr result = graph.eliminateSequential(OrderingType::METIS); * \endcode * * Example - Reusing an existing VariableIndex to improve performance, and using COLAMD ordering: * \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, std::nullopt); + * std::shared_ptr result = graph.eliminateSequential(EliminateQR, varIndex, std::nullopt); * \endcode * */ - boost::shared_ptr eliminateSequential( + std::shared_ptr eliminateSequential( OptionalOrderingType orderingType = {}, const Eliminate& function = EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex = {}) const; @@ -125,17 +125,17 @@ namespace gtsam { * * Example - Full QR elimination in specified order: * \code - * boost::shared_ptr result = graph.eliminateSequential(myOrdering, EliminateQR); + * std::shared_ptr result = graph.eliminateSequential(myOrdering, EliminateQR); * \endcode * * Example - Reusing an existing VariableIndex to improve performance: * \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, std::nullopt); + * std::shared_ptr result = graph.eliminateSequential(myOrdering, EliminateQR, varIndex, std::nullopt); * \endcode * */ - boost::shared_ptr eliminateSequential( + std::shared_ptr eliminateSequential( const Ordering& ordering, const Eliminate& function = EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex = {}) const; @@ -146,17 +146,17 @@ namespace gtsam { * * Example - Full Cholesky elimination in COLAMD order: * \code - * boost::shared_ptr result = graph.eliminateMultifrontal(EliminateCholesky); + * std::shared_ptr result = graph.eliminateMultifrontal(EliminateCholesky); * \endcode * * Example - Reusing an existing VariableIndex to improve performance, and using COLAMD ordering: * \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, {}, varIndex); + * std::shared_ptr result = graph.eliminateMultifrontal(EliminateQR, {}, varIndex); * \endcode * */ - boost::shared_ptr eliminateMultifrontal( + std::shared_ptr eliminateMultifrontal( OptionalOrderingType orderingType = {}, const Eliminate& function = EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex = {}) const; @@ -167,10 +167,10 @@ namespace gtsam { * * Example - Full QR elimination in specified order: * \code - * boost::shared_ptr result = graph.eliminateMultifrontal(EliminateQR, myOrdering); + * std::shared_ptr result = graph.eliminateMultifrontal(EliminateQR, myOrdering); * \endcode * */ - boost::shared_ptr eliminateMultifrontal( + std::shared_ptr eliminateMultifrontal( const Ordering& ordering, const Eliminate& function = EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex = {}) const; @@ -179,7 +179,7 @@ namespace gtsam { * and a remaining factor graph. This computes the factorization \f$ p(X) = p(A|B) p(B) \f$, * where \f$ A = \f$ \c variables, \f$ X \f$ is all the variables in the factor graph, and \f$ * B = X\backslash A \f$. */ - std::pair, boost::shared_ptr > + std::pair, std::shared_ptr > eliminatePartialSequential( const Ordering& ordering, const Eliminate& function = EliminationTraitsType::DefaultEliminate, @@ -189,7 +189,7 @@ namespace gtsam { * produce a Bayes net and a remaining factor graph. This computes the factorization \f$ p(X) * = p(A|B) p(B) \f$, where \f$ A = \f$ \c variables, \f$ X \f$ is all the variables in the * factor graph, and \f$ B = X\backslash A \f$. */ - std::pair, boost::shared_ptr > + std::pair, std::shared_ptr > eliminatePartialSequential( const KeyVector& variables, const Eliminate& function = EliminationTraitsType::DefaultEliminate, @@ -199,7 +199,7 @@ namespace gtsam { * tree and a remaining factor graph. This computes the factorization \f$ p(X) = p(A|B) p(B) * \f$, where \f$ A = \f$ \c variables, \f$ X \f$ is all the variables in the factor graph, and * \f$ B = X\backslash A \f$. */ - std::pair, boost::shared_ptr > + std::pair, std::shared_ptr > eliminatePartialMultifrontal( const Ordering& ordering, const Eliminate& function = EliminationTraitsType::DefaultEliminate, @@ -209,7 +209,7 @@ namespace gtsam { * produce a Bayes tree and a remaining factor graph. This computes the factorization \f$ p(X) * = p(A|B) p(B) \f$, where \f$ A = \f$ \c variables, \f$ X \f$ is all the variables in the * factor graph, and \f$ B = X\backslash A \f$. */ - std::pair, boost::shared_ptr > + std::pair, std::shared_ptr > eliminatePartialMultifrontal( const KeyVector& variables, const Eliminate& function = EliminationTraitsType::DefaultEliminate, @@ -224,7 +224,7 @@ namespace gtsam { * used. * @param variableIndex Optional pre-computed VariableIndex for the factor graph, if not * provided one will be computed. */ - boost::shared_ptr marginalMultifrontalBayesNet( + std::shared_ptr marginalMultifrontalBayesNet( boost::variant variables, const Eliminate& function = EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex = {}) const; @@ -239,7 +239,7 @@ namespace gtsam { * used. * @param variableIndex Optional pre-computed VariableIndex for the factor graph, if not * provided one will be computed. */ - boost::shared_ptr marginalMultifrontalBayesNet( + std::shared_ptr marginalMultifrontalBayesNet( boost::variant variables, const Ordering& marginalizedVariableOrdering, const Eliminate& function = EliminationTraitsType::DefaultEliminate, @@ -254,7 +254,7 @@ namespace gtsam { * used. * @param variableIndex Optional pre-computed VariableIndex for the factor graph, if not * provided one will be computed. */ - boost::shared_ptr marginalMultifrontalBayesTree( + std::shared_ptr marginalMultifrontalBayesTree( boost::variant variables, const Eliminate& function = EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex = {}) const; @@ -269,14 +269,14 @@ namespace gtsam { * used. * @param variableIndex Optional pre-computed VariableIndex for the factor graph, if not * provided one will be computed. */ - boost::shared_ptr marginalMultifrontalBayesTree( + std::shared_ptr marginalMultifrontalBayesTree( boost::variant variables, const Ordering& marginalizedVariableOrdering, const Eliminate& function = EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex = {}) const; /** Compute the marginal factor graph of the requested variables. */ - boost::shared_ptr marginal( + std::shared_ptr marginal( const KeyVector& variables, const Eliminate& function = EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex = {}) const; diff --git a/gtsam/inference/EliminationTree-inst.h b/gtsam/inference/EliminationTree-inst.h index ea79c9b55..cd1684d9d 100644 --- a/gtsam/inference/EliminationTree-inst.h +++ b/gtsam/inference/EliminationTree-inst.h @@ -17,7 +17,6 @@ */ #pragma once -#include #include #include @@ -33,7 +32,7 @@ namespace gtsam { template typename EliminationTree::sharedFactor EliminationTree::Node::eliminate( - const boost::shared_ptr& output, + const std::shared_ptr& output, const Eliminate& function, const FastVector& childrenResults) const { // This function eliminates one node (Node::eliminate) - see below eliminate for the whole tree. @@ -98,7 +97,7 @@ namespace gtsam { { // Retrieve the factors involving this variable and create the current node const FactorIndices& factors = structure[order[j]]; - const sharedNode node = boost::make_shared(); + const sharedNode node = std::make_shared(); node->key = order[j]; // for row i \in Struct[A*j] do @@ -183,18 +182,18 @@ namespace gtsam { /* ************************************************************************* */ template - std::pair, boost::shared_ptr > + std::pair, std::shared_ptr > EliminationTree::eliminate(Eliminate function) const { gttic(EliminationTree_eliminate); // Allocate result - auto result = boost::make_shared(); + auto result = std::make_shared(); // Run tree elimination algorithm FastVector remainingFactors = inference::EliminateTree(result, *this, function); // Add remaining factors that were not involved with eliminated variables - auto allRemainingFactors = boost::make_shared(); + auto allRemainingFactors = std::make_shared(); allRemainingFactors->push_back(remainingFactors_.begin(), remainingFactors_.end()); allRemainingFactors->push_back(remainingFactors.begin(), remainingFactors.end()); diff --git a/gtsam/inference/EliminationTree.h b/gtsam/inference/EliminationTree.h index 70e10b3bd..2a3abdcf6 100644 --- a/gtsam/inference/EliminationTree.h +++ b/gtsam/inference/EliminationTree.h @@ -18,7 +18,7 @@ #pragma once #include -#include +#include #include #include @@ -52,32 +52,32 @@ namespace gtsam { { protected: typedef EliminationTree This; ///< This class - typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class + typedef std::shared_ptr shared_ptr; ///< Shared pointer to this class public: typedef GRAPH FactorGraphType; ///< The factor graph type typedef typename GRAPH::FactorType FactorType; ///< The type of factors - typedef typename boost::shared_ptr sharedFactor; ///< Shared pointer to a factor + typedef typename std::shared_ptr sharedFactor; ///< Shared pointer to a factor typedef BAYESNET BayesNetType; ///< The BayesNet corresponding to FACTOR typedef typename BayesNetType::ConditionalType ConditionalType; ///< The type of conditionals - typedef typename boost::shared_ptr sharedConditional; ///< Shared pointer to a conditional + typedef typename std::shared_ptr sharedConditional; ///< Shared pointer to a conditional typedef typename GRAPH::Eliminate Eliminate; struct Node { typedef FastVector Factors; - typedef FastVector > Children; + typedef FastVector > Children; Key key; ///< key associated with root Factors factors; ///< factors associated with root Children children; ///< sub-trees - sharedFactor eliminate(const boost::shared_ptr& output, + sharedFactor eliminate(const std::shared_ptr& output, const Eliminate& function, const FastVector& childrenFactors) const; void print(const std::string& str, const KeyFormatter& keyFormatter) const; }; - typedef boost::shared_ptr sharedNode; ///< Shared pointer to Node + typedef std::shared_ptr sharedNode; ///< Shared pointer to Node protected: /** concept check */ @@ -126,7 +126,7 @@ namespace gtsam { * in GaussianFactorGraph.h * @return The Bayes net and factor graph resulting from elimination */ - std::pair, boost::shared_ptr > + std::pair, std::shared_ptr > eliminate(Eliminate function) const; /// @} diff --git a/gtsam/inference/Factor.h b/gtsam/inference/Factor.h index f59a5972d..74baa4508 100644 --- a/gtsam/inference/Factor.h +++ b/gtsam/inference/Factor.h @@ -22,7 +22,7 @@ #pragma once #include -#include +#include #include #include @@ -70,7 +70,7 @@ namespace gtsam { private: // These typedefs are private because they must be overridden in derived classes. typedef Factor This; ///< This class - typedef boost::shared_ptr shared_ptr; ///< A shared_ptr to this class. + typedef std::shared_ptr shared_ptr; ///< A shared_ptr to this class. public: /// Iterator over keys diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index 2e9dd3d53..62259dd31 100644 --- a/gtsam/inference/FactorGraph.h +++ b/gtsam/inference/FactorGraph.h @@ -30,7 +30,6 @@ #include // for Eigen::aligned_allocator #include -#include #include #include @@ -97,7 +96,7 @@ template class FactorGraph { public: typedef FACTOR FactorType; ///< factor type - typedef boost::shared_ptr + typedef std::shared_ptr sharedFactor; ///< Shared pointer to a factor typedef sharedFactor value_type; typedef typename FastVector::iterator iterator; @@ -105,7 +104,7 @@ class FactorGraph { private: typedef FactorGraph This; ///< Typedef for this class - typedef boost::shared_ptr + typedef std::shared_ptr shared_ptr; ///< Shared pointer for this class /// Check if a DERIVEDFACTOR is in fact derived from FactorType. @@ -168,7 +167,7 @@ class FactorGraph { * FactorGraph fg = {make_shared(), ...}; */ template > - FactorGraph(std::initializer_list> sharedFactors) + FactorGraph(std::initializer_list> sharedFactors) : factors_(sharedFactors) {} /// @} @@ -183,14 +182,14 @@ class FactorGraph { /// Add a factor directly using a shared_ptr. template - IsDerived push_back(boost::shared_ptr factor) { - factors_.push_back(boost::shared_ptr(factor)); + IsDerived push_back(std::shared_ptr factor) { + factors_.push_back(std::shared_ptr(factor)); } /// Emplace a shared pointer to factor of given type. template IsDerived emplace_shared(Args&&... args) { - factors_.push_back(boost::allocate_shared( + factors_.push_back(std::allocate_shared( Eigen::aligned_allocator(), std::forward(args)...)); } @@ -201,13 +200,13 @@ class FactorGraph { */ template IsDerived push_back(const DERIVEDFACTOR& factor) { - factors_.push_back(boost::allocate_shared( + factors_.push_back(std::allocate_shared( Eigen::aligned_allocator(), factor)); } /// `add` is a synonym for push_back. template - IsDerived add(boost::shared_ptr factor) { + IsDerived add(std::shared_ptr factor) { push_back(factor); } @@ -216,7 +215,7 @@ class FactorGraph { typename std::enable_if< std::is_base_of::value, boost::assign::list_inserter>>::type - operator+=(boost::shared_ptr factor) { + operator+=(std::shared_ptr factor) { return boost::assign::make_list_inserter(RefCallPushBack(*this))( factor); } diff --git a/gtsam/inference/ISAM-inst.h b/gtsam/inference/ISAM-inst.h index 6026e052a..b93c58c48 100644 --- a/gtsam/inference/ISAM-inst.h +++ b/gtsam/inference/ISAM-inst.h @@ -41,7 +41,7 @@ void ISAM::updateInternal(const FactorGraphType& newFactors, // Add the orphaned subtrees for (const sharedClique& orphan : *orphans) - factors += boost::make_shared >(orphan); + factors += std::make_shared >(orphan); // Get an ordering where the new keys are eliminated last const VariableIndex index(factors); diff --git a/gtsam/inference/JunctionTree-inst.h b/gtsam/inference/JunctionTree-inst.h index 1c68aa0da..86be13760 100644 --- a/gtsam/inference/JunctionTree-inst.h +++ b/gtsam/inference/JunctionTree-inst.h @@ -47,21 +47,21 @@ struct ConstructorTraversalData { // Pre-order visitor function static ConstructorTraversalData ConstructorTraversalVisitorPre( - const boost::shared_ptr& node, + const std::shared_ptr& node, ConstructorTraversalData& parentData) { // On the pre-order pass, before children have been visited, we just set up // a traversal data structure with its own JT node, and create a child // pointer in its parent. ConstructorTraversalData myData = ConstructorTraversalData(&parentData); myData.junctionTreeNode = - boost::make_shared(node->key, node->factors); + std::make_shared(node->key, node->factors); parentData.junctionTreeNode->addChild(myData.junctionTreeNode); return myData; } // Post-order visitor function static void ConstructorTraversalVisitorPostAlg2( - const boost::shared_ptr& ETreeNode, + const std::shared_ptr& ETreeNode, const ConstructorTraversalData& myData) { // In this post-order visitor, we combine the symbolic elimination results // from the elimination tree children and symbolically eliminate the current @@ -140,7 +140,7 @@ JunctionTree::JunctionTree( typedef ConstructorTraversalData Data; Data rootData(0); // Make a dummy node to gather the junction tree roots - rootData.junctionTreeNode = boost::make_shared(); + rootData.junctionTreeNode = std::make_shared(); treeTraversal::DepthFirstForest(eliminationTree, rootData, Data::ConstructorTraversalVisitorPre, Data::ConstructorTraversalVisitorPostAlg2); diff --git a/gtsam/inference/JunctionTree.h b/gtsam/inference/JunctionTree.h index d1a69f944..351d6ae4b 100644 --- a/gtsam/inference/JunctionTree.h +++ b/gtsam/inference/JunctionTree.h @@ -52,7 +52,7 @@ namespace gtsam { public: typedef JunctionTree This; ///< This class - typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class + typedef std::shared_ptr shared_ptr; ///< Shared pointer to this class typedef EliminatableClusterTree Base; ///< Our base class protected: diff --git a/gtsam/inference/MetisIndex.h b/gtsam/inference/MetisIndex.h index 7431bff4c..4b1f3dfce 100644 --- a/gtsam/inference/MetisIndex.h +++ b/gtsam/inference/MetisIndex.h @@ -44,7 +44,7 @@ namespace gtsam { */ class GTSAM_EXPORT MetisIndex { public: - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; typedef boost::bimap bm_type; private: diff --git a/gtsam/inference/Ordering.h b/gtsam/inference/Ordering.h index e875ed961..5841203fb 100644 --- a/gtsam/inference/Ordering.h +++ b/gtsam/inference/Ordering.h @@ -43,7 +43,7 @@ public: }; typedef Ordering This; ///< Typedef to this class - typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class + typedef std::shared_ptr shared_ptr; ///< shared_ptr to this class /// Create an empty ordering GTSAM_EXPORT diff --git a/gtsam/inference/VariableIndex.h b/gtsam/inference/VariableIndex.h index fa9944e87..9573f5988 100644 --- a/gtsam/inference/VariableIndex.h +++ b/gtsam/inference/VariableIndex.h @@ -23,8 +23,6 @@ #include #include -#include - #include #include #include @@ -42,7 +40,7 @@ namespace gtsam { */ class GTSAM_EXPORT VariableIndex { public: - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; typedef FactorIndices::iterator Factor_iterator; typedef FactorIndices::const_iterator Factor_const_iterator; diff --git a/gtsam/inference/graph-inl.h b/gtsam/inference/graph-inl.h index 3ea66405b..93bb2a51c 100644 --- a/gtsam/inference/graph-inl.h +++ b/gtsam/inference/graph-inl.h @@ -80,7 +80,7 @@ SDGraph toBoostGraph(const G& graph) { continue; // Cast the factor to the user-specified factor type F - boost::shared_ptr factor = boost::dynamic_pointer_cast(*itFactor); + std::shared_ptr factor = std::dynamic_pointer_cast(*itFactor); // Ignore factors that are not of type F if (!factor) continue; @@ -154,11 +154,11 @@ template class compose_key_visitor : public boost::default_bfs_visitor { private: - boost::shared_ptr config_; + std::shared_ptr config_; public: - compose_key_visitor(boost::shared_ptr config_in) {config_ = config_in;} + compose_key_visitor(std::shared_ptr config_in) {config_ = config_in;} template void tree_edge(Edge edge, const Graph& g) const { KEY key_from = boost::get(boost::vertex_name, g, boost::source(edge, g)); @@ -171,7 +171,7 @@ public: /* ************************************************************************* */ template -boost::shared_ptr composePoses(const G& graph, const PredecessorMap& tree, +std::shared_ptr composePoses(const G& graph, const PredecessorMap& tree, const POSE& rootPose) { //TODO: change edge_weight_t to edge_pose_t @@ -197,7 +197,7 @@ boost::shared_ptr composePoses(const G& graph, const PredecessorMap throw std::invalid_argument("composePoses: only support factors with at most two keys"); // e.g. in pose2graph, nonlinear factor needs to be converted to pose2factor - boost::shared_ptr factor = boost::dynamic_pointer_cast(nl_factor); + std::shared_ptr factor = std::dynamic_pointer_cast(nl_factor); if (!factor) continue; KEY key1 = factor->key1(); @@ -218,7 +218,7 @@ boost::shared_ptr composePoses(const G& graph, const PredecessorMap } // compose poses - boost::shared_ptr config(new Values); + std::shared_ptr config(new Values); KEY rootKey = boost::get(boost::vertex_name, g, root); config->insert(rootKey, rootPose); compose_key_visitor vis(config); @@ -266,7 +266,7 @@ void split(const G& g, const PredecessorMap& tree, G& Ab1, G& Ab2) { continue; } - boost::shared_ptr factor2 = boost::dynamic_pointer_cast< + std::shared_ptr factor2 = std::dynamic_pointer_cast< FACTOR2>(factor); if (!factor2) continue; diff --git a/gtsam/inference/graph.h b/gtsam/inference/graph.h index 1e172e5c7..9f5f4c9bc 100644 --- a/gtsam/inference/graph.h +++ b/gtsam/inference/graph.h @@ -24,7 +24,7 @@ #include #include -#include +#include #include namespace gtsam { @@ -89,7 +89,7 @@ namespace gtsam { * Compose the poses by following the chain specified by the spanning tree */ template - boost::shared_ptr + std::shared_ptr composePoses(const G& graph, const PredecessorMap& tree, const POSE& rootPose); diff --git a/gtsam/inference/inference-inst.h b/gtsam/inference/inference-inst.h index 770b48f63..28570881c 100644 --- a/gtsam/inference/inference-inst.h +++ b/gtsam/inference/inference-inst.h @@ -19,7 +19,6 @@ #pragma once -#include #include #include diff --git a/gtsam/linear/ConjugateGradientSolver.h b/gtsam/linear/ConjugateGradientSolver.h index c92390491..b09b1a352 100644 --- a/gtsam/linear/ConjugateGradientSolver.h +++ b/gtsam/linear/ConjugateGradientSolver.h @@ -30,7 +30,7 @@ class GTSAM_EXPORT ConjugateGradientParameters : public IterativeOptimizationPar public: typedef IterativeOptimizationParameters Base; - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; size_t minIterations_; ///< minimum number of cg iterations size_t maxIterations_; ///< maximum number of cg iterations diff --git a/gtsam/linear/GaussianBayesNet.h b/gtsam/linear/GaussianBayesNet.h index 28397a88a..ee9377eea 100644 --- a/gtsam/linear/GaussianBayesNet.h +++ b/gtsam/linear/GaussianBayesNet.h @@ -39,8 +39,8 @@ namespace gtsam { typedef BayesNet Base; typedef GaussianBayesNet This; typedef GaussianConditional ConditionalType; - typedef boost::shared_ptr shared_ptr; - typedef boost::shared_ptr sharedConditional; + typedef std::shared_ptr shared_ptr; + typedef std::shared_ptr sharedConditional; /// @name Standard Constructors /// @{ @@ -71,7 +71,7 @@ namespace gtsam { */ template GaussianBayesNet( - std::initializer_list > conditionals) + std::initializer_list > conditionals) : Base(conditionals) {} /// Destructor diff --git a/gtsam/linear/GaussianBayesTree.h b/gtsam/linear/GaussianBayesTree.h index e2d8ae87a..3039960c2 100644 --- a/gtsam/linear/GaussianBayesTree.h +++ b/gtsam/linear/GaussianBayesTree.h @@ -38,11 +38,11 @@ namespace gtsam { public: typedef GaussianBayesTreeClique This; typedef BayesTreeCliqueBase Base; - typedef boost::shared_ptr shared_ptr; - typedef boost::weak_ptr weak_ptr; + typedef std::shared_ptr shared_ptr; + typedef std::weak_ptr weak_ptr; GaussianBayesTreeClique() {} virtual ~GaussianBayesTreeClique() {} - GaussianBayesTreeClique(const boost::shared_ptr& conditional) : Base(conditional) {} + GaussianBayesTreeClique(const std::shared_ptr& conditional) : Base(conditional) {} }; /* ************************************************************************* */ @@ -55,7 +55,7 @@ namespace gtsam { public: typedef GaussianBayesTree This; - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; /** Default constructor, creates an empty Bayes tree */ GaussianBayesTree() {} diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index 811893267..e78caa2d9 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -320,7 +320,7 @@ namespace gtsam { for (auto&& key : parents()) newKeys.push_back(key); // Hopefully second newAb copy below is optimized out... - return boost::make_shared(newKeys, newAb, model_); + return std::make_shared(newKeys, newAb, model_); } /* **************************************************************************/ diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h index 8bb65d8a9..1bc70d69a 100644 --- a/gtsam/linear/GaussianConditional.h +++ b/gtsam/linear/GaussianConditional.h @@ -43,7 +43,7 @@ namespace gtsam { { public: typedef GaussianConditional This; ///< Typedef to this class - typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class + typedef std::shared_ptr shared_ptr; ///< shared_ptr to this class typedef JacobianFactor BaseFactor; ///< Typedef to our factor base class typedef Conditional BaseConditional; ///< Typedef to our conditional base class @@ -103,7 +103,7 @@ namespace gtsam { /// Create shared pointer by forwarding arguments to fromMeanAndStddev. template static shared_ptr sharedMeanAndStddev(Args&&... args) { - return boost::make_shared(FromMeanAndStddev(std::forward(args)...)); + return std::make_shared(FromMeanAndStddev(std::forward(args)...)); } /** Combine several GaussianConditional into a single dense GC. The conditionals enumerated by diff --git a/gtsam/linear/GaussianDensity.h b/gtsam/linear/GaussianDensity.h index 16b4e0dfb..417cdac16 100644 --- a/gtsam/linear/GaussianDensity.h +++ b/gtsam/linear/GaussianDensity.h @@ -33,7 +33,7 @@ namespace gtsam { public: - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; /// default constructor needed for serialization GaussianDensity() : diff --git a/gtsam/linear/GaussianEliminationTree.h b/gtsam/linear/GaussianEliminationTree.h index 44bd099e9..c386d25b6 100644 --- a/gtsam/linear/GaussianEliminationTree.h +++ b/gtsam/linear/GaussianEliminationTree.h @@ -30,7 +30,7 @@ namespace gtsam { public: typedef EliminationTree Base; ///< Base class typedef GaussianEliminationTree This; ///< This class - typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class + typedef std::shared_ptr shared_ptr; ///< Shared pointer to this class /** * Build the elimination tree of a factor graph using pre-computed column structure. diff --git a/gtsam/linear/GaussianFactor.h b/gtsam/linear/GaussianFactor.h index eb4ea7e06..4e7e92b74 100644 --- a/gtsam/linear/GaussianFactor.h +++ b/gtsam/linear/GaussianFactor.h @@ -39,7 +39,7 @@ namespace gtsam { { public: typedef GaussianFactor This; ///< This class - typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class + typedef std::shared_ptr shared_ptr; ///< shared_ptr to this class typedef Factor Base; ///< Our base class /** Default constructor creates empty factor */ diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index 0f1163982..f879ea0f6 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -148,10 +148,10 @@ namespace gtsam { // Convert to JacobianFactor if necessary JacobianFactor::shared_ptr jacobianFactor( - boost::dynamic_pointer_cast(factor)); + std::dynamic_pointer_cast(factor)); if (!jacobianFactor) { HessianFactor::shared_ptr hessian( - boost::dynamic_pointer_cast(factor)); + std::dynamic_pointer_cast(factor)); if (hessian) jacobianFactor.reset(new JacobianFactor(*hessian)); else @@ -345,10 +345,10 @@ namespace gtsam { /* ************************************************************************* */ namespace { JacobianFactor::shared_ptr convertToJacobianFactorPtr(const GaussianFactor::shared_ptr &gf) { - JacobianFactor::shared_ptr result = boost::dynamic_pointer_cast(gf); + JacobianFactor::shared_ptr result = std::dynamic_pointer_cast(gf); if( !result ) // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky) - result = boost::make_shared(*gf); + result = std::make_shared(*gf); return result; } } @@ -442,7 +442,7 @@ namespace gtsam { bool hasConstraints(const GaussianFactorGraph& factors) { typedef JacobianFactor J; for (const GaussianFactor::shared_ptr& factor: factors) { - J::shared_ptr jacobian(boost::dynamic_pointer_cast(factor)); + J::shared_ptr jacobian(std::dynamic_pointer_cast(factor)); if (jacobian && jacobian->get_model() && jacobian->get_model()->isConstrained()) { return true; } diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index cacb95119..f8a24245d 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -52,7 +52,7 @@ namespace gtsam { typedef GaussianBayesTree BayesTreeType; ///< Type of Bayes tree typedef GaussianJunctionTree JunctionTreeType; ///< Type of Junction tree /// The default dense elimination function - static std::pair, boost::shared_ptr > + static std::pair, std::shared_ptr > DefaultEliminate(const FactorGraphType& factors, const Ordering& keys) { return EliminatePreferCholesky(factors, keys); } /// The default ordering generation function @@ -79,7 +79,7 @@ namespace gtsam { typedef GaussianFactorGraph This; ///< Typedef to this class typedef FactorGraph Base; ///< Typedef to base factor graph type typedef EliminateableFactorGraph BaseEliminateable; ///< Typedef to base elimination class - typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class + typedef std::shared_ptr shared_ptr; ///< shared_ptr to this class /// @name Constructors /// @{ diff --git a/gtsam/linear/GaussianISAM.h b/gtsam/linear/GaussianISAM.h index 98bf5df88..9344f07f3 100644 --- a/gtsam/linear/GaussianISAM.h +++ b/gtsam/linear/GaussianISAM.h @@ -29,7 +29,7 @@ namespace gtsam { public: typedef ISAM Base; typedef GaussianISAM This; - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; /// @name Standard Constructors /// @{ diff --git a/gtsam/linear/GaussianJunctionTree.h b/gtsam/linear/GaussianJunctionTree.h index c02fd49ed..aef38f4d5 100644 --- a/gtsam/linear/GaussianJunctionTree.h +++ b/gtsam/linear/GaussianJunctionTree.h @@ -40,7 +40,7 @@ namespace gtsam { public: typedef JunctionTree Base; ///< Base class typedef GaussianJunctionTree This; ///< This class - typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class + typedef std::shared_ptr shared_ptr; ///< Shared pointer to this class /** * Build the elimination tree of a factor graph using pre-computed column structure. diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index c5cde2c15..194a7ca53 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -30,7 +30,6 @@ #include #include -#include #include #include #include @@ -381,7 +380,7 @@ void HessianFactor::updateHessian(const KeyVector& infoKeys, /* ************************************************************************* */ GaussianFactor::shared_ptr HessianFactor::negate() const { - shared_ptr result = boost::make_shared(*this); + shared_ptr result = std::make_shared(*this); // Negate the information matrix of the result result->info_.negate(); return result; @@ -464,7 +463,7 @@ Vector HessianFactor::gradient(Key key, const VectorValues& x) const { } /* ************************************************************************* */ -boost::shared_ptr HessianFactor::eliminateCholesky(const Ordering& keys) { +std::shared_ptr HessianFactor::eliminateCholesky(const Ordering& keys) { gttic(HessianFactor_eliminateCholesky); GaussianConditional::shared_ptr conditional; @@ -477,7 +476,7 @@ boost::shared_ptr HessianFactor::eliminateCholesky(const Or // TODO(frank): pre-allocate GaussianConditional and write into it const VerticalBlockMatrix Ab = info_.split(nFrontals); - conditional = boost::make_shared(keys_, nFrontals, Ab); + conditional = std::make_shared(keys_, nFrontals, Ab); // Erase the eliminated keys in this factor keys_.erase(begin(), begin() + nFrontals); @@ -521,7 +520,7 @@ VectorValues HessianFactor::solve() { } /* ************************************************************************* */ -std::pair, boost::shared_ptr > +std::pair, std::shared_ptr > EliminateCholesky(const GaussianFactorGraph& factors, const Ordering& keys) { gttic(EliminateCholesky); @@ -529,7 +528,7 @@ EliminateCholesky(const GaussianFactorGraph& factors, const Ordering& keys) { HessianFactor::shared_ptr jointFactor; try { Scatter scatter(factors, keys); - jointFactor = boost::make_shared(factors, scatter); + jointFactor = std::make_shared(factors, scatter); } catch (std::invalid_argument&) { throw InvalidDenseElimination( "EliminateCholesky was called with a request to eliminate variables that are not\n" @@ -544,8 +543,8 @@ EliminateCholesky(const GaussianFactorGraph& factors, const Ordering& keys) { } /* ************************************************************************* */ -std::pair, - boost::shared_ptr > EliminatePreferCholesky( +std::pair, + std::shared_ptr > EliminatePreferCholesky( const GaussianFactorGraph& factors, const Ordering& keys) { gttic(EliminatePreferCholesky); diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index 492df138f..0d67030d7 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -23,7 +23,6 @@ #include #include -#include namespace gtsam { @@ -107,7 +106,7 @@ namespace gtsam { typedef GaussianFactor Base; ///< Typedef to base class typedef HessianFactor This; ///< Typedef to this class - typedef boost::shared_ptr shared_ptr; ///< A shared_ptr to this class + typedef std::shared_ptr shared_ptr; ///< A shared_ptr to this class typedef SymmetricBlockMatrix::Block Block; ///< A block from the Hessian matrix typedef SymmetricBlockMatrix::constBlock constBlock; ///< A block from the Hessian matrix (const version) @@ -187,7 +186,7 @@ namespace gtsam { /** Clone this HessianFactor */ GaussianFactor::shared_ptr clone() const override { - return boost::make_shared(*this); } + return std::make_shared(*this); } /** Print the factor for debugging and testing (implementing Testable) */ void print(const std::string& s = "", @@ -349,7 +348,7 @@ namespace gtsam { * In-place elimination that returns a conditional on (ordered) keys specified, and leaves * this factor to be on the remaining keys (separator) only. Does dense partial Cholesky. */ - boost::shared_ptr eliminateCholesky(const Ordering& keys); + std::shared_ptr eliminateCholesky(const Ordering& keys); /// Solve the system A'*A delta = A'*b in-place, return delta as VectorValues VectorValues solve(); @@ -389,7 +388,7 @@ namespace gtsam { * @return The conditional and remaining factor * * \ingroup LinearSolving */ -GTSAM_EXPORT std::pair, boost::shared_ptr > +GTSAM_EXPORT std::pair, std::shared_ptr > EliminateCholesky(const GaussianFactorGraph& factors, const Ordering& keys); /** @@ -407,7 +406,7 @@ GTSAM_EXPORT std::pair, boost::shared_ptr * @return The conditional and remaining factor * * \ingroup LinearSolving */ -GTSAM_EXPORT std::pair, boost::shared_ptr > +GTSAM_EXPORT std::pair, std::shared_ptr > EliminatePreferCholesky(const GaussianFactorGraph& factors, const Ordering& keys); /// traits diff --git a/gtsam/linear/IterativeSolver.h b/gtsam/linear/IterativeSolver.h index 743f072f2..654e2ea12 100644 --- a/gtsam/linear/IterativeSolver.h +++ b/gtsam/linear/IterativeSolver.h @@ -22,7 +22,7 @@ #include #include -#include +#include #include #include @@ -45,7 +45,7 @@ class IterativeOptimizationParameters { public: - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; enum Verbosity { SILENT = 0, COMPLEXITY, ERROR } verbosity_; @@ -85,7 +85,7 @@ public: */ class IterativeSolver { public: - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; IterativeSolver() { } virtual ~IterativeSolver() { diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 6e1e7d359..6f8bd99f9 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -32,7 +32,6 @@ #include #include -#include #include #include #include @@ -200,11 +199,11 @@ FastVector _convertOrCastToJacobians( jacobians.reserve(factors.size()); for(const GaussianFactor::shared_ptr& factor: factors) { if (factor) { - if (JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast< + if (JacobianFactor::shared_ptr jf = std::dynamic_pointer_cast< JacobianFactor>(factor)) jacobians.push_back(jf); else - jacobians.push_back(boost::make_shared(*factor)); + jacobians.push_back(std::make_shared(*factor)); } } return jacobians; @@ -792,7 +791,7 @@ std::pair Eliminate // Combine and sort variable blocks in elimination order JacobianFactor::shared_ptr jointFactor; try { - jointFactor = boost::make_shared(factors, keys); + jointFactor = std::make_shared(factors, keys); } catch (std::invalid_argument&) { throw InvalidDenseElimination( "EliminateQR was called with a request to eliminate variables that are not\n" @@ -855,7 +854,7 @@ GaussianConditional::shared_ptr JacobianFactor::splitConditional(size_t nrFronta conditionalNoiseModel = noiseModel::Diagonal::Sigmas(model_->sigmas().segment(Ab_.rowStart(), Ab_.rows())); GaussianConditional::shared_ptr conditional = - boost::make_shared(Base::keys_, nrFrontals, Ab_, conditionalNoiseModel); + std::make_shared(Base::keys_, nrFrontals, Ab_, conditionalNoiseModel); const DenseIndex maxRemainingRows = std::min(Ab_.cols(), originalRowEnd) - Ab_.rowStart() - frontalDim; diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index ae661c642..40b109b11 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -24,7 +24,6 @@ #include #include -#include #include #include @@ -45,7 +44,7 @@ namespace gtsam { * variant that handles constraints (zero sigmas). Computation happens in noiseModel::Gaussian::QR * Returns a conditional on those keys, and a new factor on the separator. */ - GTSAM_EXPORT std::pair, boost::shared_ptr > + GTSAM_EXPORT std::pair, std::shared_ptr > EliminateQR(const GaussianFactorGraph& factors, const Ordering& keys); /** @@ -93,7 +92,7 @@ namespace gtsam { typedef JacobianFactor This; ///< Typedef to this class typedef GaussianFactor Base; ///< Typedef to base class - typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class + typedef std::shared_ptr shared_ptr; ///< shared_ptr to this class typedef VerticalBlockMatrix::Block ABlock; typedef VerticalBlockMatrix::constBlock constABlock; @@ -187,8 +186,8 @@ namespace gtsam { /** Clone this JacobianFactor */ GaussianFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( - boost::make_shared(*this)); + return std::static_pointer_cast( + std::make_shared(*this)); } // Implementing Testable interface @@ -354,7 +353,7 @@ namespace gtsam { JacobianFactor whiten() const; /** Eliminate the requested variables. */ - std::pair, shared_ptr> + std::pair, shared_ptr> eliminate(const Ordering& keys); /** set noiseModel correctly */ @@ -371,7 +370,7 @@ namespace gtsam { * @return The conditional and remaining factor * * \ingroup LinearSolving */ - friend GTSAM_EXPORT std::pair, shared_ptr> + friend GTSAM_EXPORT std::pair, shared_ptr> EliminateQR(const GaussianFactorGraph& factors, const Ordering& keys); /** @@ -381,7 +380,7 @@ namespace gtsam { * NOTE: looks at dimension of noise model to determine how many rows to keep. * @param nrFrontals number of keys to eliminate */ - boost::shared_ptr splitConditional(size_t nrFrontals); + std::shared_ptr splitConditional(size_t nrFrontals); protected: diff --git a/gtsam/linear/KalmanFilter.cpp b/gtsam/linear/KalmanFilter.cpp index 05e82be08..ad6d31e1e 100644 --- a/gtsam/linear/KalmanFilter.cpp +++ b/gtsam/linear/KalmanFilter.cpp @@ -26,7 +26,6 @@ #include #include -#include using namespace std; @@ -45,7 +44,7 @@ KalmanFilter::solve(const GaussianFactorGraph& factorGraph) const { // As this is a filter, all we need is the posterior P(x_t). // This is the last GaussianConditional in the resulting BayesNet GaussianConditional::shared_ptr posterior = *(--bayesNet->end()); - return boost::make_shared(*posterior); + return std::make_shared(*posterior); } /* ************************************************************************* */ @@ -93,7 +92,7 @@ KalmanFilter::State KalmanFilter::predict(const State& p, const Matrix& F, // f2(x_{t},x_{t+1}) = (F*x_{t} + B*u - x_{t+1}) * Q^-1 * (F*x_{t} + B*u - x_{t+1})^T Key k = step(p); return fuse(p, - boost::make_shared(k, -F, k + 1, I_, B * u, model)); + std::make_shared(k, -F, k + 1, I_, B * u, model)); } /* ************************************************************************* */ @@ -119,7 +118,7 @@ KalmanFilter::State KalmanFilter::predictQ(const State& p, const Matrix& F, double f = dot(b, g2); Key k = step(p); return fuse(p, - boost::make_shared(k, k + 1, G11, G12, g1, G22, g2, f)); + std::make_shared(k, k + 1, G11, G12, g1, G22, g2, f)); } /* ************************************************************************* */ @@ -128,7 +127,7 @@ KalmanFilter::State KalmanFilter::predict2(const State& p, const Matrix& A0, // Nhe factor related to the motion model is defined as // f2(x_{t},x_{t+1}) = |A0*x_{t} + A1*x_{t+1} - b|^2 Key k = step(p); - return fuse(p, boost::make_shared(k, A0, k + 1, A1, b, model)); + return fuse(p, std::make_shared(k, A0, k + 1, A1, b, model)); } /* ************************************************************************* */ @@ -138,7 +137,7 @@ KalmanFilter::State KalmanFilter::update(const State& p, const Matrix& H, // f2 = (h(x_{t}) - z_{t}) * R^-1 * (h(x_{t}) - z_{t})^T // = (x_{t} - z_{t}) * R^-1 * (x_{t} - z_{t})^T Key k = step(p); - return fuse(p, boost::make_shared(k, H, z, model)); + return fuse(p, std::make_shared(k, H, z, model)); } /* ************************************************************************* */ @@ -149,7 +148,7 @@ KalmanFilter::State KalmanFilter::updateQ(const State& p, const Matrix& H, Matrix G = Ht * M * H; Vector g = Ht * M * z; double f = dot(z, M * z); - return fuse(p, boost::make_shared(k, G, g, f)); + return fuse(p, std::make_shared(k, G, g, f)); } /* ************************************************************************* */ diff --git a/gtsam/linear/LossFunctions.h b/gtsam/linear/LossFunctions.h index ee05cb987..c84ea68fe 100644 --- a/gtsam/linear/LossFunctions.h +++ b/gtsam/linear/LossFunctions.h @@ -65,7 +65,7 @@ class GTSAM_EXPORT Base { /** the rows can be weighted independently according to the error * or uniformly with the norm of the right hand side */ enum ReweightScheme { Scalar, Block }; - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; protected: /// Strategy for reweighting \sa ReweightScheme @@ -145,7 +145,7 @@ class GTSAM_EXPORT Base { */ class GTSAM_EXPORT Null : public Base { public: - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; Null(const ReweightScheme reweight = Block) : Base(reweight) {} ~Null() override {} @@ -177,7 +177,7 @@ class GTSAM_EXPORT Fair : public Base { double c_; public: - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; Fair(double c = 1.3998, const ReweightScheme reweight = Block); double weight(double distance) const override; @@ -210,7 +210,7 @@ class GTSAM_EXPORT Huber : public Base { double k_; public: - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; Huber(double k = 1.345, const ReweightScheme reweight = Block); double weight(double distance) const override; @@ -248,7 +248,7 @@ class GTSAM_EXPORT Cauchy : public Base { double k_, ksquared_; public: - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; Cauchy(double k = 0.1, const ReweightScheme reweight = Block); double weight(double distance) const override; @@ -282,7 +282,7 @@ class GTSAM_EXPORT Tukey : public Base { double c_, csquared_; public: - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; Tukey(double c = 4.6851, const ReweightScheme reweight = Block); double weight(double distance) const override; @@ -315,7 +315,7 @@ class GTSAM_EXPORT Welsch : public Base { double c_, csquared_; public: - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; Welsch(double c = 2.9846, const ReweightScheme reweight = Block); double weight(double distance) const override; @@ -348,7 +348,7 @@ class GTSAM_EXPORT Welsch : public Base { */ class GTSAM_EXPORT GemanMcClure : public Base { public: - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; GemanMcClure(double c = 1.0, const ReweightScheme reweight = Block); ~GemanMcClure() override {} @@ -386,7 +386,7 @@ class GTSAM_EXPORT GemanMcClure : public Base { */ class GTSAM_EXPORT DCS : public Base { public: - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; DCS(double c = 1.0, const ReweightScheme reweight = Block); ~DCS() override {} @@ -428,7 +428,7 @@ class GTSAM_EXPORT L2WithDeadZone : public Base { double k_; public: - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; L2WithDeadZone(double k = 1.0, const ReweightScheme reweight = Block); double weight(double distance) const override; diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index bfd005973..32dd1c535 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -20,7 +20,6 @@ #include #include -#include #include #include @@ -93,7 +92,7 @@ Gaussian::shared_ptr Gaussian::SqrtInformation(const Matrix& R, bool smart) { return Diagonal::Sigmas(diagonal->array().inverse(), true); } // NOTE(frank): only reaches here if !(smart && diagonal) - return boost::make_shared(R.rows(), R); + return std::make_shared(R.rows(), R); } /* ************************************************************************* */ @@ -109,7 +108,7 @@ Gaussian::shared_ptr Gaussian::Information(const Matrix& information, bool smart else { Eigen::LLT llt(information); Matrix R = llt.matrixU(); - return boost::make_shared(n, R); + return std::make_shared(n, R); } } diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 37be152f4..645584e2d 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -55,7 +55,7 @@ namespace gtsam { class GTSAM_EXPORT Base { public: - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; protected: @@ -182,7 +182,7 @@ namespace gtsam { public: - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; /** constructor takes square root information matrix */ Gaussian(size_t dim = 1, @@ -251,7 +251,7 @@ namespace gtsam { * @param Ab is the m*(n+1) augmented system matrix [A b] * @return Empty SharedDiagonal() noise model: R,d are whitened */ - virtual boost::shared_ptr QR(Matrix& Ab) const; + virtual std::shared_ptr QR(Matrix& Ab) const; /// Return R itself, but note that Whiten(H) is cheaper than R*H virtual Matrix R() const { return thisR();} @@ -299,7 +299,7 @@ namespace gtsam { /** constructor - no initializations, for serialization */ Diagonal(); - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; ~Diagonal() override {} @@ -395,7 +395,7 @@ namespace gtsam { public: - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; /** * protected constructor takes sigmas. @@ -530,7 +530,7 @@ namespace gtsam { ~Isotropic() override {} - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; /** * An isotropic noise model created by specifying a standard devation sigma @@ -586,7 +586,7 @@ namespace gtsam { class GTSAM_EXPORT Unit : public Isotropic { public: - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; /** constructor for serialization */ Unit(size_t dim=1): Isotropic(dim,1.0) {} @@ -643,7 +643,7 @@ namespace gtsam { */ class GTSAM_EXPORT Robust : public Base { public: - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; protected: typedef mEstimator::Base RobustModel; diff --git a/gtsam/linear/PCGSolver.cpp b/gtsam/linear/PCGSolver.cpp index 141412159..e98cbf69d 100644 --- a/gtsam/linear/PCGSolver.cpp +++ b/gtsam/linear/PCGSolver.cpp @@ -45,7 +45,7 @@ PCGSolver::PCGSolver(const PCGSolverParameters &p) { preconditioner_ = createPreconditioner(p.preconditioner_); } -void PCGSolverParameters::setPreconditionerParams(const boost::shared_ptr preconditioner) { +void PCGSolverParameters::setPreconditionerParams(const std::shared_ptr preconditioner) { preconditioner_ = preconditioner; } diff --git a/gtsam/linear/PCGSolver.h b/gtsam/linear/PCGSolver.h index cb90ae520..5ed2c7c6d 100644 --- a/gtsam/linear/PCGSolver.h +++ b/gtsam/linear/PCGSolver.h @@ -36,7 +36,7 @@ struct PreconditionerParameters; struct GTSAM_EXPORT PCGSolverParameters: public ConjugateGradientParameters { public: typedef ConjugateGradientParameters Base; - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; PCGSolverParameters() { } @@ -51,9 +51,9 @@ public: // needed for python wrapper void print(const std::string &s) const; - boost::shared_ptr preconditioner_; + std::shared_ptr preconditioner_; - void setPreconditionerParams(const boost::shared_ptr preconditioner); + void setPreconditionerParams(const std::shared_ptr preconditioner); }; /** @@ -62,12 +62,12 @@ public: class GTSAM_EXPORT PCGSolver: public IterativeSolver { public: typedef IterativeSolver Base; - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; protected: PCGSolverParameters parameters_; - boost::shared_ptr preconditioner_; + std::shared_ptr preconditioner_; public: /* Interface to initialize a solver without a problem */ diff --git a/gtsam/linear/Preconditioner.cpp b/gtsam/linear/Preconditioner.cpp index 24a42afb8..cb7e36503 100644 --- a/gtsam/linear/Preconditioner.cpp +++ b/gtsam/linear/Preconditioner.cpp @@ -12,7 +12,7 @@ #include #include #include -#include +#include #include #include #include @@ -183,18 +183,18 @@ void BlockJacobiPreconditioner::clean() { } /***************************************************************************************/ -boost::shared_ptr createPreconditioner( - const boost::shared_ptr params) { - using boost::dynamic_pointer_cast; +std::shared_ptr createPreconditioner( + const std::shared_ptr params) { + using std::dynamic_pointer_cast; if (dynamic_pointer_cast(params)) { - return boost::make_shared(); + return std::make_shared(); } else if (dynamic_pointer_cast( params)) { - return boost::make_shared(); + return std::make_shared(); } else if (auto subgraph = dynamic_pointer_cast( params)) { - return boost::make_shared(*subgraph); + return std::make_shared(*subgraph); } throw invalid_argument( diff --git a/gtsam/linear/Preconditioner.h b/gtsam/linear/Preconditioner.h index 1a5a08090..ebd87751c 100644 --- a/gtsam/linear/Preconditioner.h +++ b/gtsam/linear/Preconditioner.h @@ -9,7 +9,7 @@ #pragma once #include -#include +#include #include #include #include @@ -23,7 +23,7 @@ class VectorValues; /* parameters for the preconditioner */ struct GTSAM_EXPORT PreconditionerParameters { - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; enum Kernel { /* Preconditioner Kernel */ GTSAM = 0, @@ -63,7 +63,7 @@ struct GTSAM_EXPORT PreconditionerParameters { * The goal of this class is to provide a general interface to all preconditioners */ class GTSAM_EXPORT Preconditioner { public: - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; typedef std::vector Dimensions; /* Generic Constructor and Destructor */ @@ -94,7 +94,7 @@ public: /*******************************************************************************************/ struct GTSAM_EXPORT DummyPreconditionerParameters : public PreconditionerParameters { typedef PreconditionerParameters Base; - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; DummyPreconditionerParameters() : Base() {} ~DummyPreconditionerParameters() override {} }; @@ -103,7 +103,7 @@ struct GTSAM_EXPORT DummyPreconditionerParameters : public PreconditionerParamet class GTSAM_EXPORT DummyPreconditioner : public Preconditioner { public: typedef Preconditioner Base; - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; public: @@ -155,7 +155,7 @@ protected: /*********************************************************************************************/ /* factory method to create preconditioners */ -boost::shared_ptr createPreconditioner(const boost::shared_ptr parameters); +std::shared_ptr createPreconditioner(const std::shared_ptr parameters); } diff --git a/gtsam/linear/Sampler.h b/gtsam/linear/Sampler.h index 5be8b445d..eb6549eb5 100644 --- a/gtsam/linear/Sampler.h +++ b/gtsam/linear/Sampler.h @@ -37,7 +37,7 @@ class GTSAM_EXPORT Sampler { mutable std::mt19937_64 generator_; public: - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; /// @name constructors /// @{ diff --git a/gtsam/linear/SubgraphBuilder.cpp b/gtsam/linear/SubgraphBuilder.cpp index de7ae7060..a9a407a1c 100644 --- a/gtsam/linear/SubgraphBuilder.cpp +++ b/gtsam/linear/SubgraphBuilder.cpp @@ -413,19 +413,19 @@ SubgraphBuilder::Weights SubgraphBuilder::weights( break; case SubgraphBuilderParameters::RHS_2NORM: { if (JacobianFactor::shared_ptr jf = - boost::dynamic_pointer_cast(gf)) { + std::dynamic_pointer_cast(gf)) { weight.push_back(jf->getb().norm()); } else if (HessianFactor::shared_ptr hf = - boost::dynamic_pointer_cast(gf)) { + std::dynamic_pointer_cast(gf)) { weight.push_back(hf->linearTerm().norm()); } } break; case SubgraphBuilderParameters::LHS_FNORM: { if (JacobianFactor::shared_ptr jf = - boost::dynamic_pointer_cast(gf)) { + std::dynamic_pointer_cast(gf)) { weight.push_back(std::sqrt(jf->getA().squaredNorm())); } else if (HessianFactor::shared_ptr hf = - boost::dynamic_pointer_cast(gf)) { + std::dynamic_pointer_cast(gf)) { weight.push_back(std::sqrt(hf->information().squaredNorm())); } } break; diff --git a/gtsam/linear/SubgraphBuilder.h b/gtsam/linear/SubgraphBuilder.h index a900c7531..925fdef33 100644 --- a/gtsam/linear/SubgraphBuilder.h +++ b/gtsam/linear/SubgraphBuilder.h @@ -23,7 +23,7 @@ #include #include -#include +#include #include @@ -94,7 +94,7 @@ class GTSAM_EXPORT Subgraph { /****************************************************************************/ struct GTSAM_EXPORT SubgraphBuilderParameters { - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; enum Skeleton { /* augmented tree */ diff --git a/gtsam/linear/SubgraphPreconditioner.cpp b/gtsam/linear/SubgraphPreconditioner.cpp index 4e761a3fd..b0a84bc2e 100644 --- a/gtsam/linear/SubgraphPreconditioner.cpp +++ b/gtsam/linear/SubgraphPreconditioner.cpp @@ -82,9 +82,9 @@ static GaussianFactorGraph convertToJacobianFactors( GaussianFactorGraph result; for (const auto &factor : gfg) if (factor) { - auto jf = boost::dynamic_pointer_cast(factor); + auto jf = std::dynamic_pointer_cast(factor); if (!jf) { - jf = boost::make_shared(*factor); + jf = std::make_shared(*factor); } result.push_back(jf); } diff --git a/gtsam/linear/SubgraphPreconditioner.h b/gtsam/linear/SubgraphPreconditioner.h index 81c8968b1..558a80bd8 100644 --- a/gtsam/linear/SubgraphPreconditioner.h +++ b/gtsam/linear/SubgraphPreconditioner.h @@ -26,7 +26,7 @@ #include #include -#include +#include #include @@ -38,7 +38,7 @@ namespace gtsam { class VectorValues; struct GTSAM_EXPORT SubgraphPreconditionerParameters : public PreconditionerParameters { - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; SubgraphPreconditionerParameters(const SubgraphBuilderParameters &p = SubgraphBuilderParameters()) : builderParams(p) {} SubgraphBuilderParameters builderParams; @@ -54,7 +54,7 @@ namespace gtsam { class GTSAM_EXPORT SubgraphPreconditioner : public Preconditioner { public: - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; private: GaussianFactorGraph Ab2_; diff --git a/gtsam/linear/SubgraphSolver.cpp b/gtsam/linear/SubgraphSolver.cpp index 0156c717e..db8271e8b 100644 --- a/gtsam/linear/SubgraphSolver.cpp +++ b/gtsam/linear/SubgraphSolver.cpp @@ -42,7 +42,7 @@ SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &Ab, auto Rc1 = *Ab1.eliminateSequential(ordering, EliminateQR); auto xbar = Rc1.optimize(); - pc_ = boost::make_shared(Ab2, Rc1, xbar); + pc_ = std::make_shared(Ab2, Rc1, xbar); } /**************************************************************************************************/ @@ -52,7 +52,7 @@ SubgraphSolver::SubgraphSolver(const GaussianBayesNet &Rc1, const Parameters ¶meters) : parameters_(parameters) { auto xbar = Rc1.optimize(); - pc_ = boost::make_shared(Ab2, Rc1, xbar); + pc_ = std::make_shared(Ab2, Rc1, xbar); } /**************************************************************************************************/ diff --git a/gtsam/linear/SubgraphSolver.h b/gtsam/linear/SubgraphSolver.h index 0598b3321..c029165ab 100644 --- a/gtsam/linear/SubgraphSolver.h +++ b/gtsam/linear/SubgraphSolver.h @@ -67,7 +67,7 @@ struct GTSAM_EXPORT SubgraphSolverParameters * * LevenbergMarquardtParams parameters; * parameters.linearSolverType = NonlinearOptimizerParams::CONJUGATE_GRADIENT; - * parameters.iterativeParams = boost::make_shared(); + * parameters.iterativeParams = std::make_shared(); * LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, parameters); * Values result = optimizer.optimize(); * @@ -79,7 +79,7 @@ class GTSAM_EXPORT SubgraphSolver : public IterativeSolver { protected: Parameters parameters_; - boost::shared_ptr pc_; ///< preconditioner object + std::shared_ptr pc_; ///< preconditioner object public: /// @name Constructors diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index 35ab628d3..29cea3976 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -24,7 +24,7 @@ #include #include -#include +#include #include @@ -80,7 +80,7 @@ namespace gtsam { public: typedef Values::iterator iterator; ///< Iterator over vector values typedef Values::const_iterator const_iterator; ///< Const iterator over vector values - typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class + typedef std::shared_ptr shared_ptr; ///< shared_ptr to this class typedef Values::value_type value_type; ///< Typedef to pair typedef value_type KeyValuePair; ///< Typedef to pair typedef std::map Dims; ///< Keyed vector dimensions diff --git a/gtsam/linear/iterative-inl.h b/gtsam/linear/iterative-inl.h index 906ee80fd..c96d643b2 100644 --- a/gtsam/linear/iterative-inl.h +++ b/gtsam/linear/iterative-inl.h @@ -20,7 +20,6 @@ #include #include -#include namespace gtsam { diff --git a/gtsam/linear/linearAlgorithms-inst.h b/gtsam/linear/linearAlgorithms-inst.h index 253e731d2..a8853f2b7 100644 --- a/gtsam/linear/linearAlgorithms-inst.h +++ b/gtsam/linear/linearAlgorithms-inst.h @@ -21,7 +21,7 @@ #include #include -#include +#include #include @@ -52,7 +52,7 @@ namespace gtsam VectorValues collectedResult; OptimizeData operator()( - const boost::shared_ptr& clique, + const std::shared_ptr& clique, OptimizeData& parentData) { OptimizeData myData; diff --git a/gtsam/linear/tests/testGaussianBayesNet.cpp b/gtsam/linear/tests/testGaussianBayesNet.cpp index 7f0641dbf..ec208fa7b 100644 --- a/gtsam/linear/tests/testGaussianBayesNet.cpp +++ b/gtsam/linear/tests/testGaussianBayesNet.cpp @@ -39,13 +39,13 @@ using symbol_shorthand::X; static const Key _x_ = 11, _y_ = 22, _z_ = 33; static GaussianBayesNet smallBayesNet = { - boost::make_shared(_x_, Vector1::Constant(9), I_1x1, _y_, I_1x1), - boost::make_shared(_y_, Vector1::Constant(5), I_1x1)}; + std::make_shared(_x_, Vector1::Constant(9), I_1x1, _y_, I_1x1), + std::make_shared(_y_, Vector1::Constant(5), I_1x1)}; static GaussianBayesNet noisyBayesNet = { - boost::make_shared(_x_, Vector1::Constant(9), I_1x1, _y_, I_1x1, + std::make_shared(_x_, Vector1::Constant(9), I_1x1, _y_, I_1x1, noiseModel::Isotropic::Sigma(1, 2.0)), - boost::make_shared(_y_, Vector1::Constant(5), I_1x1, + std::make_shared(_y_, Vector1::Constant(5), I_1x1, noiseModel::Isotropic::Sigma(1, 3.0))}; /* ************************************************************************* */ diff --git a/gtsam/linear/tests/testGaussianBayesTree.cpp b/gtsam/linear/tests/testGaussianBayesTree.cpp index ba499ea24..52a3123bf 100644 --- a/gtsam/linear/tests/testGaussianBayesTree.cpp +++ b/gtsam/linear/tests/testGaussianBayesTree.cpp @@ -36,13 +36,13 @@ namespace { const Key x1=1, x2=2, x3=3, x4=4; const SharedDiagonal chainNoise = noiseModel::Isotropic::Sigma(1, 0.5); const GaussianFactorGraph chain = { - boost::make_shared( + std::make_shared( x2, I_1x1, x1, I_1x1, (Vector(1) << 1.).finished(), chainNoise), - boost::make_shared( + std::make_shared( x2, I_1x1, x3, I_1x1, (Vector(1) << 1.).finished(), chainNoise), - boost::make_shared( + std::make_shared( x3, I_1x1, x4, I_1x1, (Vector(1) << 1.).finished(), chainNoise), - boost::make_shared( + std::make_shared( x4, I_1x1, (Vector(1) << 1.).finished(), chainNoise)}; const Ordering chainOrdering {x2, x1, x3, x4}; @@ -50,8 +50,8 @@ namespace { // Helper functions for below GaussianBayesTreeClique::shared_ptr LeafClique(const GaussianConditional& conditional) { - return boost::make_shared( - boost::make_shared(conditional)); + return std::make_shared( + std::make_shared(conditional)); } typedef std::vector Children; @@ -305,7 +305,7 @@ TEST(GaussianBayesTree, determinant_and_smallestEigenvalue) { fg += JacobianFactor(x2, -5 * I_2x2, l1, 5 * I_2x2, Vector2(-1.0, 1.5), unit2); // create corresponding Bayes tree: - boost::shared_ptr bt = fg.eliminateMultifrontal(); + std::shared_ptr bt = fg.eliminateMultifrontal(); Matrix H = fg.hessian().first; // test determinant @@ -338,8 +338,8 @@ TEST(GaussianBayesTree, LogDeterminant) { fg += JacobianFactor(x3, 10 * I_2x2, -1.0 * Vector2::Ones(), unit2); // create corresponding Bayes net and Bayes tree: - boost::shared_ptr bn = fg.eliminateSequential(); - boost::shared_ptr bt = fg.eliminateMultifrontal(); + std::shared_ptr bn = fg.eliminateSequential(); + std::shared_ptr bt = fg.eliminateMultifrontal(); // Test logDeterminant EXPECT_DOUBLES_EQUAL(bn->logDeterminant(), bt->logDeterminant(), 1e-9); diff --git a/gtsam/linear/tests/testGaussianConditional.cpp b/gtsam/linear/tests/testGaussianConditional.cpp index 0479ce9a1..a4a722012 100644 --- a/gtsam/linear/tests/testGaussianConditional.cpp +++ b/gtsam/linear/tests/testGaussianConditional.cpp @@ -27,7 +27,6 @@ #include #include -#include #include #include diff --git a/gtsam/linear/tests/testGaussianFactorGraph.cpp b/gtsam/linear/tests/testGaussianFactorGraph.cpp index 0d7003ccb..2ed4e6589 100644 --- a/gtsam/linear/tests/testGaussianFactorGraph.cpp +++ b/gtsam/linear/tests/testGaussianFactorGraph.cpp @@ -386,7 +386,7 @@ TEST(GaussianFactorGraph, clone) { // Apply an in-place change to init_graph and compare JacobianFactor::shared_ptr jacFactor0 = - boost::dynamic_pointer_cast(init_graph.at(0)); + std::dynamic_pointer_cast(init_graph.at(0)); CHECK(jacFactor0); jacFactor0->getA(jacFactor0->begin()) *= 7.; EXPECT(assert_inequal(init_graph, exp_graph)); diff --git a/gtsam/linear/tests/testHessianFactor.cpp b/gtsam/linear/tests/testHessianFactor.cpp index 92ffe71ac..5fd6e833f 100644 --- a/gtsam/linear/tests/testHessianFactor.cpp +++ b/gtsam/linear/tests/testHessianFactor.cpp @@ -467,7 +467,7 @@ TEST(HessianFactor, combine) { Vector b = Vector2(2.23606798,-1.56524758); SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector::Ones(2)); GaussianFactorGraph factors{ - boost::make_shared(0, A0, 1, A1, 2, A2, b, model)}; + std::make_shared(0, A0, 1, A1, 2, A2, b, model)}; // Form Ab' * Ab HessianFactor actual(factors); diff --git a/gtsam/linear/tests/testJacobianFactor.cpp b/gtsam/linear/tests/testJacobianFactor.cpp index e0f4d8a0b..ad722eb94 100644 --- a/gtsam/linear/tests/testJacobianFactor.cpp +++ b/gtsam/linear/tests/testJacobianFactor.cpp @@ -189,21 +189,21 @@ Key keyX(10), keyY(8), keyZ(12); double sigma1 = 0.1; Matrix A11 = I_2x2; Vector2 b1(2, -1); -auto factor1 = boost::make_shared( +auto factor1 = std::make_shared( keyX, A11, b1, noiseModel::Isotropic::Sigma(2, sigma1)); double sigma2 = 0.5; Matrix A21 = -2 * I_2x2; Matrix A22 = 3 * I_2x2; Vector2 b2(4, -5); -auto factor2 = boost::make_shared( +auto factor2 = std::make_shared( keyX, A21, keyY, A22, b2, noiseModel::Isotropic::Sigma(2, sigma2)); double sigma3 = 1.0; Matrix A32 = -4 * I_2x2; Matrix A33 = 5 * I_2x2; Vector2 b3(3, -6); -auto factor3 = boost::make_shared( +auto factor3 = std::make_shared( keyY, A32, keyZ, A33, b3, noiseModel::Isotropic::Sigma(2, sigma3)); const GaussianFactorGraph factors { factor1, factor2, factor3 }; @@ -438,11 +438,11 @@ TEST(JacobianFactor, eliminate) JacobianFactor combinedFactor(0, A0, 1, A1, b, noiseModel::Diagonal::Sigmas(sigmas, true)); GaussianFactorGraph::EliminationResult expected = combinedFactor.eliminate(Ordering{0}); - JacobianFactor::shared_ptr expectedJacobian = boost::dynamic_pointer_cast< + JacobianFactor::shared_ptr expectedJacobian = std::dynamic_pointer_cast< JacobianFactor>(expected.second); GaussianFactorGraph::EliminationResult actual = EliminateQR(gfg, Ordering{0}); - JacobianFactor::shared_ptr actualJacobian = boost::dynamic_pointer_cast< + JacobianFactor::shared_ptr actualJacobian = std::dynamic_pointer_cast< JacobianFactor>(actual.second); EXPECT(assert_equal(*expected.first, *actual.first)); @@ -541,10 +541,10 @@ TEST(JacobianFactor, EliminateQR) const SharedDiagonal sig_4D = noiseModel::Isotropic::Sigma(4, 0.5); const SharedDiagonal sig_2D = noiseModel::Isotropic::Sigma(2, 0.5); GaussianFactorGraph factors = { - boost::make_shared(KeyVector{3, 5, 7, 9, 11}, VerticalBlockMatrix(Dims{2, 2, 2, 2, 2, 1}, Ab.block(0, 0, 4, 11)), sig_4D), - boost::make_shared(KeyVector{5, 7, 9, 11}, VerticalBlockMatrix(Dims{2, 2, 2, 2, 1}, Ab.block(4, 2, 4, 9)), sig_4D), - boost::make_shared(KeyVector{7, 9, 11}, VerticalBlockMatrix(Dims{2, 2, 2, 1}, Ab.block(8, 4, 4, 7)), sig_4D), - boost::make_shared(KeyVector{11}, VerticalBlockMatrix(Dims{2, 1}, Ab.block(12, 8, 2, 3)), sig_2D)}; + std::make_shared(KeyVector{3, 5, 7, 9, 11}, VerticalBlockMatrix(Dims{2, 2, 2, 2, 2, 1}, Ab.block(0, 0, 4, 11)), sig_4D), + std::make_shared(KeyVector{5, 7, 9, 11}, VerticalBlockMatrix(Dims{2, 2, 2, 2, 1}, Ab.block(4, 2, 4, 9)), sig_4D), + std::make_shared(KeyVector{7, 9, 11}, VerticalBlockMatrix(Dims{2, 2, 2, 1}, Ab.block(8, 4, 4, 7)), sig_4D), + std::make_shared(KeyVector{11}, VerticalBlockMatrix(Dims{2, 1}, Ab.block(12, 8, 2, 3)), sig_2D)}; // extract the dense matrix for the graph Matrix actualDense = factors.augmentedJacobian(); diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index c382e0f3c..f1830f37f 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -129,14 +129,14 @@ TEST(NoiseModel, equals) //TEST(NoiseModel, ConstrainedSmart ) //{ // Gaussian::shared_ptr nonconstrained = Constrained::MixedSigmas((Vector3(sigma, 0.0, sigma), true); -// Diagonal::shared_ptr n1 = boost::dynamic_pointer_cast(nonconstrained); -// Constrained::shared_ptr n2 = boost::dynamic_pointer_cast(nonconstrained); +// Diagonal::shared_ptr n1 = std::dynamic_pointer_cast(nonconstrained); +// Constrained::shared_ptr n2 = std::dynamic_pointer_cast(nonconstrained); // EXPECT(n1); // EXPECT(!n2); // // Gaussian::shared_ptr constrained = Constrained::MixedSigmas(zero(3), true); -// Diagonal::shared_ptr c1 = boost::dynamic_pointer_cast(constrained); -// Constrained::shared_ptr c2 = boost::dynamic_pointer_cast(constrained); +// Diagonal::shared_ptr c1 = std::dynamic_pointer_cast(constrained); +// Constrained::shared_ptr c2 = std::dynamic_pointer_cast(constrained); // EXPECT(c1); // EXPECT(c2); //} diff --git a/gtsam/linear/tests/testSerializationLinear.cpp b/gtsam/linear/tests/testSerializationLinear.cpp index da093db97..38577849a 100644 --- a/gtsam/linear/tests/testSerializationLinear.cpp +++ b/gtsam/linear/tests/testSerializationLinear.cpp @@ -228,10 +228,10 @@ TEST (Serialization, gaussian_bayes_tree) { const Ordering chainOrdering {x2, x1, x3, x4}; const SharedDiagonal chainNoise = noiseModel::Isotropic::Sigma(1, 0.5); const GaussianFactorGraph chain = { - boost::make_shared(x2, (Matrix(1, 1) << 1.).finished(), x1, (Matrix(1, 1) << 1.).finished(), (Vector(1) << 1.).finished(), chainNoise), - boost::make_shared(x2, (Matrix(1, 1) << 1.).finished(), x3, (Matrix(1, 1) << 1.).finished(), (Vector(1) << 1.).finished(), chainNoise), - boost::make_shared(x3, (Matrix(1, 1) << 1.).finished(), x4, (Matrix(1, 1) << 1.).finished(), (Vector(1) << 1.).finished(), chainNoise), - boost::make_shared(x4, (Matrix(1, 1) << 1.).finished(), (Vector(1) << 1.).finished(), chainNoise)}; + std::make_shared(x2, (Matrix(1, 1) << 1.).finished(), x1, (Matrix(1, 1) << 1.).finished(), (Vector(1) << 1.).finished(), chainNoise), + std::make_shared(x2, (Matrix(1, 1) << 1.).finished(), x3, (Matrix(1, 1) << 1.).finished(), (Vector(1) << 1.).finished(), chainNoise), + std::make_shared(x3, (Matrix(1, 1) << 1.).finished(), x4, (Matrix(1, 1) << 1.).finished(), (Vector(1) << 1.).finished(), chainNoise), + std::make_shared(x4, (Matrix(1, 1) << 1.).finished(), (Vector(1) << 1.).finished(), chainNoise)}; GaussianBayesTree init = *chain.eliminateMultifrontal(chainOrdering); GaussianBayesTree expected = *chain.eliminateMultifrontal(chainOrdering); diff --git a/gtsam/navigation/AHRSFactor.cpp b/gtsam/navigation/AHRSFactor.cpp index aeaff58eb..39969a8f3 100644 --- a/gtsam/navigation/AHRSFactor.cpp +++ b/gtsam/navigation/AHRSFactor.cpp @@ -98,7 +98,7 @@ AHRSFactor::AHRSFactor( gtsam::NonlinearFactor::shared_ptr AHRSFactor::clone() const { //------------------------------------------------------------------------------ - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } @@ -189,7 +189,7 @@ AHRSFactor::AHRSFactor(Key rot_i, Key rot_j, Key bias, : Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), rot_i, rot_j, bias), _PIM_(pim) { - auto p = boost::make_shared(pim.p()); + auto p = std::make_shared(pim.p()); p->body_P_sensor = body_P_sensor; _PIM_.p_ = p; } @@ -199,7 +199,7 @@ Rot3 AHRSFactor::predict(const Rot3& rot_i, const Vector3& bias, const PreintegratedAhrsMeasurements& pim, const Vector3& omegaCoriolis, const std::optional& body_P_sensor) { - auto p = boost::make_shared(pim.p()); + auto p = std::make_shared(pim.p()); p->omegaCoriolis = omegaCoriolis; p->body_P_sensor = body_P_sensor; PreintegratedAhrsMeasurements newPim = pim; diff --git a/gtsam/navigation/AHRSFactor.h b/gtsam/navigation/AHRSFactor.h index 625400a1b..bf7ef56b0 100644 --- a/gtsam/navigation/AHRSFactor.h +++ b/gtsam/navigation/AHRSFactor.h @@ -51,7 +51,7 @@ class GTSAM_EXPORT PreintegratedAhrsMeasurements : public PreintegratedRotation * Default constructor, initialize with no measurements * @param bias Current estimate of acceleration and rotation rate biases */ - PreintegratedAhrsMeasurements(const boost::shared_ptr& p, + PreintegratedAhrsMeasurements(const std::shared_ptr& p, const Vector3& biasHat) : PreintegratedRotation(p), biasHat_(biasHat) { resetIntegration(); @@ -67,7 +67,7 @@ class GTSAM_EXPORT PreintegratedAhrsMeasurements : public PreintegratedRotation * @param preint_meas_cov: Pre-integration covariance */ PreintegratedAhrsMeasurements( - const boost::shared_ptr& p, + const std::shared_ptr& p, const Vector3& bias_hat, double deltaTij, const Rot3& deltaRij, @@ -77,7 +77,7 @@ class GTSAM_EXPORT PreintegratedAhrsMeasurements : public PreintegratedRotation biasHat_(bias_hat), preintMeasCov_(preint_meas_cov) {} - Params& p() const { return *boost::static_pointer_cast(p_);} + Params& p() const { return *std::static_pointer_cast(p_);} const Vector3& biasHat() const { return biasHat_; } const Matrix3& preintMeasCov() const { return preintMeasCov_; } @@ -113,7 +113,7 @@ class GTSAM_EXPORT PreintegratedAhrsMeasurements : public PreintegratedRotation /// @deprecated constructor, but used in tests. PreintegratedAhrsMeasurements(const Vector3& biasHat, const Matrix3& measuredOmegaCovariance) - : PreintegratedRotation(boost::make_shared()), biasHat_(biasHat) { + : PreintegratedRotation(std::make_shared()), biasHat_(biasHat) { p_->gyroscopeCovariance = measuredOmegaCovariance; resetIntegration(); } @@ -147,9 +147,9 @@ public: /** Shorthand for a smart pointer to a factor */ #if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5 - typedef typename boost::shared_ptr shared_ptr; + typedef typename std::shared_ptr shared_ptr; #else - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; #endif /** diff --git a/gtsam/navigation/AttitudeFactor.h b/gtsam/navigation/AttitudeFactor.h index 4dc5ecc0a..b412d4d26 100644 --- a/gtsam/navigation/AttitudeFactor.h +++ b/gtsam/navigation/AttitudeFactor.h @@ -86,7 +86,7 @@ public: using Base::evaluateError; /// shorthand for a smart pointer to a factor - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; /// Typedef to this class typedef Rot3AttitudeFactor This; @@ -112,7 +112,7 @@ public: /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } @@ -163,7 +163,7 @@ public: using Base::evaluateError; /// shorthand for a smart pointer to a factor - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; /// Typedef to this class typedef Pose3AttitudeFactor This; @@ -189,7 +189,7 @@ public: /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } diff --git a/gtsam/navigation/BarometricFactor.h b/gtsam/navigation/BarometricFactor.h index aad88a816..c7cf939af 100644 --- a/gtsam/navigation/BarometricFactor.h +++ b/gtsam/navigation/BarometricFactor.h @@ -43,7 +43,7 @@ class GTSAM_EXPORT BarometricFactor : public NoiseModelFactorN { using Base::evaluateError; /// shorthand for a smart pointer to a factor - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; /// Typedef to this class typedef BarometricFactor This; @@ -66,7 +66,7 @@ class GTSAM_EXPORT BarometricFactor : public NoiseModelFactorN { /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index 7958cd3d5..e6dc6e675 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -196,7 +196,7 @@ CombinedImuFactor::CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, //------------------------------------------------------------------------------ gtsam::NonlinearFactor::shared_ptr CombinedImuFactor::clone() const { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 8e6979f1b..407bf9ace 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -79,13 +79,13 @@ struct GTSAM_EXPORT PreintegrationCombinedParams : PreintegrationParams { } // Default Params for a Z-down navigation frame, such as NED: gravity points along positive Z-axis - static boost::shared_ptr MakeSharedD(double g = 9.81) { - return boost::shared_ptr(new PreintegrationCombinedParams(Vector3(0, 0, g))); + static std::shared_ptr MakeSharedD(double g = 9.81) { + return std::shared_ptr(new PreintegrationCombinedParams(Vector3(0, 0, g))); } // Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis - static boost::shared_ptr MakeSharedU(double g = 9.81) { - return boost::shared_ptr(new PreintegrationCombinedParams(Vector3(0, 0, -g))); + static std::shared_ptr MakeSharedU(double g = 9.81) { + return std::shared_ptr(new PreintegrationCombinedParams(Vector3(0, 0, -g))); } void print(const std::string& s="") const override; @@ -157,7 +157,7 @@ public: * @param biasHat Current estimate of acceleration and rotation rate biases */ PreintegratedCombinedMeasurements( - const boost::shared_ptr& p, + const std::shared_ptr& p, const imuBias::ConstantBias& biasHat = imuBias::ConstantBias()) : PreintegrationType(p, biasHat) { preintMeasCov_.setZero(); @@ -185,7 +185,7 @@ public: void resetIntegration() override; /// const reference to params, shadows definition in base class - Params& p() const { return *boost::static_pointer_cast(this->p_); } + Params& p() const { return *std::static_pointer_cast(this->p_); } /// @} /// @name Access instance variables @@ -274,9 +274,9 @@ public: /** Shorthand for a smart pointer to a factor */ #if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5 - typedef typename boost::shared_ptr shared_ptr; + typedef typename std::shared_ptr shared_ptr; #else - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; #endif /** Default constructor - only use for serialization */ diff --git a/gtsam/navigation/GPSFactor.h b/gtsam/navigation/GPSFactor.h index 7965af066..dbc164d1a 100644 --- a/gtsam/navigation/GPSFactor.h +++ b/gtsam/navigation/GPSFactor.h @@ -46,7 +46,7 @@ public: using Base::evaluateError; /// shorthand for a smart pointer to a factor - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; /// Typedef to this class typedef GPSFactor This; @@ -69,7 +69,7 @@ public: /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } @@ -127,7 +127,7 @@ public: using Base::evaluateError; /// shorthand for a smart pointer to a factor - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; /// Typedef to this class typedef GPSFactor2 This; @@ -144,7 +144,7 @@ public: /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index fc74f5034..88e45d318 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -120,7 +120,7 @@ ImuFactor::ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias, //------------------------------------------------------------------------------ NonlinearFactor::shared_ptr ImuFactor::clone() const { - return boost::static_pointer_cast( + return std::static_pointer_cast( NonlinearFactor::shared_ptr(new This(*this))); } @@ -195,7 +195,7 @@ ImuFactor::shared_ptr ImuFactor::Merge(const shared_ptr& f01, // return new factor auto pim02 = Merge(f01->preintegratedMeasurements(), f12->preintegratedMeasurements()); - return boost::make_shared(f01->key<1>(), // P0 + return std::make_shared(f01->key<1>(), // P0 f01->key<2>(), // V0 f12->key<3>(), // P2 f12->key<4>(), // V2 @@ -215,7 +215,7 @@ ImuFactor2::ImuFactor2(Key state_i, Key state_j, Key bias, //------------------------------------------------------------------------------ NonlinearFactor::shared_ptr ImuFactor2::clone() const { - return boost::static_pointer_cast( + return std::static_pointer_cast( NonlinearFactor::shared_ptr(new This(*this))); } diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index 4c1b07a82..3134a8916 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -91,7 +91,7 @@ public: * @param p Parameters, typically fixed in a single application * @param biasHat Current estimate of acceleration and rotation rate biases */ - PreintegratedImuMeasurements(const boost::shared_ptr& p, + PreintegratedImuMeasurements(const std::shared_ptr& p, const imuBias::ConstantBias& biasHat = imuBias::ConstantBias()) : PreintegrationType(p, biasHat) { preintMeasCov_.setZero(); @@ -185,9 +185,9 @@ public: /** Shorthand for a smart pointer to a factor */ #if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5 - typedef typename boost::shared_ptr shared_ptr; + typedef typename std::shared_ptr shared_ptr; #else - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; #endif /** Default constructor - only use for serialization */ diff --git a/gtsam/navigation/MagFactor.h b/gtsam/navigation/MagFactor.h index d0c62d6c4..0422d4779 100644 --- a/gtsam/navigation/MagFactor.h +++ b/gtsam/navigation/MagFactor.h @@ -59,7 +59,7 @@ public: /// @return a deep copy of this factor NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( NonlinearFactor::shared_ptr(new MagFactor(*this))); } @@ -111,7 +111,7 @@ public: /// @return a deep copy of this factor NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( NonlinearFactor::shared_ptr(new MagFactor1(*this))); } @@ -150,7 +150,7 @@ public: /// @return a deep copy of this factor NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( NonlinearFactor::shared_ptr(new MagFactor2(*this))); } @@ -194,7 +194,7 @@ public: /// @return a deep copy of this factor NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( NonlinearFactor::shared_ptr(new MagFactor3(*this))); } diff --git a/gtsam/navigation/MagPoseFactor.h b/gtsam/navigation/MagPoseFactor.h index 67a312af9..45adb2383 100644 --- a/gtsam/navigation/MagPoseFactor.h +++ b/gtsam/navigation/MagPoseFactor.h @@ -45,7 +45,7 @@ class MagPoseFactor: public NoiseModelFactorN { static const int RotDim = traits::dimension; /// Shorthand for a smart pointer to a factor. - using shared_ptr = boost::shared_ptr>; + using shared_ptr = std::shared_ptr>; /// Concept check by type. GTSAM_CONCEPT_TESTABLE_TYPE(POSE) @@ -86,7 +86,7 @@ class MagPoseFactor: public NoiseModelFactorN { /// @return a deep copy of this factor. NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( NonlinearFactor::shared_ptr(new This(*this))); } diff --git a/gtsam/navigation/ManifoldPreintegration.cpp b/gtsam/navigation/ManifoldPreintegration.cpp index baddb73e3..526bf6c73 100644 --- a/gtsam/navigation/ManifoldPreintegration.cpp +++ b/gtsam/navigation/ManifoldPreintegration.cpp @@ -27,7 +27,7 @@ namespace gtsam { //------------------------------------------------------------------------------ ManifoldPreintegration::ManifoldPreintegration( - const boost::shared_ptr& p, const Bias& biasHat) : + const std::shared_ptr& p, const Bias& biasHat) : PreintegrationBase(p, biasHat) { resetIntegration(); } diff --git a/gtsam/navigation/ManifoldPreintegration.h b/gtsam/navigation/ManifoldPreintegration.h index b9135e3e4..0f71ec416 100644 --- a/gtsam/navigation/ManifoldPreintegration.h +++ b/gtsam/navigation/ManifoldPreintegration.h @@ -59,7 +59,7 @@ public: * @param p Parameters, typically fixed in a single application * @param bias Current estimate of acceleration and rotation rate biases */ - ManifoldPreintegration(const boost::shared_ptr& p, + ManifoldPreintegration(const std::shared_ptr& p, const imuBias::ConstantBias& biasHat = imuBias::ConstantBias()); /// @} @@ -106,8 +106,8 @@ public: OptionalJacobian<9, 6> H = {}) const override; /** Dummy clone for MATLAB */ - virtual boost::shared_ptr clone() const { - return boost::shared_ptr(); + virtual std::shared_ptr clone() const { + return std::shared_ptr(); } /// @} diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h index 007deeeca..05489c5fb 100644 --- a/gtsam/navigation/PreintegratedRotation.h +++ b/gtsam/navigation/PreintegratedRotation.h @@ -94,7 +94,7 @@ class GTSAM_EXPORT PreintegratedRotation { protected: /// Parameters - boost::shared_ptr p_; + std::shared_ptr p_; double deltaTij_; ///< Time interval from i to j Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i) @@ -108,12 +108,12 @@ class GTSAM_EXPORT PreintegratedRotation { /// @{ /// Default constructor, resets integration to zero - explicit PreintegratedRotation(const boost::shared_ptr& p) : p_(p) { + explicit PreintegratedRotation(const std::shared_ptr& p) : p_(p) { resetIntegration(); } /// Explicit initialization of all class members - PreintegratedRotation(const boost::shared_ptr& p, + PreintegratedRotation(const std::shared_ptr& p, double deltaTij, const Rot3& deltaRij, const Matrix3& delRdelBiasOmega) : p_(p), deltaTij_(deltaTij), deltaRij_(deltaRij), delRdelBiasOmega_(delRdelBiasOmega) {} @@ -134,7 +134,7 @@ class GTSAM_EXPORT PreintegratedRotation { /// @name Access instance variables /// @{ - const boost::shared_ptr& params() const { + const std::shared_ptr& params() const { return p_; } const double& deltaTij() const { diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index f6e9fccb8..f573c4010 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -22,14 +22,13 @@ #include "PreintegrationBase.h" #include -#include using namespace std; namespace gtsam { //------------------------------------------------------------------------------ -PreintegrationBase::PreintegrationBase(const boost::shared_ptr& p, +PreintegrationBase::PreintegrationBase(const std::shared_ptr& p, const Bias& biasHat) : p_(p), biasHat_(biasHat), deltaTij_(0.0) { } diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index a5e9e9391..1a5366c6c 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -44,7 +44,7 @@ class GTSAM_EXPORT PreintegrationBase { typedef PreintegrationParams Params; protected: - boost::shared_ptr p_; + std::shared_ptr p_; /// Acceleration and gyro bias used for preintegration Bias biasHat_; @@ -67,7 +67,7 @@ class GTSAM_EXPORT PreintegrationBase { * @param p Parameters, typically fixed in a single application * @param bias Current estimate of acceleration and rotation rate biases */ - PreintegrationBase(const boost::shared_ptr& p, + PreintegrationBase(const std::shared_ptr& p, const imuBias::ConstantBias& biasHat = imuBias::ConstantBias()); /// @} @@ -88,7 +88,7 @@ class GTSAM_EXPORT PreintegrationBase { } /// shared pointer to params - const boost::shared_ptr& params() const { + const std::shared_ptr& params() const { return p_; } diff --git a/gtsam/navigation/PreintegrationParams.h b/gtsam/navigation/PreintegrationParams.h index 960f67e24..ab32343b8 100644 --- a/gtsam/navigation/PreintegrationParams.h +++ b/gtsam/navigation/PreintegrationParams.h @@ -17,7 +17,6 @@ #pragma once #include -#include namespace gtsam { @@ -49,13 +48,13 @@ struct GTSAM_EXPORT PreintegrationParams: PreintegratedRotationParams { n_gravity(n_gravity) {} // Default Params for a Z-down navigation frame, such as NED: gravity points along positive Z-axis - static boost::shared_ptr MakeSharedD(double g = 9.81) { - return boost::shared_ptr(new PreintegrationParams(Vector3(0, 0, g))); + static std::shared_ptr MakeSharedD(double g = 9.81) { + return std::shared_ptr(new PreintegrationParams(Vector3(0, 0, g))); } // Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis - static boost::shared_ptr MakeSharedU(double g = 9.81) { - return boost::shared_ptr(new PreintegrationParams(Vector3(0, 0, -g))); + static std::shared_ptr MakeSharedU(double g = 9.81) { + return std::shared_ptr(new PreintegrationParams(Vector3(0, 0, -g))); } void print(const std::string& s="") const override; diff --git a/gtsam/navigation/ScenarioRunner.h b/gtsam/navigation/ScenarioRunner.h index cee5a54ab..4d11793ef 100644 --- a/gtsam/navigation/ScenarioRunner.h +++ b/gtsam/navigation/ScenarioRunner.h @@ -27,7 +27,7 @@ namespace gtsam { static noiseModel::Diagonal::shared_ptr Diagonal(const Matrix& covariance) { bool smart = true; auto model = noiseModel::Gaussian::Covariance(covariance, smart); - auto diagonal = boost::dynamic_pointer_cast(model); + auto diagonal = std::dynamic_pointer_cast(model); if (!diagonal) throw std::invalid_argument("ScenarioRunner::Diagonal: not a diagonal"); return diagonal; @@ -40,7 +40,7 @@ static noiseModel::Diagonal::shared_ptr Diagonal(const Matrix& covariance) { class GTSAM_EXPORT ScenarioRunner { public: typedef imuBias::ConstantBias Bias; - typedef boost::shared_ptr SharedParams; + typedef std::shared_ptr SharedParams; private: const Scenario& scenario_; @@ -113,7 +113,7 @@ class GTSAM_EXPORT ScenarioRunner { */ class GTSAM_EXPORT CombinedScenarioRunner : public ScenarioRunner { public: - typedef boost::shared_ptr SharedParams; + typedef std::shared_ptr SharedParams; private: const SharedParams p_; diff --git a/gtsam/navigation/TangentPreintegration.cpp b/gtsam/navigation/TangentPreintegration.cpp index 56d7aa6d3..e8579daba 100644 --- a/gtsam/navigation/TangentPreintegration.cpp +++ b/gtsam/navigation/TangentPreintegration.cpp @@ -17,14 +17,13 @@ #include "TangentPreintegration.h" #include -#include using namespace std; namespace gtsam { //------------------------------------------------------------------------------ -TangentPreintegration::TangentPreintegration(const boost::shared_ptr& p, +TangentPreintegration::TangentPreintegration(const std::shared_ptr& p, const Bias& biasHat) : PreintegrationBase(p, biasHat) { resetIntegration(); diff --git a/gtsam/navigation/TangentPreintegration.h b/gtsam/navigation/TangentPreintegration.h index 80a39e0f2..d03df5cb8 100644 --- a/gtsam/navigation/TangentPreintegration.h +++ b/gtsam/navigation/TangentPreintegration.h @@ -50,7 +50,7 @@ public: * @param p Parameters, typically fixed in a single application * @param bias Current estimate of acceleration and rotation rate biases */ - TangentPreintegration(const boost::shared_ptr& p, + TangentPreintegration(const std::shared_ptr& p, const imuBias::ConstantBias& biasHat = imuBias::ConstantBias()); /// Virtual destructor @@ -120,8 +120,8 @@ public: /// @} /** Dummy clone for MATLAB */ - virtual boost::shared_ptr clone() const { - return boost::shared_ptr(); + virtual std::shared_ptr clone() const { + return std::shared_ptr(); } /// @} diff --git a/gtsam/navigation/tests/testAHRSFactor.cpp b/gtsam/navigation/tests/testAHRSFactor.cpp index e6a246b9c..89fdd4f71 100644 --- a/gtsam/navigation/tests/testAHRSFactor.cpp +++ b/gtsam/navigation/tests/testAHRSFactor.cpp @@ -131,7 +131,7 @@ TEST( AHRSFactor, PreintegratedAhrsMeasurementsConstructor ) { Matrix3 delRdelBiasOmega = Matrix3::Ones()*0.5; Matrix3 preintMeasCov = Matrix3::Ones()*0.2; PreintegratedAhrsMeasurements actualPim( - boost::make_shared(params), + std::make_shared(params), bias, deltaTij, deltaRij, diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index aacfff0f0..858c9f9fe 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -36,7 +36,7 @@ namespace testing { // Create default parameters with Z-down and above noise parameters -static boost::shared_ptr Params() { +static std::shared_ptr Params() { auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(kGravity); p->gyroscopeCovariance = kGyroSigma * kGyroSigma * I_3x3; p->accelerometerCovariance = kAccelSigma * kAccelSigma * I_3x3; diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 923d2b93f..aa2559575 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -37,7 +37,7 @@ namespace testing { // Create default parameters with Z-down and above noise parameters -static boost::shared_ptr Params() { +static std::shared_ptr Params() { auto p = PreintegrationParams::MakeSharedD(kGravity); p->gyroscopeCovariance = kGyroSigma * kGyroSigma * I_3x3; p->accelerometerCovariance = kAccelSigma * kAccelSigma * I_3x3; @@ -810,7 +810,7 @@ TEST(ImuFactor, bodyPSensorWithBias) { static const double kVelocity = 2.0, kAngularVelocity = M_PI / 6; struct ImuFactorMergeTest { - boost::shared_ptr p_; + std::shared_ptr p_; const ConstantTwistScenario forward_, loop_; ImuFactorMergeTest() @@ -853,10 +853,10 @@ struct ImuFactorMergeTest { EXPECT(assert_equal(pim02_expected, actual_pim02, tol)); ImuFactor::shared_ptr factor01 = - boost::make_shared(X(0), V(0), X(1), V(1), B(0), pim01); + std::make_shared(X(0), V(0), X(1), V(1), B(0), pim01); ImuFactor::shared_ptr factor12 = - boost::make_shared(X(1), V(1), X(2), V(2), B(0), pim12); - ImuFactor::shared_ptr factor02_expected = boost::make_shared( + std::make_shared(X(1), V(1), X(2), V(2), B(0), pim12); + ImuFactor::shared_ptr factor02_expected = std::make_shared( X(0), V(0), X(2), V(2), B(0), pim02_expected); ImuFactor::shared_ptr factor02_merged = ImuFactor::Merge(factor01, factor12); diff --git a/gtsam/navigation/tests/testManifoldPreintegration.cpp b/gtsam/navigation/tests/testManifoldPreintegration.cpp index ba92d8e92..f04663943 100644 --- a/gtsam/navigation/tests/testManifoldPreintegration.cpp +++ b/gtsam/navigation/tests/testManifoldPreintegration.cpp @@ -29,7 +29,7 @@ using namespace std::placeholders; namespace testing { // Create default parameters with Z-down and above noise parameters -static boost::shared_ptr Params() { +static std::shared_ptr Params() { auto p = PreintegrationParams::MakeSharedD(kGravity); p->gyroscopeCovariance = kGyroSigma * kGyroSigma * I_3x3; p->accelerometerCovariance = kAccelSigma * kAccelSigma * I_3x3; diff --git a/gtsam/navigation/tests/testScenarioRunner.cpp b/gtsam/navigation/tests/testScenarioRunner.cpp index adac0fb53..e8b47bfca 100644 --- a/gtsam/navigation/tests/testScenarioRunner.cpp +++ b/gtsam/navigation/tests/testScenarioRunner.cpp @@ -36,7 +36,7 @@ static const Vector3 kAccBias(0.2, 0, 0), kRotBias(0.1, 0, 0.3); static const imuBias::ConstantBias kNonZeroBias(kAccBias, kRotBias); // Create default parameters with Z-up and above noise parameters -static boost::shared_ptr defaultParams() { +static std::shared_ptr defaultParams() { auto p = PreintegrationParams::MakeSharedU(10); p->gyroscopeCovariance = kGyroSigma * kGyroSigma * I_3x3; p->accelerometerCovariance = kAccelSigma * kAccelSigma * I_3x3; diff --git a/gtsam/navigation/tests/testTangentPreintegration.cpp b/gtsam/navigation/tests/testTangentPreintegration.cpp index 2f6678b97..af91f4f2c 100644 --- a/gtsam/navigation/tests/testTangentPreintegration.cpp +++ b/gtsam/navigation/tests/testTangentPreintegration.cpp @@ -35,7 +35,7 @@ Vector9 f(const Vector9& zeta, const Vector3& a, const Vector3& w) { namespace testing { // Create default parameters with Z-down and above noise parameters -static boost::shared_ptr Params() { +static std::shared_ptr Params() { auto p = PreintegrationParams::MakeSharedD(kGravity); p->gyroscopeCovariance = kGyroSigma * kGyroSigma * I_3x3; p->accelerometerCovariance = kAccelSigma * kAccelSigma * I_3x3; diff --git a/gtsam/nonlinear/DoglegOptimizer.h b/gtsam/nonlinear/DoglegOptimizer.h index e278bf075..af6a184c0 100644 --- a/gtsam/nonlinear/DoglegOptimizer.h +++ b/gtsam/nonlinear/DoglegOptimizer.h @@ -71,7 +71,7 @@ protected: DoglegParams params_; public: - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; /// @name Standard interface /// @{ diff --git a/gtsam/nonlinear/Expression-inl.h b/gtsam/nonlinear/Expression-inl.h index 7f7600774..35eef342c 100644 --- a/gtsam/nonlinear/Expression-inl.h +++ b/gtsam/nonlinear/Expression-inl.h @@ -151,7 +151,7 @@ T Expression::value(const Values& values, } template -const boost::shared_ptr >& Expression::root() const { +const std::shared_ptr >& Expression::root() const { return root_; } @@ -284,16 +284,16 @@ std::vector > createUnknowns(size_t n, char c, size_t start) { template ScalarMultiplyExpression::ScalarMultiplyExpression(double s, const Expression& e) - : Expression(boost::make_shared>(s, e)) {} + : Expression(std::make_shared>(s, e)) {} template BinarySumExpression::BinarySumExpression(const Expression& e1, const Expression& e2) - : Expression(boost::make_shared>(e1, e2)) {} + : Expression(std::make_shared>(e1, e2)) {} template Expression& Expression::operator+=(const Expression& e) { - root_ = boost::make_shared>(*this, e); + root_ = std::make_shared>(*this, e); return *this; } diff --git a/gtsam/nonlinear/Expression.h b/gtsam/nonlinear/Expression.h index 758101d36..6e3c634cc 100644 --- a/gtsam/nonlinear/Expression.h +++ b/gtsam/nonlinear/Expression.h @@ -24,7 +24,6 @@ #include #include -#include #include // Forward declare tests @@ -55,10 +54,10 @@ public: protected: // Paul's trick shared pointer, polymorphic root of entire expression tree - boost::shared_ptr > root_; + std::shared_ptr > root_; /// Construct with a custom root - Expression(const boost::shared_ptr >& root) : root_(root) {} + Expression(const std::shared_ptr >& root) : root_(root) {} public: @@ -170,12 +169,12 @@ public: * "deep" is in quotes because the ExpressionNode hierarchy is *not* cloned. * The intent is for derived classes to be copied using only a Base pointer. */ - virtual boost::shared_ptr clone() const { - return boost::make_shared(*this); + virtual std::shared_ptr clone() const { + return std::make_shared(*this); } /// Return root - const boost::shared_ptr >& root() const; + const std::shared_ptr >& root() const; /// Return size needed for memory buffer in traceExecution size_t traceSize() const; diff --git a/gtsam/nonlinear/ExpressionFactor.h b/gtsam/nonlinear/ExpressionFactor.h index 797f47002..f859b3d92 100644 --- a/gtsam/nonlinear/ExpressionFactor.h +++ b/gtsam/nonlinear/ExpressionFactor.h @@ -58,7 +58,7 @@ protected: // Provide access to the Matrix& version of unwhitenedError: using NoiseModelFactor::unwhitenedError; - typedef boost::shared_ptr > shared_ptr; + typedef std::shared_ptr > shared_ptr; /** * Constructor: creates a factor from a measurement and measurement function @@ -112,20 +112,20 @@ protected: } } - boost::shared_ptr linearize(const Values& x) const override { + std::shared_ptr linearize(const Values& x) const override { // Only linearize if the factor is active if (!active(x)) - return boost::shared_ptr(); + return std::shared_ptr(); // In case noise model is constrained, we need to provide a noise model SharedDiagonal noiseModel; if (noiseModel_ && noiseModel_->isConstrained()) { - noiseModel = boost::static_pointer_cast( + noiseModel = std::static_pointer_cast( noiseModel_)->unit(); } // Create a writeable JacobianFactor in advance - boost::shared_ptr factor( + std::shared_ptr factor( new JacobianFactor(keys_, dims_, Dim, noiseModel)); // Wrap keys and VerticalBlockMatrix into structure passed to expression_ @@ -152,7 +152,7 @@ protected: /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } diff --git a/gtsam/nonlinear/ExpressionFactorGraph.h b/gtsam/nonlinear/ExpressionFactorGraph.h index 6c09066c2..5de166f82 100644 --- a/gtsam/nonlinear/ExpressionFactorGraph.h +++ b/gtsam/nonlinear/ExpressionFactorGraph.h @@ -43,7 +43,7 @@ public: void addExpressionFactor(const Expression& h, const T& z, const SharedNoiseModel& R) { using F = ExpressionFactor; - push_back(boost::allocate_shared(Eigen::aligned_allocator(), R, z, h)); + push_back(std::allocate_shared(Eigen::aligned_allocator(), R, z, h)); } /// @} diff --git a/gtsam/nonlinear/ExtendedKalmanFilter-inl.h b/gtsam/nonlinear/ExtendedKalmanFilter-inl.h index e85aceb15..f252454ef 100644 --- a/gtsam/nonlinear/ExtendedKalmanFilter-inl.h +++ b/gtsam/nonlinear/ExtendedKalmanFilter-inl.h @@ -51,7 +51,7 @@ namespace gtsam { // and the key/index needs to be reset to 0, the first key in the next iteration. assert(marginal->nrFrontals() == 1); assert(marginal->nrParents() == 0); - *newPrior = boost::make_shared( + *newPrior = std::make_shared( marginal->keys().front(), marginal->getA(marginal->begin()), marginal->getb() - marginal->getA(marginal->begin()) * result[lastKey], diff --git a/gtsam/nonlinear/ExtendedKalmanFilter.h b/gtsam/nonlinear/ExtendedKalmanFilter.h index a5d6215c1..e931fc561 100644 --- a/gtsam/nonlinear/ExtendedKalmanFilter.h +++ b/gtsam/nonlinear/ExtendedKalmanFilter.h @@ -48,7 +48,7 @@ class ExtendedKalmanFilter { BOOST_CONCEPT_ASSERT((IsManifold)); public: - typedef boost::shared_ptr > shared_ptr; + typedef std::shared_ptr > shared_ptr; typedef VALUE T; protected: diff --git a/gtsam/nonlinear/FunctorizedFactor.h b/gtsam/nonlinear/FunctorizedFactor.h index ee927327f..de82da05f 100644 --- a/gtsam/nonlinear/FunctorizedFactor.h +++ b/gtsam/nonlinear/FunctorizedFactor.h @@ -87,7 +87,7 @@ class FunctorizedFactor : public NoiseModelFactorN { /// @return a deep copy of this factor NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( NonlinearFactor::shared_ptr(new FunctorizedFactor(*this))); } @@ -194,7 +194,7 @@ class FunctorizedFactor2 : public NoiseModelFactorN { /// @return a deep copy of this factor NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( NonlinearFactor::shared_ptr(new FunctorizedFactor2(*this))); } diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h index 065278573..d59eb4371 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -65,9 +65,9 @@ class GncOptimizer { nfg_.resize(graph.size()); for (size_t i = 0; i < graph.size(); i++) { if (graph[i]) { - NoiseModelFactor::shared_ptr factor = boost::dynamic_pointer_cast< + NoiseModelFactor::shared_ptr factor = std::dynamic_pointer_cast< NoiseModelFactor>(graph[i]); - auto robust = boost::dynamic_pointer_cast< + auto robust = std::dynamic_pointer_cast< noiseModel::Robust>(factor->noiseModel()); // if the factor has a robust loss, we remove the robust loss nfg_[i] = robust ? factor-> cloneWithNewNoiseModel(robust->noise()) : factor; @@ -401,10 +401,10 @@ class GncOptimizer { newGraph.resize(nfg_.size()); for (size_t i = 0; i < nfg_.size(); i++) { if (nfg_[i]) { - auto factor = boost::dynamic_pointer_cast< + auto factor = std::dynamic_pointer_cast< NoiseModelFactor>(nfg_[i]); auto noiseModel = - boost::dynamic_pointer_cast( + std::dynamic_pointer_cast( factor->noiseModel()); if (noiseModel) { Matrix newInfo = weights[i] * noiseModel->information(); diff --git a/gtsam/nonlinear/ISAM2-impl.h b/gtsam/nonlinear/ISAM2-impl.h index b196c6102..8c0af2c2d 100644 --- a/gtsam/nonlinear/ISAM2-impl.h +++ b/gtsam/nonlinear/ISAM2-impl.h @@ -49,7 +49,7 @@ class ISAM2BayesTree : public ISAM2::Base { public: typedef ISAM2::Base Base; typedef ISAM2BayesTree This; - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; ISAM2BayesTree() {} }; @@ -62,7 +62,7 @@ class ISAM2JunctionTree public: typedef JunctionTree Base; typedef ISAM2JunctionTree This; - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; explicit ISAM2JunctionTree(const GaussianEliminationTree& eliminationTree) : Base(eliminationTree) {} diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 233c922a9..dafe938c6 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -290,7 +290,7 @@ void ISAM2::recalculateIncremental(const ISAM2UpdateParams& updateParams, // Add the orphaned subtrees for (const auto& orphan : *orphans) factors += - boost::make_shared >(orphan); + std::make_shared >(orphan); gttoc(orphans); // 3. Re-order and eliminate the factor graph into a Bayes net (Algorithm @@ -525,7 +525,7 @@ void ISAM2::marginalizeLeaves( // Traverse up the tree to find the root of the marginalized subtree sharedClique clique = nodes_[j]; - while (!clique->parent_._empty()) { + while (clique->parent_.use_count() != 0) { // Check if parent contains a marginalized leaf variable. Only need to // check the first variable because it is the closest to the leaves. sharedClique parent = clique->parent(); @@ -667,7 +667,7 @@ void ISAM2::marginalizeLeaves( if (marginalFactorsIndices) marginalFactorsIndices->push_back(nonlinearFactors_.size()); nonlinearFactors_.push_back( - boost::make_shared(factor)); + std::make_shared(factor)); if (params_.cacheLinearizedFactors) linearFactors_.push_back(factor); for (Key factorKey : *factor) { fixedVariables_.insert(factorKey); diff --git a/gtsam/nonlinear/ISAM2Clique.h b/gtsam/nonlinear/ISAM2Clique.h index 9b40c02c6..0b1d567d2 100644 --- a/gtsam/nonlinear/ISAM2Clique.h +++ b/gtsam/nonlinear/ISAM2Clique.h @@ -38,8 +38,8 @@ class GTSAM_EXPORT ISAM2Clique public: typedef ISAM2Clique This; typedef BayesTreeCliqueBase Base; - typedef boost::shared_ptr shared_ptr; - typedef boost::weak_ptr weak_ptr; + typedef std::shared_ptr shared_ptr; + typedef std::weak_ptr weak_ptr; typedef GaussianConditional ConditionalType; typedef ConditionalType::shared_ptr sharedConditional; diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h index 3e4453f4f..b6b981d65 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h @@ -41,7 +41,7 @@ protected: void initTime(); public: - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; /// @name Constructors/Destructor /// @{ diff --git a/gtsam/nonlinear/LevenbergMarquardtParams.h b/gtsam/nonlinear/LevenbergMarquardtParams.h index f40443457..d7046654e 100644 --- a/gtsam/nonlinear/LevenbergMarquardtParams.h +++ b/gtsam/nonlinear/LevenbergMarquardtParams.h @@ -149,8 +149,8 @@ public: /// @{ /// @return a deep copy of this object - boost::shared_ptr clone() const { - return boost::shared_ptr(new LevenbergMarquardtParams(*this)); + std::shared_ptr clone() const { + return std::shared_ptr(new LevenbergMarquardtParams(*this)); } /// @} diff --git a/gtsam/nonlinear/LinearContainerFactor.cpp b/gtsam/nonlinear/LinearContainerFactor.cpp index 8b05bc2d9..57136214d 100644 --- a/gtsam/nonlinear/LinearContainerFactor.cpp +++ b/gtsam/nonlinear/LinearContainerFactor.cpp @@ -117,10 +117,10 @@ GaussianFactor::shared_ptr LinearContainerFactor::linearize(const Values& c) con // Apply changes due to relinearization if (isJacobian()) { - JacobianFactor::shared_ptr jacFactor = boost::dynamic_pointer_cast(linFactor); + JacobianFactor::shared_ptr jacFactor = std::dynamic_pointer_cast(linFactor); jacFactor->getb() = -jacFactor->unweighted_error(delta); } else { - HessianFactor::shared_ptr hesFactor = boost::dynamic_pointer_cast(linFactor); + HessianFactor::shared_ptr hesFactor = std::dynamic_pointer_cast(linFactor); const auto view = hesFactor->informationView(); Vector deltaVector = delta.vector(keys()); @@ -144,12 +144,12 @@ bool LinearContainerFactor::isHessian() const { /* ************************************************************************* */ JacobianFactor::shared_ptr LinearContainerFactor::toJacobian() const { - return boost::dynamic_pointer_cast(factor_); + return std::dynamic_pointer_cast(factor_); } /* ************************************************************************* */ HessianFactor::shared_ptr LinearContainerFactor::toHessian() const { - return boost::dynamic_pointer_cast(factor_); + return std::dynamic_pointer_cast(factor_); } /* ************************************************************************* */ @@ -170,7 +170,7 @@ NonlinearFactor::shared_ptr LinearContainerFactor::rekey( auto rekeyed_base_factor = Base::rekey(rekey_mapping); // Update the keys to the properties as well // Downncast so we have access to members - auto new_factor = boost::static_pointer_cast(rekeyed_base_factor); + auto new_factor = std::static_pointer_cast(rekeyed_base_factor); // Create a new Values to assign later Values newLinearizationPoint; for (size_t i = 0; i < factor_->size(); ++i) { @@ -183,7 +183,7 @@ NonlinearFactor::shared_ptr LinearContainerFactor::rekey( new_factor->linearizationPoint_ = newLinearizationPoint; // upcast back and return - return boost::static_pointer_cast(new_factor); + return std::static_pointer_cast(new_factor); } /* ************************************************************************* */ @@ -192,7 +192,7 @@ NonlinearFactor::shared_ptr LinearContainerFactor::rekey( auto rekeyed_base_factor = Base::rekey(new_keys); // Update the keys to the properties as well // Downncast so we have access to members - auto new_factor = boost::static_pointer_cast(rekeyed_base_factor); + auto new_factor = std::static_pointer_cast(rekeyed_base_factor); new_factor->factor_->keys() = new_factor->keys(); // Create a new Values to assign later Values newLinearizationPoint; @@ -203,7 +203,7 @@ NonlinearFactor::shared_ptr LinearContainerFactor::rekey( new_factor->linearizationPoint_ = newLinearizationPoint; // upcast back and return - return boost::static_pointer_cast(new_factor); + return std::static_pointer_cast(new_factor); } /* ************************************************************************* */ @@ -213,7 +213,7 @@ NonlinearFactorGraph LinearContainerFactor::ConvertLinearGraph( result.reserve(linear_graph.size()); for (const auto& f : linear_graph) if (f) - result += boost::make_shared(f, linearizationPoint); + result += std::make_shared(f, linearizationPoint); return result; } diff --git a/gtsam/nonlinear/LinearContainerFactor.h b/gtsam/nonlinear/LinearContainerFactor.h index 3a0555af6..0442f4780 100644 --- a/gtsam/nonlinear/LinearContainerFactor.h +++ b/gtsam/nonlinear/LinearContainerFactor.h @@ -41,7 +41,7 @@ protected: public: - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; /** Default constructor - necessary for serialization */ LinearContainerFactor() {} @@ -147,10 +147,10 @@ public: bool isHessian() const; /** Casts to JacobianFactor */ - boost::shared_ptr toJacobian() const; + std::shared_ptr toJacobian() const; /** Casts to HessianFactor */ - boost::shared_ptr toHessian() const; + std::shared_ptr toHessian() const; /** * Utility function for converting linear graphs to nonlinear graphs diff --git a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h index a7a0d724b..780d41635 100644 --- a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h +++ b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h @@ -51,7 +51,7 @@ public: typedef NonlinearOptimizer Base; typedef NonlinearOptimizerParams Parameters; - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; protected: Parameters params_; diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index bda16da08..022fb4523 100644 --- a/gtsam/nonlinear/NonlinearEquality.h +++ b/gtsam/nonlinear/NonlinearEquality.h @@ -172,7 +172,7 @@ public: /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } @@ -229,7 +229,7 @@ protected: public: - typedef boost::shared_ptr > shared_ptr; + typedef std::shared_ptr > shared_ptr; /** * Constructor @@ -248,7 +248,7 @@ public: /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } @@ -307,7 +307,7 @@ class NonlinearEquality2 : public NoiseModelFactorN { NonlinearEquality2() {} public: - typedef boost::shared_ptr> shared_ptr; + typedef std::shared_ptr> shared_ptr; // Provide access to the Matrix& version of evaluateError: using Base::evaluateError; @@ -326,7 +326,7 @@ class NonlinearEquality2 : public NoiseModelFactorN { /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } diff --git a/gtsam/nonlinear/NonlinearFactor.cpp b/gtsam/nonlinear/NonlinearFactor.cpp index b669be472..eac65d587 100644 --- a/gtsam/nonlinear/NonlinearFactor.cpp +++ b/gtsam/nonlinear/NonlinearFactor.cpp @@ -18,7 +18,6 @@ #include #include -#include #include namespace gtsam { @@ -90,7 +89,7 @@ bool NoiseModelFactor::equals(const NonlinearFactor& f, double tol) const { /* ************************************************************************* */ NoiseModelFactor::shared_ptr NoiseModelFactor::cloneWithNewNoiseModel( const SharedNoiseModel newNoise) const { - NoiseModelFactor::shared_ptr new_factor = boost::dynamic_pointer_cast(clone()); + NoiseModelFactor::shared_ptr new_factor = std::dynamic_pointer_cast(clone()); new_factor->noiseModel_ = newNoise; return new_factor; } @@ -149,12 +148,12 @@ double NoiseModelFactor::error(const Values& c) const { } /* ************************************************************************* */ -boost::shared_ptr NoiseModelFactor::linearize( +std::shared_ptr NoiseModelFactor::linearize( const Values& x) const { // Only linearize if the factor is active if (!active(x)) - return boost::shared_ptr(); + return std::shared_ptr(); // Call evaluate error to get Jacobians and RHS vector b std::vector A(size()); @@ -177,7 +176,7 @@ boost::shared_ptr NoiseModelFactor::linearize( if (noiseModel_ && noiseModel_->isConstrained()) return GaussianFactor::shared_ptr( new JacobianFactor(terms, b, - boost::static_pointer_cast(noiseModel_)->unit())); + std::static_pointer_cast(noiseModel_)->unit())); else { return GaussianFactor::shared_ptr(new JacobianFactor(terms, b)); } diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 3ca4614d5..73255482c 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -73,7 +73,7 @@ protected: public: - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; /// @name Standard Constructors /// @{ @@ -142,7 +142,7 @@ public: virtual bool active(const Values& /*c*/) const { return true; } /** linearize to a GaussianFactor */ - virtual boost::shared_ptr + virtual std::shared_ptr linearize(const Values& c) const = 0; /** @@ -207,7 +207,7 @@ protected: public: - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; /** Default constructor for I/O only */ NoiseModelFactor() {} @@ -294,7 +294,7 @@ public: * \f$ Ax-b \approx h(x+\delta x)-z = h(x) + A \delta x - z \f$ * Hence \f$ b = z - h(x) = - \mathtt{error\_vector}(x) \f$ */ - boost::shared_ptr linearize(const Values& x) const override; + std::shared_ptr linearize(const Values& x) const override; /** * Creates a shared_ptr clone of the diff --git a/gtsam/nonlinear/NonlinearFactorGraph.cpp b/gtsam/nonlinear/NonlinearFactorGraph.cpp index 35868558f..f8aaf4f39 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.cpp +++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp @@ -194,7 +194,7 @@ Ordering NonlinearFactorGraph::orderingCOLAMDConstrained(const FastMap SymbolicFactorGraph::shared_ptr NonlinearFactorGraph::symbolic() const { // Generate the symbolic factor graph - SymbolicFactorGraph::shared_ptr symbolic = boost::make_shared(); + SymbolicFactorGraph::shared_ptr symbolic = std::make_shared(); symbolic->reserve(size()); for (const sharedFactor& factor: factors_) { @@ -241,7 +241,7 @@ GaussianFactorGraph::shared_ptr NonlinearFactorGraph::linearize(const Values& li gttic(NonlinearFactorGraph_linearize); // create an empty linear FG - GaussianFactorGraph::shared_ptr linearFG = boost::make_shared(); + GaussianFactorGraph::shared_ptr linearFG = std::make_shared(); #ifdef GTSAM_USE_TBB diff --git a/gtsam/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h index cf0f5aa7f..95b116ab6 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.h +++ b/gtsam/nonlinear/NonlinearFactorGraph.h @@ -27,7 +27,7 @@ #include #include -#include +#include #include namespace gtsam { @@ -58,7 +58,7 @@ namespace gtsam { typedef FactorGraph Base; typedef NonlinearFactorGraph This; - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; /// @name Standard Constructors /// @{ @@ -112,7 +112,7 @@ namespace gtsam { /** * Create a symbolic factor graph */ - boost::shared_ptr symbolic() const; + std::shared_ptr symbolic() const; /** * Compute a fill-reducing ordering using COLAMD. @@ -130,10 +130,10 @@ namespace gtsam { Ordering orderingCOLAMDConstrained(const FastMap& constraints) const; /// Linearize a nonlinear factor graph - boost::shared_ptr linearize(const Values& linearizationPoint) const; + std::shared_ptr linearize(const Values& linearizationPoint) const; /// typdef for dampen functions used below - typedef std::function& hessianFactor)> Dampen; + typedef std::function& hessianFactor)> Dampen; /** * Instead of producing a GaussianFactorGraph, pre-allocate and linearize directly @@ -142,7 +142,7 @@ namespace gtsam { * An optional lambda function can be used to apply damping on the filled Hessian. * No parallelism is exploited, because all the factors write in the same memory. */ - boost::shared_ptr linearizeToHessianFactor( + std::shared_ptr linearizeToHessianFactor( const Values& values, const Dampen& dampen = nullptr) const; /** @@ -153,7 +153,7 @@ namespace gtsam { * An optional lambda function can be used to apply damping on the filled Hessian. * No parallelism is exploited, because all the factors write in the same memory. */ - boost::shared_ptr linearizeToHessianFactor( + std::shared_ptr linearizeToHessianFactor( const Values& values, const Ordering& ordering, const Dampen& dampen = nullptr) const; /// Linearize and solve in one pass. @@ -189,7 +189,7 @@ namespace gtsam { template void addExpressionFactor(const SharedNoiseModel& R, const T& z, const Expression& h) { - push_back(boost::make_shared >(R, z, h)); + push_back(std::make_shared >(R, z, h)); } /** @@ -250,7 +250,7 @@ namespace gtsam { * Linearize from Scatter rather than from Ordering. Made private because * it doesn't include gttic. */ - boost::shared_ptr linearizeToHessianFactor( + std::shared_ptr linearizeToHessianFactor( const Values& values, const Scatter& scatter, const Dampen& dampen = nullptr) const; /** Serialization function */ diff --git a/gtsam/nonlinear/NonlinearISAM.cpp b/gtsam/nonlinear/NonlinearISAM.cpp index 840712eb6..fb157fadd 100644 --- a/gtsam/nonlinear/NonlinearISAM.cpp +++ b/gtsam/nonlinear/NonlinearISAM.cpp @@ -48,7 +48,7 @@ void NonlinearISAM::update(const NonlinearFactorGraph& newFactors, const Values& // TODO: optimize for whole config? linPoint_.insert(initialValues); - boost::shared_ptr linearizedNewFactors = newFactors.linearize(linPoint_); + std::shared_ptr linearizedNewFactors = newFactors.linearize(linPoint_); // Update ISAM isam_.update(*linearizedNewFactors, eliminationFunction_); diff --git a/gtsam/nonlinear/NonlinearOptimizer.cpp b/gtsam/nonlinear/NonlinearOptimizer.cpp index 3ce6db4af..8aa611976 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearOptimizer.cpp @@ -28,7 +28,6 @@ #include #include -#include #include #include @@ -160,11 +159,11 @@ VectorValues NonlinearOptimizer::solve(const GaussianFactorGraph& gfg, throw std::runtime_error( "NonlinearOptimizer::solve: cg parameter has to be assigned ..."); - if (auto pcg = boost::dynamic_pointer_cast( + if (auto pcg = std::dynamic_pointer_cast( params.iterativeParams)) { delta = PCGSolver(*pcg).optimize(gfg); } else if (auto spcg = - boost::dynamic_pointer_cast( + std::dynamic_pointer_cast( params.iterativeParams)) { if (!params.ordering) throw std::runtime_error("SubgraphSolver needs an ordering"); diff --git a/gtsam/nonlinear/NonlinearOptimizer.h b/gtsam/nonlinear/NonlinearOptimizer.h index 6fe369dd3..f79c3614f 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.h +++ b/gtsam/nonlinear/NonlinearOptimizer.h @@ -81,7 +81,7 @@ protected: public: /** A shared pointer to this class */ - using shared_ptr = boost::shared_ptr; + using shared_ptr = std::shared_ptr; /// @name Standard interface /// @{ diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.cpp b/gtsam/nonlinear/NonlinearOptimizerParams.cpp index 91edd8f93..6fc532bf6 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.cpp +++ b/gtsam/nonlinear/NonlinearOptimizerParams.cpp @@ -67,7 +67,7 @@ std::string NonlinearOptimizerParams::verbosityTranslator( /* ************************************************************************* */ void NonlinearOptimizerParams::setIterativeParams( - const boost::shared_ptr params) { + const std::shared_ptr params) { iterativeParams = params; } diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.h b/gtsam/nonlinear/NonlinearOptimizerParams.h index 7184b3dfc..cafb235bc 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.h +++ b/gtsam/nonlinear/NonlinearOptimizerParams.h @@ -167,7 +167,7 @@ public: linearSolverType = linearSolverTranslator(solver); } - void setIterativeParams(const boost::shared_ptr params); + void setIterativeParams(const std::shared_ptr params); void setOrdering(const Ordering& ordering) { this->ordering = ordering; diff --git a/gtsam/nonlinear/PriorFactor.h b/gtsam/nonlinear/PriorFactor.h index 4202bb33c..808f66c5b 100644 --- a/gtsam/nonlinear/PriorFactor.h +++ b/gtsam/nonlinear/PriorFactor.h @@ -48,7 +48,7 @@ namespace gtsam { public: /// shorthand for a smart pointer to a factor - typedef typename boost::shared_ptr > shared_ptr; + typedef typename std::shared_ptr > shared_ptr; /// Typedef to this class typedef PriorFactor This; @@ -70,7 +70,7 @@ namespace gtsam { /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } /** implement functions needed for Testable */ diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index d7043c7ff..66e90a0b3 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -29,7 +29,7 @@ #include #include #include -#include +#include #include #include @@ -80,10 +80,10 @@ namespace gtsam { public: /// A shared_ptr to this class - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; /// A const shared_ptr to this class - typedef boost::shared_ptr const_shared_ptr; + typedef std::shared_ptr const_shared_ptr; /// A key-value pair, which you get by dereferencing iterators struct GTSAM_EXPORT KeyValuePair { diff --git a/gtsam/nonlinear/WhiteNoiseFactor.h b/gtsam/nonlinear/WhiteNoiseFactor.h index 1cd117437..a8142c28c 100644 --- a/gtsam/nonlinear/WhiteNoiseFactor.h +++ b/gtsam/nonlinear/WhiteNoiseFactor.h @@ -155,7 +155,7 @@ namespace gtsam { /// @{ /// linearize returns a Hessianfactor that is an approximation of error(p) - boost::shared_ptr linearize(const Values& x) const override { + std::shared_ptr linearize(const Values& x) const override { double u = x.at(meanKey_); double p = x.at(precisionKey_); Key j1 = meanKey_; @@ -166,7 +166,7 @@ namespace gtsam { // TODO: Frank commented this out for now, can it go? // /// @return a deep copy of this factor // gtsam::NonlinearFactor::shared_ptr clone() const override { - // return boost::static_pointer_cast( + // return std::static_pointer_cast( // gtsam::NonlinearFactor::shared_ptr(new This(*this))); } /// @} diff --git a/gtsam/nonlinear/factorTesting.h b/gtsam/nonlinear/factorTesting.h index 266aa841c..ff7b2ca35 100644 --- a/gtsam/nonlinear/factorTesting.h +++ b/gtsam/nonlinear/factorTesting.h @@ -83,7 +83,7 @@ inline bool testFactorJacobians(const std::string& name_, // Create actual value by linearize auto actual = - boost::dynamic_pointer_cast(factor.linearize(values)); + std::dynamic_pointer_cast(factor.linearize(values)); if (!actual) return false; // Check cast result and then equality diff --git a/gtsam/nonlinear/internal/ExpressionNode.h b/gtsam/nonlinear/internal/ExpressionNode.h index 4c34a1777..cf30b1f65 100644 --- a/gtsam/nonlinear/internal/ExpressionNode.h +++ b/gtsam/nonlinear/internal/ExpressionNode.h @@ -230,7 +230,7 @@ template class UnaryExpression: public ExpressionNode { typedef typename Expression::template UnaryFunction::type Function; - boost::shared_ptr > expression1_; + std::shared_ptr > expression1_; Function function_; /// Constructor with a unary function f, and input argument e1 @@ -335,8 +335,8 @@ template class BinaryExpression: public ExpressionNode { typedef typename Expression::template BinaryFunction::type Function; - boost::shared_ptr > expression1_; - boost::shared_ptr > expression2_; + std::shared_ptr > expression1_; + std::shared_ptr > expression2_; Function function_; /// Constructor with a binary function f, and two input arguments @@ -441,9 +441,9 @@ template class TernaryExpression: public ExpressionNode { typedef typename Expression::template TernaryFunction::type Function; - boost::shared_ptr > expression1_; - boost::shared_ptr > expression2_; - boost::shared_ptr > expression3_; + std::shared_ptr > expression1_; + std::shared_ptr > expression2_; + std::shared_ptr > expression3_; Function function_; /// Constructor with a ternary function f, and two input arguments @@ -562,7 +562,7 @@ class ScalarMultiplyNode : public ExpressionNode { BOOST_CONCEPT_ASSERT((gtsam::IsVectorSpace)); double scalar_; - boost::shared_ptr > expression_; + std::shared_ptr > expression_; public: /// Constructor with a unary function f, and input argument e1 @@ -643,8 +643,8 @@ class ScalarMultiplyNode : public ExpressionNode { template class BinarySumNode : public ExpressionNode { typedef ExpressionNode NodeT; - boost::shared_ptr > expression1_; - boost::shared_ptr > expression2_; + std::shared_ptr > expression1_; + std::shared_ptr > expression2_; public: explicit BinarySumNode() { diff --git a/gtsam/nonlinear/internal/LevenbergMarquardtState.h b/gtsam/nonlinear/internal/LevenbergMarquardtState.h index a055f3060..aa0a3daa3 100644 --- a/gtsam/nonlinear/internal/LevenbergMarquardtState.h +++ b/gtsam/nonlinear/internal/LevenbergMarquardtState.h @@ -27,8 +27,6 @@ #include #include -#include - #include #include #include @@ -133,7 +131,7 @@ struct LevenbergMarquardtState : public NonlinearOptimizerState { const Key& key = key_dim.first; const size_t& dim = key_dim.second; const CachedModel* item = getCachedModel(dim); - damped += boost::make_shared(key, item->A, item->b, item->model); + damped += std::make_shared(key, item->A, item->b, item->model); } return damped; } @@ -149,7 +147,7 @@ struct LevenbergMarquardtState : public NonlinearOptimizerState { const size_t dim = key_vector.second.size(); CachedModel* item = getCachedModel(dim); item->A.diagonal() = sqrtHessianDiagonal.at(key); // use diag(hessian) - damped += boost::make_shared(key, item->A, item->b, item->model); + damped += std::make_shared(key, item->A, item->b, item->model); } catch (const std::out_of_range&) { continue; // Don't attempt any damping if no key found in diagonal } diff --git a/gtsam/nonlinear/tests/testLinearContainerFactor.cpp b/gtsam/nonlinear/tests/testLinearContainerFactor.cpp index 9bd9ace98..77d40758b 100644 --- a/gtsam/nonlinear/tests/testLinearContainerFactor.cpp +++ b/gtsam/nonlinear/tests/testLinearContainerFactor.cpp @@ -322,7 +322,7 @@ TEST(TestLinearContainerFactor, hessian_relinearize) TEST(TestLinearContainerFactor, Rekey) { // Make an example factor auto nonlinear_factor = - boost::make_shared>( + std::make_shared>( gtsam::Symbol('x', 0), gtsam::Symbol('l', 0), gtsam::Point3(0, 0, 0), gtsam::noiseModel::Isotropic::Sigma(3, 1)); @@ -345,7 +345,7 @@ TEST(TestLinearContainerFactor, Rekey) { // Cast back to LCF ptr LinearContainerFactor::shared_ptr lcf_factor_rekey_ptr = - boost::static_pointer_cast(lcf_factor_rekeyed); + std::static_pointer_cast(lcf_factor_rekeyed); CHECK(lcf_factor_rekey_ptr); // For extra fun lets try linearizing this LCF @@ -364,7 +364,7 @@ TEST(TestLinearContainerFactor, Rekey) { TEST(TestLinearContainerFactor, Rekey2) { // Make an example factor auto nonlinear_factor = - boost::make_shared>( + std::make_shared>( gtsam::Symbol('x', 0), gtsam::Symbol('l', 0), gtsam::Point3(0, 0, 0), gtsam::noiseModel::Isotropic::Sigma(3, 1)); @@ -383,7 +383,7 @@ TEST(TestLinearContainerFactor, Rekey2) { // Cast back to LCF ptr LinearContainerFactor::shared_ptr lcf_factor_rekey_ptr = - boost::static_pointer_cast( + std::static_pointer_cast( lcf_factor.rekey(key_map)); CHECK(lcf_factor_rekey_ptr); } diff --git a/gtsam/nonlinear/utilities.h b/gtsam/nonlinear/utilities.h index 985e1a5f4..2997ed7d7 100644 --- a/gtsam/nonlinear/utilities.h +++ b/gtsam/nonlinear/utilities.h @@ -285,7 +285,7 @@ void insertProjectionFactors(NonlinearFactorGraph& graph, Key i, "addMeasurements: J and Z must have same number of entries"); for (int k = 0; k < Z.cols(); k++) { graph.push_back( - boost::make_shared >( + std::make_shared >( Point2(Z(0, k), Z(1, k)), model, i, Key(J(k)), K, body_P_sensor)); } } @@ -296,14 +296,14 @@ Matrix reprojectionErrors(const NonlinearFactorGraph& graph, // first count size_t K = 0, k = 0; for(const NonlinearFactor::shared_ptr& f: graph) - if (boost::dynamic_pointer_cast >( + if (std::dynamic_pointer_cast >( f)) ++K; // now fill Matrix errors(2, K); for(const NonlinearFactor::shared_ptr& f: graph) { - boost::shared_ptr > p = - boost::dynamic_pointer_cast >( + std::shared_ptr > p = + std::dynamic_pointer_cast >( f); if (p) errors.col(k++) = p->unwhitenedError(values); diff --git a/gtsam/sam/BearingRangeFactor.h b/gtsam/sam/BearingRangeFactor.h index 4f3a38eac..f4fcbf692 100644 --- a/gtsam/sam/BearingRangeFactor.h +++ b/gtsam/sam/BearingRangeFactor.h @@ -40,7 +40,7 @@ class BearingRangeFactor typedef BearingRangeFactor This; public: - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; /// Default constructor BearingRangeFactor() {} @@ -63,7 +63,7 @@ class BearingRangeFactor /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } diff --git a/gtsam/sam/RangeFactor.h b/gtsam/sam/RangeFactor.h index 477c4e0e6..fb7977d45 100644 --- a/gtsam/sam/RangeFactor.h +++ b/gtsam/sam/RangeFactor.h @@ -48,7 +48,7 @@ class RangeFactor : public ExpressionFactorN { /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } @@ -121,7 +121,7 @@ class RangeFactorWithTransform : public ExpressionFactorN { /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } diff --git a/gtsam/sfm/BinaryMeasurement.h b/gtsam/sfm/BinaryMeasurement.h index bdb45d622..1f991a05a 100644 --- a/gtsam/sfm/BinaryMeasurement.h +++ b/gtsam/sfm/BinaryMeasurement.h @@ -39,7 +39,7 @@ template class BinaryMeasurement : public Factor { public: // shorthand for a smart pointer to a measurement - using shared_ptr = typename boost::shared_ptr; + using shared_ptr = typename std::shared_ptr; private: T measured_; ///< The measurement diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp index 9e5645a57..f8cd9fc9c 100644 --- a/gtsam/sfm/ShonanAveraging.cpp +++ b/gtsam/sfm/ShonanAveraging.cpp @@ -67,19 +67,19 @@ ShonanAveragingParameters::ShonanAveragingParameters( builderParameters.augmentationWeight = SubgraphBuilderParameters::SKELETON; builderParameters.augmentationFactor = 0.0; - auto pcg = boost::make_shared(); + auto pcg = std::make_shared(); // Choose optimization method if (method == "SUBGRAPH") { lm.iterativeParams = - boost::make_shared(builderParameters); + std::make_shared(builderParameters); } else if (method == "SGPC") { pcg->preconditioner_ = - boost::make_shared(builderParameters); + std::make_shared(builderParameters); lm.iterativeParams = pcg; } else if (method == "JACOBI") { pcg->preconditioner_ = - boost::make_shared(); + std::make_shared(); lm.iterativeParams = pcg; } else if (method == "QR") { lm.setLinearSolverType("MULTIFRONTAL_QR"); @@ -142,7 +142,7 @@ ShonanAveraging::ShonanAveraging(const Measurements &measurements, template NonlinearFactorGraph ShonanAveraging::buildGraphAt(size_t p) const { NonlinearFactorGraph graph; - auto G = boost::make_shared(SO<-1>::VectorizedGenerators(p)); + auto G = std::make_shared(SO<-1>::VectorizedGenerators(p)); for (const auto &measurement : measurements_) { const auto &keys = measurement.keys(); @@ -172,7 +172,7 @@ double ShonanAveraging::costAt(size_t p, const Values &values) const { /* ************************************************************************* */ template -boost::shared_ptr +std::shared_ptr ShonanAveraging::createOptimizerAt(size_t p, const Values &initial) const { // Build graph NonlinearFactorGraph graph = buildGraphAt(p); @@ -188,7 +188,7 @@ ShonanAveraging::createOptimizerAt(size_t p, const Values &initial) const { model); } // Optimize - return boost::make_shared(graph, initial, + return std::make_shared(graph, initial, parameters_.lm); } @@ -337,13 +337,13 @@ double ShonanAveraging::cost(const Values &values) const { template static double Kappa(const BinaryMeasurement &measurement, const ShonanAveragingParameters ¶meters) { - const auto &isotropic = boost::dynamic_pointer_cast( + const auto &isotropic = std::dynamic_pointer_cast( measurement.noiseModel()); double sigma; if (isotropic) { sigma = isotropic->sigma(); } else { - const auto &robust = boost::dynamic_pointer_cast( + const auto &robust = std::dynamic_pointer_cast( measurement.noiseModel()); // Check if noise model is robust if (robust) { @@ -949,7 +949,7 @@ ShonanAveraging2::ShonanAveraging2(string g2oFile, const Parameters ¶meters) static BinaryMeasurement convertPose2ToBinaryMeasurementRot2( const BetweenFactor::shared_ptr &f) { auto gaussian = - boost::dynamic_pointer_cast(f->noiseModel()); + std::dynamic_pointer_cast(f->noiseModel()); if (!gaussian) throw std::invalid_argument( "parseMeasurements can only convert Pose2 measurements " @@ -997,7 +997,7 @@ ShonanAveraging3::ShonanAveraging3(string g2oFile, const Parameters ¶meters) static BinaryMeasurement convert( const BetweenFactor::shared_ptr &f) { auto gaussian = - boost::dynamic_pointer_cast(f->noiseModel()); + std::dynamic_pointer_cast(f->noiseModel()); if (!gaussian) throw std::invalid_argument( "parseMeasurements can only convert Pose3 measurements " diff --git a/gtsam/sfm/ShonanAveraging.h b/gtsam/sfm/ShonanAveraging.h index b40916828..989733cb5 100644 --- a/gtsam/sfm/ShonanAveraging.h +++ b/gtsam/sfm/ShonanAveraging.h @@ -184,7 +184,7 @@ class GTSAM_EXPORT ShonanAveraging { for (auto &measurement : measurements) { auto model = measurement.noiseModel(); const auto &robust = - boost::dynamic_pointer_cast(model); + std::dynamic_pointer_cast(model); SharedNoiseModel robust_model; // Check if the noise model is already robust @@ -339,7 +339,7 @@ class GTSAM_EXPORT ShonanAveraging { * @param initial initial SO(p) values * @return lm optimizer */ - boost::shared_ptr createOptimizerAt( + std::shared_ptr createOptimizerAt( size_t p, const Values &initial) const; /** diff --git a/gtsam/sfm/ShonanFactor.cpp b/gtsam/sfm/ShonanFactor.cpp index 288bd573e..7d7022b52 100644 --- a/gtsam/sfm/ShonanFactor.cpp +++ b/gtsam/sfm/ShonanFactor.cpp @@ -34,7 +34,7 @@ namespace gtsam { template ShonanFactor::ShonanFactor(Key j1, Key j2, const Rot &R12, size_t p, const SharedNoiseModel &model, - const boost::shared_ptr &G) + const std::shared_ptr &G) : NoiseModelFactorN(ConvertNoiseModel(model, p * d), j1, j2), M_(R12.matrix()), // d*d in all cases p_(p), // 4 for SO(4) @@ -44,7 +44,7 @@ ShonanFactor::ShonanFactor(Key j1, Key j2, const Rot &R12, size_t p, throw std::invalid_argument( "ShonanFactor: model with incorrect dimension."); if (!G) { - G_ = boost::make_shared(); + G_ = std::make_shared(); *G_ = SOn::VectorizedGenerators(p); // expensive! } if (static_cast(G_->rows()) != pp_ || diff --git a/gtsam/sfm/ShonanFactor.h b/gtsam/sfm/ShonanFactor.h index 3e6ea218d..3a6fa11e1 100644 --- a/gtsam/sfm/ShonanFactor.h +++ b/gtsam/sfm/ShonanFactor.h @@ -36,7 +36,7 @@ template class GTSAM_EXPORT ShonanFactor : public NoiseModelFactorN { Matrix M_; ///< measured rotation between R1 and R2 size_t p_, pp_; ///< dimensionality constants - boost::shared_ptr G_; ///< matrix of vectorized generators + std::shared_ptr G_; ///< matrix of vectorized generators // Select Rot2 or Rot3 interface based template parameter d using Rot = typename std::conditional::type; @@ -51,10 +51,10 @@ public: /// Constructor. Note we convert to d*p-dimensional noise model. /// To save memory and mallocs, pass in the vectorized Lie algebra generators: - /// G = boost::make_shared(SOn::VectorizedGenerators(p)); + /// G = std::make_shared(SOn::VectorizedGenerators(p)); ShonanFactor(Key j1, Key j2, const Rot &R12, size_t p, const SharedNoiseModel &model = nullptr, - const boost::shared_ptr &G = nullptr); + const std::shared_ptr &G = nullptr); /// @} /// @name Testable diff --git a/gtsam/sfm/ShonanGaugeFactor.h b/gtsam/sfm/ShonanGaugeFactor.h index 3152ad9d0..4ab75a3a8 100644 --- a/gtsam/sfm/ShonanGaugeFactor.h +++ b/gtsam/sfm/ShonanGaugeFactor.h @@ -47,7 +47,7 @@ class GTSAM_EXPORT ShonanGaugeFactor : public NonlinearFactor { size_t rows_; /// Constant Jacobian - boost::shared_ptr whitenedJacobian_; + std::shared_ptr whitenedJacobian_; public: /** @@ -84,7 +84,7 @@ public: } // TODO(frank): assign the right one in the right columns whitenedJacobian_ = - boost::make_shared(key, A, Vector::Zero(rows_)); + std::make_shared(key, A, Vector::Zero(rows_)); } /// Destructor @@ -97,7 +97,7 @@ public: size_t dim() const override { return rows_; } /// linearize to a GaussianFactor - boost::shared_ptr linearize(const Values &c) const override { + std::shared_ptr linearize(const Values &c) const override { return whitenedJacobian_; } }; diff --git a/gtsam/sfm/tests/testBinaryMeasurement.cpp b/gtsam/sfm/tests/testBinaryMeasurement.cpp index ae13e54c4..6737f796d 100644 --- a/gtsam/sfm/tests/testBinaryMeasurement.cpp +++ b/gtsam/sfm/tests/testBinaryMeasurement.cpp @@ -74,7 +74,7 @@ TEST(BinaryMeasurement, Rot3MakeRobust) { EXPECT_LONGS_EQUAL(rot3Measurement.key1(), kKey1); EXPECT_LONGS_EQUAL(rot3Measurement.key2(), kKey2); EXPECT(rot3Measurement.measured().equals(rot3Measured)); - const auto &robust = boost::dynamic_pointer_cast( + const auto &robust = std::dynamic_pointer_cast( rot3Measurement.noiseModel()); EXPECT(robust); } diff --git a/gtsam/sfm/tests/testShonanAveraging.cpp b/gtsam/sfm/tests/testShonanAveraging.cpp index 556289d22..d398a2a87 100644 --- a/gtsam/sfm/tests/testShonanAveraging.cpp +++ b/gtsam/sfm/tests/testShonanAveraging.cpp @@ -372,8 +372,8 @@ TEST(ShonanAveraging2, noisyToyGraphWithHuber) { // test that each factor is actually robust for (size_t i=0; i<=4; i++) { // note: last is the Gauge factor and is not robust - const auto &robust = boost::dynamic_pointer_cast( - boost::dynamic_pointer_cast(graph[i])->noiseModel()); + const auto &robust = std::dynamic_pointer_cast( + std::dynamic_pointer_cast(graph[i])->noiseModel()); EXPECT(robust); // we expect the factors to be use a robust noise model (in particular, Huber) } @@ -404,7 +404,7 @@ TEST(ShonanAveraging3, PriorWeights) { for (auto i : {0,1,2}) { const auto& m = shonan.measurement(i); auto isotropic = - boost::static_pointer_cast(m.noiseModel()); + std::static_pointer_cast(m.noiseModel()); CHECK(isotropic != nullptr); EXPECT_LONGS_EQUAL(3, isotropic->dim()); EXPECT_DOUBLES_EQUAL(0.2, isotropic->sigma(), 1e-9); diff --git a/gtsam/sfm/tests/testShonanGaugeFactor.cpp b/gtsam/sfm/tests/testShonanGaugeFactor.cpp index 344394b9c..8d6d68525 100644 --- a/gtsam/sfm/tests/testShonanGaugeFactor.cpp +++ b/gtsam/sfm/tests/testShonanGaugeFactor.cpp @@ -53,7 +53,7 @@ TEST(ShonanAveraging, GaugeFactorSO6) { JacobianFactor linearized(key, A, Vector::Zero(3)); Values values; EXPECT_LONGS_EQUAL(3, factor.dim()); - EXPECT(assert_equal(linearized, *boost::dynamic_pointer_cast( + EXPECT(assert_equal(linearized, *std::dynamic_pointer_cast( factor.linearize(values)))); } @@ -73,7 +73,7 @@ TEST(ShonanAveraging, GaugeFactorSO7) { JacobianFactor linearized(key, A, Vector::Zero(6)); Values values; EXPECT_LONGS_EQUAL(6, factor.dim()); - EXPECT(assert_equal(linearized, *boost::dynamic_pointer_cast( + EXPECT(assert_equal(linearized, *std::dynamic_pointer_cast( factor.linearize(values)))); } @@ -94,7 +94,7 @@ TEST(ShonanAveraging, GaugeFactorSO6over2) { JacobianFactor linearized(key, A, Vector::Zero(6)); Values values; EXPECT_LONGS_EQUAL(6, factor.dim()); - EXPECT(assert_equal(linearized, *boost::dynamic_pointer_cast( + EXPECT(assert_equal(linearized, *std::dynamic_pointer_cast( factor.linearize(values)))); } diff --git a/gtsam/slam/AntiFactor.h b/gtsam/slam/AntiFactor.h index b291afac9..f11575035 100644 --- a/gtsam/slam/AntiFactor.h +++ b/gtsam/slam/AntiFactor.h @@ -41,7 +41,7 @@ namespace gtsam { public: // shorthand for a smart pointer to a factor - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; /** default constructor - only use for serialization */ AntiFactor() {} @@ -53,7 +53,7 @@ namespace gtsam { /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } /** implement functions needed for Testable */ @@ -94,7 +94,7 @@ namespace gtsam { * returns a Jacobian instead of a full Hessian), but with the opposite sign. This * effectively cancels the effect of the original factor on the factor graph. */ - boost::shared_ptr linearize(const Values& c) const override { + std::shared_ptr linearize(const Values& c) const override { // Generate the linearized factor from the contained nonlinear factor GaussianFactor::shared_ptr gaussianFactor = factor_->linearize(c); diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h index 7d30c0c54..36cea67a9 100644 --- a/gtsam/slam/BetweenFactor.h +++ b/gtsam/slam/BetweenFactor.h @@ -60,7 +60,7 @@ namespace gtsam { using Base::evaluateError; // shorthand for a smart pointer to a factor - typedef typename boost::shared_ptr shared_ptr; + typedef typename std::shared_ptr shared_ptr; /// @name Standard Constructors /// @{ @@ -80,7 +80,7 @@ namespace gtsam { /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } /// @name Testable @@ -163,7 +163,7 @@ namespace gtsam { template class BetweenConstraint : public BetweenFactor { public: - typedef boost::shared_ptr > shared_ptr; + typedef std::shared_ptr > shared_ptr; /** Syntactic sugar for constrained version */ BetweenConstraint(const VALUE& measured, Key key1, Key key2, double mu = 1000.0) : diff --git a/gtsam/slam/BoundingConstraint.h b/gtsam/slam/BoundingConstraint.h index d4770b4cc..14aa96dbc 100644 --- a/gtsam/slam/BoundingConstraint.h +++ b/gtsam/slam/BoundingConstraint.h @@ -33,7 +33,7 @@ template struct BoundingConstraint1: public NoiseModelFactorN { typedef VALUE X; typedef NoiseModelFactorN Base; - typedef boost::shared_ptr > shared_ptr; + typedef std::shared_ptr > shared_ptr; // Provide access to the Matrix& version of evaluateError: using Base::evaluateError; @@ -105,7 +105,7 @@ struct BoundingConstraint2: public NoiseModelFactorN { typedef VALUE2 X2; typedef NoiseModelFactorN Base; - typedef boost::shared_ptr > shared_ptr; + typedef std::shared_ptr > shared_ptr; // Provide access to the Matrix& version of evaluateError: using Base::evaluateError; diff --git a/gtsam/slam/EssentialMatrixConstraint.h b/gtsam/slam/EssentialMatrixConstraint.h index 631748964..c533ea04d 100644 --- a/gtsam/slam/EssentialMatrixConstraint.h +++ b/gtsam/slam/EssentialMatrixConstraint.h @@ -42,7 +42,7 @@ public: using Base::evaluateError; // shorthand for a smart pointer to a factor - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; /** default constructor - only use for serialization */ EssentialMatrixConstraint() { @@ -65,7 +65,7 @@ public: /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h index 7bb68ce32..b19bb09ab 100644 --- a/gtsam/slam/EssentialMatrixFactor.h +++ b/gtsam/slam/EssentialMatrixFactor.h @@ -69,7 +69,7 @@ class EssentialMatrixFactor : public NoiseModelFactorN { template EssentialMatrixFactor(Key key, const Point2& pA, const Point2& pB, const SharedNoiseModel& model, - boost::shared_ptr K) + std::shared_ptr K) : Base(model, key) { assert(K); vA_ = EssentialMatrix::Homogeneous(K->calibrate(pA)); @@ -78,7 +78,7 @@ class EssentialMatrixFactor : public NoiseModelFactorN { /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } @@ -150,7 +150,7 @@ class EssentialMatrixFactor2 template EssentialMatrixFactor2(Key key1, Key key2, const Point2& pA, const Point2& pB, const SharedNoiseModel& model, - boost::shared_ptr K) + std::shared_ptr K) : Base(model, key1, key2), dP1_(EssentialMatrix::Homogeneous(K->calibrate(pA))), pn_(K->calibrate(pB)) { @@ -159,7 +159,7 @@ class EssentialMatrixFactor2 /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } @@ -270,12 +270,12 @@ class EssentialMatrixFactor3 : public EssentialMatrixFactor2 { template EssentialMatrixFactor3(Key key1, Key key2, const Point2& pA, const Point2& pB, const Rot3& cRb, const SharedNoiseModel& model, - boost::shared_ptr K) + std::shared_ptr K) : EssentialMatrixFactor2(key1, key2, pA, pB, model, K), cRb_(cRb) {} /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } @@ -362,7 +362,7 @@ class EssentialMatrixFactor4 /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } diff --git a/gtsam/slam/FrobeniusFactor.cpp b/gtsam/slam/FrobeniusFactor.cpp index 8c0baaf38..982430d81 100644 --- a/gtsam/slam/FrobeniusFactor.cpp +++ b/gtsam/slam/FrobeniusFactor.cpp @@ -28,7 +28,7 @@ ConvertNoiseModel(const SharedNoiseModel &model, size_t d, bool defaultToUnit) { double sigma = 1.0; if (model != nullptr) { - const auto &robust = boost::dynamic_pointer_cast(model); + const auto &robust = std::dynamic_pointer_cast(model); Vector sigmas; if (robust) { sigmas = robust->noise()->sigmas(); @@ -56,7 +56,7 @@ ConvertNoiseModel(const SharedNoiseModel &model, size_t d, bool defaultToUnit) { } exit: auto isoModel = noiseModel::Isotropic::Sigma(d, sigma); - const auto &robust = boost::dynamic_pointer_cast(model); + const auto &robust = std::dynamic_pointer_cast(model); if (robust) { return noiseModel::Robust::Create( noiseModel::mEstimator::Huber::Create(1.345), isoModel); diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index 78fdb189a..8046a210b 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -38,7 +38,6 @@ #include #include -#include #include #include @@ -79,7 +78,7 @@ public: using Base::evaluateError; // shorthand for a smart pointer to a factor - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; /** * Constructor @@ -102,7 +101,7 @@ public: /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this)));} /** @@ -138,9 +137,9 @@ public: } /// Linearize using fixed-size matrices - boost::shared_ptr linearize(const Values& values) const override { + std::shared_ptr linearize(const Values& values) const override { // Only linearize if the factor is active - if (!this->active(values)) return boost::shared_ptr(); + if (!this->active(values)) return std::shared_ptr(); const Key key1 = this->key1(), key2 = this->key2(); JacobianC H1; @@ -170,10 +169,10 @@ public: // Create new (unit) noiseModel, preserving constraints if applicable SharedDiagonal model; if (noiseModel && noiseModel->isConstrained()) { - model = boost::static_pointer_cast(noiseModel)->unit(); + model = std::static_pointer_cast(noiseModel)->unit(); } - return boost::make_shared >(key1, H1, key2, H2, b, model); + return std::make_shared >(key1, H1, key2, H2, b, model); } /** return the measured */ @@ -219,7 +218,7 @@ public: typedef NoiseModelFactorN Base;///< typedef for the base class // shorthand for a smart pointer to a factor - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; /** * Constructor @@ -237,7 +236,7 @@ public: /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this)));} /** diff --git a/gtsam/slam/InitializePose.h b/gtsam/slam/InitializePose.h index 79e3fe813..bafdd6ed4 100644 --- a/gtsam/slam/InitializePose.h +++ b/gtsam/slam/InitializePose.h @@ -40,11 +40,11 @@ static NonlinearFactorGraph buildPoseGraph(const NonlinearFactorGraph& graph) { for (const auto& factor : graph) { // recast to a between on Pose if (auto between = - boost::dynamic_pointer_cast >(factor)) + std::dynamic_pointer_cast >(factor)) poseGraph.add(between); // recast PriorFactor to BetweenFactor - if (auto prior = boost::dynamic_pointer_cast >(factor)) + if (auto prior = std::dynamic_pointer_cast >(factor)) poseGraph.emplace_shared >( kAnchorKey, prior->keys()[0], prior->prior(), prior->noiseModel()); } diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp index 75c33efa6..4bec99665 100644 --- a/gtsam/slam/InitializePose3.cpp +++ b/gtsam/slam/InitializePose3.cpp @@ -44,7 +44,7 @@ GaussianFactorGraph InitializePose3::buildLinearOrientationGraph(const Nonlinear Matrix3 Rij; double rotationPrecision = 1.0; - auto pose3Between = boost::dynamic_pointer_cast >(factor); + auto pose3Between = std::dynamic_pointer_cast >(factor); if (pose3Between){ Rij = pose3Between->measured().rotation().matrix(); Vector precisions = Vector::Zero(6); @@ -226,7 +226,7 @@ void InitializePose3::createSymbolicGraph( size_t factorId = 0; for (const auto& factor : pose3Graph) { auto pose3Between = - boost::dynamic_pointer_cast >(factor); + std::dynamic_pointer_cast >(factor); if (pose3Between) { Rot3 Rij = pose3Between->measured().rotation(); factorId2RotMap->emplace(factorId, Rij); diff --git a/gtsam/slam/JacobianFactorQR.h b/gtsam/slam/JacobianFactorQR.h index aef10eb86..5d59f3c62 100644 --- a/gtsam/slam/JacobianFactorQR.h +++ b/gtsam/slam/JacobianFactorQR.h @@ -44,7 +44,7 @@ public: //gfg.print("gfg"); // eliminate the point - boost::shared_ptr bn; + std::shared_ptr bn; GaussianFactorGraph::shared_ptr fg; KeyVector variables; variables.push_back(pointKey); diff --git a/gtsam/slam/KarcherMeanFactor-inl.h b/gtsam/slam/KarcherMeanFactor-inl.h index e714f653b..61af6ea89 100644 --- a/gtsam/slam/KarcherMeanFactor-inl.h +++ b/gtsam/slam/KarcherMeanFactor-inl.h @@ -74,6 +74,6 @@ KarcherMeanFactor::KarcherMeanFactor(const CONTAINER &keys, int d, terms[j] = A; } whitenedJacobian_ = - boost::make_shared(terms, Vector::Zero(d)); + std::make_shared(terms, Vector::Zero(d)); } } // namespace gtsam diff --git a/gtsam/slam/KarcherMeanFactor.h b/gtsam/slam/KarcherMeanFactor.h index 2b230c2f2..4e82ca1d9 100644 --- a/gtsam/slam/KarcherMeanFactor.h +++ b/gtsam/slam/KarcherMeanFactor.h @@ -51,7 +51,7 @@ template class KarcherMeanFactor : public NonlinearFactor { size_t d_; /// Constant Jacobian made of d*d identity matrices - boost::shared_ptr whitenedJacobian_; + std::shared_ptr whitenedJacobian_; public: /** @@ -73,7 +73,7 @@ public: size_t dim() const override { return d_; } /// linearize to a GaussianFactor - boost::shared_ptr linearize(const Values &c) const override { + std::shared_ptr linearize(const Values &c) const override { return whitenedJacobian_; } }; diff --git a/gtsam/slam/PoseRotationPrior.h b/gtsam/slam/PoseRotationPrior.h index c8dda6691..72c50d43f 100644 --- a/gtsam/slam/PoseRotationPrior.h +++ b/gtsam/slam/PoseRotationPrior.h @@ -57,7 +57,7 @@ public: /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } // access diff --git a/gtsam/slam/PoseTranslationPrior.h b/gtsam/slam/PoseTranslationPrior.h index 2ffafd104..85e7a0fa4 100644 --- a/gtsam/slam/PoseTranslationPrior.h +++ b/gtsam/slam/PoseTranslationPrior.h @@ -58,7 +58,7 @@ public: /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } /** h(x)-z */ diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h index e8824d22b..73ed85035 100644 --- a/gtsam/slam/ProjectionFactor.h +++ b/gtsam/slam/ProjectionFactor.h @@ -42,7 +42,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 + std::shared_ptr K_; ///< shared pointer to calibration object std::optional body_P_sensor_; ///< The pose of the sensor in the body frame // verbosity handling for Cheirality Exceptions @@ -61,7 +61,7 @@ namespace gtsam { typedef GenericProjectionFactor This; /// shorthand for a smart pointer to a factor - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; /// Default constructor GenericProjectionFactor() : @@ -79,7 +79,7 @@ namespace gtsam { * @param body_P_sensor is the transform from body to sensor frame (default identity) */ GenericProjectionFactor(const Point2& measured, const SharedNoiseModel& model, - Key poseKey, Key pointKey, const boost::shared_ptr& K, + Key poseKey, Key pointKey, const std::shared_ptr& K, std::optional body_P_sensor = {}) : Base(model, poseKey, pointKey), measured_(measured), K_(K), body_P_sensor_(body_P_sensor), throwCheirality_(false), verboseCheirality_(false) {} @@ -97,7 +97,7 @@ namespace gtsam { * @param body_P_sensor is the transform from body to sensor frame (default identity) */ GenericProjectionFactor(const Point2& measured, const SharedNoiseModel& model, - Key poseKey, Key pointKey, const boost::shared_ptr& K, + Key poseKey, Key pointKey, const std::shared_ptr& K, bool throwCheirality, bool verboseCheirality, std::optional body_P_sensor = {}) : Base(model, poseKey, pointKey), measured_(measured), K_(K), body_P_sensor_(body_P_sensor), @@ -108,7 +108,7 @@ namespace gtsam { /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } /** @@ -171,7 +171,7 @@ namespace gtsam { } /** return the calibration object */ - const boost::shared_ptr calibration() const { + const std::shared_ptr calibration() const { return K_; } diff --git a/gtsam/slam/ReferenceFrameFactor.h b/gtsam/slam/ReferenceFrameFactor.h index 412233dee..73952d3cc 100644 --- a/gtsam/slam/ReferenceFrameFactor.h +++ b/gtsam/slam/ReferenceFrameFactor.h @@ -93,7 +93,7 @@ public: ~ReferenceFrameFactor() override{} NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( NonlinearFactor::shared_ptr(new This(*this))); } /** Combined cost and derivative function using boost::optional */ diff --git a/gtsam/slam/RegularImplicitSchurFactor.h b/gtsam/slam/RegularImplicitSchurFactor.h index 340f84018..9f48d01aa 100644 --- a/gtsam/slam/RegularImplicitSchurFactor.h +++ b/gtsam/slam/RegularImplicitSchurFactor.h @@ -40,7 +40,7 @@ class RegularImplicitSchurFactor: public GaussianFactor { public: typedef RegularImplicitSchurFactor This; ///< Typedef to this class - typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class + typedef std::shared_ptr shared_ptr; ///< shared_ptr to this class protected: @@ -254,14 +254,14 @@ public: } GaussianFactor::shared_ptr clone() const override { - return boost::make_shared >(keys_, + return std::make_shared >(keys_, FBlocks_, PointCovariance_, E_, b_); throw std::runtime_error( "RegularImplicitSchurFactor::clone non implemented"); } GaussianFactor::shared_ptr negate() const override { - return boost::make_shared >(keys_, + return std::make_shared >(keys_, FBlocks_, PointCovariance_, E_, b_); throw std::runtime_error( "RegularImplicitSchurFactor::negate non implemented"); diff --git a/gtsam/slam/RotateFactor.h b/gtsam/slam/RotateFactor.h index aa88f978f..7efbef59a 100644 --- a/gtsam/slam/RotateFactor.h +++ b/gtsam/slam/RotateFactor.h @@ -40,7 +40,7 @@ public: /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } /// print @@ -94,7 +94,7 @@ public: /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } /// print diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index 1eba3c953..a5fc3be1a 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -31,7 +31,6 @@ #include #include -#include #include namespace gtsam { @@ -88,7 +87,7 @@ protected: GTSAM_MAKE_ALIGNED_OPERATOR_NEW /// shorthand for a smart pointer to a factor. - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; /// The CameraSet data structure is used to refer to a set of cameras. typedef CameraSet Cameras; @@ -105,7 +104,7 @@ protected: if (!sharedNoiseModel) throw std::runtime_error("SmartFactorBase: sharedNoiseModel is required"); - SharedIsotropic sharedIsotropic = boost::dynamic_pointer_cast< + SharedIsotropic sharedIsotropic = std::dynamic_pointer_cast< noiseModel::Isotropic>(sharedNoiseModel); if (!sharedIsotropic) @@ -342,7 +341,7 @@ protected: /// Linearize to a Hessianfactor. // TODO(dellaert): Not used/tested anywhere and not properly whitened. - boost::shared_ptr > createHessianFactor( + std::shared_ptr > createHessianFactor( const Cameras& cameras, const Point3& point, const double lambda = 0.0, bool diagonalDamping = false) const { @@ -353,7 +352,7 @@ protected: // build augmented hessian SymmetricBlockMatrix augmentedHessian = Cameras::SchurComplement(Fs, E, b); - return boost::make_shared >(keys_, + return std::make_shared >(keys_, augmentedHessian); } @@ -381,7 +380,7 @@ protected: } /// Return Jacobians as RegularImplicitSchurFactor with raw access - boost::shared_ptr > // + std::shared_ptr > // createRegularImplicitSchurFactor(const Cameras& cameras, const Point3& point, double lambda = 0.0, bool diagonalDamping = false) const { Matrix E; @@ -390,12 +389,12 @@ protected: computeJacobians(F, E, b, cameras, point); whitenJacobians(F, E, b); Matrix P = Cameras::PointCov(E, lambda, diagonalDamping); - return boost::make_shared >(keys_, F, E, + return std::make_shared >(keys_, F, E, P, b); } /// Return Jacobians as JacobianFactorQ. - boost::shared_ptr > createJacobianQFactor( + std::shared_ptr > createJacobianQFactor( const Cameras& cameras, const Point3& point, double lambda = 0.0, bool diagonalDamping = false) const { Matrix E; @@ -405,14 +404,14 @@ protected: const size_t M = b.size(); Matrix P = Cameras::PointCov(E, lambda, diagonalDamping); SharedIsotropic n = noiseModel::Isotropic::Sigma(M, noiseModel_->sigma()); - return boost::make_shared >(keys_, F, E, P, b, n); + return std::make_shared >(keys_, F, E, P, b, n); } /** * Return Jacobians as JacobianFactorSVD. * TODO(dellaert): lambda is currently ignored */ - boost::shared_ptr createJacobianSVDFactor( + std::shared_ptr createJacobianSVDFactor( const Cameras& cameras, const Point3& point, double lambda = 0.0) const { size_t m = this->keys_.size(); FBlocks F; @@ -422,7 +421,7 @@ protected: computeJacobiansSVD(F, E0, b, cameras, point); SharedIsotropic n = noiseModel::Isotropic::Sigma(M - 3, noiseModel_->sigma()); - return boost::make_shared >(keys_, F, E0, b, n); + return std::make_shared >(keys_, F, E0, b, n); } /// Create BIG block-diagonal matrix F from Fblocks diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index a38a8bd88..1434993e2 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -27,7 +27,6 @@ #include #include -#include #include namespace gtsam { @@ -68,7 +67,7 @@ protected: public: /// shorthand for a smart pointer to a factor - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; /// shorthand for a set of cameras typedef CAMERA Camera; @@ -196,7 +195,7 @@ protected: } /// Create a Hessianfactor that is an approximation of error(p). - boost::shared_ptr > createHessianFactor( + std::shared_ptr > createHessianFactor( const Cameras& cameras, const double lambda = 0.0, bool diagonalDamping = false) const { size_t numKeys = this->keys_.size(); @@ -216,7 +215,7 @@ protected: // failed: return"empty" Hessian for (Matrix& m : Gs) m = Matrix::Zero(Base::Dim, Base::Dim); for (Vector& v : gs) v = Vector::Zero(Base::Dim); - return boost::make_shared >(this->keys_, + return std::make_shared >(this->keys_, Gs, gs, 0.0); } @@ -233,60 +232,60 @@ protected: SymmetricBlockMatrix augmentedHessian = // Cameras::SchurComplement(Fs, E, b, lambda, diagonalDamping); - return boost::make_shared >( + return std::make_shared >( this->keys_, augmentedHessian); } // Create RegularImplicitSchurFactor factor. - boost::shared_ptr > createRegularImplicitSchurFactor( + std::shared_ptr > createRegularImplicitSchurFactor( const Cameras& cameras, double lambda) const { if (triangulateForLinearize(cameras)) return Base::createRegularImplicitSchurFactor(cameras, *result_, lambda); else // failed: return empty - return boost::shared_ptr >(); + return std::shared_ptr >(); } /// Create JacobianFactorQ factor. - boost::shared_ptr > createJacobianQFactor( + std::shared_ptr > createJacobianQFactor( const Cameras& cameras, double lambda) const { if (triangulateForLinearize(cameras)) return Base::createJacobianQFactor(cameras, *result_, lambda); else // failed: return empty - return boost::make_shared >(this->keys_); + return std::make_shared >(this->keys_); } /// Create JacobianFactorQ factor, takes values. - boost::shared_ptr > createJacobianQFactor( + std::shared_ptr > createJacobianQFactor( const Values& values, double lambda) const { return createJacobianQFactor(this->cameras(values), lambda); } /// Different (faster) way to compute a JacobianFactorSVD factor. - boost::shared_ptr createJacobianSVDFactor( + std::shared_ptr createJacobianSVDFactor( const Cameras& cameras, double lambda) const { if (triangulateForLinearize(cameras)) return Base::createJacobianSVDFactor(cameras, *result_, lambda); else // failed: return empty - return boost::make_shared >(this->keys_); + return std::make_shared >(this->keys_); } /// Linearize to a Hessianfactor. - virtual boost::shared_ptr > linearizeToHessian( + virtual std::shared_ptr > linearizeToHessian( const Values& values, double lambda = 0.0) const { return createHessianFactor(this->cameras(values), lambda); } /// Linearize to an Implicit Schur factor. - virtual boost::shared_ptr > linearizeToImplicit( + virtual std::shared_ptr > linearizeToImplicit( const Values& values, double lambda = 0.0) const { return createRegularImplicitSchurFactor(this->cameras(values), lambda); } /// Linearize to a JacobianfactorQ. - virtual boost::shared_ptr > linearizeToJacobian( + virtual std::shared_ptr > linearizeToJacobian( const Values& values, double lambda = 0.0) const { return createJacobianQFactor(this->cameras(values), lambda); } @@ -296,7 +295,7 @@ protected: * @param values Values structure which must contain camera poses for this factor * @return a Gaussian factor */ - boost::shared_ptr linearizeDamped(const Cameras& cameras, + std::shared_ptr linearizeDamped(const Cameras& cameras, const double lambda = 0.0) const { // depending on flag set on construction we may linearize to different linear factors switch (params_.linearizationMode) { @@ -318,7 +317,7 @@ protected: * @param values Values structure which must contain camera poses for this factor * @return a Gaussian factor */ - boost::shared_ptr linearizeDamped(const Values& values, + std::shared_ptr linearizeDamped(const Values& values, const double lambda = 0.0) const { // depending on flag set on construction we may linearize to different linear factors Cameras cameras = this->cameras(values); @@ -326,7 +325,7 @@ protected: } /// linearize - boost::shared_ptr linearize( + std::shared_ptr linearize( const Values& values) const override { return linearizeDamped(values); } diff --git a/gtsam/slam/SmartProjectionPoseFactor.h b/gtsam/slam/SmartProjectionPoseFactor.h index ce84babed..6421b3013 100644 --- a/gtsam/slam/SmartProjectionPoseFactor.h +++ b/gtsam/slam/SmartProjectionPoseFactor.h @@ -51,12 +51,12 @@ class SmartProjectionPoseFactor protected: - boost::shared_ptr K_; ///< calibration object (one for all cameras) + std::shared_ptr K_; ///< calibration object (one for all cameras) public: /// shorthand for a smart pointer to a factor - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; /** * Default constructor, only for serialization @@ -71,7 +71,7 @@ public: */ SmartProjectionPoseFactor( const SharedNoiseModel& sharedNoiseModel, - const boost::shared_ptr K, + const std::shared_ptr K, const SmartProjectionParams& params = SmartProjectionParams()) : Base(sharedNoiseModel, params), K_(K) { } @@ -85,7 +85,7 @@ public: */ SmartProjectionPoseFactor( const SharedNoiseModel& sharedNoiseModel, - const boost::shared_ptr K, + const std::shared_ptr K, const std::optional body_P_sensor, const SmartProjectionParams& params = SmartProjectionParams()) : SmartProjectionPoseFactor(sharedNoiseModel, K, params) { @@ -125,7 +125,7 @@ public: } /** return calibration shared pointers */ - inline const boost::shared_ptr calibration() const { + inline const std::shared_ptr calibration() const { return K_; } diff --git a/gtsam/slam/SmartProjectionRigFactor.h b/gtsam/slam/SmartProjectionRigFactor.h index 9065d5b23..cf564cdda 100644 --- a/gtsam/slam/SmartProjectionRigFactor.h +++ b/gtsam/slam/SmartProjectionRigFactor.h @@ -65,7 +65,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { KeyVector nonUniqueKeys_; /// cameras in the rig (fixed poses wrt body and intrinsics, for each camera) - boost::shared_ptr cameraRig_; + std::shared_ptr cameraRig_; /// vector of camera Ids (one for each observation, in the same order), /// identifying which camera took the measurement @@ -78,7 +78,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { typedef CameraSet Cameras; /// shorthand for a smart pointer to a factor - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; /// Default constructor, only for serialization SmartProjectionRigFactor() {} @@ -93,7 +93,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { */ SmartProjectionRigFactor( const SharedNoiseModel& sharedNoiseModel, - const boost::shared_ptr& cameraRig, + const std::shared_ptr& cameraRig, const SmartProjectionParams& params = SmartProjectionParams()) : Base(sharedNoiseModel, params), cameraRig_(cameraRig) { // throw exception if configuration is not supported by this factor @@ -171,7 +171,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { const KeyVector& nonUniqueKeys() const { return nonUniqueKeys_; } /// return the calibration object - const boost::shared_ptr& cameraRig() const { return cameraRig_; } + const std::shared_ptr& cameraRig() const { return cameraRig_; } /// return the calibration object const FastVector& cameraIds() const { return cameraIds_; } @@ -262,7 +262,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { } /// linearize and return a Hessianfactor that is an approximation of error(p) - boost::shared_ptr > createHessianFactor( + std::shared_ptr > createHessianFactor( const Values& values, const double& lambda = 0.0, bool diagonalDamping = false) const { // we may have multiple observation sharing the same keys (e.g., 2 cameras @@ -291,7 +291,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { if (this->params_.degeneracyMode == ZERO_ON_DEGENERACY) { for (Matrix& m : Gs) m = Matrix::Zero(DimPose, DimPose); for (Vector& v : gs) v = Vector::Zero(DimPose); - return boost::make_shared >(this->keys_, + return std::make_shared >(this->keys_, Gs, gs, 0.0); } else { throw std::runtime_error( @@ -321,7 +321,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { Base::Cameras::template SchurComplementAndRearrangeBlocks<3, 6, 6>( Fs, E, P, b, nonUniqueKeys_, this->keys_); - return boost::make_shared >( + return std::make_shared >( this->keys_, augmentedHessianUniqueKeys); } @@ -332,7 +332,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { * extrinsic pose for this factor * @return a Gaussian factor */ - boost::shared_ptr linearizeDamped( + std::shared_ptr linearizeDamped( const Values& values, const double& lambda = 0.0) const { // depending on flag set on construction we may linearize to different // linear factors @@ -346,7 +346,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { } /// linearize - boost::shared_ptr linearize( + std::shared_ptr linearize( const Values& values) const override { return this->linearizeDamped(values); } diff --git a/gtsam/slam/StereoFactor.h b/gtsam/slam/StereoFactor.h index d52860776..8d25d6032 100644 --- a/gtsam/slam/StereoFactor.h +++ b/gtsam/slam/StereoFactor.h @@ -46,7 +46,7 @@ public: // shorthand for base class type typedef NoiseModelFactorN Base; ///< typedef for base class typedef GenericStereoFactor This; ///< typedef for this class (with templates) - typedef boost::shared_ptr shared_ptr; ///< typedef for shared pointer to this object + typedef std::shared_ptr shared_ptr; ///< typedef for shared pointer to this object typedef POSE CamPose; ///< typedef for Pose Lie Value type // Provide access to the Matrix& version of evaluateError: @@ -96,7 +96,7 @@ public: /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } /** diff --git a/gtsam/slam/TriangulationFactor.h b/gtsam/slam/TriangulationFactor.h index bd8354435..c71c94b20 100644 --- a/gtsam/slam/TriangulationFactor.h +++ b/gtsam/slam/TriangulationFactor.h @@ -19,7 +19,6 @@ #include #include -#include #include namespace gtsam { @@ -60,7 +59,7 @@ public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW /// shorthand for a smart pointer to a factor - using shared_ptr = boost::shared_ptr; + using shared_ptr = std::shared_ptr; // Provide access to the Matrix& version of evaluateError: using NoiseModelFactor1::evaluateError; @@ -97,7 +96,7 @@ public: /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } @@ -148,10 +147,10 @@ public: * \f$ Ax-b \approx h(x+\delta x)-z = h(x) + A \delta x - z \f$ * Hence \f$ b = z - h(x) = - \mathtt{error\_vector}(x) \f$ */ - boost::shared_ptr linearize(const Values& x) const override { + std::shared_ptr linearize(const Values& x) const override { // Only linearize if the factor is active if (!this->active(x)) - return boost::shared_ptr(); + return std::shared_ptr(); // Allocate memory for Jacobian factor, do only once if (Ab.rows() == 0) { @@ -170,7 +169,7 @@ public: Ab(0) = A; Ab(1) = b; - return boost::make_shared(this->keys_, Ab); + return std::make_shared(this->keys_, Ab); } /** return the measurement */ diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index b5a1f8374..9633c5ea0 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -318,7 +318,7 @@ template struct ParseFactor : ParseMeasurement { typename std::optional::shared_ptr> operator()(istream &is, const string &tag) { if (auto m = ParseMeasurement::operator()(is, tag)) - return boost::make_shared>( + return std::make_shared>( m->key1(), m->key2(), m->measured(), m->noiseModel()); else return std::nullopt; @@ -329,7 +329,7 @@ template struct ParseFactor : ParseMeasurement { // Pose2 measurement parser template <> struct ParseMeasurement { // The arguments - boost::shared_ptr sampler; + std::shared_ptr sampler; size_t maxIndex; // For noise model creation @@ -372,12 +372,12 @@ template <> struct ParseMeasurement { /* ************************************************************************* */ // Create a sampler to corrupt a measurement -boost::shared_ptr createSampler(const SharedNoiseModel &model) { - auto noise = boost::dynamic_pointer_cast(model); +std::shared_ptr createSampler(const SharedNoiseModel &model) { + auto noise = std::dynamic_pointer_cast(model); if (!noise) throw invalid_argument("gtsam::load: invalid noise model for adding noise" "(current version assumes diagonal noise model)!"); - return boost::shared_ptr(new Sampler(noise)); + return std::shared_ptr(new Sampler(noise)); } /* ************************************************************************* */ @@ -400,7 +400,7 @@ parseMeasurements(const std::string &filename, // Extract Rot2 measurement from Pose2 measurement static BinaryMeasurement convert(const BinaryMeasurement &p) { auto gaussian = - boost::dynamic_pointer_cast(p.noiseModel()); + std::dynamic_pointer_cast(p.noiseModel()); if (!gaussian) throw std::invalid_argument( "parseMeasurements can only convert Pose2 measurements " @@ -503,7 +503,7 @@ GraphAndValues load2D(const string &filename, SharedNoiseModel model, KernelFunctionType kernelFunctionType) { // Single pass for poses and landmarks. - auto initial = boost::make_shared(); + auto initial = std::make_shared(); Parser insert = [maxIndex, &initial](istream &is, const string &tag) { if (auto indexedPose = parseVertexPose(is, tag)) { if (!maxIndex || indexedPose->first <= maxIndex) @@ -517,7 +517,7 @@ GraphAndValues load2D(const string &filename, SharedNoiseModel model, parseLines(filename, insert); // Single pass for Pose2 and bearing-range factors. - auto graph = boost::make_shared(); + auto graph = std::make_shared(); // Instantiate factor parser ParseFactor parseBetweenFactor( @@ -597,7 +597,7 @@ void save2D(const NonlinearFactorGraph &graph, const Values &config, Matrix3 R = model->R(); Matrix3 RR = R.transpose() * R; for (auto f : graph) { - auto factor = boost::dynamic_pointer_cast>(f); + auto factor = std::dynamic_pointer_cast>(f); if (!factor) continue; @@ -667,11 +667,11 @@ void writeG2o(const NonlinearFactorGraph &graph, const Values &estimate, // save edges (2D or 3D) for (const auto &factor_ : graph) { - auto factor = boost::dynamic_pointer_cast>(factor_); + auto factor = std::dynamic_pointer_cast>(factor_); if (factor) { SharedNoiseModel model = factor->noiseModel(); auto gaussianModel = - boost::dynamic_pointer_cast(model); + std::dynamic_pointer_cast(model); if (!gaussianModel) { model->print("model\n"); throw invalid_argument("writeG2o: invalid noise model!"); @@ -689,13 +689,13 @@ void writeG2o(const NonlinearFactorGraph &graph, const Values &estimate, stream << endl; } - auto factor3D = boost::dynamic_pointer_cast>(factor_); + auto factor3D = std::dynamic_pointer_cast>(factor_); if (factor3D) { SharedNoiseModel model = factor3D->noiseModel(); - boost::shared_ptr gaussianModel = - boost::dynamic_pointer_cast(model); + std::shared_ptr gaussianModel = + std::dynamic_pointer_cast(model); if (!gaussianModel) { model->print("model\n"); throw invalid_argument("writeG2o: invalid noise model!"); @@ -804,7 +804,7 @@ istream &operator>>(istream &is, Matrix6 &m) { // Pose3 measurement parser template <> struct ParseMeasurement { // The arguments - boost::shared_ptr sampler; + std::shared_ptr sampler; size_t maxIndex; // The actual parser @@ -875,7 +875,7 @@ parseMeasurements(const std::string &filename, // Extract Rot3 measurement from Pose3 measurement static BinaryMeasurement convert(const BinaryMeasurement &p) { auto gaussian = - boost::dynamic_pointer_cast(p.noiseModel()); + std::dynamic_pointer_cast(p.noiseModel()); if (!gaussian) throw std::invalid_argument( "parseMeasurements can only convert Pose3 measurements " @@ -914,8 +914,8 @@ parseFactors(const std::string &filename, /* ************************************************************************* */ GraphAndValues load3D(const string &filename) { - auto graph = boost::make_shared(); - auto initial = boost::make_shared(); + auto graph = std::make_shared(); + auto initial = std::make_shared(); // Instantiate factor parser. maxIndex is always zero for load3D. ParseFactor parseFactor({nullptr, 0}); diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index d5bad59b4..ad1bb40ef 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -34,7 +34,6 @@ #include #include -#include #include #include // for pair #include diff --git a/gtsam/slam/lago.cpp b/gtsam/slam/lago.cpp index f8b092f86..cb9d98218 100644 --- a/gtsam/slam/lago.cpp +++ b/gtsam/slam/lago.cpp @@ -102,13 +102,13 @@ void getSymbolicGraph( // Get keys for which you want the orientation size_t id = 0; // Loop over the factors - for(const boost::shared_ptr& factor: g) { + for(const std::shared_ptr& factor: g) { if (factor->keys().size() == 2) { Key key1 = factor->keys()[0]; Key key2 = factor->keys()[1]; // recast to a between - boost::shared_ptr > pose2Between = - boost::dynamic_pointer_cast >(factor); + std::shared_ptr > pose2Between = + std::dynamic_pointer_cast >(factor); if (!pose2Between) continue; // get the orientation - measured().theta(); @@ -139,8 +139,8 @@ static void getDeltaThetaAndNoise(NonlinearFactor::shared_ptr factor, Vector& deltaTheta, noiseModel::Diagonal::shared_ptr& model_deltaTheta) { // Get the relative rotation measurement from the between factor - boost::shared_ptr > pose2Between = - boost::dynamic_pointer_cast >(factor); + std::shared_ptr > pose2Between = + std::dynamic_pointer_cast >(factor); if (!pose2Between) throw invalid_argument( "buildLinearOrientationGraph: invalid between factor!"); @@ -148,8 +148,8 @@ static void getDeltaThetaAndNoise(NonlinearFactor::shared_ptr factor, // Retrieve the noise model for the relative rotation SharedNoiseModel model = pose2Between->noiseModel(); - boost::shared_ptr diagonalModel = - boost::dynamic_pointer_cast(model); + std::shared_ptr diagonalModel = + std::dynamic_pointer_cast(model); if (!diagonalModel) throw invalid_argument("buildLinearOrientationGraph: invalid noise model " "(current version assumes diagonal noise model)!"); @@ -202,7 +202,7 @@ static PredecessorMap findOdometricPath( Key minKey = initialize::kAnchorKey; // this initialization does not matter bool minUnassigned = true; - for(const boost::shared_ptr& factor: pose2Graph) { + for(const std::shared_ptr& factor: pose2Graph) { Key key1 = std::min(factor->keys()[0], factor->keys()[1]); Key key2 = std::max(factor->keys()[0], factor->keys()[1]); @@ -275,10 +275,10 @@ Values computePoses(const NonlinearFactorGraph& pose2graph, GaussianFactorGraph linearPose2graph; // We include the linear version of each between factor - for(const boost::shared_ptr& factor: pose2graph) { + for(const std::shared_ptr& factor: pose2graph) { - boost::shared_ptr > pose2Between = - boost::dynamic_pointer_cast >(factor); + std::shared_ptr > pose2Between = + std::dynamic_pointer_cast >(factor); if (pose2Between) { Key key1 = pose2Between->keys()[0]; @@ -303,8 +303,8 @@ Values computePoses(const NonlinearFactorGraph& pose2graph, J1(0, 2) = s1 * dx + c1 * dy; J1(1, 2) = -c1 * dx + s1 * dy; // Retrieve the noise model for the relative rotation - boost::shared_ptr diagonalModel = - boost::dynamic_pointer_cast( + std::shared_ptr diagonalModel = + std::dynamic_pointer_cast( pose2Between->noiseModel()); linearPose2graph.add(key1, J1, key2, I3, b, diagonalModel); diff --git a/gtsam/slam/tests/PinholeFactor.h b/gtsam/slam/tests/PinholeFactor.h index 7266a4aea..4d843f535 100644 --- a/gtsam/slam/tests/PinholeFactor.h +++ b/gtsam/slam/tests/PinholeFactor.h @@ -39,9 +39,9 @@ class PinholeFactor : public SmartFactorBase > { size_t expectedNumberCameras = 10) : Base(sharedNoiseModel, body_P_sensor, expectedNumberCameras) {} double error(const Values& values) const override { return 0.0; } - boost::shared_ptr linearize( + std::shared_ptr linearize( const Values& values) const override { - return boost::shared_ptr(new JacobianFactor()); + return std::shared_ptr(new JacobianFactor()); } }; diff --git a/gtsam/slam/tests/smartFactorScenarios.h b/gtsam/slam/tests/smartFactorScenarios.h index eff942799..9518406d8 100644 --- a/gtsam/slam/tests/smartFactorScenarios.h +++ b/gtsam/slam/tests/smartFactorScenarios.h @@ -124,7 +124,7 @@ typedef PinholePose Camera; typedef CameraSet Cameras; typedef SmartProjectionPoseFactor SmartFactor; typedef SmartProjectionRigFactor SmartRigFactor; -static const boost::shared_ptr sharedBundlerK(new Cal3Bundler(500, 1e-3, +static const std::shared_ptr sharedBundlerK(new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000)); static const Camera level_camera(level_pose, sharedBundlerK); diff --git a/gtsam/slam/tests/testAntiFactor.cpp b/gtsam/slam/tests/testAntiFactor.cpp index 4188f5639..31e38964e 100644 --- a/gtsam/slam/tests/testAntiFactor.cpp +++ b/gtsam/slam/tests/testAntiFactor.cpp @@ -61,7 +61,7 @@ TEST( AntiFactor, NegativeHessian) // Linearize the AntiFactor into a Hessian Factor GaussianFactor::shared_ptr antiGaussian = antiFactor->linearize(values); - HessianFactor::shared_ptr antiHessian = boost::dynamic_pointer_cast(antiGaussian); + HessianFactor::shared_ptr antiHessian = std::dynamic_pointer_cast(antiGaussian); Matrix expected_information = -originalHessian->information(); Matrix actual_information = antiHessian->information(); diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index be638d51a..9413ae4bd 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -101,7 +101,7 @@ TEST(dataSet, load2D) { BetweenFactor expected(1, 0, Pose2(-0.99879, 0.0417574, -0.00818381), model); BetweenFactor::shared_ptr actual = - boost::dynamic_pointer_cast>(graph->at(0)); + std::dynamic_pointer_cast>(graph->at(0)); EXPECT(assert_equal(expected, *actual)); // Check binary measurements, Pose2 @@ -117,7 +117,7 @@ TEST(dataSet, load2D) { const auto actualFactors = parseFactors(filename); for (size_t i : {0, 1, 2, 3, 4, 5}) { EXPECT(assert_equal( - *boost::dynamic_pointer_cast>(graph->at(i)), + *std::dynamic_pointer_cast>(graph->at(i)), *actualFactors[i], 1e-5)); } @@ -201,7 +201,7 @@ TEST(dataSet, readG2o3D) { const auto actualFactors = parseFactors(g2oFile); for (size_t i : {0, 1, 2, 3, 4, 5}) { EXPECT(assert_equal( - *boost::dynamic_pointer_cast>(expectedGraph[i]), + *std::dynamic_pointer_cast>(expectedGraph[i]), *actualFactors[i], 1e-5)); } diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index ef22bad2a..0f9c2ef9f 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -547,7 +547,7 @@ Point2 pA(size_t i) { return data.tracks[i].measurements[0].second; } Point2 pB(size_t i) { return data.tracks[i].measurements[1].second; } Cal3Bundler trueK = Cal3Bundler(500, 0, 0); -boost::shared_ptr K = boost::make_shared(trueK); +std::shared_ptr K = std::make_shared(trueK); PinholeCamera camera2(data.cameras[1].pose(), trueK); Vector vA(size_t i) { diff --git a/gtsam/slam/tests/testGeneralSFMFactor.cpp b/gtsam/slam/tests/testGeneralSFMFactor.cpp index 23ddb8207..553791385 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor.cpp @@ -28,7 +28,7 @@ #include #include -#include +#include #include #include @@ -50,16 +50,16 @@ class Graph: public NonlinearFactorGraph { public: void addMeasurement(int i, int j, const Point2& z, const SharedNoiseModel& model) { - push_back(boost::make_shared(z, model, X(i), L(j))); + push_back(std::make_shared(z, model, X(i), L(j))); } void addCameraConstraint(int j, const GeneralCamera& p) { - boost::shared_ptr factor(new CameraConstraint(X(j), p)); + std::shared_ptr factor(new CameraConstraint(X(j), p)); push_back(factor); } void addPoint3Constraint(int j, const Point3& p) { - boost::shared_ptr factor(new Point3Constraint(L(j), p)); + std::shared_ptr factor(new Point3Constraint(L(j), p)); push_back(factor); } @@ -115,9 +115,9 @@ static vector genCameraVariableCalibration() { return X; } -static boost::shared_ptr getOrdering( +static std::shared_ptr getOrdering( const vector& cameras, const vector& landmarks) { - boost::shared_ptr ordering(new Ordering); + std::shared_ptr ordering(new Ordering); for (size_t i = 0; i < landmarks.size(); ++i) ordering->push_back(L(i)); for (size_t i = 0; i < cameras.size(); ++i) @@ -131,10 +131,10 @@ TEST( GeneralSFMFactor, equals ) { Point2 z(323., 240.); const Symbol cameraFrameNumber('x', 1), landmarkNumber('l', 1); const SharedNoiseModel sigma(noiseModel::Unit::Create(1)); - boost::shared_ptr factor1( + std::shared_ptr factor1( new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); - boost::shared_ptr factor2( + std::shared_ptr factor2( new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); EXPECT(assert_equal(*factor1, *factor2)); @@ -510,7 +510,7 @@ TEST( GeneralSFMFactor, BinaryJacobianFactor2 ) { for (size_t j = 0; j < landmarks.size(); ++j) { Point2 z = cameras[i].project(landmarks[j]); Projection::shared_ptr nonlinear = // - boost::make_shared(z, sigma1, X(i), L(j)); + std::make_shared(z, sigma1, X(i), L(j)); GaussianFactor::shared_ptr factor = nonlinear->linearize(values); HessianFactor hessian(*factor); JacobianFactor jacobian(hessian); diff --git a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp index d8ac0aa5b..f8385352f 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp @@ -28,7 +28,7 @@ #include #include -#include +#include #include #include @@ -51,16 +51,16 @@ class Graph: public NonlinearFactorGraph { public: void addMeasurement(const int& i, const int& j, const Point2& z, const SharedNoiseModel& model) { - push_back(boost::make_shared(z, model, X(i), L(j))); + push_back(std::make_shared(z, model, X(i), L(j))); } void addCameraConstraint(int j, const GeneralCamera& p) { - boost::shared_ptr factor(new CameraConstraint(X(j), p)); + std::shared_ptr factor(new CameraConstraint(X(j), p)); push_back(factor); } void addPoint3Constraint(int j, const Point3& p) { - boost::shared_ptr factor(new Point3Constraint(L(j), p)); + std::shared_ptr factor(new Point3Constraint(L(j), p)); push_back(factor); } @@ -87,10 +87,10 @@ TEST( GeneralSFMFactor_Cal3Bundler, equals ) { Point2 z(323., 240.); const Symbol cameraFrameNumber('x', 1), landmarkNumber('l', 1); const SharedNoiseModel sigma(noiseModel::Unit::Create(1)); - boost::shared_ptr factor1( + std::shared_ptr factor1( new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); - boost::shared_ptr factor2( + std::shared_ptr factor2( new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); EXPECT(assert_equal(*factor1, *factor2)); @@ -100,7 +100,7 @@ TEST( GeneralSFMFactor_Cal3Bundler, equals ) { TEST( GeneralSFMFactor_Cal3Bundler, error ) { Point2 z(3., 0.); const SharedNoiseModel sigma(noiseModel::Unit::Create(1)); - boost::shared_ptr factor(new Projection(z, sigma, X(1), L(1))); + std::shared_ptr factor(new Projection(z, sigma, X(1), L(1))); // For the following configuration, the factor predicts 320,240 Values values; Rot3 R; @@ -152,10 +152,10 @@ static vector genCameraVariableCalibration() { return cameras; } -static boost::shared_ptr getOrdering( +static std::shared_ptr getOrdering( const std::vector& cameras, const std::vector& landmarks) { - boost::shared_ptr ordering(new Ordering); + std::shared_ptr ordering(new Ordering); for (size_t i = 0; i < landmarks.size(); ++i) ordering->push_back(L(i)); for (size_t i = 0; i < cameras.size(); ++i) diff --git a/gtsam/slam/tests/testSmartFactorBase.cpp b/gtsam/slam/tests/testSmartFactorBase.cpp index 544fd3264..52deb2bf4 100644 --- a/gtsam/slam/tests/testSmartFactorBase.cpp +++ b/gtsam/slam/tests/testSmartFactorBase.cpp @@ -84,9 +84,9 @@ class StereoFactor : public SmartFactorBase { StereoFactor(const SharedNoiseModel& sharedNoiseModel) : Base(sharedNoiseModel) {} double error(const Values& values) const override { return 0.0; } - boost::shared_ptr linearize( + std::shared_ptr linearize( const Values& values) const override { - return boost::shared_ptr(new JacobianFactor()); + return std::shared_ptr(new JacobianFactor()); } }; diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index 2eb799663..807f485c7 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -355,7 +355,7 @@ TEST( SmartProjectionPoseFactor, Factors ) { // Create smart factors KeyVector views {x1, x2}; - SmartFactor::shared_ptr smartFactor1 = boost::make_shared(model, sharedK); + SmartFactor::shared_ptr smartFactor1 = std::make_shared(model, sharedK); smartFactor1->add(measurements_cam1, views); SmartFactor::Cameras cameras; @@ -405,7 +405,7 @@ TEST( SmartProjectionPoseFactor, Factors ) { RegularHessianFactor<6> expected(x1, x2, G11, G12, g1, G22, g2, f); expectedInformation = expected.information(); - boost::shared_ptr > actual = + std::shared_ptr > actual = smartFactor1->createHessianFactor(cameras, 0.0); EXPECT(assert_equal(expectedInformation, actual->information(), 1e-6)); EXPECT(assert_equal(expected, *actual, 1e-6)); @@ -450,7 +450,7 @@ TEST( SmartProjectionPoseFactor, Factors ) { JacobianFactorQ<6, 2> expectedQ(keys, Fs, E, P, b, n); EXPECT(assert_equal(expectedInformation, expectedQ.information(), 1e-6)); - boost::shared_ptr > actualQ = + std::shared_ptr > actualQ = smartFactor1->createJacobianQFactor(cameras, 0.0); CHECK(actualQ); EXPECT(assert_equal(expectedInformation, actualQ->information(), 1e-6)); @@ -467,7 +467,7 @@ TEST( SmartProjectionPoseFactor, Factors ) { // createRegularImplicitSchurFactor RegularImplicitSchurFactor expected(keys, Fs, E, whiteP, b); - boost::shared_ptr > actual = + std::shared_ptr > actual = smartFactor1->createRegularImplicitSchurFactor(cameras, 0.0); CHECK(actual); EXPECT(assert_equal(expectedInformation, expected.information(), 1e-6)); @@ -486,7 +486,7 @@ TEST( SmartProjectionPoseFactor, Factors ) { JacobianFactor expected(x1, s * A1, x2, s * A2, b, n); EXPECT(assert_equal(expectedInformation, expected.information(), 1e-6)); - boost::shared_ptr actual = + std::shared_ptr actual = smartFactor1->createJacobianSVDFactor(cameras, 0.0); CHECK(actual); EXPECT(assert_equal(expectedInformation, actual->information(), 1e-6)); @@ -887,14 +887,14 @@ TEST( SmartProjectionPoseFactor, CheckHessian) { Point3(0.0897734171, -0.110201006, 0.901022872)), values.at(x3))); - boost::shared_ptr factor1 = smartFactor1->linearize(values); - boost::shared_ptr factor2 = smartFactor2->linearize(values); - boost::shared_ptr factor3 = smartFactor3->linearize(values); + std::shared_ptr factor1 = smartFactor1->linearize(values); + std::shared_ptr factor2 = smartFactor2->linearize(values); + std::shared_ptr factor3 = smartFactor3->linearize(values); Matrix CumulativeInformation = factor1->information() + factor2->information() + factor3->information(); - boost::shared_ptr GaussianGraph = graph.linearize( + std::shared_ptr GaussianGraph = graph.linearize( values); Matrix GraphInformation = GaussianGraph->hessian().first; @@ -1071,7 +1071,7 @@ TEST( SmartProjectionPoseFactor, Hessian ) { values.insert(x1, cam1.pose()); values.insert(x2, cam2.pose()); - boost::shared_ptr factor = smartFactor1->linearize(values); + std::shared_ptr factor = smartFactor1->linearize(values); // compute triangulation from linearization point // compute reprojection errors (sum squared) @@ -1099,7 +1099,7 @@ TEST( SmartProjectionPoseFactor, HessianWithRotation ) { values.insert(x2, cam2.pose()); values.insert(x3, cam3.pose()); - boost::shared_ptr factor = smartFactorInstance->linearize( + std::shared_ptr factor = smartFactorInstance->linearize( values); Pose3 poseDrift = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0)); @@ -1109,7 +1109,7 @@ TEST( SmartProjectionPoseFactor, HessianWithRotation ) { rotValues.insert(x2, poseDrift.compose(pose_right)); rotValues.insert(x3, poseDrift.compose(pose_above)); - boost::shared_ptr factorRot = smartFactorInstance->linearize( + std::shared_ptr factorRot = smartFactorInstance->linearize( rotValues); // Hessian is invariant to rotations in the nondegenerate case @@ -1123,7 +1123,7 @@ TEST( SmartProjectionPoseFactor, HessianWithRotation ) { tranValues.insert(x2, poseDrift2.compose(pose_right)); tranValues.insert(x3, poseDrift2.compose(pose_above)); - boost::shared_ptr factorRotTran = + std::shared_ptr factorRotTran = smartFactorInstance->linearize(tranValues); // Hessian is invariant to rotations and translations in the nondegenerate case @@ -1152,7 +1152,7 @@ TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ) { values.insert(x2, cam2.pose()); values.insert(x3, cam3.pose()); - boost::shared_ptr factor = smartFactor->linearize(values); + std::shared_ptr factor = smartFactor->linearize(values); Pose3 poseDrift = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0)); @@ -1161,7 +1161,7 @@ TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ) { rotValues.insert(x2, poseDrift.compose(level_pose)); rotValues.insert(x3, poseDrift.compose(level_pose)); - boost::shared_ptr factorRot = // + std::shared_ptr factorRot = // smartFactor->linearize(rotValues); // Hessian is invariant to rotations in the nondegenerate case @@ -1175,7 +1175,7 @@ TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ) { tranValues.insert(x2, poseDrift2.compose(level_pose)); tranValues.insert(x3, poseDrift2.compose(level_pose)); - boost::shared_ptr factorRotTran = smartFactor->linearize( + std::shared_ptr factorRotTran = smartFactor->linearize( tranValues); // Hessian is invariant to rotations and translations in the nondegenerate case diff --git a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp index 2fa6e7290..67bbfa3ba 100644 --- a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp @@ -66,7 +66,7 @@ SmartProjectionParams params( /* ************************************************************************* */ TEST(SmartProjectionRigFactor, Constructor) { using namespace vanillaRig; - boost::shared_ptr cameraRig(new Cameras()); + std::shared_ptr cameraRig(new Cameras()); cameraRig->push_back(Camera(Pose3(), sharedK)); SmartRigFactor::shared_ptr factor1( new SmartRigFactor(model, cameraRig, params)); @@ -75,7 +75,7 @@ TEST(SmartProjectionRigFactor, Constructor) { /* ************************************************************************* */ TEST(SmartProjectionRigFactor, Constructor2) { using namespace vanillaRig; - boost::shared_ptr cameraRig(new Cameras()); + std::shared_ptr cameraRig(new Cameras()); SmartProjectionParams params2( gtsam::HESSIAN, gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors @@ -86,7 +86,7 @@ TEST(SmartProjectionRigFactor, Constructor2) { /* ************************************************************************* */ TEST(SmartProjectionRigFactor, Constructor3) { using namespace vanillaRig; - boost::shared_ptr cameraRig(new Cameras()); + std::shared_ptr cameraRig(new Cameras()); cameraRig->push_back(Camera(Pose3(), sharedK)); SmartRigFactor::shared_ptr factor1( new SmartRigFactor(model, cameraRig, params)); @@ -96,7 +96,7 @@ TEST(SmartProjectionRigFactor, Constructor3) { /* ************************************************************************* */ TEST(SmartProjectionRigFactor, Constructor4) { using namespace vanillaRig; - boost::shared_ptr cameraRig(new Cameras()); + std::shared_ptr cameraRig(new Cameras()); cameraRig->push_back(Camera(Pose3(), sharedK)); SmartProjectionParams params2( gtsam::HESSIAN, @@ -109,7 +109,7 @@ TEST(SmartProjectionRigFactor, Constructor4) { /* ************************************************************************* */ TEST(SmartProjectionRigFactor, Equals) { using namespace vanillaRig; - boost::shared_ptr cameraRig(new Cameras()); // single camera in the rig + std::shared_ptr cameraRig(new Cameras()); // single camera in the rig cameraRig->push_back(Camera(Pose3(), sharedK)); SmartRigFactor::shared_ptr factor1( @@ -137,7 +137,7 @@ TEST(SmartProjectionRigFactor, noiseless) { Point2 level_uv = level_camera.project(landmark1); Point2 level_uv_right = level_camera_right.project(landmark1); - boost::shared_ptr cameraRig(new Cameras()); // single camera in the rig + std::shared_ptr cameraRig(new Cameras()); // single camera in the rig cameraRig->push_back(Camera(Pose3(), sharedK)); SmartRigFactor factor(model, cameraRig, params); @@ -194,7 +194,7 @@ TEST(SmartProjectionRigFactor, noiseless) { TEST(SmartProjectionRigFactor, noisy) { using namespace vanillaRig; - boost::shared_ptr cameraRig(new Cameras()); // single camera in the rig + std::shared_ptr cameraRig(new Cameras()); // single camera in the rig cameraRig->push_back(Camera(Pose3(), sharedK)); // Project two landmarks into two cameras @@ -238,7 +238,7 @@ TEST(SmartProjectionRigFactor, smartFactorWithSensorBodyTransform) { // create arbitrary body_T_sensor (transforms from sensor to body) Pose3 body_T_sensor = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(1, 1, 1)); - boost::shared_ptr cameraRig(new Cameras()); // single camera in the rig + std::shared_ptr cameraRig(new Cameras()); // single camera in the rig cameraRig->push_back(Camera(body_T_sensor, sharedK)); // These are the poses we want to estimate, from camera measurements @@ -323,7 +323,7 @@ TEST(SmartProjectionRigFactor, smartFactorWithMultipleCameras) { Pose3(Rot3::Ypr(-M_PI / 5, 0., -M_PI / 2), Point3(0, 0, 1)); Pose3 body_T_sensor3 = Pose3(); - boost::shared_ptr cameraRig(new Cameras()); // single camera in the rig + std::shared_ptr cameraRig(new Cameras()); // single camera in the rig cameraRig->push_back(Camera(body_T_sensor1, sharedK)); cameraRig->push_back(Camera(body_T_sensor2, sharedK)); cameraRig->push_back(Camera(body_T_sensor3, sharedK)); @@ -404,7 +404,7 @@ TEST(SmartProjectionRigFactor, 3poses_smart_projection_factor) { using namespace vanillaPose2; Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; - boost::shared_ptr cameraRig(new Cameras()); // single camera in the rig + std::shared_ptr cameraRig(new Cameras()); // single camera in the rig cameraRig->push_back(Camera(Pose3(), sharedK2)); // Project three landmarks into three cameras @@ -492,10 +492,10 @@ TEST(SmartProjectionRigFactor, Factors) { KeyVector views{x1, x2}; FastVector cameraIds{0, 0}; - boost::shared_ptr cameraRig(new Cameras()); // single camera in the rig + std::shared_ptr cameraRig(new Cameras()); // single camera in the rig cameraRig->push_back(Camera(Pose3(), sharedK)); - SmartRigFactor::shared_ptr smartFactor1 = boost::make_shared( + SmartRigFactor::shared_ptr smartFactor1 = std::make_shared( model, cameraRig, params); smartFactor1->add(measurements_cam1, views); // we have a single camera so use default cameraIds @@ -552,7 +552,7 @@ TEST(SmartProjectionRigFactor, Factors) { values.insert(x1, cam1.pose()); values.insert(x2, cam2.pose()); - boost::shared_ptr> actual = + std::shared_ptr> actual = smartFactor1->createHessianFactor(values, 0.0); EXPECT(assert_equal(expectedInformation, actual->information(), 1e-6)); EXPECT(assert_equal(expected, *actual, 1e-6)); @@ -575,7 +575,7 @@ TEST(SmartProjectionRigFactor, 3poses_iterative_smart_projection_factor) { projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); // create smart factor - boost::shared_ptr cameraRig(new Cameras()); // single camera in the rig + std::shared_ptr cameraRig(new Cameras()); // single camera in the rig cameraRig->push_back(Camera(Pose3(), sharedK)); FastVector cameraIds{0, 0, 0}; SmartRigFactor::shared_ptr smartFactor1( @@ -643,7 +643,7 @@ TEST(SmartProjectionRigFactor, landmarkDistance) { params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); params.setEnableEPI(false); - boost::shared_ptr cameraRig(new Cameras()); // single camera in the rig + std::shared_ptr cameraRig(new Cameras()); // single camera in the rig cameraRig->push_back(Camera(Pose3(), sharedK)); FastVector cameraIds{0, 0, 0}; @@ -714,7 +714,7 @@ TEST(SmartProjectionRigFactor, dynamicOutlierRejection) { params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold); - boost::shared_ptr cameraRig(new Cameras()); // single camera in the rig + std::shared_ptr cameraRig(new Cameras()); // single camera in the rig cameraRig->push_back(Camera(Pose3(), sharedK)); FastVector cameraIds{0, 0, 0}; @@ -780,7 +780,7 @@ TEST(SmartProjectionRigFactor, CheckHessian) { params.setRankTolerance(10); params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); - boost::shared_ptr cameraRig(new Cameras()); // single camera in the rig + std::shared_ptr cameraRig(new Cameras()); // single camera in the rig cameraRig->push_back(Camera(Pose3(), sharedK)); FastVector cameraIds{0, 0, 0}; @@ -817,14 +817,14 @@ TEST(SmartProjectionRigFactor, CheckHessian) { Point3(0.0897734171, -0.110201006, 0.901022872)), values.at(x3))); - boost::shared_ptr factor1 = smartFactor1->linearize(values); - boost::shared_ptr factor2 = smartFactor2->linearize(values); - boost::shared_ptr factor3 = smartFactor3->linearize(values); + std::shared_ptr factor1 = smartFactor1->linearize(values); + std::shared_ptr factor2 = smartFactor2->linearize(values); + std::shared_ptr factor3 = smartFactor3->linearize(values); Matrix CumulativeInformation = factor1->information() + factor2->information() + factor3->information(); - boost::shared_ptr GaussianGraph = + std::shared_ptr GaussianGraph = graph.linearize(values); Matrix GraphInformation = GaussianGraph->hessian().first; @@ -856,7 +856,7 @@ TEST(SmartProjectionRigFactor, Hessian) { measurements_cam1.push_back(cam1_uv1); measurements_cam1.push_back(cam2_uv1); - boost::shared_ptr cameraRig(new Cameras()); // single camera in the rig + std::shared_ptr cameraRig(new Cameras()); // single camera in the rig cameraRig->push_back(Camera(Pose3(), sharedK2)); FastVector cameraIds{0, 0}; @@ -874,7 +874,7 @@ TEST(SmartProjectionRigFactor, Hessian) { values.insert(x1, cam1.pose()); values.insert(x2, cam2.pose()); - boost::shared_ptr factor = smartFactor1->linearize(values); + std::shared_ptr factor = smartFactor1->linearize(values); // compute triangulation from linearization point // compute reprojection errors (sum squared) @@ -886,7 +886,7 @@ TEST(SmartProjectionRigFactor, Hessian) { /* ************************************************************************* */ TEST(SmartProjectionRigFactor, ConstructorWithCal3Bundler) { using namespace bundlerPose; - boost::shared_ptr cameraRig(new Cameras()); // single camera in the rig + std::shared_ptr cameraRig(new Cameras()); // single camera in the rig cameraRig->push_back(Camera(Pose3(), sharedBundlerK)); SmartProjectionParams params; @@ -914,7 +914,7 @@ TEST(SmartProjectionRigFactor, Cal3Bundler) { KeyVector views{x1, x2, x3}; - boost::shared_ptr cameraRig(new Cameras()); // single camera in the rig + std::shared_ptr cameraRig(new Cameras()); // single camera in the rig cameraRig->push_back(Camera(Pose3(), sharedBundlerK)); FastVector cameraIds{0, 0, 0}; @@ -985,7 +985,7 @@ TEST(SmartProjectionRigFactor, // create inputs KeyVector keys{x1, x2, x3, x1}; - boost::shared_ptr cameraRig(new Cameras()); // single camera in the rig + std::shared_ptr cameraRig(new Cameras()); // single camera in the rig cameraRig->push_back(Camera(Pose3(), sharedK)); FastVector cameraIds{0, 0, 0, 0}; @@ -1015,7 +1015,7 @@ TEST(SmartProjectionRigFactor, // ==== check Hessian of smartFactor1 ===== // -- compute actual Hessian - boost::shared_ptr linearfactor1 = + std::shared_ptr linearfactor1 = smartFactor1->linearize(values); Matrix actualHessian = linearfactor1->information(); @@ -1113,7 +1113,7 @@ TEST(SmartProjectionRigFactor, optimization_3poses_measurementsFromSamePose) { // create inputs KeyVector keys{x1, x2, x3}; - boost::shared_ptr cameraRig(new Cameras()); // single camera in the rig + std::shared_ptr cameraRig(new Cameras()); // single camera in the rig cameraRig->push_back(Camera(Pose3(), sharedK)); FastVector cameraIds{0, 0, 0}; FastVector cameraIdsRedundant{0, 0, 0, 0}; @@ -1199,7 +1199,7 @@ TEST(SmartProjectionRigFactor, timing) { Camera cam1(pose1, sharedKSimple), cam2(pose2, sharedKSimple); Pose3 body_P_sensorId = Pose3(); - boost::shared_ptr cameraRig(new Cameras()); // single camera in the rig + std::shared_ptr cameraRig(new Cameras()); // single camera in the rig cameraRig->push_back(Camera(body_P_sensorId, sharedKSimple)); // one landmarks 1m in front of camera @@ -1264,7 +1264,7 @@ TEST(SmartProjectionFactorP, optimization_3poses_sphericalCamera) { keys.push_back(x2); keys.push_back(x3); - boost::shared_ptr cameraRig(new Cameras()); + std::shared_ptr cameraRig(new Cameras()); cameraRig->push_back(Camera(Pose3(), emptyK)); SmartProjectionParams params( @@ -1357,7 +1357,7 @@ TEST(SmartProjectionFactorP, timing_sphericalCamera) { size_t nrTests = 1000; for (size_t i = 0; i < nrTests; i++) { - boost::shared_ptr> cameraRig( + std::shared_ptr> cameraRig( new CameraSet()); // single camera in the rig cameraRig->push_back(SphericalCamera(body_P_sensorId, emptyK)); @@ -1376,7 +1376,7 @@ TEST(SmartProjectionFactorP, timing_sphericalCamera) { } for (size_t i = 0; i < nrTests; i++) { - boost::shared_ptr>> cameraRig( + std::shared_ptr>> cameraRig( new CameraSet>()); // single camera in the rig cameraRig->push_back(PinholePose(body_P_sensorId, sharedKSimple)); @@ -1419,7 +1419,7 @@ TEST(SmartProjectionFactorP, 2poses_rankTol) { gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors params.setRankTolerance(1); - boost::shared_ptr>> cameraRig( + std::shared_ptr>> cameraRig( new CameraSet>()); // single camera in the rig cameraRig->push_back(PinholePose(Pose3(), sharedK)); @@ -1457,7 +1457,7 @@ TEST(SmartProjectionFactorP, 2poses_rankTol) { gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors params.setRankTolerance(0.1); - boost::shared_ptr>> cameraRig( + std::shared_ptr>> cameraRig( new CameraSet>()); // single camera in the rig cameraRig->push_back(PinholePose(Pose3(), canonicalK)); @@ -1494,7 +1494,7 @@ TEST(SmartProjectionFactorP, 2poses_rankTol) { gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors params.setRankTolerance(0.01); - boost::shared_ptr>> cameraRig( + std::shared_ptr>> cameraRig( new CameraSet>()); // single camera in the rig cameraRig->push_back(PinholePose(Pose3(), canonicalK)); @@ -1533,7 +1533,7 @@ TEST(SmartProjectionFactorP, 2poses_sphericalCamera_rankTol) { Camera cam1(poseA); Camera cam2(poseB); - boost::shared_ptr> cameraRig( + std::shared_ptr> cameraRig( new CameraSet()); // single camera in the rig cameraRig->push_back(SphericalCamera(Pose3(), emptyK)); diff --git a/gtsam/slam/tests/testStereoFactor.cpp b/gtsam/slam/tests/testStereoFactor.cpp index c802603db..bcd1e9a25 100644 --- a/gtsam/slam/tests/testStereoFactor.cpp +++ b/gtsam/slam/tests/testStereoFactor.cpp @@ -34,7 +34,7 @@ using namespace gtsam; static Pose3 camera1(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0,0,6.25)); -static boost::shared_ptr K(new Cal3_S2Stereo(625, 625, 0, 320, 240, 0.5)); +static std::shared_ptr K(new Cal3_S2Stereo(625, 625, 0, 320, 240, 0.5)); // point X Y Z in meters static Point3 p(0, 0, 5); diff --git a/gtsam/slam/tests/testTriangulationFactor.cpp b/gtsam/slam/tests/testTriangulationFactor.cpp index bdfade5f2..4173a54f8 100644 --- a/gtsam/slam/tests/testTriangulationFactor.cpp +++ b/gtsam/slam/tests/testTriangulationFactor.cpp @@ -32,8 +32,8 @@ using namespace gtsam; using namespace std::placeholders; // Some common constants -static const boost::shared_ptr sharedCal = // - boost::make_shared(1500, 1200, 0, 640, 480); +static const std::shared_ptr sharedCal = // + std::make_shared(1500, 1200, 0, 640, 480); // Looking along X-axis, 1 meter above ground plane (x-y) static const Rot3 upright = Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2); diff --git a/gtsam/symbolic/SymbolicBayesNet.h b/gtsam/symbolic/SymbolicBayesNet.h index ef1505108..31b1bac33 100644 --- a/gtsam/symbolic/SymbolicBayesNet.h +++ b/gtsam/symbolic/SymbolicBayesNet.h @@ -34,8 +34,8 @@ namespace gtsam { typedef BayesNet Base; typedef SymbolicBayesNet This; typedef SymbolicConditional ConditionalType; - typedef boost::shared_ptr shared_ptr; - typedef boost::shared_ptr sharedConditional; + typedef std::shared_ptr shared_ptr; + typedef std::shared_ptr sharedConditional; /// @name Standard Constructors /// @{ @@ -64,12 +64,12 @@ namespace gtsam { * Constructor that takes an initializer list of shared pointers. * SymbolicBayesNet bn = {make_shared(), ...}; */ - SymbolicBayesNet(std::initializer_list> conditionals) + SymbolicBayesNet(std::initializer_list> conditionals) : Base(conditionals) {} /// Construct from a single conditional SymbolicBayesNet(SymbolicConditional&& c) { - push_back(boost::make_shared(c)); + push_back(std::make_shared(c)); } /** @@ -79,7 +79,7 @@ namespace gtsam { * SymbolicBayesNet(SymbolicConditional(...))(SymbolicConditional(...)); */ SymbolicBayesNet& operator()(SymbolicConditional&& c) { - push_back(boost::make_shared(c)); + push_back(std::make_shared(c)); return *this; } diff --git a/gtsam/symbolic/SymbolicBayesTree.h b/gtsam/symbolic/SymbolicBayesTree.h index 3fdf1011b..0a6b6608e 100644 --- a/gtsam/symbolic/SymbolicBayesTree.h +++ b/gtsam/symbolic/SymbolicBayesTree.h @@ -36,11 +36,11 @@ namespace gtsam { public: typedef SymbolicBayesTreeClique This; typedef BayesTreeCliqueBase Base; - typedef boost::shared_ptr shared_ptr; - typedef boost::weak_ptr weak_ptr; + typedef std::shared_ptr shared_ptr; + typedef std::weak_ptr weak_ptr; SymbolicBayesTreeClique() {} virtual ~SymbolicBayesTreeClique() {} - SymbolicBayesTreeClique(const boost::shared_ptr& conditional) : Base(conditional) {} + SymbolicBayesTreeClique(const std::shared_ptr& conditional) : Base(conditional) {} }; /* ************************************************************************* */ @@ -54,7 +54,7 @@ namespace gtsam { public: typedef SymbolicBayesTree This; - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; /** Default constructor, creates an empty Bayes tree */ SymbolicBayesTree() {} diff --git a/gtsam/symbolic/SymbolicConditional.h b/gtsam/symbolic/SymbolicConditional.h index 6bd966450..1411ceafe 100644 --- a/gtsam/symbolic/SymbolicConditional.h +++ b/gtsam/symbolic/SymbolicConditional.h @@ -41,7 +41,7 @@ namespace gtsam { typedef SymbolicConditional This; /// Typedef to this class typedef SymbolicFactor BaseFactor; /// Typedef to the factor base class typedef Conditional BaseConditional; /// Typedef to the conditional base class - typedef boost::shared_ptr shared_ptr; /// Boost shared_ptr to this class + typedef std::shared_ptr shared_ptr; /// Boost shared_ptr to this class typedef BaseFactor::iterator iterator; /// iterator to keys typedef BaseFactor::const_iterator const_iterator; /// const_iterator to keys @@ -77,7 +77,7 @@ namespace gtsam { template static SymbolicConditional::shared_ptr FromIteratorsShared(ITERATOR firstKey, ITERATOR lastKey, size_t nrFrontals) { - SymbolicConditional::shared_ptr result = boost::make_shared(); + SymbolicConditional::shared_ptr result = std::make_shared(); result->keys_.assign(firstKey, lastKey); result->nrFrontals_ = nrFrontals; return result; @@ -96,7 +96,7 @@ namespace gtsam { } /// Copy this object as its actual derived type. - SymbolicFactor::shared_ptr clone() const { return boost::make_shared(*this); } + SymbolicFactor::shared_ptr clone() const { return std::make_shared(*this); } /// @} /// @name Testable diff --git a/gtsam/symbolic/SymbolicEliminationTree.h b/gtsam/symbolic/SymbolicEliminationTree.h index 5ec886653..a68675e1d 100644 --- a/gtsam/symbolic/SymbolicEliminationTree.h +++ b/gtsam/symbolic/SymbolicEliminationTree.h @@ -30,7 +30,7 @@ namespace gtsam { public: typedef EliminationTree Base; ///< Base class typedef SymbolicEliminationTree This; ///< This class - typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class + typedef std::shared_ptr shared_ptr; ///< Shared pointer to this class /** Build the elimination tree of a factor graph using pre-computed column structure. * @param factorGraph The factor graph for which to build the elimination tree diff --git a/gtsam/symbolic/SymbolicFactor-inst.h b/gtsam/symbolic/SymbolicFactor-inst.h index e3b4c3297..85ddf9c79 100644 --- a/gtsam/symbolic/SymbolicFactor-inst.h +++ b/gtsam/symbolic/SymbolicFactor-inst.h @@ -23,8 +23,7 @@ #include #include -#include -#include +#include #include @@ -35,14 +34,14 @@ namespace gtsam /** Implementation of dense elimination function for symbolic factors. This is a templated * version for internally doing symbolic elimination on any factor. */ template - std::pair, boost::shared_ptr > + std::pair, std::shared_ptr > EliminateSymbolic(const FactorGraph& factors, const Ordering& keys) { gttic(EliminateSymbolic); // Gather all keys KeySet allKeys; - for(const boost::shared_ptr& factor: factors) { + for(const std::shared_ptr& factor: factors) { allKeys.insert(factor->begin(), factor->end()); } diff --git a/gtsam/symbolic/SymbolicFactor.cpp b/gtsam/symbolic/SymbolicFactor.cpp index 671b71f93..e51b3cf29 100644 --- a/gtsam/symbolic/SymbolicFactor.cpp +++ b/gtsam/symbolic/SymbolicFactor.cpp @@ -32,7 +32,7 @@ namespace gtsam { } /* ************************************************************************* */ - std::pair, boost::shared_ptr > + std::pair, std::shared_ptr > EliminateSymbolic(const SymbolicFactorGraph& factors, const Ordering& keys) { return internal::EliminateSymbolic(factors, keys); @@ -45,7 +45,7 @@ namespace gtsam { } /* ************************************************************************* */ - std::pair, boost::shared_ptr > + std::pair, std::shared_ptr > SymbolicFactor::eliminate(const Ordering& keys) const { SymbolicFactorGraph graph; diff --git a/gtsam/symbolic/SymbolicFactor.h b/gtsam/symbolic/SymbolicFactor.h index 12b3374c8..86ebdffbf 100644 --- a/gtsam/symbolic/SymbolicFactor.h +++ b/gtsam/symbolic/SymbolicFactor.h @@ -21,8 +21,7 @@ #include #include -#include -#include +#include #include @@ -45,7 +44,7 @@ namespace gtsam { typedef SymbolicConditional ConditionalType; /** Overriding the shared_ptr typedef */ - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; /// @name Standard Constructors /// @{ @@ -83,7 +82,7 @@ namespace gtsam { virtual ~SymbolicFactor() {} /// Copy this object as its actual derived type. - SymbolicFactor::shared_ptr clone() const { return boost::make_shared(*this); } + SymbolicFactor::shared_ptr clone() const { return std::make_shared(*this); } /// @} @@ -119,7 +118,7 @@ namespace gtsam { /** Constructor from a collection of keys */ template static SymbolicFactor::shared_ptr FromIteratorsShared(KEYITERATOR beginKey, KEYITERATOR endKey) { - SymbolicFactor::shared_ptr result = boost::make_shared(); + SymbolicFactor::shared_ptr result = std::make_shared(); result->keys_.assign(beginKey, endKey); return result; } @@ -148,7 +147,7 @@ namespace gtsam { /** Eliminate the variables in \c keys, in the order specified in \c keys, returning a * conditional and marginal. */ - std::pair, boost::shared_ptr > + std::pair, std::shared_ptr > eliminate(const Ordering& keys) const; /// @} @@ -170,7 +169,7 @@ namespace gtsam { * one of the factor graph elimination functions (see EliminateableFactorGraph). The factor * graph elimination functions do sparse variable elimination, and use this function to eliminate * single variables or variable cliques. */ - GTSAM_EXPORT std::pair, boost::shared_ptr > + GTSAM_EXPORT std::pair, std::shared_ptr > EliminateSymbolic(const SymbolicFactorGraph& factors, const Ordering& keys); /// traits diff --git a/gtsam/symbolic/SymbolicFactorGraph.cpp b/gtsam/symbolic/SymbolicFactorGraph.cpp index 2824a799c..ddcb3b2f3 100644 --- a/gtsam/symbolic/SymbolicFactorGraph.cpp +++ b/gtsam/symbolic/SymbolicFactorGraph.cpp @@ -15,7 +15,6 @@ * @author Frank Dellaert */ -#include #include #include @@ -41,22 +40,22 @@ namespace gtsam { /* ************************************************************************* */ void SymbolicFactorGraph::push_factor(Key key) { - push_back(boost::make_shared(key)); + push_back(std::make_shared(key)); } /* ************************************************************************* */ void SymbolicFactorGraph::push_factor(Key key1, Key key2) { - push_back(boost::make_shared(key1,key2)); + push_back(std::make_shared(key1,key2)); } /* ************************************************************************* */ void SymbolicFactorGraph::push_factor(Key key1, Key key2, Key key3) { - push_back(boost::make_shared(key1,key2,key3)); + push_back(std::make_shared(key1,key2,key3)); } /* ************************************************************************* */ void SymbolicFactorGraph::push_factor(Key key1, Key key2, Key key3, Key key4) { - push_back(boost::make_shared(key1,key2,key3,key4)); + push_back(std::make_shared(key1,key2,key3,key4)); } /* ************************************************************************* */ diff --git a/gtsam/symbolic/SymbolicFactorGraph.h b/gtsam/symbolic/SymbolicFactorGraph.h index 219ba7bf5..bb37b4436 100644 --- a/gtsam/symbolic/SymbolicFactorGraph.h +++ b/gtsam/symbolic/SymbolicFactorGraph.h @@ -43,7 +43,7 @@ namespace gtsam { typedef SymbolicBayesTree BayesTreeType; ///< Type of Bayes tree typedef SymbolicJunctionTree JunctionTreeType; ///< Type of Junction tree /// The default dense elimination function - static std::pair, boost::shared_ptr > + static std::pair, std::shared_ptr > DefaultEliminate(const FactorGraphType& factors, const Ordering& keys) { return EliminateSymbolic(factors, keys); } /// The default ordering generation function @@ -67,7 +67,7 @@ namespace gtsam { typedef SymbolicFactorGraph This; ///< Typedef to this class typedef FactorGraph Base; ///< Typedef to base factor graph type typedef EliminateableFactorGraph BaseEliminateable; ///< Typedef to base elimination class - typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class + typedef std::shared_ptr shared_ptr; ///< shared_ptr to this class /// @name Standard Constructors /// @{ @@ -92,12 +92,12 @@ namespace gtsam { * FactorGraph fg = {make_shared(), ...}; */ SymbolicFactorGraph( - std::initializer_list> sharedFactors) + std::initializer_list> sharedFactors) : Base(sharedFactors) {} /// Construct from a single factor SymbolicFactorGraph(SymbolicFactor&& c) { - push_back(boost::make_shared(c)); + push_back(std::make_shared(c)); } /** @@ -107,7 +107,7 @@ namespace gtsam { * SymbolicFactorGraph(SymbolicFactor(...))(SymbolicFactor(...)); */ SymbolicFactorGraph& operator()(SymbolicFactor&& c) { - push_back(boost::make_shared(c)); + push_back(std::make_shared(c)); return *this; } diff --git a/gtsam/symbolic/SymbolicISAM.h b/gtsam/symbolic/SymbolicISAM.h index 874acd582..1742b6f81 100644 --- a/gtsam/symbolic/SymbolicISAM.h +++ b/gtsam/symbolic/SymbolicISAM.h @@ -28,7 +28,7 @@ namespace gtsam { public: typedef ISAM Base; typedef SymbolicISAM This; - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; /// @name Standard Constructors /// @{ diff --git a/gtsam/symbolic/SymbolicJunctionTree.h b/gtsam/symbolic/SymbolicJunctionTree.h index f1168f962..a6ca49e62 100644 --- a/gtsam/symbolic/SymbolicJunctionTree.h +++ b/gtsam/symbolic/SymbolicJunctionTree.h @@ -52,7 +52,7 @@ namespace gtsam { public: typedef JunctionTree Base; ///< Base class typedef SymbolicJunctionTree This; ///< This class - typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class + typedef std::shared_ptr shared_ptr; ///< Shared pointer to this class /** * Build the elimination tree of a factor graph using pre-computed column structure. diff --git a/gtsam/symbolic/tests/symbolicExampleGraphs.h b/gtsam/symbolic/tests/symbolicExampleGraphs.h index 716f502a4..3d30534e9 100644 --- a/gtsam/symbolic/tests/symbolicExampleGraphs.h +++ b/gtsam/symbolic/tests/symbolicExampleGraphs.h @@ -48,32 +48,32 @@ namespace gtsam { }; const SymbolicFactorGraph simpleTestGraph1 { - boost::make_shared(0,1), - boost::make_shared(0,2), - boost::make_shared(1,4), - boost::make_shared(2,4), - boost::make_shared(3,4)}; + std::make_shared(0,1), + std::make_shared(0,2), + std::make_shared(1,4), + std::make_shared(2,4), + std::make_shared(3,4)}; const SymbolicBayesNet simpleTestGraph1BayesNet { - boost::make_shared(0,1,2), - boost::make_shared(1,2,4), - boost::make_shared(2,4), - boost::make_shared(3,4), - boost::make_shared(4)}; + std::make_shared(0,1,2), + std::make_shared(1,2,4), + std::make_shared(2,4), + std::make_shared(3,4), + std::make_shared(4)}; const SymbolicFactorGraph simpleTestGraph2 { - boost::make_shared(0,1), - boost::make_shared(0,2), - boost::make_shared(1,3), - boost::make_shared(1,4), - boost::make_shared(2,3), - boost::make_shared(4,5)}; + std::make_shared(0,1), + std::make_shared(0,2), + std::make_shared(1,3), + std::make_shared(1,4), + std::make_shared(2,3), + std::make_shared(4,5)}; /** 1 - 0 - 2 - 3 */ const SymbolicFactorGraph simpleChain { - boost::make_shared(1,0), - boost::make_shared(0,2), - boost::make_shared(2,3)}; + std::make_shared(1,0), + std::make_shared(0,2), + std::make_shared(2,3)}; /* ************************************************************************* * * 2 3 @@ -81,11 +81,11 @@ namespace gtsam { ****************************************************************************/ SymbolicBayesTree __simpleChainBayesTree() { SymbolicBayesTree result; - result.insertRoot(boost::make_shared( - boost::make_shared( + result.insertRoot(std::make_shared( + std::make_shared( SymbolicConditional::FromKeys(KeyVector{2,3}, 2)))); - result.addClique(boost::make_shared( - boost::make_shared( + result.addClique(std::make_shared( + std::make_shared( SymbolicConditional::FromKeys(KeyVector{0,1,2}, 2))), result.roots().front()); return result; @@ -104,21 +104,21 @@ namespace gtsam { // Factor graph for Asia example const SymbolicFactorGraph asiaGraph = { - boost::make_shared(_T_), - boost::make_shared(_S_), - boost::make_shared(_T_, _E_, _L_), - boost::make_shared(_L_, _S_), - boost::make_shared(_S_, _B_), - boost::make_shared(_E_, _B_), - boost::make_shared(_E_, _X_)}; + std::make_shared(_T_), + std::make_shared(_S_), + std::make_shared(_T_, _E_, _L_), + std::make_shared(_L_, _S_), + std::make_shared(_S_, _B_), + std::make_shared(_E_, _B_), + std::make_shared(_E_, _X_)}; const SymbolicBayesNet asiaBayesNet = { - boost::make_shared(_T_, _E_, _L_), - boost::make_shared(_X_, _E_), - boost::make_shared(_E_, _B_, _L_), - boost::make_shared(_S_, _B_, _L_), - boost::make_shared(_L_, _B_), - boost::make_shared(_B_)}; + std::make_shared(_T_, _E_, _L_), + std::make_shared(_X_, _E_), + std::make_shared(_E_, _B_, _L_), + std::make_shared(_S_, _B_, _L_), + std::make_shared(_L_, _B_), + std::make_shared(_B_)}; /* ************************************************************************* */ // Allow creating Cliques and Keys in `list_of` chaining style: @@ -128,8 +128,8 @@ namespace gtsam { inline sharedClique LeafClique(const Keys::Result& keys, DenseIndex nrFrontals) { - return boost::make_shared( - boost::make_shared( + return std::make_shared( + std::make_shared( SymbolicConditional::FromKeys(keys, nrFrontals))); } diff --git a/gtsam/symbolic/tests/testSymbolicBayesNet.cpp b/gtsam/symbolic/tests/testSymbolicBayesNet.cpp index 7795d5b89..64caf399d 100644 --- a/gtsam/symbolic/tests/testSymbolicBayesNet.cpp +++ b/gtsam/symbolic/tests/testSymbolicBayesNet.cpp @@ -24,7 +24,6 @@ #include -#include using namespace std; using namespace gtsam; diff --git a/gtsam/symbolic/tests/testSymbolicBayesTree.cpp b/gtsam/symbolic/tests/testSymbolicBayesTree.cpp index f1cc6ee4f..d14523719 100644 --- a/gtsam/symbolic/tests/testSymbolicBayesTree.cpp +++ b/gtsam/symbolic/tests/testSymbolicBayesTree.cpp @@ -235,7 +235,7 @@ TEST(BayesTree, removeTop) { SymbolicBayesTree bayesTree = asiaBayesTree; // create a new factor to be inserted - // boost::shared_ptr newFactor(new IndexFactor(_S_,_B_)); + // std::shared_ptr newFactor(new IndexFactor(_S_,_B_)); // Remove the contaminated part of the Bayes tree SymbolicBayesNet bn; @@ -253,7 +253,7 @@ TEST(BayesTree, removeTop) { orphans | indirected)); // Try removeTop again with a factor that should not change a thing - // boost::shared_ptr newFactor2(new IndexFactor(_B_)); + // std::shared_ptr newFactor2(new IndexFactor(_B_)); SymbolicBayesNet bn2; SymbolicBayesTree::Cliques orphans2; bayesTree.removeTop(Keys(_B_), &bn2, &orphans2); diff --git a/gtsam/symbolic/tests/testSymbolicConditional.cpp b/gtsam/symbolic/tests/testSymbolicConditional.cpp index d8c13d092..e888fd3cd 100644 --- a/gtsam/symbolic/tests/testSymbolicConditional.cpp +++ b/gtsam/symbolic/tests/testSymbolicConditional.cpp @@ -15,7 +15,6 @@ * @author Frank Dellaert */ -#include #include #include @@ -75,7 +74,7 @@ TEST( SymbolicConditional, fourParents ) /* ************************************************************************* */ TEST( SymbolicConditional, FromRange ) { - auto c0 = boost::make_shared( + auto c0 = std::make_shared( SymbolicConditional::FromKeys(KeyVector{1, 2, 3, 4, 5}, 2)); LONGS_EQUAL(2, (long)c0->nrFrontals()); LONGS_EQUAL(3, (long)c0->nrParents()); diff --git a/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp b/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp index cb370f553..c93eb8593 100644 --- a/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp +++ b/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp @@ -21,7 +21,6 @@ #include #include -#include #include #include "symbolicExampleGraphs.h" diff --git a/gtsam/symbolic/tests/testSymbolicFactor.cpp b/gtsam/symbolic/tests/testSymbolicFactor.cpp index a8001232a..646b80ad6 100644 --- a/gtsam/symbolic/tests/testSymbolicFactor.cpp +++ b/gtsam/symbolic/tests/testSymbolicFactor.cpp @@ -21,7 +21,6 @@ #include #include -#include #include using namespace std; @@ -68,9 +67,9 @@ TEST(SymbolicFactor, Constructors) TEST(SymbolicFactor, EliminateSymbolic) { const SymbolicFactorGraph factors = { - boost::make_shared(2, 4, 6), - boost::make_shared(1, 2, 5), - boost::make_shared(0, 3)}; + std::make_shared(2, 4, 6), + std::make_shared(1, 2, 5), + std::make_shared(0, 3)}; const SymbolicFactor expectedFactor(4,5,6); const SymbolicConditional expectedConditional = diff --git a/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp b/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp index 8d5885d87..6c8924be4 100644 --- a/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp +++ b/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp @@ -97,10 +97,10 @@ TEST(SymbolicFactorGraph, eliminateFullMultifrontal) { TEST(SymbolicFactorGraph, eliminatePartialMultifrontal) { SymbolicBayesTree expectedBayesTree; SymbolicConditional::shared_ptr root = - boost::make_shared( + std::make_shared( SymbolicConditional::FromKeys(KeyVector{4, 5, 1}, 2)); expectedBayesTree.insertRoot( - boost::make_shared(root)); + std::make_shared(root)); const auto expectedFactorGraph = SymbolicFactorGraph(SymbolicFactor(0, 1))(SymbolicFactor(0, 2))( @@ -116,10 +116,10 @@ TEST(SymbolicFactorGraph, eliminatePartialMultifrontal) { SymbolicBayesTree expectedBayesTree2; SymbolicBayesTreeClique::shared_ptr root2 = - boost::make_shared( - boost::make_shared(4, 1)); - root2->children.push_back(boost::make_shared( - boost::make_shared(5, 4))); + std::make_shared( + std::make_shared(4, 1)); + root2->children.push_back(std::make_shared( + std::make_shared(5, 4))); expectedBayesTree2.insertRoot(root2); SymbolicBayesTree::shared_ptr actualBayesTree2; @@ -152,11 +152,11 @@ TEST(SymbolicFactorGraph, eliminate_disconnected_graph) { // create expected Chordal bayes Net SymbolicBayesNet expected; - expected.push_back(boost::make_shared(0, 1, 2)); - expected.push_back(boost::make_shared(1, 2)); - expected.push_back(boost::make_shared(2)); - expected.push_back(boost::make_shared(3, 4)); - expected.push_back(boost::make_shared(4)); + expected.push_back(std::make_shared(0, 1, 2)); + expected.push_back(std::make_shared(1, 2)); + expected.push_back(std::make_shared(2)); + expected.push_back(std::make_shared(3, 4)); + expected.push_back(std::make_shared(4)); Ordering order; order += 0, 1, 2, 3, 4; diff --git a/gtsam_unstable/base/BTree.h b/gtsam_unstable/base/BTree.h index edabd2528..bb72c1d95 100644 --- a/gtsam_unstable/base/BTree.h +++ b/gtsam_unstable/base/BTree.h @@ -21,7 +21,7 @@ #include #include -#include +#include #include namespace gtsam { @@ -75,7 +75,7 @@ namespace gtsam { // We store a shared pointer to the root of the functional tree // composed of Node classes. If root_==nullptr, the tree is empty. - typedef boost::shared_ptr sharedNode; + typedef std::shared_ptr sharedNode; sharedNode root_; inline const value_type& keyValue() const { return root_->keyValue_;} diff --git a/gtsam_unstable/base/tests/testBTree.cpp b/gtsam_unstable/base/tests/testBTree.cpp index b74a5e5ea..d54e1067b 100644 --- a/gtsam_unstable/base/tests/testBTree.cpp +++ b/gtsam_unstable/base/tests/testBTree.cpp @@ -16,8 +16,6 @@ * @author Frank Dellaert */ -#include - #include #include diff --git a/gtsam_unstable/discrete/AllDiff.cpp b/gtsam_unstable/discrete/AllDiff.cpp index cf598b3f4..2bd9e6dfd 100644 --- a/gtsam_unstable/discrete/AllDiff.cpp +++ b/gtsam_unstable/discrete/AllDiff.cpp @@ -9,7 +9,6 @@ #include #include -#include #include namespace gtsam { @@ -96,7 +95,7 @@ Constraint::shared_ptr AllDiff::partiallyApply(const DiscreteValues& values) con if (values.find(k) == values.end()) { newKeys.push_back(DiscreteKey(k, cardinalities_.at(k))); } - return boost::make_shared(newKeys); + return std::make_shared(newKeys); } /* ************************************************************************* */ diff --git a/gtsam_unstable/discrete/CSP.cpp b/gtsam_unstable/discrete/CSP.cpp index 08143c469..351bb5af1 100644 --- a/gtsam_unstable/discrete/CSP.cpp +++ b/gtsam_unstable/discrete/CSP.cpp @@ -30,7 +30,7 @@ bool CSP::runArcConsistency(const VariableIndex& index, // Otherwise, loop over all factors/constraints for variable with given key. for (size_t f : factors) { // If this factor is a constraint, call its ensureArcConsistency method: - auto constraint = boost::dynamic_pointer_cast((*this)[f]); + auto constraint = std::dynamic_pointer_cast((*this)[f]); if (constraint) { changed = constraint->ensureArcConsistency(key, domains) || changed; } @@ -75,7 +75,7 @@ CSP CSP::partiallyApply(const Domains& domains) const { // Reduce all existing factors: for (const DiscreteFactor::shared_ptr& f : factors_) { - auto constraint = boost::dynamic_pointer_cast(f); + auto constraint = std::dynamic_pointer_cast(f); if (!constraint) throw runtime_error("CSP:runArcConsistency: non-constraint factor"); Constraint::shared_ptr reduced = constraint->partiallyApply(domains); diff --git a/gtsam_unstable/discrete/Constraint.h b/gtsam_unstable/discrete/Constraint.h index 691c659fc..0622c833c 100644 --- a/gtsam_unstable/discrete/Constraint.h +++ b/gtsam_unstable/discrete/Constraint.h @@ -35,7 +35,7 @@ using Domains = std::map; */ class GTSAM_UNSTABLE_EXPORT Constraint : public DiscreteFactor { public: - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; protected: /// Construct unary constraint factor. diff --git a/gtsam_unstable/discrete/Domain.cpp b/gtsam_unstable/discrete/Domain.cpp index f2b7d9f95..bbbc87667 100644 --- a/gtsam_unstable/discrete/Domain.cpp +++ b/gtsam_unstable/discrete/Domain.cpp @@ -9,7 +9,6 @@ #include #include -#include #include namespace gtsam { @@ -82,7 +81,7 @@ Constraint::shared_ptr Domain::partiallyApply(const DiscreteValues& values) cons DiscreteValues::const_iterator it = values.find(key()); if (it != values.end() && !contains(it->second)) throw runtime_error("Domain::partiallyApply: unsatisfiable"); - return boost::make_shared(*this); + return std::make_shared(*this); } /* ************************************************************************* */ @@ -90,7 +89,7 @@ Constraint::shared_ptr Domain::partiallyApply(const Domains& domains) const { const Domain& Dk = domains.at(key()); if (Dk.isSingleton() && !contains(*Dk.begin())) throw runtime_error("Domain::partiallyApply: unsatisfiable"); - return boost::make_shared(Dk); + return std::make_shared(Dk); } /* ************************************************************************* */ diff --git a/gtsam_unstable/discrete/Domain.h b/gtsam_unstable/discrete/Domain.h index 5d96f0dc9..0f5b5fdf9 100644 --- a/gtsam_unstable/discrete/Domain.h +++ b/gtsam_unstable/discrete/Domain.h @@ -22,7 +22,7 @@ class GTSAM_UNSTABLE_EXPORT Domain : public Constraint { std::set values_; /// allowed values public: - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; // Constructor on Discrete Key initializes an "all-allowed" domain Domain(const DiscreteKey& dkey) diff --git a/gtsam_unstable/discrete/SingleValue.cpp b/gtsam_unstable/discrete/SingleValue.cpp index fc5fab83f..6b78f38f5 100644 --- a/gtsam_unstable/discrete/SingleValue.cpp +++ b/gtsam_unstable/discrete/SingleValue.cpp @@ -10,7 +10,6 @@ #include #include -#include namespace gtsam { @@ -60,7 +59,7 @@ Constraint::shared_ptr SingleValue::partiallyApply(const DiscreteValues& values) DiscreteValues::const_iterator it = values.find(keys_[0]); if (it != values.end() && it->second != value_) throw runtime_error("SingleValue::partiallyApply: unsatisfiable"); - return boost::make_shared(keys_[0], cardinality_, value_); + return std::make_shared(keys_[0], cardinality_, value_); } /* ************************************************************************* */ @@ -69,7 +68,7 @@ Constraint::shared_ptr SingleValue::partiallyApply( const Domain& Dk = domains.at(keys_[0]); if (Dk.isSingleton() && !Dk.contains(value_)) throw runtime_error("SingleValue::partiallyApply: unsatisfiable"); - return boost::make_shared(discreteKey(), value_); + return std::make_shared(discreteKey(), value_); } /* ************************************************************************* */ diff --git a/gtsam_unstable/discrete/SingleValue.h b/gtsam_unstable/discrete/SingleValue.h index 3b2d6e80b..1c726d4d0 100644 --- a/gtsam_unstable/discrete/SingleValue.h +++ b/gtsam_unstable/discrete/SingleValue.h @@ -25,7 +25,7 @@ class GTSAM_UNSTABLE_EXPORT SingleValue : public Constraint { } public: - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; /// Construct from key, cardinality, and given value. SingleValue(Key key, size_t n, size_t value) diff --git a/gtsam_unstable/discrete/tests/testLoopyBelief.cpp b/gtsam_unstable/discrete/tests/testLoopyBelief.cpp index 0e27b5fbb..63df7d7c4 100644 --- a/gtsam_unstable/discrete/tests/testLoopyBelief.cpp +++ b/gtsam_unstable/discrete/tests/testLoopyBelief.cpp @@ -112,14 +112,14 @@ class LoopyBelief { // Incorporate new the factor to belief if (!beliefAtKey) beliefAtKey = - boost::dynamic_pointer_cast(message); + std::dynamic_pointer_cast(message); else - beliefAtKey = boost::make_shared( + beliefAtKey = std::make_shared( (*beliefAtKey) * - (*boost::dynamic_pointer_cast(message))); + (*std::dynamic_pointer_cast(message))); } if (starGraphs_.at(key).unary) - beliefAtKey = boost::make_shared( + beliefAtKey = std::make_shared( (*beliefAtKey) * (*starGraphs_.at(key).unary)); if (debug) beliefAtKey->print("New belief at key: "); // normalize belief @@ -135,7 +135,7 @@ class LoopyBelief { DecisionTreeFactor sumFactor(allDiscreteKeys.at(key), sumFactorTable); if (debug) sumFactor.print("denomFactor: "); beliefAtKey = - boost::make_shared((*beliefAtKey) / sumFactor); + std::make_shared((*beliefAtKey) / sumFactor); if (debug) beliefAtKey->print("New belief at key normalized: "); beliefs->push_back(beliefAtKey); allMessages[key] = messages; @@ -148,16 +148,16 @@ class LoopyBelief { for (Key neighbor : starGraphs_.at(key).correctedBeliefIndices | boost::adaptors::map_keys) { DecisionTreeFactor correctedBelief = - (*boost::dynamic_pointer_cast( + (*std::dynamic_pointer_cast( beliefs->at(beliefFactors[key].front()))) / - (*boost::dynamic_pointer_cast( + (*std::dynamic_pointer_cast( messages.at(neighbor))); if (debug) correctedBelief.print("correctedBelief: "); size_t beliefIndex = starGraphs_.at(neighbor).correctedBeliefIndices.at(key); starGraphs_.at(neighbor).star->replace( beliefIndex, - boost::make_shared(correctedBelief)); + std::make_shared(correctedBelief)); } } @@ -187,12 +187,12 @@ class LoopyBelief { // accumulate unary factors if (graph.at(factorIndex)->size() == 1) { if (!prodOfUnaries) - prodOfUnaries = boost::dynamic_pointer_cast( + prodOfUnaries = std::dynamic_pointer_cast( graph.at(factorIndex)); else - prodOfUnaries = boost::make_shared( + prodOfUnaries = std::make_shared( *prodOfUnaries * - (*boost::dynamic_pointer_cast( + (*std::dynamic_pointer_cast( graph.at(factorIndex)))); } } diff --git a/gtsam_unstable/dynamics/FullIMUFactor.h b/gtsam_unstable/dynamics/FullIMUFactor.h index 9ba7f67fa..325321d13 100644 --- a/gtsam_unstable/dynamics/FullIMUFactor.h +++ b/gtsam_unstable/dynamics/FullIMUFactor.h @@ -57,7 +57,7 @@ public: /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } /** Check if two factors are equal */ diff --git a/gtsam_unstable/dynamics/IMUFactor.h b/gtsam_unstable/dynamics/IMUFactor.h index fe3b7dc51..102c9311e 100644 --- a/gtsam_unstable/dynamics/IMUFactor.h +++ b/gtsam_unstable/dynamics/IMUFactor.h @@ -50,7 +50,7 @@ public: /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } /** Check if two factors are equal */ diff --git a/gtsam_unstable/dynamics/Pendulum.h b/gtsam_unstable/dynamics/Pendulum.h index f520de7b7..3fd09f639 100644 --- a/gtsam_unstable/dynamics/Pendulum.h +++ b/gtsam_unstable/dynamics/Pendulum.h @@ -36,7 +36,7 @@ public: // Provide access to the Matrix& version of evaluateError: using Base::evaluateError; - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; ///Constructor. k1: q_{k+1}, k: q_k, velKey: velocity variable depending on the chosen method, h: time step PendulumFactor1(Key k1, Key k, Key velKey, double h, double mu = 1000.0) @@ -44,7 +44,7 @@ public: /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new PendulumFactor1(*this))); } /** q_k + h*v - q_k1 = 0, with optional derivatives */ @@ -86,7 +86,7 @@ public: // Provide access to the Matrix& version of evaluateError: using Base::evaluateError; - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; ///Constructor. vk1: v_{k+1}, vk: v_k, qkey: q's key depending on the chosen method, h: time step PendulumFactor2(Key vk1, Key vk, Key qkey, double h, double r = 1.0, double g = 9.81, double mu = 1000.0) @@ -94,7 +94,7 @@ public: /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new PendulumFactor2(*this))); } /** v_k - h*g/L*sin(q) - v_k1 = 0, with optional derivatives */ @@ -137,7 +137,7 @@ public: // Provide access to the Matrix& version of evaluateError: using Base::evaluateError; - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; ///Constructor PendulumFactorPk(Key pKey, Key qKey, Key qKey1, @@ -147,7 +147,7 @@ public: /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new PendulumFactorPk(*this))); } /** 1/h mr^2 (qk1-qk)+mgrh (1-a) sin((1-a)pk + a*pk1) - pk = 0, with optional derivatives */ @@ -195,7 +195,7 @@ public: // Provide access to the Matrix& version of evaluateError: using Base::evaluateError; - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; ///Constructor PendulumFactorPk1(Key pKey1, Key qKey, Key qKey1, @@ -205,7 +205,7 @@ public: /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new PendulumFactorPk1(*this))); } /** 1/h mr^2 (qk1-qk) - mgrh a sin((1-a)pk + a*pk1) - pk1 = 0, with optional derivatives */ diff --git a/gtsam_unstable/dynamics/SimpleHelicopter.h b/gtsam_unstable/dynamics/SimpleHelicopter.h index 9a536fba0..753c70fac 100644 --- a/gtsam_unstable/dynamics/SimpleHelicopter.h +++ b/gtsam_unstable/dynamics/SimpleHelicopter.h @@ -41,7 +41,7 @@ public: /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new Reconstruction(*this))); } /** \f$ log((g_k\exp(h\xi_k))^{-1}g_{k+1}) = 0 \f$, with optional derivatives */ @@ -106,7 +106,7 @@ public: /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new DiscreteEulerPoincareHelicopter(*this))); } /** DEP, with optional derivatives diff --git a/gtsam_unstable/dynamics/VelocityConstraint.h b/gtsam_unstable/dynamics/VelocityConstraint.h index 1c3dbcde7..bd6a40008 100644 --- a/gtsam_unstable/dynamics/VelocityConstraint.h +++ b/gtsam_unstable/dynamics/VelocityConstraint.h @@ -79,7 +79,7 @@ public: /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new VelocityConstraint(*this))); } /** diff --git a/gtsam_unstable/dynamics/VelocityConstraint3.h b/gtsam_unstable/dynamics/VelocityConstraint3.h index db546c86c..ab06b32a4 100644 --- a/gtsam_unstable/dynamics/VelocityConstraint3.h +++ b/gtsam_unstable/dynamics/VelocityConstraint3.h @@ -26,7 +26,7 @@ public: // Provide access to the Matrix& version of evaluateError: using Base::evaluateError; - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; ///TODO: comment VelocityConstraint3(Key key1, Key key2, Key velKey, double dt, double mu = 1000.0) @@ -35,7 +35,7 @@ public: /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new VelocityConstraint3(*this))); } /** x1 + v*dt - x2 = 0, with optional derivatives */ diff --git a/gtsam_unstable/examples/FixedLagSmootherExample.cpp b/gtsam_unstable/examples/FixedLagSmootherExample.cpp index 16ede58e0..d0cd918f8 100644 --- a/gtsam_unstable/examples/FixedLagSmootherExample.cpp +++ b/gtsam_unstable/examples/FixedLagSmootherExample.cpp @@ -152,7 +152,7 @@ int main(int argc, char** argv) { auto &factorGraph = smootherISAM2.getFactors(); // Linearize to a Gaussian factor graph - boost::shared_ptr linearGraph = factorGraph.linearize(result); + std::shared_ptr linearGraph = factorGraph.linearize(result); // Converts the linear graph into a Jacobian factor and extracts the Jacobian matrix Matrix jacobian = linearGraph->jacobian().first; diff --git a/gtsam_unstable/examples/ISAM2_SmartFactorStereo_IMU.cpp b/gtsam_unstable/examples/ISAM2_SmartFactorStereo_IMU.cpp index 16eda23d4..e386add76 100644 --- a/gtsam_unstable/examples/ISAM2_SmartFactorStereo_IMU.cpp +++ b/gtsam_unstable/examples/ISAM2_SmartFactorStereo_IMU.cpp @@ -45,7 +45,7 @@ struct IMUHelper { } // expect IMU to be rotated in image space co-ords - auto p = boost::make_shared( + auto p = std::make_shared( Vector3(0.0, 9.8, 0.0)); p->accelerometerCovariance = diff --git a/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp b/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp index 2fccf6b18..37becfef1 100644 --- a/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp +++ b/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp @@ -144,7 +144,7 @@ int main(int argc, char** argv) { // initialize smart range factors size_t ids[] = { 1, 6, 0, 5 }; - typedef boost::shared_ptr SmartPtr; + typedef std::shared_ptr SmartPtr; map smartFactors; if (smart) { for(size_t jj: ids) { diff --git a/gtsam_unstable/geometry/InvDepthCamera3.h b/gtsam_unstable/geometry/InvDepthCamera3.h index 8b2797938..7f02984bf 100644 --- a/gtsam_unstable/geometry/InvDepthCamera3.h +++ b/gtsam_unstable/geometry/InvDepthCamera3.h @@ -31,7 +31,7 @@ template class InvDepthCamera3 { private: Pose3 pose_; ///< The camera pose - boost::shared_ptr k_; ///< The fixed camera calibration + std::shared_ptr k_; ///< The fixed camera calibration public: @@ -42,7 +42,7 @@ public: InvDepthCamera3() {} /** constructor with pose and calibration */ - InvDepthCamera3(const Pose3& pose, const boost::shared_ptr& k) : + InvDepthCamera3(const Pose3& pose, const std::shared_ptr& k) : pose_(pose),k_(k) {} /// @} @@ -55,7 +55,7 @@ public: inline Pose3& pose() { return pose_; } /// return calibration - inline const boost::shared_ptr& calibration() const { return k_; } + inline const std::shared_ptr& calibration() const { return k_; } /// print void print(const std::string& s = "") const { diff --git a/gtsam_unstable/linear/ActiveSetSolver-inl.h b/gtsam_unstable/linear/ActiveSetSolver-inl.h index 350985cf4..cba639e5d 100644 --- a/gtsam_unstable/linear/ActiveSetSolver-inl.h +++ b/gtsam_unstable/linear/ActiveSetSolver-inl.h @@ -149,7 +149,7 @@ Template JacobianFactor::shared_ptr This::createDualFactor( if (Aterms.size() > 0) { Vector b = problem_.costGradient(key, delta); // to compute the least-square approximation of dual variables - return boost::make_shared(Aterms, b); + return std::make_shared(Aterms, b); } else { return nullptr; } diff --git a/gtsam_unstable/linear/EqualityFactorGraph.h b/gtsam_unstable/linear/EqualityFactorGraph.h index fb3f4c076..1078190c3 100644 --- a/gtsam_unstable/linear/EqualityFactorGraph.h +++ b/gtsam_unstable/linear/EqualityFactorGraph.h @@ -29,7 +29,7 @@ namespace gtsam { */ class EqualityFactorGraph: public FactorGraph { public: - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; /// Add a linear inequality, forwards arguments to LinearInequality. template void add(Args &&... args) { diff --git a/gtsam_unstable/linear/InequalityFactorGraph.h b/gtsam_unstable/linear/InequalityFactorGraph.h index 7016a7e97..0960cce32 100644 --- a/gtsam_unstable/linear/InequalityFactorGraph.h +++ b/gtsam_unstable/linear/InequalityFactorGraph.h @@ -34,7 +34,7 @@ private: typedef FactorGraph Base; public: - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; /** print */ void print( diff --git a/gtsam_unstable/linear/LP.h b/gtsam_unstable/linear/LP.h index fc00c2240..dbb744d3e 100644 --- a/gtsam_unstable/linear/LP.h +++ b/gtsam_unstable/linear/LP.h @@ -49,7 +49,7 @@ KeyDimMap collectKeyDim(const LinearGraph& linearGraph) { * Data structure of a Linear Program */ struct LP { - using shared_ptr = boost::shared_ptr; + using shared_ptr = std::shared_ptr; LinearCost cost; //!< Linear cost factor EqualityFactorGraph equalities; //!< Linear equality constraints: cE(x) = 0 diff --git a/gtsam_unstable/linear/LinearCost.h b/gtsam_unstable/linear/LinearCost.h index 9ac7e16af..c5447e645 100644 --- a/gtsam_unstable/linear/LinearCost.h +++ b/gtsam_unstable/linear/LinearCost.h @@ -32,7 +32,7 @@ class LinearCost: public JacobianFactor { public: typedef LinearCost This; ///< Typedef to this class typedef JacobianFactor Base; ///< Typedef to base class - typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class + typedef std::shared_ptr shared_ptr; ///< shared_ptr to this class public: /** default constructor for I/O */ @@ -100,8 +100,8 @@ public: /** Clone this LinearCost */ GaussianFactor::shared_ptr clone() const override { - return boost::static_pointer_cast < GaussianFactor - > (boost::make_shared < LinearCost > (*this)); + return std::static_pointer_cast < GaussianFactor + > (std::make_shared < LinearCost > (*this)); } /** Special error_vector for constraints (A*x-b) */ diff --git a/gtsam_unstable/linear/LinearEquality.h b/gtsam_unstable/linear/LinearEquality.h index d2c8586f2..5cf60686a 100644 --- a/gtsam_unstable/linear/LinearEquality.h +++ b/gtsam_unstable/linear/LinearEquality.h @@ -30,7 +30,7 @@ class LinearEquality: public JacobianFactor { public: typedef LinearEquality This; ///< Typedef to this class typedef JacobianFactor Base; ///< Typedef to base class - typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class + typedef std::shared_ptr shared_ptr; ///< shared_ptr to this class private: Key dualKey_; @@ -101,8 +101,8 @@ public: /** Clone this LinearEquality */ GaussianFactor::shared_ptr clone() const override { - return boost::static_pointer_cast < GaussianFactor - > (boost::make_shared < LinearEquality > (*this)); + return std::static_pointer_cast < GaussianFactor + > (std::make_shared < LinearEquality > (*this)); } /// dual key diff --git a/gtsam_unstable/linear/LinearInequality.h b/gtsam_unstable/linear/LinearInequality.h index dbb9004ea..3059189ed 100644 --- a/gtsam_unstable/linear/LinearInequality.h +++ b/gtsam_unstable/linear/LinearInequality.h @@ -34,7 +34,7 @@ class LinearInequality: public JacobianFactor { public: typedef LinearInequality This; ///< Typedef to this class typedef JacobianFactor Base; ///< Typedef to base class - typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class + typedef std::shared_ptr shared_ptr; ///< shared_ptr to this class private: Key dualKey_; @@ -115,8 +115,8 @@ public: /** Clone this LinearInequality */ GaussianFactor::shared_ptr clone() const override { - return boost::static_pointer_cast < GaussianFactor - > (boost::make_shared < LinearInequality > (*this)); + return std::static_pointer_cast < GaussianFactor + > (std::make_shared < LinearInequality > (*this)); } /// dual key diff --git a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h index 79bd22f9d..e1206d942 100644 --- a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h +++ b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h @@ -31,7 +31,7 @@ class GTSAM_UNSTABLE_EXPORT BatchFixedLagSmoother : public FixedLagSmoother { public: /// Typedef for a shared pointer to an Incremental Fixed-Lag Smoother - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; /** default constructor */ BatchFixedLagSmoother(double smootherLag = 0.0, const LevenbergMarquardtParams& parameters = LevenbergMarquardtParams(), bool enforceConsistency = true) : diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp index 0ec1a7426..36cb6165f 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp @@ -28,7 +28,7 @@ void ConcurrentBatchFilter::PrintNonlinearFactor(const NonlinearFactor::shared_p const std::string& indent, const KeyFormatter& keyFormatter) { std::cout << indent; if(factor) { - if(boost::dynamic_pointer_cast(factor)) { + if(std::dynamic_pointer_cast(factor)) { std::cout << "l( "; } else { std::cout << "f( "; @@ -65,8 +65,8 @@ void ConcurrentBatchFilter::PrintLinearFactor(const GaussianFactor::shared_ptr& const std::string& indent, const KeyFormatter& keyFormatter) { std::cout << indent; if(factor) { - JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast(factor); - HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast(factor); + JacobianFactor::shared_ptr jf = std::dynamic_pointer_cast(factor); + HessianFactor::shared_ptr hf = std::dynamic_pointer_cast(factor); if(jf) { std::cout << "j( "; } else if(hf) { diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h index b88d5c739..b9e0f59ff 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h +++ b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h @@ -31,7 +31,7 @@ namespace gtsam { class GTSAM_UNSTABLE_EXPORT ConcurrentBatchFilter : public ConcurrentFilter { public: - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; typedef ConcurrentFilter Base; ///< typedef for base class /** Meta information returned about the update */ diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp index d6b1e81df..fbc5cc67f 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp @@ -385,7 +385,7 @@ void ConcurrentBatchSmoother::updateSmootherSummarization() { void ConcurrentBatchSmoother::PrintNonlinearFactor(const NonlinearFactor::shared_ptr& factor, const std::string& indent, const KeyFormatter& keyFormatter) { std::cout << indent; if(factor) { - if(boost::dynamic_pointer_cast(factor)) { + if(std::dynamic_pointer_cast(factor)) { std::cout << "l( "; } else { std::cout << "f( "; diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h index 38b39594b..2afe78854 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h +++ b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h @@ -31,7 +31,7 @@ namespace gtsam { class GTSAM_UNSTABLE_EXPORT ConcurrentBatchSmoother : public ConcurrentSmoother { public: - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; typedef ConcurrentSmoother Base; ///< typedef for base class /** Meta information returned about the update */ diff --git a/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.cpp b/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.cpp index 66d03bfa2..9dc5b3fad 100644 --- a/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.cpp @@ -78,7 +78,7 @@ NonlinearFactorGraph calculateMarginalFactors(const NonlinearFactorGraph& graph, NonlinearFactorGraph marginalFactors; marginalFactors.reserve(marginalLinearFactors.size()); for(const GaussianFactor::shared_ptr& gaussianFactor: marginalLinearFactors) { - marginalFactors += boost::make_shared(gaussianFactor, theta); + marginalFactors += std::make_shared(gaussianFactor, theta); } return marginalFactors; diff --git a/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h b/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h index c87b99275..80c10aa66 100644 --- a/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h +++ b/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h @@ -38,7 +38,7 @@ void GTSAM_UNSTABLE_EXPORT synchronize(ConcurrentFilter& filter, ConcurrentSmoot */ class GTSAM_UNSTABLE_EXPORT ConcurrentFilter { public: - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; /** Default constructor */ ConcurrentFilter() {}; @@ -100,7 +100,7 @@ public: */ class GTSAM_UNSTABLE_EXPORT ConcurrentSmoother { public: - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; /** Default constructor */ ConcurrentSmoother() {}; diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.h b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.h index 8e1409523..ae7f09096 100644 --- a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.h +++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.h @@ -31,7 +31,7 @@ class GTSAM_UNSTABLE_EXPORT ConcurrentIncrementalFilter : public virtual Concurr public: - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; typedef ConcurrentFilter Base; ///< typedef for base class /** Meta information returned about the update */ diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.h b/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.h index 92d8c462a..334197fc6 100644 --- a/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.h +++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.h @@ -30,7 +30,7 @@ namespace gtsam { class GTSAM_UNSTABLE_EXPORT ConcurrentIncrementalSmoother : public virtual ConcurrentSmoother { public: - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; typedef ConcurrentSmoother Base; ///< typedef for base class /** Meta information returned about the update */ diff --git a/gtsam_unstable/nonlinear/FixedLagSmoother.h b/gtsam_unstable/nonlinear/FixedLagSmoother.h index 17fcf3908..10e13271d 100644 --- a/gtsam_unstable/nonlinear/FixedLagSmoother.h +++ b/gtsam_unstable/nonlinear/FixedLagSmoother.h @@ -35,7 +35,7 @@ class GTSAM_UNSTABLE_EXPORT FixedLagSmoother { public: /// Typedef for a shared pointer to an Incremental Fixed-Lag Smoother - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; /// Typedef for a Key-Timestamp map/database typedef std::map KeyTimestampMap; diff --git a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h index b2910b139..beef95c1b 100644 --- a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h +++ b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h @@ -35,7 +35,7 @@ class GTSAM_UNSTABLE_EXPORT IncrementalFixedLagSmoother: public FixedLagSmoother public: /// Typedef for a shared pointer to an Incremental Fixed-Lag Smoother - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; /** default constructor */ IncrementalFixedLagSmoother(double smootherLag = 0.0, diff --git a/gtsam_unstable/nonlinear/LinearizedFactor.cpp b/gtsam_unstable/nonlinear/LinearizedFactor.cpp index 0c821b872..a3f2132f8 100644 --- a/gtsam_unstable/nonlinear/LinearizedFactor.cpp +++ b/gtsam_unstable/nonlinear/LinearizedFactor.cpp @@ -100,7 +100,7 @@ double LinearizedJacobianFactor::error(const Values& c) const { } /* ************************************************************************* */ -boost::shared_ptr +std::shared_ptr LinearizedJacobianFactor::linearize(const Values& c) const { // Create the 'terms' data structure for the Jacobian constructor @@ -112,7 +112,7 @@ LinearizedJacobianFactor::linearize(const Values& c) const { // compute rhs Vector b = -error_vector(c); - return boost::shared_ptr(new JacobianFactor(terms, b, noiseModel::Unit::Create(dim()))); + return std::shared_ptr(new JacobianFactor(terms, b, noiseModel::Unit::Create(dim()))); } /* ************************************************************************* */ @@ -199,7 +199,7 @@ double LinearizedHessianFactor::error(const Values& c) const { } /* ************************************************************************* */ -boost::shared_ptr +std::shared_ptr LinearizedHessianFactor::linearize(const Values& c) const { // Construct an error vector in key-order from the Values @@ -239,8 +239,8 @@ LinearizedHessianFactor::linearize(const Values& c) const { } // Create a Hessian Factor from the modified info matrix - //return boost::shared_ptr(new HessianFactor(js, newInfo)); - return boost::shared_ptr(new HessianFactor(keys(), Gs, gs, f)); + //return std::shared_ptr(new HessianFactor(js, newInfo)); + return std::shared_ptr(new HessianFactor(keys(), Gs, gs, f)); } } // \namespace aspn diff --git a/gtsam_unstable/nonlinear/LinearizedFactor.h b/gtsam_unstable/nonlinear/LinearizedFactor.h index 151bf8a25..5bf791089 100644 --- a/gtsam_unstable/nonlinear/LinearizedFactor.h +++ b/gtsam_unstable/nonlinear/LinearizedFactor.h @@ -35,7 +35,7 @@ public: typedef LinearizedGaussianFactor This; /** shared pointer for convenience */ - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; protected: @@ -82,7 +82,7 @@ public: typedef LinearizedJacobianFactor This; /** shared pointer for convenience */ - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; typedef VerticalBlockMatrix::Block ABlock; typedef VerticalBlockMatrix::constBlock constABlock; @@ -113,7 +113,7 @@ public: /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } // Testable @@ -140,7 +140,7 @@ public: * Reimplemented from NoiseModelFactor to directly copy out the * matrices and only update the RHS b with an updated residual */ - boost::shared_ptr linearize(const Values& c) const override; + std::shared_ptr linearize(const Values& c) const override; /** (A*x-b) */ Vector error_vector(const Values& c) const; @@ -173,7 +173,7 @@ public: typedef LinearizedHessianFactor This; /** shared pointer for convenience */ - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; /** hessian block data types */ typedef SymmetricBlockMatrix::Block Block; ///< A block from the Hessian matrix @@ -203,7 +203,7 @@ public: /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } // Testable @@ -271,7 +271,7 @@ public: * Reimplemented from NoiseModelFactor to directly copy out the * matrices and only update the RHS b with an updated residual */ - boost::shared_ptr linearize(const Values& c) const override; + std::shared_ptr linearize(const Values& c) const override; private: /** Serialization function */ diff --git a/gtsam_unstable/nonlinear/NonlinearClusterTree.h b/gtsam_unstable/nonlinear/NonlinearClusterTree.h index 5d089f123..d308514ae 100644 --- a/gtsam_unstable/nonlinear/NonlinearClusterTree.h +++ b/gtsam_unstable/nonlinear/NonlinearClusterTree.h @@ -36,8 +36,8 @@ class NonlinearClusterTree : public ClusterTree { return factors.linearize(values); } - static NonlinearCluster* DownCast(const boost::shared_ptr& cluster) { - auto nonlinearCluster = boost::dynamic_pointer_cast(cluster); + static NonlinearCluster* DownCast(const std::shared_ptr& cluster) { + auto nonlinearCluster = std::dynamic_pointer_cast(cluster); if (!nonlinearCluster) throw std::runtime_error("Expected NonlinearCluster"); return nonlinearCluster.get(); diff --git a/gtsam_unstable/nonlinear/tests/testCustomChartExpression.cpp b/gtsam_unstable/nonlinear/tests/testCustomChartExpression.cpp index 937108a67..a7e266cb5 100644 --- a/gtsam_unstable/nonlinear/tests/testCustomChartExpression.cpp +++ b/gtsam_unstable/nonlinear/tests/testCustomChartExpression.cpp @@ -80,17 +80,17 @@ TEST(ExpressionCustomChart, projection) { ExpressionFactor f(noiseModel::Unit::Create(pval.size()), pval, point); - boost::shared_ptr gfstandard = f.linearize(standard); - boost::shared_ptr jfstandard = // - boost::dynamic_pointer_cast(gfstandard); + std::shared_ptr gfstandard = f.linearize(standard); + std::shared_ptr jfstandard = // + std::dynamic_pointer_cast(gfstandard); typedef std::pair Jacobian; Jacobian Jstandard = jfstandard->jacobianUnweighted(); EXPECT(assert_equal(Eigen::Matrix3d::Identity(), Jstandard.first, 1e-10)); - boost::shared_ptr gfcustom = f.linearize(custom); - boost::shared_ptr jfcustom = // - boost::dynamic_pointer_cast(gfcustom); + std::shared_ptr gfcustom = f.linearize(custom); + std::shared_ptr jfcustom = // + std::dynamic_pointer_cast(gfcustom); Eigen::MatrixXd expectedJacobian = Eigen::MatrixXd::Zero(3,2); expectedJacobian(0,0) = 2.0; diff --git a/gtsam_unstable/nonlinear/tests/testLinearizedFactor.cpp b/gtsam_unstable/nonlinear/tests/testLinearizedFactor.cpp index 21e271bd8..5c9bc20e3 100644 --- a/gtsam_unstable/nonlinear/tests/testLinearizedFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testLinearizedFactor.cpp @@ -43,7 +43,7 @@ TEST( LinearizedFactor, equals_jacobian ) // Create two identical factors and make sure they're equal - JacobianFactor::shared_ptr jf = boost::static_pointer_cast(betweenFactor.linearize(values)); + JacobianFactor::shared_ptr jf = std::static_pointer_cast(betweenFactor.linearize(values)); LinearizedJacobianFactor jacobian1(jf, values); LinearizedJacobianFactor jacobian2(jf, values); @@ -65,13 +65,13 @@ TEST( LinearizedFactor, clone_jacobian ) BetweenFactor betweenFactor(x1, x2, measured, model); // Create one factor that is a clone of the other and make sure they're equal - JacobianFactor::shared_ptr jf = boost::static_pointer_cast(betweenFactor.linearize(values)); + JacobianFactor::shared_ptr jf = std::static_pointer_cast(betweenFactor.linearize(values)); LinearizedJacobianFactor jacobian1(jf, values); - LinearizedJacobianFactor::shared_ptr jacobian2 = boost::static_pointer_cast(jacobian1.clone()); + LinearizedJacobianFactor::shared_ptr jacobian2 = std::static_pointer_cast(jacobian1.clone()); CHECK(assert_equal(jacobian1, *jacobian2)); - JacobianFactor::shared_ptr jf1 = boost::static_pointer_cast(jacobian1.linearize(values)); - JacobianFactor::shared_ptr jf2 = boost::static_pointer_cast(jacobian2->linearize(values)); + JacobianFactor::shared_ptr jf1 = std::static_pointer_cast(jacobian1.linearize(values)); + JacobianFactor::shared_ptr jf2 = std::static_pointer_cast(jacobian2->linearize(values)); CHECK(assert_equal(*jf1, *jf2)); Matrix information1 = jf1->augmentedInformation(); @@ -94,7 +94,7 @@ TEST( LinearizedFactor, add_jacobian ) BetweenFactor betweenFactor(x1, x2, measured, model); // Create two factor graphs, one using 'push_back' and one using 'add' and make sure they're equal - JacobianFactor::shared_ptr jf = boost::static_pointer_cast(betweenFactor.linearize(values)); + JacobianFactor::shared_ptr jf = std::static_pointer_cast(betweenFactor.linearize(values)); LinearizedJacobianFactor::shared_ptr jacobian(new LinearizedJacobianFactor(jf, values)); NonlinearFactorGraph graph1; graph1.push_back(jacobian); NonlinearFactorGraph graph2; graph2.push_back(*jacobian); @@ -124,7 +124,7 @@ TEST( LinearizedFactor, add_jacobian ) // // // // Create a linearized jacobian factors -// JacobianFactor::shared_ptr jf = boost::static_pointer_cast(betweenFactor.linearize(values)); +// JacobianFactor::shared_ptr jf = std::static_pointer_cast(betweenFactor.linearize(values)); // LinearizedJacobianFactor jacobian(jf, values); // // @@ -176,7 +176,7 @@ TEST( LinearizedFactor, equals_hessian ) // Create two identical factors and make sure they're equal - JacobianFactor::shared_ptr jf = boost::static_pointer_cast(betweenFactor.linearize(values)); + JacobianFactor::shared_ptr jf = std::static_pointer_cast(betweenFactor.linearize(values)); HessianFactor::shared_ptr hf(new HessianFactor(*jf)); LinearizedHessianFactor hessian1(hf, values); LinearizedHessianFactor hessian2(hf, values); @@ -200,10 +200,10 @@ TEST( LinearizedFactor, clone_hessian ) // Create two identical factors and make sure they're equal - JacobianFactor::shared_ptr jf = boost::static_pointer_cast(betweenFactor.linearize(values)); + JacobianFactor::shared_ptr jf = std::static_pointer_cast(betweenFactor.linearize(values)); HessianFactor::shared_ptr hf(new HessianFactor(*jf)); LinearizedHessianFactor hessian1(hf, values); - LinearizedHessianFactor::shared_ptr hessian2 = boost::static_pointer_cast(hessian1.clone()); + LinearizedHessianFactor::shared_ptr hessian2 = std::static_pointer_cast(hessian1.clone()); CHECK(assert_equal(hessian1, *hessian2)); } @@ -224,7 +224,7 @@ TEST( LinearizedFactor, add_hessian ) // Create two identical factors and make sure they're equal - JacobianFactor::shared_ptr jf = boost::static_pointer_cast(betweenFactor.linearize(values)); + JacobianFactor::shared_ptr jf = std::static_pointer_cast(betweenFactor.linearize(values)); HessianFactor::shared_ptr hf(new HessianFactor(*jf)); LinearizedHessianFactor::shared_ptr hessian(new LinearizedHessianFactor(hf, values)); NonlinearFactorGraph graph1; graph1.push_back(hessian); @@ -252,7 +252,7 @@ TEST( LinearizedFactor, add_hessian ) // // // // Create a linearized hessian factor -// JacobianFactor::shared_ptr jf = boost::static_pointer_cast(betweenFactor.linearize(values)); +// JacobianFactor::shared_ptr jf = std::static_pointer_cast(betweenFactor.linearize(values)); // HessianFactor::shared_ptr hf(new HessianFactor(*jf)); // LinearizedHessianFactor hessian(hf, values); // diff --git a/gtsam_unstable/nonlinear/tests/testNonlinearClusterTree.cpp b/gtsam_unstable/nonlinear/tests/testNonlinearClusterTree.cpp index e0c234b7b..9083ab172 100644 --- a/gtsam_unstable/nonlinear/tests/testNonlinearClusterTree.cpp +++ b/gtsam_unstable/nonlinear/tests/testNonlinearClusterTree.cpp @@ -67,9 +67,9 @@ TEST(NonlinearClusterTree, Clusters) { // NOTE(frank): Order matters here as factors are removed! VariableIndex variableIndex(graph); typedef NonlinearClusterTree::NonlinearCluster Cluster; - auto marginalCluster = boost::shared_ptr(new Cluster(variableIndex, {x1}, &graph)); - auto landmarkCluster = boost::shared_ptr(new Cluster(variableIndex, {l1, l2}, &graph)); - auto rootCluster = boost::shared_ptr(new Cluster(variableIndex, {x2, x3}, &graph)); + auto marginalCluster = std::shared_ptr(new Cluster(variableIndex, {x1}, &graph)); + auto landmarkCluster = std::shared_ptr(new Cluster(variableIndex, {l1, l2}, &graph)); + auto rootCluster = std::shared_ptr(new Cluster(variableIndex, {x2, x3}, &graph)); EXPECT_LONGS_EQUAL(3, marginalCluster->nrFactors()); EXPECT_LONGS_EQUAL(2, landmarkCluster->nrFactors()); @@ -87,7 +87,7 @@ TEST(NonlinearClusterTree, Clusters) { Ordering ordering; ordering.push_back(x1); auto expected = gfg->eliminatePartialSequential(ordering); - auto expectedFactor = boost::dynamic_pointer_cast(expected.second->at(0)); + auto expectedFactor = std::dynamic_pointer_cast(expected.second->at(0)); if (!expectedFactor) throw std::runtime_error("Expected HessianFactor"); @@ -106,9 +106,9 @@ static NonlinearClusterTree Construct() { NonlinearFactorGraph graph = planarSLAMGraph(); VariableIndex variableIndex(graph); typedef NonlinearClusterTree::NonlinearCluster Cluster; - auto marginalCluster = boost::shared_ptr(new Cluster(variableIndex, {x1}, &graph)); - auto landmarkCluster = boost::shared_ptr(new Cluster(variableIndex, {l1, l2}, &graph)); - auto rootCluster = boost::shared_ptr(new Cluster(variableIndex, {x2, x3}, &graph)); + auto marginalCluster = std::shared_ptr(new Cluster(variableIndex, {x1}, &graph)); + auto landmarkCluster = std::shared_ptr(new Cluster(variableIndex, {l1, l2}, &graph)); + auto rootCluster = std::shared_ptr(new Cluster(variableIndex, {x2, x3}, &graph)); // Build the tree NonlinearClusterTree clusterTree; diff --git a/gtsam_unstable/nonlinear/tests/testParticleFactor.cpp b/gtsam_unstable/nonlinear/tests/testParticleFactor.cpp index 1cf94b161..e4c6f3ca2 100644 --- a/gtsam_unstable/nonlinear/tests/testParticleFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testParticleFactor.cpp @@ -17,7 +17,6 @@ */ #include -#include namespace gtsam { @@ -29,7 +28,7 @@ class ParticleFactor { public: typedef ParticleFactor This; ///< Typedef to this class - typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class + typedef std::shared_ptr shared_ptr; ///< shared_ptr to this class }; @@ -54,7 +53,7 @@ public: * @param P0 covariance at time 0, given as a diagonal Gaussian 'model' */ State init(const Vector& x0, const SharedDiagonal& P0) { - return boost::make_shared >(); + return std::make_shared >(); } }; diff --git a/gtsam_unstable/partition/GenericGraph.cpp b/gtsam_unstable/partition/GenericGraph.cpp index 92f0266d0..5f32ab557 100644 --- a/gtsam_unstable/partition/GenericGraph.cpp +++ b/gtsam_unstable/partition/GenericGraph.cpp @@ -6,7 +6,6 @@ * Description: generic graph types used in partitioning */ #include -#include #include #include @@ -396,7 +395,7 @@ namespace gtsam { namespace partition { hasOdometry = cameraToCamera[camera1] == camera2; if (nrCommonLandmarks > 0 || hasOdometry) { nrTotalConstraints = 2 * nrCommonLandmarks + (hasOdometry ? 6 : 0); - reducedGraph.push_back(boost::make_shared(camera1, camera2, + reducedGraph.push_back(std::make_shared(camera1, camera2, factorIndex++, NODE_POSE_3D, NODE_POSE_3D, nrTotalConstraints)); } } diff --git a/gtsam_unstable/partition/GenericGraph.h b/gtsam_unstable/partition/GenericGraph.h index 3dc640e97..bcfd77336 100644 --- a/gtsam_unstable/partition/GenericGraph.h +++ b/gtsam_unstable/partition/GenericGraph.h @@ -13,7 +13,7 @@ #include #include #include -#include +#include #include "PartitionWorkSpace.h" @@ -45,7 +45,7 @@ namespace gtsam { namespace partition { }; /** graph is a collection of factors */ - typedef boost::shared_ptr sharedGenericFactor2D; + typedef std::shared_ptr sharedGenericFactor2D; typedef std::vector GenericGraph2D; /** merge nodes in DSF using constraints captured by the given graph */ @@ -93,7 +93,7 @@ namespace gtsam { namespace partition { }; /** graph is a collection of factors */ - typedef boost::shared_ptr sharedGenericFactor3D; + typedef std::shared_ptr sharedGenericFactor3D; typedef std::vector GenericGraph3D; /** merge nodes in DSF using constraints captured by the given graph */ @@ -126,7 +126,7 @@ namespace gtsam { namespace partition { }; /** graph is a collection of factors */ - typedef boost::shared_ptr sharedGenericUnaryFactor; + typedef std::shared_ptr sharedGenericUnaryFactor; typedef std::vector GenericUnaryGraph; /*************************************************** diff --git a/gtsam_unstable/partition/NestedDissection-inl.h b/gtsam_unstable/partition/NestedDissection-inl.h index 28076bda0..c377998dd 100644 --- a/gtsam_unstable/partition/NestedDissection-inl.h +++ b/gtsam_unstable/partition/NestedDissection-inl.h @@ -8,7 +8,6 @@ #pragma once -#include #include "partition/FindSeparator-inl.h" #include "OrderedSymbols.h" @@ -38,13 +37,13 @@ namespace gtsam { namespace partition { keys.push_back(i); WorkSpace workspace(numNodes); - root_ = recursivePartition(gfg, unaryFactors, keys, vector(), numNodeStopPartition, minNodesPerMap, boost::shared_ptr(), workspace, verbose); + root_ = recursivePartition(gfg, unaryFactors, keys, vector(), numNodeStopPartition, minNodesPerMap, std::shared_ptr(), workspace, verbose); } /* ************************************************************************* */ template NestedDissection::NestedDissection( - const NLG& fg, const Ordering& ordering, const boost::shared_ptr& cuts, const bool verbose) : fg_(fg), ordering_(ordering){ + const NLG& fg, const Ordering& ordering, const std::shared_ptr& cuts, const bool verbose) : fg_(fg), ordering_(ordering){ GenericUnaryGraph unaryFactors; GenericGraph gfg; boost::tie(unaryFactors, gfg) = fg.createGenericGraph(ordering); @@ -62,13 +61,13 @@ namespace gtsam { namespace partition { keys.push_back(i); WorkSpace workspace(numNodes); - root_ = recursivePartition(gfg, unaryFactors, keys, vector(), cuts, boost::shared_ptr(), workspace, verbose); + root_ = recursivePartition(gfg, unaryFactors, keys, vector(), cuts, std::shared_ptr(), workspace, verbose); } /* ************************************************************************* */ template - boost::shared_ptr NestedDissection::makeSubNLG( - const NLG& fg, const vector& frontals, const vector& sep, const boost::shared_ptr& parent) const { + std::shared_ptr NestedDissection::makeSubNLG( + const NLG& fg, const vector& frontals, const vector& sep, const std::shared_ptr& parent) const { OrderedSymbols frontalKeys; for(const size_t index: frontals) frontalKeys.push_back(int2symbol_[index]); @@ -77,7 +76,7 @@ namespace gtsam { namespace partition { for(const size_t index: sep) sepKeys.insert(int2symbol_[index]); - return boost::make_shared(fg, frontalKeys, sepKeys, parent); + return std::make_shared(fg, frontalKeys, sepKeys, parent); } /* ************************************************************************* */ @@ -171,9 +170,9 @@ namespace gtsam { namespace partition { /* ************************************************************************* */ template - boost::shared_ptr NestedDissection::recursivePartition( + std::shared_ptr NestedDissection::recursivePartition( const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors, const vector& frontals, const vector& sep, - const int numNodeStopPartition, const int minNodesPerMap, const boost::shared_ptr& parent, WorkSpace& workspace, const bool verbose) const { + const int numNodeStopPartition, const int minNodesPerMap, const std::shared_ptr& parent, WorkSpace& workspace, const bool verbose) const { // if no split needed NLG sepFactors; // factors that should remain in the current cluster @@ -195,7 +194,7 @@ namespace gtsam { namespace partition { frontalFactors, frontalUnaryFactors, sepFactors, childFrontals, childSeps, localFrontals, weeklinks); // make a new cluster - boost::shared_ptr current = makeSubNLG(sepFactors, localFrontals, sep, parent); + std::shared_ptr current = makeSubNLG(sepFactors, localFrontals, sep, parent); current->setWeeklinks(weeklinks); // check whether all the submaps are fully constrained @@ -205,7 +204,7 @@ namespace gtsam { namespace partition { // create child clusters for (int i=0; i child = recursivePartition(frontalFactors[i], frontalUnaryFactors[i], childFrontals[i], childSeps[i], + std::shared_ptr child = recursivePartition(frontalFactors[i], frontalUnaryFactors[i], childFrontals[i], childSeps[i], numNodeStopPartition, minNodesPerMap, current, workspace, verbose); current->addChild(child); } @@ -215,9 +214,9 @@ namespace gtsam { namespace partition { /* ************************************************************************* */ template - boost::shared_ptr NestedDissection::recursivePartition( + std::shared_ptr NestedDissection::recursivePartition( const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors, const vector& frontals, const vector& sep, - const boost::shared_ptr& cuts, const boost::shared_ptr& parent, WorkSpace& workspace, const bool verbose) const { + const std::shared_ptr& cuts, const std::shared_ptr& parent, WorkSpace& workspace, const bool verbose) const { // if there is no need to cut any more NLG sepFactors; // factors that should remain in the current cluster @@ -237,13 +236,13 @@ namespace gtsam { namespace partition { frontalFactors, frontalUnaryFactors, sepFactors, childFrontals, childSeps, localFrontals, weeklinks); // make a new cluster - boost::shared_ptr current = makeSubNLG(sepFactors, localFrontals, sep, parent); + std::shared_ptr current = makeSubNLG(sepFactors, localFrontals, sep, parent); current->setWeeklinks(weeklinks); // create child clusters for (int i=0; i<2; i++) { - boost::shared_ptr child = recursivePartition(frontalFactors[i], frontalUnaryFactors[i], childFrontals[i], childSeps[i], - cuts->children.empty() ? boost::shared_ptr() : cuts->children[i], current, workspace, verbose); + std::shared_ptr child = recursivePartition(frontalFactors[i], frontalUnaryFactors[i], childFrontals[i], childSeps[i], + cuts->children.empty() ? std::shared_ptr() : cuts->children[i], current, workspace, verbose); current->addChild(child); } return current; diff --git a/gtsam_unstable/partition/NestedDissection.h b/gtsam_unstable/partition/NestedDissection.h index 82304a79d..071f3d80a 100644 --- a/gtsam_unstable/partition/NestedDissection.h +++ b/gtsam_unstable/partition/NestedDissection.h @@ -9,7 +9,7 @@ #pragma once #include -#include +#include #include namespace gtsam { namespace partition { @@ -20,7 +20,7 @@ namespace gtsam { namespace partition { template class NestedDissection { public: - typedef boost::shared_ptr sharedSubNLG; + typedef std::shared_ptr sharedSubNLG; private: NLG fg_; // the original nonlinear factor graph @@ -36,11 +36,11 @@ namespace gtsam { namespace partition { NestedDissection(const NLG& fg, const Ordering& ordering, const int numNodeStopPartition, const int minNodesPerMap, const bool verbose = false); /* constructor with pre-determined cuts*/ - NestedDissection(const NLG& fg, const Ordering& ordering, const boost::shared_ptr& cuts, const bool verbose = false); + NestedDissection(const NLG& fg, const Ordering& ordering, const std::shared_ptr& cuts, const bool verbose = false); private: /* convert generic subgraph to nonlinear subgraph */ - sharedSubNLG makeSubNLG(const NLG& fg, const std::vector& frontals, const std::vector& sep, const boost::shared_ptr& parent) const; + sharedSubNLG makeSubNLG(const NLG& fg, const std::vector& frontals, const std::vector& sep, const std::shared_ptr& parent) const; void processFactor(const typename GenericGraph::value_type& factor, const std::vector& partitionTable, // input std::vector& frontalFactors, NLG& sepFactors, std::vector >& childSeps, // output factor graphs @@ -58,11 +58,11 @@ namespace gtsam { namespace partition { /* recursively partition the generic graph */ sharedSubNLG recursivePartition(const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors, const std::vector& frontals, const std::vector& sep, - const int numNodeStopPartition, const int minNodesPerMap, const boost::shared_ptr& parent, WorkSpace& workspace, const bool verbose) const; + const int numNodeStopPartition, const int minNodesPerMap, const std::shared_ptr& parent, WorkSpace& workspace, const bool verbose) const; /* recursively partition the generic graph */ sharedSubNLG recursivePartition(const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors, const std::vector& frontals, const std::vector& sep, - const boost::shared_ptr& cuts, const boost::shared_ptr& parent, WorkSpace& workspace, const bool verbose) const; + const std::shared_ptr& cuts, const std::shared_ptr& parent, WorkSpace& workspace, const bool verbose) const; }; diff --git a/gtsam_unstable/partition/PartitionWorkSpace.h b/gtsam_unstable/partition/PartitionWorkSpace.h index b0bd8113e..b4817304f 100644 --- a/gtsam_unstable/partition/PartitionWorkSpace.h +++ b/gtsam_unstable/partition/PartitionWorkSpace.h @@ -9,7 +9,7 @@ #pragma once #include -#include +#include namespace gtsam { namespace partition { @@ -18,7 +18,7 @@ namespace gtsam { namespace partition { // the work space, preallocated memory struct WorkSpace { std::vector dictionary; // a mapping from the integer key in the original graph to 0-based index in the subgraph, useful when handling a subset of keys and graphs - boost::shared_ptr > dsf; // a block memory pre-allocated for DSFVector + std::shared_ptr > dsf; // a block memory pre-allocated for DSFVector PartitionTable partitionTable; // a mapping from a key to the submap index, 0 means the separator, i means the ith submap // constructor @@ -38,7 +38,7 @@ namespace gtsam { namespace partition { // manually defined cuts struct Cuts { PartitionTable partitionTable; - std::vector > children; + std::vector > children; }; }} // namespace diff --git a/gtsam_unstable/partition/tests/testFindSeparator.cpp b/gtsam_unstable/partition/tests/testFindSeparator.cpp index b086be7ff..a659c3c22 100644 --- a/gtsam_unstable/partition/tests/testFindSeparator.cpp +++ b/gtsam_unstable/partition/tests/testFindSeparator.cpp @@ -6,7 +6,6 @@ * Description: unit tests for FindSeparator */ -#include #include #include @@ -22,10 +21,10 @@ using namespace gtsam::partition; TEST ( Partition, separatorPartitionByMetis ) { GenericGraph2D graph; - graph.push_back(boost::make_shared(0, NODE_POSE_2D, 3, NODE_LANDMARK_2D)); - graph.push_back(boost::make_shared(2, NODE_POSE_2D, 4, NODE_LANDMARK_2D)); - graph.push_back(boost::make_shared(0, NODE_POSE_2D, 1, NODE_POSE_2D)); - graph.push_back(boost::make_shared(1, NODE_POSE_2D, 2, NODE_POSE_2D)); + graph.push_back(std::make_shared(0, NODE_POSE_2D, 3, NODE_LANDMARK_2D)); + graph.push_back(std::make_shared(2, NODE_POSE_2D, 4, NODE_LANDMARK_2D)); + graph.push_back(std::make_shared(0, NODE_POSE_2D, 1, NODE_POSE_2D)); + graph.push_back(std::make_shared(1, NODE_POSE_2D, 2, NODE_POSE_2D)); std::vector keys{0, 1, 2, 3, 4}; WorkSpace workspace(5); @@ -47,10 +46,10 @@ TEST ( Partition, separatorPartitionByMetis ) TEST ( Partition, separatorPartitionByMetis2 ) { GenericGraph2D graph; - graph.push_back(boost::make_shared(1, NODE_POSE_2D, 5, NODE_LANDMARK_2D)); - graph.push_back(boost::make_shared(3, NODE_POSE_2D, 6, NODE_LANDMARK_2D)); - graph.push_back(boost::make_shared(1, NODE_POSE_2D, 2, NODE_POSE_2D)); - graph.push_back(boost::make_shared(2, NODE_POSE_2D, 3, NODE_POSE_2D)); + graph.push_back(std::make_shared(1, NODE_POSE_2D, 5, NODE_LANDMARK_2D)); + graph.push_back(std::make_shared(3, NODE_POSE_2D, 6, NODE_LANDMARK_2D)); + graph.push_back(std::make_shared(1, NODE_POSE_2D, 2, NODE_POSE_2D)); + graph.push_back(std::make_shared(2, NODE_POSE_2D, 3, NODE_POSE_2D)); std::vector keys{1, 2, 3, 5, 6}; WorkSpace workspace(8); @@ -71,9 +70,9 @@ TEST ( Partition, separatorPartitionByMetis2 ) TEST ( Partition, edgePartitionByMetis ) { GenericGraph3D graph; - graph.push_back(boost::make_shared(0, 1, 0, NODE_POSE_3D, NODE_POSE_3D)); - graph.push_back(boost::make_shared(1, 2, 1, NODE_POSE_3D, NODE_POSE_3D)); - graph.push_back(boost::make_shared(2, 3, 2, NODE_POSE_3D, NODE_POSE_3D)); + graph.push_back(std::make_shared(0, 1, 0, NODE_POSE_3D, NODE_POSE_3D)); + graph.push_back(std::make_shared(1, 2, 1, NODE_POSE_3D, NODE_POSE_3D)); + graph.push_back(std::make_shared(2, 3, 2, NODE_POSE_3D, NODE_POSE_3D)); std::vector keys{0, 1, 2, 3}; WorkSpace workspace(6); @@ -101,10 +100,10 @@ TEST ( Partition, edgePartitionByMetis ) TEST ( Partition, edgePartitionByMetis2 ) { GenericGraph3D graph; - graph.push_back(boost::make_shared(0, 1, 0, NODE_POSE_3D, NODE_POSE_3D, 1)); - graph.push_back(boost::make_shared(1, 2, 1, NODE_POSE_3D, NODE_POSE_3D, 1)); - graph.push_back(boost::make_shared(2, 3, 2, NODE_POSE_3D, NODE_POSE_3D, 20)); - graph.push_back(boost::make_shared(3, 4, 3, NODE_POSE_3D, NODE_POSE_3D, 1)); + graph.push_back(std::make_shared(0, 1, 0, NODE_POSE_3D, NODE_POSE_3D, 1)); + graph.push_back(std::make_shared(1, 2, 1, NODE_POSE_3D, NODE_POSE_3D, 1)); + graph.push_back(std::make_shared(2, 3, 2, NODE_POSE_3D, NODE_POSE_3D, 20)); + graph.push_back(std::make_shared(3, 4, 3, NODE_POSE_3D, NODE_POSE_3D, 1)); std::vector keys{0, 1, 2, 3, 4}; WorkSpace workspace(6); @@ -125,10 +124,10 @@ TEST ( Partition, edgePartitionByMetis2 ) TEST ( Partition, findSeparator ) { GenericGraph2D graph; - graph.push_back(boost::make_shared(0, NODE_POSE_2D, 3, NODE_LANDMARK_2D)); - graph.push_back(boost::make_shared(2, NODE_POSE_2D, 4, NODE_LANDMARK_2D)); - graph.push_back(boost::make_shared(0, NODE_POSE_2D, 1, NODE_POSE_2D)); - graph.push_back(boost::make_shared(1, NODE_POSE_2D, 2, NODE_POSE_2D)); + graph.push_back(std::make_shared(0, NODE_POSE_2D, 3, NODE_LANDMARK_2D)); + graph.push_back(std::make_shared(2, NODE_POSE_2D, 4, NODE_LANDMARK_2D)); + graph.push_back(std::make_shared(0, NODE_POSE_2D, 1, NODE_POSE_2D)); + graph.push_back(std::make_shared(1, NODE_POSE_2D, 2, NODE_POSE_2D)); std::vector keys{0, 1, 2, 3, 4}; WorkSpace workspace(5); @@ -151,10 +150,10 @@ TEST ( Partition, findSeparator ) TEST ( Partition, findSeparator2 ) { GenericGraph2D graph; - graph.push_back(boost::make_shared(1, NODE_POSE_2D, 5, NODE_LANDMARK_2D)); - graph.push_back(boost::make_shared(3, NODE_POSE_2D, 6, NODE_LANDMARK_2D)); - graph.push_back(boost::make_shared(1, NODE_POSE_2D, 2, NODE_POSE_2D)); - graph.push_back(boost::make_shared(2, NODE_POSE_2D, 3, NODE_POSE_2D)); + graph.push_back(std::make_shared(1, NODE_POSE_2D, 5, NODE_LANDMARK_2D)); + graph.push_back(std::make_shared(3, NODE_POSE_2D, 6, NODE_LANDMARK_2D)); + graph.push_back(std::make_shared(1, NODE_POSE_2D, 2, NODE_POSE_2D)); + graph.push_back(std::make_shared(2, NODE_POSE_2D, 3, NODE_POSE_2D)); std::vector keys{1, 2, 3, 5, 6}; WorkSpace workspace(8); @@ -184,13 +183,13 @@ TEST ( Partition, findSeparator3_with_reduced_camera ) { GenericGraph3D graph; for (int j=1; j<=8; j++) - graph.push_back(boost::make_shared(25, j)); + graph.push_back(std::make_shared(25, j)); for (int j=1; j<=16; j++) - graph.push_back(boost::make_shared(26, j)); + graph.push_back(std::make_shared(26, j)); for (int j=9; j<=24; j++) - graph.push_back(boost::make_shared(27, j)); + graph.push_back(std::make_shared(27, j)); for (int j=17; j<=24; j++) - graph.push_back(boost::make_shared(28, j)); + graph.push_back(std::make_shared(28, j)); std::vector keys; for(int i=1; i<=28; i++) diff --git a/gtsam_unstable/partition/tests/testGenericGraph.cpp b/gtsam_unstable/partition/tests/testGenericGraph.cpp index dc7d26035..6f326dda1 100644 --- a/gtsam_unstable/partition/tests/testGenericGraph.cpp +++ b/gtsam_unstable/partition/tests/testGenericGraph.cpp @@ -10,7 +10,6 @@ #include -#include #include @@ -27,19 +26,19 @@ using namespace gtsam::partition; TEST ( GenerciGraph, findIslands ) { GenericGraph2D graph; - graph.push_back(boost::make_shared(1, NODE_POSE_2D, 7, NODE_LANDMARK_2D)); - graph.push_back(boost::make_shared(2, NODE_POSE_2D, 7, NODE_LANDMARK_2D)); - graph.push_back(boost::make_shared(3, NODE_POSE_2D, 7, NODE_LANDMARK_2D)); - graph.push_back(boost::make_shared(3, NODE_POSE_2D, 8, NODE_LANDMARK_2D)); - graph.push_back(boost::make_shared(4, NODE_POSE_2D, 8, NODE_LANDMARK_2D)); - graph.push_back(boost::make_shared(4, NODE_POSE_2D, 9, NODE_LANDMARK_2D)); - graph.push_back(boost::make_shared(5, NODE_POSE_2D, 9, NODE_LANDMARK_2D)); - graph.push_back(boost::make_shared(6, NODE_POSE_2D, 9, NODE_LANDMARK_2D)); + graph.push_back(std::make_shared(1, NODE_POSE_2D, 7, NODE_LANDMARK_2D)); + graph.push_back(std::make_shared(2, NODE_POSE_2D, 7, NODE_LANDMARK_2D)); + graph.push_back(std::make_shared(3, NODE_POSE_2D, 7, NODE_LANDMARK_2D)); + graph.push_back(std::make_shared(3, NODE_POSE_2D, 8, NODE_LANDMARK_2D)); + graph.push_back(std::make_shared(4, NODE_POSE_2D, 8, NODE_LANDMARK_2D)); + graph.push_back(std::make_shared(4, NODE_POSE_2D, 9, NODE_LANDMARK_2D)); + graph.push_back(std::make_shared(5, NODE_POSE_2D, 9, NODE_LANDMARK_2D)); + graph.push_back(std::make_shared(6, NODE_POSE_2D, 9, NODE_LANDMARK_2D)); - graph.push_back(boost::make_shared(1, NODE_POSE_2D, 2, NODE_POSE_2D)); - graph.push_back(boost::make_shared(2, NODE_POSE_2D, 3, NODE_POSE_2D)); - graph.push_back(boost::make_shared(4, NODE_POSE_2D, 5, NODE_POSE_2D)); - graph.push_back(boost::make_shared(5, NODE_POSE_2D, 6, NODE_POSE_2D)); + graph.push_back(std::make_shared(1, NODE_POSE_2D, 2, NODE_POSE_2D)); + graph.push_back(std::make_shared(2, NODE_POSE_2D, 3, NODE_POSE_2D)); + graph.push_back(std::make_shared(4, NODE_POSE_2D, 5, NODE_POSE_2D)); + graph.push_back(std::make_shared(5, NODE_POSE_2D, 6, NODE_POSE_2D)); std::vector keys{1, 2, 3, 4, 5, 6, 7, 8, 9}; WorkSpace workspace(10); // from 0 to 9 @@ -60,19 +59,19 @@ TEST ( GenerciGraph, findIslands ) TEST( GenerciGraph, findIslands2 ) { GenericGraph2D graph; - graph.push_back(boost::make_shared(1, NODE_POSE_2D, 7, NODE_LANDMARK_2D)); - graph.push_back(boost::make_shared(2, NODE_POSE_2D, 7, NODE_LANDMARK_2D)); - graph.push_back(boost::make_shared(3, NODE_POSE_2D, 7, NODE_LANDMARK_2D)); - graph.push_back(boost::make_shared(3, NODE_POSE_2D, 8, NODE_LANDMARK_2D)); - graph.push_back(boost::make_shared(4, NODE_POSE_2D, 7, NODE_LANDMARK_2D)); - graph.push_back(boost::make_shared(4, NODE_POSE_2D, 8, NODE_LANDMARK_2D)); - graph.push_back(boost::make_shared(5, NODE_POSE_2D, 8, NODE_LANDMARK_2D)); - graph.push_back(boost::make_shared(6, NODE_POSE_2D, 8, NODE_LANDMARK_2D)); + graph.push_back(std::make_shared(1, NODE_POSE_2D, 7, NODE_LANDMARK_2D)); + graph.push_back(std::make_shared(2, NODE_POSE_2D, 7, NODE_LANDMARK_2D)); + graph.push_back(std::make_shared(3, NODE_POSE_2D, 7, NODE_LANDMARK_2D)); + graph.push_back(std::make_shared(3, NODE_POSE_2D, 8, NODE_LANDMARK_2D)); + graph.push_back(std::make_shared(4, NODE_POSE_2D, 7, NODE_LANDMARK_2D)); + graph.push_back(std::make_shared(4, NODE_POSE_2D, 8, NODE_LANDMARK_2D)); + graph.push_back(std::make_shared(5, NODE_POSE_2D, 8, NODE_LANDMARK_2D)); + graph.push_back(std::make_shared(6, NODE_POSE_2D, 8, NODE_LANDMARK_2D)); - graph.push_back(boost::make_shared(1, NODE_POSE_2D, 2, NODE_POSE_2D)); - graph.push_back(boost::make_shared(2, NODE_POSE_2D, 3, NODE_POSE_2D)); - graph.push_back(boost::make_shared(4, NODE_POSE_2D, 5, NODE_POSE_2D)); - graph.push_back(boost::make_shared(5, NODE_POSE_2D, 6, NODE_POSE_2D)); + graph.push_back(std::make_shared(1, NODE_POSE_2D, 2, NODE_POSE_2D)); + graph.push_back(std::make_shared(2, NODE_POSE_2D, 3, NODE_POSE_2D)); + graph.push_back(std::make_shared(4, NODE_POSE_2D, 5, NODE_POSE_2D)); + graph.push_back(std::make_shared(5, NODE_POSE_2D, 6, NODE_POSE_2D)); std::vector keys{1, 2, 3, 4, 5, 6, 7, 8}; WorkSpace workspace(15); // from 0 to 8, but testing over-allocation here @@ -90,11 +89,11 @@ TEST( GenerciGraph, findIslands2 ) TEST ( GenerciGraph, findIslands3 ) { GenericGraph2D graph; - graph.push_back(boost::make_shared(1, NODE_POSE_2D, 5, NODE_LANDMARK_2D)); - graph.push_back(boost::make_shared(4, NODE_POSE_2D, 6, NODE_LANDMARK_2D)); + graph.push_back(std::make_shared(1, NODE_POSE_2D, 5, NODE_LANDMARK_2D)); + graph.push_back(std::make_shared(4, NODE_POSE_2D, 6, NODE_LANDMARK_2D)); - graph.push_back(boost::make_shared(2, NODE_POSE_2D, 3, NODE_POSE_2D)); - graph.push_back(boost::make_shared(3, NODE_POSE_2D, 4, NODE_POSE_2D)); + graph.push_back(std::make_shared(2, NODE_POSE_2D, 3, NODE_POSE_2D)); + graph.push_back(std::make_shared(3, NODE_POSE_2D, 4, NODE_POSE_2D)); std::vector keys{1, 2, 3, 4, 5, 6}; WorkSpace workspace(7); // from 0 to 9 @@ -113,8 +112,8 @@ TEST ( GenerciGraph, findIslands3 ) TEST ( GenerciGraph, findIslands4 ) { GenericGraph2D graph; - graph.push_back(boost::make_shared(3, NODE_POSE_2D, 4, NODE_LANDMARK_2D)); - graph.push_back(boost::make_shared(7, NODE_POSE_2D, 7, NODE_LANDMARK_2D)); + graph.push_back(std::make_shared(3, NODE_POSE_2D, 4, NODE_LANDMARK_2D)); + graph.push_back(std::make_shared(7, NODE_POSE_2D, 7, NODE_LANDMARK_2D)); std::vector keys{3, 4, 7}; WorkSpace workspace(8); // from 0 to 7 @@ -135,13 +134,13 @@ TEST ( GenerciGraph, findIslands4 ) TEST ( GenerciGraph, findIslands5 ) { GenericGraph2D graph; - graph.push_back(boost::make_shared(1, NODE_POSE_2D, 5, NODE_LANDMARK_2D)); - graph.push_back(boost::make_shared(2, NODE_POSE_2D, 5, NODE_LANDMARK_2D)); - graph.push_back(boost::make_shared(3, NODE_POSE_2D, 5, NODE_LANDMARK_2D)); - graph.push_back(boost::make_shared(4, NODE_POSE_2D, 5, NODE_LANDMARK_2D)); + graph.push_back(std::make_shared(1, NODE_POSE_2D, 5, NODE_LANDMARK_2D)); + graph.push_back(std::make_shared(2, NODE_POSE_2D, 5, NODE_LANDMARK_2D)); + graph.push_back(std::make_shared(3, NODE_POSE_2D, 5, NODE_LANDMARK_2D)); + graph.push_back(std::make_shared(4, NODE_POSE_2D, 5, NODE_LANDMARK_2D)); - graph.push_back(boost::make_shared(1, NODE_POSE_2D, 3, NODE_POSE_2D)); - graph.push_back(boost::make_shared(2, NODE_POSE_2D, 4, NODE_POSE_2D)); + graph.push_back(std::make_shared(1, NODE_POSE_2D, 3, NODE_POSE_2D)); + graph.push_back(std::make_shared(2, NODE_POSE_2D, 4, NODE_POSE_2D)); std::vector keys{1, 2, 3, 4, 5}; @@ -163,11 +162,11 @@ TEST ( GenerciGraph, findIslands5 ) TEST ( GenerciGraph, reduceGenericGraph ) { GenericGraph3D graph; - graph.push_back(boost::make_shared(1, 3)); - graph.push_back(boost::make_shared(1, 4)); - graph.push_back(boost::make_shared(1, 5)); - graph.push_back(boost::make_shared(2, 5)); - graph.push_back(boost::make_shared(2, 6)); + graph.push_back(std::make_shared(1, 3)); + graph.push_back(std::make_shared(1, 4)); + graph.push_back(std::make_shared(1, 5)); + graph.push_back(std::make_shared(2, 5)); + graph.push_back(std::make_shared(2, 6)); std::vector cameraKeys, landmarkKeys; cameraKeys.push_back(1); @@ -198,12 +197,12 @@ TEST ( GenerciGraph, reduceGenericGraph ) TEST ( GenericGraph, reduceGenericGraph2 ) { GenericGraph3D graph; - graph.push_back(boost::make_shared(1, 3, 0, NODE_POSE_3D, NODE_LANDMARK_3D)); - graph.push_back(boost::make_shared(1, 4, 1, NODE_POSE_3D, NODE_LANDMARK_3D)); - graph.push_back(boost::make_shared(1, 5, 2, NODE_POSE_3D, NODE_LANDMARK_3D)); - graph.push_back(boost::make_shared(2, 5, 3, NODE_POSE_3D, NODE_LANDMARK_3D)); - graph.push_back(boost::make_shared(2, 6, 4, NODE_POSE_3D, NODE_LANDMARK_3D)); - graph.push_back(boost::make_shared(2, 7, 5, NODE_POSE_3D, NODE_POSE_3D)); + graph.push_back(std::make_shared(1, 3, 0, NODE_POSE_3D, NODE_LANDMARK_3D)); + graph.push_back(std::make_shared(1, 4, 1, NODE_POSE_3D, NODE_LANDMARK_3D)); + graph.push_back(std::make_shared(1, 5, 2, NODE_POSE_3D, NODE_LANDMARK_3D)); + graph.push_back(std::make_shared(2, 5, 3, NODE_POSE_3D, NODE_LANDMARK_3D)); + graph.push_back(std::make_shared(2, 6, 4, NODE_POSE_3D, NODE_LANDMARK_3D)); + graph.push_back(std::make_shared(2, 7, 5, NODE_POSE_3D, NODE_POSE_3D)); std::vector cameraKeys, landmarkKeys; cameraKeys.push_back(1); diff --git a/gtsam_unstable/partition/tests/testNestedDissection.cpp b/gtsam_unstable/partition/tests/testNestedDissection.cpp index 36ce647c3..da896e33d 100644 --- a/gtsam_unstable/partition/tests/testNestedDissection.cpp +++ b/gtsam_unstable/partition/tests/testNestedDissection.cpp @@ -6,7 +6,6 @@ * Description: unit tests for NestedDissection */ -#include #include #include "SubmapPlanarSLAM.h" @@ -210,17 +209,17 @@ TEST ( NestedDissection, manual_cuts ) Ordering ordering; ordering += x0, x1, x2, l1, l2, l3, l4, l5, l6; // define cuts - boost::shared_ptr cuts(new Cuts()); + std::shared_ptr cuts(new Cuts()); cuts->partitionTable = PartitionTable(9, -1); PartitionTable* p = &cuts->partitionTable; //x0 x1 x2 l1 l2 l3 l4 l5 l6 (*p)[0]=1; (*p)[1]=0; (*p)[2]=2; (*p)[3]=1; (*p)[4]=0; (*p)[5]=2; (*p)[6]=1; (*p)[7]=0; (*p)[8]=2; - cuts->children.push_back(boost::shared_ptr(new Cuts())); + cuts->children.push_back(std::shared_ptr(new Cuts())); cuts->children[0]->partitionTable = PartitionTable(9, -1); p = &cuts->children[0]->partitionTable; //x0 x1 x2 l1 l2 l3 l4 l5 l6 (*p)[0]=0; (*p)[1]=-1; (*p)[2]=-1; (*p)[3]=1; (*p)[4]=-1; (*p)[5]=-1; (*p)[6]=2; (*p)[7]=-1; (*p)[8]=-1; - cuts->children.push_back(boost::shared_ptr(new Cuts())); + cuts->children.push_back(std::shared_ptr(new Cuts())); cuts->children[1]->partitionTable = PartitionTable(9, -1); p = &cuts->children[1]->partitionTable; //x0 x1 x2 l1 l2 l3 l4 l5 l6 (*p)[0]=-1; (*p)[1]=-1; (*p)[2]=0; (*p)[3]=-1; (*p)[4]=-1; (*p)[5]=1; (*p)[6]=-1; (*p)[7]=-1; (*p)[8]=2; diff --git a/gtsam_unstable/slam/BetweenFactorEM.h b/gtsam_unstable/slam/BetweenFactorEM.h index 138f06f48..f175e30b3 100644 --- a/gtsam_unstable/slam/BetweenFactorEM.h +++ b/gtsam_unstable/slam/BetweenFactorEM.h @@ -62,7 +62,7 @@ private: public: // shorthand for a smart pointer to a factor - typedef typename boost::shared_ptr shared_ptr; + typedef typename std::shared_ptr shared_ptr; /** default constructor - only use for serialization */ BetweenFactorEM() { @@ -126,10 +126,10 @@ public: * Hence \f$ b = z - h(x) = - \mathtt{error\_vector}(x) \f$ */ /* This version of linearize recalculates the noise model each time */ - boost::shared_ptr linearize(const Values &x) const override { + std::shared_ptr linearize(const Values &x) const override { // Only linearize if the factor is active if (!this->active(x)) - return boost::shared_ptr(); + return std::shared_ptr(); //std::cout<<"About to linearize"< shared_ptr; + typedef std::shared_ptr shared_ptr; /** default constructor - only use for serialization */ BiasedGPSFactor() {} diff --git a/gtsam_unstable/slam/DummyFactor.cpp b/gtsam_unstable/slam/DummyFactor.cpp index f1897cf85..7d964e065 100644 --- a/gtsam_unstable/slam/DummyFactor.cpp +++ b/gtsam_unstable/slam/DummyFactor.cpp @@ -35,11 +35,11 @@ bool DummyFactor::equals(const NonlinearFactor& f, double tol) const { } /* ************************************************************************* */ -boost::shared_ptr +std::shared_ptr DummyFactor::linearize(const Values& c) const { // Only linearize if the factor is active if (!this->active(c)) - return boost::shared_ptr(); + return std::shared_ptr(); // Fill in terms with zero matrices std::vector > terms(this->size()); diff --git a/gtsam_unstable/slam/DummyFactor.h b/gtsam_unstable/slam/DummyFactor.h index 0484fc4c3..13ecad3b6 100644 --- a/gtsam_unstable/slam/DummyFactor.h +++ b/gtsam_unstable/slam/DummyFactor.h @@ -54,7 +54,7 @@ public: size_t dim() const override { return rowDim_; } /** linearize to a GaussianFactor */ - boost::shared_ptr linearize(const Values& c) const override; + std::shared_ptr linearize(const Values& c) const override; /** * Creates a shared_ptr clone of the factor - needs to be specialized to allow @@ -63,7 +63,7 @@ public: * By default, throws exception if subclass does not implement the function. */ NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( NonlinearFactor::shared_ptr(new DummyFactor(*this))); } diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h index c6c960ac1..05ba708aa 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h @@ -114,7 +114,7 @@ public: using Base::evaluateError; // shorthand for a smart pointer to a factor - typedef typename boost::shared_ptr shared_ptr; + typedef typename std::shared_ptr shared_ptr; /** default constructor - only use for serialization */ EquivInertialNavFactor_GlobalVel() {} diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h index f4c20a2ba..390b49faa 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h @@ -112,7 +112,7 @@ public: using Base::evaluateError; // shorthand for a smart pointer to a factor - typedef typename boost::shared_ptr shared_ptr; + typedef typename std::shared_ptr shared_ptr; /** default constructor - only use for serialization */ EquivInertialNavFactor_GlobalVel_NoBias() {} diff --git a/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h b/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h index 73f2ceb47..87389a40d 100644 --- a/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h +++ b/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h @@ -58,7 +58,7 @@ public: using Base::evaluateError; // shorthand for a smart pointer to a factor - typedef typename boost::shared_ptr shared_ptr; + typedef typename std::shared_ptr shared_ptr; /** default constructor - only use for serialization */ GaussMarkov1stOrderFactor() {} @@ -127,7 +127,7 @@ private: /* In practice, square root of the information matrix is represented, so that: * R_d (approx)= R / sqrt(delta_t) * */ - noiseModel::Gaussian::shared_ptr gaussian_model = boost::dynamic_pointer_cast(model); + noiseModel::Gaussian::shared_ptr gaussian_model = std::dynamic_pointer_cast(model); SharedGaussian model_d(noiseModel::Gaussian::SqrtInformation(gaussian_model->R()/sqrt(delta_t))); return model_d; } diff --git a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h index 508f1f294..a4d69f1dd 100644 --- a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h +++ b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h @@ -99,7 +99,7 @@ public: using Base::evaluateError; // shorthand for a smart pointer to a factor - typedef typename boost::shared_ptr shared_ptr; + typedef typename std::shared_ptr shared_ptr; /** default constructor - only use for serialization */ InertialNavFactor_GlobalVelocity() {} diff --git a/gtsam_unstable/slam/InvDepthFactor3.h b/gtsam_unstable/slam/InvDepthFactor3.h index c620bbc87..c10ff3b01 100644 --- a/gtsam_unstable/slam/InvDepthFactor3.h +++ b/gtsam_unstable/slam/InvDepthFactor3.h @@ -29,7 +29,7 @@ protected: // Keep a copy of measurement and calibration for I/O Point2 measured_; ///< 2D measurement - boost::shared_ptr K_; ///< shared pointer to calibration object + std::shared_ptr K_; ///< shared pointer to calibration object public: @@ -43,7 +43,7 @@ public: typedef InvDepthFactor3 This; /// shorthand for a smart pointer to a factor - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; /// Default constructor InvDepthFactor3() : diff --git a/gtsam_unstable/slam/InvDepthFactorVariant1.h b/gtsam_unstable/slam/InvDepthFactorVariant1.h index af0bcba55..0774eaa87 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant1.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant1.h @@ -43,7 +43,7 @@ public: typedef InvDepthFactorVariant1 This; /// shorthand for a smart pointer to a factor - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; /// Default constructor InvDepthFactorVariant1() : diff --git a/gtsam_unstable/slam/InvDepthFactorVariant2.h b/gtsam_unstable/slam/InvDepthFactorVariant2.h index b6ce1a890..18f866c56 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant2.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant2.h @@ -45,7 +45,7 @@ public: typedef InvDepthFactorVariant2 This; /// shorthand for a smart pointer to a factor - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; /// Default constructor InvDepthFactorVariant2() : diff --git a/gtsam_unstable/slam/InvDepthFactorVariant3.h b/gtsam_unstable/slam/InvDepthFactorVariant3.h index e8c2781d4..4f0ac58a6 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant3.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant3.h @@ -43,7 +43,7 @@ public: typedef InvDepthFactorVariant3a This; /// shorthand for a smart pointer to a factor - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; /// Default constructor InvDepthFactorVariant3a() : @@ -168,7 +168,7 @@ public: typedef InvDepthFactorVariant3b This; /// shorthand for a smart pointer to a factor - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; /// Default constructor InvDepthFactorVariant3b() : diff --git a/gtsam_unstable/slam/MultiProjectionFactor.h b/gtsam_unstable/slam/MultiProjectionFactor.h index 5ba25abd2..3adf227bc 100644 --- a/gtsam_unstable/slam/MultiProjectionFactor.h +++ b/gtsam_unstable/slam/MultiProjectionFactor.h @@ -37,7 +37,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 + std::shared_ptr K_; ///< shared pointer to calibration object std::optional body_P_sensor_; ///< The pose of the sensor in the body frame @@ -54,7 +54,7 @@ namespace gtsam { typedef MultiProjectionFactor This; /// shorthand for a smart pointer to a factor - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; /// Default constructor MultiProjectionFactor() : throwCheirality_(false), verboseCheirality_(false) {} @@ -70,7 +70,7 @@ namespace gtsam { * @param body_P_sensor is the transform from body to sensor frame (default identity) */ MultiProjectionFactor(const Vector& measured, const SharedNoiseModel& model, - KeySet poseKeys, Key pointKey, const boost::shared_ptr& K, + KeySet poseKeys, Key pointKey, const std::shared_ptr& K, std::optional body_P_sensor = {}) : Base(model), measured_(measured), K_(K), body_P_sensor_(body_P_sensor), throwCheirality_(false), verboseCheirality_(false) { @@ -91,7 +91,7 @@ namespace gtsam { * @param body_P_sensor is the transform from body to sensor frame (default identity) */ MultiProjectionFactor(const Vector& measured, const SharedNoiseModel& model, - KeySet poseKeys, Key pointKey, const boost::shared_ptr& K, + KeySet poseKeys, Key pointKey, const std::shared_ptr& K, bool throwCheirality, bool verboseCheirality, std::optional body_P_sensor = {}) : Base(model), measured_(measured), K_(K), body_P_sensor_(body_P_sensor), @@ -102,7 +102,7 @@ namespace gtsam { /// @return a deep copy of this factor NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( NonlinearFactor::shared_ptr(new This(*this))); } /** @@ -201,7 +201,7 @@ namespace gtsam { } /** return the calibration object */ - inline const boost::shared_ptr calibration() const { + inline const std::shared_ptr calibration() const { return K_; } diff --git a/gtsam_unstable/slam/PartialPriorFactor.h b/gtsam_unstable/slam/PartialPriorFactor.h index ab4a2bce3..6e29ca6d5 100644 --- a/gtsam_unstable/slam/PartialPriorFactor.h +++ b/gtsam_unstable/slam/PartialPriorFactor.h @@ -87,7 +87,7 @@ namespace gtsam { /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } /** implement functions needed for Testable */ diff --git a/gtsam_unstable/slam/PoseBetweenFactor.h b/gtsam_unstable/slam/PoseBetweenFactor.h index 9e275116b..339407a3f 100644 --- a/gtsam_unstable/slam/PoseBetweenFactor.h +++ b/gtsam_unstable/slam/PoseBetweenFactor.h @@ -48,7 +48,7 @@ namespace gtsam { using Base::evaluateError; // shorthand for a smart pointer to a factor - typedef typename boost::shared_ptr shared_ptr; + typedef typename std::shared_ptr shared_ptr; /** default constructor - only use for serialization */ PoseBetweenFactor() {} @@ -63,7 +63,7 @@ namespace gtsam { /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } /** implement functions needed for Testable */ diff --git a/gtsam_unstable/slam/PosePriorFactor.h b/gtsam_unstable/slam/PosePriorFactor.h index 4e55bd649..a5e5facbd 100644 --- a/gtsam_unstable/slam/PosePriorFactor.h +++ b/gtsam_unstable/slam/PosePriorFactor.h @@ -45,7 +45,7 @@ namespace gtsam { using Base::evaluateError; /// shorthand for a smart pointer to a factor - typedef typename boost::shared_ptr > shared_ptr; + typedef typename std::shared_ptr > shared_ptr; /** default constructor - only use for serialization */ PosePriorFactor() {} @@ -60,7 +60,7 @@ namespace gtsam { /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } /** implement functions needed for Testable */ diff --git a/gtsam_unstable/slam/PoseToPointFactor.h b/gtsam_unstable/slam/PoseToPointFactor.h index 335e3b8d0..62f681b2e 100644 --- a/gtsam_unstable/slam/PoseToPointFactor.h +++ b/gtsam_unstable/slam/PoseToPointFactor.h @@ -34,7 +34,7 @@ class PoseToPointFactor : public NoiseModelFactorN { using Base::evaluateError; // shorthand for a smart pointer to a factor - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; /** default constructor - only use for serialization */ PoseToPointFactor() {} @@ -68,7 +68,7 @@ class PoseToPointFactor : public NoiseModelFactorN { /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } diff --git a/gtsam_unstable/slam/ProjectionFactorPPP.h b/gtsam_unstable/slam/ProjectionFactorPPP.h index 9c09257d3..dea28528d 100644 --- a/gtsam_unstable/slam/ProjectionFactorPPP.h +++ b/gtsam_unstable/slam/ProjectionFactorPPP.h @@ -35,7 +35,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 + std::shared_ptr K_; ///< shared pointer to calibration object // verbosity handling for Cheirality Exceptions bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false) @@ -53,7 +53,7 @@ namespace gtsam { typedef ProjectionFactorPPP This; /// shorthand for a smart pointer to a factor - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; /// Default constructor ProjectionFactorPPP() : @@ -72,7 +72,7 @@ namespace gtsam { */ ProjectionFactorPPP(const Point2& measured, const SharedNoiseModel& model, Key poseKey, Key transformKey, Key pointKey, - const boost::shared_ptr& K) : + const std::shared_ptr& K) : Base(model, poseKey, transformKey, pointKey), measured_(measured), K_(K), throwCheirality_(false), verboseCheirality_(false) {} @@ -89,7 +89,7 @@ namespace gtsam { */ ProjectionFactorPPP(const Point2& measured, const SharedNoiseModel& model, Key poseKey, Key transformKey, Key pointKey, - const boost::shared_ptr& K, + const std::shared_ptr& K, bool throwCheirality, bool verboseCheirality) : Base(model, poseKey, transformKey, pointKey), measured_(measured), K_(K), throwCheirality_(throwCheirality), verboseCheirality_(verboseCheirality) {} @@ -99,7 +99,7 @@ namespace gtsam { /// @return a deep copy of this factor NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( NonlinearFactor::shared_ptr(new This(*this))); } /** @@ -159,7 +159,7 @@ namespace gtsam { } /** return the calibration object */ - inline const boost::shared_ptr calibration() const { + inline const std::shared_ptr calibration() const { return K_; } diff --git a/gtsam_unstable/slam/ProjectionFactorPPPC.h b/gtsam_unstable/slam/ProjectionFactorPPPC.h index b4288d7a0..a3dd0850a 100644 --- a/gtsam_unstable/slam/ProjectionFactorPPPC.h +++ b/gtsam_unstable/slam/ProjectionFactorPPPC.h @@ -52,7 +52,7 @@ class GTSAM_UNSTABLE_EXPORT ProjectionFactorPPPC typedef ProjectionFactorPPPC This; /// shorthand for a smart pointer to a factor - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; /// Default constructor ProjectionFactorPPPC() : @@ -86,7 +86,7 @@ class GTSAM_UNSTABLE_EXPORT ProjectionFactorPPPC /// @return a deep copy of this factor NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( NonlinearFactor::shared_ptr(new This(*this))); } /** diff --git a/gtsam_unstable/slam/ProjectionFactorRollingShutter.h b/gtsam_unstable/slam/ProjectionFactorRollingShutter.h index 2d2c2c6c2..938f20fd6 100644 --- a/gtsam_unstable/slam/ProjectionFactorRollingShutter.h +++ b/gtsam_unstable/slam/ProjectionFactorRollingShutter.h @@ -47,7 +47,7 @@ class GTSAM_UNSTABLE_EXPORT ProjectionFactorRollingShutter Point2 measured_; ///< 2D measurement double alpha_; ///< interpolation parameter in [0,1] corresponding to the ///< point2 measurement - boost::shared_ptr K_; ///< shared pointer to calibration object + std::shared_ptr K_; ///< shared pointer to calibration object std::optional body_P_sensor_; ///< The pose of the sensor in the body frame @@ -69,7 +69,7 @@ class GTSAM_UNSTABLE_EXPORT ProjectionFactorRollingShutter typedef ProjectionFactorRollingShutter This; /// shorthand for a smart pointer to a factor - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; /// Default constructor ProjectionFactorRollingShutter() @@ -94,7 +94,7 @@ class GTSAM_UNSTABLE_EXPORT ProjectionFactorRollingShutter ProjectionFactorRollingShutter( const Point2& measured, double alpha, const SharedNoiseModel& model, Key poseKey_a, Key poseKey_b, Key pointKey, - const boost::shared_ptr& K, + const std::shared_ptr& K, std::optional body_P_sensor = {}) : Base(model, poseKey_a, poseKey_b, pointKey), measured_(measured), @@ -124,7 +124,7 @@ class GTSAM_UNSTABLE_EXPORT ProjectionFactorRollingShutter ProjectionFactorRollingShutter( const Point2& measured, double alpha, const SharedNoiseModel& model, Key poseKey_a, Key poseKey_b, Key pointKey, - const boost::shared_ptr& K, bool throwCheirality, + const std::shared_ptr& K, bool throwCheirality, bool verboseCheirality, std::optional body_P_sensor = {}) : Base(model, poseKey_a, poseKey_b, pointKey), @@ -140,7 +140,7 @@ class GTSAM_UNSTABLE_EXPORT ProjectionFactorRollingShutter /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } @@ -182,7 +182,7 @@ class GTSAM_UNSTABLE_EXPORT ProjectionFactorRollingShutter const Point2& measured() const { return measured_; } /** return the calibration object */ - inline const boost::shared_ptr calibration() const { return K_; } + inline const std::shared_ptr calibration() const { return K_; } /** returns the rolling shutter interp param*/ inline double alpha() const { return alpha_; } diff --git a/gtsam_unstable/slam/RelativeElevationFactor.h b/gtsam_unstable/slam/RelativeElevationFactor.h index 5beb454bc..db86ead96 100644 --- a/gtsam_unstable/slam/RelativeElevationFactor.h +++ b/gtsam_unstable/slam/RelativeElevationFactor.h @@ -47,7 +47,7 @@ public: /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } /** h(x)-z */ diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index 59a562d2c..0aac1e0fa 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -62,7 +62,7 @@ class GTSAM_UNSTABLE_EXPORT SmartProjectionPoseFactorRollingShutter /// one or more cameras taking observations (fixed poses wrt body + fixed /// intrinsics) - boost::shared_ptr cameraRig_; + std::shared_ptr cameraRig_; /// vector of camera Ids (one for each observation, in the same order), /// identifying which camera took the measurement @@ -85,7 +85,7 @@ class GTSAM_UNSTABLE_EXPORT SmartProjectionPoseFactorRollingShutter typedef CameraSet Cameras; /// shorthand for a smart pointer to a factor - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; /// Default constructor, only for serialization SmartProjectionPoseFactorRollingShutter() {} @@ -99,7 +99,7 @@ class GTSAM_UNSTABLE_EXPORT SmartProjectionPoseFactorRollingShutter */ SmartProjectionPoseFactorRollingShutter( const SharedNoiseModel& sharedNoiseModel, - const boost::shared_ptr& cameraRig, + const std::shared_ptr& cameraRig, const SmartProjectionParams& params = SmartProjectionParams()) : Base(sharedNoiseModel, params), cameraRig_(cameraRig) { // throw exception if configuration is not supported by this factor @@ -204,7 +204,7 @@ class GTSAM_UNSTABLE_EXPORT SmartProjectionPoseFactorRollingShutter const std::vector& alphas() const { return alphas_; } /// return the calibration object - const boost::shared_ptr& cameraRig() const { return cameraRig_; } + const std::shared_ptr& cameraRig() const { return cameraRig_; } /// return the calibration object const FastVector& cameraIds() const { return cameraIds_; } @@ -358,7 +358,7 @@ class GTSAM_UNSTABLE_EXPORT SmartProjectionPoseFactorRollingShutter } /// linearize and return a Hessianfactor that is an approximation of error(p) - boost::shared_ptr> createHessianFactor( + std::shared_ptr> createHessianFactor( const Values& values, const double& lambda = 0.0, bool diagonalDamping = false) const { // we may have multiple observation sharing the same keys (due to the @@ -388,7 +388,7 @@ class GTSAM_UNSTABLE_EXPORT SmartProjectionPoseFactorRollingShutter if (this->params_.degeneracyMode == ZERO_ON_DEGENERACY) { for (Matrix& m : Gs) m = Matrix::Zero(DimPose, DimPose); for (Vector& v : gs) v = Vector::Zero(DimPose); - return boost::make_shared>(this->keys_, + return std::make_shared>(this->keys_, Gs, gs, 0.0); } else { throw std::runtime_error( @@ -424,7 +424,7 @@ class GTSAM_UNSTABLE_EXPORT SmartProjectionPoseFactorRollingShutter Base::Cameras::template SchurComplementAndRearrangeBlocks<3, 12, 6>( Fs, E, P, b, nonuniqueKeys, this->keys_); - return boost::make_shared>( + return std::make_shared>( this->keys_, augmentedHessianUniqueKeys); } @@ -435,7 +435,7 @@ class GTSAM_UNSTABLE_EXPORT SmartProjectionPoseFactorRollingShutter * extrinsic pose for this factor * @return a Gaussian factor */ - boost::shared_ptr linearizeDamped( + std::shared_ptr linearizeDamped( const Values& values, const double& lambda = 0.0) const { // depending on flag set on construction we may linearize to different // linear factors @@ -450,7 +450,7 @@ class GTSAM_UNSTABLE_EXPORT SmartProjectionPoseFactorRollingShutter } /// linearize - boost::shared_ptr linearize( + std::shared_ptr linearize( const Values& values) const override { return this->linearizeDamped(values); } diff --git a/gtsam_unstable/slam/SmartRangeFactor.h b/gtsam_unstable/slam/SmartRangeFactor.h index 17270f392..57df1b41f 100644 --- a/gtsam_unstable/slam/SmartRangeFactor.h +++ b/gtsam_unstable/slam/SmartRangeFactor.h @@ -180,7 +180,7 @@ class SmartRangeFactor: public NoiseModelFactor { /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } }; diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index 1a2a648e3..c05bd1fd0 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -30,7 +30,6 @@ #include #include -#include #include #include #include @@ -72,7 +71,7 @@ protected: public: /// shorthand for a smart pointer to a factor - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; /// Vector of cameras typedef CameraSet Cameras; @@ -198,7 +197,7 @@ public: } /// linearize returns a Hessianfactor that is an approximation of error(p) - boost::shared_ptr > createHessianFactor( + std::shared_ptr > createHessianFactor( const Cameras& cameras, const double lambda = 0.0, bool diagonalDamping = false) const { @@ -220,7 +219,7 @@ public: m = Matrix::Zero(Base::Dim, Base::Dim); for(Vector& v: gs) v = Vector::Zero(Base::Dim); - return boost::make_shared >(this->keys_, + return std::make_shared >(this->keys_, Gs, gs, 0.0); } @@ -237,59 +236,59 @@ public: SymmetricBlockMatrix augmentedHessian = // Cameras::SchurComplement(Fs, E, b, lambda, diagonalDamping); - return boost::make_shared >(this->keys_, + return std::make_shared >(this->keys_, augmentedHessian); } // create factor -// boost::shared_ptr > createRegularImplicitSchurFactor( +// std::shared_ptr > createRegularImplicitSchurFactor( // const Cameras& cameras, double lambda) const { // if (triangulateForLinearize(cameras)) // return Base::createRegularImplicitSchurFactor(cameras, *result_, lambda); // else // // failed: return empty -// return boost::shared_ptr >(); +// return std::shared_ptr >(); // } // // /// create factor -// boost::shared_ptr > createJacobianQFactor( +// std::shared_ptr > createJacobianQFactor( // const Cameras& cameras, double lambda) const { // if (triangulateForLinearize(cameras)) // return Base::createJacobianQFactor(cameras, *result_, lambda); // else // // failed: return empty -// return boost::make_shared >(this->keys_); +// return std::make_shared >(this->keys_); // } // // /// Create a factor, takes values -// boost::shared_ptr > createJacobianQFactor( +// std::shared_ptr > createJacobianQFactor( // const Values& values, double lambda) const { // return createJacobianQFactor(this->cameras(values), lambda); // } /// different (faster) way to compute Jacobian factor - boost::shared_ptr createJacobianSVDFactor( + std::shared_ptr createJacobianSVDFactor( const Cameras& cameras, double lambda) const { if (triangulateForLinearize(cameras)) return Base::createJacobianSVDFactor(cameras, *result_, lambda); else - return boost::make_shared >(this->keys_); + return std::make_shared >(this->keys_); } // /// linearize to a Hessianfactor -// virtual boost::shared_ptr > linearizeToHessian( +// virtual std::shared_ptr > linearizeToHessian( // const Values& values, double lambda = 0.0) const { // return createHessianFactor(this->cameras(values), lambda); // } // /// linearize to an Implicit Schur factor -// virtual boost::shared_ptr > linearizeToImplicit( +// virtual std::shared_ptr > linearizeToImplicit( // const Values& values, double lambda = 0.0) const { // return createRegularImplicitSchurFactor(this->cameras(values), lambda); // } // // /// linearize to a JacobianfactorQ -// virtual boost::shared_ptr > linearizeToJacobian( +// virtual std::shared_ptr > linearizeToJacobian( // const Values& values, double lambda = 0.0) const { // return createJacobianQFactor(this->cameras(values), lambda); // } @@ -299,7 +298,7 @@ public: * @param values Values structure which must contain camera poses for this factor * @return a Gaussian factor */ - boost::shared_ptr linearizeDamped(const Cameras& cameras, + std::shared_ptr linearizeDamped(const Cameras& cameras, const double lambda = 0.0) const { // depending on flag set on construction we may linearize to different linear factors switch (params_.linearizationMode) { @@ -321,7 +320,7 @@ public: * @param values Values structure which must contain camera poses for this factor * @return a Gaussian factor */ - boost::shared_ptr linearizeDamped(const Values& values, + std::shared_ptr linearizeDamped(const Values& values, const double lambda = 0.0) const { // depending on flag set on construction we may linearize to different linear factors Cameras cameras = this->cameras(values); @@ -329,7 +328,7 @@ public: } /// linearize - boost::shared_ptr linearize( + std::shared_ptr linearize( const Values& values) const override { return linearizeDamped(values); } diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.cpp b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.cpp index 07344ab04..c4a473154 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.cpp +++ b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.cpp @@ -28,7 +28,7 @@ SmartStereoProjectionFactorPP::SmartStereoProjectionFactorPP( void SmartStereoProjectionFactorPP::add( const StereoPoint2& measured, const Key& w_P_body_key, const Key& body_P_cam_key, - const boost::shared_ptr& K) { + const std::shared_ptr& K) { // we index by cameras.. Base::add(measured, w_P_body_key); // but we also store the extrinsic calibration keys in the same order @@ -45,7 +45,7 @@ void SmartStereoProjectionFactorPP::add( void SmartStereoProjectionFactorPP::add( const std::vector& measurements, const KeyVector& world_P_body_keys, const KeyVector& body_P_cam_keys, - const std::vector>& Ks) { + const std::vector>& Ks) { assert(world_P_body_keys.size() == measurements.size()); assert(world_P_body_keys.size() == body_P_cam_keys.size()); assert(world_P_body_keys.size() == Ks.size()); @@ -65,7 +65,7 @@ void SmartStereoProjectionFactorPP::add( void SmartStereoProjectionFactorPP::add( const std::vector& measurements, const KeyVector& world_P_body_keys, const KeyVector& body_P_cam_keys, - const boost::shared_ptr& K) { + const std::shared_ptr& K) { assert(world_P_body_keys.size() == measurements.size()); assert(world_P_body_keys.size() == body_P_cam_keys.size()); for (size_t i = 0; i < measurements.size(); i++) { diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h index 0c9174bf6..df5aed412 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h @@ -44,7 +44,7 @@ class GTSAM_UNSTABLE_EXPORT SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor { protected: /// shared pointer to calibration object (one for each camera) - std::vector> K_all_; + std::vector> K_all_; /// The keys corresponding to the pose of the body (with respect to an external world frame) for each view KeyVector world_P_body_keys_; @@ -62,7 +62,7 @@ class GTSAM_UNSTABLE_EXPORT SmartStereoProjectionFactorPP typedef SmartStereoProjectionFactorPP This; /// shorthand for a smart pointer to a factor - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; static const int DimBlock = 12; ///< Camera dimension: 6 for body pose, 6 for extrinsic pose static const int DimPose = 6; ///< Pose3 dimension @@ -92,7 +92,7 @@ class GTSAM_UNSTABLE_EXPORT SmartStereoProjectionFactorPP */ void add(const StereoPoint2& measured, const Key& world_P_body_key, const Key& body_P_cam_key, - const boost::shared_ptr& K); + const std::shared_ptr& K); /** * Variant of the previous one in which we include a set of measurements @@ -105,7 +105,7 @@ class GTSAM_UNSTABLE_EXPORT SmartStereoProjectionFactorPP */ void add(const std::vector& measurements, const KeyVector& w_P_body_keys, const KeyVector& body_P_cam_keys, - const std::vector>& Ks); + const std::vector>& Ks); /** * Variant of the previous one in which we include a set of measurements with @@ -119,7 +119,7 @@ class GTSAM_UNSTABLE_EXPORT SmartStereoProjectionFactorPP */ void add(const std::vector& measurements, const KeyVector& w_P_body_keys, const KeyVector& body_P_cam_keys, - const boost::shared_ptr& K); + const std::shared_ptr& K); /** * print @@ -143,7 +143,7 @@ class GTSAM_UNSTABLE_EXPORT SmartStereoProjectionFactorPP double error(const Values& values) const override; /** return the calibration object */ - inline std::vector> calibration() const { + inline std::vector> calibration() const { return K_all_; } @@ -204,7 +204,7 @@ class GTSAM_UNSTABLE_EXPORT SmartStereoProjectionFactorPP } /// linearize and return a Hessianfactor that is an approximation of error(p) - boost::shared_ptr > createHessianFactor( + std::shared_ptr > createHessianFactor( const Values& values, const double lambda = 0.0, bool diagonalDamping = false) const { @@ -230,7 +230,7 @@ class GTSAM_UNSTABLE_EXPORT SmartStereoProjectionFactorPP m = Matrix::Zero(DimPose, DimPose); for (Vector& v : gs) v = Vector::Zero(DimPose); - return boost::make_shared < RegularHessianFactor + return std::make_shared < RegularHessianFactor > (keys_, Gs, gs, 0.0); } @@ -260,7 +260,7 @@ class GTSAM_UNSTABLE_EXPORT SmartStereoProjectionFactorPP Cameras::SchurComplementAndRearrangeBlocks<3,DimBlock,DimPose>(Fs,E,P,b, nonuniqueKeys, keys_); - return boost::make_shared < RegularHessianFactor + return std::make_shared < RegularHessianFactor > (keys_, augmentedHessianUniqueKeys); } @@ -269,7 +269,7 @@ class GTSAM_UNSTABLE_EXPORT SmartStereoProjectionFactorPP * @param values Values structure which must contain camera poses and extrinsic pose for this factor * @return a Gaussian factor */ - boost::shared_ptr linearizeDamped( + std::shared_ptr linearizeDamped( const Values& values, const double lambda = 0.0) const { // depending on flag set on construction we may linearize to different linear factors switch (params_.linearizationMode) { @@ -282,7 +282,7 @@ class GTSAM_UNSTABLE_EXPORT SmartStereoProjectionFactorPP } /// linearize - boost::shared_ptr linearize(const Values& values) const + std::shared_ptr linearize(const Values& values) const override { return linearizeDamped(values); } diff --git a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.cpp b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.cpp index 4f6ef18ab..57913385a 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.cpp +++ b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.cpp @@ -31,14 +31,14 @@ SmartStereoProjectionPoseFactor::SmartStereoProjectionPoseFactor( void SmartStereoProjectionPoseFactor::add( const StereoPoint2& measured, const Key& poseKey, - const boost::shared_ptr& K) { + const std::shared_ptr& K) { Base::add(measured, poseKey); K_all_.push_back(K); } void SmartStereoProjectionPoseFactor::add( const std::vector& measurements, const KeyVector& poseKeys, - const std::vector>& Ks) { + const std::vector>& Ks) { assert(measurements.size() == poseKeys.size()); assert(poseKeys.size() == Ks.size()); Base::add(measurements, poseKeys); @@ -47,7 +47,7 @@ void SmartStereoProjectionPoseFactor::add( void SmartStereoProjectionPoseFactor::add( const std::vector& measurements, const KeyVector& poseKeys, - const boost::shared_ptr& K) { + const std::shared_ptr& K) { assert(poseKeys.size() == measurements.size()); for (size_t i = 0; i < measurements.size(); i++) { Base::add(measurements[i], poseKeys[i]); @@ -58,7 +58,7 @@ void SmartStereoProjectionPoseFactor::add( void SmartStereoProjectionPoseFactor::print( const std::string& s, const KeyFormatter& keyFormatter) const { std::cout << s << "SmartStereoProjectionPoseFactor, z = \n "; - for (const boost::shared_ptr& K : K_all_) { + for (const std::shared_ptr& K : K_all_) { K->print("calibration = "); } Base::print("", keyFormatter); diff --git a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h index 7295413ca..3e8270b25 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h @@ -47,7 +47,7 @@ class GTSAM_UNSTABLE_EXPORT SmartStereoProjectionPoseFactor : public SmartStereoProjectionFactor { protected: /// shared pointer to calibration object (one for each camera) - std::vector> K_all_; + std::vector> K_all_; public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW @@ -59,7 +59,7 @@ class GTSAM_UNSTABLE_EXPORT SmartStereoProjectionPoseFactor typedef SmartStereoProjectionPoseFactor This; /// shorthand for a smart pointer to a factor - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; /** * Constructor @@ -83,7 +83,7 @@ class GTSAM_UNSTABLE_EXPORT SmartStereoProjectionPoseFactor * @param K is the (fixed) camera calibration */ void add(const StereoPoint2& measured, const Key& poseKey, - const boost::shared_ptr& K); + const std::shared_ptr& K); /** * Variant of the previous one in which we include a set of measurements @@ -95,7 +95,7 @@ class GTSAM_UNSTABLE_EXPORT SmartStereoProjectionPoseFactor */ void add(const std::vector& measurements, const KeyVector& poseKeys, - const std::vector>& Ks); + const std::vector>& Ks); /** * Variant of the previous one in which we include a set of measurements with @@ -108,7 +108,7 @@ class GTSAM_UNSTABLE_EXPORT SmartStereoProjectionPoseFactor */ void add(const std::vector& measurements, const KeyVector& poseKeys, - const boost::shared_ptr& K); + const std::shared_ptr& K); /** * print @@ -127,7 +127,7 @@ class GTSAM_UNSTABLE_EXPORT SmartStereoProjectionPoseFactor double error(const Values& values) const override; /** return the calibration object */ - inline std::vector> calibration() const { + inline std::vector> calibration() const { return K_all_; } diff --git a/gtsam_unstable/slam/TOAFactor.h b/gtsam_unstable/slam/TOAFactor.h index 8ad4a14a6..784bb03ca 100644 --- a/gtsam_unstable/slam/TOAFactor.h +++ b/gtsam_unstable/slam/TOAFactor.h @@ -59,7 +59,7 @@ class TOAFactor : public ExpressionFactor { model, speed) {} static void InsertEvent(Key key, const Event& event, - boost::shared_ptr values) { + std::shared_ptr values) { values->insert(key, event); } }; diff --git a/gtsam_unstable/slam/TSAMFactors.h b/gtsam_unstable/slam/TSAMFactors.h index 3d65f0f61..3e3157745 100644 --- a/gtsam_unstable/slam/TSAMFactors.h +++ b/gtsam_unstable/slam/TSAMFactors.h @@ -31,7 +31,7 @@ class DeltaFactor: public NoiseModelFactorN { public: typedef DeltaFactor This; typedef NoiseModelFactorN Base; - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; private: Point2 measured_; ///< the measurement @@ -62,7 +62,7 @@ class DeltaFactorBase: public NoiseModelFactorN { public: typedef DeltaFactorBase This; typedef NoiseModelFactorN Base; - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; private: Point2 measured_; ///< the measurement @@ -118,7 +118,7 @@ class OdometryFactorBase: public NoiseModelFactorN { public: typedef OdometryFactorBase This; typedef NoiseModelFactorN Base; - typedef boost::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; private: Pose2 measured_; ///< the measurement diff --git a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h index d2d28ef9b..e269e6506 100644 --- a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h +++ b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h @@ -61,7 +61,7 @@ namespace gtsam { public: // shorthand for a smart pointer to a factor - typedef typename boost::shared_ptr shared_ptr; + typedef typename std::shared_ptr shared_ptr; /** default constructor - only use for serialization */ TransformBtwRobotsUnaryFactor() {} @@ -81,7 +81,7 @@ namespace gtsam { /** Clone */ - gtsam::NonlinearFactor::shared_ptr clone() const override { return boost::make_shared(*this); } + gtsam::NonlinearFactor::shared_ptr clone() const override { return std::make_shared(*this); } /** implement functions needed for Testable */ @@ -139,10 +139,10 @@ namespace gtsam { * Hence \f$ b = z - h(x) = - \mathtt{error\_vector}(x) \f$ */ /* This version of linearize recalculates the noise model each time */ - boost::shared_ptr linearize(const gtsam::Values& x) const override { + std::shared_ptr linearize(const gtsam::Values& x) const override { // Only linearize if the factor is active if (!this->active(x)) - return boost::shared_ptr(); + return std::shared_ptr(); //std::cout<<"About to linearize"< shared_ptr; + typedef typename std::shared_ptr shared_ptr; /** default constructor - only use for serialization */ TransformBtwRobotsUnaryFactorEM() {} @@ -97,7 +97,7 @@ namespace gtsam { /** Clone */ - NonlinearFactor::shared_ptr clone() const override { return boost::make_shared(*this); } + NonlinearFactor::shared_ptr clone() const override { return std::make_shared(*this); } /** implement functions needed for Testable */ @@ -162,10 +162,10 @@ namespace gtsam { * Hence \f$ b = z - h(x) = - \mathtt{error\_vector}(x) \f$ */ /* This version of linearize recalculates the noise model each time */ - boost::shared_ptr linearize(const Values& x) const override { + std::shared_ptr linearize(const Values& x) const override { // Only linearize if the factor is active if (!this->active(x)) - return boost::shared_ptr(); + return std::shared_ptr(); //std::cout<<"About to linearize"<(cells)); + push_back(std::make_shared(cells)); } diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp index 098d33744..3735e65a8 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -85,7 +85,7 @@ typedef SmartProjectionPoseFactorRollingShutter> /* ************************************************************************* */ TEST(SmartProjectionPoseFactorRollingShutter, Constructor) { using namespace vanillaPoseRS; - boost::shared_ptr cameraRig(new Cameras()); + std::shared_ptr cameraRig(new Cameras()); cameraRig->push_back(Camera(Pose3::Identity(), sharedK)); SmartFactorRS::shared_ptr factor1( new SmartFactorRS(model, cameraRig, params)); @@ -94,7 +94,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, Constructor) { /* ************************************************************************* */ TEST(SmartProjectionPoseFactorRollingShutter, Constructor2) { using namespace vanillaPoseRS; - boost::shared_ptr cameraRig(new Cameras()); + std::shared_ptr cameraRig(new Cameras()); cameraRig->push_back(Camera(Pose3::Identity(), sharedK)); params.setRankTolerance(rankTol); SmartFactorRS factor1(model, cameraRig, params); @@ -103,7 +103,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, Constructor2) { /* ************************************************************************* */ TEST(SmartProjectionPoseFactorRollingShutter, add) { using namespace vanillaPoseRS; - boost::shared_ptr cameraRig(new Cameras()); + std::shared_ptr cameraRig(new Cameras()); cameraRig->push_back(Camera(Pose3::Identity(), sharedK)); SmartFactorRS::shared_ptr factor1( new SmartFactorRS(model, cameraRig, params)); @@ -132,7 +132,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, Equals) { FastVector cameraIds{0, 0, 0}; - boost::shared_ptr cameraRig(new Cameras()); + std::shared_ptr cameraRig(new Cameras()); cameraRig->push_back(Camera(body_P_sensor, sharedK)); // create by adding a batch of measurements with a bunch of calibrations @@ -187,7 +187,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, Equals) { } { // create slightly different factors (different extrinsics) and show equal // returns false - boost::shared_ptr cameraRig2(new Cameras()); + std::shared_ptr cameraRig2(new Cameras()); cameraRig2->push_back(Camera(body_P_sensor * body_P_sensor, sharedK)); SmartFactorRS::shared_ptr factor1( new SmartFactorRS(model, cameraRig2, params)); @@ -230,7 +230,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, noiselessErrorAndJacobians) { Point2 level_uv_right = cam2.project(landmark1); Pose3 body_P_sensorId = Pose3::Identity(); - boost::shared_ptr cameraRig(new Cameras()); + std::shared_ptr cameraRig(new Cameras()); cameraRig->push_back(Camera(body_P_sensorId, sharedK)); SmartFactorRS factor(model, cameraRig, params); @@ -308,7 +308,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians) { Point2 level_uv_right = cam2.project(landmark1); Pose3 body_P_sensorNonId = body_P_sensor; - boost::shared_ptr cameraRig(new Cameras()); + std::shared_ptr cameraRig(new Cameras()); cameraRig->push_back(Camera(body_P_sensorNonId, sharedK)); SmartFactorRS factor(model, cameraRig, params); @@ -402,7 +402,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses) { interp_factors.push_back(interp_factor2); interp_factors.push_back(interp_factor3); - boost::shared_ptr cameraRig(new Cameras()); + std::shared_ptr cameraRig(new Cameras()); cameraRig->push_back(Camera(Pose3::Identity(), sharedK)); SmartFactorRS::shared_ptr smartFactor1( @@ -476,7 +476,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_multiCam) { interp_factors.push_back(interp_factor2); interp_factors.push_back(interp_factor3); - boost::shared_ptr cameraRig(new Cameras()); + std::shared_ptr cameraRig(new Cameras()); cameraRig->push_back(Camera(body_P_sensor, sharedK)); cameraRig->push_back(Camera(Pose3::Identity(), sharedK)); @@ -564,7 +564,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_multiCam2) { interp_factors.push_back(interp_factor2); interp_factors.push_back(interp_factor3); - boost::shared_ptr cameraRig(new Cameras()); + std::shared_ptr cameraRig(new Cameras()); cameraRig->push_back(Camera(body_T_sensor1, sharedK)); cameraRig->push_back(Camera(body_T_sensor2, sharedK)); cameraRig->push_back(Camera(body_T_sensor3, sharedK)); @@ -646,7 +646,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses) { measurements_lmk1.push_back(cam1.project(landmark1)); measurements_lmk1.push_back(cam2.project(landmark1)); - boost::shared_ptr cameraRig(new Cameras()); + std::shared_ptr cameraRig(new Cameras()); cameraRig->push_back(Camera(body_P_sensorId, sharedKSimple)); SmartFactorRS::shared_ptr smartFactor1( @@ -707,7 +707,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses) { Values values; values.insert(x1, pose1); values.insert(x2, pose2); - boost::shared_ptr> actual = + std::shared_ptr> actual = smartFactor1->createHessianFactor(values); EXPECT(assert_equal(expectedInformation, actual->information(), 1e-6)); EXPECT(assert_equal(expected, *actual, 1e-6)); @@ -744,7 +744,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI) { params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); params.setEnableEPI(true); - boost::shared_ptr cameraRig(new Cameras()); + std::shared_ptr cameraRig(new Cameras()); cameraRig->push_back(Camera(Pose3::Identity(), sharedK)); SmartFactorRS smartFactor1(model, cameraRig, params); @@ -813,7 +813,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); params.setEnableEPI(false); - boost::shared_ptr cameraRig(new Cameras()); + std::shared_ptr cameraRig(new Cameras()); cameraRig->push_back(Camera(Pose3::Identity(), sharedK)); SmartFactorRS smartFactor1(model, cameraRig, params); @@ -891,7 +891,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold); params.setEnableEPI(false); - boost::shared_ptr cameraRig(new Cameras()); + std::shared_ptr cameraRig(new Cameras()); cameraRig->push_back(Camera(Pose3::Identity(), sharedK)); SmartFactorRS::shared_ptr smartFactor1( @@ -958,7 +958,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, interp_factors.push_back(interp_factor2); interp_factors.push_back(interp_factor3); - boost::shared_ptr cameraRig(new Cameras()); + std::shared_ptr cameraRig(new Cameras()); cameraRig->push_back(Camera(Pose3::Identity(), sharedK)); SmartFactorRS::shared_ptr smartFactor1( @@ -987,7 +987,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, // ==== check Hessian of smartFactor1 ===== // -- compute actual Hessian - boost::shared_ptr linearfactor1 = + std::shared_ptr linearfactor1 = smartFactor1->linearize(values); Matrix actualHessian = linearfactor1->information(); @@ -1099,7 +1099,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, interp_factors.push_back(interp_factor3); interp_factors.push_back(interp_factor1); - boost::shared_ptr cameraRig(new Cameras()); + std::shared_ptr cameraRig(new Cameras()); cameraRig->push_back(Camera(Pose3::Identity(), sharedK)); SmartFactorRS::shared_ptr smartFactor1( @@ -1128,7 +1128,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, // ==== check Hessian of smartFactor1 ===== // -- compute actual Hessian - boost::shared_ptr linearfactor1 = + std::shared_ptr linearfactor1 = smartFactor1->linearize(values); Matrix actualHessian = linearfactor1->information(); @@ -1258,7 +1258,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, interp_factors_redundant.push_back( interp_factors.at(0)); // we readd the first interp factor - boost::shared_ptr cameraRig(new Cameras()); + std::shared_ptr cameraRig(new Cameras()); cameraRig->push_back(Camera(Pose3::Identity(), sharedK)); SmartFactorRS::shared_ptr smartFactor1( @@ -1347,7 +1347,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, timing) { size_t nrTests = 10000; for (size_t i = 0; i < nrTests; i++) { - boost::shared_ptr cameraRig(new Cameras()); + std::shared_ptr cameraRig(new Cameras()); cameraRig->push_back(Camera(body_P_sensorId, sharedKSimple)); SmartFactorRS::shared_ptr smartFactorRS(new SmartFactorRS( @@ -1428,7 +1428,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, gtsam::ZERO_ON_DEGENERACY); // only config that works with RS factors params.setRankTolerance(0.1); - boost::shared_ptr cameraRig(new Cameras()); + std::shared_ptr cameraRig(new Cameras()); cameraRig->push_back(Camera(Pose3::Identity(), emptyK)); SmartFactorRS_spherical::shared_ptr smartFactor1( diff --git a/gtsam_unstable/slam/tests/testSmartStereoFactor_iSAM2.cpp b/gtsam_unstable/slam/tests/testSmartStereoFactor_iSAM2.cpp index 98b18371f..62cd4cc38 100644 --- a/gtsam_unstable/slam/tests/testSmartStereoFactor_iSAM2.cpp +++ b/gtsam_unstable/slam/tests/testSmartStereoFactor_iSAM2.cpp @@ -156,7 +156,7 @@ TEST(testISAM2SmartFactor, Stereo_Batch) { using symbol_shorthand::X; const auto K = - boost::make_shared(fx, fy, .0, cx, cy, baseline); + std::make_shared(fx, fy, .0, cx, cy, baseline); // Pose prior - at identity auto priorPoseNoise = noiseModel::Diagonal::Sigmas( @@ -184,7 +184,7 @@ TEST(testISAM2SmartFactor, Stereo_Batch) { SmartProjectionParams parm(HESSIAN, ZERO_ON_DEGENERACY); smartFactors[stObs.lm_id] = - boost::make_shared(noise, parm); + std::make_shared(noise, parm); batch_graph.push_back(smartFactors[stObs.lm_id]); } @@ -242,7 +242,7 @@ TEST(testISAM2SmartFactor, Stereo_iSAM2) { using symbol_shorthand::X; const auto K = - boost::make_shared(fx, fy, .0, cx, cy, baseline); + std::make_shared(fx, fy, .0, cx, cy, baseline); ISAM2Params parameters; parameters.relinearizeThreshold = 0.01; @@ -290,7 +290,7 @@ TEST(testISAM2SmartFactor, Stereo_iSAM2) { SmartProjectionParams params(HESSIAN, ZERO_ON_DEGENERACY); smartFactors[stObs.lm_id] = - boost::make_shared(noise, params); + std::make_shared(noise, params); newFactor2lm[newFactors.size()] = stObs.lm_id; newFactors.push_back(smartFactors[stObs.lm_id]); } else { diff --git a/gtsam_unstable/slam/tests/testSmartStereoProjectionFactorPP.cpp b/gtsam_unstable/slam/tests/testSmartStereoProjectionFactorPP.cpp index 095bedfab..b4e0e65f3 100644 --- a/gtsam_unstable/slam/tests/testSmartStereoProjectionFactorPP.cpp +++ b/gtsam_unstable/slam/tests/testSmartStereoProjectionFactorPP.cpp @@ -348,7 +348,7 @@ TEST( SmartStereoProjectionFactorPP, noisy_error_multipleExtrinsics ) { measurements.push_back(cam1_uv); measurements.push_back(cam2_uv); - vector > Ks; ///< shared pointer to calibration object (one for each camera) + vector > Ks; ///< shared pointer to calibration object (one for each camera) Ks.push_back(K); Ks.push_back(K); diff --git a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp index 9f61d4fb0..1eceb8061 100644 --- a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp @@ -269,7 +269,7 @@ TEST( SmartStereoProjectionPoseFactor, noisy ) { measurements.push_back(level_uv); measurements.push_back(level_uv_right); - vector > Ks; ///< shared pointer to calibration object (one for each camera) + vector > Ks; ///< shared pointer to calibration object (one for each camera) Ks.push_back(K); Ks.push_back(K); @@ -1096,17 +1096,17 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) { values.insert(x3, pose3 * noise_pose); // TODO: next line throws Cheirality exception on Mac - boost::shared_ptr hessianFactor1 = smartFactor1->linearize( + std::shared_ptr hessianFactor1 = smartFactor1->linearize( values); - boost::shared_ptr hessianFactor2 = smartFactor2->linearize( + std::shared_ptr hessianFactor2 = smartFactor2->linearize( values); - boost::shared_ptr hessianFactor3 = smartFactor3->linearize( + std::shared_ptr hessianFactor3 = smartFactor3->linearize( values); Matrix CumulativeInformation = hessianFactor1->information() + hessianFactor2->information() + hessianFactor3->information(); - boost::shared_ptr GaussianGraph = graph.linearize( + std::shared_ptr GaussianGraph = graph.linearize( values); Matrix GraphInformation = GaussianGraph->hessian().first; @@ -1297,7 +1297,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) { // values.insert(x1, pose1); // values.insert(x2, pose2); // -// boost::shared_ptr hessianFactor = smartFactor1->linearize(values); +// std::shared_ptr hessianFactor = smartFactor1->linearize(values); // // // compute triangulation from linearization point // // compute reprojection errors (sum squared) @@ -1338,7 +1338,7 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ) { values.insert(x2, pose2); values.insert(x3, pose3); - boost::shared_ptr hessianFactor = + std::shared_ptr hessianFactor = smartFactorInstance->linearize(values); // hessianFactor->print("Hessian factor \n"); @@ -1349,7 +1349,7 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ) { rotValues.insert(x2, poseDrift.compose(pose2)); rotValues.insert(x3, poseDrift.compose(pose3)); - boost::shared_ptr hessianFactorRot = + std::shared_ptr hessianFactorRot = smartFactorInstance->linearize(rotValues); // hessianFactorRot->print("Hessian factor \n"); @@ -1366,7 +1366,7 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ) { tranValues.insert(x2, poseDrift2.compose(pose2)); tranValues.insert(x3, poseDrift2.compose(pose3)); - boost::shared_ptr hessianFactorRotTran = + std::shared_ptr hessianFactorRotTran = smartFactorInstance->linearize(tranValues); // Hessian is invariant to rotations and translations in the nondegenerate case @@ -1406,7 +1406,7 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationNonDegenerate ) { values.insert(x2, pose2); values.insert(x3, pose3); - boost::shared_ptr hessianFactor = smartFactor->linearize( + std::shared_ptr hessianFactor = smartFactor->linearize( values); // check that it is non degenerate @@ -1419,7 +1419,7 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationNonDegenerate ) { rotValues.insert(x2, poseDrift.compose(pose2)); rotValues.insert(x3, poseDrift.compose(pose3)); - boost::shared_ptr hessianFactorRot = smartFactor->linearize( + std::shared_ptr hessianFactorRot = smartFactor->linearize( rotValues); // check that it is non degenerate @@ -1438,7 +1438,7 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationNonDegenerate ) { tranValues.insert(x2, poseDrift2.compose(pose2)); tranValues.insert(x3, poseDrift2.compose(pose3)); - boost::shared_ptr hessianFactorRotTran = + std::shared_ptr hessianFactorRotTran = smartFactor->linearize(tranValues); // Hessian is invariant to rotations and translations in the degenerate case diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 79a27f17f..b79c5f9ea 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -86,7 +86,7 @@ pybind_wrap(${GTSAM_PYTHON_TARGET} # target ${PROJECT_SOURCE_DIR}/python/gtsam/gtsam.tpl gtsam # libs "gtsam;gtsam_header" # dependencies - ON # use_boost + OFF # use_boost ) set_target_properties(${GTSAM_PYTHON_TARGET} PROPERTIES @@ -166,7 +166,7 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON) ${PROJECT_SOURCE_DIR}/python/gtsam_unstable/gtsam_unstable.tpl gtsam_unstable # libs "gtsam_unstable;gtsam_unstable_header" # dependencies - ON # use_boost + OFF # use_boost ) set_target_properties(${GTSAM_PYTHON_UNSTABLE_TARGET} PROPERTIES diff --git a/python/gtsam/preamble/slam.h b/python/gtsam/preamble/slam.h index f7bf5863c..6788bd3c9 100644 --- a/python/gtsam/preamble/slam.h +++ b/python/gtsam/preamble/slam.h @@ -12,7 +12,7 @@ */ PYBIND11_MAKE_OPAQUE( - std::vector > >); + std::vector > >); PYBIND11_MAKE_OPAQUE( - std::vector > >); + std::vector > >); PYBIND11_MAKE_OPAQUE(gtsam::Rot3Vector); diff --git a/python/gtsam/specializations/slam.h b/python/gtsam/specializations/slam.h index 6a439c370..af579d9df 100644 --- a/python/gtsam/specializations/slam.h +++ b/python/gtsam/specializations/slam.h @@ -12,9 +12,9 @@ */ py::bind_vector< - std::vector>>>( + std::vector>>>( m_, "BetweenFactorPose3s"); py::bind_vector< - std::vector>>>( + std::vector>>>( m_, "BetweenFactorPose2s"); py::bind_vector(m_, "Rot3Vector"); diff --git a/tests/simulated2D.h b/tests/simulated2D.h index e1f0c5116..5334cbf45 100644 --- a/tests/simulated2D.h +++ b/tests/simulated2D.h @@ -40,7 +40,7 @@ namespace simulated2D { public: typedef gtsam::Values Base; ///< base class - typedef boost::shared_ptr sharedPoint; ///< shortcut to shared Point type + typedef std::shared_ptr sharedPoint; ///< shortcut to shared Point type /// Constructor Values() : nrPoses_(0), nrPoints_(0) { @@ -130,7 +130,7 @@ namespace simulated2D { public: typedef NoiseModelFactorN Base; ///< base class typedef GenericPrior This; - typedef boost::shared_ptr > shared_ptr; + typedef std::shared_ptr > shared_ptr; typedef VALUE Pose; ///< shortcut to Pose type // Provide access to the Matrix& version of evaluateError: @@ -152,7 +152,7 @@ namespace simulated2D { /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } private: @@ -177,7 +177,7 @@ namespace simulated2D { public: typedef NoiseModelFactorN Base; ///< base class typedef GenericOdometry This; - typedef boost::shared_ptr > shared_ptr; + typedef std::shared_ptr > shared_ptr; typedef VALUE Pose; ///< shortcut to Pose type // Provide access to the Matrix& version of evaluateError: @@ -200,7 +200,7 @@ namespace simulated2D { /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } private: @@ -225,7 +225,7 @@ namespace simulated2D { public: typedef NoiseModelFactorN Base; ///< base class typedef GenericMeasurement This; - typedef boost::shared_ptr > shared_ptr; + typedef std::shared_ptr > shared_ptr; typedef POSE Pose; ///< shortcut to Pose type typedef LANDMARK Landmark; ///< shortcut to Landmark type @@ -249,7 +249,7 @@ namespace simulated2D { /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } private: diff --git a/tests/simulated2DConstraints.h b/tests/simulated2DConstraints.h index fad7e0c39..ae626d1fa 100644 --- a/tests/simulated2DConstraints.h +++ b/tests/simulated2DConstraints.h @@ -55,14 +55,14 @@ namespace simulated2D { struct ScalarCoordConstraint1: public BoundingConstraint1 { typedef BoundingConstraint1 Base; ///< Base class convenience typedef typedef ScalarCoordConstraint1 This; ///< This class convenience typedef - typedef boost::shared_ptr > shared_ptr; ///< boost::shared_ptr convenience typedef + typedef std::shared_ptr > shared_ptr; ///< std::shared_ptr convenience typedef typedef VALUE Point; ///< Constrained variable type ~ScalarCoordConstraint1() override {} /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } /** @@ -130,7 +130,7 @@ namespace simulated2D { /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } /** @@ -179,7 +179,7 @@ namespace simulated2D { /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } /** diff --git a/tests/simulated2DOriented.h b/tests/simulated2DOriented.h index 983432728..8dae71fd3 100644 --- a/tests/simulated2DOriented.h +++ b/tests/simulated2DOriented.h @@ -128,7 +128,7 @@ namespace simulated2DOriented { /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } }; diff --git a/tests/smallExample.h b/tests/smallExample.h index 6311999e5..08341245c 100644 --- a/tests/smallExample.h +++ b/tests/smallExample.h @@ -33,7 +33,7 @@ namespace example { /** * Create small example for non-linear factor graph */ -// inline boost::shared_ptr sharedNonlinearFactorGraph(); +// inline std::shared_ptr sharedNonlinearFactorGraph(); // inline NonlinearFactorGraph createNonlinearFactorGraph(); /** @@ -48,7 +48,7 @@ namespace example { /** * create a noisy values structure for a nonlinear factor graph */ -// inline boost::shared_ptr sharedNoisyValues(); +// inline std::shared_ptr sharedNoisyValues(); // inline Values createNoisyValues(); /** @@ -75,7 +75,7 @@ namespace example { /** * Create really non-linear factor graph (cos/sin) */ -// inline boost::shared_ptr +// inline std::shared_ptr //sharedReallyNonlinearFactorGraph(); // inline NonlinearFactorGraph createReallyNonlinearFactorGraph(); @@ -156,7 +156,7 @@ namespace example { // using namespace gtsam::noiseModel; namespace impl { -typedef boost::shared_ptr shared_nlf; +typedef std::shared_ptr shared_nlf; static SharedDiagonal kSigma1_0 = noiseModel::Isotropic::Sigma(2,1.0); static SharedDiagonal kSigma0_1 = noiseModel::Isotropic::Sigma(2,0.1); @@ -169,14 +169,14 @@ static const Key _x_=0, _y_=1, _z_=2; /* ************************************************************************* */ -inline boost::shared_ptr +inline std::shared_ptr sharedNonlinearFactorGraph(const SharedNoiseModel &noiseModel1 = impl::kSigma0_1, const SharedNoiseModel &noiseModel2 = impl::kSigma0_2) { using namespace impl; using symbol_shorthand::L; using symbol_shorthand::X; // Create - boost::shared_ptr nlfg(new NonlinearFactorGraph); + std::shared_ptr nlfg(new NonlinearFactorGraph); // prior on x1 Point2 mu(0, 0); @@ -229,10 +229,10 @@ inline VectorValues createVectorValues() { } /* ************************************************************************* */ -inline boost::shared_ptr sharedNoisyValues() { +inline std::shared_ptr sharedNoisyValues() { using symbol_shorthand::X; using symbol_shorthand::L; - boost::shared_ptr c(new Values); + std::shared_ptr c(new Values); c->insert(X(1), Point2(0.1, 0.1)); c->insert(X(2), Point2(1.4, 0.2)); c->insert(L(1), Point2(0.1, -1.1)); @@ -344,7 +344,7 @@ struct UnaryFactor: public gtsam::NoiseModelFactorN { } gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new UnaryFactor(*this))); } }; @@ -354,22 +354,22 @@ struct UnaryFactor: public gtsam::NoiseModelFactorN { inline NonlinearFactorGraph nonlinearFactorGraphWithGivenSigma(const double sigma) { using symbol_shorthand::X; using symbol_shorthand::L; - boost::shared_ptr fg(new NonlinearFactorGraph); + std::shared_ptr fg(new NonlinearFactorGraph); Point2 z(1.0, 0.0); - boost::shared_ptr factor( + std::shared_ptr factor( new smallOptimize::UnaryFactor(z, noiseModel::Isotropic::Sigma(2,sigma), X(1))); fg->push_back(factor); return *fg; } /* ************************************************************************* */ -inline boost::shared_ptr sharedReallyNonlinearFactorGraph() { +inline std::shared_ptr sharedReallyNonlinearFactorGraph() { using symbol_shorthand::X; using symbol_shorthand::L; - boost::shared_ptr fg(new NonlinearFactorGraph); + std::shared_ptr fg(new NonlinearFactorGraph); Point2 z(1.0, 0.0); double sigma = 0.1; - boost::shared_ptr factor( + std::shared_ptr factor( new smallOptimize::UnaryFactor(z, noiseModel::Isotropic::Sigma(2,sigma), X(1))); fg->push_back(factor); return fg; @@ -382,11 +382,11 @@ inline NonlinearFactorGraph createReallyNonlinearFactorGraph() { /* ************************************************************************* */ inline NonlinearFactorGraph sharedNonRobustFactorGraphWithOutliers() { using symbol_shorthand::X; - boost::shared_ptr fg(new NonlinearFactorGraph); + std::shared_ptr fg(new NonlinearFactorGraph); Point2 z(0.0, 0.0); double sigma = 0.1; - boost::shared_ptr> factor( + std::shared_ptr> factor( new PriorFactor(X(1), z, noiseModel::Isotropic::Sigma(2,sigma))); // 3 noiseless inliers fg->push_back(factor); @@ -395,7 +395,7 @@ inline NonlinearFactorGraph sharedNonRobustFactorGraphWithOutliers() { // 1 outlier Point2 z_out(1.0, 0.0); - boost::shared_ptr> factor_out( + std::shared_ptr> factor_out( new PriorFactor(X(1), z_out, noiseModel::Isotropic::Sigma(2,sigma))); fg->push_back(factor_out); @@ -405,12 +405,12 @@ inline NonlinearFactorGraph sharedNonRobustFactorGraphWithOutliers() { /* ************************************************************************* */ inline NonlinearFactorGraph sharedRobustFactorGraphWithOutliers() { using symbol_shorthand::X; - boost::shared_ptr fg(new NonlinearFactorGraph); + std::shared_ptr fg(new NonlinearFactorGraph); Point2 z(0.0, 0.0); double sigma = 0.1; auto gmNoise = noiseModel::Robust::Create( noiseModel::mEstimator::GemanMcClure::Create(1.0), noiseModel::Isotropic::Sigma(2,sigma)); - boost::shared_ptr> factor( + std::shared_ptr> factor( new PriorFactor(X(1), z, gmNoise)); // 3 noiseless inliers fg->push_back(factor); @@ -419,7 +419,7 @@ inline NonlinearFactorGraph sharedRobustFactorGraphWithOutliers() { // 1 outlier Point2 z_out(1.0, 0.0); - boost::shared_ptr> factor_out( + std::shared_ptr> factor_out( new PriorFactor(X(1), z_out, gmNoise)); fg->push_back(factor_out); @@ -663,7 +663,7 @@ inline std::pair planarGraph(size_t N) { xtrue.insert(key(x, y), Point2((double)x, (double)y)); // linearize around zero - boost::shared_ptr gfg = nlfg.linearize(zeros); + std::shared_ptr gfg = nlfg.linearize(zeros); return std::make_pair(*gfg, xtrue); } diff --git a/tests/testExpressionFactor.cpp b/tests/testExpressionFactor.cpp index 5a3678392..4f4891d8a 100644 --- a/tests/testExpressionFactor.cpp +++ b/tests/testExpressionFactor.cpp @@ -65,7 +65,7 @@ TEST(ExpressionFactor, Leaf) { // Check values and derivatives. EXPECT_DOUBLES_EQUAL(old.error(values), f.error(values), 1e-9); EXPECT_LONGS_EQUAL(2, f.dim()); - boost::shared_ptr gf2 = f.linearize(values); + std::shared_ptr gf2 = f.linearize(values); EXPECT(assert_equal(*old.linearize(values), *gf2, 1e-9)); } @@ -85,7 +85,7 @@ TEST(ExpressionFactor, Model) { // Check values and derivatives. EXPECT_DOUBLES_EQUAL(old.error(values), f.error(values), 1e-9); EXPECT_LONGS_EQUAL(2, f.dim()); - boost::shared_ptr gf2 = f.linearize(values); + std::shared_ptr gf2 = f.linearize(values); EXPECT(assert_equal(*old.linearize(values), *gf2, 1e-9)); EXPECT_CORRECT_FACTOR_JACOBIANS(f, values, 1e-5, 1e-5); // another way } @@ -104,7 +104,7 @@ TEST(ExpressionFactor, Constrained) { ExpressionFactor f(model, Point2(0, 0), p); EXPECT_DOUBLES_EQUAL(old.error(values), f.error(values), 1e-9); EXPECT_LONGS_EQUAL(2, f.dim()); - boost::shared_ptr gf2 = f.linearize(values); + std::shared_ptr gf2 = f.linearize(values); EXPECT(assert_equal(*old.linearize(values), *gf2, 1e-9)); } @@ -126,9 +126,9 @@ TEST(ExpressionFactor, Unary) { // Concise version ExpressionFactor f(model, measured, project(p)); EXPECT_LONGS_EQUAL(2, f.dim()); - boost::shared_ptr gf = f.linearize(values); - boost::shared_ptr jf = // - boost::dynamic_pointer_cast(gf); + std::shared_ptr gf = f.linearize(values); + std::shared_ptr jf = // + std::dynamic_pointer_cast(gf); EXPECT(assert_equal(expected, *jf, 1e-9)); } @@ -221,7 +221,7 @@ TEST(ExpressionFactor, Shallow) { // Create old-style factor to create expected value and derivatives GenericProjectionFactor old(measured, model, 1, 2, - boost::make_shared()); + std::make_shared()); double expected_error = old.error(values); GaussianFactor::shared_ptr expected = old.linearize(values); @@ -265,7 +265,7 @@ TEST(ExpressionFactor, Shallow) { ExpressionFactor f2(model, measured, expression); EXPECT_DOUBLES_EQUAL(expected_error, f2.error(values), 1e-9); EXPECT_LONGS_EQUAL(2, f2.dim()); - boost::shared_ptr gf2 = f2.linearize(values); + std::shared_ptr gf2 = f2.linearize(values); EXPECT(assert_equal(*expected, *gf2, 1e-9)); } @@ -298,7 +298,7 @@ TEST(ExpressionFactor, tree) { ExpressionFactor f(model, measured, uv_hat); EXPECT_DOUBLES_EQUAL(expected_error, f.error(values), 1e-9); EXPECT_LONGS_EQUAL(2, f.dim()); - boost::shared_ptr gf = f.linearize(values); + std::shared_ptr gf = f.linearize(values); EXPECT(assert_equal(*expected, *gf, 1e-9)); // Concise version @@ -306,14 +306,14 @@ TEST(ExpressionFactor, tree) { uncalibrate(K, project(transformTo(x, p)))); EXPECT_DOUBLES_EQUAL(expected_error, f2.error(values), 1e-9); EXPECT_LONGS_EQUAL(2, f2.dim()); - boost::shared_ptr gf2 = f2.linearize(values); + std::shared_ptr gf2 = f2.linearize(values); EXPECT(assert_equal(*expected, *gf2, 1e-9)); // Try ternary version ExpressionFactor f3(model, measured, project3(x, p, K)); EXPECT_DOUBLES_EQUAL(expected_error, f3.error(values), 1e-9); EXPECT_LONGS_EQUAL(2, f3.dim()); - boost::shared_ptr gf3 = f3.linearize(values); + std::shared_ptr gf3 = f3.linearize(values); EXPECT(assert_equal(*expected, *gf3, 1e-9)); } @@ -340,9 +340,9 @@ TEST(ExpressionFactor, Compose1) { // Check linearization JacobianFactor expected(1, I_3x3, 2, I_3x3, Z_3x1); - boost::shared_ptr gf = f.linearize(values); - boost::shared_ptr jf = // - boost::dynamic_pointer_cast(gf); + std::shared_ptr gf = f.linearize(values); + std::shared_ptr jf = // + std::dynamic_pointer_cast(gf); EXPECT(assert_equal(expected, *jf,1e-9)); } @@ -369,9 +369,9 @@ TEST(ExpressionFactor, compose2) { // Check linearization JacobianFactor expected(1, 2 * I_3x3, Z_3x1); - boost::shared_ptr gf = f.linearize(values); - boost::shared_ptr jf = // - boost::dynamic_pointer_cast(gf); + std::shared_ptr gf = f.linearize(values); + std::shared_ptr jf = // + std::dynamic_pointer_cast(gf); EXPECT(assert_equal(expected, *jf,1e-9)); } @@ -398,9 +398,9 @@ TEST(ExpressionFactor, compose3) { // Check linearization JacobianFactor expected(3, I_3x3, Z_3x1); - boost::shared_ptr gf = f.linearize(values); - boost::shared_ptr jf = // - boost::dynamic_pointer_cast(gf); + std::shared_ptr gf = f.linearize(values); + std::shared_ptr jf = // + std::dynamic_pointer_cast(gf); EXPECT(assert_equal(expected, *jf,1e-9)); } @@ -443,9 +443,9 @@ TEST(ExpressionFactor, composeTernary) { // Check linearization JacobianFactor expected(1, I_3x3, 2, I_3x3, 3, I_3x3, Z_3x1); - boost::shared_ptr gf = f.linearize(values); - boost::shared_ptr jf = // - boost::dynamic_pointer_cast(gf); + std::shared_ptr gf = f.linearize(values); + std::shared_ptr jf = // + std::dynamic_pointer_cast(gf); EXPECT(assert_equal(expected, *jf,1e-9)); } @@ -659,7 +659,7 @@ public: /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } diff --git a/tests/testExtendedKalmanFilter.cpp b/tests/testExtendedKalmanFilter.cpp index e9747e624..3fce79754 100644 --- a/tests/testExtendedKalmanFilter.cpp +++ b/tests/testExtendedKalmanFilter.cpp @@ -193,7 +193,7 @@ public: * Ax-b \approx h(x1+dx1,x2+dx2)-z = h(x1,x2) + A2*dx1 + A2*dx2 - z * Hence b = z - h(x1,x2) = - error_vector(x) */ - boost::shared_ptr linearize(const Values& c) const override { + std::shared_ptr linearize(const Values& c) const override { using X1 = ValueType<1>; using X2 = ValueType<2>; const X1& x1 = c.at(key<1>()); @@ -201,7 +201,7 @@ public: Matrix A1, A2; Vector b = -evaluateError(x1, x2, A1, A2); SharedDiagonal constrained = - boost::dynamic_pointer_cast(this->noiseModel_); + std::dynamic_pointer_cast(this->noiseModel_); if (constrained.get() != nullptr) { return JacobianFactor::shared_ptr(new JacobianFactor(key<1>(), A1, key<2>(), A2, b, constrained)); @@ -328,13 +328,13 @@ public: * Ax-b \approx h(x1+dx1)-z = h(x1) + A1*dx1 - z * Hence b = z - h(x1) = - error_vector(x) */ - boost::shared_ptr linearize(const Values& c) const override { + std::shared_ptr linearize(const Values& c) const override { using X = ValueType<1>; const X& x1 = c.at(key()); Matrix A1; Vector b = -evaluateError(x1, A1); SharedDiagonal constrained = - boost::dynamic_pointer_cast(this->noiseModel_); + std::dynamic_pointer_cast(this->noiseModel_); if (constrained.get() != nullptr) { return JacobianFactor::shared_ptr(new JacobianFactor(key(), A1, b, constrained)); } diff --git a/tests/testGaussianISAM.cpp b/tests/testGaussianISAM.cpp index 1c1ce90d5..63cf654e5 100644 --- a/tests/testGaussianISAM.cpp +++ b/tests/testGaussianISAM.cpp @@ -43,7 +43,7 @@ TEST( ISAM, iSAM_smoother ) // run iSAM for every factor GaussianISAM actual; - for(boost::shared_ptr factor: smoother) { + for(std::shared_ptr factor: smoother) { GaussianFactorGraph factorGraph; factorGraph.push_back(factor); actual.update(factorGraph); diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index 1275ce33d..8c4238e2f 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -29,7 +29,7 @@ namespace br { using namespace boost::adaptors; using namespace boost::range; } using namespace std; using namespace gtsam; -using boost::shared_ptr; +using std::shared_ptr; static const SharedNoiseModel model; diff --git a/tests/testGaussianJunctionTreeB.cpp b/tests/testGaussianJunctionTreeB.cpp index 681554645..2740babd0 100644 --- a/tests/testGaussianJunctionTreeB.cpp +++ b/tests/testGaussianJunctionTreeB.cpp @@ -250,14 +250,14 @@ TEST( GaussianJunctionTreeB, constructor2 ) { // // // Compute marginal directly from marginal factor // GaussianFactor::shared_ptr marginalFactor = GaussianMultifrontalSolver(gfg).marginalFactor(1); -// JacobianFactor::shared_ptr marginalJacobian = boost::dynamic_pointer_cast(marginalFactor); +// JacobianFactor::shared_ptr marginalJacobian = std::dynamic_pointer_cast(marginalFactor); // Matrix actual2 = inverse(marginalJacobian->getA(marginalJacobian->begin()).transpose() * marginalJacobian->getA(marginalJacobian->begin())); // // // Compute marginal directly from BayesTree // GaussianBayesTree gbt; // gbt.insert(GaussianJunctionTree(gfg).eliminate(EliminateCholesky)); // marginalFactor = gbt.marginalFactor(1, EliminateCholesky); -// marginalJacobian = boost::dynamic_pointer_cast(marginalFactor); +// marginalJacobian = std::dynamic_pointer_cast(marginalFactor); // Matrix actual3 = inverse(marginalJacobian->getA(marginalJacobian->begin()).transpose() * marginalJacobian->getA(marginalJacobian->begin())); // // EXPECT(assert_equal(expected, actual1)); diff --git a/tests/testGradientDescentOptimizer.cpp b/tests/testGradientDescentOptimizer.cpp index a44a28273..83b0f9e8a 100644 --- a/tests/testGradientDescentOptimizer.cpp +++ b/tests/testGradientDescentOptimizer.cpp @@ -20,8 +20,6 @@ #include -#include -#include #include using namespace std; diff --git a/tests/testGraph.cpp b/tests/testGraph.cpp index 64c86c4c4..0d2a25fbd 100644 --- a/tests/testGraph.cpp +++ b/tests/testGraph.cpp @@ -26,7 +26,7 @@ #include -#include +#include #include @@ -93,7 +93,7 @@ TEST( Graph, composePoses ) Pose2 rootPose = p2; - boost::shared_ptr actual = composePoses, Pose2, Key> (graph, tree, rootPose); + std::shared_ptr actual = composePoses, Pose2, Key> (graph, tree, rootPose); Values expected; expected.insert(1, p1); diff --git a/tests/testIterative.cpp b/tests/testIterative.cpp index 9d6e58ac7..5519a7f04 100644 --- a/tests/testIterative.cpp +++ b/tests/testIterative.cpp @@ -92,7 +92,7 @@ TEST( Iterative, conjugateGradientDescent_hard_constraint ) graph += NonlinearEquality(X(1), pose1); graph += BetweenFactor(X(1),X(2), Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1)); - boost::shared_ptr fg = graph.linearize(config); + std::shared_ptr fg = graph.linearize(config); VectorValues zeros = config.zeroVectors(); @@ -119,7 +119,7 @@ TEST( Iterative, conjugateGradientDescent_soft_constraint ) graph.addPrior(X(1), Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3, 1e-10)); graph += BetweenFactor(X(1),X(2), Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1)); - boost::shared_ptr fg = graph.linearize(config); + std::shared_ptr fg = graph.linearize(config); VectorValues zeros = config.zeroVectors(); diff --git a/tests/testNonlinearEquality.cpp b/tests/testNonlinearEquality.cpp index 9d91a344b..efc0020a5 100644 --- a/tests/testNonlinearEquality.cpp +++ b/tests/testNonlinearEquality.cpp @@ -41,7 +41,7 @@ static const double tol = 1e-5; typedef PriorFactor PosePrior; typedef NonlinearEquality PoseNLE; -typedef boost::shared_ptr shared_poseNLE; +typedef std::shared_ptr shared_poseNLE; static Symbol key('x', 1); @@ -521,7 +521,7 @@ TEST (testNonlinearEqualityConstraint, stereo_constrained ) { static double fov = 60; // degrees static int w = 640, h = 480; static Cal3_S2 K(fov, w, h); - static boost::shared_ptr shK(new Cal3_S2(K)); + static std::shared_ptr shK(new Cal3_S2(K)); // create initial estimates Rot3 faceTowardsY(Point3(1, 0, 0), Point3(0, 0, -1), Point3(0, 1, 0)); diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp index c0bee0aaa..06ae3366c 100644 --- a/tests/testNonlinearFactor.cpp +++ b/tests/testNonlinearFactor.cpp @@ -41,7 +41,7 @@ using namespace example; using symbol_shorthand::X; using symbol_shorthand::L; -typedef boost::shared_ptr shared_nlf; +typedef std::shared_ptr shared_nlf; /* ************************************************************************* */ TEST( NonlinearFactor, equals ) @@ -90,7 +90,7 @@ TEST( NonlinearFactor, NonlinearFactor ) // calculate the error_vector from the factor "f1" // error_vector = [0.1 0.1] - Vector actual_e = boost::dynamic_pointer_cast(factor)->unwhitenedError(cfg); + Vector actual_e = std::dynamic_pointer_cast(factor)->unwhitenedError(cfg); CHECK(assert_equal(0.1*Vector::Ones(2),actual_e)); // error = 0.5 * [1 1] * [1;1] = 1 @@ -323,7 +323,7 @@ TEST( NonlinearFactor, cloneWithNewNoiseModel ) // create actual NonlinearFactorGraph actual; SharedNoiseModel noise2 = noiseModel::Isotropic::Sigma(2,sigma2); - actual.push_back( boost::dynamic_pointer_cast(nfg[0])->cloneWithNewNoiseModel(noise2) ); + actual.push_back( std::dynamic_pointer_cast(nfg[0])->cloneWithNewNoiseModel(noise2) ); // check it's all good CHECK(assert_equal(expected, actual)); @@ -352,7 +352,7 @@ class TestFactor1 : public NoiseModelFactor1 { } gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new TestFactor1(*this))); } }; @@ -365,7 +365,7 @@ TEST(NonlinearFactor, NoiseModelFactor1) { EXPECT(assert_equal((Vector(1) << 1.0).finished(), tf.unwhitenedError(tv))); DOUBLES_EQUAL(0.25 / 2.0, tf.error(tv), 1e-9); JacobianFactor jf( - *boost::dynamic_pointer_cast(tf.linearize(tv))); + *std::dynamic_pointer_cast(tf.linearize(tv))); LONGS_EQUAL((long)L(1), (long)jf.keys()[0]); EXPECT(assert_equal((Matrix)(Matrix(1, 1) << 0.5).finished(), jf.getA(jf.begin()))); @@ -417,7 +417,7 @@ class TestFactor4 : public NoiseModelFactor4 { } gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new TestFactor4(*this))); } }; @@ -431,7 +431,7 @@ TEST(NonlinearFactor, NoiseModelFactor4) { tv.insert(X(4), double((4.0))); EXPECT(assert_equal((Vector(1) << 30.0).finished(), tf.unwhitenedError(tv))); DOUBLES_EQUAL(0.5 * 30.0 * 30.0 / 4.0, tf.error(tv), 1e-9); - JacobianFactor jf(*boost::dynamic_pointer_cast(tf.linearize(tv))); + JacobianFactor jf(*std::dynamic_pointer_cast(tf.linearize(tv))); LONGS_EQUAL((long)X(1), (long)jf.keys()[0]); LONGS_EQUAL((long)X(2), (long)jf.keys()[1]); LONGS_EQUAL((long)X(3), (long)jf.keys()[2]); @@ -522,7 +522,7 @@ TEST(NonlinearFactor, NoiseModelFactor5) { tv.insert(X(5), double((5.0))); EXPECT(assert_equal((Vector(1) << 55.0).finished(), tf.unwhitenedError(tv))); DOUBLES_EQUAL(0.5 * 55.0 * 55.0 / 4.0, tf.error(tv), 1e-9); - JacobianFactor jf(*boost::dynamic_pointer_cast(tf.linearize(tv))); + JacobianFactor jf(*std::dynamic_pointer_cast(tf.linearize(tv))); LONGS_EQUAL((long)X(1), (long)jf.keys()[0]); LONGS_EQUAL((long)X(2), (long)jf.keys()[1]); LONGS_EQUAL((long)X(3), (long)jf.keys()[2]); @@ -577,7 +577,7 @@ TEST(NonlinearFactor, NoiseModelFactor6) { tv.insert(X(6), double((6.0))); EXPECT(assert_equal((Vector(1) << 91.0).finished(), tf.unwhitenedError(tv))); DOUBLES_EQUAL(0.5 * 91.0 * 91.0 / 4.0, tf.error(tv), 1e-9); - JacobianFactor jf(*boost::dynamic_pointer_cast(tf.linearize(tv))); + JacobianFactor jf(*std::dynamic_pointer_cast(tf.linearize(tv))); LONGS_EQUAL((long)X(1), (long)jf.keys()[0]); LONGS_EQUAL((long)X(2), (long)jf.keys()[1]); LONGS_EQUAL((long)X(3), (long)jf.keys()[2]); @@ -630,7 +630,7 @@ TEST(NonlinearFactor, NoiseModelFactorN) { tv.insert(X(4), double((4.0))); EXPECT(assert_equal((Vector(1) << 30.0).finished(), tf.unwhitenedError(tv))); DOUBLES_EQUAL(0.5 * 30.0 * 30.0 / 4.0, tf.error(tv), 1e-9); - JacobianFactor jf(*boost::dynamic_pointer_cast(tf.linearize(tv))); + JacobianFactor jf(*std::dynamic_pointer_cast(tf.linearize(tv))); LONGS_EQUAL((long)X(1), (long)jf.keys()[0]); LONGS_EQUAL((long)X(2), (long)jf.keys()[1]); LONGS_EQUAL((long)X(3), (long)jf.keys()[2]); diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index cc82892f7..c15de4f5d 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -32,7 +32,6 @@ #include #include -#include using boost::adaptors::map_values; #include diff --git a/tests/testPCGSolver.cpp b/tests/testPCGSolver.cpp index 558f7c1e4..9497c00d7 100644 --- a/tests/testPCGSolver.cpp +++ b/tests/testPCGSolver.cpp @@ -26,8 +26,6 @@ #include -#include - #include #include @@ -127,8 +125,8 @@ TEST( GaussianFactorGraphSystem, multiply_getb) TEST(PCGSolver, dummy) { LevenbergMarquardtParams params; params.linearSolverType = LevenbergMarquardtParams::Iterative; - auto pcg = boost::make_shared(); - pcg->preconditioner_ = boost::make_shared(); + auto pcg = std::make_shared(); + pcg->preconditioner_ = std::make_shared(); params.iterativeParams = pcg; NonlinearFactorGraph fg = example::createReallyNonlinearFactorGraph(); @@ -147,9 +145,9 @@ TEST(PCGSolver, dummy) { TEST(PCGSolver, blockjacobi) { LevenbergMarquardtParams params; params.linearSolverType = LevenbergMarquardtParams::Iterative; - auto pcg = boost::make_shared(); + auto pcg = std::make_shared(); pcg->preconditioner_ = - boost::make_shared(); + std::make_shared(); params.iterativeParams = pcg; NonlinearFactorGraph fg = example::createReallyNonlinearFactorGraph(); @@ -168,8 +166,8 @@ TEST(PCGSolver, blockjacobi) { TEST(PCGSolver, subgraph) { LevenbergMarquardtParams params; params.linearSolverType = LevenbergMarquardtParams::Iterative; - auto pcg = boost::make_shared(); - pcg->preconditioner_ = boost::make_shared(); + auto pcg = std::make_shared(); + pcg->preconditioner_ = std::make_shared(); params.iterativeParams = pcg; NonlinearFactorGraph fg = example::createReallyNonlinearFactorGraph(); diff --git a/tests/testPreconditioner.cpp b/tests/testPreconditioner.cpp index f00916475..8e3c6dcc5 100644 --- a/tests/testPreconditioner.cpp +++ b/tests/testPreconditioner.cpp @@ -53,20 +53,20 @@ TEST( PCGsolver, verySimpleLinearSystem) { // Solve the system using Preconditioned Conjugate Gradient solver // Common PCG parameters - gtsam::PCGSolverParameters::shared_ptr pcg = boost::make_shared(); + gtsam::PCGSolverParameters::shared_ptr pcg = std::make_shared(); pcg->setMaxIterations(500); pcg->setEpsilon_abs(0.0); pcg->setEpsilon_rel(0.0); //pcg->setVerbosity("ERROR"); // With Dummy preconditioner - pcg->preconditioner_ = boost::make_shared(); + pcg->preconditioner_ = std::make_shared(); VectorValues deltaPCGDummy = PCGSolver(*pcg).optimize(simpleGFG); EXPECT(assert_equal(exactSolution, deltaPCGDummy, 1e-7)); //deltaPCGDummy.print("PCG Dummy"); // With Block-Jacobi preconditioner - pcg->preconditioner_ = boost::make_shared(); + pcg->preconditioner_ = std::make_shared(); // It takes more than 1000 iterations for this test pcg->setMaxIterations(1500); VectorValues deltaPCGJacobi = PCGSolver(*pcg).optimize(simpleGFG); @@ -104,20 +104,20 @@ TEST(PCGSolver, simpleLinearSystem) { // Solve the system using Preconditioned Conjugate Gradient solver // Common PCG parameters - gtsam::PCGSolverParameters::shared_ptr pcg = boost::make_shared(); + gtsam::PCGSolverParameters::shared_ptr pcg = std::make_shared(); pcg->setMaxIterations(500); pcg->setEpsilon_abs(0.0); pcg->setEpsilon_rel(0.0); //pcg->setVerbosity("ERROR"); // With Dummy preconditioner - pcg->preconditioner_ = boost::make_shared(); + pcg->preconditioner_ = std::make_shared(); VectorValues deltaPCGDummy = PCGSolver(*pcg).optimize(simpleGFG); EXPECT(assert_equal(expectedSolution, deltaPCGDummy, 1e-5)); //deltaPCGDummy.print("PCG Dummy"); // With Block-Jacobi preconditioner - pcg->preconditioner_ = boost::make_shared(); + pcg->preconditioner_ = std::make_shared(); VectorValues deltaPCGJacobi = PCGSolver(*pcg).optimize(simpleGFG); EXPECT(assert_equal(expectedSolution, deltaPCGJacobi, 1e-5)); //deltaPCGJacobi.print("PCG Jacobi"); diff --git a/tests/testSerializationSlam.cpp b/tests/testSerializationSlam.cpp index f986d812a..e03ef015b 100644 --- a/tests/testSerializationSlam.cpp +++ b/tests/testSerializationSlam.cpp @@ -280,7 +280,7 @@ TEST (testSerializationSLAM, factors) { Cal3_S2Stereo cal3_s2stereo(1.0, 2.0, 3.0, 4.0, 5.0, 1.0); CalibratedCamera calibratedCamera(pose3); PinholeCamera simpleCamera(pose3, cal3_s2); - StereoCamera stereoCamera(pose3, boost::make_shared(cal3_s2stereo)); + StereoCamera stereoCamera(pose3, std::make_shared(cal3_s2stereo)); Symbol a01('a',1), a02('a',2), a03('a',3), a04('a',4), a05('a',5), @@ -364,14 +364,14 @@ TEST (testSerializationSLAM, factors) { BearingRangeFactor2D bearingRangeFactor2D(a08, a03, rot2, 2.0, model2); - GenericProjectionFactorCal3_S2 genericProjectionFactorCal3_S2(point2, model2, a09, a05, boost::make_shared(cal3_s2)); - GenericProjectionFactorCal3DS2 genericProjectionFactorCal3DS2(point2, model2, a09, a05, boost::make_shared(cal3ds2)); + GenericProjectionFactorCal3_S2 genericProjectionFactorCal3_S2(point2, model2, a09, a05, std::make_shared(cal3_s2)); + GenericProjectionFactorCal3DS2 genericProjectionFactorCal3DS2(point2, model2, a09, a05, std::make_shared(cal3ds2)); GeneralSFMFactorCal3_S2 generalSFMFactorCal3_S2(point2, model2, a13, a05); GeneralSFMFactor2Cal3_S2 generalSFMFactor2Cal3_S2(point2, model2, a09, a05, a10); - GenericStereoFactor3D genericStereoFactor3D(stereoPoint2, model3, a09, a05, boost::make_shared(cal3_s2stereo)); + GenericStereoFactor3D genericStereoFactor3D(stereoPoint2, model3, a09, a05, std::make_shared(cal3_s2stereo)); NonlinearFactorGraph graph; diff --git a/tests/testSimulated2DOriented.cpp b/tests/testSimulated2DOriented.cpp index 837fd1689..4168a42fc 100644 --- a/tests/testSimulated2DOriented.cpp +++ b/tests/testSimulated2DOriented.cpp @@ -64,7 +64,7 @@ TEST( simulated2DOriented, constructor ) simulated2DOriented::Values config; config.insert(X(1), Pose2(1., 0., 0.2)); config.insert(X(2), Pose2(2., 0., 0.1)); - boost::shared_ptr lf = factor.linearize(config); + std::shared_ptr lf = factor.linearize(config); } /* ************************************************************************* */ diff --git a/timing/timeAdaptAutoDiff.cpp b/timing/timeAdaptAutoDiff.cpp index d5431cee7..9924908f6 100644 --- a/timing/timeAdaptAutoDiff.cpp +++ b/timing/timeAdaptAutoDiff.cpp @@ -54,12 +54,12 @@ int main() { NonlinearFactor::shared_ptr f1, f2, f3; // Dedicated factor - f1 = boost::make_shared >(z, model, 1, 2); + f1 = std::make_shared >(z, model, 1, 2); time("GeneralSFMFactor : ", f1, values); // ExpressionFactor Point2_ expression2(camera, &Camera::project2, point); - f3 = boost::make_shared >(model, z, expression2); + f3 = std::make_shared >(model, z, expression2); time("Point2_(camera, &Camera::project, point): ", f3, values); // AdaptAutoDiff @@ -68,7 +68,7 @@ int main() { values.insert(2,Vector3(0,0,1)); typedef AdaptAutoDiff AdaptedSnavely; Expression expression(AdaptedSnavely(), Expression(1), Expression(2)); - f2 = boost::make_shared >(model, z, expression); + f2 = std::make_shared >(model, z, expression); time("Point2_(AdaptedSnavely(), camera, point): ", f2, values); return 0; diff --git a/timing/timeCameraExpression.cpp b/timing/timeCameraExpression.cpp index 7a492822d..af7d07349 100644 --- a/timing/timeCameraExpression.cpp +++ b/timing/timeCameraExpression.cpp @@ -29,7 +29,7 @@ using namespace gtsam; #define time timeSingleThreaded -boost::shared_ptr fixedK(new Cal3_S2()); +std::shared_ptr fixedK(new Cal3_S2()); Point2 myProject(const Pose3& pose, const Point3& point, OptionalJacobian<2,6> H1, OptionalJacobian<2,3> H2) { @@ -60,14 +60,14 @@ int main() { // Oct 3, 2014, Macbook Air // 4.2 musecs/call NonlinearFactor::shared_ptr f1 = - boost::make_shared >(z, model, 1, 2, 3); + std::make_shared >(z, model, 1, 2, 3); time("GeneralSFMFactor2 : ", f1, values); // ExpressionFactor // Oct 3, 2014, Macbook Air // 20.3 musecs/call NonlinearFactor::shared_ptr f2 = - boost::make_shared >(model, z, + std::make_shared >(model, z, uncalibrate(K, project(transformTo(x, p)))); time("Bin(Leaf,Un(Bin(Leaf,Leaf))): ", f2, values); @@ -75,7 +75,7 @@ int main() { // Oct 3, 2014, Macbook Air // 20.3 musecs/call NonlinearFactor::shared_ptr f3 = - boost::make_shared >(model, z, + std::make_shared >(model, z, project3(x, p, K)); time("Ternary(Leaf,Leaf,Leaf) : ", f3, values); @@ -84,7 +84,7 @@ int main() { // Dedicated factor // Oct 3, 2014, Macbook Air // 3.4 musecs/call - NonlinearFactor::shared_ptr g1 = boost::make_shared< + NonlinearFactor::shared_ptr g1 = std::make_shared< GenericProjectionFactor >(z, model, 1, 2, fixedK); time("GenericProjectionFactor: ", g1, values); @@ -92,7 +92,7 @@ int main() { // Oct 3, 2014, Macbook Air // 16.0 musecs/call NonlinearFactor::shared_ptr g2 = - boost::make_shared >(model, z, + std::make_shared >(model, z, uncalibrate(Cal3_S2_(*fixedK), project(transformTo(x, p)))); time("Bin(Cnst,Un(Bin(Leaf,Leaf))): ", g2, values); @@ -100,7 +100,7 @@ int main() { // Oct 3, 2014, Macbook Air // 9.0 musecs/call NonlinearFactor::shared_ptr g3 = - boost::make_shared >(model, z, + std::make_shared >(model, z, Point2_(myProject, x, p)); time("Binary(Leaf,Leaf) : ", g3, values); return 0; diff --git a/timing/timeFactorOverhead.cpp b/timing/timeFactorOverhead.cpp index 038901388..de20f86a7 100644 --- a/timing/timeFactorOverhead.cpp +++ b/timing/timeFactorOverhead.cpp @@ -69,7 +69,7 @@ int main(int argc, char *argv[]) { Vector b(blockdim); for(size_t j=0; j(key, A, b, noise)); + blockGfgs[trial].push_back(std::make_shared(key, A, b, noise)); } } gttoc_(blockbuild); @@ -117,7 +117,7 @@ int main(int argc, char *argv[]) { for(size_t j=0; j(key, Acomb, bcomb, + combGfgs[trial].push_back(std::make_shared(key, Acomb, bcomb, noiseModel::Isotropic::Sigma(blockdim*nBlocks, 1.0))); } gttoc(combbuild); diff --git a/timing/timeIncremental.cpp b/timing/timeIncremental.cpp index 2b5ec474d..b8bcca5e7 100644 --- a/timing/timeIncremental.cpp +++ b/timing/timeIncremental.cpp @@ -60,7 +60,7 @@ double chi2_red(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& c // the factor graph already includes a factor for the prior/equality constraint. // double dof = graph.size() - config.size(); int graph_dim = 0; - for(const boost::shared_ptr& nlf: graph) { + for(const std::shared_ptr& nlf: graph) { graph_dim += nlf->dim(); } double dof = graph_dim - config.dim(); // kaess: changed to dim @@ -104,7 +104,7 @@ int main(int argc, char *argv[]) { NonlinearFactor::shared_ptr measurementf = measurements[nextMeasurement]; if(BetweenFactor::shared_ptr measurement = - boost::dynamic_pointer_cast >(measurementf)) + std::dynamic_pointer_cast >(measurementf)) { // Stop collecting measurements that are for future steps if(measurement->key<1>() > step || measurement->key<2>() > step) @@ -137,7 +137,7 @@ int main(int argc, char *argv[]) { } } else if(BearingRangeFactor::shared_ptr measurement = - boost::dynamic_pointer_cast >(measurementf)) + std::dynamic_pointer_cast >(measurementf)) { Key poseKey = measurement->keys()[0], lmKey = measurement->keys()[1]; diff --git a/timing/timeOneCameraExpression.cpp b/timing/timeOneCameraExpression.cpp index 73b2ff55f..09c23afa3 100644 --- a/timing/timeOneCameraExpression.cpp +++ b/timing/timeOneCameraExpression.cpp @@ -46,7 +46,7 @@ int main() { // Oct 3, 2014, Macbook Air // 20.3 musecs/call //#define TERNARY - NonlinearFactor::shared_ptr f = boost::make_shared > + NonlinearFactor::shared_ptr f = std::make_shared > #ifdef TERNARY (model, z, project3(x, p, K)); #else diff --git a/timing/timeSFMBALsmart.cpp b/timing/timeSFMBALsmart.cpp index fe2f7b925..13e8da5aa 100644 --- a/timing/timeSFMBALsmart.cpp +++ b/timing/timeSFMBALsmart.cpp @@ -36,7 +36,7 @@ int main(int argc, char* argv[]) { // Add smart factors to graph NonlinearFactorGraph graph; for (size_t j = 0; j < db.numberTracks(); j++) { - auto smartFactor = boost::make_shared(gNoiseModel); + auto smartFactor = std::make_shared(gNoiseModel); for (const SfmMeasurement& m : db.tracks[j].measurements) { size_t i = m.first; Point2 z = m.second; diff --git a/timing/timeSFMExpressions.cpp b/timing/timeSFMExpressions.cpp index 8ab949be7..6cb554d73 100644 --- a/timing/timeSFMExpressions.cpp +++ b/timing/timeSFMExpressions.cpp @@ -56,7 +56,7 @@ int main() { NonlinearFactorGraph graph; for (size_t i = 0; i < M; i++) { for (size_t j = 0; j < N; j++) { - NonlinearFactor::shared_ptr f = boost::make_shared< + NonlinearFactor::shared_ptr f = std::make_shared< ExpressionFactor > #ifdef TERNARY (model, z, project3(x[i], p[j], K)); diff --git a/timing/timeShonanFactor.cpp b/timing/timeShonanFactor.cpp index 60dd6f47a..fc97d01a8 100644 --- a/timing/timeShonanFactor.cpp +++ b/timing/timeShonanFactor.cpp @@ -65,7 +65,7 @@ int main(int argc, char* argv[]) { // graph.add(NonlinearEquality(0, SOn::Identity(4))); auto priorModel = noiseModel::Isotropic::Sigma(6, 10000); graph.add(PriorFactor(0, SOn::Identity(4), priorModel)); - auto G = boost::make_shared(SOn::VectorizedGenerators(4)); + auto G = std::make_shared(SOn::VectorizedGenerators(4)); for (const auto &m : measurements) { const auto &keys = m.keys(); const Rot3 &Rij = m.measured(); @@ -82,10 +82,10 @@ int main(int argc, char* argv[]) { params.setLinearSolverType("MULTIFRONTAL_QR"); // params.setVerbosityLM("SUMMARY"); // params.linearSolverType = LevenbergMarquardtParams::Iterative; - // auto pcg = boost::make_shared(); + // auto pcg = std::make_shared(); // pcg->preconditioner_ = - // boost::make_shared(); - // boost::make_shared(); + // std::make_shared(); + // std::make_shared(); // params.iterativeParams = pcg; // Optimize diff --git a/timing/timeVirtual.cpp b/timing/timeVirtual.cpp index c42f28ee3..d4f9634d5 100644 --- a/timing/timeVirtual.cpp +++ b/timing/timeVirtual.cpp @@ -18,7 +18,7 @@ #include -#include +#include #include #include @@ -88,13 +88,13 @@ int main(int argc, char *argv[]) { gttic_(shared_plain_alloc_dealloc); for(size_t i=0; i obj(new Plain(i)); + std::shared_ptr obj(new Plain(i)); } gttoc_(shared_plain_alloc_dealloc); gttic_(shared_virtual_alloc_dealloc); for(size_t i=0; i obj(new Virtual(i)); + std::shared_ptr obj(new Virtual(i)); } gttoc_(shared_virtual_alloc_dealloc); @@ -131,14 +131,14 @@ int main(int argc, char *argv[]) { gttic_(shared_plain_alloc_dealloc_call); for(size_t i=0; i obj(new Plain(i)); + std::shared_ptr obj(new Plain(i)); obj->setData(i+1); } gttoc_(shared_plain_alloc_dealloc_call); gttic_(shared_virtual_alloc_dealloc_call); for(size_t i=0; i obj(new Virtual(i)); + std::shared_ptr obj(new Virtual(i)); obj->setData(i+1); } gttoc_(shared_virtual_alloc_dealloc_call); diff --git a/timing/timeVirtual2.cpp b/timing/timeVirtual2.cpp index 8c7c40246..3d6e02288 100644 --- a/timing/timeVirtual2.cpp +++ b/timing/timeVirtual2.cpp @@ -18,9 +18,6 @@ #include -#include -#include - #include using namespace std;