12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Moved matlab wrapped PoseRTV into the gtsam namespace

release/4.3a0
Alex Cunningham 2012-06-04 18:23:18 +00:00
parent 74322b0764
commit fc437cc641
1 changed files with 25 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

49
gtsam_unstable/gtsam_unstable.h
View File

@ -2,15 +2,14 @@
* Matlab toolbox interface definition for gtsam_unstable * 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 // specify the classes from gtsam we are using
class gtsam::Point3; class gtsam::Point3;
class gtsam::Rot3; class gtsam::Rot3;
class gtsam::Pose3; class gtsam::Pose3;
class gtsam::SharedNoiseModel; class gtsam::SharedNoiseModel;
namespace gtsam {
#include <gtsam_unstable/dynamics/PoseRTV.h> #include <gtsam_unstable/dynamics/PoseRTV.h>
class PoseRTV { class PoseRTV {
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); PoseRTV(double roll, double pitch, double yaw, double x, double y, double z, double vx, double vy, double vz);
// testable // testable
bool equals(const PoseRTV& other, double tol) const; bool equals(const gtsam::PoseRTV& other, double tol) const;
void print(string s) const; void print(string s) const;
// access // access
@ -39,27 +38,29 @@ class PoseRTV {
// manifold/Lie // manifold/Lie
static size_t Dim(); static size_t Dim();
size_t dim() const; size_t dim() const;
PoseRTV retract(Vector v) const; gtsam::PoseRTV retract(Vector v) const;
Vector localCoordinates(const PoseRTV& p) const; Vector localCoordinates(const gtsam::PoseRTV& p) const;
static PoseRTV Expmap(Vector v); static gtsam::PoseRTV Expmap(Vector v);
static Vector Logmap(const PoseRTV& p); static Vector Logmap(const gtsam::PoseRTV& p);
PoseRTV inverse() const; gtsam::PoseRTV inverse() const;
PoseRTV compose(const PoseRTV& p) const; gtsam::PoseRTV compose(const gtsam::PoseRTV& p) const;
PoseRTV between(const PoseRTV& p) const; gtsam::PoseRTV between(const gtsam::PoseRTV& p) const;
// measurement // measurement
double range(const PoseRTV& other) const; double range(const gtsam::PoseRTV& other) const;
PoseRTV transformed_from(const gtsam::Pose3& trans) const; gtsam::PoseRTV transformed_from(const gtsam::Pose3& trans) const;
// IMU/dynamics // IMU/dynamics
PoseRTV planarDynamics(double vel_rate, double heading_rate, double max_accel, double dt) const; gtsam::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; gtsam::PoseRTV flyingDynamics(double pitch_rate, double heading_rate, double lift_control, double dt) const;
PoseRTV generalDynamics(Vector accel, Vector gyro, double dt) const; gtsam::PoseRTV generalDynamics(Vector accel, Vector gyro, double dt) const;
Vector imuPrediction(const PoseRTV& x2, double dt) const; Vector imuPrediction(const gtsam::PoseRTV& x2, double dt) const;
gtsam::Point3 translationIntegration(const PoseRTV& x2, double dt) const; gtsam::Point3 translationIntegration(const gtsam::PoseRTV& x2, double dt) const;
Vector translationIntegrationVec(const PoseRTV& x2, double dt) const; Vector translationIntegrationVec(const gtsam::PoseRTV& x2, double dt) const;
}; };
}///\namespace gtsam
#include <gtsam_unstable/dynamics/imuSystem.h> #include <gtsam_unstable/dynamics/imuSystem.h>
namespace imu { namespace imu {
@ -67,8 +68,8 @@ class Values {
Values(); Values();
void print(string s) const; void print(string s) const;
void insertPose(size_t key, const PoseRTV& pose); void insertPose(size_t key, const gtsam::PoseRTV& pose);
PoseRTV pose(size_t key) const; gtsam::PoseRTV pose(size_t key) const;
}; };
class Graph { class Graph {
@ -76,8 +77,8 @@ class Graph {
void print(string s) const; void print(string s) const;
// prior factors // prior factors
void addPrior(size_t key, const PoseRTV& pose, const gtsam::SharedNoiseModel& noiseModel); void addPrior(size_t key, const gtsam::PoseRTV& pose, const gtsam::SharedNoiseModel& noiseModel);
void addConstraint(size_t key, const PoseRTV& pose); void addConstraint(size_t key, const gtsam::PoseRTV& pose);
void addHeightPrior(size_t key, double z, const gtsam::SharedNoiseModel& noiseModel); void addHeightPrior(size_t key, double z, const gtsam::SharedNoiseModel& noiseModel);
// inertial factors // inertial factors
@ -86,7 +87,7 @@ class Graph {
void addVelocityConstraint(size_t key1, size_t key2, double dt, const gtsam::SharedNoiseModel& noiseModel); void addVelocityConstraint(size_t key1, size_t key2, double dt, const gtsam::SharedNoiseModel& noiseModel);
// other measurements // 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); void addRange(size_t key1, size_t key2, double z, const gtsam::SharedNoiseModel& noiseModel);
// optimization // optimization