61 lines
1.8 KiB
C
61 lines
1.8 KiB
C
|
// These are currently broken
|
||
|
|
// Solve by parsing a namespace pose2SLAM::Config and making a Pose2SLAMConfig class
|
||
|
|
// We also have to solve the shared pointer mess to avoid duplicate methods
|
||
|
|
|
||
|
|
class Point2Prior {
|
||
|
|
Point2Prior(Vector mu, double sigma, string key);
|
||
|
|
Vector error_vector(const VectorConfig& c) const;
|
||
|
|
GaussianFactor* linearize(const VectorConfig& c) const;
|
||
|
|
double sigma();
|
||
|
|
Vector measurement();
|
||
|
|
double error(const VectorConfig& c) const;
|
||
|
|
void print(string s) const;
|
||
|
|
};
|
||
|
|
|
||
|
|
class Simulated2DOdometry {
|
||
|
|
Simulated2DOdometry(Vector odo, double sigma, string key, string key2);
|
||
|
|
Vector error_vector(const VectorConfig& c) const;
|
||
|
|
GaussianFactor* linearize(const VectorConfig& c) const;
|
||
|
|
double sigma();
|
||
|
|
Vector measurement();
|
||
|
|
double error(const VectorConfig& c) const;
|
||
|
|
void print(string s) const;
|
||
|
|
};
|
||
|
|
|
||
|
|
class Simulated2DMeasurement {
|
||
|
|
Simulated2DMeasurement(Vector odo, double sigma, string key, string key2);
|
||
|
|
Vector error_vector(const VectorConfig& c) const;
|
||
|
|
GaussianFactor* linearize(const VectorConfig& c) const;
|
||
|
|
double sigma();
|
||
|
|
Vector measurement();
|
||
|
|
double error(const VectorConfig& c) const;
|
||
|
|
void print(string s) const;
|
||
|
|
};
|
||
|
|
|
||
|
|
class Pose2Config{
|
||
|
|
Pose2Config();
|
||
|
|
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 Pose2Config& c) const;
|
||
|
|
size_t size() const;
|
||
|
|
GaussianFactor* linearize(const Pose2Config& config) const;
|
||
|
|
};
|
||
|
|
|
||
|
|
class Pose2Graph{
|
||
|
|
Pose2Graph();
|
||
|
|
void print(string s) const;
|
||
|
|
GaussianFactorGraph* linearize_(const Pose2Config& config) const;
|
||
|
|
void push_back(Pose2Factor* factor);
|
||
|
|
};
|
||
|
|
|
||
|
|
|