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

Streamlining of SLAM namespaces: 

planarSLAM Values and Graph now derive from Pose3SLAM. 
visualSLAM Values and Graph now derive from pose3SLAM.
Several methods have been renamed to make them consistent through these 4 namespaces:
addPrior -> addPosePrior
addHardConstraint -> addPoseConstraint
addOdometry/addConstraint -> addRelativePose
All gtsam and matlab examples/tests run.
PS: please don't use the deprecated typedefs in these namespaces.
release/4.3a0
Frank Dellaert 2012-06-24 02:48:12 +00:00
parent 5acc52bbae
commit 2d0ce1c3ca
55 changed files with 523 additions and 652 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
examples/LocalizationExample.cpp
View File

@ -65,8 +65,8 @@ int main(int argc, char** argv) {
// add two odometry factors
Pose2 odometry(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case)
SharedDiagonal odometryNoise = Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta
graph.addOdometry(1, 2, odometry, odometryNoise);
graph.addOdometry(2, 3, odometry, odometryNoise);
graph.addRelativePose(1, 2, odometry, odometryNoise);
graph.addRelativePose(2, 3, odometry, odometryNoise);
// add unary measurement factors, like GPS, on all three poses
SharedDiagonal noiseModel = Diagonal::Sigmas(Vector_(2, 0.1, 0.1)); // 10cm std on x,y

6
examples/OdometryExample.cpp
View File

@ -42,13 +42,13 @@ int main(int argc, char** argv) {
// add a Gaussian prior on pose x_1
Pose2 priorMean(0.0, 0.0, 0.0); // prior mean is at origin
SharedDiagonal priorNoise = Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta
graph.addPrior(1, priorMean, priorNoise); // add directly to graph
graph.addPosePrior(1, priorMean, priorNoise); // add directly to graph
// add two odometry factors
Pose2 odometry(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case)
SharedDiagonal odometryNoise = Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta
graph.addOdometry(1, 2, odometry, odometryNoise);
graph.addOdometry(2, 3, odometry, odometryNoise);
graph.addRelativePose(1, 2, odometry, odometryNoise);
graph.addRelativePose(2, 3, odometry, odometryNoise);
// print
graph.print("\nFactor graph:\n");

6
examples/PlanarSLAMExample.cpp
View File

@ -46,13 +46,13 @@ int main(int argc, char** argv) {
// add a Gaussian prior on pose x_1
Pose2 priorMean(0.0, 0.0, 0.0); // prior mean is at origin
SharedDiagonal priorNoise = Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta
graph.addPrior(i1, priorMean, priorNoise); // add directly to graph
graph.addPosePrior(i1, priorMean, priorNoise); // add directly to graph
// add two odometry factors
Pose2 odometry(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case)
SharedDiagonal odometryNoise = Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta
graph.addOdometry(i1, i2, odometry, odometryNoise);
graph.addOdometry(i2, i3, odometry, odometryNoise);
graph.addRelativePose(i1, i2, odometry, odometryNoise);
graph.addRelativePose(i2, i3, odometry, odometryNoise);
// create a noise model for the landmark measurements
SharedDiagonal measurementNoise = Diagonal::Sigmas(Vector_(2, 0.1, 0.2)); // 0.1 rad std on bearing, 20cm on range

12
examples/Pose2SLAMExample.cpp
View File

@ -32,18 +32,18 @@ int main(int argc, char** argv) {
// 2a. Add Gaussian prior
Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
SharedDiagonal priorNoise = Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1));
graph.addPrior(1, priorMean, priorNoise);
graph.addPosePrior(1, priorMean, priorNoise);
// 2b. Add odometry factors
SharedDiagonal odometryNoise = Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1));
graph.addOdometry(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise);
graph.addOdometry(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
graph.addOdometry(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
graph.addOdometry(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
graph.addRelativePose(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise);
graph.addRelativePose(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
graph.addRelativePose(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
graph.addRelativePose(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
// 2c. Add pose constraint
SharedDiagonal model = Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1));
graph.addConstraint(5, 2, Pose2(2.0, 0.0, M_PI_2), model);
graph.addRelativePose(5, 2, Pose2(2.0, 0.0, M_PI_2), model);
// print
graph.print("\nFactor graph:\n");

6
examples/Pose2SLAMExample_advanced.cpp
View File

@ -38,13 +38,13 @@ int main(int argc, char** argv) {
/* 2.a add prior */
Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
SharedDiagonal priorNoise = Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta
graph.addPrior(1, priorMean, priorNoise); // add directly to graph
graph.addPosePrior(1, priorMean, priorNoise); // add directly to graph
/* 2.b add odometry */
SharedDiagonal odometryNoise = Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta
Pose2 odometry(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case)
graph.addOdometry(1, 2, odometry, odometryNoise);
graph.addOdometry(2, 3, odometry, odometryNoise);
graph.addRelativePose(1, 2, odometry, odometryNoise);
graph.addRelativePose(2, 3, odometry, odometryNoise);
graph.print("full graph");
/* 3. Create the data structure to hold the initial estimate to the solution

2
examples/Pose2SLAMExample_graph.cpp
View File

@ -39,7 +39,7 @@ int main(int argc, char** argv) {
// Add a Gaussian prior on first poses
Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
SharedDiagonal priorNoise = Diagonal::Sigmas(Vector_(3, 0.01, 0.01, 0.01));
graph->addPrior(0, priorMean, priorNoise);
graph->addPosePrior(0, priorMean, priorNoise);
// Single Step Optimization using Levenberg-Marquardt
pose2SLAM::Values result = graph->optimize(*initial);

12
examples/Pose2SLAMwSPCG.cpp
View File

@ -37,18 +37,18 @@ int main(void) {
// 2a. Add Gaussian prior
Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
SharedDiagonal priorNoise = Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1));
graph.addPrior(1, priorMean, priorNoise);
graph.addPosePrior(1, priorMean, priorNoise);
// 2b. Add odometry factors
SharedDiagonal odometryNoise = Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1));
graph.addOdometry(1, 2, Pose2(2.0, 0.0, 0.0 ), odometryNoise);
graph.addOdometry(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
graph.addOdometry(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
graph.addOdometry(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
graph.addRelativePose(1, 2, Pose2(2.0, 0.0, 0.0 ), odometryNoise);
graph.addRelativePose(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
graph.addRelativePose(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
graph.addRelativePose(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
// 2c. Add pose constraint
SharedDiagonal constraintUncertainty = Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1));
graph.addConstraint(5, 2, Pose2(2.0, 0.0, M_PI_2), constraintUncertainty);
graph.addRelativePose(5, 2, Pose2(2.0, 0.0, M_PI_2), constraintUncertainty);
// print
graph.print("\nFactor graph:\n");

4
examples/VisualISAMExample.cpp
View File

@ -54,7 +54,7 @@ int main(int argc, char* argv[]) {
newFactors.addPosePrior(X(0), data.poses[0], data.noiseX);
// Second pose with odometry measurement
newFactors.addOdometry(X(0), X(1), data.odometry, data.noiseX);
newFactors.addRelativePose(X(0), X(1), data.odometry, data.noiseX);
// Visual measurements at both poses
for (size_t i=0; i<2; ++i) {
@ -85,7 +85,7 @@ int main(int argc, char* argv[]) {
visualSLAM::Graph newFactors;
// Factor for odometry measurements, simulated by adding Gaussian noise to the ground-truth.
Pose3 odoMeasurement = data.odometry;
newFactors.addOdometry(X(i-1), X(i), data.odometry, data.noiseX);
newFactors.addRelativePose(X(i-1), X(i), data.odometry, data.noiseX);
// Factors for visual measurements
for (size_t j=0; j<data.z[i].size(); ++j) {
newFactors.addMeasurement(data.z[i][j], data.noiseZ, X(i), L(j), data.sK);

103
gtsam.h
View File

@ -989,9 +989,7 @@ class Values {
void insertPose(size_t key, const gtsam::Pose2& pose);
void updatePose(size_t key, const gtsam::Pose2& pose);
gtsam::Pose2 pose(size_t i);
Vector xs() const;
Vector ys() const;
Vector thetas() const;
Matrix poses() const;
};
class Graph {
@ -1014,12 +1012,11 @@ class Graph {
const gtsam::Ordering& ordering) const;
// pose2SLAM-specific
void addPrior(size_t key, const gtsam::Pose2& pose, const gtsam::noiseModel::Base* noiseModel);
void addPoseConstraint(size_t key, const gtsam::Pose2& pose);
void addOdometry(size_t key1, size_t key2, const gtsam::Pose2& odometry, const gtsam::noiseModel::Base* noiseModel);
void addConstraint(size_t key1, size_t key2, const gtsam::Pose2& odometry, const gtsam::noiseModel::Base* noiseModel);
pose2SLAM::Values optimize(const pose2SLAM::Values& initialEstimate) const;
pose2SLAM::Values optimizeSPCG(const pose2SLAM::Values& initialEstimate) const;
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& odometry, 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;
};
@ -1036,13 +1033,12 @@ class Values {
Values();
size_t size() const;
void print(string s) const;
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);
Vector xs() const;
Vector ys() const;
Vector zs() const;
Matrix translations() const;
};
class Graph {
@ -1065,10 +1061,11 @@ class Graph {
const gtsam::Ordering& ordering) const;
// pose3SLAM-specific
void addPrior(size_t key, const gtsam::Pose3& p, const gtsam::noiseModel::Base* model);
void addConstraint(size_t key1, size_t key2, const gtsam::Pose3& z, const gtsam::noiseModel::Base* model);
void addHardConstraint(size_t i, const gtsam::Pose3& p);
pose3SLAM::Values optimize(const pose3SLAM::Values& initialEstimate) const;
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;
};
@ -1085,12 +1082,17 @@ class Values {
Values();
size_t size() const;
void print(string s) const;
static planarSLAM::Values Circle(size_t n, double R);
void insertPose(size_t key, const gtsam::Pose2& pose);
void insertPoint(size_t key, const gtsam::Point2& point);
void updatePose(size_t key, const gtsam::Pose2& pose);
gtsam::Pose2 pose(size_t i);
Matrix poses() const;
void insertPoint(size_t key, const gtsam::Point2& point);
void updatePoint(size_t key, const gtsam::Point2& point);
gtsam::Pose2 pose(size_t key) const;
gtsam::Point2 point(size_t key) const;
Matrix points() const;
};
class Graph {
@ -1112,15 +1114,20 @@ class Graph {
gtsam::GaussianFactorGraph* linearize(const planarSLAM::Values& values,
const gtsam::Ordering& ordering) const;
// planarSLAM-specific
void addPrior(size_t key, const gtsam::Pose2& pose, const gtsam::noiseModel::Base* noiseModel);
// pose2SLAM-inherited
void addPoseConstraint(size_t key, const gtsam::Pose2& pose);
void addOdometry(size_t key1, size_t key2, const gtsam::Pose2& odometry, const gtsam::noiseModel::Base* noiseModel);
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& odometry, 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);
planarSLAM::Values optimize(const planarSLAM::Values& initialEstimate);
gtsam::Marginals marginals(const planarSLAM::Values& solution) const;
};
class Odometry {
@ -1142,23 +1149,26 @@ namespace visualSLAM {
class Values {
Values();
void insertPose(size_t key, const gtsam::Pose3& pose);
void insertPoint(size_t key, const gtsam::Point3& pose);
void updatePose(size_t key, const gtsam::Pose3& pose);
void updatePoint(size_t key, const gtsam::Point3& pose);
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;
// visualSLAM specific
void insertPoint(size_t key, const gtsam::Point3& pose);
void updatePoint(size_t key, const gtsam::Point3& pose);
size_t nrPoses() const;
size_t nrPoints() const;
void print(string s) const;
gtsam::Pose3 pose(size_t i);
gtsam::Point3 point(size_t j);
visualSLAM::Values allPoses() const;
visualSLAM::Values allPoints() const;
gtsam::KeyVector keys() const; // Note the switch to KeyVector, rather than KeyList
bool exists(size_t key);
Vector xs() const;
Vector ys() const;
Vector zs() const;
Matrix points() const;
};
@ -1179,25 +1189,24 @@ class Graph {
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 addStereoMeasurement(const gtsam::StereoPoint2& measured, const gtsam::noiseModel::Base* model,
size_t poseKey, size_t pointKey, const gtsam::Cal3_S2Stereo* K);
// Constraints
void addPoseConstraint(size_t poseKey, const gtsam::Pose3& p);
void addPointConstraint(size_t pointKey, const gtsam::Point3& p);
// Priors
void addPosePrior(size_t poseKey, const gtsam::Pose3& p, const gtsam::noiseModel::Base* model);
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);
void addOdometry(size_t poseKey1, size_t poseKey2, const gtsam::Pose3& odometry, const gtsam::noiseModel::Base* model);
visualSLAM::Values optimize(const visualSLAM::Values& initialEstimate, size_t verbosity) const;
gtsam::Marginals marginals(const visualSLAM::Values& solution) const;
visualSLAM::LevenbergMarquardtOptimizer optimizer(const visualSLAM::Values& initialEstimate, const gtsam::LevenbergMarquardtParams& parameters) const;
};
class ISAM {

6
gtsam/slam/dataset.cpp
View File

@ -158,7 +158,7 @@ pair<pose2SLAM::Graph::shared_ptr, pose2SLAM::Values::shared_ptr> load2D(
poses->insert(id2, poses->at<Pose2>(id1) * l1Xl2);
pose2SLAM::Graph::sharedFactor factor(
new pose2SLAM::Odometry(id1, id2, l1Xl2, *model));
new BetweenFactor<Pose2>(id1, id2, l1Xl2, *model));
graph->push_back(factor);
}
is.ignore(LINESIZE, '\n');
@ -189,8 +189,8 @@ void save2D(const pose2SLAM::Graph& graph, const Values& config,
Matrix RR = trans(R) * R; //prod(trans(R),R);
BOOST_FOREACH(boost::shared_ptr<NonlinearFactor> factor_, graph)
{
boost::shared_ptr<pose2SLAM::Odometry> factor =
boost::dynamic_pointer_cast<pose2SLAM::Odometry>(factor_);
boost::shared_ptr<BetweenFactor<Pose2> > factor =
boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor_);
if (!factor)
continue;

38
gtsam/slam/planarSLAM.cpp
View File

@ -22,49 +22,39 @@
namespace planarSLAM {
/* ************************************************************************* */
Graph::Graph(const NonlinearFactorGraph& graph) :
NonlinearFactorGraph(graph) {}
/* ************************************************************************* */
void Graph::addPrior(Key i, const Pose2& p, const SharedNoiseModel& model) {
sharedFactor factor(new Prior(i, p, model));
push_back(factor);
Matrix Values::points() const {
size_t j=0;
ConstFiltered<Point2> points = filter<Point2>();
Matrix result(points.size(),2);
BOOST_FOREACH(const ConstFiltered<Point2>::KeyValuePair& keyValue, points)
result.row(j++) = keyValue.value.vector();
return result;
}
/* ************************************************************************* */
void Graph::addPoseConstraint(Key i, const Pose2& p) {
sharedFactor factor(new Constraint(i, p));
push_back(factor);
void Graph::addPointConstraint(Key pointKey, const Point2& p) {
push_back(boost::make_shared<NonlinearEquality<Point2> >(pointKey, p));
}
/* ************************************************************************* */
void Graph::addOdometry(Key i1, Key i2, const Pose2& odometry, const SharedNoiseModel& model) {
sharedFactor factor(new Odometry(i1, i2, odometry, model));
push_back(factor);
void Graph::addPointPrior(Key pointKey, const Point2& p, const SharedNoiseModel& model) {
push_back(boost::make_shared<PriorFactor<Point2> >(pointKey, p, model));
}
/* ************************************************************************* */
void Graph::addBearing(Key i, Key j, const Rot2& z, const SharedNoiseModel& model) {
sharedFactor factor(new Bearing(i, j, z, model));
push_back(factor);
push_back(boost::make_shared<BearingFactor<Pose2, Point2> >(i, j, z, model));
}
/* ************************************************************************* */
void Graph::addRange(Key i, Key j, double z, const SharedNoiseModel& model) {
sharedFactor factor(new Range(i, j, z, model));
push_back(factor);
push_back(boost::make_shared<RangeFactor<Pose2, Point2> >(i, j, z, model));
}
/* ************************************************************************* */
void Graph::addBearingRange(Key i, Key j, const Rot2& z1,
double z2, const SharedNoiseModel& model) {
sharedFactor factor(new BearingRange(i, j, z1, z2, model));
push_back(factor);
}
/* ************************************************************************* */
Values Graph::optimize(const Values& initialEstimate) const {
return LevenbergMarquardtOptimizer(*this, initialEstimate).optimize();
push_back(boost::make_shared<BearingRangeFactor<Pose2, Point2> >(i, j, z1, z2, model));
}
/* ************************************************************************* */

73
gtsam/slam/planarSLAM.h
View File

@ -17,6 +17,7 @@
#pragma once
#include <gtsam/slam/pose2SLAM.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/RangeFactor.h>
@ -33,75 +34,50 @@ namespace planarSLAM {
using namespace gtsam;
/*
* List of typedefs for factors
*/
/// A hard constraint for poses to enforce particular values
typedef NonlinearEquality<Pose2> Constraint;
/// A prior factor to bias the value of a pose
typedef PriorFactor<Pose2> Prior;
/// A factor between two poses set with a Pose2
typedef BetweenFactor<Pose2> Odometry;
/// A factor between a pose and a point to express difference in rotation (set with a Rot2)
typedef BearingFactor<Pose2, Point2> Bearing;
/// A factor between a pose and a point to express distance between them (set with a double)
typedef RangeFactor<Pose2, Point2> Range;
/// A factor between a pose and a point to express difference in rotation and location
typedef BearingRangeFactor<Pose2, Point2> BearingRange;
/*
* Values class, inherited from Values, mainly used as a convenience for MATLAB wrapper
* Values class, inherited from pose2SLAM::Values, mainly used as a convenience for MATLAB wrapper
* @addtogroup SLAM
*/
struct Values: public gtsam::Values {
struct Values: public pose2SLAM::Values {
/// Default constructor
Values() {}
/// Copy constructor
Values(const gtsam::Values& values) :
gtsam::Values(values) {
pose2SLAM::Values(values) {
}
/// get a pose
Pose2 pose(Key i) const { return at<Pose2>(i); }
/// get a point
Point2 point(Key j) const { return at<Point2>(j); }
/// insert a pose
void insertPose(Key i, const Pose2& pose) { insert(i, pose); }
/// insert a point
void insertPoint(Key j, const Point2& point) { insert(j, point); }
/// update a pose
void updatePose(Key i, const Pose2& pose) { update(i, pose); }
/// update a point
void updatePoint(Key j, const Point2& point) { update(j, point); }
/// get all [x,y] coordinates in a 2*n matrix
Matrix points() const;
};
/**
* Graph class, inherited from NonlinearFactorGraph, used as a convenience for MATLAB wrapper
* @addtogroup SLAM
*/
struct Graph: public NonlinearFactorGraph {
struct Graph: public pose2SLAM::Graph {
/// Default constructor for a NonlinearFactorGraph
Graph(){}
/// Creates a NonlinearFactorGraph based on another NonlinearFactorGraph
Graph(const NonlinearFactorGraph& graph);
Graph(const NonlinearFactorGraph& graph):
pose2SLAM::Graph(graph) {}
/// Biases the value of pose key with Pose2 p given a noise model
void addPrior(Key i, const Pose2& pose, const SharedNoiseModel& noiseModel);
/// Creates a hard constraint on a point
void addPointConstraint(Key j, const Point2& p);
/// Creates a hard constraint on the ith pose
void addPoseConstraint(Key i, const Pose2& pose);
/// Creates an odometry factor between poses with keys i1 and i2
void addOdometry(Key i1, Key i2, const Pose2& odometry, const SharedNoiseModel& model);
/// Adds a prior with mean p and given noise model on point j
void addPointPrior(Key j, const Point2& p, const SharedNoiseModel& model);
/// Creates a bearing measurement from pose i to point j
void addBearing(Key i, Key j, const Rot2& bearing, const SharedNoiseModel& model);
@ -111,16 +87,21 @@ namespace planarSLAM {
/// Creates a range/bearing measurement from pose i to point j
void addBearingRange(Key i, Key j, const Rot2& bearing, double range, const SharedNoiseModel& model);
/// Optimize
Values optimize(const Values& initialEstimate) const;
/// Return a Marginals object
Marginals marginals(const Values& solution) const {
return Marginals(*this,solution);
}
};
} // planarSLAM
/**
* Backwards compatibility and wrap use only, avoid using
*/
namespace planarSLAM {
typedef gtsam::NonlinearEquality<Pose2> Constraint; ///< \deprecated typedef for backwards compatibility
typedef gtsam::PriorFactor<Pose2> Prior; ///< \deprecated typedef for backwards compatibility
typedef gtsam::BetweenFactor<Pose2> Odometry; ///< \deprecated typedef for backwards compatibility
typedef gtsam::BearingFactor<Pose2, Point2> Bearing; ///< \deprecated typedef for backwards compatibility
typedef gtsam::RangeFactor<Pose2, Point2> Range; ///< \deprecated typedef for backwards compatibility
typedef gtsam::BearingRangeFactor<Pose2, Point2> BearingRange; ///< \deprecated typedef for backwards compatibility
}

61
gtsam/slam/pose2SLAM.cpp
View File

@ -17,7 +17,6 @@
#include <gtsam/linear/SimpleSPCGSolver.h>
#include <gtsam/slam/pose2SLAM.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
// Use pose2SLAM namespace for specific SLAM instance
@ -33,67 +32,53 @@ namespace pose2SLAM {
}
/* ************************************************************************* */
Vector Values::xs() const {
Matrix Values::poses() const {
size_t j=0;
Vector result(size());
ConstFiltered<Pose2> poses = filter<Pose2>();
BOOST_FOREACH(const ConstFiltered<Pose2>::KeyValuePair& keyValue, poses)
result(j++) = keyValue.value.x();
Matrix result(poses.size(),3);
BOOST_FOREACH(const ConstFiltered<Pose2>::KeyValuePair& keyValue, poses) {
const Pose2& r = keyValue.value;
result.row(j++) = Matrix_(1,3, r.x(), r.y(), r.theta());
}
return result;
}
/* ************************************************************************* */
Vector Values::ys() const {
size_t j=0;
Vector result(size());
ConstFiltered<Pose2> poses = filter<Pose2>();
BOOST_FOREACH(const ConstFiltered<Pose2>::KeyValuePair& keyValue, poses)
result(j++) = keyValue.value.y();
return result;
}
/* ************************************************************************* */
Vector Values::thetas() const {
size_t j=0;
Vector result(size());
ConstFiltered<Pose2> poses = filter<Pose2>();
BOOST_FOREACH(const ConstFiltered<Pose2>::KeyValuePair& keyValue, poses)
result(j++) = keyValue.value.theta ();
return result;
}
/* ************************************************************************* */
void Graph::addPrior(Key i, const Pose2& p, const SharedNoiseModel& model) {
sharedFactor factor(new Prior(i, p, model));
push_back(factor);
}
/* ************************************************************************* */
void Graph::addPoseConstraint(Key i, const Pose2& p) {
sharedFactor factor(new HardConstraint(i, p));
sharedFactor factor(new NonlinearEquality<Pose2>(i, p));
push_back(factor);
}
/* ************************************************************************* */
void Graph::addOdometry(Key i1, Key i2, const Pose2& z,
void Graph::addPosePrior(Key i, const Pose2& p, const SharedNoiseModel& model) {
sharedFactor factor(new PriorFactor<Pose2>(i, p, model));
push_back(factor);
}
/* ************************************************************************* */
void Graph::addRelativePose(Key i1, Key i2, const Pose2& z,
const SharedNoiseModel& model) {
sharedFactor factor(new Odometry(i1, i2, z, model));
sharedFactor factor(new BetweenFactor<Pose2>(i1, i2, z, model));
push_back(factor);
}
/* ************************************************************************* */
Values Graph::optimize(const Values& initialEstimate) const {
return LevenbergMarquardtOptimizer(*this, initialEstimate).optimize();
Values Graph::optimize(const Values& initialEstimate, size_t verbosity) const {
LevenbergMarquardtParams params;
params.verbosity = (NonlinearOptimizerParams::Verbosity)verbosity;
LevenbergMarquardtOptimizer optimizer(*this, initialEstimate,params);
return optimizer.optimize();
}
Values Graph::optimizeSPCG(const Values& initialEstimate) const {
/* ************************************************************************* */
Values Graph::optimizeSPCG(const Values& initialEstimate, size_t verbosity) const {
LevenbergMarquardtParams params;
params.verbosity = (NonlinearOptimizerParams::Verbosity)verbosity;
params.linearSolverType = SuccessiveLinearizationParams::CG;
params.iterativeParams = boost::make_shared<SimpleSPCGSolverParameters>();
return LevenbergMarquardtOptimizer(*this, initialEstimate, params).optimize();
}
/* ************************************************************************* */
} // pose2SLAM

47
gtsam/slam/pose2SLAM.h
View File

@ -22,13 +22,14 @@
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/geometry/Pose2.h>
// Use pose2SLAM namespace for specific SLAM instance
namespace pose2SLAM {
using namespace gtsam;
using namespace gtsam;
/*
* Values class, inherited from Values, mainly used as a convenience for MATLAB wrapper
@ -64,22 +65,14 @@ namespace pose2SLAM {
/// get a pose
Pose2 pose(Key i) const { return at<Pose2>(i); }
Vector xs() const; ///< get all x coordinates in a matrix
Vector ys() const; ///< get all y coordinates in a matrix
Vector thetas() const; ///< get all orientations in a matrix
/// get all [x,y,theta] coordinates in a 3*m matrix
Matrix poses() const;
};
/**
* List of typedefs for factors
*/
/// A hard constraint to enforce a specific value for a pose
typedef NonlinearEquality<Pose2> HardConstraint;
/// A prior factor on a pose with Pose2 data type.
typedef PriorFactor<Pose2> Prior;
/// A factor to add an odometry measurement between two poses.
typedef BetweenFactor<Pose2> Odometry;
/**
* Graph class, inherited from NonlinearFactorGraph, used as a convenience for MATLAB wrapper
* @addtogroup SLAM
@ -92,25 +85,23 @@ namespace pose2SLAM {
Graph(){}
/// Creates a NonlinearFactorGraph based on another NonlinearFactorGraph
Graph(const NonlinearFactorGraph& graph);
/// Adds a Pose2 prior with mean p and given noise model on pose i
void addPrior(Key i, const Pose2& p, const SharedNoiseModel& model);
Graph(const NonlinearFactorGraph& graph):
NonlinearFactorGraph(graph) {}
/// Creates a hard constraint for key i with the given Pose2 p.
void addPoseConstraint(Key i, const Pose2& p);
/// Creates an odometry factor between poses with keys i1 and i2
void addOdometry(Key i1, Key i2, const Pose2& z, const SharedNoiseModel& model);
/// Adds a Pose2 prior with mean p and given noise model on pose i
void addPosePrior(Key i, const Pose2& p, const SharedNoiseModel& model);
/// AddConstraint adds a soft constraint between factor between keys i and j
void addConstraint(Key i1, Key i2, const Pose2& z, const SharedNoiseModel& model) {
addOdometry(i1,i2,z,model); // same for now
}
/// Creates an relative pose factor between poses with keys i1 and i2
void addRelativePose(Key i1, Key i2, const Pose2& z, const SharedNoiseModel& model);
/// Optimize
Values optimize(const Values& initialEstimate) const;
Values optimizeSPCG(const Values& initialEstimate) const;
Values optimize(const Values& initialEstimate, size_t verbosity=NonlinearOptimizerParams::SILENT) const;
/// Optimize using subgraph preconditioning
Values optimizeSPCG(const Values& initialEstimate, size_t verbosity=NonlinearOptimizerParams::SILENT) const;
/// Return a Marginals object
Marginals marginals(const Values& solution) const {
@ -121,5 +112,11 @@ namespace pose2SLAM {
} // pose2SLAM
/**
* Backwards compatibility and wrap use only, avoid using
*/
namespace pose2SLAM {
typedef gtsam::NonlinearEquality<Pose2> HardConstraint; ///< \deprecated typedef for backwards compatibility
typedef gtsam::PriorFactor<Pose2> Prior; ///< \deprecated typedef for backwards compatibility
typedef gtsam::BetweenFactor<Pose2> Odometry; ///< \deprecated typedef for backwards compatibility
}

57
gtsam/slam/pose3SLAM.cpp
View File

@ -42,51 +42,38 @@ namespace pose3SLAM {
}
/* ************************************************************************* */
Vector Values::xs() const {
size_t j=0;
Vector result(size());
ConstFiltered<Pose3> poses = filter<Pose3>();
BOOST_FOREACH(const ConstFiltered<Pose3>::KeyValuePair& keyValue, poses)
result(j++) = keyValue.value.x();
return result;
}
Matrix Values::translations() const {
size_t j=0;
ConstFiltered<Pose3> poses = filter<Pose3>();
Matrix result(poses.size(),3);
BOOST_FOREACH(const ConstFiltered<Pose3>::KeyValuePair& keyValue, poses)
result.row(j++) = keyValue.value.translation().vector();
return result;
}
/* ************************************************************************* */
Vector Values::ys() const {
size_t j=0;
Vector result(size());
ConstFiltered<Pose3> poses = filter<Pose3>();
BOOST_FOREACH(const ConstFiltered<Pose3>::KeyValuePair& keyValue, poses)
result(j++) = keyValue.value.y();
return result;
}
/* ************************************************************************* */
Vector Values::zs() const {
size_t j=0;
Vector result(size());
ConstFiltered<Pose3> poses = filter<Pose3>();
BOOST_FOREACH(const ConstFiltered<Pose3>::KeyValuePair& keyValue, poses)
result(j++) = keyValue.value.z();
return result;
}
/* ************************************************************************* */
void Graph::addPrior(Key i, const Pose3& p, const SharedNoiseModel& model) {
sharedFactor factor(new Prior(i, p, model));
void Graph::addPoseConstraint(Key i, const Pose3& p) {
sharedFactor factor(new NonlinearEquality<Pose3>(i, p));
push_back(factor);
}
/* ************************************************************************* */
void Graph::addConstraint(Key i1, Key i2, const Pose3& z, const SharedNoiseModel& model) {
sharedFactor factor(new Constraint(i1, i2, z, model));
void Graph::addPosePrior(Key i, const Pose3& p, const SharedNoiseModel& model) {
sharedFactor factor(new PriorFactor<Pose3>(i, p, model));
push_back(factor);
}
/* ************************************************************************* */
void Graph::addHardConstraint(Key i, const Pose3& p) {
sharedFactor factor(new HardConstraint(i, p));
push_back(factor);
void Graph::addRelativePose(Key i1, Key i2, const Pose3& z, const SharedNoiseModel& model) {
push_back(boost::make_shared<BetweenFactor<Pose3> >(i1, i2, z, model));
}
/* ************************************************************************* */
Values Graph::optimize(const Values& initialEstimate, size_t verbosity) const {
LevenbergMarquardtParams params;
params.verbosity = (NonlinearOptimizerParams::Verbosity)verbosity;
LevenbergMarquardtOptimizer optimizer(*this, initialEstimate,params);
return optimizer.optimize();
}
/* ************************************************************************* */

87
gtsam/slam/pose3SLAM.h
View File

@ -19,16 +19,16 @@
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/geometry/Pose3.h>
/// Use pose3SLAM namespace for specific SLAM instance
namespace pose3SLAM {
using namespace gtsam;
using namespace gtsam;
/*
* Values class, inherited from Values, mainly used as a convenience for MATLAB wrapper
@ -63,17 +63,11 @@ namespace pose3SLAM {
/// get a pose
Pose3 pose(Key i) const { return at<Pose3>(i); }
Vector xs() const; ///< get all x coordinates in a matrix
Vector ys() const; ///< get all y coordinates in a matrix
Vector zs() const; ///< get all z coordinates in a matrix
};
/// check if value with specified key exists
bool exists(Key i) const { return gtsam::Values::exists(i); }
/// A prior factor on Key with Pose3 data type.
typedef PriorFactor<Pose3> Prior;
/// A factor to put constraints between two factors.
typedef BetweenFactor<Pose3> Constraint;
/// A hard constraint would enforce that the given key would have the input value in the results.
typedef NonlinearEquality<Pose3> HardConstraint;
Matrix translations() const; ///< get all pose translations coordinates in a matrix
};
/**
* Graph class, inherited from NonlinearFactorGraph, used as a convenience for MATLAB wrapper
@ -81,33 +75,70 @@ namespace pose3SLAM {
*/
struct Graph: public NonlinearFactorGraph {
/// Adds a factor between keys of the same type
void addPrior(Key i, const Pose3& p, const SharedNoiseModel& model);
/**
* Add a prior on a pose
* @param key variable key of the camera pose
* @param p around which soft prior is defined
* @param model uncertainty model of this prior
*/
void addPosePrior(Key poseKey, const Pose3& p = Pose3(), const SharedNoiseModel& model = noiseModel::Unit::Create(6));
/// Creates a between factor between keys i and j with a noise model with Pos3 z in the graph
void addConstraint(Key i1, Key i2, const Pose3& z, const SharedNoiseModel& model);
/**
* Add a constraint on a pose (for now, *must* be satisfied in any Values)
* @param key variable key of the camera pose
* @param p to which pose to constrain it to
*/
void addPoseConstraint(Key poseKey, const Pose3& p = Pose3());
/// Creates a hard constraint for key i with the given Pose3 p.
void addHardConstraint(Key i, const Pose3& p);
/**
* Add a relative pose measurement between two poses
* @param x1 variable key of the first camera pose
* @param x2 variable key of the second camera pose
* @param relative pose measurement from x1 to x2 (x1.between(x2))
* @param model uncertainty model of this measurement
*/
void addRelativePose(Key x1, Key x2, const Pose3& z, const SharedNoiseModel& model);
/// Optimize
Values optimize(const Values& initialEstimate) {
return LevenbergMarquardtOptimizer(*this, initialEstimate).optimize();
}
/**
* Optimize the graph
* @param initialEstimate initial estimate of all variables in the graph
* @param pointKey variable key of the landmark
* @param range approximate range to landmark
* @param model uncertainty model of this prior
*/
Values optimize(const Values& initialEstimate, size_t verbosity=NonlinearOptimizerParams::SILENT) const;
/**
* Setup and return a LevenbargMarquardtOptimizer
* @param initialEstimate initial estimate of all variables in the graph
* @param parameters optimizer's parameters
* @return a LevenbergMarquardtOptimizer object, which you can use to control the optimization process
*/
LevenbergMarquardtOptimizer optimizer(const Values& initialEstimate,
const LevenbergMarquardtParams& parameters = LevenbergMarquardtParams()) const {
return LevenbergMarquardtOptimizer((*this), initialEstimate, parameters);
}
/// Return a Marginals object
Marginals marginals(const Values& solution) const {
return Marginals(*this,solution);
return Marginals(*this,solution);
}
};
} // pose3SLAM
/**
* Backwards compatibility
* Backwards compatibility and wrap use only, avoid using
*/
namespace gtsam {
typedef pose3SLAM::Prior Pose3Prior; ///< Typedef for Prior class for backwards compatibility
typedef pose3SLAM::Constraint Pose3Factor; ///< Typedef for Constraint class for backwards compatibility
typedef pose3SLAM::Graph Pose3Graph; ///< Typedef for Graph class for backwards compatibility
namespace pose3SLAM {
typedef gtsam::PriorFactor<Pose3> Prior; ///< \deprecated typedef for backwards compatibility
typedef gtsam::BetweenFactor<Pose3> Constraint; ///< \deprecated typedef for backwards compatibility
typedef gtsam::NonlinearEquality<Pose3> HardConstraint; ///< \deprecated typedef for backwards compatibility
}
namespace gtsam {
typedef pose3SLAM::Prior Pose3Prior; ///< \deprecated typedef for backwards compatibility
typedef pose3SLAM::Constraint Pose3Factor; ///< \deprecated typedef for backwards compatibility
typedef pose3SLAM::Graph Pose3Graph; ///< \deprecated typedef for backwards compatibility
}

6
gtsam/slam/tests/testAntiFactor.cpp
View File

@ -96,8 +96,8 @@ TEST( AntiFactor, EquivalentBayesNet)
SharedNoiseModel sigma(noiseModel::Unit::Create(Pose3::Dim()));
boost::shared_ptr<pose3SLAM::Graph> graph(new pose3SLAM::Graph());
graph->addPrior(1, pose1, sigma);
graph->addConstraint(1, 2, pose1.between(pose2), sigma);
graph->addPosePrior(1, pose1, sigma);
graph->addRelativePose(1, 2, pose1.between(pose2), sigma);
// Create a configuration corresponding to the ground truth
boost::shared_ptr<Values> values(new Values());
@ -115,7 +115,7 @@ TEST( AntiFactor, EquivalentBayesNet)
VectorValues expectedDeltas = optimize(*expectedBayesNet);
// Add an additional factor between Pose1 and Pose2
pose3SLAM::Constraint::shared_ptr f1(new pose3SLAM::Constraint(1, 2, z, sigma));
BetweenFactor<Pose3>::shared_ptr f1(new BetweenFactor<Pose3>(1, 2, z, sigma));
graph->push_back(f1);
// Add the corresponding AntiFactor between Pose1 and Pose2

18
gtsam/slam/tests/testPlanarSLAM.cpp
View File

@ -38,7 +38,7 @@ SharedNoiseModel
/* ************************************************************************* */
TEST( planarSLAM, PriorFactor_equals )
{
planarSLAM::Prior factor1(i2, x1, I3), factor2(i2, x2, I3);
PriorFactor<Pose2> factor1(i2, x1, I3), factor2(i2, x2, I3);
EXPECT(assert_equal(factor1, factor1, 1e-5));
EXPECT(assert_equal(factor2, factor2, 1e-5));
EXPECT(assert_inequal(factor1, factor2, 1e-5));
@ -49,7 +49,7 @@ TEST( planarSLAM, BearingFactor )
{
// Create factor
Rot2 z = Rot2::fromAngle(M_PI_4 + 0.1); // h(x) - z = -0.1
planarSLAM::Bearing factor(i2, j3, z, sigma);
BearingFactor<Pose2, Point2> factor(i2, j3, z, sigma);
// create config
planarSLAM::Values c;
@ -64,7 +64,7 @@ TEST( planarSLAM, BearingFactor )
/* ************************************************************************* */
TEST( planarSLAM, BearingFactor_equals )
{
planarSLAM::Bearing
BearingFactor<Pose2, Point2>
factor1(i2, j3, Rot2::fromAngle(0.1), sigma),
factor2(i2, j3, Rot2::fromAngle(2.3), sigma);
EXPECT(assert_equal(factor1, factor1, 1e-5));
@ -77,7 +77,7 @@ TEST( planarSLAM, RangeFactor )
{
// Create factor
double z(sqrt(2.0) - 0.22); // h(x) - z = 0.22
planarSLAM::Range factor(i2, j3, z, sigma);
RangeFactor<Pose2, Point2> factor(i2, j3, z, sigma);
// create config
planarSLAM::Values c;
@ -92,7 +92,7 @@ TEST( planarSLAM, RangeFactor )
/* ************************************************************************* */
TEST( planarSLAM, RangeFactor_equals )
{
planarSLAM::Range factor1(i2, j3, 1.2, sigma), factor2(i2, j3, 7.2, sigma);
RangeFactor<Pose2, Point2> factor1(i2, j3, 1.2, sigma), factor2(i2, j3, 7.2, sigma);
EXPECT(assert_equal(factor1, factor1, 1e-5));
EXPECT(assert_equal(factor2, factor2, 1e-5));
EXPECT(assert_inequal(factor1, factor2, 1e-5));
@ -104,7 +104,7 @@ TEST( planarSLAM, BearingRangeFactor )
// Create factor
Rot2 r = Rot2::fromAngle(M_PI_4 + 0.1); // h(x) - z = -0.1
double b(sqrt(2.0) - 0.22); // h(x) - z = 0.22
planarSLAM::BearingRange factor(i2, j3, r, b, sigma2);
BearingRangeFactor<Pose2, Point2> factor(i2, j3, r, b, sigma2);
// create config
planarSLAM::Values c;
@ -119,7 +119,7 @@ TEST( planarSLAM, BearingRangeFactor )
/* ************************************************************************* */
TEST( planarSLAM, BearingRangeFactor_equals )
{
planarSLAM::BearingRange
BearingRangeFactor<Pose2, Point2>
factor1(i2, j3, Rot2::fromAngle(0.1), 7.3, sigma2),
factor2(i2, j3, Rot2::fromAngle(3), 2.0, sigma2);
EXPECT(assert_equal(factor1, factor1, 1e-5));
@ -137,7 +137,7 @@ TEST( planarSLAM, BearingRangeFactor_poses )
/* ************************************************************************* */
TEST( planarSLAM, PoseConstraint_equals )
{
planarSLAM::Constraint factor1(i2, x2), factor2(i2, x3);
NonlinearEquality<Pose2> factor1(i2, x2), factor2(i2, x3);
EXPECT(assert_equal(factor1, factor1, 1e-5));
EXPECT(assert_equal(factor2, factor2, 1e-5));
EXPECT(assert_inequal(factor1, factor2, 1e-5));
@ -159,7 +159,7 @@ TEST( planarSLAM, constructor )
G.addPoseConstraint(i2, x2); // make it feasible :-)
// Add odometry
G.addOdometry(i2, i3, Pose2(0, 0, M_PI_4), I3);
G.addRelativePose(i2, i3, Pose2(0, 0, M_PI_4), I3);
// Create bearing factor
Rot2 z1 = Rot2::fromAngle(M_PI_4 + 0.1); // h(x) - z = -0.1

55
gtsam/slam/tests/testPose2SLAM.cpp
View File

@ -32,7 +32,7 @@ using namespace boost::assign;
#include <iostream>
using namespace std;
typedef pose2SLAM::Odometry Pose2Factor;
typedef BetweenFactor<Pose2> Pose2Factor;
// common measurement covariance
static double sx=0.5, sy=0.5,st=0.1;
@ -49,9 +49,8 @@ TEST_UNSAFE( Pose2SLAM, XYT )
pose2SLAM::Values values;
values.insertPose(1,Pose2(1,2,3));
values.insertPose(2,Pose2(4,5,6));
EXPECT(assert_equal(Vector_(2,1.0,4.0),values.xs()));
EXPECT(assert_equal(Vector_(2,2.0,5.0),values.ys()));
EXPECT(assert_equal(Vector_(2,3.0,6.0-2*M_PI),values.thetas()));
Matrix expected = Matrix_(2,3, 1.0,2.0,3.0, 4.0,5.0,6.0-2*M_PI);
EXPECT(assert_equal(expected,values.poses()));
}
/* ************************************************************************* */
@ -106,7 +105,7 @@ TEST_UNSAFE( Pose2SLAM, constructor )
// create a factor between unknown poses p1 and p2
Pose2 measured(2,2,M_PI_2);
pose2SLAM::Graph graph;
graph.addOdometry(1,2,measured, covariance);
graph.addRelativePose(1,2,measured, covariance);
// get the size of the graph
size_t actual = graph.size();
// verify
@ -121,7 +120,7 @@ TEST_UNSAFE( Pose2SLAM, linearization )
Pose2 measured(2,2,M_PI_2);
Pose2Factor constraint(1, 2, measured, covariance);
pose2SLAM::Graph graph;
graph.addOdometry(1,2,measured, covariance);
graph.addRelativePose(1,2,measured, covariance);
// Choose a linearization point
Pose2 p1(1.1,2,M_PI_2); // robot at (1.1,2) looking towards y (ground truth is at 1,2, see testPose2)
@ -159,7 +158,7 @@ TEST_UNSAFE(Pose2SLAM, optimize) {
// create a Pose graph with one equality constraint and one measurement
pose2SLAM::Graph fg;
fg.addPoseConstraint(0, Pose2(0,0,0));
fg.addOdometry(0, 1, Pose2(1,2,M_PI_2), covariance);
fg.addRelativePose(0, 1, Pose2(1,2,M_PI_2), covariance);
// Create initial config
Values initial;
@ -196,11 +195,11 @@ TEST_UNSAFE(Pose2SLAM, optimizeSPCG) {
// create a Pose graph with one equality constraint and one measurement
pose2SLAM::Graph fg;
fg.addPrior(0, Pose2(0,0,0), noiseModel::Diagonal::Sigmas(Vector_(3,3.0,3.0,1.0)));
fg.addOdometry(0, 1, Pose2(1,2,M_PI_2), covariance);
fg.addPosePrior(0, Pose2(0,0,0), noiseModel::Diagonal::Sigmas(Vector_(3,3.0,3.0,1.0)));
fg.addRelativePose(0, 1, Pose2(1,2,M_PI_2), covariance);
// [Duy] For some unknown reason, SPCG needs this constraint to work. GaussNewton doesn't need this.
fg.addConstraint(0, 1, Pose2(1,2,M_PI_2), noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)));
fg.addRelativePose(0, 1, Pose2(1,2,M_PI_2), noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)));
// Create initial config
Values initial;
@ -208,7 +207,7 @@ TEST_UNSAFE(Pose2SLAM, optimizeSPCG) {
initial.insert(1, Pose2(0,0,0));
// Optimize using SPCG
Values actual = fg.optimizeSPCG(initial);
Values actual = fg.optimizeSPCG(initial,0);
// Try GaussNewton without the above constraint to see if the problem is underconstrained. Still works!
Values actual2 = GaussNewtonOptimizer(fg, initial).optimize();
@ -233,9 +232,9 @@ TEST_UNSAFE(Pose2SLAM, optimizeThreePoses) {
pose2SLAM::Graph fg;
fg.addPoseConstraint(0, p0);
Pose2 delta = p0.between(p1);
fg.addOdometry(0, 1, delta, covariance);
fg.addOdometry(1, 2, delta, covariance);
fg.addOdometry(2, 0, delta, covariance);
fg.addRelativePose(0, 1, delta, covariance);
fg.addRelativePose(1, 2, delta, covariance);
fg.addRelativePose(2, 0, delta, covariance);
// Create initial config
pose2SLAM::Values initial;
@ -269,12 +268,12 @@ TEST_UNSAFE(Pose2SLAM, optimizeCircle) {
pose2SLAM::Graph fg;
fg.addPoseConstraint(0, p0);
Pose2 delta = p0.between(p1);
fg.addOdometry(0,1, delta, covariance);
fg.addOdometry(1,2, delta, covariance);
fg.addOdometry(2,3, delta, covariance);
fg.addOdometry(3,4, delta, covariance);
fg.addOdometry(4,5, delta, covariance);
fg.addOdometry(5,0, delta, covariance);
fg.addRelativePose(0,1, delta, covariance);
fg.addRelativePose(1,2, delta, covariance);
fg.addRelativePose(2,3, delta, covariance);
fg.addRelativePose(3,4, delta, covariance);
fg.addRelativePose(4,5, delta, covariance);
fg.addRelativePose(5,0, delta, covariance);
// Create initial config
pose2SLAM::Values initial;
@ -357,9 +356,9 @@ TEST_UNSAFE(Pose2SLAM, optimize2) {
///* ************************************************************************* */
// SL-NEEDED? TEST_UNSAFE(Pose2SLAM, findMinimumSpanningTree) {
// pose2SLAM::Graph G, T, C;
// G.addConstraint(1, 2, Pose2(0.,0.,0.), I3);
// G.addConstraint(1, 3, Pose2(0.,0.,0.), I3);
// G.addConstraint(2, 3, Pose2(0.,0.,0.), I3);
// G.addPoseConstraint(1, 2, Pose2(0.,0.,0.), I3);
// G.addPoseConstraint(1, 3, Pose2(0.,0.,0.), I3);
// G.addPoseConstraint(2, 3, Pose2(0.,0.,0.), I3);
//
// PredecessorMap<pose2SLAM::pose2SLAM::PoseKey> tree =
// G.findMinimumSpanningTree<pose2SLAM::pose2SLAM::PoseKey, Pose2Factor>();
@ -371,9 +370,9 @@ TEST_UNSAFE(Pose2SLAM, optimize2) {
///* ************************************************************************* */
// SL-NEEDED? TEST_UNSAFE(Pose2SLAM, split) {
// pose2SLAM::Graph G, T, C;
// G.addConstraint(1, 2, Pose2(0.,0.,0.), I3);
// G.addConstraint(1, 3, Pose2(0.,0.,0.), I3);
// G.addConstraint(2, 3, Pose2(0.,0.,0.), I3);
// G.addPoseConstraint(1, 2, Pose2(0.,0.,0.), I3);
// G.addPoseConstraint(1, 3, Pose2(0.,0.,0.), I3);
// G.addPoseConstraint(2, 3, Pose2(0.,0.,0.), I3);
//
// PredecessorMap<pose2SLAM::pose2SLAM::PoseKey> tree;
// tree.insert(1,2);
@ -436,7 +435,7 @@ TEST_UNSAFE( Pose2Prior, error )
x0.insert(1, p1);
// Create factor
pose2SLAM::Prior factor(1, p1, sigmas);
PriorFactor<Pose2> factor(1, p1, sigmas);
// Actual linearization
Ordering ordering(*x0.orderingArbitrary());
@ -463,7 +462,7 @@ TEST_UNSAFE( Pose2Prior, error )
/* ************************************************************************* */
// common Pose2Prior for tests below
static gtsam::Pose2 priorVal(2,2,M_PI_2);
static pose2SLAM::Prior priorFactor(1, priorVal, sigmas);
static PriorFactor<Pose2> priorFactor(1, priorVal, sigmas);
/* ************************************************************************* */
// The error |A*dx-b| approximates (h(x0+dx)-z) = -error_vector

16
gtsam/slam/tests/testPose3SLAM.cpp
View File

@ -48,16 +48,16 @@ TEST(Pose3Graph, optimizeCircle) {
// create a Pose graph with one equality constraint and one measurement
pose3SLAM::Graph fg;
fg.addHardConstraint(0, gT0);
fg.addPoseConstraint(0, gT0);
Pose3 _0T1 = gT0.between(gT1); // inv(gT0)*gT1
double theta = M_PI/3.0;
CHECK(assert_equal(Pose3(Rot3::yaw(-theta),Point3(radius*sin(theta),-radius*cos(theta),0)),_0T1));
fg.addConstraint(0,1, _0T1, covariance);
fg.addConstraint(1,2, _0T1, covariance);
fg.addConstraint(2,3, _0T1, covariance);
fg.addConstraint(3,4, _0T1, covariance);
fg.addConstraint(4,5, _0T1, covariance);
fg.addConstraint(5,0, _0T1, covariance);
fg.addRelativePose(0,1, _0T1, covariance);
fg.addRelativePose(1,2, _0T1, covariance);
fg.addRelativePose(2,3, _0T1, covariance);
fg.addRelativePose(3,4, _0T1, covariance);
fg.addRelativePose(4,5, _0T1, covariance);
fg.addRelativePose(5,0, _0T1, covariance);
// Create initial config
Values initial;
@ -121,7 +121,7 @@ TEST( Pose3Factor, error )
// Create factor
SharedNoiseModel I6(noiseModel::Unit::Create(6));
pose3SLAM::Constraint factor(1, 2, z, I6);
BetweenFactor<Pose3> factor(1, 2, z, I6);
// Create config
Values x;

14
gtsam/slam/tests/testProjectionFactor.cpp
View File

@ -41,6 +41,8 @@ static shared_ptrK sK(new Cal3_S2(K));
using symbol_shorthand::X;
using symbol_shorthand::L;
typedef GenericProjectionFactor<Pose3, Point3> MyProjectionFactor;
/* ************************************************************************* */
TEST( ProjectionFactor, nonStandard )
{
@ -53,8 +55,8 @@ TEST( ProjectionFactor, error )
// Create the factor with a measurement that is 3 pixels off in x
Point2 z(323.,240.);
int i=1, j=1;
boost::shared_ptr<visualSLAM::ProjectionFactor>
factor(new visualSLAM::ProjectionFactor(z, sigma, X(i), L(j), sK));
boost::shared_ptr<MyProjectionFactor>
factor(new MyProjectionFactor(z, sigma, X(i), L(j), sK));
// For the following values structure, the factor predicts 320,240
Values config;
@ -102,11 +104,11 @@ TEST( ProjectionFactor, equals )
// Create two identical factors and make sure they're equal
Vector z = Vector_(2,323.,240.);
int i=1, j=1;
boost::shared_ptr<visualSLAM::ProjectionFactor>
factor1(new visualSLAM::ProjectionFactor(z, sigma, X(i), L(j), sK));
boost::shared_ptr<MyProjectionFactor>
factor1(new MyProjectionFactor(z, sigma, X(i), L(j), sK));
boost::shared_ptr<visualSLAM::ProjectionFactor>
factor2(new visualSLAM::ProjectionFactor(z, sigma, X(i), L(j), sK));
boost::shared_ptr<MyProjectionFactor>
factor2(new MyProjectionFactor(z, sigma, X(i), L(j), sK));
CHECK(assert_equal(*factor1, *factor2));
}

6
gtsam/slam/tests/testStereoFactor.cpp
View File

@ -46,6 +46,8 @@ static SharedNoiseModel sigma(noiseModel::Unit::Create(1));
using symbol_shorthand::X;
using symbol_shorthand::L;
typedef GenericStereoFactor<Pose3, Point3> MyStereoFactor;
/* ************************************************************************* */
TEST( StereoFactor, singlePoint)
{
@ -53,11 +55,11 @@ TEST( StereoFactor, singlePoint)
boost::shared_ptr<Cal3_S2Stereo> K(new Cal3_S2Stereo(625, 625, 0, 320, 240, 0.5));
NonlinearFactorGraph graph;
graph.add(visualSLAM::PoseConstraint(X(1),camera1));
graph.add(NonlinearEquality<Pose3>(X(1),camera1));
StereoPoint2 z14(320,320.0-50, 240);
// arguments: measurement, sigma, cam#, measurement #, K, baseline (m)
graph.add(visualSLAM::StereoFactor(z14,sigma, X(1), L(1), K));
graph.add(MyStereoFactor(z14,sigma, X(1), L(1), K));
// Create a configuration corresponding to the ground truth
Values values;

78
gtsam/slam/visualSLAM.cpp
View File

@ -38,36 +38,6 @@ namespace visualSLAM {
return points.size();
}
/* ************************************************************************* */
Vector Values::xs() const {
size_t j=0;
ConstFiltered<Pose3> poses = filter<Pose3>();
Vector result(poses.size());
BOOST_FOREACH(const ConstFiltered<Pose3>::KeyValuePair& keyValue, poses)
result(j++) = keyValue.value.x();
return result;
}
/* ************************************************************************* */
Vector Values::ys() const {
size_t j=0;
ConstFiltered<Pose3> poses = filter<Pose3>();
Vector result(poses.size());
BOOST_FOREACH(const ConstFiltered<Pose3>::KeyValuePair& keyValue, poses)
result(j++) = keyValue.value.y();
return result;
}
/* ************************************************************************* */
Vector Values::zs() const {
size_t j=0;
ConstFiltered<Pose3> poses = filter<Pose3>();
Vector result(poses.size());
BOOST_FOREACH(const ConstFiltered<Pose3>::KeyValuePair& keyValue, poses)
result(j++) = keyValue.value.z();
return result;
}
/* ************************************************************************* */
Matrix Values::points() const {
size_t j=0;
@ -78,54 +48,32 @@ namespace visualSLAM {
return result;
}
/* ************************************************************************* */
void Graph::addPointConstraint(Key pointKey, const Point3& p) {
push_back(make_shared<NonlinearEquality<Point3> >(pointKey, p));
}
/* ************************************************************************* */
void Graph::addPointPrior(Key pointKey, const Point3& p, const SharedNoiseModel& model) {
push_back(make_shared<PriorFactor<Point3> >(pointKey, p, model));
}
/* ************************************************************************* */
void Graph::addMeasurement(const Point2& measured, const SharedNoiseModel& model,
Key poseKey, Key pointKey, const shared_ptrK K) {
push_back(make_shared<ProjectionFactor>(measured, model, poseKey, pointKey, K));
push_back(make_shared<GenericProjectionFactor<Pose3, Point3> >(measured, model, poseKey, pointKey, K));
}
/* ************************************************************************* */
void Graph::addStereoMeasurement(const StereoPoint2& measured, const SharedNoiseModel& model,
Key poseKey, Key pointKey, const shared_ptrKStereo K) {
push_back(make_shared<StereoFactor>(measured, model, poseKey, pointKey, K));
}
/* ************************************************************************* */
void Graph::addPoseConstraint(Key poseKey, const Pose3& p) {
push_back(make_shared<PoseConstraint>(poseKey, p));
}
/* ************************************************************************* */
void Graph::addPointConstraint(Key pointKey, const Point3& p) {
push_back(make_shared<PointConstraint>(pointKey, p));
}
/* ************************************************************************* */
void Graph::addPosePrior(Key poseKey, const Pose3& p, const SharedNoiseModel& model) {
push_back(make_shared<PosePrior>(poseKey, p, model));
}
/* ************************************************************************* */
void Graph::addPointPrior(Key pointKey, const Point3& p, const SharedNoiseModel& model) {
push_back(make_shared<PointPrior>(pointKey, p, model));
push_back(make_shared<GenericStereoFactor<Pose3, Point3> >(measured, model, poseKey, pointKey, K));
}
/* ************************************************************************* */
void Graph::addRangeFactor(Key poseKey, Key pointKey, double range, const SharedNoiseModel& model) {
push_back(make_shared<RangeFactor>(poseKey, pointKey, range, model));
push_back(make_shared<gtsam::RangeFactor<Pose3, Point3> >(poseKey, pointKey, range, model));
}
/* ************************************************************************* */
void Graph::addOdometry(Key x1, Key x2, const Pose3& odometry, const SharedNoiseModel& model) {
push_back(make_shared<BetweenFactor<Pose3> >(x1, x2, odometry, model));
}
/* ************************************************************************* */
Values Graph::optimize(const Values& initialEstimate, size_t verbosity) const {
LevenbergMarquardtParams params;
params.verbosity = (NonlinearOptimizerParams::Verbosity)verbosity;
LevenbergMarquardtOptimizer optimizer(*this, initialEstimate,params);
return optimizer.optimize();
}
/* ************************************************************************* */
}

161
gtsam/slam/visualSLAM.h
View File

@ -19,39 +19,20 @@
#pragma once
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/pose3SLAM.h>
#include <gtsam/slam/RangeFactor.h>
#include <gtsam/slam/StereoFactor.h>
#include <gtsam/slam/ProjectionFactor.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/nonlinear/NonlinearISAM.h>
#include <gtsam/nonlinear/NonlinearOptimizer.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/geometry/SimpleCamera.h>
namespace visualSLAM {
using namespace gtsam;
/**
* Typedefs that make up the visualSLAM namespace.
*/
typedef NonlinearEquality<Pose3> PoseConstraint; ///< put a hard constraint on a pose
typedef NonlinearEquality<Point3> PointConstraint; ///< put a hard constraint on a point
typedef PriorFactor<Pose3> PosePrior; ///< put a soft prior on a Pose
typedef PriorFactor<Point3> PointPrior; ///< put a soft prior on a point
typedef RangeFactor<Pose3, Point3> RangeFactor; ///< put a factor on the range from a pose to a point
/// monocular and stereo camera typedefs for general use
typedef GenericProjectionFactor<Pose3, Point3> ProjectionFactor;
typedef GenericStereoFactor<Pose3, Point3> StereoFactor;
/// Values class, inherited from Values, mainly used as a convenience for MATLAB wrapper
struct Values: public gtsam::Values {
struct Values: public pose3SLAM::Values {
typedef boost::shared_ptr<Values> shared_ptr;
typedef gtsam::Values::ConstFiltered<Pose3> PoseFiltered;
@ -62,28 +43,22 @@ namespace visualSLAM {
/// Copy constructor
Values(const gtsam::Values& values) :
gtsam::Values(values) {
pose3SLAM::Values(values) {
}
/// Constructor from filtered values view of poses
Values(const PoseFiltered& view) :
gtsam::Values(view) {
pose3SLAM::Values(view) {
}
/// Constructor from filtered values view of points
Values(const PointFiltered& view) :
gtsam::Values(view) {
pose3SLAM::Values(view) {
}
/// insert a pose
void insertPose(Key i, const Pose3& pose) { insert(i, pose); }
/// insert a point
void insertPoint(Key j, const Point3& point) { insert(j, point); }
/// update a pose
void updatePose(Key i, const Pose3& pose) { update(i, pose); }
/// update a point
void updatePoint(Key j, const Point3& point) { update(j, point); }
@ -93,9 +68,6 @@ namespace visualSLAM {
/// get number of points
size_t nrPoints() const;
/// get a pose
Pose3 pose(Key i) const { return at<Pose3>(i); }
/// get a point
Point3 point(Key j) const { return at<Point3>(j); }
@ -105,13 +77,6 @@ namespace visualSLAM {
/// get a const view containing only points
PointFiltered allPoints() const { return this->filter<Point3>(); }
/// check if value with specified key exists
bool exists(Key i) const { return gtsam::Values::exists(i); }
Vector xs() const; ///< get all pose x coordinates in a matrix
Vector ys() const; ///< get all pose y coordinates in a matrix
Vector zs() const; ///< get all pose z coordinates in a matrix
Matrix points() const; ///< get all point coordinates in a matrix
};
@ -119,7 +84,7 @@ namespace visualSLAM {
/**
* Non-linear factor graph for vanilla visual SLAM
*/
class Graph: public NonlinearFactorGraph {
class Graph: public pose3SLAM::Graph {
public:
/// shared pointer to this type of graph
@ -139,6 +104,32 @@ namespace visualSLAM {
return NonlinearFactorGraph::equals(p, tol);
}
/**
* Add a constraint on a point (for now, *must* be satisfied in any Values)
* @param key variable key of the landmark
* @param p point around which soft prior is defined
*/
void addPointConstraint(Key pointKey, const Point3& p = Point3());
/**
* Add a prior on a landmark
* @param key variable key of the landmark
* @param p to which point to constrain it to
* @param model uncertainty model of this prior
*/
void addPointPrior(Key pointKey, const Point3& p = Point3(),
const SharedNoiseModel& model = noiseModel::Unit::Create(3));
/**
* Add a range prior to a landmark
* @param poseKey variable key of the camera pose
* @param pointKey variable key of the landmark
* @param range approximate range to landmark
* @param model uncertainty model of this prior
*/
void addRangeFactor(Key poseKey, Key pointKey, double range,
const SharedNoiseModel& model = noiseModel::Unit::Create(1));
/**
* Add a projection factor measurement (monocular)
* @param measured the measurement
@ -161,78 +152,6 @@ namespace visualSLAM {
void addStereoMeasurement(const StereoPoint2& measured, const SharedNoiseModel& model,
Key poseKey, Key pointKey, const shared_ptrKStereo K);
/**
* Add a constraint on a pose (for now, *must* be satisfied in any Values)
* @param key variable key of the camera pose
* @param p to which pose to constrain it to
*/
void addPoseConstraint(Key poseKey, const Pose3& p = Pose3());
/**
* Add a constraint on a point (for now, *must* be satisfied in any Values)
* @param key variable key of the landmark
* @param p point around which soft prior is defined
*/
void addPointConstraint(Key pointKey, const Point3& p = Point3());
/**
* Add a prior on a pose
* @param key variable key of the camera pose
* @param p around which soft prior is defined
* @param model uncertainty model of this prior
*/
void addPosePrior(Key poseKey, const Pose3& p = Pose3(), const SharedNoiseModel& model = noiseModel::Unit::Create(6));
/**
* Add a prior on a landmark
* @param key variable key of the landmark
* @param p to which point to constrain it to
* @param model uncertainty model of this prior
*/
void addPointPrior(Key pointKey, const Point3& p = Point3(), const SharedNoiseModel& model = noiseModel::Unit::Create(3));
/**
* Add a range prior to a landmark
* @param poseKey variable key of the camera pose
* @param pointKey variable key of the landmark
* @param range approximate range to landmark
* @param model uncertainty model of this prior
*/
void addRangeFactor(Key poseKey, Key pointKey, double range, const SharedNoiseModel& model = noiseModel::Unit::Create(1));
/**
* Add an odometry between two poses
* @param x1 variable key of the first camera pose
* @param x2 variable key of the second camera pose
* @param odometry measurement from x1 to x2 (x1.between(x2))
* @param model uncertainty model of this measurement
*/
void addOdometry(Key x1, Key x2, const Pose3& odometry, const SharedNoiseModel& model);
/**
* Optimize the graph
* @param initialEstimate initial estimate of all variables in the graph
* @param pointKey variable key of the landmark
* @param range approximate range to landmark
* @param model uncertainty model of this prior
*/
Values optimize(const Values& initialEstimate, size_t verbosity) const;
/**
* Setup and return a LevenbargMarquardtOptimizer
* @param initialEstimate initial estimate of all variables in the graph
* @param parameters optimizer's parameters
* @return a LevenbergMarquardtOptimizer object, which you can use to control the optimization process
*/
LevenbergMarquardtOptimizer optimizer(const Values& initialEstimate, const LevenbergMarquardtParams& parameters = LevenbergMarquardtParams()) const {
return LevenbergMarquardtOptimizer((*this), initialEstimate, parameters);
}
/// Return a Marginals object
Marginals marginals(const Values& solution) const {
return Marginals(*this,solution);
}
}; // Graph
/**
@ -240,4 +159,18 @@ namespace visualSLAM {
*/
typedef gtsam::NonlinearISAM ISAM;
} // namespaces
} // visualSLAM
/**
* Backwards compatibility and wrap use only, avoid using
*/
namespace visualSLAM {
typedef gtsam::NonlinearEquality<Pose3> PoseConstraint; ///< \deprecated typedef for backwards compatibility
typedef gtsam::NonlinearEquality<Point3> PointConstraint; ///< \deprecated typedef for backwards compatibility
typedef gtsam::PriorFactor<Pose3> PosePrior; ///< \deprecated typedef for backwards compatibility
typedef gtsam::PriorFactor<Point3> PointPrior; ///< \deprecated typedef for backwards compatibility
typedef gtsam::RangeFactor<Pose3, Point3> RangeFactor; ///< \deprecated typedef for backwards compatibility
typedef gtsam::GenericProjectionFactor<Pose3, Point3> ProjectionFactor; ///< \deprecated typedef for backwards compatibility
typedef gtsam::GenericStereoFactor<Pose3, Point3> StereoFactor; ///< \deprecated typedef for backwards compatibility
}

0
matlab/examples/VisualISAMGenerateData.m → matlab/VisualISAMGenerateData.m
View File

2
matlab/VisualISAMInitialize.m
View File

@ -45,7 +45,7 @@ for i=1:2
end
%% Add odometry between frames 1 and 2
newFactors.addOdometry(symbol('x',1), symbol('x',2), data.odometry{1}, noiseModels.odometry);
newFactors.addRelativePose(symbol('x',1), symbol('x',2), data.odometry{1}, noiseModels.odometry);
%% Update ISAM
if options.batchInitialization % Do a full optimize for first two poses

2
matlab/VisualISAMStep.m
View File

@ -10,7 +10,7 @@ initialEstimates = visualSLAMValues;
%% Add odometry
i = result.nrPoses+1;
odometry = data.odometry{i-1};
newFactors.addOdometry(symbol('x',i-1), symbol('x',i), odometry, noiseModels.odometry);
newFactors.addRelativePose(symbol('x',i-1), symbol('x',i), odometry, noiseModels.odometry);
%% Add visual measurement factors and initializations as necessary
for k=1:length(data.Z{i})

15
matlab/examples/LocalizationExample.m
View File

@ -21,15 +21,15 @@ graph = pose2SLAMGraph;
%% Add two odometry factors
odometry = gtsamPose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case)
odometryNoise = gtsamnoiseModelDiagonal_Sigmas([0.2; 0.2; 0.1]); % 20cm std on x,y, 0.1 rad on theta
graph.addOdometry(1, 2, odometry, odometryNoise);
graph.addOdometry(2, 3, odometry, odometryNoise);
graph.addRelativePose(1, 2, odometry, odometryNoise);
graph.addRelativePose(2, 3, odometry, odometryNoise);
%% Add three "GPS" measurements
% We use Pose2 Priors here with high variance on theta
noiseModel = gtsamnoiseModelDiagonal_Sigmas([0.1; 0.1; 10]);
graph.addPrior(1, gtsamPose2(0.0, 0.0, 0.0), noiseModel);
graph.addPrior(2, gtsamPose2(2.0, 0.0, 0.0), noiseModel);
graph.addPrior(3, gtsamPose2(4.0, 0.0, 0.0), noiseModel);
graph.addPosePrior(1, gtsamPose2(0.0, 0.0, 0.0), noiseModel);
graph.addPosePrior(2, gtsamPose2(2.0, 0.0, 0.0), noiseModel);
graph.addPosePrior(3, gtsamPose2(4.0, 0.0, 0.0), noiseModel);
%% print
graph.print(sprintf('\nFactor graph:\n'));
@ -42,12 +42,13 @@ initialEstimate.insertPose(3, gtsamPose2(4.1, 0.1, 0.1));
initialEstimate.print(sprintf('\nInitial estimate:\n '));
%% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
result = graph.optimize(initialEstimate);
result = graph.optimize(initialEstimate,1);
result.print(sprintf('\nFinal result:\n '));
%% Plot Covariance Ellipses
cla;
plot(result.xs(),result.ys(),'k*-'); hold on
X=result.poses();
plot(X(:,1),X(:,2),'k*-'); hold on
marginals = graph.marginals(result);
P={};
for i=1:result.size()

11
matlab/examples/OdometryExample.m
View File

@ -21,13 +21,13 @@ graph = pose2SLAMGraph;
%% Add a Gaussian prior on pose x_1
priorMean = gtsamPose2(0.0, 0.0, 0.0); % prior mean is at origin
priorNoise = gtsamnoiseModelDiagonal_Sigmas([0.3; 0.3; 0.1]); % 30cm std on x,y, 0.1 rad on theta
graph.addPrior(1, priorMean, priorNoise); % add directly to graph
graph.addPosePrior(1, priorMean, priorNoise); % add directly to graph
%% Add two odometry factors
odometry = gtsamPose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case)
odometryNoise = gtsamnoiseModelDiagonal_Sigmas([0.2; 0.2; 0.1]); % 20cm std on x,y, 0.1 rad on theta
graph.addOdometry(1, 2, odometry, odometryNoise);
graph.addOdometry(2, 3, odometry, odometryNoise);
graph.addRelativePose(1, 2, odometry, odometryNoise);
graph.addRelativePose(2, 3, odometry, odometryNoise);
%% print
graph.print(sprintf('\nFactor graph:\n'));
@ -40,12 +40,13 @@ initialEstimate.insertPose(3, gtsamPose2(4.1, 0.1, 0.1));
initialEstimate.print(sprintf('\nInitial estimate:\n '));
%% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
result = graph.optimize(initialEstimate);
result = graph.optimize(initialEstimate,1);
result.print(sprintf('\nFinal result:\n '));
%% Plot Covariance Ellipses
cla;
plot(result.xs(),result.ys(),'k*-'); hold on
X=result.poses();
plot(X(:,1),X(:,2),'k*-'); hold on
marginals = graph.marginals(result);
P={};
for i=1:result.size()

8
matlab/examples/PlanarSLAMExample.m
View File

@ -29,13 +29,13 @@ graph = planarSLAMGraph;
%% Add prior
priorMean = gtsamPose2(0.0, 0.0, 0.0); % prior at origin
priorNoise = gtsamnoiseModelDiagonal_Sigmas([0.3; 0.3; 0.1]);
graph.addPrior(i1, priorMean, priorNoise); % add directly to graph
graph.addPosePrior(i1, priorMean, priorNoise); % add directly to graph
%% Add odometry
odometry = gtsamPose2(2.0, 0.0, 0.0);
odometryNoise = gtsamnoiseModelDiagonal_Sigmas([0.2; 0.2; 0.1]);
graph.addOdometry(i1, i2, odometry, odometryNoise);
graph.addOdometry(i2, i3, odometry, odometryNoise);
graph.addRelativePose(i1, i2, odometry, odometryNoise);
graph.addRelativePose(i2, i3, odometry, odometryNoise);
%% Add bearing/range measurement factors
degrees = pi/180;
@ -58,7 +58,7 @@ initialEstimate.insertPoint(j2, gtsamPoint2(4.1, 1.8));
initialEstimate.print(sprintf('\nInitial estimate:\n'));
%% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
result = graph.optimize(initialEstimate);
result = graph.optimize(initialEstimate,1);
result.print(sprintf('\nFinal result:\n'));
%% Plot Covariance Ellipses

6
matlab/examples/PlanarSLAMExample_sampling.m
View File

@ -16,11 +16,11 @@ i1 = symbol('x',1); i2 = symbol('x',2); i3 = symbol('x',3);
graph = planarSLAMGraph;
priorMean = gtsamPose2(0.0, 0.0, 0.0); % prior at origin
priorNoise = gtsamnoiseModelDiagonal_Sigmas([0.3; 0.3; 0.1]);
graph.addPrior(i1, priorMean, priorNoise); % add directly to graph
graph.addPosePrior(i1, priorMean, priorNoise); % add directly to graph
odometry = gtsamPose2(2.0, 0.0, 0.0);
odometryNoise = gtsamnoiseModelDiagonal_Sigmas([0.2; 0.2; 0.1]);
graph.addOdometry(i1, i2, odometry, odometryNoise);
graph.addOdometry(i2, i3, odometry, odometryNoise);
graph.addRelativePose(i1, i2, odometry, odometryNoise);
graph.addRelativePose(i2, i3, odometry, odometryNoise);
%% Except, for measurements we offer a choice
j1 = symbol('l',1); j2 = symbol('l',2);

17
matlab/examples/Pose2SLAMExample.m
View File

@ -25,19 +25,19 @@ graph = pose2SLAMGraph;
% gaussian for prior
priorMean = gtsamPose2(0.0, 0.0, 0.0); % prior at origin
priorNoise = gtsamnoiseModelDiagonal_Sigmas([0.3; 0.3; 0.1]);
graph.addPrior(1, priorMean, priorNoise); % add directly to graph
graph.addPosePrior(1, priorMean, priorNoise); % add directly to graph
%% Add odometry
% general noisemodel for odometry
odometryNoise = gtsamnoiseModelDiagonal_Sigmas([0.2; 0.2; 0.1]);
graph.addOdometry(1, 2, gtsamPose2(2.0, 0.0, 0.0 ), odometryNoise);
graph.addOdometry(2, 3, gtsamPose2(2.0, 0.0, pi/2), odometryNoise);
graph.addOdometry(3, 4, gtsamPose2(2.0, 0.0, pi/2), odometryNoise);
graph.addOdometry(4, 5, gtsamPose2(2.0, 0.0, pi/2), odometryNoise);
graph.addRelativePose(1, 2, gtsamPose2(2.0, 0.0, 0.0 ), odometryNoise);
graph.addRelativePose(2, 3, gtsamPose2(2.0, 0.0, pi/2), odometryNoise);
graph.addRelativePose(3, 4, gtsamPose2(2.0, 0.0, pi/2), odometryNoise);
graph.addRelativePose(4, 5, gtsamPose2(2.0, 0.0, pi/2), odometryNoise);
%% Add pose constraint
model = gtsamnoiseModelDiagonal_Sigmas([0.2; 0.2; 0.1]);
graph.addConstraint(5, 2, gtsamPose2(2.0, 0.0, pi/2), model);
graph.addRelativePose(5, 2, gtsamPose2(2.0, 0.0, pi/2), model);
% print
graph.print(sprintf('\nFactor graph:\n'));
@ -52,12 +52,13 @@ initialEstimate.insertPose(5, gtsamPose2(2.1, 2.1,-pi/2));
initialEstimate.print(sprintf('\nInitial estimate:\n'));
%% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
result = graph.optimize(initialEstimate);
result = graph.optimize(initialEstimate,1);
result.print(sprintf('\nFinal result:\n'));
%% Plot Covariance Ellipses
cla;
plot(result.xs(),result.ys(),'k*-'); hold on
X=result.poses();
plot(X(:,1),X(:,2),'k*-'); hold on
plot([result.pose(5).x;result.pose(2).x],[result.pose(5).y;result.pose(2).y],'r-');
marginals = graph.marginals(result);

8
matlab/examples/Pose2SLAMExample_advanced.m
View File

@ -27,14 +27,14 @@ graph = pose2SLAMGraph;
% gaussian for prior
priorNoise = gtsamnoiseModelDiagonal_Sigmas([0.3; 0.3; 0.1]);
priorMean = gtsamPose2(0.0, 0.0, 0.0); % prior at origin
graph.addPrior(1, priorMean, priorNoise); % add directly to graph
graph.addPosePrior(1, priorMean, priorNoise); % add directly to graph
%% Add odometry
% general noisemodel for odometry
odometryNoise = gtsamnoiseModelDiagonal_Sigmas([0.2; 0.2; 0.1]);
odometry = gtsamPose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case)
graph.addOdometry(1, 2, odometry, odometryNoise);
graph.addOdometry(2, 3, odometry, odometryNoise);
graph.addRelativePose(1, 2, odometry, odometryNoise);
graph.addRelativePose(2, 3, odometry, odometryNoise);
%% Add measurements
% general noisemodel for measurements
@ -60,7 +60,7 @@ initialEstimate.print('initial estimate');
%result.print('final result');
%% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
result = graph.optimize(initialEstimate);
result = graph.optimize(initialEstimate,1);
result.print('final result');
%% Get the corresponding dense matrix

12
matlab/examples/Pose2SLAMExample_circle.m
View File

@ -20,12 +20,12 @@ fg = pose2SLAMGraph;
fg.addPoseConstraint(0, p0);
delta = p0.between(p1);
covariance = gtsamnoiseModelDiagonal_Sigmas([0.05; 0.05; 5*pi/180]);
fg.addOdometry(0,1, delta, covariance);
fg.addOdometry(1,2, delta, covariance);
fg.addOdometry(2,3, delta, covariance);
fg.addOdometry(3,4, delta, covariance);
fg.addOdometry(4,5, delta, covariance);
fg.addOdometry(5,0, delta, covariance);
fg.addRelativePose(0,1, delta, covariance);
fg.addRelativePose(1,2, delta, covariance);
fg.addRelativePose(2,3, delta, covariance);
fg.addRelativePose(3,4, delta, covariance);
fg.addRelativePose(4,5, delta, covariance);
fg.addRelativePose(5,0, delta, covariance);
%% Create initial config
initial = pose2SLAMValues;

2
matlab/examples/Pose2SLAMExample_graph.m
View File

@ -18,7 +18,7 @@ initial.print(sprintf('Initial estimate:\n'));
%% Add a Gaussian prior on pose x_1
priorMean = gtsamPose2(0.0, 0.0, 0.0); % prior mean is at origin
priorNoise = gtsamnoiseModelDiagonal_Sigmas([0.01; 0.01; 0.01]);
graph.addPrior(0, priorMean, priorNoise); % add directly to graph
graph.addPosePrior(0, priorMean, priorNoise); % add directly to graph
%% Plot Initial Estimate
figure(1);clf

2
matlab/examples/Pose2SLAMwSPCG.m
View File

@ -35,7 +35,7 @@ graph.addOdometry(4, 5, gtsamPose2(2.0, 0.0, pi/2), odometryNoise);
%% Add pose constraint
model = gtsamnoiseModelDiagonal_Sigmas([0.2; 0.2; 0.1]);
graph.addConstraint(5, 2, gtsamPose2(2.0, 0.0, pi/2), model);
graph.addRelativePose(5, 2, gtsamPose2(2.0, 0.0, pi/2), model);
% print
graph.print(sprintf('\nFactor graph:\n'));

19
matlab/examples/Pose3SLAMExample.m
View File

@ -17,15 +17,15 @@ p1 = hexagon.pose(1);
%% create a Pose graph with one equality constraint and one measurement
fg = pose3SLAMGraph;
fg.addHardConstraint(0, p0);
fg.addPoseConstraint(0, p0);
delta = p0.between(p1);
covariance = gtsamnoiseModelDiagonal_Sigmas([0.05; 0.05; 0.05; 5*pi/180; 5*pi/180; 5*pi/180]);
fg.addConstraint(0,1, delta, covariance);
fg.addConstraint(1,2, delta, covariance);
fg.addConstraint(2,3, delta, covariance);
fg.addConstraint(3,4, delta, covariance);
fg.addConstraint(4,5, delta, covariance);
fg.addConstraint(5,0, delta, covariance);
fg.addRelativePose(0,1, delta, covariance);
fg.addRelativePose(1,2, delta, covariance);
fg.addRelativePose(2,3, delta, covariance);
fg.addRelativePose(3,4, delta, covariance);
fg.addRelativePose(4,5, delta, covariance);
fg.addRelativePose(5,0, delta, covariance);
%% Create initial config
initial = pose3SLAMValues;
@ -39,10 +39,11 @@ initial.insertPose(5, hexagon.pose(5).retract(s*randn(6,1)));
%% Plot Initial Estimate
cla
plot3(initial.xs(),initial.ys(),initial.zs(),'g-*');
T=initial.translations();
plot3(T(:,1),T(:,2),T(:,3),'g-*');
%% optimize
result = fg.optimize(initial);
result = fg.optimize(initial,1);
%% Show Result
hold on; plot3DTrajectory(result,'b-*', true, 0.3);

10
matlab/examples/StereoVOExample.m
View File

@ -67,13 +67,15 @@ axis equal
view(-38,12)
% initial trajectory in red (rotated so Z is up)
plot3(initialEstimate.zs(),-initialEstimate.xs(),-initialEstimate.ys(), '-*r','LineWidth',2);
T=initialEstimate.translations;
plot3(T(:,3),-T(:,1),-T(:,2), '-*r','LineWidth',2);
% final trajectory in green (rotated so Z is up)
plot3(result.zs(),-result.xs(),-result.ys(), '-*g','LineWidth',2);
T=result.translations;
plot3(T(:,3),-T(:,1),-T(:,2), '-*g','LineWidth',2);
xlabel('X (m)'); ylabel('Y (m)'); zlabel('Z (m)');
% optimized 3D points (rotated so Z is up)
points = result.points();
plot3(points(:,3),-points(:,1),-points(:,2),'*');
P = result.points();
plot3(P(:,3),-P(:,1),-P(:,2),'*');

2
matlab/load3D.m
View File

@ -41,7 +41,7 @@ for i=1:n
t = gtsamPoint3(e{4}, e{5}, e{6});
R = gtsamRot3_ypr(e{9}, e{8}, e{7});
dpose = gtsamPose3(R,t);
graph.addConstraint(i1, i2, dpose, model);
graph.addRelativePose(i1, i2, dpose, model);
if successive
if i2>i1
initial.insertPose(i2,initial.pose(i1).compose(dpose));

3
matlab/plot3DTrajectory.m
View File

@ -3,7 +3,8 @@ function plot3DTrajectory(values,style,frames,scale)
if nargin<3,frames=false;end
if nargin<4,scale=0;end
plot3(values.xs(),values.ys(),values.zs(),style); hold on
T=values.translations()
plot3(T(:,1),T(:,2),T(:,3),style); hold on
if frames
N=values.size;
for i=0:N-1

8
matlab/tests/testLocalizationExample.m
View File

@ -16,8 +16,8 @@ graph = pose2SLAMGraph;
%% Add two odometry factors
odometry = gtsamPose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case)
odometryNoise = gtsamnoiseModelDiagonal_Sigmas([0.2; 0.2; 0.1]); % 20cm std on x,y, 0.1 rad on theta
graph.addOdometry(1, 2, odometry, odometryNoise);
graph.addOdometry(2, 3, odometry, odometryNoise);
graph.addRelativePose(1, 2, odometry, odometryNoise);
graph.addRelativePose(2, 3, odometry, odometryNoise);
%% Add three "GPS" measurements
% We use Pose2 Priors here with high variance on theta
@ -27,7 +27,7 @@ groundTruth.insertPose(2, gtsamPose2(2.0, 0.0, 0.0));
groundTruth.insertPose(3, gtsamPose2(4.0, 0.0, 0.0));
noiseModel = gtsamnoiseModelDiagonal_Sigmas([0.1; 0.1; 10]);
for i=1:3
graph.addPrior(i, groundTruth.pose(i), noiseModel);
graph.addPosePrior(i, groundTruth.pose(i), noiseModel);
end
%% Initialize to noisy points
@ -37,7 +37,7 @@ initialEstimate.insertPose(2, gtsamPose2(2.3, 0.1,-0.2));
initialEstimate.insertPose(3, gtsamPose2(4.1, 0.1, 0.1));
%% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
result = graph.optimize(initialEstimate);
result = graph.optimize(initialEstimate,0);
%% Plot Covariance Ellipses
marginals = graph.marginals(result);

8
matlab/tests/testOdometryExample.m
View File

@ -16,13 +16,13 @@ graph = pose2SLAMGraph;
%% Add a Gaussian prior on pose x_1
priorMean = gtsamPose2(0.0, 0.0, 0.0); % prior mean is at origin
priorNoise = gtsamnoiseModelDiagonal_Sigmas([0.3; 0.3; 0.1]); % 30cm std on x,y, 0.1 rad on theta
graph.addPrior(1, priorMean, priorNoise); % add directly to graph
graph.addPosePrior(1, priorMean, priorNoise); % add directly to graph
%% Add two odometry factors
odometry = gtsamPose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case)
odometryNoise = gtsamnoiseModelDiagonal_Sigmas([0.2; 0.2; 0.1]); % 20cm std on x,y, 0.1 rad on theta
graph.addOdometry(1, 2, odometry, odometryNoise);
graph.addOdometry(2, 3, odometry, odometryNoise);
graph.addRelativePose(1, 2, odometry, odometryNoise);
graph.addRelativePose(2, 3, odometry, odometryNoise);
%% Initialize to noisy points
initialEstimate = pose2SLAMValues;
@ -31,7 +31,7 @@ initialEstimate.insertPose(2, gtsamPose2(2.3, 0.1,-0.2));
initialEstimate.insertPose(3, gtsamPose2(4.1, 0.1, 0.1));
%% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
result = graph.optimize(initialEstimate);
result = graph.optimize(initialEstimate,0);
marginals = graph.marginals(result);
marginals.marginalCovariance(i);

8
matlab/tests/testPlanarSLAMExample.m
View File

@ -29,13 +29,13 @@ graph = planarSLAMGraph;
%% Add prior
priorMean = gtsamPose2(0.0, 0.0, 0.0); % prior at origin
priorNoise = gtsamnoiseModelDiagonal_Sigmas([0.3; 0.3; 0.1]);
graph.addPrior(i1, priorMean, priorNoise); % add directly to graph
graph.addPosePrior(i1, priorMean, priorNoise); % add directly to graph
%% Add odometry
odometry = gtsamPose2(2.0, 0.0, 0.0);
odometryNoise = gtsamnoiseModelDiagonal_Sigmas([0.2; 0.2; 0.1]);
graph.addOdometry(i1, i2, odometry, odometryNoise);
graph.addOdometry(i2, i3, odometry, odometryNoise);
graph.addRelativePose(i1, i2, odometry, odometryNoise);
graph.addRelativePose(i2, i3, odometry, odometryNoise);
%% Add bearing/range measurement factors
degrees = pi/180;
@ -53,7 +53,7 @@ initialEstimate.insertPoint(j1, gtsamPoint2(1.8, 2.1));
initialEstimate.insertPoint(j2, gtsamPoint2(4.1, 1.8));
%% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
result = graph.optimize(initialEstimate);
result = graph.optimize(initialEstimate,0);
marginals = graph.marginals(result);
%% Check first pose and point equality

16
matlab/tests/testPose2SLAMExample.m
View File

@ -25,19 +25,19 @@ graph = pose2SLAMGraph;
% gaussian for prior
priorMean = gtsamPose2(0.0, 0.0, 0.0); % prior at origin
priorNoise = gtsamnoiseModelDiagonal_Sigmas([0.3; 0.3; 0.1]);
graph.addPrior(1, priorMean, priorNoise); % add directly to graph
graph.addPosePrior(1, priorMean, priorNoise); % add directly to graph
%% Add odometry
% general noisemodel for odometry
odometryNoise = gtsamnoiseModelDiagonal_Sigmas([0.2; 0.2; 0.1]);
graph.addOdometry(1, 2, gtsamPose2(2.0, 0.0, 0.0 ), odometryNoise);
graph.addOdometry(2, 3, gtsamPose2(2.0, 0.0, pi/2), odometryNoise);
graph.addOdometry(3, 4, gtsamPose2(2.0, 0.0, pi/2), odometryNoise);
graph.addOdometry(4, 5, gtsamPose2(2.0, 0.0, pi/2), odometryNoise);
graph.addRelativePose(1, 2, gtsamPose2(2.0, 0.0, 0.0 ), odometryNoise);
graph.addRelativePose(2, 3, gtsamPose2(2.0, 0.0, pi/2), odometryNoise);
graph.addRelativePose(3, 4, gtsamPose2(2.0, 0.0, pi/2), odometryNoise);
graph.addRelativePose(4, 5, gtsamPose2(2.0, 0.0, pi/2), odometryNoise);
%% Add pose constraint
model = gtsamnoiseModelDiagonal_Sigmas([0.2; 0.2; 0.1]);
graph.addConstraint(5, 2, gtsamPose2(2.0, 0.0, pi/2), model);
graph.addRelativePose(5, 2, gtsamPose2(2.0, 0.0, pi/2), model);
%% Initialize to noisy points
initialEstimate = pose2SLAMValues;
@ -48,8 +48,8 @@ initialEstimate.insertPose(4, gtsamPose2(4.0, 2.0, pi ));
initialEstimate.insertPose(5, gtsamPose2(2.1, 2.1,-pi/2));
%% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
result = graph.optimize(initialEstimate);
resultSPCG = graph.optimizeSPCG(initialEstimate);
result = graph.optimize(initialEstimate,0);
resultSPCG = graph.optimizeSPCG(initialEstimate,0);
%% Plot Covariance Ellipses
marginals = graph.marginals(result);

16
matlab/tests/testPose3SLAMExample.m
View File

@ -17,15 +17,15 @@ p1 = hexagon.pose(1);
%% create a Pose graph with one equality constraint and one measurement
fg = pose3SLAMGraph;
fg.addHardConstraint(0, p0);
fg.addPoseConstraint(0, p0);
delta = p0.between(p1);
covariance = gtsamnoiseModelDiagonal_Sigmas([0.05; 0.05; 0.05; 5*pi/180; 5*pi/180; 5*pi/180]);
fg.addConstraint(0,1, delta, covariance);
fg.addConstraint(1,2, delta, covariance);
fg.addConstraint(2,3, delta, covariance);
fg.addConstraint(3,4, delta, covariance);
fg.addConstraint(4,5, delta, covariance);
fg.addConstraint(5,0, delta, covariance);
fg.addRelativePose(0,1, delta, covariance);
fg.addRelativePose(1,2, delta, covariance);
fg.addRelativePose(2,3, delta, covariance);
fg.addRelativePose(3,4, delta, covariance);
fg.addRelativePose(4,5, delta, covariance);
fg.addRelativePose(5,0, delta, covariance);
%% Create initial config
initial = pose3SLAMValues;
@ -38,7 +38,7 @@ initial.insertPose(4, hexagon.pose(4).retract(s*randn(6,1)));
initial.insertPose(5, hexagon.pose(5).retract(s*randn(6,1)));
%% optimize
result = fg.optimize(initial);
result = fg.optimize(initial,0);
pose_1 = result.pose(1);
CHECK('pose_1.equals(gtsamPose3,1e-4)',pose_1.equals(p1,1e-4));

72
tests/testGaussianISAM2.cpp
View File

@ -53,7 +53,7 @@ ISAM2 createSlamlikeISAM2(
// Add a prior at time 0 and update isam
{
planarSLAM::Graph newfactors;
newfactors.addPrior(0, Pose2(0.0, 0.0, 0.0), odoNoise);
newfactors.addPosePrior(0, Pose2(0.0, 0.0, 0.0), odoNoise);
fullgraph.push_back(newfactors);
Values init;
@ -66,7 +66,7 @@ ISAM2 createSlamlikeISAM2(
// Add odometry from time 0 to time 5
for( ; i<5; ++i) {
planarSLAM::Graph newfactors;
newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
fullgraph.push_back(newfactors);
Values init;
@ -79,7 +79,7 @@ ISAM2 createSlamlikeISAM2(
// Add odometry from time 5 to 6 and landmark measurement at time 5
{
planarSLAM::Graph newfactors;
newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise);
newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise);
fullgraph.push_back(newfactors);
@ -99,7 +99,7 @@ ISAM2 createSlamlikeISAM2(
// Add odometry from time 6 to time 10
for( ; i<10; ++i) {
planarSLAM::Graph newfactors;
newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
fullgraph.push_back(newfactors);
Values init;
@ -112,7 +112,7 @@ ISAM2 createSlamlikeISAM2(
// Add odometry from time 10 to 11 and landmark measurement at time 10
{
planarSLAM::Graph newfactors;
newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
fullgraph.push_back(newfactors);
@ -257,7 +257,7 @@ TEST_UNSAFE(ISAM2, AddVariables) {
//
// Ordering ordering; ordering += (0), (0), (1);
// planarSLAM::Graph graph;
// graph.addPrior((0), Pose2(), noiseModel::Unit::Create(Pose2::dimension));
// graph.addPosePrior((0), Pose2(), noiseModel::Unit::Create(Pose2::dimension));
// graph.addRange((0), (0), 1.0, noiseModel::Unit::Create(1));
//
// FastSet<Index> expected;
@ -364,7 +364,7 @@ TEST(ISAM2, slamlike_solution_gaussnewton)
// Add a prior at time 0 and update isam
{
planarSLAM::Graph newfactors;
newfactors.addPrior(0, Pose2(0.0, 0.0, 0.0), odoNoise);
newfactors.addPosePrior(0, Pose2(0.0, 0.0, 0.0), odoNoise);
fullgraph.push_back(newfactors);
Values init;
@ -379,7 +379,7 @@ TEST(ISAM2, slamlike_solution_gaussnewton)
// Add odometry from time 0 to time 5
for( ; i<5; ++i) {
planarSLAM::Graph newfactors;
newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
fullgraph.push_back(newfactors);
Values init;
@ -392,7 +392,7 @@ TEST(ISAM2, slamlike_solution_gaussnewton)
// Add odometry from time 5 to 6 and landmark measurement at time 5
{
planarSLAM::Graph newfactors;
newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise);
newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise);
fullgraph.push_back(newfactors);
@ -412,7 +412,7 @@ TEST(ISAM2, slamlike_solution_gaussnewton)
// Add odometry from time 6 to time 10
for( ; i<10; ++i) {
planarSLAM::Graph newfactors;
newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
fullgraph.push_back(newfactors);
Values init;
@ -425,7 +425,7 @@ TEST(ISAM2, slamlike_solution_gaussnewton)
// Add odometry from time 10 to 11 and landmark measurement at time 10
{
planarSLAM::Graph newfactors;
newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
fullgraph.push_back(newfactors);
@ -484,7 +484,7 @@ TEST(ISAM2, slamlike_solution_dogleg)
// Add a prior at time 0 and update isam
{
planarSLAM::Graph newfactors;
newfactors.addPrior(0, Pose2(0.0, 0.0, 0.0), odoNoise);
newfactors.addPosePrior(0, Pose2(0.0, 0.0, 0.0), odoNoise);
fullgraph.push_back(newfactors);
Values init;
@ -499,7 +499,7 @@ TEST(ISAM2, slamlike_solution_dogleg)
// Add odometry from time 0 to time 5
for( ; i<5; ++i) {
planarSLAM::Graph newfactors;
newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
fullgraph.push_back(newfactors);
Values init;
@ -512,7 +512,7 @@ TEST(ISAM2, slamlike_solution_dogleg)
// Add odometry from time 5 to 6 and landmark measurement at time 5
{
planarSLAM::Graph newfactors;
newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise);
newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise);
fullgraph.push_back(newfactors);
@ -532,7 +532,7 @@ TEST(ISAM2, slamlike_solution_dogleg)
// Add odometry from time 6 to time 10
for( ; i<10; ++i) {
planarSLAM::Graph newfactors;
newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
fullgraph.push_back(newfactors);
Values init;
@ -545,7 +545,7 @@ TEST(ISAM2, slamlike_solution_dogleg)
// Add odometry from time 10 to 11 and landmark measurement at time 10
{
planarSLAM::Graph newfactors;
newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
fullgraph.push_back(newfactors);
@ -604,7 +604,7 @@ TEST(ISAM2, slamlike_solution_gaussnewton_qr)
// Add a prior at time 0 and update isam
{
planarSLAM::Graph newfactors;
newfactors.addPrior(0, Pose2(0.0, 0.0, 0.0), odoNoise);
newfactors.addPosePrior(0, Pose2(0.0, 0.0, 0.0), odoNoise);
fullgraph.push_back(newfactors);
Values init;
@ -619,7 +619,7 @@ TEST(ISAM2, slamlike_solution_gaussnewton_qr)
// Add odometry from time 0 to time 5
for( ; i<5; ++i) {
planarSLAM::Graph newfactors;
newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
fullgraph.push_back(newfactors);
Values init;
@ -632,7 +632,7 @@ TEST(ISAM2, slamlike_solution_gaussnewton_qr)
// Add odometry from time 5 to 6 and landmark measurement at time 5
{
planarSLAM::Graph newfactors;
newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise);
newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise);
fullgraph.push_back(newfactors);
@ -652,7 +652,7 @@ TEST(ISAM2, slamlike_solution_gaussnewton_qr)
// Add odometry from time 6 to time 10
for( ; i<10; ++i) {
planarSLAM::Graph newfactors;
newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
fullgraph.push_back(newfactors);
Values init;
@ -665,7 +665,7 @@ TEST(ISAM2, slamlike_solution_gaussnewton_qr)
// Add odometry from time 10 to 11 and landmark measurement at time 10
{
planarSLAM::Graph newfactors;
newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
fullgraph.push_back(newfactors);
@ -724,7 +724,7 @@ TEST(ISAM2, slamlike_solution_dogleg_qr)
// Add a prior at time 0 and update isam
{
planarSLAM::Graph newfactors;
newfactors.addPrior(0, Pose2(0.0, 0.0, 0.0), odoNoise);
newfactors.addPosePrior(0, Pose2(0.0, 0.0, 0.0), odoNoise);
fullgraph.push_back(newfactors);
Values init;
@ -739,7 +739,7 @@ TEST(ISAM2, slamlike_solution_dogleg_qr)
// Add odometry from time 0 to time 5
for( ; i<5; ++i) {
planarSLAM::Graph newfactors;
newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
fullgraph.push_back(newfactors);
Values init;
@ -752,7 +752,7 @@ TEST(ISAM2, slamlike_solution_dogleg_qr)
// Add odometry from time 5 to 6 and landmark measurement at time 5
{
planarSLAM::Graph newfactors;
newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise);
newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise);
fullgraph.push_back(newfactors);
@ -772,7 +772,7 @@ TEST(ISAM2, slamlike_solution_dogleg_qr)
// Add odometry from time 6 to time 10
for( ; i<10; ++i) {
planarSLAM::Graph newfactors;
newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
fullgraph.push_back(newfactors);
Values init;
@ -785,7 +785,7 @@ TEST(ISAM2, slamlike_solution_dogleg_qr)
// Add odometry from time 10 to 11 and landmark measurement at time 10
{
planarSLAM::Graph newfactors;
newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
fullgraph.push_back(newfactors);
@ -941,7 +941,7 @@ TEST(ISAM2, removeFactors)
// Add a prior at time 0 and update isam
{
planarSLAM::Graph newfactors;
newfactors.addPrior(0, Pose2(0.0, 0.0, 0.0), odoNoise);
newfactors.addPosePrior(0, Pose2(0.0, 0.0, 0.0), odoNoise);
fullgraph.push_back(newfactors);
Values init;
@ -956,7 +956,7 @@ TEST(ISAM2, removeFactors)
// Add odometry from time 0 to time 5
for( ; i<5; ++i) {
planarSLAM::Graph newfactors;
newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
fullgraph.push_back(newfactors);
Values init;
@ -969,7 +969,7 @@ TEST(ISAM2, removeFactors)
// Add odometry from time 5 to 6 and landmark measurement at time 5
{
planarSLAM::Graph newfactors;
newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise);
newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise);
fullgraph.push_back(newfactors);
@ -989,7 +989,7 @@ TEST(ISAM2, removeFactors)
// Add odometry from time 6 to time 10
for( ; i<10; ++i) {
planarSLAM::Graph newfactors;
newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
fullgraph.push_back(newfactors);
Values init;
@ -1002,7 +1002,7 @@ TEST(ISAM2, removeFactors)
// Add odometry from time 10 to 11 and landmark measurement at time 10
{
planarSLAM::Graph newfactors;
newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
fullgraph.push_back(newfactors[0]);
@ -1130,7 +1130,7 @@ TEST(ISAM2, constrained_ordering)
// Add a prior at time 0 and update isam
{
planarSLAM::Graph newfactors;
newfactors.addPrior(0, Pose2(0.0, 0.0, 0.0), odoNoise);
newfactors.addPosePrior(0, Pose2(0.0, 0.0, 0.0), odoNoise);
fullgraph.push_back(newfactors);
Values init;
@ -1145,7 +1145,7 @@ TEST(ISAM2, constrained_ordering)
// Add odometry from time 0 to time 5
for( ; i<5; ++i) {
planarSLAM::Graph newfactors;
newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
fullgraph.push_back(newfactors);
Values init;
@ -1161,7 +1161,7 @@ TEST(ISAM2, constrained_ordering)
// Add odometry from time 5 to 6 and landmark measurement at time 5
{
planarSLAM::Graph newfactors;
newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise);
newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise);
fullgraph.push_back(newfactors);
@ -1181,7 +1181,7 @@ TEST(ISAM2, constrained_ordering)
// Add odometry from time 6 to time 10
for( ; i<10; ++i) {
planarSLAM::Graph newfactors;
newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
fullgraph.push_back(newfactors);
Values init;
@ -1194,7 +1194,7 @@ TEST(ISAM2, constrained_ordering)
// Add odometry from time 10 to 11 and landmark measurement at time 10
{
planarSLAM::Graph newfactors;
newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
fullgraph.push_back(newfactors);

14
tests/testGaussianJunctionTreeB.cpp
View File

@ -136,19 +136,19 @@ TEST(GaussianJunctionTree, slamlike) {
size_t i = 0;
newfactors = planarSLAM::Graph();
newfactors.addPrior(X(0), Pose2(0.0, 0.0, 0.0), odoNoise);
newfactors.addPosePrior(X(0), Pose2(0.0, 0.0, 0.0), odoNoise);
init.insert(X(0), Pose2(0.01, 0.01, 0.01));
fullgraph.push_back(newfactors);
for( ; i<5; ++i) {
newfactors = planarSLAM::Graph();
newfactors.addOdometry(X(i), X(i+1), Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addRelativePose(X(i), X(i+1), Pose2(1.0, 0.0, 0.0), odoNoise);
init.insert(X(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
fullgraph.push_back(newfactors);
}
newfactors = planarSLAM::Graph();
newfactors.addOdometry(X(i), X(i+1), Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addRelativePose(X(i), X(i+1), Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addBearingRange(X(i), L(0), Rot2::fromAngle(M_PI/4.0), 5.0, brNoise);
newfactors.addBearingRange(X(i), L(1), Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise);
init.insert(X(i+1), Pose2(1.01, 0.01, 0.01));
@ -159,13 +159,13 @@ TEST(GaussianJunctionTree, slamlike) {
for( ; i<5; ++i) {
newfactors = planarSLAM::Graph();
newfactors.addOdometry(X(i), X(i+1), Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addRelativePose(X(i), X(i+1), Pose2(1.0, 0.0, 0.0), odoNoise);
init.insert(X(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
fullgraph.push_back(newfactors);
}
newfactors = planarSLAM::Graph();
newfactors.addOdometry(X(i), X(i+1), Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addRelativePose(X(i), X(i+1), Pose2(1.0, 0.0, 0.0), odoNoise);
newfactors.addBearingRange(X(i), L(0), Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
newfactors.addBearingRange(X(i), L(1), Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
init.insert(X(i+1), Pose2(6.9, 0.1, 0.01));
@ -194,8 +194,8 @@ TEST(GaussianJunctionTree, simpleMarginal) {
// Create a simple graph
pose2SLAM::Graph fg;
fg.addPrior(X(0), Pose2(), noiseModel::Isotropic::Sigma(3, 10.0));
fg.addOdometry(X(0), X(1), Pose2(1.0, 0.0, 0.0), noiseModel::Diagonal::Sigmas(Vector_(3, 10.0, 1.0, 1.0)));
fg.addPosePrior(X(0), Pose2(), noiseModel::Isotropic::Sigma(3, 10.0));
fg.addRelativePose(X(0), X(1), Pose2(1.0, 0.0, 0.0), noiseModel::Diagonal::Sigmas(Vector_(3, 10.0, 1.0, 1.0)));
Values init;
init.insert(X(0), Pose2());

12
tests/testGradientDescentOptimizer.cpp
View File

@ -27,18 +27,18 @@ boost::tuple<pose2SLAM::Graph, Values> generateProblem() {
// 2a. Add Gaussian prior
Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1));
graph.addPrior(1, priorMean, priorNoise);
graph.addPosePrior(1, priorMean, priorNoise);
// 2b. Add odometry factors
SharedDiagonal odometryNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1));
graph.addOdometry(1, 2, Pose2(2.0, 0.0, 0.0 ), odometryNoise);
graph.addOdometry(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
graph.addOdometry(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
graph.addOdometry(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
graph.addRelativePose(1, 2, Pose2(2.0, 0.0, 0.0 ), odometryNoise);
graph.addRelativePose(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
graph.addRelativePose(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
graph.addRelativePose(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
// 2c. Add pose constraint
SharedDiagonal constraintUncertainty = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1));
graph.addConstraint(5, 2, Pose2(2.0, 0.0, M_PI_2), constraintUncertainty);
graph.addRelativePose(5, 2, Pose2(2.0, 0.0, M_PI_2), constraintUncertainty);
// 3. Create the data structure to hold the initialEstimate estinmate to the solution
pose2SLAM::Values initialEstimate;

6
tests/testGraph.cpp
View File

@ -79,9 +79,9 @@ TEST( Graph, composePoses )
SharedNoiseModel cov = noiseModel::Unit::Create(3);
Pose2 p1(1.0, 2.0, 0.3), p2(4.0, 5.0, 0.6), p3(7.0, 8.0, 0.9), p4(2.0, 2.0, 2.9);
Pose2 p12=p1.between(p2), p23=p2.between(p3), p43=p4.between(p3);
graph.addOdometry(1,2, p12, cov);
graph.addOdometry(2,3, p23, cov);
graph.addOdometry(4,3, p43, cov);
graph.addRelativePose(1,2, p12, cov);
graph.addRelativePose(2,3, p23, cov);
graph.addRelativePose(4,3, p43, cov);
PredecessorMap<Key> tree;
tree.insert(1,2);

6
tests/testInferenceB.cpp
View File

@ -58,9 +58,9 @@ TEST( inference, marginals2)
SharedDiagonal poseModel(noiseModel::Isotropic::Sigma(3, 0.1));
SharedDiagonal pointModel(noiseModel::Isotropic::Sigma(3, 0.1));
fg.addPrior(X(0), Pose2(), poseModel);
fg.addOdometry(X(0), X(1), Pose2(1.0,0.0,0.0), poseModel);
fg.addOdometry(X(1), X(2), Pose2(1.0,0.0,0.0), poseModel);
fg.addPosePrior(X(0), Pose2(), poseModel);
fg.addRelativePose(X(0), X(1), Pose2(1.0,0.0,0.0), poseModel);
fg.addRelativePose(X(1), X(2), Pose2(1.0,0.0,0.0), poseModel);
fg.addBearingRange(X(0), L(0), Rot2(), 1.0, pointModel);
fg.addBearingRange(X(1), L(0), Rot2(), 1.0, pointModel);
fg.addBearingRange(X(2), L(0), Rot2(), 1.0, pointModel);

2
tests/testNonlinearISAM.cpp
View File

@ -41,7 +41,7 @@ TEST(testNonlinearISAM, markov_chain ) {
Pose2 z(1.0, 2.0, 0.1);
for (size_t i=1; i<=nrPoses; ++i) {
Graph new_factors;
new_factors.addOdometry(i-1, i, z, model);
new_factors.addRelativePose(i-1, i, z, model);
planarSLAM::Values new_init;
// perform a check on changing orderings

2
tests/testNonlinearOptimizer.cpp
View File

@ -176,7 +176,7 @@ TEST( NonlinearOptimizer, Factorization )
pose2SLAM::Graph graph;
graph.addPrior(X(1), Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3, 1e-10));
graph.addOdometry(X(1),X(2), Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1));
graph.addRelativePose(X(1),X(2), Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1));
Ordering ordering;
ordering.push_back(X(1));

4
tests/testOccupancyGrid.cpp
View File

@ -119,7 +119,7 @@ public:
}
///add a prior
void addPrior(Index cell, double prior){
void addPosePrior(Index cell, double prior){
size_t numStates = 2;
DiscreteKey key(cell, numStates);
@ -295,7 +295,7 @@ TEST_UNSAFE( OccupancyGrid, Test1) {
Pose2 pose(0,0,0);
double range = 1;
occupancyGrid.addPrior(0, 0.7);
occupancyGrid.addPosePrior(0, 0.7);
EXPECT_LONGS_EQUAL(1, occupancyGrid.size());
occupancyGrid.addLaser(pose, range);

2
tests/timeMultifrontalOnDataset.cpp
View File

@ -48,7 +48,7 @@ int main(int argc, char *argv[]) {
// Add a prior on the first pose
if (soft_prior)
data.first->addPrior(0, Pose2(), noiseModel::Isotropic::Sigma(Pose2::Dim(), 0.0005));
data.first->addPosePrior(0, Pose2(), noiseModel::Isotropic::Sigma(Pose2::Dim(), 0.0005));
else
data.first->addPoseConstraint(0, Pose2());

2
tests/timeSequentialOnDataset.cpp
View File

@ -48,7 +48,7 @@ int main(int argc, char *argv[]) {
// Add a prior on the first pose
if (soft_prior)
data.first->addPrior(0, Pose2(), noiseModel::Isotropic::Sigma(Pose2::Dim(), 0.0005));
data.first->addPosePrior(0, Pose2(), noiseModel::Isotropic::Sigma(Pose2::Dim(), 0.0005));
else
data.first->addPoseConstraint(0, Pose2());