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

replaced casts

release/4.3a0
kartik arcot 2023-01-17 14:39:55 -08:00 committed by Kartik Arcot
parent 852e8768c0
commit c037e0a1fc
124 changed files with 269 additions and 269 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
examples/LocalizationExample.cpp
View File

@ -105,7 +105,7 @@ class UnaryFactor: public NoiseModelFactorN<Pose2> {
// 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<gtsam::NonlinearFactor>(
return std::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new UnaryFactor(*this))); }
// Additionally, we encourage you the use of unit testing your custom factors,

2
examples/Pose3SLAMExample_changeKeys.cpp
View File

@ -67,7 +67,7 @@ int main(const int argc, const char *argv[]) {
NonlinearFactorGraph simpleGraph;
for(const std::shared_ptr<NonlinearFactor>& factor: *graph) {
std::shared_ptr<BetweenFactor<Pose3> > pose3Between =
boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
std::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
if (pose3Between){
Key key1, key2;
if(add){

2
examples/SFMExample_SmartFactor.cpp
View File

@ -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<SmartFactor>(graph[j]);
SmartFactor::shared_ptr smart = std::dynamic_pointer_cast<SmartFactor>(graph[j]);
if (smart) {
// The output of point() is in std::optional<Point3>, as sometimes
// the triangulation operation inside smart factor will encounter degeneracy.

2
examples/SFMExample_SmartFactorPCG.cpp
View File

@ -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<SmartFactor>(graph[j]);
auto smart = std::dynamic_pointer_cast<SmartFactor>(graph[j]);
if (smart) {
std::optional<Point3> point = smart->point(result);
if (point) // ignore if std::optional return nullptr

6
examples/SolverComparer.cpp
View File

@ -259,7 +259,7 @@ void runIncremental()
while(nextMeasurement < datasetMeasurements.size())
{
if(BetweenFactor<Pose>::shared_ptr factor =
boost::dynamic_pointer_cast<BetweenFactor<Pose> >(datasetMeasurements[nextMeasurement]))
std::dynamic_pointer_cast<BetweenFactor<Pose> >(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<Pose>::shared_ptr factor =
boost::dynamic_pointer_cast<BetweenFactor<Pose> >(measurementf))
std::dynamic_pointer_cast<BetweenFactor<Pose> >(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<Pose, Point2>::shared_ptr factor =
boost::dynamic_pointer_cast<BearingRangeFactor<Pose, Point2> >(measurementf))
std::dynamic_pointer_cast<BearingRangeFactor<Pose, Point2> >(measurementf))
{
Key poseKey = factor->keys()[0], lmKey = factor->keys()[1];

2
gtsam/basis/tests/testFourier.cpp
View File

@ -94,7 +94,7 @@ TEST(Basis, Manual) {
auto linearizedFactor = predictFactor.linearize(values);
auto linearizedJacobianFactor =
boost::dynamic_pointer_cast<JacobianFactor>(linearizedFactor);
std::dynamic_pointer_cast<JacobianFactor>(linearizedFactor);
CHECK(linearizedJacobianFactor); // makes sure it's indeed a JacobianFactor
EXPECT(assert_equal(linearFactor, *linearizedJacobianFactor, 1e-9));
}

22
gtsam/discrete/DecisionTree-inl.h
View File

@ -207,10 +207,10 @@ namespace gtsam {
for(auto branch: f->branches()) {
assert(branch->isLeaf());
nrAssignments +=
boost::dynamic_pointer_cast<const Leaf>(branch)->nrAssignments();
std::dynamic_pointer_cast<const Leaf>(branch)->nrAssignments();
}
NodePtr newLeaf(
new Leaf(boost::dynamic_pointer_cast<const Leaf>(f0)->constant(),
new Leaf(std::dynamic_pointer_cast<const Leaf>(f0)->constant(),
nrAssignments));
return newLeaf;
} else
@ -569,7 +569,7 @@ namespace gtsam {
if (it->root_->isLeaf())
continue;
std::shared_ptr<const Choice> c =
boost::dynamic_pointer_cast<const Choice>(it->root_);
std::dynamic_pointer_cast<const Choice>(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<M, X>::Leaf;
if (auto leaf = boost::dynamic_pointer_cast<const MXLeaf>(f)) {
if (auto leaf = std::dynamic_pointer_cast<const MXLeaf>(f)) {
return NodePtr(new Leaf(Y_of_X(leaf->constant()), leaf->nrAssignments()));
}
// Check if Choice
using MXChoice = typename DecisionTree<M, X>::Choice;
auto choice = boost::dynamic_pointer_cast<const MXChoice>(f);
auto choice = std::dynamic_pointer_cast<const MXChoice>(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<L, Y>::NodePtr& node) const {
using Leaf = typename DecisionTree<L, Y>::Leaf;
if (auto leaf = boost::dynamic_pointer_cast<const Leaf>(node))
if (auto leaf = std::dynamic_pointer_cast<const Leaf>(node))
return f(leaf->constant());
using Choice = typename DecisionTree<L, Y>::Choice;
auto choice = boost::dynamic_pointer_cast<const Choice>(node);
auto choice = std::dynamic_pointer_cast<const Choice>(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<L, Y>::NodePtr& node) const {
using Leaf = typename DecisionTree<L, Y>::Leaf;
if (auto leaf = boost::dynamic_pointer_cast<const Leaf>(node))
if (auto leaf = std::dynamic_pointer_cast<const Leaf>(node))
return f(*leaf);
using Choice = typename DecisionTree<L, Y>::Choice;
auto choice = boost::dynamic_pointer_cast<const Choice>(node);
auto choice = std::dynamic_pointer_cast<const Choice>(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<L, Y>::NodePtr& node) {
using Leaf = typename DecisionTree<L, Y>::Leaf;
if (auto leaf = boost::dynamic_pointer_cast<const Leaf>(node))
if (auto leaf = std::dynamic_pointer_cast<const Leaf>(node))
return f(assignment, leaf->constant());
using Choice = typename DecisionTree<L, Y>::Choice;
auto choice = boost::dynamic_pointer_cast<const Choice>(node);
auto choice = std::dynamic_pointer_cast<const Choice>(node);
if (!choice)
throw std::invalid_argument("DecisionTree::VisitWith: Invalid NodePtr");
for (size_t i = 0; i < choice->nrChoices(); i++) {

4
gtsam/discrete/DiscreteFactorGraph.cpp
View File

@ -54,7 +54,7 @@ namespace gtsam {
DiscreteKeys DiscreteFactorGraph::discreteKeys() const {
DiscreteKeys result;
for (auto&& factor : *this) {
if (auto p = boost::dynamic_pointer_cast<DecisionTreeFactor>(factor)) {
if (auto p = std::dynamic_pointer_cast<DecisionTreeFactor>(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<DiscreteConditional>(lookup), max);
std::dynamic_pointer_cast<DiscreteConditional>(lookup), max);
}
/* ************************************************************************ */

2
gtsam/discrete/DiscreteLookupDAG.cpp
View File

@ -106,7 +106,7 @@ DiscreteLookupDAG DiscreteLookupDAG::FromBayesNet(
DiscreteLookupDAG dag;
for (auto&& conditional : bayesNet) {
if (auto lookupTable =
boost::dynamic_pointer_cast<DiscreteLookupTable>(conditional)) {
std::dynamic_pointer_cast<DiscreteLookupTable>(conditional)) {
dag.push_back(lookupTable);
} else {
throw std::runtime_error(

14
gtsam/discrete/tests/testDecisionTree.cpp
View File

@ -326,7 +326,7 @@ TEST(DecisionTree, NrAssignments) {
const std::pair<string, size_t> 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<const DT::Leaf>(tree.root_);
auto leaf = std::dynamic_pointer_cast<const DT::Leaf>(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<const DT::Choice>(tree2.root_);
auto root = std::dynamic_pointer_cast<const DT::Choice>(tree2.root_);
CHECK(root);
auto choice0 = boost::dynamic_pointer_cast<const DT::Choice>(root->branches()[0]);
auto choice0 = std::dynamic_pointer_cast<const DT::Choice>(root->branches()[0]);
CHECK(choice0);
EXPECT(choice0->branches()[0]->isLeaf());
auto choice00 = boost::dynamic_pointer_cast<const DT::Leaf>(choice0->branches()[0]);
auto choice00 = std::dynamic_pointer_cast<const DT::Leaf>(choice0->branches()[0]);
CHECK(choice00);
EXPECT_LONGS_EQUAL(2, choice00->nrAssignments());
auto choice1 = boost::dynamic_pointer_cast<const DT::Choice>(root->branches()[1]);
auto choice1 = std::dynamic_pointer_cast<const DT::Choice>(root->branches()[1]);
CHECK(choice1);
auto choice10 = boost::dynamic_pointer_cast<const DT::Choice>(choice1->branches()[0]);
auto choice10 = std::dynamic_pointer_cast<const DT::Choice>(choice1->branches()[0]);
CHECK(choice10);
auto choice11 = boost::dynamic_pointer_cast<const DT::Leaf>(choice1->branches()[1]);
auto choice11 = std::dynamic_pointer_cast<const DT::Leaf>(choice1->branches()[1]);
CHECK(choice11);
EXPECT(choice11->isLeaf());
EXPECT_LONGS_EQUAL(2, choice11->nrAssignments());

6
gtsam/discrete/tests/testDiscreteMarginals.cpp
View File

@ -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<DecisionTreeFactor>(actualM0),1e-5));
EXPECT(assert_equal(expectedM0, *std::dynamic_pointer_cast<DecisionTreeFactor>(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<DecisionTreeFactor>(actualM1),1e-5));
EXPECT(assert_equal(expectedM1, *std::dynamic_pointer_cast<DecisionTreeFactor>(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<DecisionTreeFactor>(actualM)));
expectedM, *std::dynamic_pointer_cast<DecisionTreeFactor>(actualM)));
}
}

2
gtsam/hybrid/GaussianMixture.cpp
View File

@ -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<GaussianConditional>(ptr);
auto conditional = std::dynamic_pointer_cast<GaussianConditional>(ptr);
if (conditional)
return conditional;
else

2
gtsam/hybrid/HybridBayesNet.cpp
View File

@ -155,7 +155,7 @@ void HybridBayesNet::updateDiscreteConditionals(
// Apply prunerFunc to the underlying AlgebraicDecisionTree
auto discreteTree =
boost::dynamic_pointer_cast<DecisionTreeFactor::ADT>(discrete);
std::dynamic_pointer_cast<DecisionTreeFactor::ADT>(discrete);
DecisionTreeFactor::ADT prunedDiscreteTree =
discreteTree->apply(prunerFunc(prunedDecisionTree, *conditional));

6
gtsam/hybrid/HybridConditional.h
View File

@ -152,7 +152,7 @@ class GTSAM_EXPORT HybridConditional
* @return GaussianMixture::shared_ptr otherwise
*/
GaussianMixture::shared_ptr asMixture() const {
return boost::dynamic_pointer_cast<GaussianMixture>(inner_);
return std::dynamic_pointer_cast<GaussianMixture>(inner_);
}
/**
@ -161,7 +161,7 @@ class GTSAM_EXPORT HybridConditional
* @return GaussianConditional::shared_ptr otherwise
*/
GaussianConditional::shared_ptr asGaussian() const {
return boost::dynamic_pointer_cast<GaussianConditional>(inner_);
return std::dynamic_pointer_cast<GaussianConditional>(inner_);
}
/**
@ -170,7 +170,7 @@ class GTSAM_EXPORT HybridConditional
* @return DiscreteConditional::shared_ptr
*/
DiscreteConditional::shared_ptr asDiscrete() const {
return boost::dynamic_pointer_cast<DiscreteConditional>(inner_);
return std::dynamic_pointer_cast<DiscreteConditional>(inner_);
}
/// Get the type-erased pointer to the inner type

6
gtsam/hybrid/HybridFactorGraph.cpp
View File

@ -28,12 +28,12 @@ namespace gtsam {
std::set<DiscreteKey> HybridFactorGraph::discreteKeys() const {
std::set<DiscreteKey> keys;
for (auto& factor : factors_) {
if (auto p = boost::dynamic_pointer_cast<DecisionTreeFactor>(factor)) {
if (auto p = std::dynamic_pointer_cast<DecisionTreeFactor>(factor)) {
for (const DiscreteKey& key : p->discreteKeys()) {
keys.insert(key);
}
}
if (auto p = boost::dynamic_pointer_cast<HybridFactor>(factor)) {
if (auto p = std::dynamic_pointer_cast<HybridFactor>(factor)) {
for (const DiscreteKey& key : p->discreteKeys()) {
keys.insert(key);
}
@ -65,7 +65,7 @@ std::unordered_map<Key, DiscreteKey> HybridFactorGraph::discreteKeyMap() const {
const KeySet HybridFactorGraph::continuousKeySet() const {
KeySet keys;
for (auto& factor : factors_) {
if (auto p = boost::dynamic_pointer_cast<HybridFactor>(factor)) {
if (auto p = std::dynamic_pointer_cast<HybridFactor>(factor)) {
for (const Key& key : p->continuousKeys()) {
keys.insert(key);
}

2
gtsam/hybrid/HybridGaussianFactorGraph.cpp
View File

@ -57,7 +57,7 @@ template class EliminateableFactorGraph<HybridGaussianFactorGraph>;
using OrphanWrapper = BayesTreeOrphanWrapper<HybridBayesTree::Clique>;
using boost::dynamic_pointer_cast;
using std::dynamic_pointer_cast;
/* ************************************************************************ */
// Throw a runtime exception for method specified in string s, and factor f:

4
gtsam/hybrid/HybridJunctionTree.cpp
View File

@ -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<HybridFactor>(f)) {
if (auto hf = std::dynamic_pointer_cast<HybridFactor>(f)) {
for (auto& k : hf->discreteKeys()) {
data.discreteKeys.insert(k.first);
}
} else if (auto hf = boost::dynamic_pointer_cast<DecisionTreeFactor>(f)) {
} else if (auto hf = std::dynamic_pointer_cast<DecisionTreeFactor>(f)) {
for (auto& k : hf->discreteKeys()) {
data.discreteKeys.insert(k.first);
}

2
gtsam/hybrid/HybridNonlinearFactorGraph.cpp
View File

@ -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<HybridGaussianFactorGraph>();

6
gtsam/hybrid/MixtureFactor.h
View File

@ -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<NonlinearFactor>(f)) {
if (auto nf = std::dynamic_pointer_cast<NonlinearFactor>(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<NoiseModelFactor>(factor)) {
std::dynamic_pointer_cast<NoiseModelFactor>(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::Gaussian>(noiseModel);
std::dynamic_pointer_cast<noiseModel::Gaussian>(noiseModel);
if (gaussianNoiseModel) {
// If the noise model is Gaussian, retrieve the information matrix
infoMat = gaussianNoiseModel->information();

2
gtsam/hybrid/tests/Switching.h
View File

@ -161,7 +161,7 @@ struct Switching {
auto motion_models = motionModels(k, between_sigma);
std::vector<NonlinearFactor::shared_ptr> components;
for (auto &&f : motion_models) {
components.push_back(boost::dynamic_pointer_cast<NonlinearFactor>(f));
components.push_back(std::dynamic_pointer_cast<NonlinearFactor>(f));
}
nonlinearFactorGraph.emplace_shared<MixtureFactor>(
keys, DiscreteKeys{modes[k]}, components);

2
gtsam/hybrid/tests/testHybridBayesNet.cpp
View File

@ -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<DecisionTreeFactor::ADT>(
std::dynamic_pointer_cast<DecisionTreeFactor::ADT>(
pruned_discrete_conditionals);
// The checker functor verifies the values for us.

4
gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp
View File

@ -624,11 +624,11 @@ TEST(HybridGaussianFactorGraph, assembleGraphTree) {
// Create expected decision tree with two factor graphs:
// Get mixture factor:
auto mixture = boost::dynamic_pointer_cast<GaussianMixtureFactor>(fg.at(0));
auto mixture = std::dynamic_pointer_cast<GaussianMixtureFactor>(fg.at(0));
CHECK(mixture);
// Get prior factor:
const auto gf = boost::dynamic_pointer_cast<HybridConditional>(fg.at(1));
const auto gf = std::dynamic_pointer_cast<HybridConditional>(fg.at(1));
CHECK(gf);
using GF = GaussianFactor::shared_ptr;
const GF prior = gf->asGaussian();

6
gtsam/inference/graph-inl.h
View File

@ -80,7 +80,7 @@ SDGraph<KEY> toBoostGraph(const G& graph) {
continue;
// Cast the factor to the user-specified factor type F
std::shared_ptr<F> factor = boost::dynamic_pointer_cast<F>(*itFactor);
std::shared_ptr<F> factor = std::dynamic_pointer_cast<F>(*itFactor);
// Ignore factors that are not of type F
if (!factor) continue;
@ -197,7 +197,7 @@ std::shared_ptr<Values> composePoses(const G& graph, const PredecessorMap<KEY>&
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> factor = boost::dynamic_pointer_cast<Factor>(nl_factor);
std::shared_ptr<Factor> factor = std::dynamic_pointer_cast<Factor>(nl_factor);
if (!factor) continue;
KEY key1 = factor->key1();
@ -266,7 +266,7 @@ void split(const G& g, const PredecessorMap<KEY>& tree, G& Ab1, G& Ab2) {
continue;
}
std::shared_ptr<FACTOR2> factor2 = boost::dynamic_pointer_cast<
std::shared_ptr<FACTOR2> factor2 = std::dynamic_pointer_cast<
FACTOR2>(factor);
if (!factor2) continue;

8
gtsam/linear/GaussianFactorGraph.cpp
View File

@ -148,10 +148,10 @@ namespace gtsam {
// Convert to JacobianFactor if necessary
JacobianFactor::shared_ptr jacobianFactor(
boost::dynamic_pointer_cast<JacobianFactor>(factor));
std::dynamic_pointer_cast<JacobianFactor>(factor));
if (!jacobianFactor) {
HessianFactor::shared_ptr hessian(
boost::dynamic_pointer_cast<HessianFactor>(factor));
std::dynamic_pointer_cast<HessianFactor>(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<JacobianFactor>(gf);
JacobianFactor::shared_ptr result = std::dynamic_pointer_cast<JacobianFactor>(gf);
if( !result )
// Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky)
result = std::make_shared<JacobianFactor>(*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<J>(factor));
J::shared_ptr jacobian(std::dynamic_pointer_cast<J>(factor));
if (jacobian && jacobian->get_model() && jacobian->get_model()->isConstrained()) {
return true;
}

2
gtsam/linear/JacobianFactor.cpp
View File

@ -200,7 +200,7 @@ FastVector<JacobianFactor::shared_ptr> _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

2
gtsam/linear/JacobianFactor.h
View File

@ -187,7 +187,7 @@ namespace gtsam {
/** Clone this JacobianFactor */
GaussianFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<GaussianFactor>(
return std::static_pointer_cast<GaussianFactor>(
std::make_shared<JacobianFactor>(*this));
}

2
gtsam/linear/Preconditioner.cpp
View File

@ -185,7 +185,7 @@ void BlockJacobiPreconditioner::clean() {
/***************************************************************************************/
std::shared_ptr<Preconditioner> createPreconditioner(
const std::shared_ptr<PreconditionerParameters> params) {
using boost::dynamic_pointer_cast;
using std::dynamic_pointer_cast;
if (dynamic_pointer_cast<DummyPreconditionerParameters>(params)) {
return std::make_shared<DummyPreconditioner>();
} else if (dynamic_pointer_cast<BlockJacobiPreconditionerParameters>(

8
gtsam/linear/SubgraphBuilder.cpp
View File

@ -413,19 +413,19 @@ SubgraphBuilder::Weights SubgraphBuilder::weights(
break;
case SubgraphBuilderParameters::RHS_2NORM: {
if (JacobianFactor::shared_ptr jf =
boost::dynamic_pointer_cast<JacobianFactor>(gf)) {
std::dynamic_pointer_cast<JacobianFactor>(gf)) {
weight.push_back(jf->getb().norm());
} else if (HessianFactor::shared_ptr hf =
boost::dynamic_pointer_cast<HessianFactor>(gf)) {
std::dynamic_pointer_cast<HessianFactor>(gf)) {
weight.push_back(hf->linearTerm().norm());
}
} break;
case SubgraphBuilderParameters::LHS_FNORM: {
if (JacobianFactor::shared_ptr jf =
boost::dynamic_pointer_cast<JacobianFactor>(gf)) {
std::dynamic_pointer_cast<JacobianFactor>(gf)) {
weight.push_back(std::sqrt(jf->getA().squaredNorm()));
} else if (HessianFactor::shared_ptr hf =
boost::dynamic_pointer_cast<HessianFactor>(gf)) {
std::dynamic_pointer_cast<HessianFactor>(gf)) {
weight.push_back(std::sqrt(hf->information().squaredNorm()));
}
} break;

2
gtsam/linear/SubgraphPreconditioner.cpp
View File

@ -82,7 +82,7 @@ static GaussianFactorGraph convertToJacobianFactors(
GaussianFactorGraph result;
for (const auto &factor : gfg)
if (factor) {
auto jf = boost::dynamic_pointer_cast<JacobianFactor>(factor);
auto jf = std::dynamic_pointer_cast<JacobianFactor>(factor);
if (!jf) {
jf = std::make_shared<JacobianFactor>(*factor);
}

2
gtsam/linear/tests/testGaussianFactorGraph.cpp
View File

@ -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<JacobianFactor>(init_graph.at(0));
std::dynamic_pointer_cast<JacobianFactor>(init_graph.at(0));
CHECK(jacFactor0);
jacFactor0->getA(jacFactor0->begin()) *= 7.;
EXPECT(assert_inequal(init_graph, exp_graph));

4
gtsam/linear/tests/testJacobianFactor.cpp
View File

@ -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));

8
gtsam/linear/tests/testNoiseModel.cpp
View File

@ -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<Diagonal>(nonconstrained);
// Constrained::shared_ptr n2 = boost::dynamic_pointer_cast<Constrained>(nonconstrained);
// Diagonal::shared_ptr n1 = std::dynamic_pointer_cast<Diagonal>(nonconstrained);
// Constrained::shared_ptr n2 = std::dynamic_pointer_cast<Constrained>(nonconstrained);
// EXPECT(n1);
// EXPECT(!n2);
//
// Gaussian::shared_ptr constrained = Constrained::MixedSigmas(zero(3), true);
// Diagonal::shared_ptr c1 = boost::dynamic_pointer_cast<Diagonal>(constrained);
// Constrained::shared_ptr c2 = boost::dynamic_pointer_cast<Constrained>(constrained);
// Diagonal::shared_ptr c1 = std::dynamic_pointer_cast<Diagonal>(constrained);
// Constrained::shared_ptr c2 = std::dynamic_pointer_cast<Constrained>(constrained);
// EXPECT(c1);
// EXPECT(c2);
//}

2
gtsam/navigation/AHRSFactor.cpp
View File

@ -98,7 +98,7 @@ AHRSFactor::AHRSFactor(
gtsam::NonlinearFactor::shared_ptr AHRSFactor::clone() const {
//------------------------------------------------------------------------------
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
return std::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}

2
gtsam/navigation/AHRSFactor.h
View File

@ -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<Params>(p_);}
Params& p() const { return *std::static_pointer_cast<Params>(p_);}
const Vector3& biasHat() const { return biasHat_; }
const Matrix3& preintMeasCov() const { return preintMeasCov_; }

4
gtsam/navigation/AttitudeFactor.h
View File

@ -112,7 +112,7 @@ public:
/// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
return std::static_pointer_cast<gtsam::NonlinearFactor>(
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<gtsam::NonlinearFactor>(
return std::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}

2
gtsam/navigation/BarometricFactor.h
View File

@ -66,7 +66,7 @@ class GTSAM_EXPORT BarometricFactor : public NoiseModelFactorN<Pose3, double> {
/// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
return std::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}

2
gtsam/navigation/CombinedImuFactor.cpp
View File

@ -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<gtsam::NonlinearFactor>(
return std::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}

2
gtsam/navigation/CombinedImuFactor.h
View File

@ -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<Params>(this->p_); }
Params& p() const { return *std::static_pointer_cast<Params>(this->p_); }
/// @}
/// @name Access instance variables

4
gtsam/navigation/GPSFactor.h
View File

@ -69,7 +69,7 @@ public:
/// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
return std::static_pointer_cast<gtsam::NonlinearFactor>(
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<gtsam::NonlinearFactor>(
return std::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}

4
gtsam/navigation/ImuFactor.cpp
View File

@ -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<NonlinearFactor>(
return std::static_pointer_cast<NonlinearFactor>(
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<NonlinearFactor>(
return std::static_pointer_cast<NonlinearFactor>(
NonlinearFactor::shared_ptr(new This(*this)));
}

8
gtsam/navigation/MagFactor.h
View File

@ -59,7 +59,7 @@ public:
/// @return a deep copy of this factor
NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<NonlinearFactor>(
return std::static_pointer_cast<NonlinearFactor>(
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<NonlinearFactor>(
return std::static_pointer_cast<NonlinearFactor>(
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<NonlinearFactor>(
return std::static_pointer_cast<NonlinearFactor>(
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<NonlinearFactor>(
return std::static_pointer_cast<NonlinearFactor>(
NonlinearFactor::shared_ptr(new MagFactor3(*this)));
}

2
gtsam/navigation/MagPoseFactor.h
View File

@ -86,7 +86,7 @@ class MagPoseFactor: public NoiseModelFactorN<POSE> {
/// @return a deep copy of this factor.
NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<NonlinearFactor>(
return std::static_pointer_cast<NonlinearFactor>(
NonlinearFactor::shared_ptr(new This(*this)));
}

2
gtsam/navigation/ScenarioRunner.h
View File

@ -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<noiseModel::Diagonal>(model);
auto diagonal = std::dynamic_pointer_cast<noiseModel::Diagonal>(model);
if (!diagonal)
throw std::invalid_argument("ScenarioRunner::Diagonal: not a diagonal");
return diagonal;

4
gtsam/nonlinear/ExpressionFactor.h
View File

@ -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::Constrained>(
noiseModel = std::static_pointer_cast<noiseModel::Constrained>(
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<gtsam::NonlinearFactor>(
return std::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}

4
gtsam/nonlinear/FunctorizedFactor.h
View File

@ -87,7 +87,7 @@ class FunctorizedFactor : public NoiseModelFactorN<T> {
/// @return a deep copy of this factor
NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<NonlinearFactor>(
return std::static_pointer_cast<NonlinearFactor>(
NonlinearFactor::shared_ptr(new FunctorizedFactor<R, T>(*this)));
}
@ -194,7 +194,7 @@ class FunctorizedFactor2 : public NoiseModelFactorN<T1, T2> {
/// @return a deep copy of this factor
NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<NonlinearFactor>(
return std::static_pointer_cast<NonlinearFactor>(
NonlinearFactor::shared_ptr(new FunctorizedFactor2<R, T1, T2>(*this)));
}

8
gtsam/nonlinear/GncOptimizer.h
View File

@ -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<noiseModel::Gaussian>(
std::dynamic_pointer_cast<noiseModel::Gaussian>(
factor->noiseModel());
if (noiseModel) {
Matrix newInfo = weights[i] * noiseModel->information();

2
gtsam/nonlinear/ISAM2.cpp
View File

@ -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();

16
gtsam/nonlinear/LinearContainerFactor.cpp
View File

@ -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<JacobianFactor>(linFactor);
JacobianFactor::shared_ptr jacFactor = std::dynamic_pointer_cast<JacobianFactor>(linFactor);
jacFactor->getb() = -jacFactor->unweighted_error(delta);
} else {
HessianFactor::shared_ptr hesFactor = boost::dynamic_pointer_cast<HessianFactor>(linFactor);
HessianFactor::shared_ptr hesFactor = std::dynamic_pointer_cast<HessianFactor>(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<JacobianFactor>(factor_);
return std::dynamic_pointer_cast<JacobianFactor>(factor_);
}
/* ************************************************************************* */
HessianFactor::shared_ptr LinearContainerFactor::toHessian() const {
return boost::dynamic_pointer_cast<HessianFactor>(factor_);
return std::dynamic_pointer_cast<HessianFactor>(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<LinearContainerFactor>(rekeyed_base_factor);
auto new_factor = std::static_pointer_cast<LinearContainerFactor>(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<NonlinearFactor>(new_factor);
return std::static_pointer_cast<NonlinearFactor>(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<LinearContainerFactor>(rekeyed_base_factor);
auto new_factor = std::static_pointer_cast<LinearContainerFactor>(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<NonlinearFactor>(new_factor);
return std::static_pointer_cast<NonlinearFactor>(new_factor);
}
/* ************************************************************************* */

6
gtsam/nonlinear/NonlinearEquality.h
View File

@ -172,7 +172,7 @@ public:
/// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
return std::static_pointer_cast<gtsam::NonlinearFactor>(
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<gtsam::NonlinearFactor>(
return std::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}
@ -326,7 +326,7 @@ class NonlinearEquality2 : public NoiseModelFactorN<T, T> {
/// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
return std::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}

4
gtsam/nonlinear/NonlinearFactor.cpp
View File

@ -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<NoiseModelFactor>(clone());
NoiseModelFactor::shared_ptr new_factor = std::dynamic_pointer_cast<NoiseModelFactor>(clone());
new_factor->noiseModel_ = newNoise;
return new_factor;
}
@ -177,7 +177,7 @@ std::shared_ptr<GaussianFactor> NoiseModelFactor::linearize(
if (noiseModel_ && noiseModel_->isConstrained())
return GaussianFactor::shared_ptr(
new JacobianFactor(terms, b,
boost::static_pointer_cast<Constrained>(noiseModel_)->unit()));
std::static_pointer_cast<Constrained>(noiseModel_)->unit()));
else {
return GaussianFactor::shared_ptr(new JacobianFactor(terms, b));
}

4
gtsam/nonlinear/NonlinearOptimizer.cpp
View File

@ -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<PCGSolverParameters>(
if (auto pcg = std::dynamic_pointer_cast<PCGSolverParameters>(
params.iterativeParams)) {
delta = PCGSolver(*pcg).optimize(gfg);
} else if (auto spcg =
boost::dynamic_pointer_cast<SubgraphSolverParameters>(
std::dynamic_pointer_cast<SubgraphSolverParameters>(
params.iterativeParams)) {
if (!params.ordering)
throw std::runtime_error("SubgraphSolver needs an ordering");

2
gtsam/nonlinear/PriorFactor.h
View File

@ -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<gtsam::NonlinearFactor>(
return std::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/** implement functions needed for Testable */

2
gtsam/nonlinear/WhiteNoiseFactor.h
View File

@ -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<gtsam::NonlinearFactor>(
// return std::static_pointer_cast<gtsam::NonlinearFactor>(
// gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/// @}

2
gtsam/nonlinear/factorTesting.h
View File

@ -83,7 +83,7 @@ inline bool testFactorJacobians(const std::string& name_,
// Create actual value by linearize
auto actual =
boost::dynamic_pointer_cast<JacobianFactor>(factor.linearize(values));
std::dynamic_pointer_cast<JacobianFactor>(factor.linearize(values));
if (!actual) return false;
// Check cast result and then equality

4
gtsam/nonlinear/tests/testLinearContainerFactor.cpp
View File

@ -345,7 +345,7 @@ TEST(TestLinearContainerFactor, Rekey) {
// Cast back to LCF ptr
LinearContainerFactor::shared_ptr lcf_factor_rekey_ptr =
boost::static_pointer_cast<LinearContainerFactor>(lcf_factor_rekeyed);
std::static_pointer_cast<LinearContainerFactor>(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<LinearContainerFactor>(
std::static_pointer_cast<LinearContainerFactor>(
lcf_factor.rekey(key_map));
CHECK(lcf_factor_rekey_ptr);
}

4
gtsam/nonlinear/utilities.h
View File

@ -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<const GenericProjectionFactor<Pose3, Point3> >(
if (std::dynamic_pointer_cast<const GenericProjectionFactor<Pose3, Point3> >(
f))
++K;
// now fill
Matrix errors(2, K);
for(const NonlinearFactor::shared_ptr& f: graph) {
std::shared_ptr<const GenericProjectionFactor<Pose3, Point3> > p =
boost::dynamic_pointer_cast<const GenericProjectionFactor<Pose3, Point3> >(
std::dynamic_pointer_cast<const GenericProjectionFactor<Pose3, Point3> >(
f);
if (p)
errors.col(k++) = p->unwhitenedError(values);

2
gtsam/sam/BearingRangeFactor.h
View File

@ -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<gtsam::NonlinearFactor>(
return std::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}

4
gtsam/sam/RangeFactor.h
View File

@ -48,7 +48,7 @@ class RangeFactor : public ExpressionFactorN<T, A1, A2> {
/// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
return std::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}
@ -121,7 +121,7 @@ class RangeFactorWithTransform : public ExpressionFactorN<T, A1, A2> {
/// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
return std::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}

8
gtsam/sfm/ShonanAveraging.cpp
View File

@ -337,13 +337,13 @@ double ShonanAveraging<d>::cost(const Values &values) const {
template <typename T, size_t d>
static double Kappa(const BinaryMeasurement<T> &measurement,
const ShonanAveragingParameters<d> &parameters) {
const auto &isotropic = boost::dynamic_pointer_cast<noiseModel::Isotropic>(
const auto &isotropic = std::dynamic_pointer_cast<noiseModel::Isotropic>(
measurement.noiseModel());
double sigma;
if (isotropic) {
sigma = isotropic->sigma();
} else {
const auto &robust = boost::dynamic_pointer_cast<noiseModel::Robust>(
const auto &robust = std::dynamic_pointer_cast<noiseModel::Robust>(
measurement.noiseModel());
// Check if noise model is robust
if (robust) {
@ -949,7 +949,7 @@ ShonanAveraging2::ShonanAveraging2(string g2oFile, const Parameters &parameters)
static BinaryMeasurement<Rot2> convertPose2ToBinaryMeasurementRot2(
const BetweenFactor<Pose2>::shared_ptr &f) {
auto gaussian =
boost::dynamic_pointer_cast<noiseModel::Gaussian>(f->noiseModel());
std::dynamic_pointer_cast<noiseModel::Gaussian>(f->noiseModel());
if (!gaussian)
throw std::invalid_argument(
"parseMeasurements<Rot2> can only convert Pose2 measurements "
@ -997,7 +997,7 @@ ShonanAveraging3::ShonanAveraging3(string g2oFile, const Parameters &parameters)
static BinaryMeasurement<Rot3> convert(
const BetweenFactor<Pose3>::shared_ptr &f) {
auto gaussian =
boost::dynamic_pointer_cast<noiseModel::Gaussian>(f->noiseModel());
std::dynamic_pointer_cast<noiseModel::Gaussian>(f->noiseModel());
if (!gaussian)
throw std::invalid_argument(
"parseMeasurements<Rot3> can only convert Pose3 measurements "

2
gtsam/sfm/ShonanAveraging.h
View File

@ -184,7 +184,7 @@ class GTSAM_EXPORT ShonanAveraging {
for (auto &measurement : measurements) {
auto model = measurement.noiseModel();
const auto &robust =
boost::dynamic_pointer_cast<noiseModel::Robust>(model);
std::dynamic_pointer_cast<noiseModel::Robust>(model);
SharedNoiseModel robust_model;
// Check if the noise model is already robust

2
gtsam/sfm/tests/testBinaryMeasurement.cpp
View File

@ -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<noiseModel::Robust>(
const auto &robust = std::dynamic_pointer_cast<noiseModel::Robust>(
rot3Measurement.noiseModel());
EXPECT(robust);
}

6
gtsam/sfm/tests/testShonanAveraging.cpp
View File

@ -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<noiseModel::Robust>(
boost::dynamic_pointer_cast<NoiseModelFactor>(graph[i])->noiseModel());
const auto &robust = std::dynamic_pointer_cast<noiseModel::Robust>(
std::dynamic_pointer_cast<NoiseModelFactor>(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<noiseModel::Isotropic>(m.noiseModel());
std::static_pointer_cast<noiseModel::Isotropic>(m.noiseModel());
CHECK(isotropic != nullptr);
EXPECT_LONGS_EQUAL(3, isotropic->dim());
EXPECT_DOUBLES_EQUAL(0.2, isotropic->sigma(), 1e-9);

6
gtsam/sfm/tests/testShonanGaugeFactor.cpp
View File

@ -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<JacobianFactor>(
EXPECT(assert_equal(linearized, *std::dynamic_pointer_cast<JacobianFactor>(
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<JacobianFactor>(
EXPECT(assert_equal(linearized, *std::dynamic_pointer_cast<JacobianFactor>(
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<JacobianFactor>(
EXPECT(assert_equal(linearized, *std::dynamic_pointer_cast<JacobianFactor>(
factor.linearize(values))));
}

2
gtsam/slam/AntiFactor.h
View File

@ -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<gtsam::NonlinearFactor>(
return std::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/** implement functions needed for Testable */

2
gtsam/slam/BetweenFactor.h
View File

@ -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<gtsam::NonlinearFactor>(
return std::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/// @name Testable

2
gtsam/slam/EssentialMatrixConstraint.h
View File

@ -65,7 +65,7 @@ public:
/// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
return std::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}

8
gtsam/slam/EssentialMatrixFactor.h
View File

@ -78,7 +78,7 @@ class EssentialMatrixFactor : public NoiseModelFactorN<EssentialMatrix> {
/// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
return std::static_pointer_cast<gtsam::NonlinearFactor>(
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<gtsam::NonlinearFactor>(
return std::static_pointer_cast<gtsam::NonlinearFactor>(
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<gtsam::NonlinearFactor>(
return std::static_pointer_cast<gtsam::NonlinearFactor>(
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<gtsam::NonlinearFactor>(
return std::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}

4
gtsam/slam/FrobeniusFactor.cpp
View File

@ -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<noiseModel::Robust>(model);
const auto &robust = std::dynamic_pointer_cast<noiseModel::Robust>(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<noiseModel::Robust>(model);
const auto &robust = std::dynamic_pointer_cast<noiseModel::Robust>(model);
if (robust) {
return noiseModel::Robust::Create(
noiseModel::mEstimator::Huber::Create(1.345), isoModel);

6
gtsam/slam/GeneralSFMFactor.h
View File

@ -102,7 +102,7 @@ public:
/// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
return std::static_pointer_cast<gtsam::NonlinearFactor>(
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::Constrained>(noiseModel)->unit();
model = std::static_pointer_cast<noiseModel::Constrained>(noiseModel)->unit();
}
return std::make_shared<BinaryJacobianFactor<2, DimC, DimL> >(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<gtsam::NonlinearFactor>(
return std::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this)));}
/**

4
gtsam/slam/InitializePose.h
View File

@ -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<BetweenFactor<Pose> >(factor))
std::dynamic_pointer_cast<BetweenFactor<Pose> >(factor))
poseGraph.add(between);
// recast PriorFactor<Pose> to BetweenFactor<Pose>
if (auto prior = boost::dynamic_pointer_cast<PriorFactor<Pose> >(factor))
if (auto prior = std::dynamic_pointer_cast<PriorFactor<Pose> >(factor))
poseGraph.emplace_shared<BetweenFactor<Pose> >(
kAnchorKey, prior->keys()[0], prior->prior(), prior->noiseModel());
}

4
gtsam/slam/InitializePose3.cpp
View File

@ -44,7 +44,7 @@ GaussianFactorGraph InitializePose3::buildLinearOrientationGraph(const Nonlinear
Matrix3 Rij;
double rotationPrecision = 1.0;
auto pose3Between = boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
auto pose3Between = std::dynamic_pointer_cast<BetweenFactor<Pose3> >(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<BetweenFactor<Pose3> >(factor);
std::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
if (pose3Between) {
Rot3 Rij = pose3Between->measured().rotation();
factorId2RotMap->emplace(factorId, Rij);

2
gtsam/slam/PoseRotationPrior.h
View File

@ -57,7 +57,7 @@ public:
/// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
return std::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
// access

2
gtsam/slam/PoseTranslationPrior.h
View File

@ -58,7 +58,7 @@ public:
/// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
return std::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/** h(x)-z */

2
gtsam/slam/ProjectionFactor.h
View File

@ -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<gtsam::NonlinearFactor>(
return std::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/**

2
gtsam/slam/ReferenceFrameFactor.h
View File

@ -93,7 +93,7 @@ public:
~ReferenceFrameFactor() override{}
NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<NonlinearFactor>(
return std::static_pointer_cast<NonlinearFactor>(
NonlinearFactor::shared_ptr(new This(*this))); }
/** Combined cost and derivative function using boost::optional */

4
gtsam/slam/RotateFactor.h
View File

@ -40,7 +40,7 @@ public:
/// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
return std::static_pointer_cast<gtsam::NonlinearFactor>(
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<gtsam::NonlinearFactor>(
return std::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/// print

2
gtsam/slam/SmartFactorBase.h
View File

@ -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)

2
gtsam/slam/StereoFactor.h
View File

@ -96,7 +96,7 @@ public:
/// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
return std::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/**

2
gtsam/slam/TriangulationFactor.h
View File

@ -97,7 +97,7 @@ public:
/// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
return std::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}

16
gtsam/slam/dataset.cpp
View File

@ -373,7 +373,7 @@ template <> struct ParseMeasurement<Pose2> {
/* ************************************************************************* */
// Create a sampler to corrupt a measurement
std::shared_ptr<Sampler> createSampler(const SharedNoiseModel &model) {
auto noise = boost::dynamic_pointer_cast<noiseModel::Diagonal>(model);
auto noise = std::dynamic_pointer_cast<noiseModel::Diagonal>(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<Rot2> convert(const BinaryMeasurement<Pose2> &p) {
auto gaussian =
boost::dynamic_pointer_cast<noiseModel::Gaussian>(p.noiseModel());
std::dynamic_pointer_cast<noiseModel::Gaussian>(p.noiseModel());
if (!gaussian)
throw std::invalid_argument(
"parseMeasurements<Rot2> 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<BetweenFactor<Pose2>>(f);
auto factor = std::dynamic_pointer_cast<BetweenFactor<Pose2>>(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<BetweenFactor<Pose2>>(factor_);
auto factor = std::dynamic_pointer_cast<BetweenFactor<Pose2>>(factor_);
if (factor) {
SharedNoiseModel model = factor->noiseModel();
auto gaussianModel =
boost::dynamic_pointer_cast<noiseModel::Gaussian>(model);
std::dynamic_pointer_cast<noiseModel::Gaussian>(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<BetweenFactor<Pose3>>(factor_);
auto factor3D = std::dynamic_pointer_cast<BetweenFactor<Pose3>>(factor_);
if (factor3D) {
SharedNoiseModel model = factor3D->noiseModel();
std::shared_ptr<noiseModel::Gaussian> gaussianModel =
boost::dynamic_pointer_cast<noiseModel::Gaussian>(model);
std::dynamic_pointer_cast<noiseModel::Gaussian>(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<Rot3> convert(const BinaryMeasurement<Pose3> &p) {
auto gaussian =
boost::dynamic_pointer_cast<noiseModel::Gaussian>(p.noiseModel());
std::dynamic_pointer_cast<noiseModel::Gaussian>(p.noiseModel());
if (!gaussian)
throw std::invalid_argument(
"parseMeasurements<Rot3> can only convert Pose3 measurements "

10
gtsam/slam/lago.cpp
View File

@ -108,7 +108,7 @@ void getSymbolicGraph(
Key key2 = factor->keys()[1];
// recast to a between
std::shared_ptr<BetweenFactor<Pose2> > pose2Between =
boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor);
std::dynamic_pointer_cast<BetweenFactor<Pose2> >(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<BetweenFactor<Pose2> > pose2Between =
boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor);
std::dynamic_pointer_cast<BetweenFactor<Pose2> >(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<noiseModel::Diagonal> diagonalModel =
boost::dynamic_pointer_cast<noiseModel::Diagonal>(model);
std::dynamic_pointer_cast<noiseModel::Diagonal>(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<NonlinearFactor>& factor: pose2graph) {
std::shared_ptr<BetweenFactor<Pose2> > pose2Between =
boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor);
std::dynamic_pointer_cast<BetweenFactor<Pose2> >(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<noiseModel::Diagonal> diagonalModel =
boost::dynamic_pointer_cast<noiseModel::Diagonal>(
std::dynamic_pointer_cast<noiseModel::Diagonal>(
pose2Between->noiseModel());
linearPose2graph.add(key1, J1, key2, I3, b, diagonalModel);

2
gtsam/slam/tests/testAntiFactor.cpp
View File

@ -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<HessianFactor>(antiGaussian);
HessianFactor::shared_ptr antiHessian = std::dynamic_pointer_cast<HessianFactor>(antiGaussian);
Matrix expected_information = -originalHessian->information();
Matrix actual_information = antiHessian->information();

6
gtsam/slam/tests/testDataset.cpp
View File

@ -101,7 +101,7 @@ TEST(dataSet, load2D) {
BetweenFactor<Pose2> expected(1, 0, Pose2(-0.99879, 0.0417574, -0.00818381),
model);
BetweenFactor<Pose2>::shared_ptr actual =
boost::dynamic_pointer_cast<BetweenFactor<Pose2>>(graph->at(0));
std::dynamic_pointer_cast<BetweenFactor<Pose2>>(graph->at(0));
EXPECT(assert_equal(expected, *actual));
// Check binary measurements, Pose2
@ -117,7 +117,7 @@ TEST(dataSet, load2D) {
const auto actualFactors = parseFactors<Pose2>(filename);
for (size_t i : {0, 1, 2, 3, 4, 5}) {
EXPECT(assert_equal(
*boost::dynamic_pointer_cast<BetweenFactor<Pose2>>(graph->at(i)),
*std::dynamic_pointer_cast<BetweenFactor<Pose2>>(graph->at(i)),
*actualFactors[i], 1e-5));
}
@ -201,7 +201,7 @@ TEST(dataSet, readG2o3D) {
const auto actualFactors = parseFactors<Pose3>(g2oFile);
for (size_t i : {0, 1, 2, 3, 4, 5}) {
EXPECT(assert_equal(
*boost::dynamic_pointer_cast<BetweenFactor<Pose3>>(expectedGraph[i]),
*std::dynamic_pointer_cast<BetweenFactor<Pose3>>(expectedGraph[i]),
*actualFactors[i], 1e-5));
}

4
gtsam_unstable/discrete/CSP.cpp
View File

@ -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<Constraint>((*this)[f]);
auto constraint = std::dynamic_pointer_cast<Constraint>((*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<Constraint>(f);
auto constraint = std::dynamic_pointer_cast<Constraint>(f);
if (!constraint)
throw runtime_error("CSP:runArcConsistency: non-constraint factor");
Constraint::shared_ptr reduced = constraint->partiallyApply(domains);

12
gtsam_unstable/discrete/tests/testLoopyBelief.cpp
View File

@ -112,11 +112,11 @@ class LoopyBelief {
// Incorporate new the factor to belief
if (!beliefAtKey)
beliefAtKey =
boost::dynamic_pointer_cast<DecisionTreeFactor>(message);
std::dynamic_pointer_cast<DecisionTreeFactor>(message);
else
beliefAtKey = std::make_shared<DecisionTreeFactor>(
(*beliefAtKey) *
(*boost::dynamic_pointer_cast<DecisionTreeFactor>(message)));
(*std::dynamic_pointer_cast<DecisionTreeFactor>(message)));
}
if (starGraphs_.at(key).unary)
beliefAtKey = std::make_shared<DecisionTreeFactor>(
@ -148,9 +148,9 @@ class LoopyBelief {
for (Key neighbor : starGraphs_.at(key).correctedBeliefIndices |
boost::adaptors::map_keys) {
DecisionTreeFactor correctedBelief =
(*boost::dynamic_pointer_cast<DecisionTreeFactor>(
(*std::dynamic_pointer_cast<DecisionTreeFactor>(
beliefs->at(beliefFactors[key].front()))) /
(*boost::dynamic_pointer_cast<DecisionTreeFactor>(
(*std::dynamic_pointer_cast<DecisionTreeFactor>(
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<DecisionTreeFactor>(
prodOfUnaries = std::dynamic_pointer_cast<DecisionTreeFactor>(
graph.at(factorIndex));
else
prodOfUnaries = std::make_shared<DecisionTreeFactor>(
*prodOfUnaries *
(*boost::dynamic_pointer_cast<DecisionTreeFactor>(
(*std::dynamic_pointer_cast<DecisionTreeFactor>(
graph.at(factorIndex))));
}
}

2
gtsam_unstable/dynamics/FullIMUFactor.h
View File

@ -57,7 +57,7 @@ public:
/// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
return std::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/** Check if two factors are equal */

2
gtsam_unstable/dynamics/IMUFactor.h
View File

@ -50,7 +50,7 @@ public:
/// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
return std::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/** Check if two factors are equal */

8
gtsam_unstable/dynamics/Pendulum.h
View File

@ -44,7 +44,7 @@ public:
/// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
return std::static_pointer_cast<gtsam::NonlinearFactor>(
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<gtsam::NonlinearFactor>(
return std::static_pointer_cast<gtsam::NonlinearFactor>(
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<gtsam::NonlinearFactor>(
return std::static_pointer_cast<gtsam::NonlinearFactor>(
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<gtsam::NonlinearFactor>(
return std::static_pointer_cast<gtsam::NonlinearFactor>(
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 */

4
gtsam_unstable/dynamics/SimpleHelicopter.h
View File

@ -41,7 +41,7 @@ public:
/// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
return std::static_pointer_cast<gtsam::NonlinearFactor>(
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<gtsam::NonlinearFactor>(
return std::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new DiscreteEulerPoincareHelicopter(*this))); }
/** DEP, with optional derivatives

2
gtsam_unstable/dynamics/VelocityConstraint.h
View File

@ -79,7 +79,7 @@ public:
/// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
return std::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new VelocityConstraint(*this))); }
/**

2
gtsam_unstable/dynamics/VelocityConstraint3.h
View File

@ -35,7 +35,7 @@ public:
/// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
return std::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new VelocityConstraint3(*this))); }
/** x1 + v*dt - x2 = 0, with optional derivatives */

2
gtsam_unstable/linear/LinearCost.h
View File

@ -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));
}

2
gtsam_unstable/linear/LinearEquality.h
View File

@ -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));
}

2
gtsam_unstable/linear/LinearInequality.h
View File

@ -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));
}

6
gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp
View File

@ -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<LinearContainerFactor>(factor)) {
if(std::dynamic_pointer_cast<LinearContainerFactor>(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<JacobianFactor>(factor);
HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast<HessianFactor>(factor);
JacobianFactor::shared_ptr jf = std::dynamic_pointer_cast<JacobianFactor>(factor);
HessianFactor::shared_ptr hf = std::dynamic_pointer_cast<HessianFactor>(factor);
if(jf) {
std::cout << "j( ";
} else if(hf) {

2
gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp
View File

@ -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<LinearContainerFactor>(factor)) {
if(std::dynamic_pointer_cast<LinearContainerFactor>(factor)) {
std::cout << "l( ";
} else {
std::cout << "f( ";

4
gtsam_unstable/nonlinear/LinearizedFactor.h
View File

@ -113,7 +113,7 @@ public:
/// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
return std::static_pointer_cast<gtsam::NonlinearFactor>(
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<gtsam::NonlinearFactor>(
return std::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
// Testable

2
gtsam_unstable/nonlinear/NonlinearClusterTree.h
View File

@ -37,7 +37,7 @@ class NonlinearClusterTree : public ClusterTree<NonlinearFactorGraph> {
}
static NonlinearCluster* DownCast(const std::shared_ptr<Cluster>& cluster) {
auto nonlinearCluster = boost::dynamic_pointer_cast<NonlinearCluster>(cluster);
auto nonlinearCluster = std::dynamic_pointer_cast<NonlinearCluster>(cluster);
if (!nonlinearCluster)
throw std::runtime_error("Expected NonlinearCluster");
return nonlinearCluster.get();

4
gtsam_unstable/nonlinear/tests/testCustomChartExpression.cpp
View File

@ -82,7 +82,7 @@ TEST(ExpressionCustomChart, projection) {
std::shared_ptr<GaussianFactor> gfstandard = f.linearize(standard);
std::shared_ptr<JacobianFactor> jfstandard = //
boost::dynamic_pointer_cast<JacobianFactor>(gfstandard);
std::dynamic_pointer_cast<JacobianFactor>(gfstandard);
typedef std::pair<Eigen::MatrixXd, Eigen::VectorXd> Jacobian;
Jacobian Jstandard = jfstandard->jacobianUnweighted();
@ -90,7 +90,7 @@ TEST(ExpressionCustomChart, projection) {
std::shared_ptr<GaussianFactor> gfcustom = f.linearize(custom);
std::shared_ptr<JacobianFactor> jfcustom = //
boost::dynamic_pointer_cast<JacobianFactor>(gfcustom);
std::dynamic_pointer_cast<JacobianFactor>(gfcustom);
Eigen::MatrixXd expectedJacobian = Eigen::MatrixXd::Zero(3,2);
expectedJacobian(0,0) = 2.0;

24
gtsam_unstable/nonlinear/tests/testLinearizedFactor.cpp
View File

@ -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<JacobianFactor>(betweenFactor.linearize(values));
JacobianFactor::shared_ptr jf = std::static_pointer_cast<JacobianFactor>(betweenFactor.linearize(values));
LinearizedJacobianFactor jacobian1(jf, values);
LinearizedJacobianFactor jacobian2(jf, values);
@ -65,13 +65,13 @@ TEST( LinearizedFactor, clone_jacobian )
BetweenFactor<Point3> 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<JacobianFactor>(betweenFactor.linearize(values));
JacobianFactor::shared_ptr jf = std::static_pointer_cast<JacobianFactor>(betweenFactor.linearize(values));
LinearizedJacobianFactor jacobian1(jf, values);
LinearizedJacobianFactor::shared_ptr jacobian2 = boost::static_pointer_cast<LinearizedJacobianFactor>(jacobian1.clone());
LinearizedJacobianFactor::shared_ptr jacobian2 = std::static_pointer_cast<LinearizedJacobianFactor>(jacobian1.clone());
CHECK(assert_equal(jacobian1, *jacobian2));
JacobianFactor::shared_ptr jf1 = boost::static_pointer_cast<JacobianFactor>(jacobian1.linearize(values));
JacobianFactor::shared_ptr jf2 = boost::static_pointer_cast<JacobianFactor>(jacobian2->linearize(values));
JacobianFactor::shared_ptr jf1 = std::static_pointer_cast<JacobianFactor>(jacobian1.linearize(values));
JacobianFactor::shared_ptr jf2 = std::static_pointer_cast<JacobianFactor>(jacobian2->linearize(values));
CHECK(assert_equal(*jf1, *jf2));
Matrix information1 = jf1->augmentedInformation();
@ -94,7 +94,7 @@ TEST( LinearizedFactor, add_jacobian )
BetweenFactor<Point3> 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<JacobianFactor>(betweenFactor.linearize(values));
JacobianFactor::shared_ptr jf = std::static_pointer_cast<JacobianFactor>(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<JacobianFactor>(betweenFactor.linearize(values));
// JacobianFactor::shared_ptr jf = std::static_pointer_cast<JacobianFactor>(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<JacobianFactor>(betweenFactor.linearize(values));
JacobianFactor::shared_ptr jf = std::static_pointer_cast<JacobianFactor>(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<JacobianFactor>(betweenFactor.linearize(values));
JacobianFactor::shared_ptr jf = std::static_pointer_cast<JacobianFactor>(betweenFactor.linearize(values));
HessianFactor::shared_ptr hf(new HessianFactor(*jf));
LinearizedHessianFactor hessian1(hf, values);
LinearizedHessianFactor::shared_ptr hessian2 = boost::static_pointer_cast<LinearizedHessianFactor>(hessian1.clone());
LinearizedHessianFactor::shared_ptr hessian2 = std::static_pointer_cast<LinearizedHessianFactor>(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<JacobianFactor>(betweenFactor.linearize(values));
JacobianFactor::shared_ptr jf = std::static_pointer_cast<JacobianFactor>(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<JacobianFactor>(betweenFactor.linearize(values));
// JacobianFactor::shared_ptr jf = std::static_pointer_cast<JacobianFactor>(betweenFactor.linearize(values));
// HessianFactor::shared_ptr hf(new HessianFactor(*jf));
// LinearizedHessianFactor hessian(hf, values);
//

Some files were not shown because too many files have changed in this diff Show More