From 9d2a3bf14edb2e07d91559758692b4aa42ee315c Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Fri, 27 Jul 2012 19:02:11 +0000 Subject: [PATCH] Finished denamespacing and reorganizing matlab code --- gtsam.h | 423 +----------------- gtsam/nonlinear/Symbol.h | 9 + gtsam/slam/dataset.cpp | 20 +- gtsam/slam/dataset.h | 16 +- matlab/{ => +gtsam_utils}/CHECK.m | 0 matlab/{ => +gtsam_utils}/EQUALITY.m | 0 .../VisualISAMGenerateData.m | 0 .../VisualISAMInitialize.m | 43 +- .../VisualISAMPlot.m | 34 +- matlab/+gtsam_utils/VisualISAMStep.m | 44 ++ matlab/{ => +gtsam_utils}/circlePose2.m | 2 +- matlab/{ => +gtsam_utils}/circlePose3.m | 2 +- matlab/{ => +gtsam_utils}/covarianceEllipse.m | 66 +-- .../{ => +gtsam_utils}/covarianceEllipse3D.m | 0 .../findExampleDataFile.m | 4 +- matlab/{ => +gtsam_utils}/load2D.m | 0 matlab/{ => +gtsam_utils}/load3D.m | 0 matlab/{ => +gtsam_utils}/plot2DPoints.m | 4 +- matlab/{ => +gtsam_utils}/plot2DTrajectory.m | 4 +- matlab/{ => +gtsam_utils}/plot3DPoints.m | 4 +- matlab/{ => +gtsam_utils}/plot3DTrajectory.m | 4 +- matlab/{ => +gtsam_utils}/plotCamera.m | 0 matlab/{ => +gtsam_utils}/plotPoint2.m | 2 +- matlab/{ => +gtsam_utils}/plotPoint3.m | 2 +- matlab/{ => +gtsam_utils}/plotPose2.m | 2 +- matlab/{ => +gtsam_utils}/plotPose3.m | 2 +- matlab/CMakeLists.txt | 11 +- matlab/examples/+support/VisualISAMStep.m | 44 -- .../LocalizationExample.m | 2 +- .../OdometryExample.m | 2 +- .../PlanarSLAMExample.m | 4 +- .../PlanarSLAMExample_sampling.m | 9 +- .../Pose2SLAMExample.m | 4 +- .../Pose2SLAMExample_advanced.m | 1 + .../Pose2SLAMExample_circle.m | 14 +- .../Pose2SLAMExample_graph.m | 13 +- .../Pose2SLAMwSPCG.m | 0 .../Pose3SLAMExample.m | 14 +- .../Pose3SLAMExample_graph.m | 11 +- .../{examples => gtsam_examples}/SBAExample.m | 2 +- .../{examples => gtsam_examples}/SFMExample.m | 6 +- .../StereoVOExample.m | 7 +- .../StereoVOExample_large.m | 14 +- .../VisualISAMDemo.m | 0 .../VisualISAMExample.m | 10 +- .../VisualISAM_gui.fig | Bin .../VisualISAM_gui.m | 20 +- .../gtsamExamples.fig | Bin .../gtsamExamples.m | 0 .../testJacobianFactor.m | 1 + .../{tests => gtsam_tests}/testKalmanFilter.m | 4 + .../testLocalizationExample.m | 1 + .../testOdometryExample.m | 2 + .../testPlanarSLAMExample.m | 2 + .../testPose2SLAMExample.m | 1 + .../testPose3SLAMExample.m | 7 +- .../{tests => gtsam_tests}/testSFMExample.m | 22 +- .../testStereoVOExample.m | 40 +- .../testVisualISAMExample.m | 11 +- matlab/{tests => gtsam_tests}/test_gtsam.m | 18 +- matlab/symbol.m | 4 - matlab/symbolChr.m | 4 - matlab/symbolIndex.m | 4 - 63 files changed, 324 insertions(+), 672 deletions(-) rename matlab/{ => +gtsam_utils}/CHECK.m (100%) rename matlab/{ => +gtsam_utils}/EQUALITY.m (100%) rename matlab/{examples/+support => +gtsam_utils}/VisualISAMGenerateData.m (100%) rename matlab/{examples/+support => +gtsam_utils}/VisualISAMInitialize.m (54%) rename matlab/{examples/+support => +gtsam_utils}/VisualISAMPlot.m (55%) create mode 100644 matlab/+gtsam_utils/VisualISAMStep.m rename matlab/{ => +gtsam_utils}/circlePose2.m (95%) rename matlab/{ => +gtsam_utils}/circlePose3.m (96%) rename matlab/{ => +gtsam_utils}/covarianceEllipse.m (96%) rename matlab/{ => +gtsam_utils}/covarianceEllipse3D.m (100%) rename matlab/{examples/+support => +gtsam_utils}/findExampleDataFile.m (74%) rename matlab/{ => +gtsam_utils}/load2D.m (100%) rename matlab/{ => +gtsam_utils}/load3D.m (100%) rename matlab/{ => +gtsam_utils}/plot2DPoints.m (88%) rename matlab/{ => +gtsam_utils}/plot2DTrajectory.m (93%) rename matlab/{ => +gtsam_utils}/plot3DPoints.m (88%) rename matlab/{ => +gtsam_utils}/plot3DTrajectory.m (91%) rename matlab/{ => +gtsam_utils}/plotCamera.m (100%) rename matlab/{ => +gtsam_utils}/plotPoint2.m (76%) rename matlab/{ => +gtsam_utils}/plotPoint3.m (80%) rename matlab/{ => +gtsam_utils}/plotPose2.m (84%) rename matlab/{ => +gtsam_utils}/plotPose3.m (94%) delete mode 100644 matlab/examples/+support/VisualISAMStep.m rename matlab/{examples => gtsam_examples}/LocalizationExample.m (96%) rename matlab/{examples => gtsam_examples}/OdometryExample.m (96%) rename matlab/{examples => gtsam_examples}/PlanarSLAMExample.m (96%) rename matlab/{examples => gtsam_examples}/PlanarSLAMExample_sampling.m (92%) rename matlab/{examples => gtsam_examples}/Pose2SLAMExample.m (96%) rename matlab/{examples => gtsam_examples}/Pose2SLAMExample_advanced.m (99%) rename matlab/{examples => gtsam_examples}/Pose2SLAMExample_circle.m (87%) rename matlab/{examples => gtsam_examples}/Pose2SLAMExample_graph.m (79%) rename matlab/{examples => gtsam_examples}/Pose2SLAMwSPCG.m (100%) rename matlab/{examples => gtsam_examples}/Pose3SLAMExample.m (88%) rename matlab/{examples => gtsam_examples}/Pose3SLAMExample_graph.m (77%) rename matlab/{examples => gtsam_examples}/SBAExample.m (97%) rename matlab/{examples => gtsam_examples}/SFMExample.m (94%) rename matlab/{examples => gtsam_examples}/StereoVOExample.m (94%) rename matlab/{examples => gtsam_examples}/StereoVOExample_large.m (84%) rename matlab/{examples => gtsam_examples}/VisualISAMDemo.m (100%) rename matlab/{examples => gtsam_examples}/VisualISAMExample.m (75%) rename matlab/{examples => gtsam_examples}/VisualISAM_gui.fig (100%) rename matlab/{examples => gtsam_examples}/VisualISAM_gui.m (92%) rename matlab/{examples => gtsam_examples}/gtsamExamples.fig (100%) rename matlab/{examples => gtsam_examples}/gtsamExamples.m (100%) rename matlab/{tests => gtsam_tests}/testJacobianFactor.m (98%) rename matlab/{tests => gtsam_tests}/testKalmanFilter.m (96%) rename matlab/{tests => gtsam_tests}/testLocalizationExample.m (98%) rename matlab/{tests => gtsam_tests}/testOdometryExample.m (98%) rename matlab/{tests => gtsam_tests}/testPlanarSLAMExample.m (98%) rename matlab/{tests => gtsam_tests}/testPose2SLAMExample.m (99%) rename matlab/{tests => gtsam_tests}/testPose3SLAMExample.m (96%) rename matlab/{tests => gtsam_tests}/testSFMExample.m (77%) rename matlab/{tests => gtsam_tests}/testStereoVOExample.m (59%) rename matlab/{tests => gtsam_tests}/testVisualISAMExample.m (82%) rename matlab/{tests => gtsam_tests}/test_gtsam.m (100%) delete mode 100644 matlab/symbol.m delete mode 100644 matlab/symbolChr.m delete mode 100644 matlab/symbolIndex.m diff --git a/gtsam.h b/gtsam.h index 646e44141..6f87670ca 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1023,14 +1023,9 @@ class KalmanFilter { //************************************************************************* #include -class Symbol { - Symbol(char c, size_t j); - Symbol(size_t k); - void print(string s) const; - size_t key() const; - size_t index() const; - char chr() const; -}; +size_t symbol(char chr, size_t index); +char symbolChr(size_t key); +size_t symbolIndex(size_t key); #include class Ordering { @@ -1415,6 +1410,8 @@ typedef gtsam::RangeFactor RangeFactorSimple typedef gtsam::RangeFactor RangeFactorCalibratedCamera; typedef gtsam::RangeFactor RangeFactorSimpleCamera; + +#include template virtual class BearingFactor : gtsam::NonlinearFactor { BearingFactor(size_t key1, size_t key2, const ROTATION& measured, const gtsam::noiseModel::Base* noiseModel); @@ -1423,6 +1420,7 @@ virtual class BearingFactor : gtsam::NonlinearFactor { typedef gtsam::BearingFactor BearingFactor2D; +#include template virtual class BearingRangeFactor : gtsam::NonlinearFactor { BearingRangeFactor(size_t poseKey, size_t pointKey, const ROTATION& measuredBearing, double measuredRange, const gtsam::noiseModel::Base* noiseModel); @@ -1473,412 +1471,7 @@ virtual class GenericStereoFactor : gtsam::NonlinearFactor { typedef gtsam::GenericStereoFactor GenericStereoFactor3D; #include -pair load2D(string filename, +pair load2D(string filename, gtsam::noiseModel::Diagonal* model, int maxID, bool addNoise, bool smart); -} //\namespace gtsam - -//************************************************************************* -// Pose2SLAM -//************************************************************************* - -namespace pose2SLAM { - -#include -class Values { - Values(); - Values(const pose2SLAM::Values& values); - size_t size() const; - void print(string s) const; - bool exists(size_t key); - gtsam::KeyVector keys() const; // Note the switch to KeyVector, rather than KeyList - - static pose2SLAM::Values Circle(size_t n, double R); - void insertPose(size_t key, const gtsam::Pose2& pose); - void updatePose(size_t key, const gtsam::Pose2& pose); - gtsam::Pose2 pose(size_t i); - Matrix poses() const; -}; - -#include -class Graph { - Graph(); - Graph(const gtsam::NonlinearFactorGraph& graph); - Graph(const pose2SLAM::Graph& graph); - - // FactorGraph - void print(string s) const; - bool equals(const pose2SLAM::Graph& fg, double tol) const; - size_t size() const; - bool empty() const; - void remove(size_t i); - size_t nrFactors() const; - gtsam::NonlinearFactor* at(size_t i) const; - - // NonlinearFactorGraph - double error(const pose2SLAM::Values& values) const; - double probPrime(const pose2SLAM::Values& values) const; - gtsam::Ordering* orderingCOLAMD(const pose2SLAM::Values& values) const; - gtsam::GaussianFactorGraph* linearize(const pose2SLAM::Values& values, - const gtsam::Ordering& ordering) const; - - // pose2SLAM-specific - void addPoseConstraint(size_t key, const gtsam::Pose2& pose); - void addPosePrior(size_t key, const gtsam::Pose2& pose, const gtsam::noiseModel::Base* noiseModel); - void addRelativePose(size_t key1, size_t key2, const gtsam::Pose2& relativePoseMeasurement, const gtsam::noiseModel::Base* noiseModel); - pose2SLAM::Values optimize(const pose2SLAM::Values& initialEstimate, size_t verbosity) const; - pose2SLAM::Values optimizeSPCG(const pose2SLAM::Values& initialEstimate, size_t verbosity) const; - gtsam::Marginals marginals(const pose2SLAM::Values& solution) const; -}; - -} //\namespace pose2SLAM - -//************************************************************************* -// Pose3SLAM -//************************************************************************* - -namespace pose3SLAM { - -#include -class Values { - Values(); - Values(const pose3SLAM::Values& values); - size_t size() const; - void print(string s) const; - bool exists(size_t key); - gtsam::KeyVector keys() const; // Note the switch to KeyVector, rather than KeyList - - static pose3SLAM::Values Circle(size_t n, double R); - void insertPose(size_t key, const gtsam::Pose3& pose); - void updatePose(size_t key, const gtsam::Pose3& pose); - gtsam::Pose3 pose(size_t i); - Matrix translations() const; -}; - -#include -class Graph { - Graph(); - Graph(const gtsam::NonlinearFactorGraph& graph); - Graph(const pose3SLAM::Graph& graph); - - // FactorGraph - void print(string s) const; - bool equals(const pose3SLAM::Graph& fg, double tol) const; - size_t size() const; - bool empty() const; - void remove(size_t i); - size_t nrFactors() const; - gtsam::NonlinearFactor* at(size_t i) const; - - // NonlinearFactorGraph - double error(const pose3SLAM::Values& values) const; - double probPrime(const pose3SLAM::Values& values) const; - gtsam::Ordering* orderingCOLAMD(const pose3SLAM::Values& values) const; - gtsam::GaussianFactorGraph* linearize(const pose3SLAM::Values& values, - const gtsam::Ordering& ordering) const; - - // pose3SLAM-specific - void addPoseConstraint(size_t i, const gtsam::Pose3& p); - void addPosePrior(size_t key, const gtsam::Pose3& p, const gtsam::noiseModel::Base* model); - void addRelativePose(size_t key1, size_t key2, const gtsam::Pose3& z, const gtsam::noiseModel::Base* model); - pose3SLAM::Values optimize(const pose3SLAM::Values& initialEstimate, size_t verbosity) const; - // FIXME gtsam::LevenbergMarquardtOptimizer optimizer(const pose3SLAM::Values& initialEstimate, const gtsam::LevenbergMarquardtParams& parameters) const; - gtsam::Marginals marginals(const pose3SLAM::Values& solution) const; -}; - -} //\namespace pose3SLAM - -//************************************************************************* -// planarSLAM -//************************************************************************* - -namespace planarSLAM { - -#include -class Values { - Values(); - Values(const planarSLAM::Values& values); - size_t size() const; - void print(string s) const; - bool exists(size_t key); - gtsam::KeyVector keys() const; // Note the switch to KeyVector, rather than KeyList - - // inherited from pose2SLAM - static planarSLAM::Values Circle(size_t n, double R); - void insertPose(size_t key, const gtsam::Pose2& pose); - void updatePose(size_t key, const gtsam::Pose2& pose); - gtsam::Pose2 pose(size_t i); - Matrix poses() const; - - // Access to poses - planarSLAM::Values allPoses() const; - size_t nrPoses() const; - gtsam::KeyVector poseKeys() const; // Note the switch to KeyVector, rather than KeyList - - // Access to points - planarSLAM::Values allPoints() const; - size_t nrPoints() const; - gtsam::KeyVector pointKeys() const; // Note the switch to KeyVector, rather than KeyList - - void insertPoint(size_t key, const gtsam::Point2& point); - void updatePoint(size_t key, const gtsam::Point2& point); - gtsam::Point2 point(size_t key) const; - Matrix points() const; -}; - -#include -class Graph { - Graph(); - Graph(const gtsam::NonlinearFactorGraph& graph); - Graph(const pose2SLAM::Graph& graph); - Graph(const planarSLAM::Graph& graph); - - // FactorGraph - void print(string s) const; - bool equals(const planarSLAM::Graph& fg, double tol) const; - size_t size() const; - bool empty() const; - void remove(size_t i); - size_t nrFactors() const; - gtsam::NonlinearFactor* at(size_t i) const; - - // NonlinearFactorGraph - double error(const planarSLAM::Values& values) const; - double probPrime(const planarSLAM::Values& values) const; - gtsam::Ordering* orderingCOLAMD(const planarSLAM::Values& values) const; - gtsam::GaussianFactorGraph* linearize(const planarSLAM::Values& values, - const gtsam::Ordering& ordering) const; - - // pose2SLAM-inherited - void addPoseConstraint(size_t key, const gtsam::Pose2& pose); - void addPosePrior(size_t key, const gtsam::Pose2& pose, const gtsam::noiseModel::Base* noiseModel); - void addRelativePose(size_t key1, size_t key2, const gtsam::Pose2& relativePoseMeasurement, const gtsam::noiseModel::Base* noiseModel); - planarSLAM::Values optimize(const planarSLAM::Values& initialEstimate, size_t verbosity) const; - planarSLAM::Values optimizeSPCG(const planarSLAM::Values& initialEstimate, size_t verbosity) const; - gtsam::Marginals marginals(const planarSLAM::Values& solution) const; - - // planarSLAM-specific - void addPointConstraint(size_t pointKey, const gtsam::Point2& p); - void addPointPrior(size_t pointKey, const gtsam::Point2& p, const gtsam::noiseModel::Base* model); - void addBearing(size_t poseKey, size_t pointKey, const gtsam::Rot2& bearing, const gtsam::noiseModel::Base* noiseModel); - void addRange(size_t poseKey, size_t pointKey, double range, const gtsam::noiseModel::Base* noiseModel); - void addBearingRange(size_t poseKey, size_t pointKey, const gtsam::Rot2& bearing,double range, const gtsam::noiseModel::Base* noiseModel); -}; - -#include -class Odometry { - Odometry(size_t key1, size_t key2, const gtsam::Pose2& measured, - const gtsam::noiseModel::Base* model); - void print(string s) const; - gtsam::GaussianFactor* linearize(const planarSLAM::Values& center, - const gtsam::Ordering& ordering) const; -}; - -} //\namespace planarSLAM - -//************************************************************************* -// VisualSLAM -//************************************************************************* - -namespace visualSLAM { - -#include -class Values { - Values(); - Values(const visualSLAM::Values& values); - size_t size() const; - void print(string s) const; - bool exists(size_t key); - gtsam::KeyVector keys() const; // Note the switch to KeyVector, rather than KeyList - - // pose3SLAM inherited - static visualSLAM::Values Circle(size_t n, double R); - void insertPose(size_t key, const gtsam::Pose3& pose); - void updatePose(size_t key, const gtsam::Pose3& pose); - gtsam::Pose3 pose(size_t i); - Matrix translations() const; - - // Access to poses - visualSLAM::Values allPoses() const; - size_t nrPoses() const; - gtsam::KeyVector poseKeys() const; // Note the switch to KeyVector, rather than KeyList - - // Access to points - visualSLAM::Values allPoints() const; - size_t nrPoints() const; - gtsam::KeyVector pointKeys() const; // Note the switch to KeyVector, rather than KeyList - - void insertPoint(size_t key, const gtsam::Point3& pose); - void updatePoint(size_t key, const gtsam::Point3& pose); - gtsam::Point3 point(size_t j); - void insertBackprojections(const gtsam::SimpleCamera& c, Vector J, Matrix Z, double depth); - void perturbPoints(double sigma, size_t seed); - Matrix points() const; -}; - -#include -class Graph { - Graph(); - Graph(const gtsam::NonlinearFactorGraph& graph); - Graph(const pose3SLAM::Graph& graph); - Graph(const visualSLAM::Graph& graph); - - // FactorGraph - void print(string s) const; - bool equals(const visualSLAM::Graph& fg, double tol) const; - size_t size() const; - bool empty() const; - void remove(size_t i); - size_t nrFactors() const; - gtsam::NonlinearFactor* at(size_t i) const; - - double error(const visualSLAM::Values& values) const; - gtsam::Ordering* orderingCOLAMD(const visualSLAM::Values& values) const; - gtsam::GaussianFactorGraph* linearize(const visualSLAM::Values& values, - const gtsam::Ordering& ordering) const; - - // pose3SLAM-inherited - void addPoseConstraint(size_t i, const gtsam::Pose3& p); - void addPosePrior(size_t key, const gtsam::Pose3& p, const gtsam::noiseModel::Base* model); - void addRelativePose(size_t key1, size_t key2, const gtsam::Pose3& z, const gtsam::noiseModel::Base* model); - visualSLAM::Values optimize(const visualSLAM::Values& initialEstimate, size_t verbosity) const; - visualSLAM::LevenbergMarquardtOptimizer optimizer(const visualSLAM::Values& initialEstimate, const gtsam::LevenbergMarquardtParams& parameters) const; - gtsam::Marginals marginals(const visualSLAM::Values& solution) const; - - // Priors and constraints - void addPointConstraint(size_t pointKey, const gtsam::Point3& p); - void addPointPrior(size_t pointKey, const gtsam::Point3& p, const gtsam::noiseModel::Base* model); - void addRangeFactor(size_t poseKey, size_t pointKey, double range, const gtsam::noiseModel::Base* model); - - // Measurements - void addMeasurement(const gtsam::Point2& measured, - const gtsam::noiseModel::Base* model, size_t poseKey, size_t pointKey, - const gtsam::Cal3_S2* K); - void addMeasurements(size_t i, Vector J, Matrix Z, - const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K); - void addStereoMeasurement(const gtsam::StereoPoint2& measured, - const gtsam::noiseModel::Base* model, size_t poseKey, size_t pointKey, - const gtsam::Cal3_S2Stereo* K); - - // Information - Matrix reprojectionErrors(const visualSLAM::Values& values) const; - -}; - -#include -class ISAM { - ISAM(); - ISAM(int reorderInterval); - void print(string s) const; - void printStats() const; - void saveGraph(string s) const; - visualSLAM::Values estimate() const; - Matrix marginalCovariance(size_t key) const; - int reorderInterval() const; - int reorderCounter() const; - void update(const visualSLAM::Graph& newFactors, const visualSLAM::Values& initialValues); - void reorder_relinearize(); - void addKey(size_t key); - void setOrdering(const gtsam::Ordering& new_ordering); - - // These might be expensive as instead of a reference the wrapper will make a copy - gtsam::GaussianISAM bayesTree() const; - visualSLAM::Values getLinearizationPoint() const; - gtsam::Ordering getOrdering() const; - gtsam::NonlinearFactorGraph getFactorsUnsafe() const; -}; - -#include -class LevenbergMarquardtOptimizer { - double lambda() const; - void iterate(); - double error() const; - size_t iterations() const; - visualSLAM::Values optimize(); - visualSLAM::Values optimizeSafely(); - visualSLAM::Values values() const; -}; - -} //\namespace visualSLAM - -//************************************************************************ -// sparse BA -//************************************************************************ - -namespace sparseBA { - -#include -class Values { - Values(); - Values(const sparseBA::Values& values); - size_t size() const; - void print(string s) const; - bool exists(size_t key); - gtsam::KeyVector keys() const; - - // Access to cameras - sparseBA::Values allSimpleCameras() const ; - size_t nrSimpleCameras() const ; - gtsam::KeyVector simpleCameraKeys() const ; - void insertSimpleCamera(size_t j, const gtsam::SimpleCamera& camera); - void updateSimpleCamera(size_t j, const gtsam::SimpleCamera& camera); - gtsam::SimpleCamera simpleCamera(size_t j) const; - - // Access to points, inherited from visualSLAM - sparseBA::Values allPoints() const; - size_t nrPoints() const; - gtsam::KeyVector pointKeys() const; // Note the switch to KeyVector, rather than KeyList - void insertPoint(size_t key, const gtsam::Point3& pose); - void updatePoint(size_t key, const gtsam::Point3& pose); - gtsam::Point3 point(size_t j); - Matrix points() const; -}; - -#include -class Graph { - Graph(); - Graph(const gtsam::NonlinearFactorGraph& graph); - Graph(const sparseBA::Graph& graph); - - // Information - Matrix reprojectionErrors(const sparseBA::Values& values) const; - - // inherited from FactorGraph - void print(string s) const; - bool equals(const sparseBA::Graph& fg, double tol) const; - size_t size() const; - bool empty() const; - void remove(size_t i); - size_t nrFactors() const; - gtsam::NonlinearFactor* at(size_t i) const; - - double error(const sparseBA::Values& values) const; - gtsam::Ordering* orderingCOLAMD(const sparseBA::Values& values) const; - gtsam::GaussianFactorGraph* linearize(const sparseBA::Values& values, const gtsam::Ordering& ordering) const; - - sparseBA::Values optimize(const sparseBA::Values& initialEstimate, size_t verbosity) const; - sparseBA::LevenbergMarquardtOptimizer optimizer(const sparseBA::Values& initialEstimate, const gtsam::LevenbergMarquardtParams& parameters) const; - gtsam::Marginals marginals(const sparseBA::Values& solution) const; - - // inherited from visualSLAM - void addPointConstraint(size_t pointKey, const gtsam::Point3& p); - void addPointPrior(size_t pointKey, const gtsam::Point3& p, const gtsam::noiseModel::Base* model); - - // add factors - void addSimpleCameraPrior(size_t cameraKey, const gtsam::SimpleCamera &camera, gtsam::noiseModel::Base* model); - void addSimpleCameraConstraint(size_t cameraKey, const gtsam::SimpleCamera &camera); - void addSimpleCameraMeasurement(const gtsam::Point2 &z, gtsam::noiseModel::Base* model, size_t cameraKey, size_t pointKey); -}; - -#include -class LevenbergMarquardtOptimizer { - double lambda() const; - void iterate(); - double error() const; - size_t iterations() const; - sparseBA::Values optimize(); - sparseBA::Values optimizeSafely(); - sparseBA::Values values() const; -}; -} //\namespace sparseBA - +} diff --git a/gtsam/nonlinear/Symbol.h b/gtsam/nonlinear/Symbol.h index b4bda4f1a..c128b1105 100644 --- a/gtsam/nonlinear/Symbol.h +++ b/gtsam/nonlinear/Symbol.h @@ -122,6 +122,15 @@ private: } }; +/** Create a symbol key from a character and index, i.e. x5. */ +inline Key symbol(unsigned char c, size_t j) { return (Key)Symbol(c,j); } + +/** Return the character portion of a symbol key. */ +inline unsigned char symbolChr(Key key) { return Symbol(key).chr(); } + +/** Return the index portion of a symbol key. */ +inline size_t symbolIndex(Key key) { return Symbol(key).index(); } + namespace symbol_shorthand { inline Key A(size_t j) { return Symbol('a', j); } inline Key B(size_t j) { return Symbol('b', j); } diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 0502bb69b..59df1da67 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -20,8 +20,10 @@ #include #include +#include #include #include +#include using namespace std; @@ -78,23 +80,23 @@ pair > dataset(const string& dataset, } /* ************************************************************************* */ -pair load2D( - pair > dataset, +pair load2D( + pair > dataset, int maxID, bool addNoise, bool smart) { return load2D(dataset.first, dataset.second, maxID, addNoise, smart); } /* ************************************************************************* */ -pair load2D( - const string& filename, boost::optional model, int maxID, +pair load2D( + const string& filename, boost::optional model, int maxID, bool addNoise, bool smart) { cout << "Will try to read " << filename << endl; ifstream is(filename.c_str()); if (!is) throw std::invalid_argument("load2D: can not find the file!"); - pose2SLAM::Values::shared_ptr poses(new pose2SLAM::Values); - pose2SLAM::Graph::shared_ptr graph(new pose2SLAM::Graph); + Values::shared_ptr poses(new Values); + NonlinearFactorGraph::shared_ptr graph(new NonlinearFactorGraph); string tag; @@ -155,7 +157,7 @@ pair load2D( if (!poses->exists(id2)) poses->insert(id2, poses->at(id1) * l1Xl2); - pose2SLAM::Graph::sharedFactor factor( + NonlinearFactor::shared_ptr factor( new BetweenFactor(id1, id2, l1Xl2, *model)); graph->push_back(factor); } @@ -169,8 +171,8 @@ pair load2D( } /* ************************************************************************* */ -void save2D(const pose2SLAM::Graph& graph, const Values& config, - const SharedDiagonal model, const string& filename) { +void save2D(const NonlinearFactorGraph& graph, const Values& config, + const noiseModel::Diagonal::shared_ptr model, const string& filename) { fstream stream(filename.c_str(), fstream::out); diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index a099fb3d5..d3f00aae2 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -18,7 +18,9 @@ #pragma once -#include +#include +#include + #include namespace gtsam { @@ -39,8 +41,8 @@ namespace gtsam { * @param addNoise add noise to the edges * @param smart try to reduce complexity of covariance to cheapest model */ - std::pair load2D( - std::pair > dataset, + std::pair load2D( + std::pair > dataset, int maxID = 0, bool addNoise = false, bool smart = true); /** @@ -51,15 +53,15 @@ namespace gtsam { * @param addNoise add noise to the edges * @param smart try to reduce complexity of covariance to cheapest model */ - std::pair load2D( + std::pair load2D( const std::string& filename, boost::optional model = boost::optional< - gtsam::SharedDiagonal>(), int maxID = 0, bool addNoise = false, + noiseModel::Diagonal::shared_ptr>(), int maxID = 0, bool addNoise = false, bool smart = true); /** save 2d graph */ - void save2D(const pose2SLAM::Graph& graph, const Values& config, - const gtsam::SharedDiagonal model, const std::string& filename); + void save2D(const NonlinearFactorGraph& graph, const Values& config, + const noiseModel::Diagonal::shared_ptr model, const std::string& filename); /** * Load TORO 3D Graph diff --git a/matlab/CHECK.m b/matlab/+gtsam_utils/CHECK.m similarity index 100% rename from matlab/CHECK.m rename to matlab/+gtsam_utils/CHECK.m diff --git a/matlab/EQUALITY.m b/matlab/+gtsam_utils/EQUALITY.m similarity index 100% rename from matlab/EQUALITY.m rename to matlab/+gtsam_utils/EQUALITY.m diff --git a/matlab/examples/+support/VisualISAMGenerateData.m b/matlab/+gtsam_utils/VisualISAMGenerateData.m similarity index 100% rename from matlab/examples/+support/VisualISAMGenerateData.m rename to matlab/+gtsam_utils/VisualISAMGenerateData.m diff --git a/matlab/examples/+support/VisualISAMInitialize.m b/matlab/+gtsam_utils/VisualISAMInitialize.m similarity index 54% rename from matlab/examples/+support/VisualISAMInitialize.m rename to matlab/+gtsam_utils/VisualISAMInitialize.m index 90ba1e0c5..1e408d3d9 100644 --- a/matlab/examples/+support/VisualISAMInitialize.m +++ b/matlab/+gtsam_utils/VisualISAMInitialize.m @@ -1,66 +1,75 @@ -function [noiseModels,isam,result] = VisualISAMInitialize(data,truth,options) +function [noiseModels,isam,result,nextPoseIndex] = VisualISAMInitialize(data,truth,options) % VisualInitialize: initialize visualSLAM::iSAM object and noise parameters % Authors: Duy Nguyen Ta, Frank Dellaert and Alex Cunningham %% Initialize iSAM -isam = visualSLAM.ISAM(options.reorderInterval); +import gtsam.* +params = gtsam.ISAM2Params; +if options.alwaysRelinearize + params.setRelinearizeSkip(1); +end +isam = ISAM2; %% Set Noise parameters import gtsam.* noiseModels.pose = noiseModel.Diagonal.Sigmas([0.001 0.001 0.001 0.1 0.1 0.1]'); %noiseModels.odometry = noiseModel.Diagonal.Sigmas([0.001 0.001 0.001 0.1 0.1 0.1]'); -noiseModels.odometry = noiseModel.Diagonal.Sigmas([0.1 0.1 0.1 1.0 1.0 1.0]'); +noiseModels.odometry = noiseModel.Diagonal.Sigmas([0.05 0.05 0.05 0.2 0.2 0.2]'); noiseModels.point = noiseModel.Isotropic.Sigma(3, 0.1); noiseModels.measurement = noiseModel.Isotropic.Sigma(2, 1.0); %% Add constraints/priors % TODO: should not be from ground truth! -newFactors = visualSLAM.Graph; -initialEstimates = visualSLAM.Values; +import gtsam.* +newFactors = NonlinearFactorGraph; +initialEstimates = Values; for i=1:2 ii = symbol('x',i); if i==1 if options.hardConstraint % add hard constraint - newFactors.addPoseConstraint(ii,truth.cameras{1}.pose); + newFactors.add(NonlinearEqualityPose3(ii,truth.cameras{1}.pose)); else - newFactors.addPosePrior(ii,truth.cameras{i}.pose, noiseModels.pose); + newFactors.add(PriorFactorPose3(ii,truth.cameras{i}.pose, noiseModels.pose)); end end - initialEstimates.insertPose(ii,truth.cameras{i}.pose); + initialEstimates.insert(ii,truth.cameras{i}.pose); end +nextPoseIndex = 3; + %% Add visual measurement factors from two first poses and initialize observed landmarks +import gtsam.* for i=1:2 ii = symbol('x',i); for k=1:length(data.Z{i}) j = data.J{i}{k}; jj = symbol('l',data.J{i}{k}); - newFactors.addMeasurement(data.Z{i}{k}, noiseModels.measurement, ii, jj, data.K); + newFactors.add(GenericProjectionFactorCal3_S2(data.Z{i}{k}, noiseModels.measurement, ii, jj, data.K)); % TODO: initial estimates should not be from ground truth! if ~initialEstimates.exists(jj) - initialEstimates.insertPoint(jj, truth.points{j}); + initialEstimates.insert(jj, truth.points{j}); end if options.pointPriors % add point priors - newFactors.addPointPrior(jj, truth.points{j}, noiseModels.point); + newFactors.add(priorFactorPoint3(jj, truth.points{j}, noiseModels.point)); end end end %% Add odometry between frames 1 and 2 -newFactors.addRelativePose(symbol('x',1), symbol('x',2), data.odometry{1}, noiseModels.odometry); +import gtsam.* +newFactors.add(BetweenFactorPose3(symbol('x',1), symbol('x',2), data.odometry{1}, noiseModels.odometry)); %% Update ISAM +import gtsam.* if options.batchInitialization % Do a full optimize for first two poses - fullyOptimized = newFactors.optimize(initialEstimates,0); + batchOptimizer = LevenbergMarquardtOptimizer(newFactors, initialEstimates); + fullyOptimized = batchOptimizer.optimize(); isam.update(newFactors, fullyOptimized); else isam.update(newFactors, initialEstimates); end % figure(1);tic; % t=toc; plot(frame_i,t,'r.'); tic -result = isam.estimate(); +result = isam.calculateEstimate(); % t=toc; plot(frame_i,t,'g.'); -if options.alwaysRelinearize % re-linearize - isam.reorder_relinearize(); -end diff --git a/matlab/examples/+support/VisualISAMPlot.m b/matlab/+gtsam_utils/VisualISAMPlot.m similarity index 55% rename from matlab/examples/+support/VisualISAMPlot.m rename to matlab/+gtsam_utils/VisualISAMPlot.m index 1f2b7d710..41aa49ac6 100644 --- a/matlab/examples/+support/VisualISAMPlot.m +++ b/matlab/+gtsam_utils/VisualISAMPlot.m @@ -1,39 +1,33 @@ function VisualISAMPlot(truth, data, isam, result, options) -% VisualISAMPlot: plot current state of visualSLAM::iSAM object +% VisualISAMPlot: plot current state of ISAM2 object % Authors: Duy Nguyen Ta and Frank Dellaert -M = result.nrPoses; -N = result.nrPoints; - h=gca; cla(h); hold on; %% Plot points % Can't use data because current frame might not see all points -pointKeys = result.allPoints().keys(); -for j=0:N-1 % NOTE: uses indexing directly from a C++ vector, so zero-indexed - jj = pointKeys.at(j); - point_j = result.point(jj); - plot3(point_j.x, point_j.y, point_j.z,'marker','o'); - P = isam.marginalCovariance(jj); - covarianceEllipse3D([point_j.x;point_j.y;point_j.z],P); -end +marginals = Marginals(isam.getFactorsUnsafe(), isam.calculateEstimate()); % TODO - this is slow +gtsam_utils.plot3DPoints(result, [], marginals); %% Plot cameras import gtsam.* -for i=1:options.cameraInterval:M - ii = symbol('x',i); - pose_i = result.pose(ii); - if options.hardConstraint && (i==1) - plotPose3(pose_i,[],10); +M = 1; +while result.exists(symbol('x',M)) + ii = symbol('x',M); + pose_i = result.at(ii); + if options.hardConstraint && (M==1) + gtsam_utils.plotPose3(pose_i,[],10); else - P = isam.marginalCovariance(ii); - plotPose3(pose_i,P,10); + P = marginals.marginalCovariance(ii); + gtsam_utils.plotPose3(pose_i,P,10); end if options.drawTruePoses % show ground truth - plotPose3(truth.cameras{i}.pose,[],10); + gtsam_utils.plotPose3(truth.cameras{M}.pose,[],10); end + + M = M + options.cameraInterval; end %% draw diff --git a/matlab/+gtsam_utils/VisualISAMStep.m b/matlab/+gtsam_utils/VisualISAMStep.m new file mode 100644 index 000000000..71b1c849c --- /dev/null +++ b/matlab/+gtsam_utils/VisualISAMStep.m @@ -0,0 +1,44 @@ +function [isam,result,nextPoseIndex] = VisualISAMStep(data,noiseModels,isam,result,truth,nextPoseIndex) +% VisualISAMStep: execute one update step of visualSLAM::iSAM object +% Authors: Duy Nguyen Ta and Frank Dellaert + +% iSAM expects us to give it a new set of factors +% along with initial estimates for any new variables introduced. +import gtsam.* +newFactors = NonlinearFactorGraph; +initialEstimates = Values; + +%% Add odometry +import gtsam.* +odometry = data.odometry{nextPoseIndex-1}; +newFactors.add(BetweenFactorPose3(symbol('x',nextPoseIndex-1), symbol('x',nextPoseIndex), odometry, noiseModels.odometry)); + +%% Add visual measurement factors and initializations as necessary +import gtsam.* +for k=1:length(data.Z{nextPoseIndex}) + zij = data.Z{nextPoseIndex}{k}; + j = data.J{nextPoseIndex}{k}; + jj = symbol('l', j); + newFactors.add(GenericProjectionFactorCal3_S2(zij, noiseModels.measurement, symbol('x',nextPoseIndex), jj, data.K)); + % TODO: initialize with something other than truth + if ~result.exists(jj) && ~initialEstimates.exists(jj) + lmInit = truth.points{j}; + initialEstimates.insert(jj, lmInit); + end +end + +%% Initial estimates for the new pose. +import gtsam.* +prevPose = result.at(symbol('x',nextPoseIndex-1)); +initialEstimates.insert(symbol('x',nextPoseIndex), prevPose.compose(odometry)); + +%% Update ISAM +% figure(1);tic; +isam.update(newFactors, initialEstimates); +% t=toc; plot(frame_i,t,'r.'); tic +result = isam.calculateEstimate(); +% t=toc; plot(frame_i,t,'g.'); + +% Update nextPoseIndex to return +nextPoseIndex = nextPoseIndex + 1; + diff --git a/matlab/circlePose2.m b/matlab/+gtsam_utils/circlePose2.m similarity index 95% rename from matlab/circlePose2.m rename to matlab/+gtsam_utils/circlePose2.m index 7153fc128..0679c16b3 100644 --- a/matlab/circlePose2.m +++ b/matlab/+gtsam_utils/circlePose2.m @@ -23,7 +23,7 @@ values = gtsam.Values; theta = 0.0; dtheta = 2*pi()/numPoses; for i = 1:numPoses - key = gtsam.Symbol(symbolChar, i-1).key(); + key = gtsam.symbol(symbolChar, i-1); pose = gtsam.Pose2(radius*cos(theta), radius*sin(theta), pi()/2 + theta); values.insert(key, pose); theta = theta + dtheta; diff --git a/matlab/circlePose3.m b/matlab/+gtsam_utils/circlePose3.m similarity index 96% rename from matlab/circlePose3.m rename to matlab/+gtsam_utils/circlePose3.m index 0e74a2f1b..9085ab0d0 100644 --- a/matlab/circlePose3.m +++ b/matlab/+gtsam_utils/circlePose3.m @@ -24,7 +24,7 @@ theta = 0.0; dtheta = 2*pi()/numPoses; gRo = gtsam.Rot3([0, 1, 0 ; 1, 0, 0 ; 0, 0, -1]); for i = 1:numPoses - key = gtsam.Symbol(symbolChar, i-1).key(); + key = gtsam.symbol(symbolChar, i-1); gti = gtsam.Point3(radius*cos(theta), radius*sin(theta), 0); oRi = gtsam.Rot3.Yaw(-theta); % negative yaw goes counterclockwise, with Z down ! gTi = gtsam.Pose3(gRo.compose(oRi), gti); diff --git a/matlab/covarianceEllipse.m b/matlab/+gtsam_utils/covarianceEllipse.m similarity index 96% rename from matlab/covarianceEllipse.m rename to matlab/+gtsam_utils/covarianceEllipse.m index 491e099c3..b4579862e 100644 --- a/matlab/covarianceEllipse.m +++ b/matlab/+gtsam_utils/covarianceEllipse.m @@ -1,34 +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 - +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/matlab/covarianceEllipse3D.m b/matlab/+gtsam_utils/covarianceEllipse3D.m similarity index 100% rename from matlab/covarianceEllipse3D.m rename to matlab/+gtsam_utils/covarianceEllipse3D.m diff --git a/matlab/examples/+support/findExampleDataFile.m b/matlab/+gtsam_utils/findExampleDataFile.m similarity index 74% rename from matlab/examples/+support/findExampleDataFile.m rename to matlab/+gtsam_utils/findExampleDataFile.m index 5fd23352e..9c40960b9 100644 --- a/matlab/examples/+support/findExampleDataFile.m +++ b/matlab/+gtsam_utils/findExampleDataFile.m @@ -3,8 +3,8 @@ function datafile = findExampleDataFile(datasetName) [ myPath, ~, ~ ] = fileparts(mfilename('fullpath')); searchPath = { ... - fullfile(myPath, [ '../../../examples/Data/' datasetName ]) ... - fullfile(myPath, [ '../Data/' datasetName ]) }; + fullfile(myPath, [ '../../examples/Data/' datasetName ]) ... + fullfile(myPath, [ '../gtsam_examples/Data/' datasetName ]) }; datafile = []; for path = searchPath if exist(path{:}, 'file') diff --git a/matlab/load2D.m b/matlab/+gtsam_utils/load2D.m similarity index 100% rename from matlab/load2D.m rename to matlab/+gtsam_utils/load2D.m diff --git a/matlab/load3D.m b/matlab/+gtsam_utils/load3D.m similarity index 100% rename from matlab/load3D.m rename to matlab/+gtsam_utils/load3D.m diff --git a/matlab/plot2DPoints.m b/matlab/+gtsam_utils/plot2DPoints.m similarity index 88% rename from matlab/plot2DPoints.m rename to matlab/+gtsam_utils/plot2DPoints.m index d03676699..b98434627 100644 --- a/matlab/plot2DPoints.m +++ b/matlab/+gtsam_utils/plot2DPoints.m @@ -22,9 +22,9 @@ for i = 0:keys.size-1 if isa(p, 'gtsam.Point2') if haveMarginals P = marginals.marginalCovariance(key); - plotPoint2(p, linespec, P); + gtsam_utils.plotPoint2(p, linespec, P); else - plotPoint2(p, linespec); + gtsam_utils.plotPoint2(p, linespec); end end end diff --git a/matlab/plot2DTrajectory.m b/matlab/+gtsam_utils/plot2DTrajectory.m similarity index 93% rename from matlab/plot2DTrajectory.m rename to matlab/+gtsam_utils/plot2DTrajectory.m index fcbfe4b92..c962c993b 100644 --- a/matlab/plot2DTrajectory.m +++ b/matlab/+gtsam_utils/plot2DTrajectory.m @@ -31,7 +31,7 @@ for i = 0:keys.size-1 plot([ x.x; lastPose.x ], [ x.y; lastPose.y ], linespec); if haveMarginals P = marginals.marginalCovariance(lastKey); - plotPose2(lastPose, 'g', P); + gtsam_utils.plotPose2(lastPose, 'g', P); end end lastIndex = i; @@ -43,7 +43,7 @@ if ~isempty(lastIndex) && haveMarginals lastKey = keys.at(lastIndex); lastPose = values.at(lastKey); P = marginals.marginalCovariance(lastKey); - plotPose2(lastPose, 'g', P); + gtsam_utils.plotPose2(lastPose, 'g', P); end if ~holdstate diff --git a/matlab/plot3DPoints.m b/matlab/+gtsam_utils/plot3DPoints.m similarity index 88% rename from matlab/plot3DPoints.m rename to matlab/+gtsam_utils/plot3DPoints.m index 264298ef4..e7a5ab0a0 100644 --- a/matlab/plot3DPoints.m +++ b/matlab/+gtsam_utils/plot3DPoints.m @@ -22,9 +22,9 @@ for i = 0:keys.size-1 if isa(p, 'gtsam.Point3') if haveMarginals P = marginals.marginalCovariance(key); - plotPoint3(p, linespec, P); + gtsam_utils.plotPoint3(p, linespec, P); else - plotPoint3(p, linespec); + gtsam_utils.plotPoint3(p, linespec); end end end diff --git a/matlab/plot3DTrajectory.m b/matlab/+gtsam_utils/plot3DTrajectory.m similarity index 91% rename from matlab/plot3DTrajectory.m rename to matlab/+gtsam_utils/plot3DTrajectory.m index 771d567fa..08b2f0daa 100644 --- a/matlab/plot3DTrajectory.m +++ b/matlab/+gtsam_utils/plot3DTrajectory.m @@ -28,7 +28,7 @@ for i = 0:keys.size-1 else P = []; end - plotPose3(lastPose, P, scale); + gtsam_utils.plotPose3(lastPose, P, scale); end lastIndex = i; end @@ -43,7 +43,7 @@ if ~isempty(lastIndex) else P = []; end - plotPose3(lastPose, P, scale); + gtsam_utils.plotPose3(lastPose, P, scale); end if ~holdstate diff --git a/matlab/plotCamera.m b/matlab/+gtsam_utils/plotCamera.m similarity index 100% rename from matlab/plotCamera.m rename to matlab/+gtsam_utils/plotCamera.m diff --git a/matlab/plotPoint2.m b/matlab/+gtsam_utils/plotPoint2.m similarity index 76% rename from matlab/plotPoint2.m rename to matlab/+gtsam_utils/plotPoint2.m index fcc274d90..a1bc85a88 100644 --- a/matlab/plotPoint2.m +++ b/matlab/+gtsam_utils/plotPoint2.m @@ -6,5 +6,5 @@ else plot(p.x,p.y,color); end if exist('P', 'var') - covarianceEllipse([p.x;p.y],P,color(1)); + gtsam_utils.covarianceEllipse([p.x;p.y],P,color(1)); end \ No newline at end of file diff --git a/matlab/plotPoint3.m b/matlab/+gtsam_utils/plotPoint3.m similarity index 80% rename from matlab/plotPoint3.m rename to matlab/+gtsam_utils/plotPoint3.m index 89d5613d5..6aafd5f21 100644 --- a/matlab/plotPoint3.m +++ b/matlab/+gtsam_utils/plotPoint3.m @@ -6,7 +6,7 @@ else plot3(p.x,p.y,p.z,color); end if exist('P', 'var') - covarianceEllipse3D([p.x;p.y;p.z],P); + gtsam_utils.covarianceEllipse3D([p.x;p.y;p.z],P); end end diff --git a/matlab/plotPose2.m b/matlab/+gtsam_utils/plotPose2.m similarity index 84% rename from matlab/plotPose2.m rename to matlab/+gtsam_utils/plotPose2.m index 54b1e68f0..a9997dc2c 100644 --- a/matlab/plotPose2.m +++ b/matlab/+gtsam_utils/plotPose2.m @@ -9,5 +9,5 @@ quiver(pose.x,pose.y,c,s,axisLength,color); if nargin>2 pPp = P(1:2,1:2); % covariance matrix in pose coordinate frame gRp = [c -s;s c]; % rotation from pose to global - covarianceEllipse([pose.x;pose.y],gRp*pPp*gRp',color); + gtsam_utils.covarianceEllipse([pose.x;pose.y],gRp*pPp*gRp',color); end \ No newline at end of file diff --git a/matlab/plotPose3.m b/matlab/+gtsam_utils/plotPose3.m similarity index 94% rename from matlab/plotPose3.m rename to matlab/+gtsam_utils/plotPose3.m index 956b231be..d24a998c8 100644 --- a/matlab/plotPose3.m +++ b/matlab/+gtsam_utils/plotPose3.m @@ -25,7 +25,7 @@ end if (nargin>2) && (~isempty(P)) pPp = P(4:6,4:6); % covariance matrix in pose coordinate frame gPp = gRp*pPp*gRp'; % convert the covariance matrix to global coordinate frame - covarianceEllipse3D(C,gPp); + gtsam_utils.covarianceEllipse3D(C,gPp); end end diff --git a/matlab/CMakeLists.txt b/matlab/CMakeLists.txt index 0b07b51b1..66fa99def 100644 --- a/matlab/CMakeLists.txt +++ b/matlab/CMakeLists.txt @@ -10,14 +10,17 @@ install(FILES ${matlab_utils} DESTINATION ${GTSAM_TOOLBOX_INSTALL_PATH}/) # Tests message(STATUS "Installing Matlab Toolbox Tests") -file(GLOB matlab_tests "${GTSAM_SOURCE_ROOT_DIR}/matlab/tests/*.m") -install(FILES ${matlab_tests} DESTINATION ${GTSAM_TOOLBOX_INSTALL_PATH}/gtsam_tests) +install(DIRECTORY "${GTSAM_SOURCE_ROOT_DIR}/matlab/gtsam_tests" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}" FILES_MATCHING PATTERN "*.m") # Examples message(STATUS "Installing Matlab Toolbox Examples") # Matlab files: *.m and *.fig -install(DIRECTORY "${GTSAM_SOURCE_ROOT_DIR}/matlab/examples/" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}/gtsam_examples" FILES_MATCHING PATTERN "*.m") -install(DIRECTORY "${GTSAM_SOURCE_ROOT_DIR}/matlab/examples/" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}/gtsam_examples" FILES_MATCHING PATTERN "*.fig") +install(DIRECTORY "${GTSAM_SOURCE_ROOT_DIR}/matlab/gtsam_examples" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}" FILES_MATCHING PATTERN "*.m") +install(DIRECTORY "${GTSAM_SOURCE_ROOT_DIR}/matlab/gtsam_examples" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}" FILES_MATCHING PATTERN "*.fig") + +# Utilities +message(STATUS "Installing Matlab Toolbox Utilities") +install(DIRECTORY "${GTSAM_SOURCE_ROOT_DIR}/matlab/+gtsam_utils" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}" FILES_MATCHING PATTERN "*.m") message(STATUS "Installing Matlab Toolbox Examples (Data)") # Data files: *.graph and *.txt diff --git a/matlab/examples/+support/VisualISAMStep.m b/matlab/examples/+support/VisualISAMStep.m deleted file mode 100644 index ef056553e..000000000 --- a/matlab/examples/+support/VisualISAMStep.m +++ /dev/null @@ -1,44 +0,0 @@ -function [isam,result] = VisualISAMStep(data,noiseModels,isam,result,truth, options) -% VisualISAMStep: execute one update step of visualSLAM::iSAM object -% Authors: Duy Nguyen Ta and Frank Dellaert - -% iSAM expects us to give it a new set of factors -% along with initial estimates for any new variables introduced. -newFactors = visualSLAM.Graph; -initialEstimates = visualSLAM.Values; - -%% Add odometry -import gtsam.* -i = result.nrPoses+1; -odometry = data.odometry{i-1}; -newFactors.addRelativePose(symbol('x',i-1), symbol('x',i), odometry, noiseModels.odometry); - -%% Add visual measurement factors and initializations as necessary -import gtsam.* -for k=1:length(data.Z{i}) - zij = data.Z{i}{k}; - j = data.J{i}{k}; - jj = symbol('l', j); - newFactors.addMeasurement(zij, noiseModels.measurement, symbol('x',i), jj, data.K); - % TODO: initialize with something other than truth - if ~result.exists(jj) && ~initialEstimates.exists(jj) - lmInit = truth.points{j}; - initialEstimates.insertPoint(jj, lmInit); - end -end - -%% Initial estimates for the new pose. -import gtsam.* -prevPose = result.pose(symbol('x',i-1)); -initialEstimates.insertPose(symbol('x',i), prevPose.compose(odometry)); - -%% Update ISAM -% figure(1);tic; -isam.update(newFactors, initialEstimates); -% t=toc; plot(frame_i,t,'r.'); tic -result = isam.estimate(); -% t=toc; plot(frame_i,t,'g.'); - -if options.alwaysRelinearize % re-linearize - isam.reorder_relinearize(); -end diff --git a/matlab/examples/LocalizationExample.m b/matlab/gtsam_examples/LocalizationExample.m similarity index 96% rename from matlab/examples/LocalizationExample.m rename to matlab/gtsam_examples/LocalizationExample.m index e4ee76d7c..aad3af98c 100644 --- a/matlab/examples/LocalizationExample.m +++ b/matlab/gtsam_examples/LocalizationExample.m @@ -55,7 +55,7 @@ import gtsam.* cla; hold on; -plot2DTrajectory(result, [], Marginals(graph, result)); +gtsam_utils.plot2DTrajectory(result, [], Marginals(graph, result)); axis([-0.6 4.8 -1 1]) axis equal diff --git a/matlab/examples/OdometryExample.m b/matlab/gtsam_examples/OdometryExample.m similarity index 96% rename from matlab/examples/OdometryExample.m rename to matlab/gtsam_examples/OdometryExample.m index 81733cfb0..e47becda5 100644 --- a/matlab/examples/OdometryExample.m +++ b/matlab/gtsam_examples/OdometryExample.m @@ -52,7 +52,7 @@ import gtsam.* cla; hold on; -plot2DTrajectory(result, [], Marginals(graph, result)); +gtsam_utils.plot2DTrajectory(result, [], Marginals(graph, result)); axis([-0.6 4.8 -1 1]) axis equal diff --git a/matlab/examples/PlanarSLAMExample.m b/matlab/gtsam_examples/PlanarSLAMExample.m similarity index 96% rename from matlab/examples/PlanarSLAMExample.m rename to matlab/gtsam_examples/PlanarSLAMExample.m index 9b4600578..056db501e 100644 --- a/matlab/examples/PlanarSLAMExample.m +++ b/matlab/gtsam_examples/PlanarSLAMExample.m @@ -72,8 +72,8 @@ import gtsam.* cla;hold on marginals = Marginals(graph, result); -plot2DTrajectory(result, [], marginals); -plot2DPoints(result, [], marginals); +gtsam_utils.plot2DTrajectory(result, [], marginals); +gtsam_utils.plot2DPoints(result, [], marginals); plot([result.at(i1).x; result.at(j1).x],[result.at(i1).y; result.at(j1).y], 'c-'); plot([result.at(i2).x; result.at(j1).x],[result.at(i2).y; result.at(j1).y], 'c-'); diff --git a/matlab/examples/PlanarSLAMExample_sampling.m b/matlab/gtsam_examples/PlanarSLAMExample_sampling.m similarity index 92% rename from matlab/examples/PlanarSLAMExample_sampling.m rename to matlab/gtsam_examples/PlanarSLAMExample_sampling.m index 58686c362..5604d0ddc 100644 --- a/matlab/examples/PlanarSLAMExample_sampling.m +++ b/matlab/gtsam_examples/PlanarSLAMExample_sampling.m @@ -39,6 +39,7 @@ end graph.add(BearingRangeFactor2D(i3, j2, Rot2(90*degrees), 2, brNoise)); %% Initialize MCMC sampler with ground truth +import gtsam.* sample = Values; sample.insert(i1, Pose2(0,0,0)); sample.insert(i2, Pose2(2,0,0)); @@ -47,11 +48,12 @@ sample.insert(j1, Point2(2,2)); sample.insert(j2, Point2(4,2)); %% Calculate and plot Covariance Ellipses +import gtsam.* cla;hold on marginals = Marginals(graph, sample); -plot2DTrajectory(sample, [], marginals); -plot2DPoints(sample, [], marginals); +gtsam_utils.plot2DTrajectory(sample, [], marginals); +gtsam_utils.plot2DPoints(sample, [], marginals); for j=1:2 key = symbol('l',j); @@ -66,9 +68,10 @@ plot([sample.at(i3).x; sample.at(j2).x],[sample.at(i3).y; sample.at(j2).y], 'c-' view(2); axis auto; axis equal %% Do Sampling on point 2 +import gtsam.* N=1000; for s=1:N delta = S{2}*randn(2,1); proposedPoint = Point2(point{2}.x+delta(1),point{2}.y+delta(2)); - plotPoint2(proposedPoint,'k.') + gtsam_utils.plotPoint2(proposedPoint,'k.') end \ No newline at end of file diff --git a/matlab/examples/Pose2SLAMExample.m b/matlab/gtsam_examples/Pose2SLAMExample.m similarity index 96% rename from matlab/examples/Pose2SLAMExample.m rename to matlab/gtsam_examples/Pose2SLAMExample.m index 1e26ce33d..a8ad32254 100644 --- a/matlab/examples/Pose2SLAMExample.m +++ b/matlab/gtsam_examples/Pose2SLAMExample.m @@ -56,17 +56,19 @@ initialEstimate.insert(5, Pose2(2.1, 2.1,-pi/2)); initialEstimate.print(sprintf('\nInitial estimate:\n')); %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd +import gtsam.* optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate); result = optimizer.optimizeSafely(); result.print(sprintf('\nFinal result:\n')); %% Plot Covariance Ellipses +import gtsam.* cla; hold on plot([result.at(5).x;result.at(2).x],[result.at(5).y;result.at(2).y],'r-','LineWidth',2); marginals = Marginals(graph, result); -plot2DTrajectory(result, [], marginals); +gtsam_utils.plot2DTrajectory(result, [], marginals); axis([-0.6 4.8 -1 1]) axis equal diff --git a/matlab/examples/Pose2SLAMExample_advanced.m b/matlab/gtsam_examples/Pose2SLAMExample_advanced.m similarity index 99% rename from matlab/examples/Pose2SLAMExample_advanced.m rename to matlab/gtsam_examples/Pose2SLAMExample_advanced.m index b496c44e7..fab340f2a 100644 --- a/matlab/examples/Pose2SLAMExample_advanced.m +++ b/matlab/gtsam_examples/Pose2SLAMExample_advanced.m @@ -77,5 +77,6 @@ IJS = gfg.sparseJacobian_(); Ab=sparse(IJS(1,:),IJS(2,:),IJS(3,:)); A = Ab(:,1:end-1); b = full(Ab(:,end)); +clf spy(A); title('Non-zero entries in measurement Jacobian'); diff --git a/matlab/examples/Pose2SLAMExample_circle.m b/matlab/gtsam_examples/Pose2SLAMExample_circle.m similarity index 87% rename from matlab/examples/Pose2SLAMExample_circle.m rename to matlab/gtsam_examples/Pose2SLAMExample_circle.m index 3d2265d76..b40c67322 100644 --- a/matlab/examples/Pose2SLAMExample_circle.m +++ b/matlab/gtsam_examples/Pose2SLAMExample_circle.m @@ -10,14 +10,14 @@ % @author Frank Dellaert %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -import gtsam.* - %% Create a hexagon of poses -hexagon = circlePose2(6,1.0); +import gtsam.* +hexagon = gtsam_utils.circlePose2(6,1.0); p0 = hexagon.at(0); p1 = hexagon.at(1); %% create a Pose graph with one equality constraint and one measurement +import gtsam.* fg = NonlinearFactorGraph; fg.add(NonlinearEqualityPose2(0, p0)); delta = p0.between(p1); @@ -30,6 +30,7 @@ fg.add(BetweenFactorPose2(4,5, delta, covariance)); fg.add(BetweenFactorPose2(5,0, delta, covariance)); %% Create initial config +import gtsam.* initial = Values; initial.insert(0, p0); initial.insert(1, hexagon.at(1).retract([-0.1, 0.1,-0.1]')); @@ -39,15 +40,18 @@ initial.insert(4, hexagon.at(4).retract([ 0.1,-0.1, 0.1]')); initial.insert(5, hexagon.at(5).retract([-0.1, 0.1,-0.1]')); %% Plot Initial Estimate +import gtsam.* cla -plot2DTrajectory(initial, 'g*-'); axis equal +gtsam_utils.plot2DTrajectory(initial, 'g*-'); axis equal %% optimize +import gtsam.* optimizer = DoglegOptimizer(fg, initial); result = optimizer.optimizeSafely; %% Show Result -hold on; plot2DTrajectory(result, 'b*-'); +import gtsam.* +hold on; gtsam_utils.plot2DTrajectory(result, 'b*-'); view(2); axis([-1.5 1.5 -1.5 1.5]); result.print(sprintf('\nFinal result:\n')); diff --git a/matlab/examples/Pose2SLAMExample_graph.m b/matlab/gtsam_examples/Pose2SLAMExample_graph.m similarity index 79% rename from matlab/examples/Pose2SLAMExample_graph.m rename to matlab/gtsam_examples/Pose2SLAMExample_graph.m index f5193a7de..980a11cfa 100644 --- a/matlab/examples/Pose2SLAMExample_graph.m +++ b/matlab/gtsam_examples/Pose2SLAMExample_graph.m @@ -11,12 +11,12 @@ %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% Find data file -datafile = support.findExampleDataFile('w100-odom.graph'); +datafile = gtsam_utils.findExampleDataFile('w100-odom.graph'); %% Initialize graph, initial estimate, and odometry noise import gtsam.* model = noiseModel.Diagonal.Sigmas([0.05; 0.05; 5*pi/180]); -[graph,initial] = load2D(datafile, model); +[graph,initial] = gtsam_utils.load2D(datafile, model); initial.print(sprintf('Initial estimate:\n')); %% Add a Gaussian prior on pose x_1 @@ -26,22 +26,25 @@ priorNoise = noiseModel.Diagonal.Sigmas([0.01; 0.01; 0.01]); graph.add(PriorFactorPose2(0, priorMean, priorNoise)); % add directly to graph %% Plot Initial Estimate +import gtsam.* cla -plot2DTrajectory(initial, 'g-*'); axis equal +gtsam_utils.plot2DTrajectory(initial, 'g-*'); axis equal %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd +import gtsam.* optimizer = LevenbergMarquardtOptimizer(graph, initial); result = optimizer.optimizeSafely; -hold on; plot2DTrajectory(result, 'b-*'); +hold on; gtsam_utils.plot2DTrajectory(result, 'b-*'); result.print(sprintf('\nFinal result:\n')); %% Plot Covariance Ellipses +import gtsam.* marginals = Marginals(graph, result); P={}; for i=1:result.size()-1 pose_i = result.at(i); P{i}=marginals.marginalCovariance(i); - plotPose2(pose_i,'b',P{i}) + gtsam_utils.plotPose2(pose_i,'b',P{i}) end view(2) axis([-15 10 -15 10]); axis equal; diff --git a/matlab/examples/Pose2SLAMwSPCG.m b/matlab/gtsam_examples/Pose2SLAMwSPCG.m similarity index 100% rename from matlab/examples/Pose2SLAMwSPCG.m rename to matlab/gtsam_examples/Pose2SLAMwSPCG.m diff --git a/matlab/examples/Pose3SLAMExample.m b/matlab/gtsam_examples/Pose3SLAMExample.m similarity index 88% rename from matlab/examples/Pose3SLAMExample.m rename to matlab/gtsam_examples/Pose3SLAMExample.m index 0d2bd237f..bdc101b9d 100644 --- a/matlab/examples/Pose3SLAMExample.m +++ b/matlab/gtsam_examples/Pose3SLAMExample.m @@ -10,14 +10,14 @@ % @author Frank Dellaert %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -import gtsam.* - %% Create a hexagon of poses -hexagon = circlePose3(6,1.0); +import gtsam.* +hexagon = gtsam_utils.circlePose3(6,1.0); p0 = hexagon.at(0); p1 = hexagon.at(1); %% create a Pose graph with one equality constraint and one measurement +import gtsam.* fg = NonlinearFactorGraph; fg.add(NonlinearEqualityPose3(0, p0)); delta = p0.between(p1); @@ -30,6 +30,7 @@ fg.add(BetweenFactorPose3(4,5, delta, covariance)); fg.add(BetweenFactorPose3(5,0, delta, covariance)); %% Create initial config +import gtsam.* initial = Values; s = 0.10; initial.insert(0, p0); @@ -40,15 +41,18 @@ initial.insert(4, hexagon.at(4).retract(s*randn(6,1))); initial.insert(5, hexagon.at(5).retract(s*randn(6,1))); %% Plot Initial Estimate +import gtsam.* cla -plot3DTrajectory(initial, 'g-*'); +gtsam_utils.plot3DTrajectory(initial, 'g-*'); %% optimize +import gtsam.* optimizer = DoglegOptimizer(fg, initial); result = optimizer.optimizeSafely(); %% Show Result -hold on; plot3DTrajectory(result, 'b-*', true, 0.3); +import gtsam.* +hold on; gtsam_utils.plot3DTrajectory(result, 'b-*', true, 0.3); axis([-2 2 -2 2 -1 1]); axis equal view(-37,40) diff --git a/matlab/examples/Pose3SLAMExample_graph.m b/matlab/gtsam_examples/Pose3SLAMExample_graph.m similarity index 77% rename from matlab/examples/Pose3SLAMExample_graph.m rename to matlab/gtsam_examples/Pose3SLAMExample_graph.m index 1d33c30e4..574894878 100644 --- a/matlab/examples/Pose3SLAMExample_graph.m +++ b/matlab/gtsam_examples/Pose3SLAMExample_graph.m @@ -16,26 +16,27 @@ N = 2500; % dataset = 'sphere2500_groundtruth.txt'; dataset = 'sphere2500.txt'; -datafile = support.findExampleDataFile(dataset); +datafile = gtsam_utils.findExampleDataFile(dataset); %% Initialize graph, initial estimate, and odometry noise import gtsam.* model = noiseModel.Diagonal.Sigmas([0.05; 0.05; 0.05; 5*pi/180; 5*pi/180; 5*pi/180]); -[graph,initial]=load3D(datafile,model,true,N); +[graph,initial]=gtsam_utils.load3D(datafile,model,true,N); %% Plot Initial Estimate +import gtsam.* cla first = initial.at(0); plot3(first.x(),first.y(),first.z(),'r*'); hold on -plot3DTrajectory(initial,'g-',false); +gtsam_utils.plot3DTrajectory(initial,'g-',false); drawnow; %% Read again, now with all constraints, and optimize import gtsam.* -graph = load3D(datafile, model, false, N); +graph = gtsam_utils.load3D(datafile, model, false, N); graph.add(NonlinearEqualityPose3(0, first)); optimizer = LevenbergMarquardtOptimizer(graph, initial); result = optimizer.optimizeSafely(); -plot3DTrajectory(result, 'r-', false); axis equal; +gtsam_utils.plot3DTrajectory(result, 'r-', false); axis equal; view(3); axis equal; \ No newline at end of file diff --git a/matlab/examples/SBAExample.m b/matlab/gtsam_examples/SBAExample.m similarity index 97% rename from matlab/examples/SBAExample.m rename to matlab/gtsam_examples/SBAExample.m index a9f101613..852c83fdc 100644 --- a/matlab/examples/SBAExample.m +++ b/matlab/gtsam_examples/SBAExample.m @@ -22,7 +22,7 @@ options.nrCameras = 10; options.showImages = false; %% Generate data -[data,truth] = support.VisualISAMGenerateData(options); +[data,truth] = gtsam_utils.VisualISAMGenerateData(options); measurementNoiseSigma = 1.0; pointNoiseSigma = 0.1; diff --git a/matlab/examples/SFMExample.m b/matlab/gtsam_examples/SFMExample.m similarity index 94% rename from matlab/examples/SFMExample.m rename to matlab/gtsam_examples/SFMExample.m index 2153ba647..7f583b386 100644 --- a/matlab/examples/SFMExample.m +++ b/matlab/gtsam_examples/SFMExample.m @@ -22,7 +22,7 @@ options.nrCameras = 10; options.showImages = false; %% Generate data -[data,truth] = support.VisualISAMGenerateData(options); +[data,truth] = gtsam_utils.VisualISAMGenerateData(options); measurementNoiseSigma = 1.0; pointNoiseSigma = 0.1; @@ -86,8 +86,8 @@ marginals = Marginals(graph, result); cla hold on; -plot3DPoints(result, [], marginals); -plot3DTrajectory(result, '*', 1, 8, marginals); +gtsam_utils.plot3DPoints(result, [], marginals); +gtsam_utils.plot3DTrajectory(result, '*', 1, 8, marginals); axis([-40 40 -40 40 -10 20]);axis equal view(3) diff --git a/matlab/examples/StereoVOExample.m b/matlab/gtsam_examples/StereoVOExample.m similarity index 94% rename from matlab/examples/StereoVOExample.m rename to matlab/gtsam_examples/StereoVOExample.m index 468123529..e62c4395c 100644 --- a/matlab/examples/StereoVOExample.m +++ b/matlab/gtsam_examples/StereoVOExample.m @@ -18,6 +18,7 @@ % - No noise on measurements %% Create keys for variables +import gtsam.* x1 = symbol('x',1); x2 = symbol('x',2); l1 = symbol('l',1); l2 = symbol('l',2); l3 = symbol('l',3); @@ -74,6 +75,6 @@ axis equal view(-38,12) camup([0;1;0]); -plot3DTrajectory(initialEstimate, 'r', 1, 0.3); -plot3DTrajectory(result, 'g', 1, 0.3); -plot3DPoints(result); +gtsam_utils.plot3DTrajectory(initialEstimate, 'r', 1, 0.3); +gtsam_utils.plot3DTrajectory(result, 'g', 1, 0.3); +gtsam_utils.plot3DPoints(result); diff --git a/matlab/examples/StereoVOExample_large.m b/matlab/gtsam_examples/StereoVOExample_large.m similarity index 84% rename from matlab/examples/StereoVOExample_large.m rename to matlab/gtsam_examples/StereoVOExample_large.m index fb45dd8a7..5ae3a8fd7 100644 --- a/matlab/examples/StereoVOExample_large.m +++ b/matlab/gtsam_examples/StereoVOExample_large.m @@ -13,7 +13,7 @@ %% Load calibration import gtsam.* % format: fx fy skew cx cy baseline -calib = dlmread(support.findExampleDataFile('VO_calibration.txt')); +calib = dlmread(gtsam_utils.findExampleDataFile('VO_calibration.txt')); K = Cal3_S2Stereo(calib(1), calib(2), calib(3), calib(4), calib(5), calib(6)); stereo_model = noiseModel.Diagonal.Sigmas([1.0; 1.0; 1.0]); @@ -26,7 +26,7 @@ initial = Values; % row format: camera_id 4x4 pose (row, major) import gtsam.* fprintf(1,'Reading data\n'); -cameras = dlmread(support.findExampleDataFile('VO_camera_poses_large.txt')); +cameras = dlmread(gtsam_utils.findExampleDataFile('VO_camera_poses_large.txt')); for i=1:size(cameras,1) pose = Pose3(reshape(cameras(i,2:17),4,4)'); initial.insert(symbol('x',cameras(i,1)),pose); @@ -35,7 +35,7 @@ end %% load stereo measurements and initialize landmarks % camera_id landmark_id uL uR v X Y Z import gtsam.* -measurements = dlmread(support.findExampleDataFile('VO_stereo_factors_large.txt')); +measurements = dlmread(gtsam_utils.findExampleDataFile('VO_stereo_factors_large.txt')); fprintf(1,'Creating Graph\n'); tic for i=1:size(measurements,1) @@ -54,11 +54,13 @@ end toc %% add a constraint on the starting pose +import gtsam.* key = symbol('x',1); first_pose = initial.at(key); graph.add(NonlinearEqualityPose3(key, first_pose)); %% optimize +import gtsam.* fprintf(1,'Optimizing\n'); tic optimizer = LevenbergMarquardtOptimizer(graph, initial); result = optimizer.optimizeSafely(); @@ -67,9 +69,9 @@ toc %% visualize initial trajectory, final trajectory, and final points cla; hold on; -plot3DTrajectory(initial, 'r', 1, 0.5); -plot3DTrajectory(result, 'g', 1, 0.5); -plot3DPoints(result); +gtsam_utils.plot3DTrajectory(initial, 'r', 1, 0.5); +gtsam_utils.plot3DTrajectory(result, 'g', 1, 0.5); +gtsam_utils.plot3DPoints(result); axis([-5 20 -20 20 0 100]); axis equal diff --git a/matlab/examples/VisualISAMDemo.m b/matlab/gtsam_examples/VisualISAMDemo.m similarity index 100% rename from matlab/examples/VisualISAMDemo.m rename to matlab/gtsam_examples/VisualISAMDemo.m diff --git a/matlab/examples/VisualISAMExample.m b/matlab/gtsam_examples/VisualISAMExample.m similarity index 75% rename from matlab/examples/VisualISAMExample.m rename to matlab/gtsam_examples/VisualISAMExample.m index 7553ed2a4..64634d336 100644 --- a/matlab/examples/VisualISAMExample.m +++ b/matlab/gtsam_examples/VisualISAMExample.m @@ -32,17 +32,17 @@ options.saveFigures = false; options.saveDotFiles = false; %% Generate data -[data,truth] = support.VisualISAMGenerateData(options); +[data,truth] = gtsam_utils.VisualISAMGenerateData(options); %% Initialize iSAM with the first pose and points -[noiseModels,isam,result] = support.VisualISAMInitialize(data,truth,options); +[noiseModels,isam,result,nextPose] = gtsam_utils.VisualISAMInitialize(data,truth,options); cla; -support.VisualISAMPlot(truth, data, isam, result, options) +gtsam_utils.VisualISAMPlot(truth, data, isam, result, options) %% Main loop for iSAM: stepping through all poses for frame_i=3:options.nrCameras - [isam,result] = support.VisualISAMStep(data,noiseModels,isam,result,truth,options); + [isam,result,nextPose] = gtsam_utils.VisualISAMStep(data,noiseModels,isam,result,truth,nextPose); if mod(frame_i,options.drawInterval)==0 - support.VisualISAMPlot(truth, data, isam, result, options) + gtsam_utils.VisualISAMPlot(truth, data, isam, result, options) end end diff --git a/matlab/examples/VisualISAM_gui.fig b/matlab/gtsam_examples/VisualISAM_gui.fig similarity index 100% rename from matlab/examples/VisualISAM_gui.fig rename to matlab/gtsam_examples/VisualISAM_gui.fig diff --git a/matlab/examples/VisualISAM_gui.m b/matlab/gtsam_examples/VisualISAM_gui.m similarity index 92% rename from matlab/examples/VisualISAM_gui.m rename to matlab/gtsam_examples/VisualISAM_gui.m index 765a6f5ac..f7990268f 100644 --- a/matlab/examples/VisualISAM_gui.m +++ b/matlab/gtsam_examples/VisualISAM_gui.m @@ -224,32 +224,32 @@ function saveGraphsCB_Callback(hObject, ~, handles) % --- Executes on button press in intializeButton. function intializeButton_Callback(hObject, ~, handles) -global frame_i truth data noiseModels isam result options +global frame_i truth data noiseModels isam result nextPoseIndex options % initialize global options initOptions(handles) % Generate Data -[data,truth] = support.VisualISAMGenerateData(options); +[data,truth] = gtsam_utils.VisualISAMGenerateData(options); % Initialize and plot -[noiseModels,isam,result] = support.VisualISAMInitialize(data,truth,options); +[noiseModels,isam,result,nextPoseIndex] = gtsam_utils.VisualISAMInitialize(data,truth,options); cla -support.VisualISAMPlot(truth, data, isam, result, options) +gtsam_utils.VisualISAMPlot(truth, data, isam, result, options) frame_i = 2; showFramei(hObject, handles) % --- Executes on button press in runButton. function runButton_Callback(hObject, ~, handles) -global frame_i truth data noiseModels isam result options +global frame_i truth data noiseModels isam result nextPoseIndex options while (frame_i