From fc437cc6416810ccd838442621440bfa5bbdd0e5 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Mon, 4 Jun 2012 18:23:18 +0000 Subject: [PATCH] Moved matlab wrapped PoseRTV into the gtsam namespace --- gtsam_unstable/gtsam_unstable.h | 49 +++++++++++++++++---------------- 1 file changed, 25 insertions(+), 24 deletions(-) diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h index 376706e7f..91377da8c 100644 --- a/gtsam_unstable/gtsam_unstable.h +++ b/gtsam_unstable/gtsam_unstable.h @@ -2,15 +2,14 @@ * 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; +namespace gtsam { + #include class PoseRTV { PoseRTV(); @@ -22,7 +21,7 @@ class PoseRTV { 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; + bool equals(const gtsam::PoseRTV& other, double tol) const; void print(string s) const; // access @@ -39,27 +38,29 @@ class PoseRTV { // 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; + 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 PoseRTV& other) const; - PoseRTV transformed_from(const gtsam::Pose3& trans) const; + double range(const gtsam::PoseRTV& other) const; + gtsam::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; + 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 namespace imu { @@ -67,8 +68,8 @@ class Values { Values(); void print(string s) const; - void insertPose(size_t key, const PoseRTV& pose); - PoseRTV pose(size_t key) const; + void insertPose(size_t key, const gtsam::PoseRTV& pose); + gtsam::PoseRTV pose(size_t key) const; }; class Graph { @@ -76,8 +77,8 @@ class 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 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 @@ -86,7 +87,7 @@ class Graph { 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 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