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

Added matlab functions for generating poses in a circle. This replaces the need for the SLAM namespace functions of the same purpose. I mean it this time. Edited the wrong copy before committing.

release/4.3a0
Stephen Williams 2012-07-26 13:23:04 +00:00
parent f93760327e
commit 078eb1bb4f
2 changed files with 27 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
matlab/circlePose2.m
View File

@ -1,18 +1,29 @@
function values = circlePose2(numPoses, radius, symbolChar)
% circlePose2: generate a set of poses in a circle. This function
% circlePose3: generate a set of poses in a circle. This function
% returns those poses inside a gtsam.Values object, with sequential
% keys starting from 1. An optional character may be provided, which
% keys starting from 0. An optional character may be provided, which
% will be stored in the msb of each key (i.e. gtsam.Symbol).
%
% We use aerospace/navlab convention, X forward, Y right, Z down
% First pose will be at (R,0,0)
% ^y ^ X
% | |
% z-->xZ--> Y (z pointing towards viewer, Z pointing away from viewer)
% Vehicle at p0 is looking towards y axis (X-axis points towards world y)
if nargin<3,symbolChar=0x00;end
if nargin<3,symbolChar=0;end
if nargin<2,radius=1.0;end
if nargin<1,numPoses=8;end
% Force symbolChar to be a single character
symbolChar = char(symbolChar);
symbolChar = symbolChar(1);
values = gtsam.Values;
theta = 0.0;
dtheta = 2*pi()/numPoses;
for i = 1:numPoses
key = gtsam.Symbol(symbolChar, i);
key = gtsam.Symbol(symbolChar, i-1).key();
pose = gtsam.Pose2(radius*cos(theta), radius*sin(theta), pi()/2 + theta);
values.insert(key, pose);
theta = theta + dtheta;

20
matlab/circlePose3.m
View File

@ -1,7 +1,7 @@
function values = circlePose2(numPoses, radius, symbolChar)
function values = circlePose3(numPoses, radius, symbolChar)
% circlePose3: generate a set of poses in a circle. This function
% returns those poses inside a gtsam.Values object, with sequential
% keys starting from 1. An optional character may be provided, which
% keys starting from 0. An optional character may be provided, which
% will be stored in the msb of each key (i.e. gtsam.Symbol).
%
% We use aerospace/navlab convention, X forward, Y right, Z down
@ -11,19 +11,23 @@ function values = circlePose2(numPoses, radius, symbolChar)
% z-->xZ--> Y (z pointing towards viewer, Z pointing away from viewer)
% Vehicle at p0 is looking towards y axis (X-axis points towards world y)
if nargin<3,symbolChar=0x00;end
if nargin<3,symbolChar=0;end
if nargin<2,radius=1.0;end
if nargin<1,numPoses=8;end
% Force symbolChar to be a single character
symbolChar = char(symbolChar);
symbolChar = symbolChar(1);
values = gtsam.Values;
theta = 0.0;
dtheta = 2*pi()/numPoses;
gR0 = gtsam.Rot3(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1));
gRo = gtsam.Rot3([0, 1, 0 ; 1, 0, 0 ; 0, 0, -1]);
for i = 1:numPoses
key = gtsam.Symbol(symbolChar, i);
gti = Point3(radius*cos(theta), radius*sin(theta), 0);
_0Ri = gtsam.Rot3.yaw(-theta); % negative yaw goes counterclockwise, with Z down !
gTi = gtsam.Pose3(gR0.compose(_0Ri), gti);
key = gtsam.Symbol(symbolChar, i-1).key();
gti = gtsam.Point3(radius*cos(theta), radius*sin(theta), 0);
oRi = gtsam.Rot3.Yaw(-theta); % negative yaw goes counterclockwise, with Z down !
gTi = gtsam.Pose3(gRo.compose(oRi), gti);
values.insert(key, gTi);
theta = theta + dtheta;
end