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

cleaned up code a bit

release/4.3a0
Frank Dellaert 2012-06-09 21:31:22 +00:00
parent b602e75a99
commit f9db53fdb8
4 changed files with 40 additions and 49 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam/discrete/DecisionTreeFactor.cpp
View File

@ -22,6 +22,7 @@
#include <gtsam/base/FastSet.h>
#include <boost/foreach.hpp>
#include <boost/make_shared.hpp>
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<DecisionTreeFactor>(dkeys, result);
}
/* ************************************************************************* */

27
gtsam/inference/SymbolicFactorGraph.cpp
View File

@ -15,13 +15,13 @@
* @author Frank Dellaert
*/
#include <boost/make_shared.hpp>
#include <gtsam/inference/SymbolicFactorGraph.h>
#include <gtsam/inference/BayesNet.h>
#include <gtsam/inference/EliminationTree.h>
#include <gtsam/inference/IndexConditional.h>
#include <boost/make_shared.hpp>
namespace gtsam {
using namespace std;
@ -32,26 +32,22 @@ namespace gtsam {
/* ************************************************************************* */
void SymbolicFactorGraph::push_factor(Index key) {
boost::shared_ptr<IndexFactor> factor(new IndexFactor(key));
push_back(factor);
push_back(boost::make_shared<IndexFactor>(key));
}
/** Push back binary factor */
void SymbolicFactorGraph::push_factor(Index key1, Index key2) {
boost::shared_ptr<IndexFactor> factor(new IndexFactor(key1,key2));
push_back(factor);
push_back(boost::make_shared<IndexFactor>(key1,key2));
}
/** Push back ternary factor */
void SymbolicFactorGraph::push_factor(Index key1, Index key2, Index key3) {
boost::shared_ptr<IndexFactor> factor(new IndexFactor(key1,key2,key3));
push_back(factor);
push_back(boost::make_shared<IndexFactor>(key1,key2,key3));
}
/** Push back 4-way factor */
void SymbolicFactorGraph::push_factor(Index key1, Index key2, Index key3, Index key4) {
boost::shared_ptr<IndexFactor> factor(new IndexFactor(key1,key2,key3,key4));
push_back(factor);
push_back(boost::make_shared<IndexFactor>(key1,key2,key3,key4));
}
/* ************************************************************************* */
@ -66,7 +62,7 @@ namespace gtsam {
/* ************************************************************************* */
IndexFactor::shared_ptr CombineSymbolic(
const FactorGraph<IndexFactor>& factors, const FastMap<Index,
std::vector<Index> >& variableSlots) {
vector<Index> >& variableSlots) {
IndexFactor::shared_ptr combined(Combine<IndexFactor, Index> (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<IndexConditional::shared_ptr, IndexFactor::shared_ptr> result;
std::vector<Index> 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<Index> newKeys(keys.begin(), keys.end());
return make_pair(new IndexConditional(newKeys, nrFrontals),
new IndexFactor(newKeys.begin() + nrFrontals, newKeys.end()));
}
/* ************************************************************************* */

33
gtsam/linear/GaussianBayesNet.cpp
View File

@ -15,19 +15,20 @@
* @author Frank Dellaert
*/
#include <stdarg.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/JacobianFactorGraph.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/inference/BayesNet-inl.h>
#include <boost/foreach.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/tuple/tuple.hpp>
#include <gtsam/linear/JacobianFactorGraph.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/inference/BayesNet-inl.h>
#include <stdarg.h>
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<VectorValues> allocateVectorValues(const GaussianBayesNet& bn) {
shared_ptr<VectorValues> allocateVectorValues(const GaussianBayesNet& bn) {
vector<size_t> dimensions(bn.size());
Index var = 0;
BOOST_FOREACH(const boost::shared_ptr<const GaussianConditional> conditional, bn) {
BOOST_FOREACH(const shared_ptr<const GaussianConditional> conditional, bn) {
dimensions[var++] = conditional->dim();
}
return boost::shared_ptr<VectorValues>(new VectorValues(dimensions));
return shared_ptr<VectorValues>(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<const GaussianConditional> cg, bn) {
BOOST_REVERSE_FOREACH(const shared_ptr<const GaussianConditional> 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<const GaussianConditional> cg, bn) {
BOOST_REVERSE_FOREACH(const shared_ptr<const GaussianConditional> 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<Eigen::Upper>().solve(xS);
}
BOOST_FOREACH(const boost::shared_ptr<const GaussianConditional> cg, bn) {
BOOST_FOREACH(const shared_ptr<const GaussianConditional> 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<const GaussianConditional> cg, bn)
BOOST_FOREACH(const shared_ptr<const GaussianConditional> cg, bn)
cg->solveTransposeInPlace(gy);
// Scale gy
@ -196,7 +197,7 @@ pair<Matrix,Vector> matrix(const GaussianBayesNet& bn) {
Index key; size_t I;
FOREACH_PAIR(key,I,mapping) {
// find corresponding conditional
boost::shared_ptr<const GaussianConditional> cg = bn[key];
shared_ptr<const GaussianConditional> cg = bn[key];
// get sigmas
Vector sigmas = cg->get_sigmas();
@ -233,7 +234,7 @@ pair<Matrix,Vector> matrix(const GaussianBayesNet& bn) {
double determinant(const GaussianBayesNet& bayesNet) {
double logDet = 0.0;
BOOST_FOREACH(boost::shared_ptr<const GaussianConditional> cg, bayesNet){
BOOST_FOREACH(shared_ptr<const GaussianConditional> cg, bayesNet){
logDet += cg->get_R().diagonal().unaryExpr(ptr_fun<double,double>(log)).sum();
}

25
gtsam/slam/visualSLAM.cpp
View File

@ -17,6 +17,9 @@
#include <gtsam/slam/visualSLAM.h>
#include <gtsam/slam/BetweenFactor.h>
#include <boost/make_shared.hpp>
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<ProjectionFactor> factor(new ProjectionFactor(measured, model, poseKey, pointKey, K));
push_back(factor);
push_back(make_shared<ProjectionFactor>(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<StereoFactor> factor(new StereoFactor(measured, model, poseKey, pointKey, K));
push_back(factor);
push_back(make_shared<StereoFactor>(measured, model, poseKey, pointKey, K));
}
/* ************************************************************************* */
void Graph::addPoseConstraint(Key poseKey, const Pose3& p) {
boost::shared_ptr<PoseConstraint> factor(new PoseConstraint(poseKey, p));
push_back(factor);
push_back(make_shared<PoseConstraint>(poseKey, p));
}
/* ************************************************************************* */
void Graph::addPointConstraint(Key pointKey, const Point3& p) {
boost::shared_ptr<PointConstraint> factor(new PointConstraint(pointKey, p));
push_back(factor);
push_back(make_shared<PointConstraint>(pointKey, p));
}
/* ************************************************************************* */
void Graph::addPosePrior(Key poseKey, const Pose3& p, const SharedNoiseModel& model) {
boost::shared_ptr<PosePrior> factor(new PosePrior(poseKey, p, model));
push_back(factor);
push_back(make_shared<PosePrior>(poseKey, p, model));
}
/* ************************************************************************* */
void Graph::addPointPrior(Key pointKey, const Point3& p, const SharedNoiseModel& model) {
boost::shared_ptr<PointPrior> factor(new PointPrior(pointKey, p, model));
push_back(factor);
push_back(make_shared<PointPrior>(pointKey, p, model));
}
/* ************************************************************************* */
void Graph::addRangeFactor(Key poseKey, Key pointKey, double range, const SharedNoiseModel& model) {
push_back(boost::shared_ptr<RangeFactor>(new RangeFactor(poseKey, pointKey, range, model)));
push_back(make_shared<RangeFactor>(poseKey, pointKey, range, model));
}
/* ************************************************************************* */
void Graph::addOdometry(Key x1, Key x2, const Pose3& odometry, const SharedNoiseModel& model) {
push_back(boost::shared_ptr<BetweenFactor<Pose3> >(new BetweenFactor<Pose3>(x1, x2, odometry, model)));
push_back(make_shared<BetweenFactor<Pose3> >(x1, x2, odometry, model));
}
/* ************************************************************************* */