diff --git a/gtsam/discrete/DecisionTreeFactor.cpp b/gtsam/discrete/DecisionTreeFactor.cpp index 105ff3a38..7f566a115 100644 --- a/gtsam/discrete/DecisionTreeFactor.cpp +++ b/gtsam/discrete/DecisionTreeFactor.cpp @@ -22,6 +22,7 @@ #include #include +#include using namespace std; @@ -94,8 +95,7 @@ namespace gtsam { Index j = keys()[i]; dkeys.push_back(DiscreteKey(j,cardinality(j))); } - shared_ptr f(new DecisionTreeFactor(dkeys, result)); - return f; + return boost::make_shared(dkeys, result); } /* ************************************************************************* */ diff --git a/gtsam/inference/SymbolicFactorGraph.cpp b/gtsam/inference/SymbolicFactorGraph.cpp index 2ae0b483a..514ba156b 100644 --- a/gtsam/inference/SymbolicFactorGraph.cpp +++ b/gtsam/inference/SymbolicFactorGraph.cpp @@ -15,13 +15,13 @@ * @author Frank Dellaert */ -#include - #include #include #include #include +#include + namespace gtsam { using namespace std; @@ -32,26 +32,22 @@ namespace gtsam { /* ************************************************************************* */ void SymbolicFactorGraph::push_factor(Index key) { - boost::shared_ptr factor(new IndexFactor(key)); - push_back(factor); + push_back(boost::make_shared(key)); } /** Push back binary factor */ void SymbolicFactorGraph::push_factor(Index key1, Index key2) { - boost::shared_ptr factor(new IndexFactor(key1,key2)); - push_back(factor); + push_back(boost::make_shared(key1,key2)); } /** Push back ternary factor */ void SymbolicFactorGraph::push_factor(Index key1, Index key2, Index key3) { - boost::shared_ptr factor(new IndexFactor(key1,key2,key3)); - push_back(factor); + push_back(boost::make_shared(key1,key2,key3)); } /** Push back 4-way factor */ void SymbolicFactorGraph::push_factor(Index key1, Index key2, Index key3, Index key4) { - boost::shared_ptr factor(new IndexFactor(key1,key2,key3,key4)); - push_back(factor); + push_back(boost::make_shared(key1,key2,key3,key4)); } /* ************************************************************************* */ @@ -66,7 +62,7 @@ namespace gtsam { /* ************************************************************************* */ IndexFactor::shared_ptr CombineSymbolic( const FactorGraph& factors, const FastMap >& variableSlots) { + vector >& variableSlots) { IndexFactor::shared_ptr combined(Combine (factors, variableSlots)); // combined->assertInvariants(); return combined; @@ -84,12 +80,9 @@ namespace gtsam { if (keys.size() < 1) throw invalid_argument( "IndexFactor::CombineAndEliminate called on factors with no variables."); - pair result; - std::vector newKeys(keys.begin(),keys.end()); - result.first.reset(new IndexConditional(newKeys, nrFrontals)); - result.second.reset(new IndexFactor(newKeys.begin()+nrFrontals, newKeys.end())); - - return result; + vector newKeys(keys.begin(), keys.end()); + return make_pair(new IndexConditional(newKeys, nrFrontals), + new IndexFactor(newKeys.begin() + nrFrontals, newKeys.end())); } /* ************************************************************************* */ diff --git a/gtsam/linear/GaussianBayesNet.cpp b/gtsam/linear/GaussianBayesNet.cpp index 62b63bb27..d1f0d8f8d 100644 --- a/gtsam/linear/GaussianBayesNet.cpp +++ b/gtsam/linear/GaussianBayesNet.cpp @@ -15,19 +15,20 @@ * @author Frank Dellaert */ -#include +#include +#include +#include +#include + #include +#include #include -#include -#include -#include - -#include +#include using namespace std; using namespace gtsam; - +using boost::shared_ptr; // trick from some reading group #define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL) @@ -72,13 +73,13 @@ void push_front(GaussianBayesNet& bn, Index key, Vector d, Matrix R, } /* ************************************************************************* */ -boost::shared_ptr allocateVectorValues(const GaussianBayesNet& bn) { +shared_ptr allocateVectorValues(const GaussianBayesNet& bn) { vector dimensions(bn.size()); Index var = 0; - BOOST_FOREACH(const boost::shared_ptr conditional, bn) { + BOOST_FOREACH(const shared_ptr conditional, bn) { dimensions[var++] = conditional->dim(); } - return boost::shared_ptr(new VectorValues(dimensions)); + return shared_ptr(new VectorValues(dimensions)); } /* ************************************************************************* */ @@ -92,7 +93,7 @@ VectorValues optimize(const GaussianBayesNet& bn) { // (R*x)./sigmas = y by solving x=inv(R)*(y.*sigmas) void optimizeInPlace(const GaussianBayesNet& bn, VectorValues& x) { /** solve each node in turn in topological sort order (parents first)*/ - BOOST_REVERSE_FOREACH(const boost::shared_ptr cg, bn) { + BOOST_REVERSE_FOREACH(const shared_ptr cg, bn) { // i^th part of R*x=y, x=inv(R)*y // (Rii*xi + R_i*x(i+1:))./si = yi <-> xi = inv(Rii)*(yi.*si - R_i*x(i+1:)) cg->solveInPlace(x); @@ -102,14 +103,14 @@ void optimizeInPlace(const GaussianBayesNet& bn, VectorValues& x) { /* ************************************************************************* */ VectorValues backSubstitute(const GaussianBayesNet& bn, const VectorValues& input) { VectorValues output = input; - BOOST_REVERSE_FOREACH(const boost::shared_ptr cg, bn) { + BOOST_REVERSE_FOREACH(const shared_ptr cg, bn) { const Index key = *(cg->beginFrontals()); Vector xS = internal::extractVectorValuesSlices(output, cg->beginParents(), cg->endParents()); xS = input[key] - cg->get_S() * xS; output[key] = cg->get_R().triangularView().solve(xS); } - BOOST_FOREACH(const boost::shared_ptr cg, bn) { + BOOST_FOREACH(const shared_ptr cg, bn) { cg->scaleFrontalsBySigma(output); } @@ -130,7 +131,7 @@ VectorValues backSubstituteTranspose(const GaussianBayesNet& bn, // we loop from first-eliminated to last-eliminated // i^th part of L*gy=gx is done block-column by block-column of L - BOOST_FOREACH(const boost::shared_ptr cg, bn) + BOOST_FOREACH(const shared_ptr cg, bn) cg->solveTransposeInPlace(gy); // Scale gy @@ -196,7 +197,7 @@ pair matrix(const GaussianBayesNet& bn) { Index key; size_t I; FOREACH_PAIR(key,I,mapping) { // find corresponding conditional - boost::shared_ptr cg = bn[key]; + shared_ptr cg = bn[key]; // get sigmas Vector sigmas = cg->get_sigmas(); @@ -233,7 +234,7 @@ pair matrix(const GaussianBayesNet& bn) { double determinant(const GaussianBayesNet& bayesNet) { double logDet = 0.0; - BOOST_FOREACH(boost::shared_ptr cg, bayesNet){ + BOOST_FOREACH(shared_ptr cg, bayesNet){ logDet += cg->get_R().diagonal().unaryExpr(ptr_fun(log)).sum(); } diff --git a/gtsam/slam/visualSLAM.cpp b/gtsam/slam/visualSLAM.cpp index 1331ef20f..bb3cd19d1 100644 --- a/gtsam/slam/visualSLAM.cpp +++ b/gtsam/slam/visualSLAM.cpp @@ -17,6 +17,9 @@ #include #include +#include + +using boost::make_shared; namespace visualSLAM { @@ -62,49 +65,43 @@ namespace visualSLAM { /* ************************************************************************* */ void Graph::addMeasurement(const Point2& measured, const SharedNoiseModel& model, Key poseKey, Key pointKey, const shared_ptrK K) { - boost::shared_ptr factor(new ProjectionFactor(measured, model, poseKey, pointKey, K)); - push_back(factor); + push_back(make_shared(measured, model, poseKey, pointKey, K)); } /* ************************************************************************* */ void Graph::addStereoMeasurement(const StereoPoint2& measured, const SharedNoiseModel& model, Key poseKey, Key pointKey, const shared_ptrKStereo K) { - boost::shared_ptr factor(new StereoFactor(measured, model, poseKey, pointKey, K)); - push_back(factor); + push_back(make_shared(measured, model, poseKey, pointKey, K)); } /* ************************************************************************* */ void Graph::addPoseConstraint(Key poseKey, const Pose3& p) { - boost::shared_ptr factor(new PoseConstraint(poseKey, p)); - push_back(factor); + push_back(make_shared(poseKey, p)); } /* ************************************************************************* */ void Graph::addPointConstraint(Key pointKey, const Point3& p) { - boost::shared_ptr factor(new PointConstraint(pointKey, p)); - push_back(factor); + push_back(make_shared(pointKey, p)); } /* ************************************************************************* */ void Graph::addPosePrior(Key poseKey, const Pose3& p, const SharedNoiseModel& model) { - boost::shared_ptr factor(new PosePrior(poseKey, p, model)); - push_back(factor); + push_back(make_shared(poseKey, p, model)); } /* ************************************************************************* */ void Graph::addPointPrior(Key pointKey, const Point3& p, const SharedNoiseModel& model) { - boost::shared_ptr factor(new PointPrior(pointKey, p, model)); - push_back(factor); + push_back(make_shared(pointKey, p, model)); } /* ************************************************************************* */ void Graph::addRangeFactor(Key poseKey, Key pointKey, double range, const SharedNoiseModel& model) { - push_back(boost::shared_ptr(new RangeFactor(poseKey, pointKey, range, model))); + push_back(make_shared(poseKey, pointKey, range, model)); } /* ************************************************************************* */ void Graph::addOdometry(Key x1, Key x2, const Pose3& odometry, const SharedNoiseModel& model) { - push_back(boost::shared_ptr >(new BetweenFactor(x1, x2, odometry, model))); + push_back(make_shared >(x1, x2, odometry, model)); } /* ************************************************************************* */