150 lines
3.6 KiB
C++
150 lines
3.6 KiB
C++
// These are currently broken
|
|
// 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
|
|
|
|
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{
|
|
Pose2Values();
|
|
Pose2 get(string key) const;
|
|
void insert(string name, const Pose2& val);
|
|
void print(string s) const;
|
|
void clear();
|
|
int size();
|
|
};
|
|
|
|
class Pose2Factor {
|
|
Pose2Factor(string key1, string key2,
|
|
const Pose2& measured, Matrix measurement_covariance);
|
|
void print(string name) const;
|
|
double error(const Pose2Values& c) const;
|
|
size_t size() const;
|
|
GaussianFactor* linearize(const Pose2Values& config) const;
|
|
};
|
|
|
|
class Pose2Graph{
|
|
Pose2Graph();
|
|
void print(string s) const;
|
|
GaussianFactorGraph* linearize_(const Pose2Values& config) const;
|
|
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;
|
|
};
|
|
|
|
|