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

Replaced calls to the namespace Circle functions with the newly added matlab function

release/4.3a0
Stephen Williams 2012-07-26 13:33:40 +00:00
parent 078eb1bb4f
commit c2935c5dd1
3 changed files with 30 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
matlab/examples/Pose2SLAMExample_circle.m
View File

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

19
matlab/examples/Pose3SLAMExample.m
View File

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

19
matlab/tests/testPose3SLAMExample.m
View File

@ -10,13 +10,14 @@
% @author Frank Dellaert % @author Frank Dellaert
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
import gtsam.*
%% Create a hexagon of poses %% Create a hexagon of poses
hexagon = pose3SLAM.Values.Circle(6,1.0); hexagon = circlePose3(6,1.0);
p0 = hexagon.pose(0); p0 = hexagon.at(0);
p1 = hexagon.pose(1); p1 = hexagon.at(1);
%% create a Pose graph with one equality constraint and one measurement %% create a Pose graph with one equality constraint and one measurement
import gtsam.*
fg = NonlinearFactorGraph; fg = NonlinearFactorGraph;
fg.add(NonlinearEqualityPose3(0, p0)); fg.add(NonlinearEqualityPose3(0, p0));
delta = p0.between(p1); delta = p0.between(p1);
@ -32,11 +33,11 @@ fg.add(BetweenFactorPose3(5,0, delta, covariance));
initial = Values; initial = Values;
s = 0.10; s = 0.10;
initial.insert(0, p0); initial.insert(0, p0);
initial.insert(1, hexagon.pose(1).retract(s*randn(6,1))); initial.insert(1, hexagon.at(1).retract(s*randn(6,1)));
initial.insert(2, hexagon.pose(2).retract(s*randn(6,1))); initial.insert(2, hexagon.at(2).retract(s*randn(6,1)));
initial.insert(3, hexagon.pose(3).retract(s*randn(6,1))); initial.insert(3, hexagon.at(3).retract(s*randn(6,1)));
initial.insert(4, hexagon.pose(4).retract(s*randn(6,1))); initial.insert(4, hexagon.at(4).retract(s*randn(6,1)));
initial.insert(5, hexagon.pose(5).retract(s*randn(6,1))); initial.insert(5, hexagon.at(5).retract(s*randn(6,1)));
%% optimize %% optimize
optimizer = LevenbergMarquardtOptimizer(fg, initial); optimizer = LevenbergMarquardtOptimizer(fg, initial);