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 // circumstances, the following code that employs the default copy constructor should
// work fine. // work fine.
gtsam::NonlinearFactor::shared_ptr clone() const override { 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))); } gtsam::NonlinearFactor::shared_ptr(new UnaryFactor(*this))); }
// Additionally, we encourage you the use of unit testing your custom factors, // 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; NonlinearFactorGraph simpleGraph;
for(const std::shared_ptr<NonlinearFactor>& factor: *graph) { for(const std::shared_ptr<NonlinearFactor>& factor: *graph) {
std::shared_ptr<BetweenFactor<Pose3> > pose3Between = std::shared_ptr<BetweenFactor<Pose3> > pose3Between =
boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor); std::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
if (pose3Between){ if (pose3Between){
Key key1, key2; Key key1, key2;
if(add){ 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) { for (size_t j = 0; j < points.size(); ++j) {
// The graph stores Factor shared_ptrs, so we cast back to a SmartFactor first // 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) { if (smart) {
// The output of point() is in std::optional<Point3>, as sometimes // The output of point() is in std::optional<Point3>, as sometimes
// the triangulation operation inside smart factor will encounter degeneracy. // 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"); result.print("Final results:\n");
Values landmark_result; Values landmark_result;
for (size_t j = 0; j < points.size(); ++j) { 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) { if (smart) {
std::optional<Point3> point = smart->point(result); std::optional<Point3> point = smart->point(result);
if (point) // ignore if std::optional return nullptr if (point) // ignore if std::optional return nullptr

6
examples/SolverComparer.cpp
View File

@ -259,7 +259,7 @@ void runIncremental()
while(nextMeasurement < datasetMeasurements.size()) while(nextMeasurement < datasetMeasurements.size())
{ {
if(BetweenFactor<Pose>::shared_ptr factor = 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>(); Key key1 = factor->key<1>(), key2 = factor->key<2>();
if(((int)key1 >= firstStep && key1 < key2) || ((int)key2 >= firstStep && key2 < key1)) { if(((int)key1 >= firstStep && key1 < key2) || ((int)key2 >= firstStep && key2 < key1)) {
@ -310,7 +310,7 @@ void runIncremental()
NonlinearFactor::shared_ptr measurementf = datasetMeasurements[nextMeasurement]; NonlinearFactor::shared_ptr measurementf = datasetMeasurements[nextMeasurement];
if(BetweenFactor<Pose>::shared_ptr factor = 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 // Stop collecting measurements that are for future steps
if(factor->key<1>() > step || factor->key<2>() > step) if(factor->key<1>() > step || factor->key<2>() > step)
@ -346,7 +346,7 @@ void runIncremental()
} }
} }
else if(BearingRangeFactor<Pose, Point2>::shared_ptr factor = 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]; 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 linearizedFactor = predictFactor.linearize(values);
auto linearizedJacobianFactor = auto linearizedJacobianFactor =
boost::dynamic_pointer_cast<JacobianFactor>(linearizedFactor); std::dynamic_pointer_cast<JacobianFactor>(linearizedFactor);
CHECK(linearizedJacobianFactor); // makes sure it's indeed a JacobianFactor CHECK(linearizedJacobianFactor); // makes sure it's indeed a JacobianFactor
EXPECT(assert_equal(linearFactor, *linearizedJacobianFactor, 1e-9)); 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()) { for(auto branch: f->branches()) {
assert(branch->isLeaf()); assert(branch->isLeaf());
nrAssignments += nrAssignments +=
boost::dynamic_pointer_cast<const Leaf>(branch)->nrAssignments(); std::dynamic_pointer_cast<const Leaf>(branch)->nrAssignments();
} }
NodePtr newLeaf( NodePtr newLeaf(
new Leaf(boost::dynamic_pointer_cast<const Leaf>(f0)->constant(), new Leaf(std::dynamic_pointer_cast<const Leaf>(f0)->constant(),
nrAssignments)); nrAssignments));
return newLeaf; return newLeaf;
} else } else
@ -569,7 +569,7 @@ namespace gtsam {
if (it->root_->isLeaf()) if (it->root_->isLeaf())
continue; continue;
std::shared_ptr<const Choice> c = 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) { if (!highestLabel || c->label() > *highestLabel) {
highestLabel = c->label(); highestLabel = c->label();
nrChoices = c->nrChoices(); nrChoices = c->nrChoices();
@ -678,13 +678,13 @@ namespace gtsam {
// functions. // functions.
// If leaf, apply unary conversion "op" and create a unique leaf. // If leaf, apply unary conversion "op" and create a unique leaf.
using MXLeaf = typename DecisionTree<M, X>::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())); return NodePtr(new Leaf(Y_of_X(leaf->constant()), leaf->nrAssignments()));
} }
// Check if Choice // Check if Choice
using MXChoice = typename DecisionTree<M, X>::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( if (!choice) throw std::invalid_argument(
"DecisionTree::convertFrom: Invalid NodePtr"); "DecisionTree::convertFrom: Invalid NodePtr");
@ -720,11 +720,11 @@ namespace gtsam {
/// Do a depth-first visit on the tree rooted at node. /// Do a depth-first visit on the tree rooted at node.
void operator()(const typename DecisionTree<L, Y>::NodePtr& node) const { void operator()(const typename DecisionTree<L, Y>::NodePtr& node) const {
using Leaf = typename DecisionTree<L, Y>::Leaf; 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()); return f(leaf->constant());
using Choice = typename DecisionTree<L, Y>::Choice; 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) if (!choice)
throw std::invalid_argument("DecisionTree::Visit: Invalid NodePtr"); throw std::invalid_argument("DecisionTree::Visit: Invalid NodePtr");
for (auto&& branch : choice->branches()) (*this)(branch); // recurse! 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. /// Do a depth-first visit on the tree rooted at node.
void operator()(const typename DecisionTree<L, Y>::NodePtr& node) const { void operator()(const typename DecisionTree<L, Y>::NodePtr& node) const {
using Leaf = typename DecisionTree<L, Y>::Leaf; 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); return f(*leaf);
using Choice = typename DecisionTree<L, Y>::Choice; 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) if (!choice)
throw std::invalid_argument("DecisionTree::VisitLeaf: Invalid NodePtr"); throw std::invalid_argument("DecisionTree::VisitLeaf: Invalid NodePtr");
for (auto&& branch : choice->branches()) (*this)(branch); // recurse! 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. /// Do a depth-first visit on the tree rooted at node.
void operator()(const typename DecisionTree<L, Y>::NodePtr& node) { void operator()(const typename DecisionTree<L, Y>::NodePtr& node) {
using Leaf = typename DecisionTree<L, Y>::Leaf; 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()); return f(assignment, leaf->constant());
using Choice = typename DecisionTree<L, Y>::Choice; 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) if (!choice)
throw std::invalid_argument("DecisionTree::VisitWith: Invalid NodePtr"); throw std::invalid_argument("DecisionTree::VisitWith: Invalid NodePtr");
for (size_t i = 0; i < choice->nrChoices(); i++) { 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 DiscreteFactorGraph::discreteKeys() const {
DiscreteKeys result; DiscreteKeys result;
for (auto&& factor : *this) { 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(); DiscreteKeys factor_keys = p->discreteKeys();
result.insert(result.end(), factor_keys.begin(), factor_keys.end()); result.insert(result.end(), factor_keys.begin(), factor_keys.end());
} }
@ -141,7 +141,7 @@ namespace gtsam {
gttoc(lookup); gttoc(lookup);
return std::make_pair( 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; DiscreteLookupDAG dag;
for (auto&& conditional : bayesNet) { for (auto&& conditional : bayesNet) {
if (auto lookupTable = if (auto lookupTable =
boost::dynamic_pointer_cast<DiscreteLookupTable>(conditional)) { std::dynamic_pointer_cast<DiscreteLookupTable>(conditional)) {
dag.push_back(lookupTable); dag.push_back(lookupTable);
} else { } else {
throw std::runtime_error( 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); 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"); DT tree({A, B, C}, "1 1 1 1 1 1 1 1");
EXPECT(tree.root_->isLeaf()); 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()); EXPECT_LONGS_EQUAL(8, leaf->nrAssignments());
DT tree2({C, B, A}, "1 1 1 2 3 4 5 5"); DT tree2({C, B, A}, "1 1 1 2 3 4 5 5");
@ -344,20 +344,20 @@ TEST(DecisionTree, NrAssignments) {
1 1 Leaf 5 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); 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); CHECK(choice0);
EXPECT(choice0->branches()[0]->isLeaf()); 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); CHECK(choice00);
EXPECT_LONGS_EQUAL(2, choice00->nrAssignments()); 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); 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); 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); CHECK(choice11);
EXPECT(choice11->isLeaf()); EXPECT(choice11->isLeaf());
EXPECT_LONGS_EQUAL(2, choice11->nrAssignments()); EXPECT_LONGS_EQUAL(2, choice11->nrAssignments());

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

@ -133,12 +133,12 @@ TEST_UNSAFE( DiscreteMarginals, truss ) {
// test 0 // test 0
DecisionTreeFactor expectedM0(key[0],"0.666667 0.333333"); DecisionTreeFactor expectedM0(key[0],"0.666667 0.333333");
DiscreteFactor::shared_ptr actualM0 = marginals(0); 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 // test 1
DecisionTreeFactor expectedM1(key[1],"0.333333 0.666667"); DecisionTreeFactor expectedM1(key[1],"0.333333 0.666667");
DiscreteFactor::shared_ptr actualM1 = marginals(1); 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); DecisionTreeFactor expectedM(key[j], table);
DiscreteFactor::shared_ptr actualM = marginals(j); DiscreteFactor::shared_ptr actualM = marginals(j);
EXPECT(assert_equal( 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 { const DiscreteValues &discreteValues) const {
auto &ptr = conditionals_(discreteValues); auto &ptr = conditionals_(discreteValues);
if (!ptr) return nullptr; if (!ptr) return nullptr;
auto conditional = boost::dynamic_pointer_cast<GaussianConditional>(ptr); auto conditional = std::dynamic_pointer_cast<GaussianConditional>(ptr);
if (conditional) if (conditional)
return conditional; return conditional;
else else

2
gtsam/hybrid/HybridBayesNet.cpp
View File

@ -155,7 +155,7 @@ void HybridBayesNet::updateDiscreteConditionals(
// Apply prunerFunc to the underlying AlgebraicDecisionTree // Apply prunerFunc to the underlying AlgebraicDecisionTree
auto discreteTree = auto discreteTree =
boost::dynamic_pointer_cast<DecisionTreeFactor::ADT>(discrete); std::dynamic_pointer_cast<DecisionTreeFactor::ADT>(discrete);
DecisionTreeFactor::ADT prunedDiscreteTree = DecisionTreeFactor::ADT prunedDiscreteTree =
discreteTree->apply(prunerFunc(prunedDecisionTree, *conditional)); 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 * @return GaussianMixture::shared_ptr otherwise
*/ */
GaussianMixture::shared_ptr asMixture() const { 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 * @return GaussianConditional::shared_ptr otherwise
*/ */
GaussianConditional::shared_ptr asGaussian() const { 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 * @return DiscreteConditional::shared_ptr
*/ */
DiscreteConditional::shared_ptr asDiscrete() const { 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 /// 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> HybridFactorGraph::discreteKeys() const {
std::set<DiscreteKey> keys; std::set<DiscreteKey> keys;
for (auto& factor : factors_) { 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()) { for (const DiscreteKey& key : p->discreteKeys()) {
keys.insert(key); 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()) { for (const DiscreteKey& key : p->discreteKeys()) {
keys.insert(key); keys.insert(key);
} }
@ -65,7 +65,7 @@ std::unordered_map<Key, DiscreteKey> HybridFactorGraph::discreteKeyMap() const {
const KeySet HybridFactorGraph::continuousKeySet() const { const KeySet HybridFactorGraph::continuousKeySet() const {
KeySet keys; KeySet keys;
for (auto& factor : factors_) { 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()) { for (const Key& key : p->continuousKeys()) {
keys.insert(key); keys.insert(key);
} }

2
gtsam/hybrid/HybridGaussianFactorGraph.cpp
View File

@ -57,7 +57,7 @@ template class EliminateableFactorGraph<HybridGaussianFactorGraph>;
using OrphanWrapper = BayesTreeOrphanWrapper<HybridBayesTree::Clique>; 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: // 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 // Add all the discrete keys in the hybrid factors to the current data
for (const auto& f : node->factors) { 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()) { for (auto& k : hf->discreteKeys()) {
data.discreteKeys.insert(k.first); 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()) { for (auto& k : hf->discreteKeys()) {
data.discreteKeys.insert(k.first); 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( HybridGaussianFactorGraph::shared_ptr HybridNonlinearFactorGraph::linearize(
const Values& continuousValues) const { const Values& continuousValues) const {
using boost::dynamic_pointer_cast; using std::dynamic_pointer_cast;
// create an empty linear FG // create an empty linear FG
auto linearFG = std::make_shared<HybridGaussianFactorGraph>(); 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::copy(f->keys().begin(), f->keys().end(),
std::inserter(factor_keys_set, factor_keys_set.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); nonlinear_factors.push_back(nf);
} else { } else {
throw std::runtime_error( throw std::runtime_error(
@ -266,13 +266,13 @@ class MixtureFactor : public HybridFactor {
// If this is a NoiseModelFactor, we'll use its noiseModel to // If this is a NoiseModelFactor, we'll use its noiseModel to
// otherwise noiseModelFactor will be nullptr // otherwise noiseModelFactor will be nullptr
if (auto noiseModelFactor = 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 // If dynamic cast to NoiseModelFactor succeeded, see if the noise model
// is Gaussian // is Gaussian
auto noiseModel = noiseModelFactor->noiseModel(); auto noiseModel = noiseModelFactor->noiseModel();
auto gaussianNoiseModel = auto gaussianNoiseModel =
boost::dynamic_pointer_cast<noiseModel::Gaussian>(noiseModel); std::dynamic_pointer_cast<noiseModel::Gaussian>(noiseModel);
if (gaussianNoiseModel) { if (gaussianNoiseModel) {
// If the noise model is Gaussian, retrieve the information matrix // If the noise model is Gaussian, retrieve the information matrix
infoMat = gaussianNoiseModel->information(); infoMat = gaussianNoiseModel->information();

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

@ -161,7 +161,7 @@ struct Switching {
auto motion_models = motionModels(k, between_sigma); auto motion_models = motionModels(k, between_sigma);
std::vector<NonlinearFactor::shared_ptr> components; std::vector<NonlinearFactor::shared_ptr> components;
for (auto &&f : motion_models) { 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>( nonlinearFactorGraph.emplace_shared<MixtureFactor>(
keys, DiscreteKeys{modes[k]}, components); 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 // Get the pruned discrete conditionals as an AlgebraicDecisionTree
auto pruned_discrete_conditionals = posterior->at(4)->asDiscrete(); auto pruned_discrete_conditionals = posterior->at(4)->asDiscrete();
auto discrete_conditional_tree = auto discrete_conditional_tree =
boost::dynamic_pointer_cast<DecisionTreeFactor::ADT>( std::dynamic_pointer_cast<DecisionTreeFactor::ADT>(
pruned_discrete_conditionals); pruned_discrete_conditionals);
// The checker functor verifies the values for us. // 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: // Create expected decision tree with two factor graphs:
// Get mixture factor: // 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); CHECK(mixture);
// Get prior factor: // 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); CHECK(gf);
using GF = GaussianFactor::shared_ptr; using GF = GaussianFactor::shared_ptr;
const GF prior = gf->asGaussian(); const GF prior = gf->asGaussian();

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

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

8
gtsam/linear/GaussianFactorGraph.cpp
View File

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

2
gtsam/linear/JacobianFactor.cpp
View File

@ -200,7 +200,7 @@ FastVector<JacobianFactor::shared_ptr> _convertOrCastToJacobians(
jacobians.reserve(factors.size()); jacobians.reserve(factors.size());
for(const GaussianFactor::shared_ptr& factor: factors) { for(const GaussianFactor::shared_ptr& factor: factors) {
if (factor) { if (factor) {
if (JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast< if (JacobianFactor::shared_ptr jf = std::dynamic_pointer_cast<
JacobianFactor>(factor)) JacobianFactor>(factor))
jacobians.push_back(jf); jacobians.push_back(jf);
else else

2
gtsam/linear/JacobianFactor.h
View File

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

2
gtsam/linear/Preconditioner.cpp
View File

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

8
gtsam/linear/SubgraphBuilder.cpp
View File

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

2
gtsam/linear/SubgraphPreconditioner.cpp
View File

@ -82,7 +82,7 @@ static GaussianFactorGraph convertToJacobianFactors(
GaussianFactorGraph result; GaussianFactorGraph result;
for (const auto &factor : gfg) for (const auto &factor : gfg)
if (factor) { if (factor) {
auto jf = boost::dynamic_pointer_cast<JacobianFactor>(factor); auto jf = std::dynamic_pointer_cast<JacobianFactor>(factor);
if (!jf) { if (!jf) {
jf = std::make_shared<JacobianFactor>(*factor); 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 // Apply an in-place change to init_graph and compare
JacobianFactor::shared_ptr jacFactor0 = JacobianFactor::shared_ptr jacFactor0 =
boost::dynamic_pointer_cast<JacobianFactor>(init_graph.at(0)); std::dynamic_pointer_cast<JacobianFactor>(init_graph.at(0));
CHECK(jacFactor0); CHECK(jacFactor0);
jacFactor0->getA(jacFactor0->begin()) *= 7.; jacFactor0->getA(jacFactor0->begin()) *= 7.;
EXPECT(assert_inequal(init_graph, exp_graph)); 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)); JacobianFactor combinedFactor(0, A0, 1, A1, b, noiseModel::Diagonal::Sigmas(sigmas, true));
GaussianFactorGraph::EliminationResult expected = combinedFactor.eliminate(Ordering{0}); 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); JacobianFactor>(expected.second);
GaussianFactorGraph::EliminationResult actual = EliminateQR(gfg, Ordering{0}); 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); JacobianFactor>(actual.second);
EXPECT(assert_equal(*expected.first, *actual.first)); 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 ) //TEST(NoiseModel, ConstrainedSmart )
//{ //{
// Gaussian::shared_ptr nonconstrained = Constrained::MixedSigmas((Vector3(sigma, 0.0, sigma), true); // Gaussian::shared_ptr nonconstrained = Constrained::MixedSigmas((Vector3(sigma, 0.0, sigma), true);
// Diagonal::shared_ptr n1 = boost::dynamic_pointer_cast<Diagonal>(nonconstrained); // Diagonal::shared_ptr n1 = std::dynamic_pointer_cast<Diagonal>(nonconstrained);
// Constrained::shared_ptr n2 = boost::dynamic_pointer_cast<Constrained>(nonconstrained); // Constrained::shared_ptr n2 = std::dynamic_pointer_cast<Constrained>(nonconstrained);
// EXPECT(n1); // EXPECT(n1);
// EXPECT(!n2); // EXPECT(!n2);
// //
// Gaussian::shared_ptr constrained = Constrained::MixedSigmas(zero(3), true); // Gaussian::shared_ptr constrained = Constrained::MixedSigmas(zero(3), true);
// Diagonal::shared_ptr c1 = boost::dynamic_pointer_cast<Diagonal>(constrained); // Diagonal::shared_ptr c1 = std::dynamic_pointer_cast<Diagonal>(constrained);
// Constrained::shared_ptr c2 = boost::dynamic_pointer_cast<Constrained>(constrained); // Constrained::shared_ptr c2 = std::dynamic_pointer_cast<Constrained>(constrained);
// EXPECT(c1); // EXPECT(c1);
// EXPECT(c2); // EXPECT(c2);
//} //}

2
gtsam/navigation/AHRSFactor.cpp
View File

@ -98,7 +98,7 @@ AHRSFactor::AHRSFactor(
gtsam::NonlinearFactor::shared_ptr AHRSFactor::clone() const { 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))); 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), biasHat_(bias_hat),
preintMeasCov_(preint_meas_cov) {} 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 Vector3& biasHat() const { return biasHat_; }
const Matrix3& preintMeasCov() const { return preintMeasCov_; } 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 /// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override { 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))); gtsam::NonlinearFactor::shared_ptr(new This(*this)));
} }
@ -189,7 +189,7 @@ public:
/// @return a deep copy of this factor /// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override { 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))); 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 /// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override { 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))); 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 { 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))); gtsam::NonlinearFactor::shared_ptr(new This(*this)));
} }

2
gtsam/navigation/CombinedImuFactor.h
View File

@ -185,7 +185,7 @@ public:
void resetIntegration() override; void resetIntegration() override;
/// const reference to params, shadows definition in base class /// 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 /// @name Access instance variables

4
gtsam/navigation/GPSFactor.h
View File

@ -69,7 +69,7 @@ public:
/// @return a deep copy of this factor /// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override { 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))); gtsam::NonlinearFactor::shared_ptr(new This(*this)));
} }
@ -144,7 +144,7 @@ public:
/// @return a deep copy of this factor /// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override { 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))); 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 { NonlinearFactor::shared_ptr ImuFactor::clone() const {
return boost::static_pointer_cast<NonlinearFactor>( return std::static_pointer_cast<NonlinearFactor>(
NonlinearFactor::shared_ptr(new This(*this))); 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 { NonlinearFactor::shared_ptr ImuFactor2::clone() const {
return boost::static_pointer_cast<NonlinearFactor>( return std::static_pointer_cast<NonlinearFactor>(
NonlinearFactor::shared_ptr(new This(*this))); 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 /// @return a deep copy of this factor
NonlinearFactor::shared_ptr clone() const override { NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<NonlinearFactor>( return std::static_pointer_cast<NonlinearFactor>(
NonlinearFactor::shared_ptr(new MagFactor(*this))); NonlinearFactor::shared_ptr(new MagFactor(*this)));
} }
@ -111,7 +111,7 @@ public:
/// @return a deep copy of this factor /// @return a deep copy of this factor
NonlinearFactor::shared_ptr clone() const override { NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<NonlinearFactor>( return std::static_pointer_cast<NonlinearFactor>(
NonlinearFactor::shared_ptr(new MagFactor1(*this))); NonlinearFactor::shared_ptr(new MagFactor1(*this)));
} }
@ -150,7 +150,7 @@ public:
/// @return a deep copy of this factor /// @return a deep copy of this factor
NonlinearFactor::shared_ptr clone() const override { NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<NonlinearFactor>( return std::static_pointer_cast<NonlinearFactor>(
NonlinearFactor::shared_ptr(new MagFactor2(*this))); NonlinearFactor::shared_ptr(new MagFactor2(*this)));
} }
@ -194,7 +194,7 @@ public:
/// @return a deep copy of this factor /// @return a deep copy of this factor
NonlinearFactor::shared_ptr clone() const override { NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<NonlinearFactor>( return std::static_pointer_cast<NonlinearFactor>(
NonlinearFactor::shared_ptr(new MagFactor3(*this))); 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. /// @return a deep copy of this factor.
NonlinearFactor::shared_ptr clone() const 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))); 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) { static noiseModel::Diagonal::shared_ptr Diagonal(const Matrix& covariance) {
bool smart = true; bool smart = true;
auto model = noiseModel::Gaussian::Covariance(covariance, smart); 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) if (!diagonal)
throw std::invalid_argument("ScenarioRunner::Diagonal: not a diagonal"); throw std::invalid_argument("ScenarioRunner::Diagonal: not a diagonal");
return 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 // In case noise model is constrained, we need to provide a noise model
SharedDiagonal noiseModel; SharedDiagonal noiseModel;
if (noiseModel_ && noiseModel_->isConstrained()) { if (noiseModel_ && noiseModel_->isConstrained()) {
noiseModel = boost::static_pointer_cast<noiseModel::Constrained>( noiseModel = std::static_pointer_cast<noiseModel::Constrained>(
noiseModel_)->unit(); noiseModel_)->unit();
} }
@ -152,7 +152,7 @@ protected:
/// @return a deep copy of this factor /// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override { 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))); 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 /// @return a deep copy of this factor
NonlinearFactor::shared_ptr clone() const override { 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))); 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 /// @return a deep copy of this factor
NonlinearFactor::shared_ptr clone() const override { 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))); 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()); nfg_.resize(graph.size());
for (size_t i = 0; i < graph.size(); i++) { for (size_t i = 0; i < graph.size(); i++) {
if (graph[i]) { if (graph[i]) {
NoiseModelFactor::shared_ptr factor = boost::dynamic_pointer_cast< NoiseModelFactor::shared_ptr factor = std::dynamic_pointer_cast<
NoiseModelFactor>(graph[i]); NoiseModelFactor>(graph[i]);
auto robust = boost::dynamic_pointer_cast< auto robust = std::dynamic_pointer_cast<
noiseModel::Robust>(factor->noiseModel()); noiseModel::Robust>(factor->noiseModel());
// if the factor has a robust loss, we remove the robust loss // if the factor has a robust loss, we remove the robust loss
nfg_[i] = robust ? factor-> cloneWithNewNoiseModel(robust->noise()) : factor; nfg_[i] = robust ? factor-> cloneWithNewNoiseModel(robust->noise()) : factor;
@ -401,10 +401,10 @@ class GncOptimizer {
newGraph.resize(nfg_.size()); newGraph.resize(nfg_.size());
for (size_t i = 0; i < nfg_.size(); i++) { for (size_t i = 0; i < nfg_.size(); i++) {
if (nfg_[i]) { if (nfg_[i]) {
auto factor = boost::dynamic_pointer_cast< auto factor = std::dynamic_pointer_cast<
NoiseModelFactor>(nfg_[i]); NoiseModelFactor>(nfg_[i]);
auto noiseModel = auto noiseModel =
boost::dynamic_pointer_cast<noiseModel::Gaussian>( std::dynamic_pointer_cast<noiseModel::Gaussian>(
factor->noiseModel()); factor->noiseModel());
if (noiseModel) { if (noiseModel) {
Matrix newInfo = weights[i] * noiseModel->information(); 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 // Traverse up the tree to find the root of the marginalized subtree
sharedClique clique = nodes_[j]; 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 if parent contains a marginalized leaf variable. Only need to
// check the first variable because it is the closest to the leaves. // check the first variable because it is the closest to the leaves.
sharedClique parent = clique->parent(); 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 // Apply changes due to relinearization
if (isJacobian()) { 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); jacFactor->getb() = -jacFactor->unweighted_error(delta);
} else { } 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(); const auto view = hesFactor->informationView();
Vector deltaVector = delta.vector(keys()); Vector deltaVector = delta.vector(keys());
@ -144,12 +144,12 @@ bool LinearContainerFactor::isHessian() const {
/* ************************************************************************* */ /* ************************************************************************* */
JacobianFactor::shared_ptr LinearContainerFactor::toJacobian() 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 { 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); auto rekeyed_base_factor = Base::rekey(rekey_mapping);
// Update the keys to the properties as well // Update the keys to the properties as well
// Downncast so we have access to members // 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 // Create a new Values to assign later
Values newLinearizationPoint; Values newLinearizationPoint;
for (size_t i = 0; i < factor_->size(); ++i) { for (size_t i = 0; i < factor_->size(); ++i) {
@ -183,7 +183,7 @@ NonlinearFactor::shared_ptr LinearContainerFactor::rekey(
new_factor->linearizationPoint_ = newLinearizationPoint; new_factor->linearizationPoint_ = newLinearizationPoint;
// upcast back and return // 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); auto rekeyed_base_factor = Base::rekey(new_keys);
// Update the keys to the properties as well // Update the keys to the properties as well
// Downncast so we have access to members // 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(); new_factor->factor_->keys() = new_factor->keys();
// Create a new Values to assign later // Create a new Values to assign later
Values newLinearizationPoint; Values newLinearizationPoint;
@ -203,7 +203,7 @@ NonlinearFactor::shared_ptr LinearContainerFactor::rekey(
new_factor->linearizationPoint_ = newLinearizationPoint; new_factor->linearizationPoint_ = newLinearizationPoint;
// upcast back and return // 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 /// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override { 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))); gtsam::NonlinearFactor::shared_ptr(new This(*this)));
} }
@ -248,7 +248,7 @@ public:
/// @return a deep copy of this factor /// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override { 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))); gtsam::NonlinearFactor::shared_ptr(new This(*this)));
} }
@ -326,7 +326,7 @@ class NonlinearEquality2 : public NoiseModelFactorN<T, T> {
/// @return a deep copy of this factor /// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override { 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))); 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( NoiseModelFactor::shared_ptr NoiseModelFactor::cloneWithNewNoiseModel(
const SharedNoiseModel newNoise) const { 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; new_factor->noiseModel_ = newNoise;
return new_factor; return new_factor;
} }
@ -177,7 +177,7 @@ std::shared_ptr<GaussianFactor> NoiseModelFactor::linearize(
if (noiseModel_ && noiseModel_->isConstrained()) if (noiseModel_ && noiseModel_->isConstrained())
return GaussianFactor::shared_ptr( return GaussianFactor::shared_ptr(
new JacobianFactor(terms, b, new JacobianFactor(terms, b,
boost::static_pointer_cast<Constrained>(noiseModel_)->unit())); std::static_pointer_cast<Constrained>(noiseModel_)->unit()));
else { else {
return GaussianFactor::shared_ptr(new JacobianFactor(terms, b)); 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( throw std::runtime_error(
"NonlinearOptimizer::solve: cg parameter has to be assigned ..."); "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)) { params.iterativeParams)) {
delta = PCGSolver(*pcg).optimize(gfg); delta = PCGSolver(*pcg).optimize(gfg);
} else if (auto spcg = } else if (auto spcg =
boost::dynamic_pointer_cast<SubgraphSolverParameters>( std::dynamic_pointer_cast<SubgraphSolverParameters>(
params.iterativeParams)) { params.iterativeParams)) {
if (!params.ordering) if (!params.ordering)
throw std::runtime_error("SubgraphSolver needs an 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 /// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override { 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))); } gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/** implement functions needed for Testable */ /** 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? // TODO: Frank commented this out for now, can it go?
// /// @return a deep copy of this factor // /// @return a deep copy of this factor
// gtsam::NonlinearFactor::shared_ptr clone() const override { // 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))); } // 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 // Create actual value by linearize
auto actual = auto actual =
boost::dynamic_pointer_cast<JacobianFactor>(factor.linearize(values)); std::dynamic_pointer_cast<JacobianFactor>(factor.linearize(values));
if (!actual) return false; if (!actual) return false;
// Check cast result and then equality // 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 // Cast back to LCF ptr
LinearContainerFactor::shared_ptr lcf_factor_rekey_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); CHECK(lcf_factor_rekey_ptr);
// For extra fun lets try linearizing this LCF // For extra fun lets try linearizing this LCF
@ -383,7 +383,7 @@ TEST(TestLinearContainerFactor, Rekey2) {
// Cast back to LCF ptr // Cast back to LCF ptr
LinearContainerFactor::shared_ptr lcf_factor_rekey_ptr = LinearContainerFactor::shared_ptr lcf_factor_rekey_ptr =
boost::static_pointer_cast<LinearContainerFactor>( std::static_pointer_cast<LinearContainerFactor>(
lcf_factor.rekey(key_map)); lcf_factor.rekey(key_map));
CHECK(lcf_factor_rekey_ptr); CHECK(lcf_factor_rekey_ptr);
} }

4
gtsam/nonlinear/utilities.h
View File

@ -296,14 +296,14 @@ Matrix reprojectionErrors(const NonlinearFactorGraph& graph,
// first count // first count
size_t K = 0, k = 0; size_t K = 0, k = 0;
for(const NonlinearFactor::shared_ptr& f: graph) 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)) f))
++K; ++K;
// now fill // now fill
Matrix errors(2, K); Matrix errors(2, K);
for(const NonlinearFactor::shared_ptr& f: graph) { for(const NonlinearFactor::shared_ptr& f: graph) {
std::shared_ptr<const GenericProjectionFactor<Pose3, Point3> > p = 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); f);
if (p) if (p)
errors.col(k++) = p->unwhitenedError(values); 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 /// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override { 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))); 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 /// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override { 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))); 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 /// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override { 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))); 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> template <typename T, size_t d>
static double Kappa(const BinaryMeasurement<T> &measurement, static double Kappa(const BinaryMeasurement<T> &measurement,
const ShonanAveragingParameters<d> &parameters) { const ShonanAveragingParameters<d> &parameters) {
const auto &isotropic = boost::dynamic_pointer_cast<noiseModel::Isotropic>( const auto &isotropic = std::dynamic_pointer_cast<noiseModel::Isotropic>(
measurement.noiseModel()); measurement.noiseModel());
double sigma; double sigma;
if (isotropic) { if (isotropic) {
sigma = isotropic->sigma(); sigma = isotropic->sigma();
} else { } else {
const auto &robust = boost::dynamic_pointer_cast<noiseModel::Robust>( const auto &robust = std::dynamic_pointer_cast<noiseModel::Robust>(
measurement.noiseModel()); measurement.noiseModel());
// Check if noise model is robust // Check if noise model is robust
if (robust) { if (robust) {
@ -949,7 +949,7 @@ ShonanAveraging2::ShonanAveraging2(string g2oFile, const Parameters &parameters)
static BinaryMeasurement<Rot2> convertPose2ToBinaryMeasurementRot2( static BinaryMeasurement<Rot2> convertPose2ToBinaryMeasurementRot2(
const BetweenFactor<Pose2>::shared_ptr &f) { const BetweenFactor<Pose2>::shared_ptr &f) {
auto gaussian = auto gaussian =
boost::dynamic_pointer_cast<noiseModel::Gaussian>(f->noiseModel()); std::dynamic_pointer_cast<noiseModel::Gaussian>(f->noiseModel());
if (!gaussian) if (!gaussian)
throw std::invalid_argument( throw std::invalid_argument(
"parseMeasurements<Rot2> can only convert Pose2 measurements " "parseMeasurements<Rot2> can only convert Pose2 measurements "
@ -997,7 +997,7 @@ ShonanAveraging3::ShonanAveraging3(string g2oFile, const Parameters &parameters)
static BinaryMeasurement<Rot3> convert( static BinaryMeasurement<Rot3> convert(
const BetweenFactor<Pose3>::shared_ptr &f) { const BetweenFactor<Pose3>::shared_ptr &f) {
auto gaussian = auto gaussian =
boost::dynamic_pointer_cast<noiseModel::Gaussian>(f->noiseModel()); std::dynamic_pointer_cast<noiseModel::Gaussian>(f->noiseModel());
if (!gaussian) if (!gaussian)
throw std::invalid_argument( throw std::invalid_argument(
"parseMeasurements<Rot3> can only convert Pose3 measurements " "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) { for (auto &measurement : measurements) {
auto model = measurement.noiseModel(); auto model = measurement.noiseModel();
const auto &robust = const auto &robust =
boost::dynamic_pointer_cast<noiseModel::Robust>(model); std::dynamic_pointer_cast<noiseModel::Robust>(model);
SharedNoiseModel robust_model; SharedNoiseModel robust_model;
// Check if the noise model is already robust // 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.key1(), kKey1);
EXPECT_LONGS_EQUAL(rot3Measurement.key2(), kKey2); EXPECT_LONGS_EQUAL(rot3Measurement.key2(), kKey2);
EXPECT(rot3Measurement.measured().equals(rot3Measured)); 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()); rot3Measurement.noiseModel());
EXPECT(robust); EXPECT(robust);
} }

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

@ -372,8 +372,8 @@ TEST(ShonanAveraging2, noisyToyGraphWithHuber) {
// test that each factor is actually robust // 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 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>( const auto &robust = std::dynamic_pointer_cast<noiseModel::Robust>(
boost::dynamic_pointer_cast<NoiseModelFactor>(graph[i])->noiseModel()); std::dynamic_pointer_cast<NoiseModelFactor>(graph[i])->noiseModel());
EXPECT(robust); // we expect the factors to be use a robust noise model (in particular, Huber) 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}) { for (auto i : {0,1,2}) {
const auto& m = shonan.measurement(i); const auto& m = shonan.measurement(i);
auto isotropic = auto isotropic =
boost::static_pointer_cast<noiseModel::Isotropic>(m.noiseModel()); std::static_pointer_cast<noiseModel::Isotropic>(m.noiseModel());
CHECK(isotropic != nullptr); CHECK(isotropic != nullptr);
EXPECT_LONGS_EQUAL(3, isotropic->dim()); EXPECT_LONGS_EQUAL(3, isotropic->dim());
EXPECT_DOUBLES_EQUAL(0.2, isotropic->sigma(), 1e-9); 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)); JacobianFactor linearized(key, A, Vector::Zero(3));
Values values; Values values;
EXPECT_LONGS_EQUAL(3, factor.dim()); 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)))); factor.linearize(values))));
} }
@ -73,7 +73,7 @@ TEST(ShonanAveraging, GaugeFactorSO7) {
JacobianFactor linearized(key, A, Vector::Zero(6)); JacobianFactor linearized(key, A, Vector::Zero(6));
Values values; Values values;
EXPECT_LONGS_EQUAL(6, factor.dim()); 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)))); factor.linearize(values))));
} }
@ -94,7 +94,7 @@ TEST(ShonanAveraging, GaugeFactorSO6over2) {
JacobianFactor linearized(key, A, Vector::Zero(6)); JacobianFactor linearized(key, A, Vector::Zero(6));
Values values; Values values;
EXPECT_LONGS_EQUAL(6, factor.dim()); 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)))); factor.linearize(values))));
} }

2
gtsam/slam/AntiFactor.h
View File

@ -53,7 +53,7 @@ namespace gtsam {
/// @return a deep copy of this factor /// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override { 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))); } gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/** implement functions needed for Testable */ /** 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 /// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override { 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))); } gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/// @name Testable /// @name Testable

2
gtsam/slam/EssentialMatrixConstraint.h
View File

@ -65,7 +65,7 @@ public:
/// @return a deep copy of this factor /// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override { 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))); 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 /// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override { 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))); gtsam::NonlinearFactor::shared_ptr(new This(*this)));
} }
@ -159,7 +159,7 @@ class EssentialMatrixFactor2
/// @return a deep copy of this factor /// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override { 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))); gtsam::NonlinearFactor::shared_ptr(new This(*this)));
} }
@ -275,7 +275,7 @@ class EssentialMatrixFactor3 : public EssentialMatrixFactor2 {
/// @return a deep copy of this factor /// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override { 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))); gtsam::NonlinearFactor::shared_ptr(new This(*this)));
} }
@ -362,7 +362,7 @@ class EssentialMatrixFactor4
/// @return a deep copy of this factor /// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override { 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))); 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; double sigma = 1.0;
if (model != nullptr) { 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; Vector sigmas;
if (robust) { if (robust) {
sigmas = robust->noise()->sigmas(); sigmas = robust->noise()->sigmas();
@ -56,7 +56,7 @@ ConvertNoiseModel(const SharedNoiseModel &model, size_t d, bool defaultToUnit) {
} }
exit: exit:
auto isoModel = noiseModel::Isotropic::Sigma(d, sigma); 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) { if (robust) {
return noiseModel::Robust::Create( return noiseModel::Robust::Create(
noiseModel::mEstimator::Huber::Create(1.345), isoModel); 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 /// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override { 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)));} gtsam::NonlinearFactor::shared_ptr(new This(*this)));}
/** /**
@ -170,7 +170,7 @@ public:
// Create new (unit) noiseModel, preserving constraints if applicable // Create new (unit) noiseModel, preserving constraints if applicable
SharedDiagonal model; SharedDiagonal model;
if (noiseModel && noiseModel->isConstrained()) { 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); 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 /// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override { 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)));} 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) { for (const auto& factor : graph) {
// recast to a between on Pose // recast to a between on Pose
if (auto between = if (auto between =
boost::dynamic_pointer_cast<BetweenFactor<Pose> >(factor)) std::dynamic_pointer_cast<BetweenFactor<Pose> >(factor))
poseGraph.add(between); poseGraph.add(between);
// recast PriorFactor<Pose> to BetweenFactor<Pose> // 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> >( poseGraph.emplace_shared<BetweenFactor<Pose> >(
kAnchorKey, prior->keys()[0], prior->prior(), prior->noiseModel()); 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; Matrix3 Rij;
double rotationPrecision = 1.0; double rotationPrecision = 1.0;
auto pose3Between = boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor); auto pose3Between = std::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
if (pose3Between){ if (pose3Between){
Rij = pose3Between->measured().rotation().matrix(); Rij = pose3Between->measured().rotation().matrix();
Vector precisions = Vector::Zero(6); Vector precisions = Vector::Zero(6);
@ -223,7 +223,7 @@ void InitializePose3::createSymbolicGraph(
size_t factorId = 0; size_t factorId = 0;
for (const auto& factor : pose3Graph) { for (const auto& factor : pose3Graph) {
auto pose3Between = auto pose3Between =
boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor); std::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
if (pose3Between) { if (pose3Between) {
Rot3 Rij = pose3Between->measured().rotation(); Rot3 Rij = pose3Between->measured().rotation();
factorId2RotMap->emplace(factorId, Rij); factorId2RotMap->emplace(factorId, Rij);

2
gtsam/slam/PoseRotationPrior.h
View File

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

2
gtsam/slam/PoseTranslationPrior.h
View File

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

2
gtsam/slam/ProjectionFactor.h
View File

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

2
gtsam/slam/ReferenceFrameFactor.h
View File

@ -93,7 +93,7 @@ public:
~ReferenceFrameFactor() override{} ~ReferenceFrameFactor() override{}
NonlinearFactor::shared_ptr clone() const 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))); } NonlinearFactor::shared_ptr(new This(*this))); }
/** Combined cost and derivative function using boost::optional */ /** 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 /// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override { 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))); } gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/// print /// print
@ -94,7 +94,7 @@ public:
/// @return a deep copy of this factor /// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override { 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))); } gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/// print /// print

2
gtsam/slam/SmartFactorBase.h
View File

@ -105,7 +105,7 @@ protected:
if (!sharedNoiseModel) if (!sharedNoiseModel)
throw std::runtime_error("SmartFactorBase: sharedNoiseModel is required"); throw std::runtime_error("SmartFactorBase: sharedNoiseModel is required");
SharedIsotropic sharedIsotropic = boost::dynamic_pointer_cast< SharedIsotropic sharedIsotropic = std::dynamic_pointer_cast<
noiseModel::Isotropic>(sharedNoiseModel); noiseModel::Isotropic>(sharedNoiseModel);
if (!sharedIsotropic) if (!sharedIsotropic)

2
gtsam/slam/StereoFactor.h
View File

@ -96,7 +96,7 @@ public:
/// @return a deep copy of this factor /// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override { 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))); } 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 /// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override { 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))); 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 // Create a sampler to corrupt a measurement
std::shared_ptr<Sampler> createSampler(const SharedNoiseModel &model) { 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) if (!noise)
throw invalid_argument("gtsam::load: invalid noise model for adding noise" throw invalid_argument("gtsam::load: invalid noise model for adding noise"
"(current version assumes diagonal noise model)!"); "(current version assumes diagonal noise model)!");
@ -400,7 +400,7 @@ parseMeasurements(const std::string &filename,
// Extract Rot2 measurement from Pose2 measurement // Extract Rot2 measurement from Pose2 measurement
static BinaryMeasurement<Rot2> convert(const BinaryMeasurement<Pose2> &p) { static BinaryMeasurement<Rot2> convert(const BinaryMeasurement<Pose2> &p) {
auto gaussian = auto gaussian =
boost::dynamic_pointer_cast<noiseModel::Gaussian>(p.noiseModel()); std::dynamic_pointer_cast<noiseModel::Gaussian>(p.noiseModel());
if (!gaussian) if (!gaussian)
throw std::invalid_argument( throw std::invalid_argument(
"parseMeasurements<Rot2> can only convert Pose2 measurements " "parseMeasurements<Rot2> can only convert Pose2 measurements "
@ -597,7 +597,7 @@ void save2D(const NonlinearFactorGraph &graph, const Values &config,
Matrix3 R = model->R(); Matrix3 R = model->R();
Matrix3 RR = R.transpose() * R; Matrix3 RR = R.transpose() * R;
for (auto f : graph) { for (auto f : graph) {
auto factor = boost::dynamic_pointer_cast<BetweenFactor<Pose2>>(f); auto factor = std::dynamic_pointer_cast<BetweenFactor<Pose2>>(f);
if (!factor) if (!factor)
continue; continue;
@ -679,11 +679,11 @@ void writeG2o(const NonlinearFactorGraph &graph, const Values &estimate,
// save edges (2D or 3D) // save edges (2D or 3D)
for (const auto &factor_ : graph) { 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) { if (factor) {
SharedNoiseModel model = factor->noiseModel(); SharedNoiseModel model = factor->noiseModel();
auto gaussianModel = auto gaussianModel =
boost::dynamic_pointer_cast<noiseModel::Gaussian>(model); std::dynamic_pointer_cast<noiseModel::Gaussian>(model);
if (!gaussianModel) { if (!gaussianModel) {
model->print("model\n"); model->print("model\n");
throw invalid_argument("writeG2o: invalid noise model!"); throw invalid_argument("writeG2o: invalid noise model!");
@ -701,13 +701,13 @@ void writeG2o(const NonlinearFactorGraph &graph, const Values &estimate,
stream << endl; stream << endl;
} }
auto factor3D = boost::dynamic_pointer_cast<BetweenFactor<Pose3>>(factor_); auto factor3D = std::dynamic_pointer_cast<BetweenFactor<Pose3>>(factor_);
if (factor3D) { if (factor3D) {
SharedNoiseModel model = factor3D->noiseModel(); SharedNoiseModel model = factor3D->noiseModel();
std::shared_ptr<noiseModel::Gaussian> gaussianModel = std::shared_ptr<noiseModel::Gaussian> gaussianModel =
boost::dynamic_pointer_cast<noiseModel::Gaussian>(model); std::dynamic_pointer_cast<noiseModel::Gaussian>(model);
if (!gaussianModel) { if (!gaussianModel) {
model->print("model\n"); model->print("model\n");
throw invalid_argument("writeG2o: invalid noise model!"); throw invalid_argument("writeG2o: invalid noise model!");
@ -887,7 +887,7 @@ parseMeasurements(const std::string &filename,
// Extract Rot3 measurement from Pose3 measurement // Extract Rot3 measurement from Pose3 measurement
static BinaryMeasurement<Rot3> convert(const BinaryMeasurement<Pose3> &p) { static BinaryMeasurement<Rot3> convert(const BinaryMeasurement<Pose3> &p) {
auto gaussian = auto gaussian =
boost::dynamic_pointer_cast<noiseModel::Gaussian>(p.noiseModel()); std::dynamic_pointer_cast<noiseModel::Gaussian>(p.noiseModel());
if (!gaussian) if (!gaussian)
throw std::invalid_argument( throw std::invalid_argument(
"parseMeasurements<Rot3> can only convert Pose3 measurements " "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]; Key key2 = factor->keys()[1];
// recast to a between // recast to a between
std::shared_ptr<BetweenFactor<Pose2> > pose2Between = std::shared_ptr<BetweenFactor<Pose2> > pose2Between =
boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor); std::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor);
if (!pose2Between) if (!pose2Between)
continue; continue;
// get the orientation - measured().theta(); // 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 // Get the relative rotation measurement from the between factor
std::shared_ptr<BetweenFactor<Pose2> > pose2Between = std::shared_ptr<BetweenFactor<Pose2> > pose2Between =
boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor); std::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor);
if (!pose2Between) if (!pose2Between)
throw invalid_argument( throw invalid_argument(
"buildLinearOrientationGraph: invalid between factor!"); "buildLinearOrientationGraph: invalid between factor!");
@ -149,7 +149,7 @@ static void getDeltaThetaAndNoise(NonlinearFactor::shared_ptr factor,
// Retrieve the noise model for the relative rotation // Retrieve the noise model for the relative rotation
SharedNoiseModel model = pose2Between->noiseModel(); SharedNoiseModel model = pose2Between->noiseModel();
std::shared_ptr<noiseModel::Diagonal> diagonalModel = std::shared_ptr<noiseModel::Diagonal> diagonalModel =
boost::dynamic_pointer_cast<noiseModel::Diagonal>(model); std::dynamic_pointer_cast<noiseModel::Diagonal>(model);
if (!diagonalModel) if (!diagonalModel)
throw invalid_argument("buildLinearOrientationGraph: invalid noise model " throw invalid_argument("buildLinearOrientationGraph: invalid noise model "
"(current version assumes diagonal 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) { for(const std::shared_ptr<NonlinearFactor>& factor: pose2graph) {
std::shared_ptr<BetweenFactor<Pose2> > pose2Between = std::shared_ptr<BetweenFactor<Pose2> > pose2Between =
boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor); std::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor);
if (pose2Between) { if (pose2Between) {
Key key1 = pose2Between->keys()[0]; Key key1 = pose2Between->keys()[0];
@ -304,7 +304,7 @@ Values computePoses(const NonlinearFactorGraph& pose2graph,
J1(1, 2) = -c1 * dx + s1 * dy; J1(1, 2) = -c1 * dx + s1 * dy;
// Retrieve the noise model for the relative rotation // Retrieve the noise model for the relative rotation
std::shared_ptr<noiseModel::Diagonal> diagonalModel = std::shared_ptr<noiseModel::Diagonal> diagonalModel =
boost::dynamic_pointer_cast<noiseModel::Diagonal>( std::dynamic_pointer_cast<noiseModel::Diagonal>(
pose2Between->noiseModel()); pose2Between->noiseModel());
linearPose2graph.add(key1, J1, key2, I3, b, diagonalModel); 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 // Linearize the AntiFactor into a Hessian Factor
GaussianFactor::shared_ptr antiGaussian = antiFactor->linearize(values); 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 expected_information = -originalHessian->information();
Matrix actual_information = antiHessian->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), BetweenFactor<Pose2> expected(1, 0, Pose2(-0.99879, 0.0417574, -0.00818381),
model); model);
BetweenFactor<Pose2>::shared_ptr actual = 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)); EXPECT(assert_equal(expected, *actual));
// Check binary measurements, Pose2 // Check binary measurements, Pose2
@ -117,7 +117,7 @@ TEST(dataSet, load2D) {
const auto actualFactors = parseFactors<Pose2>(filename); const auto actualFactors = parseFactors<Pose2>(filename);
for (size_t i : {0, 1, 2, 3, 4, 5}) { for (size_t i : {0, 1, 2, 3, 4, 5}) {
EXPECT(assert_equal( 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)); *actualFactors[i], 1e-5));
} }
@ -201,7 +201,7 @@ TEST(dataSet, readG2o3D) {
const auto actualFactors = parseFactors<Pose3>(g2oFile); const auto actualFactors = parseFactors<Pose3>(g2oFile);
for (size_t i : {0, 1, 2, 3, 4, 5}) { for (size_t i : {0, 1, 2, 3, 4, 5}) {
EXPECT(assert_equal( EXPECT(assert_equal(
*boost::dynamic_pointer_cast<BetweenFactor<Pose3>>(expectedGraph[i]), *std::dynamic_pointer_cast<BetweenFactor<Pose3>>(expectedGraph[i]),
*actualFactors[i], 1e-5)); *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. // Otherwise, loop over all factors/constraints for variable with given key.
for (size_t f : factors) { for (size_t f : factors) {
// If this factor is a constraint, call its ensureArcConsistency method: // 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) { if (constraint) {
changed = constraint->ensureArcConsistency(key, domains) || changed; changed = constraint->ensureArcConsistency(key, domains) || changed;
} }
@ -75,7 +75,7 @@ CSP CSP::partiallyApply(const Domains& domains) const {
// Reduce all existing factors: // Reduce all existing factors:
for (const DiscreteFactor::shared_ptr& f : 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) if (!constraint)
throw runtime_error("CSP:runArcConsistency: non-constraint factor"); throw runtime_error("CSP:runArcConsistency: non-constraint factor");
Constraint::shared_ptr reduced = constraint->partiallyApply(domains); 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 // Incorporate new the factor to belief
if (!beliefAtKey) if (!beliefAtKey)
beliefAtKey = beliefAtKey =
boost::dynamic_pointer_cast<DecisionTreeFactor>(message); std::dynamic_pointer_cast<DecisionTreeFactor>(message);
else else
beliefAtKey = std::make_shared<DecisionTreeFactor>( beliefAtKey = std::make_shared<DecisionTreeFactor>(
(*beliefAtKey) * (*beliefAtKey) *
(*boost::dynamic_pointer_cast<DecisionTreeFactor>(message))); (*std::dynamic_pointer_cast<DecisionTreeFactor>(message)));
} }
if (starGraphs_.at(key).unary) if (starGraphs_.at(key).unary)
beliefAtKey = std::make_shared<DecisionTreeFactor>( beliefAtKey = std::make_shared<DecisionTreeFactor>(
@ -148,9 +148,9 @@ class LoopyBelief {
for (Key neighbor : starGraphs_.at(key).correctedBeliefIndices | for (Key neighbor : starGraphs_.at(key).correctedBeliefIndices |
boost::adaptors::map_keys) { boost::adaptors::map_keys) {
DecisionTreeFactor correctedBelief = DecisionTreeFactor correctedBelief =
(*boost::dynamic_pointer_cast<DecisionTreeFactor>( (*std::dynamic_pointer_cast<DecisionTreeFactor>(
beliefs->at(beliefFactors[key].front()))) / beliefs->at(beliefFactors[key].front()))) /
(*boost::dynamic_pointer_cast<DecisionTreeFactor>( (*std::dynamic_pointer_cast<DecisionTreeFactor>(
messages.at(neighbor))); messages.at(neighbor)));
if (debug) correctedBelief.print("correctedBelief: "); if (debug) correctedBelief.print("correctedBelief: ");
size_t beliefIndex = size_t beliefIndex =
@ -187,12 +187,12 @@ class LoopyBelief {
// accumulate unary factors // accumulate unary factors
if (graph.at(factorIndex)->size() == 1) { if (graph.at(factorIndex)->size() == 1) {
if (!prodOfUnaries) if (!prodOfUnaries)
prodOfUnaries = boost::dynamic_pointer_cast<DecisionTreeFactor>( prodOfUnaries = std::dynamic_pointer_cast<DecisionTreeFactor>(
graph.at(factorIndex)); graph.at(factorIndex));
else else
prodOfUnaries = std::make_shared<DecisionTreeFactor>( prodOfUnaries = std::make_shared<DecisionTreeFactor>(
*prodOfUnaries * *prodOfUnaries *
(*boost::dynamic_pointer_cast<DecisionTreeFactor>( (*std::dynamic_pointer_cast<DecisionTreeFactor>(
graph.at(factorIndex)))); graph.at(factorIndex))));
} }
} }

2
gtsam_unstable/dynamics/FullIMUFactor.h
View File

@ -57,7 +57,7 @@ public:
/// @return a deep copy of this factor /// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override { 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))); } gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/** Check if two factors are equal */ /** 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 /// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override { 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))); } gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/** Check if two factors are equal */ /** 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 /// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override { 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))); } gtsam::NonlinearFactor::shared_ptr(new PendulumFactor1(*this))); }
/** q_k + h*v - q_k1 = 0, with optional derivatives */ /** q_k + h*v - q_k1 = 0, with optional derivatives */
@ -94,7 +94,7 @@ public:
/// @return a deep copy of this factor /// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override { 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))); } gtsam::NonlinearFactor::shared_ptr(new PendulumFactor2(*this))); }
/** v_k - h*g/L*sin(q) - v_k1 = 0, with optional derivatives */ /** 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 /// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override { 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))); } 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 */ /** 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 /// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override { 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))); } 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 */ /** 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 /// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override { 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))); } gtsam::NonlinearFactor::shared_ptr(new Reconstruction(*this))); }
/** \f$ log((g_k\exp(h\xi_k))^{-1}g_{k+1}) = 0 \f$, with optional derivatives */ /** \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 /// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override { 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))); } gtsam::NonlinearFactor::shared_ptr(new DiscreteEulerPoincareHelicopter(*this))); }
/** DEP, with optional derivatives /** DEP, with optional derivatives

2
gtsam_unstable/dynamics/VelocityConstraint.h
View File

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

2
gtsam_unstable/linear/LinearCost.h
View File

@ -100,7 +100,7 @@ public:
/** Clone this LinearCost */ /** Clone this LinearCost */
GaussianFactor::shared_ptr clone() const override { GaussianFactor::shared_ptr clone() const override {
return boost::static_pointer_cast < GaussianFactor return std::static_pointer_cast < GaussianFactor
> (std::make_shared < LinearCost > (*this)); > (std::make_shared < LinearCost > (*this));
} }

2
gtsam_unstable/linear/LinearEquality.h
View File

@ -101,7 +101,7 @@ public:
/** Clone this LinearEquality */ /** Clone this LinearEquality */
GaussianFactor::shared_ptr clone() const override { GaussianFactor::shared_ptr clone() const override {
return boost::static_pointer_cast < GaussianFactor return std::static_pointer_cast < GaussianFactor
> (std::make_shared < LinearEquality > (*this)); > (std::make_shared < LinearEquality > (*this));
} }

2
gtsam_unstable/linear/LinearInequality.h
View File

@ -115,7 +115,7 @@ public:
/** Clone this LinearInequality */ /** Clone this LinearInequality */
GaussianFactor::shared_ptr clone() const override { GaussianFactor::shared_ptr clone() const override {
return boost::static_pointer_cast < GaussianFactor return std::static_pointer_cast < GaussianFactor
> (std::make_shared < LinearInequality > (*this)); > (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) { const std::string& indent, const KeyFormatter& keyFormatter) {
std::cout << indent; std::cout << indent;
if(factor) { if(factor) {
if(boost::dynamic_pointer_cast<LinearContainerFactor>(factor)) { if(std::dynamic_pointer_cast<LinearContainerFactor>(factor)) {
std::cout << "l( "; std::cout << "l( ";
} else { } else {
std::cout << "f( "; std::cout << "f( ";
@ -65,8 +65,8 @@ void ConcurrentBatchFilter::PrintLinearFactor(const GaussianFactor::shared_ptr&
const std::string& indent, const KeyFormatter& keyFormatter) { const std::string& indent, const KeyFormatter& keyFormatter) {
std::cout << indent; std::cout << indent;
if(factor) { if(factor) {
JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(factor); JacobianFactor::shared_ptr jf = std::dynamic_pointer_cast<JacobianFactor>(factor);
HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast<HessianFactor>(factor); HessianFactor::shared_ptr hf = std::dynamic_pointer_cast<HessianFactor>(factor);
if(jf) { if(jf) {
std::cout << "j( "; std::cout << "j( ";
} else if(hf) { } 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) { void ConcurrentBatchSmoother::PrintNonlinearFactor(const NonlinearFactor::shared_ptr& factor, const std::string& indent, const KeyFormatter& keyFormatter) {
std::cout << indent; std::cout << indent;
if(factor) { if(factor) {
if(boost::dynamic_pointer_cast<LinearContainerFactor>(factor)) { if(std::dynamic_pointer_cast<LinearContainerFactor>(factor)) {
std::cout << "l( "; std::cout << "l( ";
} else { } else {
std::cout << "f( "; std::cout << "f( ";

4
gtsam_unstable/nonlinear/LinearizedFactor.h
View File

@ -113,7 +113,7 @@ public:
/// @return a deep copy of this factor /// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override { 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))); } gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
// Testable // Testable
@ -203,7 +203,7 @@ public:
/// @return a deep copy of this factor /// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override { 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))); } gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
// Testable // 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) { 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) if (!nonlinearCluster)
throw std::runtime_error("Expected NonlinearCluster"); throw std::runtime_error("Expected NonlinearCluster");
return nonlinearCluster.get(); 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<GaussianFactor> gfstandard = f.linearize(standard);
std::shared_ptr<JacobianFactor> jfstandard = // 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; typedef std::pair<Eigen::MatrixXd, Eigen::VectorXd> Jacobian;
Jacobian Jstandard = jfstandard->jacobianUnweighted(); Jacobian Jstandard = jfstandard->jacobianUnweighted();
@ -90,7 +90,7 @@ TEST(ExpressionCustomChart, projection) {
std::shared_ptr<GaussianFactor> gfcustom = f.linearize(custom); std::shared_ptr<GaussianFactor> gfcustom = f.linearize(custom);
std::shared_ptr<JacobianFactor> jfcustom = // 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); Eigen::MatrixXd expectedJacobian = Eigen::MatrixXd::Zero(3,2);
expectedJacobian(0,0) = 2.0; 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 // 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 jacobian1(jf, values);
LinearizedJacobianFactor jacobian2(jf, values); LinearizedJacobianFactor jacobian2(jf, values);
@ -65,13 +65,13 @@ TEST( LinearizedFactor, clone_jacobian )
BetweenFactor<Point3> betweenFactor(x1, x2, measured, model); BetweenFactor<Point3> betweenFactor(x1, x2, measured, model);
// Create one factor that is a clone of the other and make sure they're equal // 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 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)); CHECK(assert_equal(jacobian1, *jacobian2));
JacobianFactor::shared_ptr jf1 = boost::static_pointer_cast<JacobianFactor>(jacobian1.linearize(values)); JacobianFactor::shared_ptr jf1 = std::static_pointer_cast<JacobianFactor>(jacobian1.linearize(values));
JacobianFactor::shared_ptr jf2 = boost::static_pointer_cast<JacobianFactor>(jacobian2->linearize(values)); JacobianFactor::shared_ptr jf2 = std::static_pointer_cast<JacobianFactor>(jacobian2->linearize(values));
CHECK(assert_equal(*jf1, *jf2)); CHECK(assert_equal(*jf1, *jf2));
Matrix information1 = jf1->augmentedInformation(); Matrix information1 = jf1->augmentedInformation();
@ -94,7 +94,7 @@ TEST( LinearizedFactor, add_jacobian )
BetweenFactor<Point3> betweenFactor(x1, x2, measured, model); 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 // 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)); LinearizedJacobianFactor::shared_ptr jacobian(new LinearizedJacobianFactor(jf, values));
NonlinearFactorGraph graph1; graph1.push_back(jacobian); NonlinearFactorGraph graph1; graph1.push_back(jacobian);
NonlinearFactorGraph graph2; graph2.push_back(*jacobian); NonlinearFactorGraph graph2; graph2.push_back(*jacobian);
@ -124,7 +124,7 @@ TEST( LinearizedFactor, add_jacobian )
// //
// //
// // Create a linearized jacobian factors // // 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); // LinearizedJacobianFactor jacobian(jf, values);
// //
// //
@ -176,7 +176,7 @@ TEST( LinearizedFactor, equals_hessian )
// Create two identical factors and make sure they're equal // 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)); HessianFactor::shared_ptr hf(new HessianFactor(*jf));
LinearizedHessianFactor hessian1(hf, values); LinearizedHessianFactor hessian1(hf, values);
LinearizedHessianFactor hessian2(hf, values); LinearizedHessianFactor hessian2(hf, values);
@ -200,10 +200,10 @@ TEST( LinearizedFactor, clone_hessian )
// Create two identical factors and make sure they're equal // 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)); HessianFactor::shared_ptr hf(new HessianFactor(*jf));
LinearizedHessianFactor hessian1(hf, values); 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)); CHECK(assert_equal(hessian1, *hessian2));
} }
@ -224,7 +224,7 @@ TEST( LinearizedFactor, add_hessian )
// Create two identical factors and make sure they're equal // 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)); HessianFactor::shared_ptr hf(new HessianFactor(*jf));
LinearizedHessianFactor::shared_ptr hessian(new LinearizedHessianFactor(hf, values)); LinearizedHessianFactor::shared_ptr hessian(new LinearizedHessianFactor(hf, values));
NonlinearFactorGraph graph1; graph1.push_back(hessian); NonlinearFactorGraph graph1; graph1.push_back(hessian);
@ -252,7 +252,7 @@ TEST( LinearizedFactor, add_hessian )
// //
// //
// // Create a linearized hessian factor // // 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)); // HessianFactor::shared_ptr hf(new HessianFactor(*jf));
// LinearizedHessianFactor hessian(hf, values); // LinearizedHessianFactor hessian(hf, values);
// //

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