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.

release/4.3a0
Stephen Williams 2012-07-26 13:08:54 +00:00
parent 7fc6a622bf
commit f93760327e
2 changed files with 48 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
matlab/circlePose2.m Normal file
View File

@ -0,0 +1,19 @@
function values = circlePose2(numPoses, radius, symbolChar)
% circlePose2: 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
% will be stored in the msb of each key (i.e. gtsam.Symbol).
if nargin<3,symbolChar=0x00;end
if nargin<2,radius=1.0;end
if nargin<1,numPoses=8;end
values = gtsam.Values;
theta = 0.0;
dtheta = 2*pi()/numPoses;
for i = 1:numPoses
key = gtsam.Symbol(symbolChar, i);
pose = gtsam.Pose2(radius*cos(theta), radius*sin(theta), pi()/2 + theta);
values.insert(key, pose);
theta = theta + dtheta;
end

29
matlab/circlePose3.m Normal file
View File

@ -0,0 +1,29 @@
function values = circlePose2(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
% 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<2,radius=1.0;end
if nargin<1,numPoses=8;end
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));
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);
values.insert(key, gTi);
theta = theta + dtheta;
end