98 lines
3.4 KiB
C++
98 lines
3.4 KiB
C++
/**
|
|
* Matlab toolbox interface definition for gtsam_unstable
|
|
*/
|
|
|
|
// specify the classes from gtsam we are using
|
|
class gtsam::Point3;
|
|
class gtsam::Rot3;
|
|
class gtsam::Pose3;
|
|
class gtsam::SharedNoiseModel;
|
|
|
|
namespace gtsam {
|
|
|
|
#include <gtsam_unstable/dynamics/PoseRTV.h>
|
|
class PoseRTV {
|
|
PoseRTV();
|
|
PoseRTV(Vector rtv);
|
|
PoseRTV(const gtsam::Point3& pt, const gtsam::Rot3& rot, const gtsam::Point3& vel);
|
|
PoseRTV(const gtsam::Rot3& rot, const gtsam::Point3& pt, const gtsam::Point3& vel);
|
|
PoseRTV(const gtsam::Pose3& pose, const gtsam::Point3& vel);
|
|
PoseRTV(const gtsam::Pose3& pose);
|
|
PoseRTV(double roll, double pitch, double yaw, double x, double y, double z, double vx, double vy, double vz);
|
|
|
|
// testable
|
|
bool equals(const gtsam::PoseRTV& other, double tol) const;
|
|
void print(string s) const;
|
|
|
|
// access
|
|
gtsam::Point3 translation() const;
|
|
gtsam::Rot3 rotation() const;
|
|
gtsam::Point3 velocity() const;
|
|
gtsam::Pose3 pose() const;
|
|
|
|
// Vector interfaces
|
|
Vector vector() const;
|
|
Vector translationVec() const;
|
|
Vector velocityVec() const;
|
|
|
|
// manifold/Lie
|
|
static size_t Dim();
|
|
size_t dim() const;
|
|
gtsam::PoseRTV retract(Vector v) const;
|
|
Vector localCoordinates(const gtsam::PoseRTV& p) const;
|
|
static gtsam::PoseRTV Expmap(Vector v);
|
|
static Vector Logmap(const gtsam::PoseRTV& p);
|
|
gtsam::PoseRTV inverse() const;
|
|
gtsam::PoseRTV compose(const gtsam::PoseRTV& p) const;
|
|
gtsam::PoseRTV between(const gtsam::PoseRTV& p) const;
|
|
|
|
// measurement
|
|
double range(const gtsam::PoseRTV& other) const;
|
|
gtsam::PoseRTV transformed_from(const gtsam::Pose3& trans) const;
|
|
|
|
// IMU/dynamics
|
|
gtsam::PoseRTV planarDynamics(double vel_rate, double heading_rate, double max_accel, double dt) const;
|
|
gtsam::PoseRTV flyingDynamics(double pitch_rate, double heading_rate, double lift_control, double dt) const;
|
|
gtsam::PoseRTV generalDynamics(Vector accel, Vector gyro, double dt) const;
|
|
Vector imuPrediction(const gtsam::PoseRTV& x2, double dt) const;
|
|
gtsam::Point3 translationIntegration(const gtsam::PoseRTV& x2, double dt) const;
|
|
Vector translationIntegrationVec(const gtsam::PoseRTV& x2, double dt) const;
|
|
};
|
|
|
|
}///\namespace gtsam
|
|
|
|
#include <gtsam_unstable/dynamics/imuSystem.h>
|
|
namespace imu {
|
|
|
|
class Values {
|
|
Values();
|
|
void print(string s) const;
|
|
|
|
void insertPose(size_t key, const gtsam::PoseRTV& pose);
|
|
gtsam::PoseRTV pose(size_t key) const;
|
|
};
|
|
|
|
class Graph {
|
|
Graph();
|
|
void print(string s) const;
|
|
|
|
// prior factors
|
|
void addPrior(size_t key, const gtsam::PoseRTV& pose, const gtsam::SharedNoiseModel& noiseModel);
|
|
void addConstraint(size_t key, const gtsam::PoseRTV& pose);
|
|
void addHeightPrior(size_t key, double z, const gtsam::SharedNoiseModel& noiseModel);
|
|
|
|
// inertial factors
|
|
void addFullIMUMeasurement(size_t key1, size_t key2, const Vector& accel, const Vector& gyro, double dt, const gtsam::SharedNoiseModel& noiseModel);
|
|
void addIMUMeasurement(size_t key1, size_t key2, const Vector& accel, const Vector& gyro, double dt, const gtsam::SharedNoiseModel& noiseModel);
|
|
void addVelocityConstraint(size_t key1, size_t key2, double dt, const gtsam::SharedNoiseModel& noiseModel);
|
|
|
|
// other measurements
|
|
void addBetween(size_t key1, size_t key2, const gtsam::PoseRTV& z, const gtsam::SharedNoiseModel& noiseModel);
|
|
void addRange(size_t key1, size_t key2, double z, const gtsam::SharedNoiseModel& noiseModel);
|
|
|
|
// optimization
|
|
imu::Values optimize(const imu::Values& init) const;
|
|
};
|
|
|
|
}///\namespace imu
|