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

new Pose3SLAM examples in C++ and MATLAB

release/4.3a0
Frank Dellaert 2012-06-04 00:41:13 +00:00
parent c558a67786
commit 877e9d4045
15 changed files with 11221 additions and 67 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10847
examples/Data/sphere_smallnoise.graph Normal file
View File

File diff suppressed because it is too large Load Diff

18
examples/Pose2SLAMExample.cpp
View File

@ -26,7 +26,7 @@ using namespace gtsam;
int main(int argc, char** argv) {
// 1. Create graph container and add factors to it
pose2SLAM::Graph graph ;
pose2SLAM::Graph graph;
// 2a. Add Gaussian prior
Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
@ -35,25 +35,25 @@ int main(int argc, char** argv) {
// 2b. Add odometry factors
SharedDiagonal odometryNoise(Vector_(3, 0.2, 0.2, 0.1));
graph.addOdometry(1, 2, Pose2(2.0, 0.0, 0.0 ), odometryNoise);
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);
// 2c. Add pose constraint
SharedDiagonal constraintUncertainty(Vector_(3, 0.2, 0.2, 0.1));
graph.addConstraint(5, 2, Pose2(2.0, 0.0, M_PI_2), constraintUncertainty);
SharedDiagonal model(Vector_(3, 0.2, 0.2, 0.1));
graph.addConstraint(5, 2, Pose2(2.0, 0.0, M_PI_2), model);
// print
graph.print("\nFactor graph:\n");
// 3. Create the data structure to hold the initialEstimate estimate to the solution
pose2SLAM::Values initialEstimate;
Pose2 x1(0.5, 0.0, 0.2 ); initialEstimate.insertPose(1, x1);
Pose2 x2(2.3, 0.1,-0.2 ); initialEstimate.insertPose(2, x2);
Pose2 x3(4.1, 0.1, M_PI_2); initialEstimate.insertPose(3, x3);
Pose2 x4(4.0, 2.0, M_PI ); initialEstimate.insertPose(4, x4);
Pose2 x5(2.1, 2.1,-M_PI_2); initialEstimate.insertPose(5, x5);
initialEstimate.insertPose(1, Pose2(0.5, 0.0, 0.2));
initialEstimate.insertPose(2, Pose2(2.3, 0.1, -0.2));
initialEstimate.insertPose(3, Pose2(4.1, 0.1, M_PI_2));
initialEstimate.insertPose(4, Pose2(4.0, 2.0, M_PI));
initialEstimate.insertPose(5, Pose2(2.1, 2.1, -M_PI_2));
initialEstimate.print("\nInitial estimate:\n");
// 4. Single Step Optimization using Levenberg-Marquardt

14
examples/matlab/Pose2SLAMExample.m
View File

@ -36,19 +36,19 @@ graph.addOdometry(3, 4, gtsamPose2(2.0, 0.0, pi/2), odometryNoise);
graph.addOdometry(4, 5, gtsamPose2(2.0, 0.0, pi/2), odometryNoise);
%% Add pose constraint
constraintUncertainty = gtsamSharedNoiseModel_Sigmas([0.2; 0.2; 0.1]);
graph.addConstraint(5, 2, gtsamPose2(2.0, 0.0, pi/2), constraintUncertainty);
model = gtsamSharedNoiseModel_Sigmas([0.2; 0.2; 0.1]);
graph.addConstraint(5, 2, gtsamPose2(2.0, 0.0, pi/2), model);
% print
graph.print(sprintf('\nFactor graph:\n'));
%% Initialize to noisy points
initialEstimate = pose2SLAMValues;
x1 = gtsamPose2(0.5, 0.0, 0.2 ); initialEstimate.insertPose(1, x1);
x2 = gtsamPose2(2.3, 0.1,-0.2 ); initialEstimate.insertPose(2, x2);
x3 = gtsamPose2(4.1, 0.1, pi/2); initialEstimate.insertPose(3, x3);
x4 = gtsamPose2(4.0, 2.0, pi ); initialEstimate.insertPose(4, x4);
x5 = gtsamPose2(2.1, 2.1,-pi/2); initialEstimate.insertPose(5, x5);
initialEstimate.insertPose(1, gtsamPose2(0.5, 0.0, 0.2 ));
initialEstimate.insertPose(2, gtsamPose2(2.3, 0.1,-0.2 ));
initialEstimate.insertPose(3, gtsamPose2(4.1, 0.1, pi/2));
initialEstimate.insertPose(4, gtsamPose2(4.0, 2.0, pi ));
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

48
examples/matlab/Pose2SLAMExample_circle.m Normal file
View File

@ -0,0 +1,48 @@
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% GTSAM Copyright 2010, Georgia Tech Research Corporation,
% Atlanta, Georgia 30332-0415
% All Rights Reserved
% Authors: Frank Dellaert, et al. (see THANKS for the full author list)
%
% See LICENSE for the license information
%
% @brief Read graph from file and perform GraphSLAM
% @author Frank Dellaert
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Create a hexagon of poses
hexagon = pose2SLAMValues_Circle(6,1.0);
p0 = hexagon.pose(0);
p1 = hexagon.pose(1);
%% create a Pose graph with one equality constraint and one measurement
fg = pose2SLAMGraph;
fg.addPoseConstraint(0, p0);
delta = p0.between(p1);
covariance = gtsamSharedNoiseModel_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);
%% Create initial config
initial = pose2SLAMValues;
initial.insertPose(0, p0);
initial.insertPose(1, hexagon.pose(1).retract([-0.1, 0.1,-0.1]'));
initial.insertPose(2, hexagon.pose(2).retract([ 0.1,-0.1, 0.1]'));
initial.insertPose(3, hexagon.pose(3).retract([-0.1, 0.1,-0.1]'));
initial.insertPose(4, hexagon.pose(4).retract([ 0.1,-0.1, 0.1]'));
initial.insertPose(5, hexagon.pose(5).retract([-0.1, 0.1,-0.1]'));
%% Plot Initial Estimate
figure(1);clf
plot(initial.xs(),initial.ys(),'g-*'); axis equal
%% optimize
result = fg.optimize(initial);
%% Show Result
hold on; plot(result.xs(),result.ys(),'b-*')
result.print(sprintf('\nFinal result:\n'));

49
examples/matlab/Pose3SLAMExample_circle.m Normal file
View File

@ -0,0 +1,49 @@
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% GTSAM Copyright 2010, Georgia Tech Research Corporation,
% Atlanta, Georgia 30332-0415
% All Rights Reserved
% Authors: Frank Dellaert, et al. (see THANKS for the full author list)
%
% See LICENSE for the license information
%
% @brief Read graph from file and perform GraphSLAM
% @author Frank Dellaert
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Create a hexagon of poses
hexagon = pose3SLAMValues_Circle(6,1.0);
p0 = hexagon.pose(0);
p1 = hexagon.pose(1);
%% create a Pose graph with one equality constraint and one measurement
fg = pose3SLAMGraph;
fg.addHardConstraint(0, p0);
delta = p0.between(p1);
covariance = gtsamSharedNoiseModel_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);
%% Create initial config
initial = pose3SLAMValues;
s = 0.10;
initial.insertPose(0, p0);
initial.insertPose(1, hexagon.pose(1).retract(s*randn(6,1)));
initial.insertPose(2, hexagon.pose(2).retract(s*randn(6,1)));
initial.insertPose(3, hexagon.pose(3).retract(s*randn(6,1)));
initial.insertPose(4, hexagon.pose(4).retract(s*randn(6,1)));
initial.insertPose(5, hexagon.pose(5).retract(s*randn(6,1)));
%% Plot Initial Estimate
figure(1);clf
plot3(initial.xs(),initial.ys(),initial.zs(),'g-*'); axis equal
%% optimize
result = fg.optimize(initial);
%% Show Result
hold on; plot3(result.xs(),result.ys(),result.zs(),'b-*')
result.print(sprintf('\nFinal result:\n'));

28
examples/matlab/Pose3SLAMExample_graph.m Normal file
View File

@ -0,0 +1,28 @@
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% GTSAM Copyright 2010, Georgia Tech Research Corporation,
% Atlanta, Georgia 30332-0415
% All Rights Reserved
% Authors: Frank Dellaert, et al. (see THANKS for the full author list)
%
% See LICENSE for the license information
%
% @brief Read graph from file and perform GraphSLAM
% @author Frank Dellaert
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Initialize graph, initial estimate, and odometry noise
model = gtsamSharedNoiseModel_Sigmas([0.05; 0.05; 0.05; 5*pi/180; 5*pi/180; 5*pi/180]);
[graph,initial]=load3D('../Data/sphere_smallnoise.graph',model,100);
%% Fix first pose
first = initial.pose(0);
graph.addHardConstraint(0, first); % add directly to graph
%% Plot Initial Estimate
figure(1);clf
plot3(initial.xs(),initial.ys(),initial.zs(),'g-'); hold on
plot3(first.x(),first.y(),first.z(),'r*');
%% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
result = graph.optimize(initial);
hold on; plot3(result.xs(),result.ys(),result.zs(),'b-');axis equal;

38
examples/matlab/load3D.m Normal file
View File

@ -0,0 +1,38 @@
function [graph,initial] = load3D(filename,model,N)
% load3D: read TORO 3D pose graph
% cannot read noise model from file yet, uses specified model
fid = fopen(filename);
if fid < 0
error(['load2D: Cannot open file ' filename]);
end
% scan all lines into a cell array
columns=textscan(fid,'%s','delimiter','\n');
fclose(fid);
lines=columns{1};
% loop over lines and add vertices
graph = pose3SLAMGraph;
initial = pose3SLAMValues;
n=size(lines,1);
if nargin<3, N=n;end
for i=1:n
line_i=lines{i};
if strcmp('VERTEX3',line_i(1:7))
v = textscan(line_i,'%s %d %f %f %f %f %f %f',1);
if v{2}<N
t = gtsamPoint3(v{3}, v{4}, v{5});
R = gtsamRot3_ypr(v{6}, v{7}, v{8});
initial.insertPose(v{2}, gtsamPose3(R,t));
end
elseif strcmp('EDGE3',line_i(1:5))
e = textscan(line_i,'%s %d %d %f %f %f %f %f %f',1);
if e{2}<N & e{3}<N
t = gtsamPoint3(e{4}, e{5}, e{6});
R = gtsamRot3_ypr(e{7}, e{8}, e{9});
graph.addConstraint(e{2}, e{3}, gtsamPose3(R,t), model);
end
end
end

96
gtsam.h
View File

@ -447,9 +447,10 @@ namespace pose2SLAM {
class Values {
Values();
void insertPose(size_t key, const gtsam::Pose2& pose);
size_t size() const;
void print(string s) const;
static pose2SLAM::Values Circle(size_t n, double R);
void insertPose(size_t key, const gtsam::Pose2& pose);
gtsam::Pose2 pose(size_t i);
Vector xs() const;
Vector ys() const;
@ -459,26 +460,79 @@ class Values {
class Graph {
Graph();
// FactorGraph
void print(string s) const;
bool equals(const pose2SLAM::Graph& fg, double tol) const;
size_t size() const;
bool empty() const;
void remove(size_t i);
size_t nrFactors() const;
// NonlinearFactorGraph
double error(const pose2SLAM::Values& values) const;
double probPrime(const pose2SLAM::Values& values) const;
gtsam::Ordering* orderingCOLAMD(const pose2SLAM::Values& values) const;
gtsam::GaussianFactorGraph* linearize(const pose2SLAM::Values& values,
const gtsam::Ordering& ordering) const;
void addPrior(size_t key, const gtsam::Pose2& pose,
const gtsam::SharedNoiseModel& noiseModel);
// pose2SLAM-specific
void addPrior(size_t key, const gtsam::Pose2& pose, const gtsam::SharedNoiseModel& noiseModel);
void addPoseConstraint(size_t key, const gtsam::Pose2& pose);
void addOdometry(size_t key1, size_t key2, const gtsam::Pose2& odometry,
const gtsam::SharedNoiseModel& noiseModel);
void addConstraint(size_t key1, size_t key2, const gtsam::Pose2& odometry,
const gtsam::SharedNoiseModel& noiseModel);
void addOdometry(size_t key1, size_t key2, const gtsam::Pose2& odometry, const gtsam::SharedNoiseModel& noiseModel);
void addConstraint(size_t key1, size_t key2, const gtsam::Pose2& odometry, const gtsam::SharedNoiseModel& noiseModel);
pose2SLAM::Values optimize(const pose2SLAM::Values& initialEstimate) const;
gtsam::Marginals marginals(const pose2SLAM::Values& solution) const;
};
}///\namespace pose2SLAM
//*************************************************************************
// Pose3SLAM
//*************************************************************************
#include <gtsam/slam/pose3SLAM.h>
namespace pose3SLAM {
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);
gtsam::Pose3 pose(size_t i);
Vector xs() const;
Vector ys() const;
Vector zs() const;
};
class Graph {
Graph();
// FactorGraph
void print(string s) const;
bool equals(const pose3SLAM::Graph& fg, double tol) const;
size_t size() const;
bool empty() const;
void remove(size_t i);
size_t nrFactors() const;
// NonlinearFactorGraph
double error(const pose3SLAM::Values& values) const;
double probPrime(const pose3SLAM::Values& values) const;
gtsam::Ordering* orderingCOLAMD(const pose3SLAM::Values& values) const;
gtsam::GaussianFactorGraph* linearize(const pose3SLAM::Values& values,
const gtsam::Ordering& ordering) const;
// pose3SLAM-specific
void addPrior(size_t key, const gtsam::Pose3& p, const gtsam::SharedNoiseModel& model);
void addConstraint(size_t key1, size_t key2, const gtsam::Pose3& z, const gtsam::SharedNoiseModel& model);
void addHardConstraint(size_t i, const gtsam::Pose3& p);
pose3SLAM::Values optimize(const pose3SLAM::Values& initialEstimate) const;
gtsam::Marginals marginals(const pose3SLAM::Values& solution) const;
};
}///\namespace pose3SLAM
//*************************************************************************
// planarSLAM
//*************************************************************************
@ -488,9 +542,10 @@ namespace planarSLAM {
class Values {
Values();
size_t size() const;
void print(string s) const;
void insertPose(size_t key, const gtsam::Pose2& pose);
void insertPoint(size_t key, const gtsam::Point2& point);
void print(string s) const;
gtsam::Pose2 pose(size_t key) const;
gtsam::Point2 point(size_t key) const;
};
@ -498,25 +553,30 @@ class Values {
class Graph {
Graph();
// FactorGraph
void print(string s) const;
bool equals(const planarSLAM::Graph& fg, double tol) const;
size_t size() const;
bool empty() const;
void remove(size_t i);
size_t nrFactors() const;
// NonlinearFactorGraph
double error(const planarSLAM::Values& values) const;
double probPrime(const planarSLAM::Values& values) const;
gtsam::Ordering* orderingCOLAMD(const planarSLAM::Values& values) const;
gtsam::GaussianFactorGraph* linearize(const planarSLAM::Values& values,
const gtsam::Ordering& ordering) const;
void addPrior(size_t key, const gtsam::Pose2& pose,
const gtsam::SharedNoiseModel& noiseModel);
// planarSLAM-specific
void addPrior(size_t key, const gtsam::Pose2& pose, const gtsam::SharedNoiseModel& noiseModel);
void addPoseConstraint(size_t key, const gtsam::Pose2& pose);
void addOdometry(size_t key1, size_t key2, const gtsam::Pose2& odometry,
const gtsam::SharedNoiseModel& noiseModel);
void addBearing(size_t poseKey, size_t pointKey, const gtsam::Rot2& bearing,
const gtsam::SharedNoiseModel& noiseModel);
void addRange(size_t poseKey, size_t pointKey, double range,
const gtsam::SharedNoiseModel& noiseModel);
void addBearingRange(size_t poseKey, size_t pointKey, const gtsam::Rot2& bearing,
double range, const gtsam::SharedNoiseModel& noiseModel);
void addOdometry(size_t key1, size_t key2, const gtsam::Pose2& odometry, const gtsam::SharedNoiseModel& noiseModel);
void addBearing(size_t poseKey, size_t pointKey, const gtsam::Rot2& bearing, const gtsam::SharedNoiseModel& noiseModel);
void addRange(size_t poseKey, size_t pointKey, double range, const gtsam::SharedNoiseModel& noiseModel);
void addBearingRange(size_t poseKey, size_t pointKey, const gtsam::Rot2& bearing,double range, const gtsam::SharedNoiseModel& noiseModel);
planarSLAM::Values optimize(const planarSLAM::Values& initialEstimate);
gtsam::Marginals marginals(const planarSLAM::Values& solution) const;
};
class Odometry {

21
gtsam/slam/planarSLAM.h
View File

@ -24,8 +24,13 @@
#include <gtsam/slam/BearingRangeFactor.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/geometry/Pose2.h>
/**
* @defgroup SLAM
*/
// Use planarSLAM namespace for specific SLAM instance
namespace planarSLAM {
@ -48,9 +53,9 @@ namespace planarSLAM {
/// A factor between a pose and a point to express difference in rotation and location
typedef BearingRangeFactor<Pose2, Point2> BearingRange;
/**
* Values class, using specific poses and points
* Mainly as a convenience for MATLAB wrapper, which does not allow for identically named methods
/*
* Values class, inherited from Values, mainly used as a convenience for MATLAB wrapper
* @ingroup SLAM
*/
struct Values: public gtsam::Values {
@ -75,7 +80,10 @@ namespace planarSLAM {
void insertPoint(Key j, const Point2& point) { insert(j, point); }
};
/// Creates a NonlinearFactorGraph with the Values type
/**
* Graph class, inherited from NonlinearFactorGraph, used as a convenience for MATLAB wrapper
* @ingroup SLAM
*/
struct Graph: public NonlinearFactorGraph {
/// Default constructor for a NonlinearFactorGraph
@ -104,6 +112,11 @@ namespace planarSLAM {
/// Optimize
Values optimize(const Values& initialEstimate) const;
/// Return a Marginals object
Marginals marginals(const Values& solution) const {
return Marginals(*this,solution);
}
};
} // planarSLAM

2
gtsam/slam/pose2SLAM.cpp
View File

@ -23,7 +23,7 @@
namespace pose2SLAM {
/* ************************************************************************* */
Values circle(size_t n, double R) {
Values Values::Circle(size_t n, double R) {
Values x;
double theta = 0, dtheta = 2 * M_PI / n;
for (size_t i = 0; i < n; i++, theta += dtheta)

28
gtsam/slam/pose2SLAM.h
View File

@ -30,7 +30,10 @@ namespace pose2SLAM {
using namespace gtsam;
/// Values class, inherited from Values, mainly used as a convenience for MATLAB wrapper
/*
* Values class, inherited from Values, mainly used as a convenience for MATLAB wrapper
* @ingroup SLAM
*/
struct Values: public gtsam::Values {
typedef boost::shared_ptr<Values> shared_ptr;
@ -43,6 +46,15 @@ namespace pose2SLAM {
gtsam::Values(values) {
}
/**
* Create a circle of n 2D poses tangent to circle of radius R, first pose at (R,0)
* @param n number of poses
* @param R radius of circle
* @param c character to use for keys
* @return circle of n 2D poses
*/
static Values Circle(size_t n, double R);
/// insert a pose
void insertPose(Key i, const Pose2& pose) { insert(i, pose); }
@ -54,15 +66,6 @@ namespace pose2SLAM {
Vector thetas() const; ///< get all orientations in a matrix
};
/**
* Create a circle of n 2D poses tangent to circle of radius R, first pose at (R,0)
* @param n number of poses
* @param R radius of circle
* @param c character to use for keys
* @return circle of n 2D poses
*/
Values circle(size_t n, double R);
/**
* List of typedefs for factors
*/
@ -74,7 +77,10 @@ namespace pose2SLAM {
/// A factor to add an odometry measurement between two poses.
typedef BetweenFactor<Pose2> Odometry;
/// Graph
/**
* Graph class, inherited from NonlinearFactorGraph, used as a convenience for MATLAB wrapper
* @ingroup SLAM
*/
struct Graph: public NonlinearFactorGraph {
typedef boost::shared_ptr<Graph> shared_ptr;

32
gtsam/slam/pose3SLAM.cpp
View File

@ -22,7 +22,7 @@
namespace pose3SLAM {
/* ************************************************************************* */
Values circle(size_t n, double radius) {
Values Values::Circle(size_t n, double radius) {
Values x;
double theta = 0, dtheta = 2 * M_PI / n;
// We use aerospace/navlab convention, X forward, Y right, Z down
@ -41,6 +41,36 @@ namespace pose3SLAM {
return x;
}
/* ************************************************************************* */
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;
}
/* ************************************************************************* */
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));

41
gtsam/slam/pose3SLAM.h
View File

@ -17,25 +17,53 @@
#pragma once
#include <gtsam/geometry/Pose3.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.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;
/*
* Values class, inherited from Values, mainly used as a convenience for MATLAB wrapper
* @ingroup SLAM
*/
struct Values: public gtsam::Values {
typedef boost::shared_ptr<Values> shared_ptr;
/// Default constructor
Values() {}
/// Copy constructor
Values(const gtsam::Values& values) :
gtsam::Values(values) {
}
/**
* Create a circle of n 3D poses tangent to circle of radius R, first pose at (R,0)
* @param n number of poses
* @param R radius of circle
* @return circle of n 3D poses
*/
Values circle(size_t n, double R);
static Values Circle(size_t n, double R);
/// insert a pose
void insertPose(Key i, const Pose3& pose) { insert(i, pose); }
/// 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
};
/// A prior factor on Key with Pose3 data type.
typedef PriorFactor<Pose3> Prior;
@ -44,7 +72,10 @@ namespace pose3SLAM {
/// A hard constraint would enforce that the given key would have the input value in the results.
typedef NonlinearEquality<Pose3> HardConstraint;
/// Graph
/**
* Graph class, inherited from NonlinearFactorGraph, used as a convenience for MATLAB wrapper
* @ingroup SLAM
*/
struct Graph: public NonlinearFactorGraph {
/// Adds a factor between keys of the same type
@ -61,6 +92,10 @@ namespace pose3SLAM {
return LevenbergMarquardtOptimizer(*this, initialEstimate).optimize();
}
/// Return a Marginals object
Marginals marginals(const Values& solution) const {
return Marginals(*this,solution);
}
};
} // pose3SLAM

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

@ -195,7 +195,7 @@ TEST_UNSAFE(Pose2SLAM, optimize) {
TEST_UNSAFE(Pose2SLAM, optimizeThreePoses) {
// Create a hexagon of poses
pose2SLAM::Values hexagon = pose2SLAM::circle(3,1.0);
pose2SLAM::Values hexagon = pose2SLAM::Values::Circle(3,1.0);
Pose2 p0 = hexagon.pose(0), p1 = hexagon.pose(1);
// create a Pose graph with one equality constraint and one measurement
@ -231,7 +231,7 @@ TEST_UNSAFE(Pose2SLAM, optimizeThreePoses) {
TEST_UNSAFE(Pose2SLAM, optimizeCircle) {
// Create a hexagon of poses
pose2SLAM::Values hexagon = pose2SLAM::circle(6,1.0);
pose2SLAM::Values hexagon = pose2SLAM::Values::Circle(6,1.0);
Pose2 p0 = hexagon.pose(0), p1 = hexagon.pose(1);
// create a Pose graph with one equality constraint and one measurement
@ -366,7 +366,7 @@ TEST_UNSAFE(Pose2Values, pose2Circle )
expected.insert(2, Pose2(-1, 0, - M_PI_2));
expected.insert(3, Pose2( 0, -1, 0 ));
pose2SLAM::Values actual = pose2SLAM::circle(4,1.0);
pose2SLAM::Values actual = pose2SLAM::Values::Circle(4,1.0);
CHECK(assert_equal(expected,actual));
}
@ -381,7 +381,7 @@ TEST_UNSAFE(Pose2SLAM, expmap )
expected.insert(3, Pose2( 0.1, -1, 0 ));
// Note expmap coordinates are in local coordinates, so shifting to right requires thought !!!
pose2SLAM::Values circle(pose2SLAM::circle(4,1.0));
pose2SLAM::Values circle = pose2SLAM::Values::Circle(4,1.0);
Ordering ordering(*circle.orderingArbitrary());
VectorValues delta(circle.dims(ordering));
delta[ordering[0]] = Vector_(3, 0.0,-0.1,0.0);

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

@ -43,7 +43,7 @@ TEST(Pose3Graph, optimizeCircle) {
// Create a hexagon of poses
double radius = 10;
Values hexagon = pose3SLAM::circle(6,radius);
Values hexagon = pose3SLAM::Values::Circle(6,radius);
Pose3 gT0 = hexagon.at<Pose3>(0), gT1 = hexagon.at<Pose3>(1);
// create a Pose graph with one equality constraint and one measurement
@ -179,7 +179,7 @@ TEST( Values, pose3Circle )
expected.insert(2, Pose3(R3, Point3(-1, 0, 0)));
expected.insert(3, Pose3(R4, Point3( 0,-1, 0)));
Values actual = pose3SLAM::circle(4,1.0);
Values actual = pose3SLAM::Values::Circle(4,1.0);
CHECK(assert_equal(expected,actual));
}
@ -199,7 +199,7 @@ TEST( Values, expmap )
0.0,0.0,0.0, 0.1, 0.0, 0.0,
0.0,0.0,0.0, 0.1, 0.0, 0.0,
0.0,0.0,0.0, 0.1, 0.0, 0.0);
Values actual = pose3SLAM::circle(4,1.0).retract(delta, ordering);
Values actual = pose3SLAM::Values::Circle(4,1.0).retract(delta, ordering);
CHECK(assert_equal(expected,actual));
}