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

matlab stuff compiles now. Moved lot of methods and classes to broken but the basic geometry and other stuff is in place.

release/4.3a0
Manohar Paluri 2010-10-20 19:08:15 +00:00
parent fa56595650
commit 07483753e2
3 changed files with 123 additions and 90 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

125
gtsam-broken.h
View File

@ -2,13 +2,76 @@
// Solve by parsing a namespace pose2SLAM::Values and making a Pose2SLAMValues class // Solve by parsing a namespace pose2SLAM::Values and making a Pose2SLAMValues class
// We also have to solve the shared pointer mess to avoid duplicate methods // We also have to solve the shared pointer mess to avoid duplicate methods
class GaussianFactor {
GaussianFactor(string key1,
Matrix A1,
Vector b_in,
const SharedDiagonal& model);
GaussianFactor(string key1,
Matrix A1,
string key2,
Matrix A2,
Vector b_in,
const SharedDiagonal& model);
GaussianFactor(string key1,
Matrix A1,
string key2,
Matrix A2,
string key3,
Matrix A3,
Vector b_in,
const SharedDiagonal& model);
void print(string s) const;
bool equals(const GaussianFactor& lf, double tol) const;
bool empty() const;
Vector get_b() const;
Matrix get_A(string key) const;
double error(const VectorValues& c) const;
bool involves(string key) const;
pair<Matrix,Vector> matrix(const Ordering& ordering) const;
pair<GaussianConditional*,GaussianFactor*> eliminate(string key) const;
};
class GaussianConditional {
GaussianConditional(string key,
Vector d,
Matrix R,
Vector sigmas);
GaussianConditional(string key,
Vector d,
Matrix R,
string name1,
Matrix S,
Vector sigmas);
GaussianConditional(string key,
Vector d,
Matrix R,
string name1,
Matrix S,
string name2,
Matrix T,
Vector sigmas);
void add(string key, Matrix S);
};
class GaussianFactorGraph {
GaussianConditional* eliminateOne(string key);
GaussianBayesNet* eliminate_(const Ordering& ordering);
VectorValues* optimize_(const Ordering& ordering);
pair<Matrix,Vector> matrix(const Ordering& ordering) const;
Matrix sparse(const Ordering& ordering) const;
VectorValues* steepestDescent_(const VectorValues& x0) const;
VectorValues* conjugateGradientDescent_(const VectorValues& x0) const;
};
class Pose2Values{ class Pose2Values{
Pose2Values(); Pose2Values();
Pose2 get(string key) const; Pose2 get(string key) const;
void insert(string name, const Pose2& val); void insert(string name, const Pose2& val);
void print(string s) const; void print(string s) const;
void clear(); void clear();
int size(); int size();
}; };
class Pose2Factor { class Pose2Factor {
@ -27,4 +90,60 @@ class Pose2Graph{
void push_back(Pose2Factor* factor); void push_back(Pose2Factor* factor);
}; };
class Ordering{
Ordering(string key);
Ordering subtract(const Ordering& keys) const;
void unique ();
void reverse ();
};
class SymbolicFactor{
SymbolicFactor(const Ordering& keys);
void print(string s) const;
};
class VectorValues {
void insert(string name, Vector val);
Vector get(string name) const;
bool contains(string name) const;
};
class Simulated2DPosePrior {
GaussianFactor* linearize(const Simulated2DValues& config) const;
};
class Simulated2DOrientedPosePrior {
GaussianFactor* linearize(const Simulated2DOrientedValues& config) const;
};
class Simulated2DPointPrior {
GaussianFactor* linearize(const Simulated2DValues& config) const;
};
class Simulated2DOdometry {
GaussianFactor* linearize(const Simulated2DValues& config) const;
};
class Simulated2DOrientedOdometry {
GaussianFactor* linearize(const Simulated2DOrientedValues& config) const;
};
class Simulated2DMeasurement {
GaussianFactor* linearize(const Simulated2DValues& config) const;
};
class Pose2SLAMOptimizer {
Pose2SLAMOptimizer(string dataset_name);
void print(string s) const;
void update(Vector x) const;
Vector optimize() const;
double error() const;
Matrix a1() const;
Matrix a2() const;
Vector b1() const;
Vector b2() const;
};

86
gtsam.h
View File

@ -9,60 +9,19 @@ class SharedDiagonal {
class Ordering { class Ordering {
Ordering(); Ordering();
Ordering(string key);
Ordering subtract(const Ordering& keys) const;
void print(string s) const; void print(string s) const;
bool equals(const Ordering& ord, double tol) const; bool equals(const Ordering& ord, double tol) const;
void push_back(string s); void push_back(string s);
void unique ();
void reverse ();
}; };
class SymbolicFactor{
SymbolicFactor(const Ordering& keys);
void print(string s) const;
};
class VectorValues { class VectorValues {
VectorValues(); VectorValues();
void print(string s) const; void print(string s) const;
bool equals(const VectorValues& expected, double tol) const; bool equals(const VectorValues& expected, double tol) const;
void insert(string name, Vector val);
Vector get(string name) const;
bool contains(string name) const;
size_t size() const; size_t size() const;
}; };
class GaussianFactor {
GaussianFactor(string key1,
Matrix A1,
Vector b_in,
const SharedDiagonal& model);
GaussianFactor(string key1,
Matrix A1,
string key2,
Matrix A2,
Vector b_in,
const SharedDiagonal& model);
GaussianFactor(string key1,
Matrix A1,
string key2,
Matrix A2,
string key3,
Matrix A3,
Vector b_in,
const SharedDiagonal& model);
void print(string s) const;
bool equals(const GaussianFactor& lf, double tol) const;
bool empty() const;
Vector get_b() const;
Matrix get_A(string key) const;
double error(const VectorValues& c) const;
bool involves(string key) const;
pair<Matrix,Vector> matrix(const Ordering& ordering) const;
pair<GaussianConditional*,GaussianFactor*> eliminate(string key) const;
};
class GaussianFactorSet { class GaussianFactorSet {
GaussianFactorSet(); GaussianFactorSet();
void push_back(GaussianFactor* factor); void push_back(GaussianFactor* factor);
@ -70,27 +29,8 @@ class GaussianFactorSet {
class GaussianConditional { class GaussianConditional {
GaussianConditional(); GaussianConditional();
GaussianConditional(string key,
Vector d,
Matrix R,
Vector sigmas);
GaussianConditional(string key,
Vector d,
Matrix R,
string name1,
Matrix S,
Vector sigmas);
GaussianConditional(string key,
Vector d,
Matrix R,
string name1,
Matrix S,
string name2,
Matrix T,
Vector sigmas);
void print(string s) const; void print(string s) const;
bool equals(const GaussianConditional &cg, double tol) const; bool equals(const GaussianConditional &cg, double tol) const;
void add(string key, Matrix S);
Vector solve(const VectorValues& x); Vector solve(const VectorValues& x);
}; };
@ -112,14 +52,6 @@ class GaussianFactorGraph {
double error(const VectorValues& c) const; double error(const VectorValues& c) const;
double probPrime(const VectorValues& c) const; double probPrime(const VectorValues& c) const;
void combine(const GaussianFactorGraph& lfg); void combine(const GaussianFactorGraph& lfg);
GaussianConditional* eliminateOne(string key);
GaussianBayesNet* eliminate_(const Ordering& ordering);
VectorValues* optimize_(const Ordering& ordering);
pair<Matrix,Vector> matrix(const Ordering& ordering) const;
Matrix sparse(const Ordering& ordering) const;
VectorValues* steepestDescent_(const VectorValues& x0) const;
VectorValues* conjugateGradientDescent_(const VectorValues& x0) const;
}; };
class Point2 { class Point2 {
@ -184,54 +116,36 @@ class Simulated2DOrientedValues {
class Simulated2DPosePrior { class Simulated2DPosePrior {
Simulated2DPosePrior(Point2& mu, const SharedDiagonal& model, int i); Simulated2DPosePrior(Point2& mu, const SharedDiagonal& model, int i);
void print(string s) const; void print(string s) const;
GaussianFactor* linearize(const Simulated2DValues& config) const;
double error(const Simulated2DValues& c) const; double error(const Simulated2DValues& c) const;
}; };
class Simulated2DOrientedPosePrior { class Simulated2DOrientedPosePrior {
Simulated2DOrientedPosePrior(Pose2& mu, const SharedDiagonal& model, int i); Simulated2DOrientedPosePrior(Pose2& mu, const SharedDiagonal& model, int i);
void print(string s) const; void print(string s) const;
GaussianFactor* linearize(const Simulated2DOrientedValues& config) const;
double error(const Simulated2DOrientedValues& c) const; double error(const Simulated2DOrientedValues& c) const;
}; };
class Simulated2DPointPrior { class Simulated2DPointPrior {
Simulated2DPointPrior(Point2& mu, const SharedDiagonal& model, int i); Simulated2DPointPrior(Point2& mu, const SharedDiagonal& model, int i);
void print(string s) const; void print(string s) const;
GaussianFactor* linearize(const Simulated2DValues& config) const;
double error(const Simulated2DValues& c) const; double error(const Simulated2DValues& c) const;
}; };
class Simulated2DOdometry { class Simulated2DOdometry {
Simulated2DOdometry(Point2& mu, const SharedDiagonal& model, int i1, int i2); Simulated2DOdometry(Point2& mu, const SharedDiagonal& model, int i1, int i2);
void print(string s) const; void print(string s) const;
GaussianFactor* linearize(const Simulated2DValues& config) const;
double error(const Simulated2DValues& c) const; double error(const Simulated2DValues& c) const;
}; };
class Simulated2DOrientedOdometry { class Simulated2DOrientedOdometry {
Simulated2DOrientedOdometry(Pose2& mu, const SharedDiagonal& model, int i1, int i2); Simulated2DOrientedOdometry(Pose2& mu, const SharedDiagonal& model, int i1, int i2);
void print(string s) const; void print(string s) const;
GaussianFactor* linearize(const Simulated2DOrientedValues& config) const;
double error(const Simulated2DOrientedValues& c) const; double error(const Simulated2DOrientedValues& c) const;
}; };
class Simulated2DMeasurement { class Simulated2DMeasurement {
Simulated2DMeasurement(Point2& mu, const SharedDiagonal& model, int i, int j); Simulated2DMeasurement(Point2& mu, const SharedDiagonal& model, int i, int j);
void print(string s) const; void print(string s) const;
GaussianFactor* linearize(const Simulated2DValues& config) const;
double error(const Simulated2DValues& c) const; double error(const Simulated2DValues& c) const;
}; };
class Pose2SLAMOptimizer {
Pose2SLAMOptimizer(string dataset_name);
void print(string s) const;
void update(Vector x) const;
Vector optimize() const;
double error() const;
Matrix a1() const;
Matrix a2() const;
Vector b1() const;
Vector b2() const;
};

2
wrap/Makefile.am
View File

@ -9,7 +9,7 @@ interfacePath = $(top_srcdir)
moduleName = gtsam moduleName = gtsam
toolboxpath = ../toolbox toolboxpath = ../toolbox
nameSpace = "gtsam" nameSpace = "gtsam"
mexFlags = "${BOOST_CPPFLAGS} -I${prefix}/include -I${prefix}/include/gtsam -L${exec_prefix}/lib -lgtsam" mexFlags = "${BOOST_CPPFLAGS} -I${prefix}/include -I${prefix}/include/gtsam/linear -I${prefix}/include/gtsam/nonlinear -I${prefix}/include/gtsam/base -I${prefix}/include/gtsam/geometry -I${prefix}/include/gtsam/slam -L${exec_prefix}/lib -lgtsam"
all: all:
./wrap ${interfacePath} ${moduleName} ${toolboxpath} ${nameSpace} ${mexFlags} ./wrap ${interfacePath} ${moduleName} ${toolboxpath} ${nameSpace} ${mexFlags}