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

Finished denamespacing and reorganizing matlab code

release/4.3a0
Richard Roberts 2012-07-27 19:02:11 +00:00
parent aef5ae269f
commit 9d2a3bf14e
63 changed files with 324 additions and 672 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

423
gtsam.h
View File

@ -1023,14 +1023,9 @@ class KalmanFilter {
//*************************************************************************
#include <gtsam/nonlinear/Symbol.h>
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 <gtsam/nonlinear/Ordering.h>
class Ordering {
@ -1415,6 +1410,8 @@ typedef gtsam::RangeFactor<gtsam::SimpleCamera, gtsam::Point3> RangeFactorSimple
typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::CalibratedCamera> RangeFactorCalibratedCamera;
typedef gtsam::RangeFactor<gtsam::SimpleCamera, gtsam::SimpleCamera> RangeFactorSimpleCamera;
#include <gtsam/slam/BearingFactor.h>
template<POSE, POINT, ROTATION>
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<gtsam::Pose2, gtsam::Point2, gtsam::Rot2> BearingFactor2D;
#include <gtsam/slam/BearingRangeFactor.h>
template<POSE, POINT, ROTATION>
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<gtsam::Pose3, gtsam::Point3> GenericStereoFactor3D;
#include <gtsam/slam/dataset.h>
pair<pose2SLAM::Graph*, pose2SLAM::Values*> load2D(string filename,
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename,
gtsam::noiseModel::Diagonal* model, int maxID, bool addNoise, bool smart);
} //\namespace gtsam
//*************************************************************************
// Pose2SLAM
//*************************************************************************
namespace pose2SLAM {
#include <gtsam/slam/pose2SLAM.h>
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 <gtsam/slam/pose2SLAM.h>
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 <gtsam/slam/pose3SLAM.h>
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 <gtsam/slam/pose3SLAM.h>
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 <gtsam/slam/planarSLAM.h>
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 <gtsam/slam/planarSLAM.h>
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 <gtsam/slam/planarSLAM.h>
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 <gtsam/slam/visualSLAM.h>
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 <gtsam/slam/visualSLAM.h>
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 <gtsam/slam/visualSLAM.h>
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 <gtsam/slam/visualSLAM.h>
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 <gtsam/slam/sparseBA.h>
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 <gtsam/slam/sparseBA.h>
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 <gtsam/slam/sparseBA.h>
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
}

9
gtsam/nonlinear/Symbol.h
View File

@ -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); }

20
gtsam/slam/dataset.cpp
View File

@ -20,8 +20,10 @@
#include <sstream>
#include <cstdlib>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/linear/Sampler.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/BetweenFactor.h>
using namespace std;
@ -78,23 +80,23 @@ pair<string, boost::optional<SharedDiagonal> > dataset(const string& dataset,
}
/* ************************************************************************* */
pair<pose2SLAM::Graph::shared_ptr, pose2SLAM::Values::shared_ptr> load2D(
pair<string, boost::optional<SharedDiagonal> > dataset,
pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
pair<string, boost::optional<noiseModel::Diagonal::shared_ptr> > dataset,
int maxID, bool addNoise, bool smart) {
return load2D(dataset.first, dataset.second, maxID, addNoise, smart);
}
/* ************************************************************************* */
pair<pose2SLAM::Graph::shared_ptr, pose2SLAM::Values::shared_ptr> load2D(
const string& filename, boost::optional<SharedDiagonal> model, int maxID,
pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
const string& filename, boost::optional<noiseModel::Diagonal::shared_ptr> 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<pose2SLAM::Graph::shared_ptr, pose2SLAM::Values::shared_ptr> load2D(
if (!poses->exists(id2))
poses->insert(id2, poses->at<Pose2>(id1) * l1Xl2);
pose2SLAM::Graph::sharedFactor factor(
NonlinearFactor::shared_ptr factor(
new BetweenFactor<Pose2>(id1, id2, l1Xl2, *model));
graph->push_back(factor);
}
@ -169,8 +171,8 @@ pair<pose2SLAM::Graph::shared_ptr, pose2SLAM::Values::shared_ptr> 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);

16
gtsam/slam/dataset.h
View File

@ -18,7 +18,9 @@
#pragma once
#include <gtsam/slam/pose2SLAM.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/linear/NoiseModel.h>
#include <string>
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<pose2SLAM::Graph::shared_ptr, pose2SLAM::Values::shared_ptr> load2D(
std::pair<std::string, boost::optional<SharedDiagonal> > dataset,
std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
std::pair<std::string, boost::optional<noiseModel::Diagonal::shared_ptr> > 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<pose2SLAM::Graph::shared_ptr, pose2SLAM::Values::shared_ptr> load2D(
std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
const std::string& filename,
boost::optional<gtsam::SharedDiagonal> 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

0
matlab/CHECK.m → matlab/+gtsam_utils/CHECK.m
View File

0
matlab/EQUALITY.m → matlab/+gtsam_utils/EQUALITY.m
View File

0
matlab/examples/+support/VisualISAMGenerateData.m → matlab/+gtsam_utils/VisualISAMGenerateData.m
View File

43
matlab/examples/+support/VisualISAMInitialize.m → matlab/+gtsam_utils/VisualISAMInitialize.m
View File

@ -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

34
matlab/examples/+support/VisualISAMPlot.m → matlab/+gtsam_utils/VisualISAMPlot.m
View File

@ -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

44
matlab/+gtsam_utils/VisualISAMStep.m Normal file
View File

@ -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;

2
matlab/circlePose2.m → matlab/+gtsam_utils/circlePose2.m
View File

@ -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;

2
matlab/circlePose3.m → matlab/+gtsam_utils/circlePose3.m
View File

@ -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);

66
matlab/covarianceEllipse.m → matlab/+gtsam_utils/covarianceEllipse.m
View File

@ -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

0
matlab/covarianceEllipse3D.m → matlab/+gtsam_utils/covarianceEllipse3D.m
View File

4
matlab/examples/+support/findExampleDataFile.m → matlab/+gtsam_utils/findExampleDataFile.m
View File

@ -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')

0
matlab/load2D.m → matlab/+gtsam_utils/load2D.m
View File

0
matlab/load3D.m → matlab/+gtsam_utils/load3D.m
View File

4
matlab/plot2DPoints.m → matlab/+gtsam_utils/plot2DPoints.m
View File

@ -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

4
matlab/plot2DTrajectory.m → matlab/+gtsam_utils/plot2DTrajectory.m
View File

@ -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

4
matlab/plot3DPoints.m → matlab/+gtsam_utils/plot3DPoints.m
View File

@ -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

4
matlab/plot3DTrajectory.m → matlab/+gtsam_utils/plot3DTrajectory.m
View File

@ -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

0
matlab/plotCamera.m → matlab/+gtsam_utils/plotCamera.m
View File

2
matlab/plotPoint2.m → matlab/+gtsam_utils/plotPoint2.m
View File

@ -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

2
matlab/plotPoint3.m → matlab/+gtsam_utils/plotPoint3.m
View File

@ -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

2
matlab/plotPose2.m → matlab/+gtsam_utils/plotPose2.m
View File

@ -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

2
matlab/plotPose3.m → matlab/+gtsam_utils/plotPose3.m
View File

@ -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

11
matlab/CMakeLists.txt
View File

@ -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

44
matlab/examples/+support/VisualISAMStep.m
View File

@ -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

2
matlab/examples/LocalizationExample.m → matlab/gtsam_examples/LocalizationExample.m
View File

@ -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

2
matlab/examples/OdometryExample.m → matlab/gtsam_examples/OdometryExample.m
View File

@ -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

4
matlab/examples/PlanarSLAMExample.m → matlab/gtsam_examples/PlanarSLAMExample.m
View File

@ -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-');

9
matlab/examples/PlanarSLAMExample_sampling.m → matlab/gtsam_examples/PlanarSLAMExample_sampling.m
View File

@ -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

4
matlab/examples/Pose2SLAMExample.m → matlab/gtsam_examples/Pose2SLAMExample.m
View File

@ -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

1
matlab/examples/Pose2SLAMExample_advanced.m → matlab/gtsam_examples/Pose2SLAMExample_advanced.m
View File

@ -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');

14
matlab/examples/Pose2SLAMExample_circle.m → matlab/gtsam_examples/Pose2SLAMExample_circle.m
View File

@ -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'));

13
matlab/examples/Pose2SLAMExample_graph.m → matlab/gtsam_examples/Pose2SLAMExample_graph.m
View File

@ -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;

0
matlab/examples/Pose2SLAMwSPCG.m → matlab/gtsam_examples/Pose2SLAMwSPCG.m
View File

14
matlab/examples/Pose3SLAMExample.m → matlab/gtsam_examples/Pose3SLAMExample.m
View File

@ -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)

11
matlab/examples/Pose3SLAMExample_graph.m → matlab/gtsam_examples/Pose3SLAMExample_graph.m
View File

@ -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;

2
matlab/examples/SBAExample.m → matlab/gtsam_examples/SBAExample.m
View File

@ -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;

6
matlab/examples/SFMExample.m → matlab/gtsam_examples/SFMExample.m
View File

@ -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)

7
matlab/examples/StereoVOExample.m → matlab/gtsam_examples/StereoVOExample.m
View File

@ -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);

14
matlab/examples/StereoVOExample_large.m → matlab/gtsam_examples/StereoVOExample_large.m
View File

@ -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

0
matlab/examples/VisualISAMDemo.m → matlab/gtsam_examples/VisualISAMDemo.m
View File

10
matlab/examples/VisualISAMExample.m → matlab/gtsam_examples/VisualISAMExample.m
View File

@ -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

0
matlab/examples/VisualISAM_gui.fig → matlab/gtsam_examples/VisualISAM_gui.fig
View File

20
matlab/examples/VisualISAM_gui.m → matlab/gtsam_examples/VisualISAM_gui.m
View File

@ -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<size(truth.cameras,2))
frame_i = frame_i+1;
showFramei(hObject, handles)
[isam,result] = support.VisualISAMStep(data,noiseModels,isam,result,truth,options);
[isam,result,nextPoseIndex] = gtsam_utils.VisualISAMStep(data,noiseModels,isam,result,truth,nextPoseIndex);
if mod(frame_i,options.drawInterval)==0
showWaiting(handles, 'Computing marginals...');
support.VisualISAMPlot(truth, data, isam, result, options)
gtsam_utils.VisualISAMPlot(truth, data, isam, result, options)
showWaiting(handles, '');
end
end
@ -257,12 +257,12 @@ end
% --- Executes on button press in stepButton.
function stepButton_Callback(hObject, ~, handles)
global frame_i truth data noiseModels isam result options
global frame_i truth data noiseModels isam result nextPoseIndex options
if (frame_i<size(truth.cameras,2))
frame_i = frame_i+1;
showFramei(hObject, handles)
[isam,result] = support.VisualISAMStep(data,noiseModels,isam,result,truth,options);
[isam,result,nextPoseIndex] = gtsam_utils.VisualISAMStep(data,noiseModels,isam,result,truth,nextPoseIndex);
showWaiting(handles, 'Computing marginals...');
support.VisualISAMPlot(truth, data, isam, result, options)
gtsam_utils.VisualISAMPlot(truth, data, isam, result, options)
showWaiting(handles, '');
end

0
matlab/examples/gtsamExamples.fig → matlab/gtsam_examples/gtsamExamples.fig
View File

0
matlab/examples/gtsamExamples.m → matlab/gtsam_examples/gtsamExamples.m
View File

1
matlab/tests/testJacobianFactor.m → matlab/gtsam_tests/testJacobianFactor.m
View File

@ -2,6 +2,7 @@
% eliminate
import gtsam.*
import gtsam_utils.*
% the combined linear factor
Ax2 = [

4
matlab/tests/testKalmanFilter.m → matlab/gtsam_tests/testKalmanFilter.m
View File

@ -57,11 +57,13 @@ x_initial = [0.0;0.0];
P_initial = 0.01*eye(2);
%% Create an KF object
import gtsam_utils.*
state = KF.init(x_initial, P_initial);
EQUALITY('expected0,state.mean', expected0,state.mean);
EQUALITY('expected0,state.mean', P00,state.covariance);
%% Run iteration 1
import gtsam_utils.*
state = KF.predict(state,F, B, u, modelQ);
EQUALITY('expected1,state.mean', expected1,state.mean);
EQUALITY('P01,state.covariance', P01,state.covariance);
@ -70,12 +72,14 @@ EQUALITY('expected1,state.mean', expected1,state.mean);
EQUALITY('I11,state.information', I11,state.information);
%% Run iteration 2
import gtsam_utils.*
state = KF.predict(state,F, B, u, modelQ);
EQUALITY('expected2,state.mean', expected2,state.mean);
state = KF.update(state,H,z2,modelR);
EQUALITY('expected2,state.mean', expected2,state.mean);
%% Run iteration 3
import gtsam_utils.*
state = KF.predict(state,F, B, u, modelQ);
EQUALITY('expected3,state.mean', expected3,state.mean);
state = KF.update(state,H,z3,modelR);

1
matlab/tests/testLocalizationExample.m → matlab/gtsam_tests/testLocalizationExample.m
View File

@ -47,6 +47,7 @@ result = optimizer.optimizeSafely();
%% Plot Covariance Ellipses
import gtsam.*
import gtsam_utils.*
marginals = Marginals(graph, result);
P={};
for i=1:result.size()

2
matlab/tests/testOdometryExample.m → matlab/gtsam_tests/testOdometryExample.m
View File

@ -35,11 +35,13 @@ initialEstimate.insert(2, Pose2(2.3, 0.1,-0.2));
initialEstimate.insert(3, Pose2(4.1, 0.1, 0.1));
%% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
import gtsam.*
optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate);
result = optimizer.optimizeSafely();
marginals = Marginals(graph, result);
marginals.marginalCovariance(1);
%% Check first pose equality
import gtsam_utils.*
pose_1 = result.at(1);
CHECK('pose_1.equals(Pose2,1e-4)',pose_1.equals(Pose2,1e-4));

2
matlab/tests/testPlanarSLAMExample.m → matlab/gtsam_tests/testPlanarSLAMExample.m
View File

@ -20,6 +20,7 @@
% - Landmarks are 2 meters away from the robot trajectory
%% Create keys for variables
import gtsam.*
i1 = symbol('x',1); i2 = symbol('x',2); i3 = symbol('x',3);
j1 = symbol('l',1); j2 = symbol('l',2);
@ -65,6 +66,7 @@ marginals = Marginals(graph, result);
%% Check first pose and point equality
import gtsam.*
import gtsam_utils.*
pose_1 = result.at(symbol('x',1));
marginals.marginalCovariance(symbol('x',1));
CHECK('pose_1.equals(Pose2,1e-4)',pose_1.equals(Pose2,1e-4));

1
matlab/tests/testPose2SLAMExample.m → matlab/gtsam_tests/testPose2SLAMExample.m
View File

@ -66,6 +66,7 @@ result = optimizer.optimizeSafely();
%% Plot Covariance Ellipses
import gtsam.*
import gtsam_utils.*
marginals = Marginals(graph, result);
P = marginals.marginalCovariance(1);

7
matlab/tests/testPose3SLAMExample.m → matlab/gtsam_tests/testPose3SLAMExample.m
View File

@ -10,14 +10,14 @@
% @author Frank Dellaert
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
import gtsam.*
%% Create a hexagon of poses
import gtsam.*
hexagon = 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,6 +41,8 @@ initial.insert(4, hexagon.at(4).retract(s*randn(6,1)));
initial.insert(5, hexagon.at(5).retract(s*randn(6,1)));
%% optimize
import gtsam.*
import gtsam_utils.*
optimizer = LevenbergMarquardtOptimizer(fg, initial);
result = optimizer.optimizeSafely;

22
matlab/tests/testSFMExample.m → matlab/gtsam_tests/testSFMExample.m
View File

@ -11,12 +11,13 @@
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
import gtsam.*
import gtsam_utils.*
options.triangle = false;
options.nrCameras = 10;
options.showImages = false;
[data,truth] = support.VisualISAMGenerateData(options);
[data,truth] = VisualISAMGenerateData(options);
measurementNoiseSigma = 1.0;
pointNoiseSigma = 0.1;
@ -37,9 +38,10 @@ end
posePriorNoise = noiseModel.Diagonal.Sigmas(poseNoiseSigmas);
graph.add(PriorFactorPose3(symbol('x',1), truth.cameras{1}.pose, posePriorNoise));
pointPriorNoise = noiseModel.Isotropic.Sigma(3,pointNoiseSigma);
graph.add(PriorFactorPose3(symbol('p',1), truth.points{1}, pointPriorNoise));
graph.add(PriorFactorPoint3(symbol('p',1), truth.points{1}, pointPriorNoise));
%% Initial estimate
import gtsam.*
initialEstimate = Values;
for i=1:size(truth.cameras,2)
pose_i = truth.cameras{i}.pose;
@ -59,14 +61,20 @@ end
result = optimizer.values();
%% Marginalization
import gtsam.*
marginals = Marginals(graph, result);
marginals.marginalCovariance(symbol('p',1));
marginals.marginalCovariance(symbol('x',1));
%% Check optimized results, should be equal to ground truth
keys = truth.keys;
for i=0:keys.size-1
truth_i = truth.at(keys.at(i));
result_i = result.at(keys.at(i));
CHECK('result_i.equals(truth_i,1e-5)',result_i.equals(truth_i,1e-5))
import gtsam.*
import gtsam_utils.*
for i=1:size(truth.cameras,2)
pose_i = result.at(symbol('x',i));
CHECK('pose_i.equals(truth.cameras{i}.pose,1e-5)',pose_i.equals(truth.cameras{i}.pose,1e-5))
end
for j=1:size(truth.points,2)
point_j = result.at(symbol('p',j));
CHECK('point_j.equals(truth.points{j},1e-5)',point_j.equals(truth.points{j},1e-5))
end

40
matlab/tests/testStereoVOExample.m → matlab/gtsam_tests/testStereoVOExample.m
View File

@ -18,16 +18,18 @@
% - 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);
%% Create graph container and add factors to it
graph = visualSLAM.Graph;
import gtsam.*
graph = NonlinearFactorGraph;
%% add a constraint on the starting pose
import gtsam.*
first_pose = Pose3();
graph.addPoseConstraint(x1, first_pose);
graph.add(NonlinearEqualityPose3(x1, first_pose));
%% Create realistic calibration and measurement noise model
% format: fx fy skew cx cy baseline
@ -38,33 +40,37 @@ stereo_model = noiseModel.Diagonal.Sigmas([1.0; 1.0; 1.0]);
%% Add measurements
import gtsam.*
% pose 1
graph.addStereoMeasurement(StereoPoint2(520, 480, 440), stereo_model, x1, l1, K);
graph.addStereoMeasurement(StereoPoint2(120, 80, 440), stereo_model, x1, l2, K);
graph.addStereoMeasurement(StereoPoint2(320, 280, 140), stereo_model, x1, l3, K);
graph.add(GenericStereoFactor3D(StereoPoint2(520, 480, 440), stereo_model, x1, l1, K));
graph.add(GenericStereoFactor3D(StereoPoint2(120, 80, 440), stereo_model, x1, l2, K));
graph.add(GenericStereoFactor3D(StereoPoint2(320, 280, 140), stereo_model, x1, l3, K));
%pose 2
graph.addStereoMeasurement(StereoPoint2(570, 520, 490), stereo_model, x2, l1, K);
graph.addStereoMeasurement(StereoPoint2( 70, 20, 490), stereo_model, x2, l2, K);
graph.addStereoMeasurement(StereoPoint2(320, 270, 115), stereo_model, x2, l3, K);
graph.add(GenericStereoFactor3D(StereoPoint2(570, 520, 490), stereo_model, x2, l1, K));
graph.add(GenericStereoFactor3D(StereoPoint2( 70, 20, 490), stereo_model, x2, l2, K));
graph.add(GenericStereoFactor3D(StereoPoint2(320, 270, 115), stereo_model, x2, l3, K));
%% Create initial estimate for camera poses and landmarks
import gtsam.*
initialEstimate = visualSLAM.Values;
initialEstimate.insertPose(x1, first_pose);
initialEstimate = Values;
initialEstimate.insert(x1, first_pose);
% noisy estimate for pose 2
initialEstimate.insertPose(x2, Pose3(Rot3(), Point3(0.1,-.1,1.1)));
initialEstimate.insert(x2, Pose3(Rot3(), Point3(0.1,-.1,1.1)));
expected_l1 = Point3( 1, 1, 5);
initialEstimate.insertPoint(l1, expected_l1);
initialEstimate.insertPoint(l2, Point3(-1, 1, 5));
initialEstimate.insertPoint(l3, Point3( 0,-.5, 5));
initialEstimate.insert(l1, expected_l1);
initialEstimate.insert(l2, Point3(-1, 1, 5));
initialEstimate.insert(l3, Point3( 0,-.5, 5));
%% optimize
result = graph.optimize(initialEstimate,0);
import gtsam.*
optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate);
result = optimizer.optimize();
%% check equality for the first pose and point
pose_x1 = result.pose(x1);
import gtsam.*
import gtsam_utils.*
pose_x1 = result.at(x1);
CHECK('pose_x1.equals(first_pose,1e-4)',pose_x1.equals(first_pose,1e-4));
point_l1 = result.point(l1);
point_l1 = result.at(l1);
CHECK('point_1.equals(expected_l1,1e-4)',point_l1.equals(expected_l1,1e-4));

11
matlab/tests/testVisualISAMExample.m → matlab/gtsam_tests/testVisualISAMExample.m
View File

@ -32,22 +32,25 @@ options.saveFigures = false;
options.saveDotFiles = false;
%% Generate data
import gtsam_utils.*
[data,truth] = VisualISAMGenerateData(options);
%% Initialize iSAM with the first pose and points
[noiseModels,isam,result] = VisualISAMInitialize(data,truth,options);
import gtsam_utils.*
[noiseModels,isam,result,nextPose] = VisualISAMInitialize(data,truth,options);
%% Main loop for iSAM: stepping through all poses
import gtsam_utils.*
for frame_i=3:options.nrCameras
[isam,result] = VisualISAMStep(data,noiseModels,isam,result,truth,options);
[isam,result,nextPose] = VisualISAMStep(data,noiseModels,isam,result,truth,nextPose);
end
for i=1:size(truth.cameras,2)
pose_i = result.pose(symbol('x',i));
pose_i = result.at(symbol('x',i));
CHECK('pose_i.equals(truth.cameras{i}.pose,1e-5)',pose_i.equals(truth.cameras{i}.pose,1e-5))
end
for j=1:size(truth.points,2)
point_j = result.point(symbol('l',j));
point_j = result.at(symbol('l',j));
CHECK('point_j.equals(truth.points{j},1e-5)',point_j.equals(truth.points{j},1e-5))
end

18
matlab/tests/test_gtsam.m → matlab/gtsam_tests/test_gtsam.m
View File

@ -9,26 +9,26 @@ testKalmanFilter
display 'Starting: testLocalizationExample'
testLocalizationExample
display 'Starting: testOdometryExample'
testOdometryExample
display 'Starting: testPlanarSLAMExample'
testPlanarSLAMExample
display 'Starting: testPose2SLAMExample'
testPose2SLAMExample
display 'Starting: testPose3SLAMExample'
testPose3SLAMExample
display 'Starting: testPlanarSLAMExample'
testPlanarSLAMExample
display 'Starting: testOdometryExample'
testOdometryExample
display 'Starting: testSFMExample'
testSFMExample
display 'Starting: testVisualISAMExample'
testVisualISAMExample
display 'Starting: testStereoVOExample'
testStereoVOExample
display 'Starting: testVisualISAMExample'
testVisualISAMExample
% end of tests
display 'Tests complete!'

4
matlab/symbol.m
View File

@ -1,4 +0,0 @@
function key = symbol(c,i)
% generate a key corresponding to a symbol
s = gtsam.Symbol(c,i);
key = s.key();

4
matlab/symbolChr.m
View File

@ -1,4 +0,0 @@
function c = symbolChr(key)
% generate the chr from a key
s = gtsam.Symbol(key);
c = s.chr();

4
matlab/symbolIndex.m
View File

@ -1,4 +0,0 @@
function i = symbolIndex(key)
% generate the index from a key
s = gtsam.Symbol(key);
i = s.index();