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

Fixed many utilities and examples

release/4.3a0
dellaert 2014-11-14 00:51:11 +01:00
parent 4fb83694a7
commit b7dc6b3687
17 changed files with 88 additions and 72 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
matlab/+gtsam/VisualISAMPlot.m
View File

@ -16,7 +16,7 @@ gtsam.plot3DPoints(result, [], marginals);
M = 1;
while result.exists(symbol('x',M))
ii = symbol('x',M);
pose_i = result.at(ii);
pose_i = result.atPose3(ii);
if options.hardConstraint && (M==1)
gtsam.plotPose3(pose_i,[],10);
else

2
matlab/+gtsam/VisualISAMStep.m
View File

@ -27,7 +27,7 @@ for k=1:length(data.Z{nextPoseIndex})
end
%% Initial estimates for the new pose.
prevPose = result.at(symbol('x',nextPoseIndex-1));
prevPose = result.atPose3(symbol('x',nextPoseIndex-1));
initialEstimates.insert(symbol('x',nextPoseIndex), prevPose.compose(odometry));
%% Update ISAM

4
matlab/+gtsam/load3D.m
View File

@ -46,9 +46,9 @@ for i=1:n
graph.add(BetweenFactorPose3(i1, i2, dpose, model));
if successive
if i2>i1
initial.insert(i2,initial.at(i1).compose(dpose));
initial.insert(i2,initial.atPose3(i1).compose(dpose));
else
initial.insert(i1,initial.at(i2).compose(dpose.inverse));
initial.insert(i1,initial.atPose3(i2).compose(dpose.inverse));
end
end
end

7
matlab/+gtsam/plot2DPoints.m
View File

@ -18,15 +18,18 @@ hold on
% Plot points and covariance matrices
for i = 0:keys.size-1
key = keys.at(i);
p = values.at(key);
if isa(p, 'gtsam.Point2')
try
p = values.atPoint2(key);
if haveMarginals
P = marginals.marginalCovariance(key);
gtsam.plotPoint2(p, linespec, P);
else
gtsam.plotPoint2(p, linespec);
end
catch err
% I guess it's not a Point2
end
end
if ~holdstate

30
matlab/+gtsam/plot2DTrajectory.m
View File

@ -8,7 +8,7 @@ function plot2DTrajectory(values, linespec, marginals)
import gtsam.*
if ~exist('linespec', 'var') || isempty(linespec)
linespec = 'k*-';
linespec = 'k*-';
end
haveMarginals = exist('marginals', 'var');
@ -25,24 +25,26 @@ plot(X,Y,linespec);
% Quiver can also be vectorized if no marginals asked
if ~haveMarginals
C = cos(theta);
S = sin(theta);
quiver(X,Y,C,S,0.1,linespec);
C = cos(theta);
S = sin(theta);
quiver(X,Y,C,S,0.1,linespec);
else
% plotPose2 does both quiver and covariance matrix
keys = KeyVector(values.keys);
for i = 0:keys.size-1
key = keys.at(i);
x = values.at(key);
if isa(x, 'gtsam.Pose2')
P = marginals.marginalCovariance(key);
gtsam.plotPose2(x,linespec(1), P);
% plotPose2 does both quiver and covariance matrix
keys = KeyVector(values.keys);
for i = 0:keys.size-1
key = keys.at(i);
try
x = values.atPose2(key);
P = marginals.marginalCovariance(key);
gtsam.plotPose2(x,linespec(1), P);
catch err
% I guess it's not a Pose2
end
end
end
end
if ~holdstate
hold off
hold off
end
end

6
matlab/+gtsam/plot3DPoints.m
View File

@ -18,14 +18,16 @@ hold on
% Plot points and covariance matrices
for i = 0:keys.size-1
key = keys.at(i);
p = values.at(key);
if isa(p, 'gtsam.Point3')
try
p = values.atPoint3(key);
if haveMarginals
P = marginals.marginalCovariance(key);
gtsam.plotPoint3(p, linespec, P);
else
gtsam.plotPoint3(p, linespec);
end
catch
% I guess it's not a Point3
end
end

53
matlab/+gtsam/plot3DTrajectory.m
View File

@ -17,39 +17,48 @@ hold on
lastIndex = [];
for i = 0:keys.size-1
key = keys.at(i);
x = values.at(key);
if isa(x, 'gtsam.Pose3')
try
x = values.atPose3(key);
if ~isempty(lastIndex)
% Draw line from last pose then covariance ellipse on top of
% last pose.
lastKey = keys.at(lastIndex);
lastPose = values.at(lastKey);
plot3([ x.x; lastPose.x ], [ x.y; lastPose.y ], [ x.z; lastPose.z ], linespec);
try
lastPose = values.atPose3(lastKey);
plot3([ x.x; lastPose.x ], [ x.y; lastPose.y ], [ x.z; lastPose.z ], linespec);
if haveMarginals
P = marginals.marginalCovariance(lastKey);
else
P = [];
end
gtsam.plotPose3(lastPose, P, scale);
catch err
warning(['no Pose3 at ' lastKey]);
end
lastIndex = i;
end
catch
warning(['no Pose3 at ' key]);
end
% Draw final pose
if ~isempty(lastIndex)
lastKey = keys.at(lastIndex);
try
lastPose = values.atPose3(lastKey);
if haveMarginals
P = marginals.marginalCovariance(lastKey);
else
P = [];
end
gtsam.plotPose3(lastPose, P, scale);
catch
warning(['no Pose3 at ' lastIndex]);
end
lastIndex = i;
end
end
% Draw final pose
if ~isempty(lastIndex)
lastKey = keys.at(lastIndex);
lastPose = values.at(lastKey);
if haveMarginals
P = marginals.marginalCovariance(lastKey);
else
P = [];
if ~holdstate
hold off
end
gtsam.plotPose3(lastPose, P, scale);
end
if ~holdstate
hold off
end
end

6
matlab/gtsam_examples/PlanarSLAMExample.m
View File

@ -71,9 +71,9 @@ marginals = Marginals(graph, result);
plot2DTrajectory(result, [], marginals);
plot2DPoints(result, 'b', marginals);
plot([result.at(i1).x; result.at(j1).x],[result.at(i1).y; result.at(j1).y], 'c-');
plot([result.at(i2).x; result.at(j1).x],[result.at(i2).y; result.at(j1).y], 'c-');
plot([result.at(i3).x; result.at(j2).x],[result.at(i3).y; result.at(j2).y], 'c-');
plot([result.atPose2(i1).x; result.atPoint2(j1).x],[result.atPose2(i1).y; result.atPoint2(j1).y], 'c-');
plot([result.atPose2(i2).x; result.atPoint2(j1).x],[result.atPose2(i2).y; result.atPoint2(j1).y], 'c-');
plot([result.atPose2(i3).x; result.atPoint2(j2).x],[result.atPose2(i3).y; result.atPoint2(j2).y], 'c-');
axis([-0.6 4.8 -1 1])
axis equal
view(2)

2
matlab/gtsam_examples/PlanarSLAMExample_graph.m
View File

@ -22,7 +22,7 @@ model = noiseModel.Diagonal.Sigmas([0.05; 0.05; 2*pi/180]);
[graph,initial] = load2D(datafile, model);
%% Add a Gaussian prior on a pose in the middle
priorMean = initial.at(40);
priorMean = initial.atPose2(40);
priorNoise = noiseModel.Diagonal.Sigmas([0.1; 0.1; 2*pi/180]);
graph.add(PriorFactorPose2(40, priorMean, priorNoise)); % add directly to graph

8
matlab/gtsam_examples/PlanarSLAMExample_sampling.m
View File

@ -55,14 +55,14 @@ plot2DPoints(sample, [], marginals);
for j=1:2
key = symbol('l',j);
point{j} = sample.at(key);
point{j} = sample.atPoint2(key);
Q{j}=marginals.marginalCovariance(key);
S{j}=chol(Q{j}); % for sampling
end
plot([sample.at(i1).x; sample.at(j1).x],[sample.at(i1).y; sample.at(j1).y], 'c-');
plot([sample.at(i2).x; sample.at(j1).x],[sample.at(i2).y; sample.at(j1).y], 'c-');
plot([sample.at(i3).x; sample.at(j2).x],[sample.at(i3).y; sample.at(j2).y], 'c-');
plot([sample.atPose2(i1).x; sample.atPoint2(j1).x],[sample.atPose2(i1).y; sample.atPoint2(j1).y], 'c-');
plot([sample.atPose2(i2).x; sample.atPoint2(j1).x],[sample.atPose2(i2).y; sample.atPoint2(j1).y], 'c-');
plot([sample.atPose2(i3).x; sample.atPoint2(j2).x],[sample.atPose2(i3).y; sample.atPoint2(j2).y], 'c-');
view(2); axis auto; axis equal
%% Do Sampling on point 2

2
matlab/gtsam_examples/Pose2SLAMExample.m
View File

@ -57,7 +57,7 @@ result.print(sprintf('\nFinal result:\n'));
%% Plot Covariance Ellipses
cla;
hold on
plot([result.at(5).x;result.at(2).x],[result.at(5).y;result.at(2).y],'r-');
plot([result.atPose2(5).x;result.atPose2(2).x],[result.atPose2(5).y;result.atPose2(2).y],'r-');
marginals = Marginals(graph, result);
plot2DTrajectory(result, [], marginals);

14
matlab/gtsam_examples/Pose2SLAMExample_circle.m
View File

@ -14,8 +14,8 @@ import gtsam.*
%% Create a hexagon of poses
hexagon = circlePose2(6,1.0);
p0 = hexagon.at(0);
p1 = hexagon.at(1);
p0 = hexagon.atPose2(0);
p1 = hexagon.atPose2(1);
%% create a Pose graph with one equality constraint and one measurement
fg = NonlinearFactorGraph;
@ -32,11 +32,11 @@ fg.add(BetweenFactorPose2(5,0, delta, covariance));
%% Create initial config
initial = Values;
initial.insert(0, p0);
initial.insert(1, hexagon.at(1).retract([-0.1, 0.1,-0.1]'));
initial.insert(2, hexagon.at(2).retract([ 0.1,-0.1, 0.1]'));
initial.insert(3, hexagon.at(3).retract([-0.1, 0.1,-0.1]'));
initial.insert(4, hexagon.at(4).retract([ 0.1,-0.1, 0.1]'));
initial.insert(5, hexagon.at(5).retract([-0.1, 0.1,-0.1]'));
initial.insert(1, hexagon.atPose2(1).retract([-0.1, 0.1,-0.1]'));
initial.insert(2, hexagon.atPose2(2).retract([ 0.1,-0.1, 0.1]'));
initial.insert(3, hexagon.atPose2(3).retract([-0.1, 0.1,-0.1]'));
initial.insert(4, hexagon.atPose2(4).retract([ 0.1,-0.1, 0.1]'));
initial.insert(5, hexagon.atPose2(5).retract([-0.1, 0.1,-0.1]'));
%% Plot Initial Estimate
cla

2
matlab/gtsam_examples/Pose2SLAMExample_graph.m
View File

@ -41,7 +41,7 @@ marginals = Marginals(graph, result);
toc
P={};
for i=1:result.size()-1
pose_i = result.at(i);
pose_i = result.atPose2(i);
P{i}=marginals.marginalCovariance(i);
plotPose2(pose_i,'b',P{i})
end

14
matlab/gtsam_examples/Pose3SLAMExample.m
View File

@ -14,8 +14,8 @@ import gtsam.*
%% Create a hexagon of poses
hexagon = circlePose3(6,1.0);
p0 = hexagon.at(0);
p1 = hexagon.at(1);
p0 = hexagon.atPose3(0);
p1 = hexagon.atPose3(1);
%% create a Pose graph with one equality constraint and one measurement
fg = NonlinearFactorGraph;
@ -33,11 +33,11 @@ fg.add(BetweenFactorPose3(5,0, delta, covariance));
initial = Values;
s = 0.10;
initial.insert(0, p0);
initial.insert(1, hexagon.at(1).retract(s*randn(6,1)));
initial.insert(2, hexagon.at(2).retract(s*randn(6,1)));
initial.insert(3, hexagon.at(3).retract(s*randn(6,1)));
initial.insert(4, hexagon.at(4).retract(s*randn(6,1)));
initial.insert(5, hexagon.at(5).retract(s*randn(6,1)));
initial.insert(1, hexagon.atPose3(1).retract(s*randn(6,1)));
initial.insert(2, hexagon.atPose3(2).retract(s*randn(6,1)));
initial.insert(3, hexagon.atPose3(3).retract(s*randn(6,1)));
initial.insert(4, hexagon.atPose3(4).retract(s*randn(6,1)));
initial.insert(5, hexagon.atPose3(5).retract(s*randn(6,1)));
%% Plot Initial Estimate
cla

2
matlab/gtsam_examples/Pose3SLAMExample_graph.m
View File

@ -28,7 +28,7 @@ model = noiseModel.Diagonal.Sigmas([5*pi/180; 5*pi/180; 5*pi/180; 0.05; 0.05; 0.
%% Plot Initial Estimate
cla
first = initial.at(0);
first = initial.atPose3(0);
plot3(first.x(),first.y(),first.z(),'r*'); hold on
plot3DTrajectory(initial,'g-',false);
drawnow;

4
matlab/gtsam_examples/StereoVOExample_large.m
View File

@ -45,7 +45,7 @@ for i=1:size(measurements,1)
if ~initial.exists(symbol('l',sf(2)))
% 3D landmarks are stored in camera coordinates: transform
% to world coordinates using the respective initial pose
pose = initial.at(symbol('x', sf(1)));
pose = initial.atPose3(symbol('x', sf(1)));
world_point = pose.transform_from(Point3(sf(6),sf(7),sf(8)));
initial.insert(symbol('l',sf(2)), world_point);
end
@ -54,7 +54,7 @@ toc
%% add a constraint on the starting pose
key = symbol('x',1);
first_pose = initial.at(key);
first_pose = initial.atPose3(key);
graph.add(NonlinearEqualityPose3(key, first_pose));
%% optimize

2
wrap/StaticMethod.h
View File

@ -42,7 +42,7 @@ struct StaticMethod: public Function, public SignatureOverloads {
// emit a list of comments, one for each overload
void comment_fragment(FileWriter& proxyFile) const {
SignatureOverloads::comment_fragment(proxyFile, name_);
SignatureOverloads::comment_fragment(proxyFile, matlabName());
}
void verifyArguments(const std::vector<std::string>& validArgs) const {