diff --git a/examples/LocalizationExample.cpp b/examples/LocalizationExample.cpp index 6963f75f1..2d2357dad 100644 --- a/examples/LocalizationExample.cpp +++ b/examples/LocalizationExample.cpp @@ -64,7 +64,7 @@ int main(int argc, char** argv) { result.print("\nFinal result:\n "); // Query the marginals - Marginals marginals(graph, result); + Marginals marginals = graph.marginals(result); cout.precision(2); cout << "\nP1:\n" << marginals.marginalCovariance(pose2SLAM::PoseKey(1)) << endl; cout << "\nP2:\n" << marginals.marginalCovariance(pose2SLAM::PoseKey(2)) << endl; diff --git a/examples/matlab/LocalizationExample.m b/examples/matlab/LocalizationExample.m index 361a1c8b1..4aa122dd8 100644 --- a/examples/matlab/LocalizationExample.m +++ b/examples/matlab/LocalizationExample.m @@ -43,8 +43,8 @@ initialEstimate.print(sprintf('\nInitial estimate:\n ')); result = graph.optimize(initialEstimate); result.print(sprintf('\nFinal result:\n ')); -%% Use an explicit Optimizer object so we can query the marginals -% marginals = gtsamMarginals(graph, result); -% marginals.marginalCovariance(pose2SLAMPoseKey(1)) -% marginals.marginalCovariance(pose2SLAMPoseKey(2)) -% marginals.marginalCovariance(pose2SLAMPoseKey(3)) +%% Query the marginals +marginals = graph.marginals(result); +x1=gtsamSymbol('x',1); marginals.marginalCovariance(x1.key) +x2=gtsamSymbol('x',2); marginals.marginalCovariance(x2.key) +x3=gtsamSymbol('x',3); marginals.marginalCovariance(x3.key) diff --git a/gtsam.h b/gtsam.h index 628c91733..665b9bb52 100644 --- a/gtsam.h +++ b/gtsam.h @@ -347,13 +347,14 @@ class GaussianFactorGraph { // Building the graph void add(gtsam::JacobianFactor* factor); - void add(Vector b); - void add(int key1, Matrix A1, Vector b, const gtsam::SharedDiagonal& model); - void add(int key1, Matrix A1, int key2, Matrix A2, Vector b, - const gtsam::SharedDiagonal& model); - void add(int key1, Matrix A1, int key2, Matrix A2, int key3, Matrix A3, - Vector b, const gtsam::SharedDiagonal& model); - void add(gtsam::HessianFactor* factor); + // all these won't work as MATLAB can't handle overloading +// void add(Vector b); +// void add(int key1, Matrix A1, Vector b, const gtsam::SharedDiagonal& model); +// void add(int key1, Matrix A1, int key2, Matrix A2, Vector b, +// const gtsam::SharedDiagonal& model); +// void add(int key1, Matrix A1, int key2, Matrix A2, int key3, Matrix A3, +// Vector b, const gtsam::SharedDiagonal& model); +// void add(gtsam::HessianFactor* factor); // error and probability double error(const gtsam::VectorValues& c) const; @@ -380,7 +381,7 @@ class GaussianSequentialSolver { class KalmanFilter { KalmanFilter(size_t n); - gtsam::GaussianDensity* init(Vector x0, const gtsam::SharedDiagonal& P0); + // gtsam::GaussianDensity* init(Vector x0, const gtsam::SharedDiagonal& P0); gtsam::GaussianDensity* init(Vector x0, Matrix P0); void print(string s) const; static int step(gtsam::GaussianDensity* p); @@ -400,6 +401,12 @@ class KalmanFilter { // nonlinear //************************************************************************* +class Symbol { + Symbol(char c, size_t j); + void print(string s) const; + size_t key() const; +}; + class Ordering { Ordering(); void print(string s) const; @@ -407,20 +414,21 @@ class Ordering { void push_back(size_t key); }; -//Andrew says: Required definitions for Marginal arguments class NonlinearFactorGraph { NonlinearFactorGraph(); - //This need to be populated with whatever functions might be needed. }; class Values { Values(); - //Same here + void print(string s) const; + bool exists(size_t j) const; }; -// Frank says: this does not work. Why not? class Marginals { Marginals(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& solution); + void print(string s) const; + Matrix marginalCovariance(size_t variable) const; + Matrix marginalInformation(size_t variable) const; }; }///\namespace gtsam @@ -471,7 +479,7 @@ class Odometry { }///\namespace planarSLAM //************************************************************************* -// gtsam::Pose2SLAM +// Pose2SLAM //************************************************************************* #include @@ -481,6 +489,7 @@ class Values { Values(); void print(string s) const; void insertPose(int key, const gtsam::Pose2& pose); + gtsam::Symbol poseKey(int i); gtsam::Pose2 pose(int i); }; @@ -497,7 +506,8 @@ class Graph { void addPrior(int key, const gtsam::Pose2& pose, const gtsam::SharedNoiseModel& noiseModel); void addPoseConstraint(int key, const gtsam::Pose2& pose); void addOdometry(int key1, int key2, const gtsam::Pose2& odometry, const gtsam::SharedNoiseModel& noiseModel); - pose2SLAM::Values optimize(const pose2SLAM::Values& initialEstimate); + pose2SLAM::Values optimize(const pose2SLAM::Values& initialEstimate) const; + gtsam::Marginals marginals(const pose2SLAM::Values& solution) const; }; }///\namespace pose2SLAM diff --git a/wrap/matlab.h b/wrap/matlab.h index 2ef81f37f..b6d200e43 100644 --- a/wrap/matlab.h +++ b/wrap/matlab.h @@ -206,39 +206,53 @@ string unwrap(const mxArray* array) { return str; } +// Check for 64-bit, as Mathworks says mxGetScalar only good for 32 bit +template +T myGetScalar(const mxArray* array) { + switch (mxGetClassID(array)) { + case mxINT64_CLASS: + return (T) *(int64_t*) mxGetData(array); + case mxUINT64_CLASS: + return (T) *(uint64_t*) mxGetData(array); + default: + // hope for the best! + return (T) mxGetScalar(array); + } +} + // specialization to bool template<> bool unwrap(const mxArray* array) { checkScalar(array,"unwrap"); - return mxGetScalar(array) != 0.0; + return myGetScalar(array); } // specialization to bool template<> char unwrap(const mxArray* array) { checkScalar(array,"unwrap"); - return (char)mxGetScalar(array); -} - -// specialization to size_t -template<> -size_t unwrap(const mxArray* array) { - checkScalar(array,"unwrap"); - return (size_t)mxGetScalar(array); + return myGetScalar(array); } // specialization to int template<> int unwrap(const mxArray* array) { checkScalar(array,"unwrap"); - return (int)mxGetScalar(array); + return myGetScalar(array); +} + +// specialization to size_t +template<> +size_t unwrap(const mxArray* array) { + checkScalar(array, "unwrap"); + return myGetScalar(array); } // specialization to double template<> double unwrap(const mxArray* array) { checkScalar(array,"unwrap"); - return (double)mxGetScalar(array); + return myGetScalar(array); } // specialization to Eigen vector