diff --git a/.cproject b/.cproject index 5ee6f0fdb..9cd83790e 100644 --- a/.cproject +++ b/.cproject @@ -689,18 +689,26 @@ true true - + make -j5 - nonlinear.testValues.run + testValues.run true true true - + make -j5 - nonlinear.testOrdering.run + testOrdering.run + true + true + true + + + make + -j5 + testKey.run true true true 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..521c768ec 100644 --- a/examples/matlab/LocalizationExample.m +++ b/examples/matlab/LocalizationExample.m @@ -43,8 +43,26 @@ 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); +x{1}=gtsamSymbol('x',1); P{1}=marginals.marginalCovariance(x{1}.key) +x{2}=gtsamSymbol('x',2); P{2}=marginals.marginalCovariance(x{2}.key) +x{3}=gtsamSymbol('x',3); P{3}=marginals.marginalCovariance(x{3}.key) + +%% Plot Trajectory +figure(1) +clf +X=[];Y=[]; +for i=1:3 + pose_i = result.pose(i); + X=[X;pose_i.x]; + Y=[Y;pose_i.y]; +end +plot(X,Y,'b*-'); + +%% Plot Covariance Ellipses +hold on +for i=1:3 + pose_i = result.pose(i); + covarianceEllipse([pose_i.x;pose_i.y],P{i},'g') +end diff --git a/examples/matlab/covarianceEllipse.m b/examples/matlab/covarianceEllipse.m new file mode 100644 index 000000000..106a10d6e --- /dev/null +++ b/examples/matlab/covarianceEllipse.m @@ -0,0 +1,34 @@ +function covarianceEllipse(x,P,color) +% covarianceEllipse: plot a Gaussian as an uncertainty ellipse +% Based on Maybeck Vol 1, page 366 +% k=2.296 corresponds to 1 std, 68.26% of all probability +% k=11.82 corresponds to 3 std, 99.74% of all probability +% +% covarianceEllipse(x,P,color) +% it is assumed x and y are the first two components of state x + +[e,s] = eig(P(1:2,1:2)); +s1 = s(1,1); +s2 = s(2,2); +k = 2.296; +[ex,ey] = ellipse( sqrt(s1*k)*e(:,1), sqrt(s2*k)*e(:,2), x(1:2) ); +line(ex,ey,'color',color); + +function [x,y] = ellipse(a,b,c); +% ellipse: return the x and y coordinates for an ellipse +% [x,y] = ellipse(a,b,c); +% a, and b are the axes. c is the center + +global ellipse_x ellipse_y +if ~exist('elipse_x') + q =0:2*pi/25:2*pi; + ellipse_x = cos(q); + ellipse_y = sin(q); +end + +points = a*ellipse_x + b*ellipse_y; +x = c(1) + points(1,:); +y = c(2) + points(2,:); +end + +end \ No newline at end of file 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/gtsam/inference/BayesNet-inl.h b/gtsam/inference/BayesNet-inl.h index 7ff8fc076..53e0ec18d 100644 --- a/gtsam/inference/BayesNet-inl.h +++ b/gtsam/inference/BayesNet-inl.h @@ -37,7 +37,7 @@ namespace gtsam { /* ************************************************************************* */ template void BayesNet::print(const string& s) const { - cout << s << ":\n"; + cout << s; BOOST_REVERSE_FOREACH(sharedConditional conditional,conditionals_) conditional->print(); } diff --git a/gtsam/nonlinear/Marginals.cpp b/gtsam/nonlinear/Marginals.cpp index 6df7dfdac..61f7f8dad 100644 --- a/gtsam/nonlinear/Marginals.cpp +++ b/gtsam/nonlinear/Marginals.cpp @@ -43,6 +43,14 @@ Marginals::Marginals(const NonlinearFactorGraph& graph, const Values& solution, bayesTree_ = *GaussianMultifrontalSolver(graph_, true).eliminate(); } +/* ************************************************************************* */ +void Marginals::print(const std::string& str, const KeyFormatter& keyFormatter) const { + ordering_.print(str+"Ordering: ", keyFormatter); + graph_.print(str+"Graph: "); + values_.print(str+"Solution: ", keyFormatter); + bayesTree_.print(str+"Bayes Tree: "); +} + /* ************************************************************************* */ Matrix Marginals::marginalCovariance(Key variable) const { return marginalInformation(variable).inverse(); diff --git a/gtsam/nonlinear/Marginals.h b/gtsam/nonlinear/Marginals.h index fe5d4b21d..f04fe310a 100644 --- a/gtsam/nonlinear/Marginals.h +++ b/gtsam/nonlinear/Marginals.h @@ -40,6 +40,16 @@ public: QR }; +protected: + + GaussianFactorGraph graph_; + Ordering ordering_; + Values values_; + Factorization factorization_; + GaussianBayesTree bayesTree_; + +public: + /** Construct a marginals class. * @param graph The factor graph defining the full joint density on all variables. * @param solution The linearization point about which to compute Gaussian marginals (usually the MLE as obtained from a NonlinearOptimizer). @@ -47,6 +57,9 @@ public: */ Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization = CHOLESKY); + /** print */ + void print(const std::string& str = "Marginals: ", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + /** Compute the marginal covariance of a single variable */ Matrix marginalCovariance(Key variable) const; @@ -60,14 +73,6 @@ public: /** Compute the joint marginal information of several variables */ JointMarginal jointMarginalInformation(const std::vector& variables) const; - -protected: - - GaussianFactorGraph graph_; - Values values_; - Ordering ordering_; - Factorization factorization_; - GaussianBayesTree bayesTree_; }; /** @@ -76,7 +81,17 @@ protected: class JointMarginal { protected: - typedef SymmetricBlockView BlockView; + + typedef SymmetricBlockView BlockView; + + Matrix fullMatrix_; + BlockView blockView_; + Ordering indices_; + + JointMarginal(const Matrix& fullMatrix, const std::vector& dims, const Ordering& indices) : + fullMatrix_(fullMatrix), blockView_(fullMatrix_, dims.begin(), dims.end()), indices_(indices) {} + + friend class Marginals; public: /** A block view of the joint marginal - this stores a reference to the @@ -107,16 +122,6 @@ public: /** Assignment operator */ JointMarginal& operator=(const JointMarginal& rhs); - -protected: - Matrix fullMatrix_; - BlockView blockView_; - Ordering indices_; - - JointMarginal(const Matrix& fullMatrix, const std::vector& dims, const Ordering& indices) : - fullMatrix_(fullMatrix), blockView_(fullMatrix_, dims.begin(), dims.end()), indices_(indices) {} - - friend class Marginals; }; } /* namespace gtsam */ diff --git a/gtsam/nonlinear/Symbol.h b/gtsam/nonlinear/Symbol.h index f671a4b30..03fd4aa1f 100644 --- a/gtsam/nonlinear/Symbol.h +++ b/gtsam/nonlinear/Symbol.h @@ -18,8 +18,8 @@ #pragma once -#include -#include +#include + #include #include #include @@ -28,7 +28,8 @@ #include #include -#include +#include +#include namespace gtsam { @@ -70,8 +71,8 @@ public: j_ = key & indexMask; } - /** Cast to integer */ - operator Key() const { + /** return Key (integer) representation */ + Key key() const { const size_t keyBits = sizeof(Key) * 8; const size_t chrBits = sizeof(unsigned char) * 8; const size_t indexBits = keyBits - chrBits; @@ -83,9 +84,12 @@ public: return key; } + /** Cast to integer */ + operator Key() const { return key(); } + // Testable Requirements void print(const std::string& s = "") const { - std::cout << s << ": " << (std::string) (*this) << std::endl; + std::cout << s << (std::string) (*this) << std::endl; } bool equals(const Symbol& expected, double tol = 0.0) const { return (*this) == expected; diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index 4af8e5a3e..4a03ab05e 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -24,8 +24,10 @@ #pragma once -#include -#include +#include +#include +#include +#include #include #include @@ -37,10 +39,8 @@ #include #include -#include -#include -#include -#include +#include +#include namespace gtsam { diff --git a/gtsam/nonlinear/tests/testKey.cpp b/gtsam/nonlinear/tests/testKey.cpp index 54a2e504a..dedb45332 100644 --- a/gtsam/nonlinear/tests/testKey.cpp +++ b/gtsam/nonlinear/tests/testKey.cpp @@ -27,11 +27,11 @@ using namespace gtsam; /* ************************************************************************* */ TEST(Key, KeySymbolConversion) { - Symbol expected('j', 4); - Key key(expected); + Symbol original('j', 4); + Key key(original); + EXPECT(assert_equal(key, original.key())) Symbol actual(key); - - EXPECT(assert_equal(expected, actual)) + EXPECT(assert_equal(original, actual)) } /* ************************************************************************* */ diff --git a/gtsam/slam/tests/testPose2SLAM.cpp b/gtsam/slam/tests/testPose2SLAM.cpp index d97f99c72..978d001a5 100644 --- a/gtsam/slam/tests/testPose2SLAM.cpp +++ b/gtsam/slam/tests/testPose2SLAM.cpp @@ -36,12 +36,12 @@ using pose2SLAM::PoseKey; // common measurement covariance static double sx=0.5, sy=0.5,st=0.1; -static noiseModel::Gaussian::shared_ptr covariance( - noiseModel::Gaussian::Covariance(Matrix_(3, 3, +static Matrix cov(Matrix_(3, 3, sx*sx, 0.0, 0.0, 0.0, sy*sy, 0.0, 0.0, 0.0, st*st - ))); + )); +static noiseModel::Gaussian::shared_ptr covariance(noiseModel::Gaussian::Covariance(cov)); //static noiseModel::Gaussian::shared_ptr I3(noiseModel::Unit::Create(3)); const Key kx0 = Symbol('x',0), kx1 = Symbol('x',1), kx2 = Symbol('x',2), kx3 = Symbol('x',3), kx4 = Symbol('x',4), kx5 = Symbol('x',5), kl1 = Symbol('l',1); @@ -175,6 +175,12 @@ TEST(Pose2SLAM, optimize) { // Check marginals Marginals marginals = fg.marginals(actual); + // Matrix expectedP0 = Infinity, as we have a pose constraint !? + // Matrix actualP0 = marginals.marginalCovariance(pose2SLAM::PoseKey(0)); + // EQUALITY(expectedP0, actualP0); + Matrix expectedP1 = cov; // the second pose really should have just the noise covariance + Matrix actualP1 = marginals.marginalCovariance(pose2SLAM::PoseKey(1)); + EQUALITY(expectedP1, actualP1); } /* ************************************************************************* */ diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 43420e1c9..4d29dd557 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -12,7 +12,6 @@ set(tests_local_libs # exclude certain files # note the source dir on each set (tests_exclude - "${CMAKE_CURRENT_SOURCE_DIR}/testPose2SLAMwSPCG.cpp" #"${CMAKE_CURRENT_SOURCE_DIR}/testOccupancyGrid.cpp" ) diff --git a/tests/testOccupancyGrid.cpp b/tests/testOccupancyGrid.cpp index 35f2df3d8..504d958f5 100644 --- a/tests/testOccupancyGrid.cpp +++ b/tests/testOccupancyGrid.cpp @@ -286,84 +286,38 @@ TEST_UNSAFE( OccupancyGrid, Test1) { //Build a small grid and test optimization //Build small grid - double width = 20; //meters - double height = 20; //meters - double resolution = 0.2; //meters + double width = 3; //meters + double height = 2; //meters + double resolution = 0.5; //meters OccupancyGrid occupancyGrid(width, height, resolution); //default center to middle //Add measurements -// Pose2 pose(0,0,0); -// double range = 4.499765; -// -// occupancyGrid.addPrior(0, 0.7); -// EXPECT_LONGS_EQUAL(1, occupancyGrid.size()); -// -// occupancyGrid.addLaser(pose, range); -// EXPECT_LONGS_EQUAL(2, occupancyGrid.size()); + Pose2 pose(0,0,0); + double range = 1; - //add lasers - int n_frames = 1; - int n_lasers_per_frame = 640; - char laser_list_file[1000]; + occupancyGrid.addPrior(0, 0.7); + EXPECT_LONGS_EQUAL(1, occupancyGrid.size()); + + occupancyGrid.addLaser(pose, range); + EXPECT_LONGS_EQUAL(2, occupancyGrid.size()); + + OccupancyGrid::Occupancy occupancy = occupancyGrid.emptyOccupancy(); + EXPECT_LONGS_EQUAL(900, occupancyGrid.laserFactorValue(0,occupancy)); - for(int i = 0; i < n_frames; i++){ - sprintf(laser_list_file, "/home/brian/Desktop/research/user/bpeasle/code/KinectInterface/Data/ScanLinesAsLasers/KinectRecording9/laser_list%.4d", i); - FILE *fptr = fopen(laser_list_file,"r"); - double x,y, theta; - double range, angle; - fscanf(fptr, "%lf %lf %lf", &x, &y, &theta); + occupancy[16] = 1; + EXPECT_LONGS_EQUAL(1, occupancyGrid.laserFactorValue(0,occupancy)); - for(int j = 0; j < n_lasers_per_frame; j++){ - fscanf(fptr, "%lf %lf", &range, &angle); - //if(j == 159){ - Pose2 pose(x,y, theta+angle); + occupancy[15] = 1; + EXPECT_LONGS_EQUAL(1000, occupancyGrid.laserFactorValue(0,occupancy)); - occupancyGrid.addLaser(pose, range); - //} - } - fclose(fptr); - - } - - -// OccupancyGrid::Occupancy occupancy = occupancyGrid.emptyOccupancy(); -// EXPECT_LONGS_EQUAL(900, occupancyGrid.laserFactorValue(0,occupancy)); -// -// -// occupancy[16] = 1; -// EXPECT_LONGS_EQUAL(1, occupancyGrid.laserFactorValue(0,occupancy)); -// -// occupancy[15] = 1; -// EXPECT_LONGS_EQUAL(1000, occupancyGrid.laserFactorValue(0,occupancy)); -// -// occupancy[16] = 0; -// EXPECT_LONGS_EQUAL(1000, occupancyGrid.laserFactorValue(0,occupancy)); + occupancy[16] = 0; + EXPECT_LONGS_EQUAL(1000, occupancyGrid.laserFactorValue(0,occupancy)); //run MCMC OccupancyGrid::Marginals occupancyMarginals = occupancyGrid.runMetropolis(50000); - //EXPECT_LONGS_EQUAL( (width*height)/pow(resolution,2), occupancyMarginals.size()); - //select a cell at a random to flip - - - printf("\n"); - for(size_t i = 0, it = 0; i < occupancyGrid.height(); i++){ - for(size_t j = 0; j < occupancyGrid.width(); j++, it++){ - printf("%.2lf ", occupancyMarginals[it]); - } - printf("\n"); - } - - char marginalsOutput[1000]; - sprintf(marginalsOutput, "/home/brian/Desktop/research/user/bpeasle/code/KinectInterface/marginals.txt"); - FILE *fptr = fopen(marginalsOutput, "w"); - fprintf(fptr, "%d %d\n", occupancyGrid.width(), occupancyGrid.height()); - - for(int i = 0; i < occupancyMarginals.size(); i++){ - fprintf(fptr, "%lf ", occupancyMarginals[i]); - } - fclose(fptr); + EXPECT_LONGS_EQUAL( (width*height)/pow(resolution,2), occupancyMarginals.size()); diff --git a/tests/testPose2SLAMwSPCG.cpp b/tests/testPose2SLAMwSPCG.cpp deleted file mode 100644 index 439228c79..000000000 --- a/tests/testPose2SLAMwSPCG.cpp +++ /dev/null @@ -1,72 +0,0 @@ -/** - * @file testPose2SLAMwSPCG - * @author Alex Cunningham - */ - -#include - -#include -#include - -using namespace std; -using namespace gtsam; -using namespace pose2SLAM; - -const double tol = 1e-5; - -/* ************************************************************************* */ -TEST(testPose2SLAMwSPCG, example1) { - - /* generate synthetic data */ - const SharedNoiseModel sigma(noiseModel::Unit::Create(0.1)); - Key x1(1), x2(2), x3(3), x4(4), x5(5), x6(6), x7(7), x8(8), x9(9); - - // create a 3 by 3 grid - // x3 x6 x9 - // x2 x5 x8 - // x1 x4 x7 - Graph graph; - graph.addConstraint(x1,x2,Pose2(0,2,0),sigma) ; - graph.addConstraint(x2,x3,Pose2(0,2,0),sigma) ; - graph.addConstraint(x4,x5,Pose2(0,2,0),sigma) ; - graph.addConstraint(x5,x6,Pose2(0,2,0),sigma) ; - graph.addConstraint(x7,x8,Pose2(0,2,0),sigma) ; - graph.addConstraint(x8,x9,Pose2(0,2,0),sigma) ; - graph.addConstraint(x1,x4,Pose2(2,0,0),sigma) ; - graph.addConstraint(x4,x7,Pose2(2,0,0),sigma) ; - graph.addConstraint(x2,x5,Pose2(2,0,0),sigma) ; - graph.addConstraint(x5,x8,Pose2(2,0,0),sigma) ; - graph.addConstraint(x3,x6,Pose2(2,0,0),sigma) ; - graph.addConstraint(x6,x9,Pose2(2,0,0),sigma) ; - graph.addPrior(x1, Pose2(0,0,0), sigma) ; - - Values initial; - initial.insert(x1, Pose2( 0, 0, 0)); - initial.insert(x2, Pose2( 0, 2.1, 0.01)); - initial.insert(x3, Pose2( 0, 3.9,-0.01)); - initial.insert(x4, Pose2(2.1,-0.1, 0)); - initial.insert(x5, Pose2(1.9, 2.1, 0.02)); - initial.insert(x6, Pose2(2.0, 3.9,-0.02)); - initial.insert(x7, Pose2(4.0, 0.1, 0.03 )); - initial.insert(x8, Pose2(3.9, 2.1, 0.01)); - initial.insert(x9, Pose2(4.1, 3.9,-0.01)); - - Values expected; - expected.insert(x1, Pose2(0.0, 0.0, 0.0)); - expected.insert(x2, Pose2(0.0, 2.0, 0.0)); - expected.insert(x3, Pose2(0.0, 4.0, 0.0)); - expected.insert(x4, Pose2(2.0, 0.0, 0.0)); - expected.insert(x5, Pose2(2.0, 2.0, 0.0)); - expected.insert(x6, Pose2(2.0, 4.0, 0.0)); - expected.insert(x7, Pose2(4.0, 0.0, 0.0 )); - expected.insert(x8, Pose2(4.0, 2.0, 0.0)); - expected.insert(x9, Pose2(4.0, 4.0, 0.0)); - - Values actual = optimizeSPCG(graph, initial); - - EXPECT(assert_equal(expected, actual, tol)); -} - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */ 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