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 <gtsam/base/FastSet.h>
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
#include <boost/make_shared.hpp>
using namespace std; using namespace std;
@ -94,8 +95,7 @@ namespace gtsam {
Index j = keys()[i]; Index j = keys()[i];
dkeys.push_back(DiscreteKey(j,cardinality(j))); dkeys.push_back(DiscreteKey(j,cardinality(j)));
} }
shared_ptr f(new DecisionTreeFactor(dkeys, result)); return boost::make_shared<DecisionTreeFactor>(dkeys, result);
return f;
} }
/* ************************************************************************* */ /* ************************************************************************* */

27
gtsam/inference/SymbolicFactorGraph.cpp
View File

@ -15,13 +15,13 @@
* @author Frank Dellaert * @author Frank Dellaert
*/ */
#include <boost/make_shared.hpp>
#include <gtsam/inference/SymbolicFactorGraph.h> #include <gtsam/inference/SymbolicFactorGraph.h>
#include <gtsam/inference/BayesNet.h> #include <gtsam/inference/BayesNet.h>
#include <gtsam/inference/EliminationTree.h> #include <gtsam/inference/EliminationTree.h>
#include <gtsam/inference/IndexConditional.h> #include <gtsam/inference/IndexConditional.h>
#include <boost/make_shared.hpp>
namespace gtsam { namespace gtsam {
using namespace std; using namespace std;
@ -32,26 +32,22 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
void SymbolicFactorGraph::push_factor(Index key) { void SymbolicFactorGraph::push_factor(Index key) {
boost::shared_ptr<IndexFactor> factor(new IndexFactor(key)); push_back(boost::make_shared<IndexFactor>(key));
push_back(factor);
} }
/** Push back binary factor */ /** Push back binary factor */
void SymbolicFactorGraph::push_factor(Index key1, Index key2) { void SymbolicFactorGraph::push_factor(Index key1, Index key2) {
boost::shared_ptr<IndexFactor> factor(new IndexFactor(key1,key2)); push_back(boost::make_shared<IndexFactor>(key1,key2));
push_back(factor);
} }
/** Push back ternary factor */ /** Push back ternary factor */
void SymbolicFactorGraph::push_factor(Index key1, Index key2, Index key3) { void SymbolicFactorGraph::push_factor(Index key1, Index key2, Index key3) {
boost::shared_ptr<IndexFactor> factor(new IndexFactor(key1,key2,key3)); push_back(boost::make_shared<IndexFactor>(key1,key2,key3));
push_back(factor);
} }
/** Push back 4-way factor */ /** Push back 4-way factor */
void SymbolicFactorGraph::push_factor(Index key1, Index key2, Index key3, Index key4) { 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(boost::make_shared<IndexFactor>(key1,key2,key3,key4));
push_back(factor);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -66,7 +62,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
IndexFactor::shared_ptr CombineSymbolic( IndexFactor::shared_ptr CombineSymbolic(
const FactorGraph<IndexFactor>& factors, const FastMap<Index, const FactorGraph<IndexFactor>& factors, const FastMap<Index,
std::vector<Index> >& variableSlots) { vector<Index> >& variableSlots) {
IndexFactor::shared_ptr combined(Combine<IndexFactor, Index> (factors, variableSlots)); IndexFactor::shared_ptr combined(Combine<IndexFactor, Index> (factors, variableSlots));
// combined->assertInvariants(); // combined->assertInvariants();
return combined; return combined;
@ -84,12 +80,9 @@ namespace gtsam {
if (keys.size() < 1) throw invalid_argument( if (keys.size() < 1) throw invalid_argument(
"IndexFactor::CombineAndEliminate called on factors with no variables."); "IndexFactor::CombineAndEliminate called on factors with no variables.");
pair<IndexConditional::shared_ptr, IndexFactor::shared_ptr> result; vector<Index> newKeys(keys.begin(), keys.end());
std::vector<Index> newKeys(keys.begin(),keys.end()); return make_pair(new IndexConditional(newKeys, nrFrontals),
result.first.reset(new IndexConditional(newKeys, nrFrontals)); new IndexFactor(newKeys.begin() + nrFrontals, newKeys.end()));
result.second.reset(new IndexFactor(newKeys.begin()+nrFrontals, newKeys.end()));
return result;
} }
/* ************************************************************************* */ /* ************************************************************************* */

33
gtsam/linear/GaussianBayesNet.cpp
View File

@ -15,19 +15,20 @@
* @author Frank Dellaert * @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/foreach.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/tuple/tuple.hpp> #include <boost/tuple/tuple.hpp>
#include <gtsam/linear/JacobianFactorGraph.h> #include <stdarg.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/inference/BayesNet-inl.h>
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
using boost::shared_ptr;
// trick from some reading group // trick from some reading group
#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL) #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()); vector<size_t> dimensions(bn.size());
Index var = 0; 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(); 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) // (R*x)./sigmas = y by solving x=inv(R)*(y.*sigmas)
void optimizeInPlace(const GaussianBayesNet& bn, VectorValues& x) { void optimizeInPlace(const GaussianBayesNet& bn, VectorValues& x) {
/** solve each node in turn in topological sort order (parents first)*/ /** 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 // 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:)) // (Rii*xi + R_i*x(i+1:))./si = yi <-> xi = inv(Rii)*(yi.*si - R_i*x(i+1:))
cg->solveInPlace(x); cg->solveInPlace(x);
@ -102,14 +103,14 @@ void optimizeInPlace(const GaussianBayesNet& bn, VectorValues& x) {
/* ************************************************************************* */ /* ************************************************************************* */
VectorValues backSubstitute(const GaussianBayesNet& bn, const VectorValues& input) { VectorValues backSubstitute(const GaussianBayesNet& bn, const VectorValues& input) {
VectorValues output = 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()); const Index key = *(cg->beginFrontals());
Vector xS = internal::extractVectorValuesSlices(output, cg->beginParents(), cg->endParents()); Vector xS = internal::extractVectorValuesSlices(output, cg->beginParents(), cg->endParents());
xS = input[key] - cg->get_S() * xS; xS = input[key] - cg->get_S() * xS;
output[key] = cg->get_R().triangularView<Eigen::Upper>().solve(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); cg->scaleFrontalsBySigma(output);
} }
@ -130,7 +131,7 @@ VectorValues backSubstituteTranspose(const GaussianBayesNet& bn,
// we loop from first-eliminated to last-eliminated // we loop from first-eliminated to last-eliminated
// i^th part of L*gy=gx is done block-column by block-column of L // 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); cg->solveTransposeInPlace(gy);
// Scale gy // Scale gy
@ -196,7 +197,7 @@ pair<Matrix,Vector> matrix(const GaussianBayesNet& bn) {
Index key; size_t I; Index key; size_t I;
FOREACH_PAIR(key,I,mapping) { FOREACH_PAIR(key,I,mapping) {
// find corresponding conditional // find corresponding conditional
boost::shared_ptr<const GaussianConditional> cg = bn[key]; shared_ptr<const GaussianConditional> cg = bn[key];
// get sigmas // get sigmas
Vector sigmas = cg->get_sigmas(); Vector sigmas = cg->get_sigmas();
@ -233,7 +234,7 @@ pair<Matrix,Vector> matrix(const GaussianBayesNet& bn) {
double determinant(const GaussianBayesNet& bayesNet) { double determinant(const GaussianBayesNet& bayesNet) {
double logDet = 0.0; 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(); 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/visualSLAM.h>
#include <gtsam/slam/BetweenFactor.h> #include <gtsam/slam/BetweenFactor.h>
#include <boost/make_shared.hpp>
using boost::make_shared;
namespace visualSLAM { namespace visualSLAM {
@ -62,49 +65,43 @@ namespace visualSLAM {
/* ************************************************************************* */ /* ************************************************************************* */
void Graph::addMeasurement(const Point2& measured, const SharedNoiseModel& model, void Graph::addMeasurement(const Point2& measured, const SharedNoiseModel& model,
Key poseKey, Key pointKey, const shared_ptrK K) { Key poseKey, Key pointKey, const shared_ptrK K) {
boost::shared_ptr<ProjectionFactor> factor(new ProjectionFactor(measured, model, poseKey, pointKey, K)); push_back(make_shared<ProjectionFactor>(measured, model, poseKey, pointKey, K));
push_back(factor);
} }
/* ************************************************************************* */ /* ************************************************************************* */
void Graph::addStereoMeasurement(const StereoPoint2& measured, const SharedNoiseModel& model, void Graph::addStereoMeasurement(const StereoPoint2& measured, const SharedNoiseModel& model,
Key poseKey, Key pointKey, const shared_ptrKStereo K) { Key poseKey, Key pointKey, const shared_ptrKStereo K) {
boost::shared_ptr<StereoFactor> factor(new StereoFactor(measured, model, poseKey, pointKey, K)); push_back(make_shared<StereoFactor>(measured, model, poseKey, pointKey, K));
push_back(factor);
} }
/* ************************************************************************* */ /* ************************************************************************* */
void Graph::addPoseConstraint(Key poseKey, const Pose3& p) { void Graph::addPoseConstraint(Key poseKey, const Pose3& p) {
boost::shared_ptr<PoseConstraint> factor(new PoseConstraint(poseKey, p)); push_back(make_shared<PoseConstraint>(poseKey, p));
push_back(factor);
} }
/* ************************************************************************* */ /* ************************************************************************* */
void Graph::addPointConstraint(Key pointKey, const Point3& p) { void Graph::addPointConstraint(Key pointKey, const Point3& p) {
boost::shared_ptr<PointConstraint> factor(new PointConstraint(pointKey, p)); push_back(make_shared<PointConstraint>(pointKey, p));
push_back(factor);
} }
/* ************************************************************************* */ /* ************************************************************************* */
void Graph::addPosePrior(Key poseKey, const Pose3& p, const SharedNoiseModel& model) { void Graph::addPosePrior(Key poseKey, const Pose3& p, const SharedNoiseModel& model) {
boost::shared_ptr<PosePrior> factor(new PosePrior(poseKey, p, model)); push_back(make_shared<PosePrior>(poseKey, p, model));
push_back(factor);
} }
/* ************************************************************************* */ /* ************************************************************************* */
void Graph::addPointPrior(Key pointKey, const Point3& p, const SharedNoiseModel& model) { void Graph::addPointPrior(Key pointKey, const Point3& p, const SharedNoiseModel& model) {
boost::shared_ptr<PointPrior> factor(new PointPrior(pointKey, p, model)); push_back(make_shared<PointPrior>(pointKey, p, model));
push_back(factor);
} }
/* ************************************************************************* */ /* ************************************************************************* */
void Graph::addRangeFactor(Key poseKey, Key pointKey, double range, const SharedNoiseModel& 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) { 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));
} }
/* ************************************************************************* */ /* ************************************************************************* */