97 lines
3.2 KiB
C++
97 lines
3.2 KiB
C++
/**
|
|
* Matlab toolbox interface definition for gtsam_unstable
|
|
*/
|
|
|
|
// Most things are in the gtsam namespace
|
|
using namespace gtsam;
|
|
|
|
// specify the classes from gtsam we are using
|
|
class gtsam::Point3;
|
|
class gtsam::Rot3;
|
|
class gtsam::Pose3;
|
|
class gtsam::SharedNoiseModel;
|
|
|
|
#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 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;
|
|
PoseRTV retract(Vector v) const;
|
|
Vector localCoordinates(const PoseRTV& p) const;
|
|
static PoseRTV Expmap(Vector v);
|
|
static Vector Logmap(const PoseRTV& p);
|
|
PoseRTV inverse() const;
|
|
PoseRTV compose(const PoseRTV& p) const;
|
|
PoseRTV between(const PoseRTV& p) const;
|
|
|
|
// measurement
|
|
double range(const PoseRTV& other) const;
|
|
PoseRTV transformed_from(const gtsam::Pose3& trans) const;
|
|
|
|
// IMU/dynamics
|
|
PoseRTV planarDynamics(double vel_rate, double heading_rate, double max_accel, double dt) const;
|
|
PoseRTV flyingDynamics(double pitch_rate, double heading_rate, double lift_control, double dt) const;
|
|
PoseRTV generalDynamics(Vector accel, Vector gyro, double dt) const;
|
|
Vector imuPrediction(const PoseRTV& x2, double dt) const;
|
|
gtsam::Point3 translationIntegration(const PoseRTV& x2, double dt) const;
|
|
Vector translationIntegrationVec(const PoseRTV& x2, double dt) const;
|
|
};
|
|
|
|
#include <gtsam_unstable/dynamics/imuSystem.h>
|
|
namespace imu {
|
|
|
|
class Values {
|
|
Values();
|
|
void print(string s) const;
|
|
|
|
void insertPose(size_t key, const PoseRTV& pose);
|
|
PoseRTV pose(size_t key) const;
|
|
};
|
|
|
|
class Graph {
|
|
Graph();
|
|
void print(string s) const;
|
|
|
|
// prior factors
|
|
void addPrior(size_t key, const PoseRTV& pose, const gtsam::SharedNoiseModel& noiseModel);
|
|
void addConstraint(size_t key, const 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 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
|