diff --git a/examples/LocalizationExample.cpp b/examples/LocalizationExample.cpp index 6302b46af..ebe7d7f8c 100644 --- a/examples/LocalizationExample.cpp +++ b/examples/LocalizationExample.cpp @@ -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/Pose3SLAMExample_changeKeys.cpp b/examples/Pose3SLAMExample_changeKeys.cpp index 2575c61ab..544a32ac0 100644 --- a/examples/Pose3SLAMExample_changeKeys.cpp +++ b/examples/Pose3SLAMExample_changeKeys.cpp @@ -67,7 +67,7 @@ int main(const int argc, const char *argv[]) { NonlinearFactorGraph simpleGraph; for(const std::shared_ptr& factor: *graph) { std::shared_ptr > pose3Between = - boost::dynamic_pointer_cast >(factor); + 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 cf0ff09fb..a913d32ad 100644 --- a/examples/SFMExample_SmartFactorPCG.cpp +++ b/examples/SFMExample_SmartFactorPCG.cpp @@ -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 f4608f232..74ec41b4a 100644 --- a/examples/SolverComparer.cpp +++ b/examples/SolverComparer.cpp @@ -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/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 8e589af1f..761b01843 100644 --- a/gtsam/discrete/DecisionTree-inl.h +++ b/gtsam/discrete/DecisionTree-inl.h @@ -207,10 +207,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 @@ -569,7 +569,7 @@ namespace gtsam { if (it->root_->isLeaf()) continue; std::shared_ptr c = - boost::dynamic_pointer_cast(it->root_); + std::dynamic_pointer_cast(it->root_); if (!highestLabel || c->label() > *highestLabel) { highestLabel = c->label(); nrChoices = c->nrChoices(); @@ -678,13 +678,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 +720,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 +757,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 +792,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/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp index 83a30d6e3..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()); } @@ -141,7 +141,7 @@ namespace gtsam { gttoc(lookup); return std::make_pair( - boost::dynamic_pointer_cast(lookup), max); + std::dynamic_pointer_cast(lookup), max); } /* ************************************************************************ */ 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/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/testDiscreteMarginals.cpp b/gtsam/discrete/tests/testDiscreteMarginals.cpp index 49a9f0a98..ad9b1b191 100644 --- a/gtsam/discrete/tests/testDiscreteMarginals.cpp +++ b/gtsam/discrete/tests/testDiscreteMarginals.cpp @@ -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/hybrid/GaussianMixture.cpp b/gtsam/hybrid/GaussianMixture.cpp index 469c89859..04f3f2afb 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 diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index 344955d08..68129bc27 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -155,7 +155,7 @@ 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)); diff --git a/gtsam/hybrid/HybridConditional.h b/gtsam/hybrid/HybridConditional.h index 1ac5dd27b..4bb7b5353 100644 --- a/gtsam/hybrid/HybridConditional.h +++ b/gtsam/hybrid/HybridConditional.h @@ -152,7 +152,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 +161,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,7 +170,7 @@ 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 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/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 64ed8de55..346f2141a 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -57,7 +57,7 @@ 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: diff --git a/gtsam/hybrid/HybridJunctionTree.cpp b/gtsam/hybrid/HybridJunctionTree.cpp index 0616f65a5..6c0e62ac9 100644 --- a/gtsam/hybrid/HybridJunctionTree.cpp +++ b/gtsam/hybrid/HybridJunctionTree.cpp @@ -62,11 +62,11 @@ struct HybridConstructorTraversalData { // 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); } diff --git a/gtsam/hybrid/HybridNonlinearFactorGraph.cpp b/gtsam/hybrid/HybridNonlinearFactorGraph.cpp index 2970cdca6..b0a85de5d 100644 --- a/gtsam/hybrid/HybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/HybridNonlinearFactorGraph.cpp @@ -43,7 +43,7 @@ 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 = std::make_shared(); diff --git a/gtsam/hybrid/MixtureFactor.h b/gtsam/hybrid/MixtureFactor.h index b422ec99c..cbc949404 100644 --- a/gtsam/hybrid/MixtureFactor.h +++ b/gtsam/hybrid/MixtureFactor.h @@ -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( @@ -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 b87b78108..5842e1f1a 100644 --- a/gtsam/hybrid/tests/Switching.h +++ b/gtsam/hybrid/tests/Switching.h @@ -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); diff --git a/gtsam/hybrid/tests/testHybridBayesNet.cpp b/gtsam/hybrid/tests/testHybridBayesNet.cpp index d8d1efc31..939c72cee 100644 --- a/gtsam/hybrid/tests/testHybridBayesNet.cpp +++ b/gtsam/hybrid/tests/testHybridBayesNet.cpp @@ -323,7 +323,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. diff --git a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp index 16a1de92b..4c8c5518e 100644 --- a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp @@ -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(); diff --git a/gtsam/inference/graph-inl.h b/gtsam/inference/graph-inl.h index 72c6a6226..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 - std::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; @@ -197,7 +197,7 @@ std::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 - std::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(); @@ -266,7 +266,7 @@ void split(const G& g, const PredecessorMap& tree, G& Ab1, G& Ab2) { continue; } - std::shared_ptr factor2 = boost::dynamic_pointer_cast< + std::shared_ptr factor2 = std::dynamic_pointer_cast< FACTOR2>(factor); if (!factor2) continue; diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index 3a18cb30e..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,7 +345,7 @@ 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 = std::make_shared(*gf); @@ -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/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 16b7e3167..b96856644 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -200,7 +200,7 @@ 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 diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index 3dd9aa5a2..43ef958b9 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -187,7 +187,7 @@ namespace gtsam { /** Clone this JacobianFactor */ GaussianFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( + return std::static_pointer_cast( std::make_shared(*this)); } diff --git a/gtsam/linear/Preconditioner.cpp b/gtsam/linear/Preconditioner.cpp index 315452213..cb7e36503 100644 --- a/gtsam/linear/Preconditioner.cpp +++ b/gtsam/linear/Preconditioner.cpp @@ -185,7 +185,7 @@ void BlockJacobiPreconditioner::clean() { /***************************************************************************************/ std::shared_ptr createPreconditioner( const std::shared_ptr params) { - using boost::dynamic_pointer_cast; + using std::dynamic_pointer_cast; if (dynamic_pointer_cast(params)) { return std::make_shared(); } else if (dynamic_pointer_cast( 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/SubgraphPreconditioner.cpp b/gtsam/linear/SubgraphPreconditioner.cpp index f4f2038e8..b0a84bc2e 100644 --- a/gtsam/linear/SubgraphPreconditioner.cpp +++ b/gtsam/linear/SubgraphPreconditioner.cpp @@ -82,7 +82,7 @@ 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 = std::make_shared(*factor); } 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/testJacobianFactor.cpp b/gtsam/linear/tests/testJacobianFactor.cpp index 6a636e311..ad722eb94 100644 --- a/gtsam/linear/tests/testJacobianFactor.cpp +++ b/gtsam/linear/tests/testJacobianFactor.cpp @@ -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)); 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/navigation/AHRSFactor.cpp b/gtsam/navigation/AHRSFactor.cpp index d85714846..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))); } diff --git a/gtsam/navigation/AHRSFactor.h b/gtsam/navigation/AHRSFactor.h index 47a2052c8..bf7ef56b0 100644 --- a/gtsam/navigation/AHRSFactor.h +++ b/gtsam/navigation/AHRSFactor.h @@ -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_; } diff --git a/gtsam/navigation/AttitudeFactor.h b/gtsam/navigation/AttitudeFactor.h index 1016afd49..b412d4d26 100644 --- a/gtsam/navigation/AttitudeFactor.h +++ b/gtsam/navigation/AttitudeFactor.h @@ -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))); } @@ -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 60ce95637..c7cf939af 100644 --- a/gtsam/navigation/BarometricFactor.h +++ b/gtsam/navigation/BarometricFactor.h @@ -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 267b6394a..407bf9ace 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -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 diff --git a/gtsam/navigation/GPSFactor.h b/gtsam/navigation/GPSFactor.h index 24fde832d..dbc164d1a 100644 --- a/gtsam/navigation/GPSFactor.h +++ b/gtsam/navigation/GPSFactor.h @@ -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))); } @@ -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 59797a008..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))); } @@ -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/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 3efd513cc..45adb2383 100644 --- a/gtsam/navigation/MagPoseFactor.h +++ b/gtsam/navigation/MagPoseFactor.h @@ -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/ScenarioRunner.h b/gtsam/navigation/ScenarioRunner.h index fee950a76..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; diff --git a/gtsam/nonlinear/ExpressionFactor.h b/gtsam/nonlinear/ExpressionFactor.h index cd736d7a6..f859b3d92 100644 --- a/gtsam/nonlinear/ExpressionFactor.h +++ b/gtsam/nonlinear/ExpressionFactor.h @@ -120,7 +120,7 @@ protected: // 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(); } @@ -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/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.cpp b/gtsam/nonlinear/ISAM2.cpp index 41da80b1f..544e4d07e 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -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(); diff --git a/gtsam/nonlinear/LinearContainerFactor.cpp b/gtsam/nonlinear/LinearContainerFactor.cpp index 19ca18afe..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); } /* ************************************************************************* */ diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index 75b904bd5..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))); } @@ -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))); } @@ -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 9d33e9937..732af0e3a 100644 --- a/gtsam/nonlinear/NonlinearFactor.cpp +++ b/gtsam/nonlinear/NonlinearFactor.cpp @@ -90,7 +90,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; } @@ -177,7 +177,7 @@ std::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/NonlinearOptimizer.cpp b/gtsam/nonlinear/NonlinearOptimizer.cpp index 3ce6db4af..01a747d28 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearOptimizer.cpp @@ -160,11 +160,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/PriorFactor.h b/gtsam/nonlinear/PriorFactor.h index 3c62bca31..808f66c5b 100644 --- a/gtsam/nonlinear/PriorFactor.h +++ b/gtsam/nonlinear/PriorFactor.h @@ -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/WhiteNoiseFactor.h b/gtsam/nonlinear/WhiteNoiseFactor.h index b01896d88..a8142c28c 100644 --- a/gtsam/nonlinear/WhiteNoiseFactor.h +++ b/gtsam/nonlinear/WhiteNoiseFactor.h @@ -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/tests/testLinearContainerFactor.cpp b/gtsam/nonlinear/tests/testLinearContainerFactor.cpp index 44c60bd4d..484b637a7 100644 --- a/gtsam/nonlinear/tests/testLinearContainerFactor.cpp +++ b/gtsam/nonlinear/tests/testLinearContainerFactor.cpp @@ -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 @@ -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 bfaff92d8..2997ed7d7 100644 --- a/gtsam/nonlinear/utilities.h +++ b/gtsam/nonlinear/utilities.h @@ -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) { std::shared_ptr > p = - boost::dynamic_pointer_cast >( + 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 e2a85dfe1..f4fcbf692 100644 --- a/gtsam/sam/BearingRangeFactor.h +++ b/gtsam/sam/BearingRangeFactor.h @@ -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/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp index 6a4f042e2..f8cd9fc9c 100644 --- a/gtsam/sfm/ShonanAveraging.cpp +++ b/gtsam/sfm/ShonanAveraging.cpp @@ -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 030f0fcf9..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 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 e45ad8562..f11575035 100644 --- a/gtsam/slam/AntiFactor.h +++ b/gtsam/slam/AntiFactor.h @@ -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 */ diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h index 51403f248..36cea67a9 100644 --- a/gtsam/slam/BetweenFactor.h +++ b/gtsam/slam/BetweenFactor.h @@ -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 diff --git a/gtsam/slam/EssentialMatrixConstraint.h b/gtsam/slam/EssentialMatrixConstraint.h index 539d03c14..c533ea04d 100644 --- a/gtsam/slam/EssentialMatrixConstraint.h +++ b/gtsam/slam/EssentialMatrixConstraint.h @@ -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 1cedc8263..b19bb09ab 100644 --- a/gtsam/slam/EssentialMatrixFactor.h +++ b/gtsam/slam/EssentialMatrixFactor.h @@ -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))); } @@ -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))); } @@ -275,7 +275,7 @@ class EssentialMatrixFactor3 : public 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))); } @@ -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 6c4257b87..b7199d161 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -102,7 +102,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)));} /** @@ -170,7 +170,7 @@ 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 std::make_shared >(key1, H1, key2, H2, b, model); @@ -237,7 +237,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 f9b99e47e..4ba973ecc 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 6bb1b0f36..a107a51de 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); @@ -223,7 +223,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/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 c0aabacce..73ed85035 100644 --- a/gtsam/slam/ProjectionFactor.h +++ b/gtsam/slam/ProjectionFactor.h @@ -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))); } /** 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/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 8ad82f8a8..6d159972f 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -105,7 +105,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) diff --git a/gtsam/slam/StereoFactor.h b/gtsam/slam/StereoFactor.h index 40141df00..8d25d6032 100644 --- a/gtsam/slam/StereoFactor.h +++ b/gtsam/slam/StereoFactor.h @@ -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 9147f3a4e..a0bf151b5 100644 --- a/gtsam/slam/TriangulationFactor.h +++ b/gtsam/slam/TriangulationFactor.h @@ -97,7 +97,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/dataset.cpp b/gtsam/slam/dataset.cpp index 16e778b7f..4f6cb2e38 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -373,7 +373,7 @@ template <> struct ParseMeasurement { /* ************************************************************************* */ // Create a sampler to corrupt a measurement std::shared_ptr createSampler(const SharedNoiseModel &model) { - auto noise = boost::dynamic_pointer_cast(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)!"); @@ -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 " @@ -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; @@ -679,11 +679,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!"); @@ -701,13 +701,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(); std::shared_ptr gaussianModel = - boost::dynamic_pointer_cast(model); + std::dynamic_pointer_cast(model); if (!gaussianModel) { model->print("model\n"); throw invalid_argument("writeG2o: invalid noise model!"); @@ -887,7 +887,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 " diff --git a/gtsam/slam/lago.cpp b/gtsam/slam/lago.cpp index 4be0039d9..cb9d98218 100644 --- a/gtsam/slam/lago.cpp +++ b/gtsam/slam/lago.cpp @@ -108,7 +108,7 @@ void getSymbolicGraph( Key key2 = factor->keys()[1]; // recast to a between std::shared_ptr > pose2Between = - boost::dynamic_pointer_cast >(factor); + std::dynamic_pointer_cast >(factor); if (!pose2Between) continue; // get the orientation - measured().theta(); @@ -140,7 +140,7 @@ static void getDeltaThetaAndNoise(NonlinearFactor::shared_ptr factor, // Get the relative rotation measurement from the between factor std::shared_ptr > pose2Between = - boost::dynamic_pointer_cast >(factor); + std::dynamic_pointer_cast >(factor); if (!pose2Between) throw invalid_argument( "buildLinearOrientationGraph: invalid between factor!"); @@ -149,7 +149,7 @@ static void getDeltaThetaAndNoise(NonlinearFactor::shared_ptr factor, // Retrieve the noise model for the relative rotation SharedNoiseModel model = pose2Between->noiseModel(); std::shared_ptr diagonalModel = - boost::dynamic_pointer_cast(model); + std::dynamic_pointer_cast(model); if (!diagonalModel) throw invalid_argument("buildLinearOrientationGraph: invalid noise model " "(current version assumes diagonal noise model)!"); @@ -278,7 +278,7 @@ Values computePoses(const NonlinearFactorGraph& pose2graph, for(const std::shared_ptr& factor: pose2graph) { std::shared_ptr > pose2Between = - boost::dynamic_pointer_cast >(factor); + std::dynamic_pointer_cast >(factor); if (pose2Between) { Key key1 = pose2Between->keys()[0]; @@ -304,7 +304,7 @@ Values computePoses(const NonlinearFactorGraph& pose2graph, J1(1, 2) = -c1 * dx + s1 * dy; // Retrieve the noise model for the relative rotation std::shared_ptr diagonalModel = - boost::dynamic_pointer_cast( + std::dynamic_pointer_cast( pose2Between->noiseModel()); linearPose2graph.add(key1, J1, key2, I3, b, diagonalModel); 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_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/tests/testLoopyBelief.cpp b/gtsam_unstable/discrete/tests/testLoopyBelief.cpp index fdca66c6a..63df7d7c4 100644 --- a/gtsam_unstable/discrete/tests/testLoopyBelief.cpp +++ b/gtsam_unstable/discrete/tests/testLoopyBelief.cpp @@ -112,11 +112,11 @@ class LoopyBelief { // Incorporate new the factor to belief if (!beliefAtKey) beliefAtKey = - boost::dynamic_pointer_cast(message); + std::dynamic_pointer_cast(message); else beliefAtKey = std::make_shared( (*beliefAtKey) * - (*boost::dynamic_pointer_cast(message))); + (*std::dynamic_pointer_cast(message))); } if (starGraphs_.at(key).unary) beliefAtKey = std::make_shared( @@ -148,9 +148,9 @@ 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 = @@ -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 = 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 5e91845b1..3fd09f639 100644 --- a/gtsam_unstable/dynamics/Pendulum.h +++ b/gtsam_unstable/dynamics/Pendulum.h @@ -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 */ @@ -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 */ @@ -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 */ @@ -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 26b8d296c..ab06b32a4 100644 --- a/gtsam_unstable/dynamics/VelocityConstraint3.h +++ b/gtsam_unstable/dynamics/VelocityConstraint3.h @@ -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/linear/LinearCost.h b/gtsam_unstable/linear/LinearCost.h index c6aff7ddb..c5447e645 100644 --- a/gtsam_unstable/linear/LinearCost.h +++ b/gtsam_unstable/linear/LinearCost.h @@ -100,7 +100,7 @@ public: /** Clone this LinearCost */ GaussianFactor::shared_ptr clone() const override { - return boost::static_pointer_cast < GaussianFactor + return std::static_pointer_cast < GaussianFactor > (std::make_shared < LinearCost > (*this)); } diff --git a/gtsam_unstable/linear/LinearEquality.h b/gtsam_unstable/linear/LinearEquality.h index b2c443657..5cf60686a 100644 --- a/gtsam_unstable/linear/LinearEquality.h +++ b/gtsam_unstable/linear/LinearEquality.h @@ -101,7 +101,7 @@ public: /** Clone this LinearEquality */ GaussianFactor::shared_ptr clone() const override { - return boost::static_pointer_cast < GaussianFactor + return std::static_pointer_cast < GaussianFactor > (std::make_shared < LinearEquality > (*this)); } diff --git a/gtsam_unstable/linear/LinearInequality.h b/gtsam_unstable/linear/LinearInequality.h index f02f42d7b..3059189ed 100644 --- a/gtsam_unstable/linear/LinearInequality.h +++ b/gtsam_unstable/linear/LinearInequality.h @@ -115,7 +115,7 @@ public: /** Clone this LinearInequality */ GaussianFactor::shared_ptr clone() const override { - return boost::static_pointer_cast < GaussianFactor + return std::static_pointer_cast < GaussianFactor > (std::make_shared < LinearInequality > (*this)); } diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp index d6456cd51..0daa4b4be 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/ConcurrentBatchSmoother.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp index 816917eb5..480aab87b 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp @@ -384,7 +384,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/LinearizedFactor.h b/gtsam_unstable/nonlinear/LinearizedFactor.h index 242bd7c22..5bf791089 100644 --- a/gtsam_unstable/nonlinear/LinearizedFactor.h +++ b/gtsam_unstable/nonlinear/LinearizedFactor.h @@ -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 @@ -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 diff --git a/gtsam_unstable/nonlinear/NonlinearClusterTree.h b/gtsam_unstable/nonlinear/NonlinearClusterTree.h index 538d42513..d308514ae 100644 --- a/gtsam_unstable/nonlinear/NonlinearClusterTree.h +++ b/gtsam_unstable/nonlinear/NonlinearClusterTree.h @@ -37,7 +37,7 @@ class NonlinearClusterTree : public ClusterTree { } static NonlinearCluster* DownCast(const std::shared_ptr& cluster) { - auto nonlinearCluster = boost::dynamic_pointer_cast(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 c9b1131b5..a7e266cb5 100644 --- a/gtsam_unstable/nonlinear/tests/testCustomChartExpression.cpp +++ b/gtsam_unstable/nonlinear/tests/testCustomChartExpression.cpp @@ -82,7 +82,7 @@ TEST(ExpressionCustomChart, projection) { std::shared_ptr gfstandard = f.linearize(standard); std::shared_ptr jfstandard = // - boost::dynamic_pointer_cast(gfstandard); + std::dynamic_pointer_cast(gfstandard); typedef std::pair Jacobian; Jacobian Jstandard = jfstandard->jacobianUnweighted(); @@ -90,7 +90,7 @@ TEST(ExpressionCustomChart, projection) { std::shared_ptr gfcustom = f.linearize(custom); std::shared_ptr jfcustom = // - boost::dynamic_pointer_cast(gfcustom); + 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 61e8346b2..9083ab172 100644 --- a/gtsam_unstable/nonlinear/tests/testNonlinearClusterTree.cpp +++ b/gtsam_unstable/nonlinear/tests/testNonlinearClusterTree.cpp @@ -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"); diff --git a/gtsam_unstable/slam/DummyFactor.h b/gtsam_unstable/slam/DummyFactor.h index 131f975b3..13ecad3b6 100644 --- a/gtsam_unstable/slam/DummyFactor.h +++ b/gtsam_unstable/slam/DummyFactor.h @@ -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/GaussMarkov1stOrderFactor.h b/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h index 7c44d79f8..87389a40d 100644 --- a/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h +++ b/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h @@ -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/MultiProjectionFactor.h b/gtsam_unstable/slam/MultiProjectionFactor.h index cb60cc723..3adf227bc 100644 --- a/gtsam_unstable/slam/MultiProjectionFactor.h +++ b/gtsam_unstable/slam/MultiProjectionFactor.h @@ -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))); } /** 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 afe03f45d..339407a3f 100644 --- a/gtsam_unstable/slam/PoseBetweenFactor.h +++ b/gtsam_unstable/slam/PoseBetweenFactor.h @@ -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 13d6484e0..a5e5facbd 100644 --- a/gtsam_unstable/slam/PosePriorFactor.h +++ b/gtsam_unstable/slam/PosePriorFactor.h @@ -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 a443ab413..62f681b2e 100644 --- a/gtsam_unstable/slam/PoseToPointFactor.h +++ b/gtsam_unstable/slam/PoseToPointFactor.h @@ -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 a1f628a64..dea28528d 100644 --- a/gtsam_unstable/slam/ProjectionFactorPPP.h +++ b/gtsam_unstable/slam/ProjectionFactorPPP.h @@ -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))); } /** diff --git a/gtsam_unstable/slam/ProjectionFactorPPPC.h b/gtsam_unstable/slam/ProjectionFactorPPPC.h index b1a2dbe4b..a3dd0850a 100644 --- a/gtsam_unstable/slam/ProjectionFactorPPPC.h +++ b/gtsam_unstable/slam/ProjectionFactorPPPC.h @@ -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 5e3d58e3f..938f20fd6 100644 --- a/gtsam_unstable/slam/ProjectionFactorRollingShutter.h +++ b/gtsam_unstable/slam/ProjectionFactorRollingShutter.h @@ -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))); } 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/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/tests/simulated2D.h b/tests/simulated2D.h index db2fb1eed..5334cbf45 100644 --- a/tests/simulated2D.h +++ b/tests/simulated2D.h @@ -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: @@ -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: @@ -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 5e723ebb6..ae626d1fa 100644 --- a/tests/simulated2DConstraints.h +++ b/tests/simulated2DConstraints.h @@ -62,7 +62,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))); } /** @@ -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 705375eed..08341245c 100644 --- a/tests/smallExample.h +++ b/tests/smallExample.h @@ -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))); } }; diff --git a/tests/testExpressionFactor.cpp b/tests/testExpressionFactor.cpp index e709ae8f3..4f4891d8a 100644 --- a/tests/testExpressionFactor.cpp +++ b/tests/testExpressionFactor.cpp @@ -128,7 +128,7 @@ TEST(ExpressionFactor, Unary) { EXPECT_LONGS_EQUAL(2, f.dim()); std::shared_ptr gf = f.linearize(values); std::shared_ptr jf = // - boost::dynamic_pointer_cast(gf); + std::dynamic_pointer_cast(gf); EXPECT(assert_equal(expected, *jf, 1e-9)); } @@ -342,7 +342,7 @@ TEST(ExpressionFactor, Compose1) { JacobianFactor expected(1, I_3x3, 2, I_3x3, Z_3x1); std::shared_ptr gf = f.linearize(values); std::shared_ptr jf = // - boost::dynamic_pointer_cast(gf); + std::dynamic_pointer_cast(gf); EXPECT(assert_equal(expected, *jf,1e-9)); } @@ -371,7 +371,7 @@ TEST(ExpressionFactor, compose2) { JacobianFactor expected(1, 2 * I_3x3, Z_3x1); std::shared_ptr gf = f.linearize(values); std::shared_ptr jf = // - boost::dynamic_pointer_cast(gf); + std::dynamic_pointer_cast(gf); EXPECT(assert_equal(expected, *jf,1e-9)); } @@ -400,7 +400,7 @@ TEST(ExpressionFactor, compose3) { JacobianFactor expected(3, I_3x3, Z_3x1); std::shared_ptr gf = f.linearize(values); std::shared_ptr jf = // - boost::dynamic_pointer_cast(gf); + std::dynamic_pointer_cast(gf); EXPECT(assert_equal(expected, *jf,1e-9)); } @@ -445,7 +445,7 @@ TEST(ExpressionFactor, composeTernary) { JacobianFactor expected(1, I_3x3, 2, I_3x3, 3, I_3x3, Z_3x1); std::shared_ptr gf = f.linearize(values); std::shared_ptr jf = // - boost::dynamic_pointer_cast(gf); + 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 e288ba306..3fce79754 100644 --- a/tests/testExtendedKalmanFilter.cpp +++ b/tests/testExtendedKalmanFilter.cpp @@ -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)); @@ -334,7 +334,7 @@ public: 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/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/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp index 3fe9608ac..06ae3366c 100644 --- a/tests/testNonlinearFactor.cpp +++ b/tests/testNonlinearFactor.cpp @@ -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/timing/timeIncremental.cpp b/timing/timeIncremental.cpp index 34860b8ea..b8bcca5e7 100644 --- a/timing/timeIncremental.cpp +++ b/timing/timeIncremental.cpp @@ -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/wrap/gtwrap/matlab_wrapper/templates.py b/wrap/gtwrap/matlab_wrapper/templates.py index fab4c185a..7783c8e9c 100644 --- a/wrap/gtwrap/matlab_wrapper/templates.py +++ b/wrap/gtwrap/matlab_wrapper/templates.py @@ -80,7 +80,7 @@ class WrapperTemplate: typedef std::shared_ptr<{cpp_name}> Shared; std::shared_ptr *asVoid = *reinterpret_cast**> (mxGetData(in[0])); out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); - Shared *self = new Shared(boost::static_pointer_cast<{cpp_name}>(*asVoid)); + Shared *self = new Shared(std::static_pointer_cast<{cpp_name}>(*asVoid)); *reinterpret_cast(mxGetData(out[0])) = self; }}\n ''') diff --git a/wrap/tests/expected/matlab/inheritance_wrapper.cpp b/wrap/tests/expected/matlab/inheritance_wrapper.cpp index 1d8a38b08..974866671 100644 --- a/wrap/tests/expected/matlab/inheritance_wrapper.cpp +++ b/wrap/tests/expected/matlab/inheritance_wrapper.cpp @@ -129,7 +129,7 @@ void MyBase_upcastFromVoid_1(int nargout, mxArray *out[], int nargin, const mxAr typedef std::shared_ptr Shared; std::shared_ptr *asVoid = *reinterpret_cast**> (mxGetData(in[0])); out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); - Shared *self = new Shared(boost::static_pointer_cast(*asVoid)); + Shared *self = new Shared(std::static_pointer_cast(*asVoid)); *reinterpret_cast(mxGetData(out[0])) = self; } @@ -164,7 +164,7 @@ void MyTemplatePoint2_upcastFromVoid_4(int nargout, mxArray *out[], int nargin, typedef std::shared_ptr> Shared; std::shared_ptr *asVoid = *reinterpret_cast**> (mxGetData(in[0])); out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); - Shared *self = new Shared(boost::static_pointer_cast>(*asVoid)); + Shared *self = new Shared(std::static_pointer_cast>(*asVoid)); *reinterpret_cast(mxGetData(out[0])) = self; } @@ -332,7 +332,7 @@ void MyTemplateMatrix_upcastFromVoid_20(int nargout, mxArray *out[], int nargin, typedef std::shared_ptr> Shared; std::shared_ptr *asVoid = *reinterpret_cast**> (mxGetData(in[0])); out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); - Shared *self = new Shared(boost::static_pointer_cast>(*asVoid)); + Shared *self = new Shared(std::static_pointer_cast>(*asVoid)); *reinterpret_cast(mxGetData(out[0])) = self; } @@ -500,7 +500,7 @@ void MyTemplateA_upcastFromVoid_36(int nargout, mxArray *out[], int nargin, cons typedef std::shared_ptr> Shared; std::shared_ptr *asVoid = *reinterpret_cast**> (mxGetData(in[0])); out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); - Shared *self = new Shared(boost::static_pointer_cast>(*asVoid)); + Shared *self = new Shared(std::static_pointer_cast>(*asVoid)); *reinterpret_cast(mxGetData(out[0])) = self; } @@ -650,7 +650,7 @@ void ForwardKinematicsFactor_upcastFromVoid_52(int nargout, mxArray *out[], int typedef std::shared_ptr Shared; std::shared_ptr *asVoid = *reinterpret_cast**> (mxGetData(in[0])); out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); - Shared *self = new Shared(boost::static_pointer_cast(*asVoid)); + Shared *self = new Shared(std::static_pointer_cast(*asVoid)); *reinterpret_cast(mxGetData(out[0])) = self; } @@ -685,7 +685,7 @@ void ParentHasTemplateDouble_upcastFromVoid_55(int nargout, mxArray *out[], int typedef std::shared_ptr> Shared; std::shared_ptr *asVoid = *reinterpret_cast**> (mxGetData(in[0])); out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); - Shared *self = new Shared(boost::static_pointer_cast>(*asVoid)); + Shared *self = new Shared(std::static_pointer_cast>(*asVoid)); *reinterpret_cast(mxGetData(out[0])) = self; }