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

created addPrior

release/4.3a0
Kai Ni 2010-02-19 02:48:47 +00:00
parent e60d903210
commit d0ce5d36da
3 changed files with 12 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
cpp/SymbolicFactorGraph.cpp
View File

@ -52,7 +52,8 @@ namespace gtsam {
Point2 pt;
float scale = 100;
ofstream of(s.c_str());
string dotfile = s + ".dot";
ofstream of(dotfile.c_str());
of << "graph G{" << endl;
of << "bgcolor=\"transparent\";" << endl;
@ -74,9 +75,7 @@ namespace gtsam {
of<<"}";
of.close();
char filename[100];
sscanf(s.c_str(), "%s.dot", filename);
string cmd = boost::str(boost::format("neato -s -n -Tpdf %s -o %s.pdf") % s % filename);
string cmd = boost::str(boost::format("neato -s -n -Tpdf %s -o %s.pdf") % dotfile % s);
system(cmd.c_str());
}

6
cpp/planarSLAM.cpp
View File

@ -20,6 +20,12 @@ namespace gtsam {
namespace planarSLAM {
void Graph::addPrior(const PoseKey& i, const Pose2& p,
const SharedGaussian& model) {
sharedFactor factor(new Prior(i, p, model));
push_back(factor);
}
void Graph::addPoseConstraint(const PoseKey& i, const Pose2& p) {
sharedFactor factor(new Constraint(i, p));
push_back(factor);

3
cpp/planarSLAM.h
View File

@ -12,6 +12,7 @@
#include "NonlinearFactor.h"
#include "TupleConfig.h"
#include "NonlinearEquality.h"
#include "PriorFactor.h"
#include "BetweenFactor.h"
#include "NonlinearFactorGraph.h"
#include "NonlinearOptimizer.h"
@ -86,12 +87,14 @@ namespace gtsam {
// Factors
typedef NonlinearEquality<Config, PoseKey, Pose2> Constraint;
typedef PriorFactor<Config, PoseKey, Pose2> Prior;
typedef BetweenFactor<Config, PoseKey, Pose2> Odometry;
typedef BearingFactor<Config, PoseKey, PointKey> Bearing;
typedef RangeFactor<Config, PoseKey, PointKey> Range;
// Graph
struct Graph: public NonlinearFactorGraph<Config> {
void addPrior(const PoseKey& i, const Pose2& p, const SharedGaussian& model);
void addPoseConstraint(const PoseKey& i, const Pose2& p);
void addOdometry(const PoseKey& i, const PoseKey& j, const Pose2& z,
const SharedGaussian& model);