From 078eb1bb4fd8929a07952fd81db339a1a4465de6 Mon Sep 17 00:00:00 2001
From: Stephen Williams <stephen.vincent.williams@gmail.com>
Date: Thu, 26 Jul 2012 13:23:04 +0000
Subject: [PATCH] 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.

---
 matlab/circlePose2.m | 19 +++++++++++++++----
 matlab/circlePose3.m | 20 ++++++++++++--------
 2 files changed, 27 insertions(+), 12 deletions(-)

diff --git a/matlab/circlePose2.m b/matlab/circlePose2.m
index 4da70f9bf..7153fc128 100644
--- a/matlab/circlePose2.m
+++ b/matlab/circlePose2.m
@@ -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;
diff --git a/matlab/circlePose3.m b/matlab/circlePose3.m
index bd102ad78..0e74a2f1b 100644
--- a/matlab/circlePose3.m
+++ b/matlab/circlePose3.m
@@ -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