102 lines
2.3 KiB
C++
102 lines
2.3 KiB
C++
class Point2 {
|
|
Point2();
|
|
Point2(double x, double y);
|
|
void print(string s) const;
|
|
double x();
|
|
double y();
|
|
};
|
|
|
|
class Point3 {
|
|
Point3();
|
|
Point3(double x, double y, double z);
|
|
Point3(Vector v);
|
|
void print(string s) const;
|
|
Vector vector() const;
|
|
double x();
|
|
double y();
|
|
double z();
|
|
};
|
|
|
|
class Rot2 {
|
|
Rot2();
|
|
Rot2(double theta);
|
|
void print(string s) const;
|
|
bool equals(const Rot2& pose, double tol) const;
|
|
double c() const;
|
|
double s() const;
|
|
};
|
|
|
|
class Pose2 {
|
|
Pose2();
|
|
Pose2(const Pose2& pose);
|
|
Pose2(double x, double y, double theta);
|
|
Pose2(double theta, const Point2& t);
|
|
Pose2(const Rot2& r, const Point2& t);
|
|
void print(string s) const;
|
|
bool equals(const Pose2& pose, double tol) const;
|
|
double x() const;
|
|
double y() const;
|
|
double theta() const;
|
|
size_t dim() const;
|
|
};
|
|
|
|
class SharedGaussian {
|
|
SharedGaussian(Matrix covariance);
|
|
SharedGaussian(Vector sigmas);
|
|
};
|
|
|
|
class SharedDiagonal {
|
|
SharedDiagonal(Vector sigmas);
|
|
};
|
|
|
|
class VectorValues {
|
|
VectorValues();
|
|
VectorValues(size_t nVars, size_t varDim);
|
|
void print(string s) const;
|
|
bool equals(const VectorValues& expected, double tol) const;
|
|
size_t size() const;
|
|
void reserve(size_t nVars, size_t totalDims);
|
|
size_t push_back_preallocated(Vector vector);
|
|
};
|
|
|
|
|
|
|
|
|
|
class Landmark2 {
|
|
Landmark2();
|
|
Landmark2(double x, double y);
|
|
void print(string s) const;
|
|
double x();
|
|
double y();
|
|
};
|
|
|
|
class Ordering{
|
|
void print(string s) const;
|
|
bool equals(const Ordering& ord, double tol) const;
|
|
};
|
|
|
|
class PlanarSLAMValues {
|
|
PlanarSLAMValues();
|
|
void print(string s) const;
|
|
void insertPose(int key, const Pose2& pose);
|
|
void insertPoint(int key, const Point2& point);
|
|
};
|
|
|
|
class PlanarSLAMGraph {
|
|
PlanarSLAMGraph();
|
|
double error(const PlanarSLAMValues& c) const;
|
|
Ordering* orderingCOLAMD(const PlanarSLAMValues& config) const;
|
|
void print(string s) const;
|
|
void addPrior(size_t i, const Pose2& p, const SharedNoiseModel& model);
|
|
void addPoseConstraint(size_t i, const Pose2& p);
|
|
void addOdometry(size_t i, size_t j, const Pose2& z,
|
|
const SharedNoiseModel& model);
|
|
void addBearing(size_t i, size_t j, const Rot2& z,
|
|
const SharedNoiseModel& model);
|
|
void addRange(size_t i, size_t j, double z, const SharedNoiseModel& model);
|
|
void addBearingRange(size_t i, size_t j, const Rot2& z1, double z2,
|
|
const SharedNoiseModel& model);
|
|
PlanarSLAMValues* optimize(const PlanarSLAMValues& initialEstimate);
|
|
};
|
|
|