Merge remote-tracking branch 'origin/develop' into feature/Feature/FixedValues

gtsam.h

@@ -629,28 +629,13 @@ class Cal3_S2 {
   void serialize() const;
 };
 
-#include <gtsam/geometry/Cal3DS2.h>
+#include <gtsam/geometry/Cal3DS2_Base.h>
-class Cal3DS2 {
+virtual class Cal3DS2_Base {
   // Standard Constructors
-  Cal3DS2();
+  Cal3DS2_Base();
-  Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2, double k3, double k4);
-  Cal3DS2(Vector v);
 
   // Testable
   void print(string s) const;
-  bool equals(const gtsam::Cal3DS2& rhs, double tol) const;
-
-  // Manifold
-  static size_t Dim();
-  size_t dim() const;
-  gtsam::Cal3DS2 retract(Vector v) const;
-  Vector localCoordinates(const gtsam::Cal3DS2& c) const;
-
-  // Action on Point2
-  gtsam::Point2 calibrate(const gtsam::Point2& p, double tol) const;
-  gtsam::Point2 calibrate(const gtsam::Point2& p) const;
-  gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
-  // TODO: D2d functions that start with an uppercase letter
 
   // Standard Interface
   double fx() const;
@@ -658,14 +643,66 @@ class Cal3DS2 {
   double skew() const;
   double px() const;
   double py() const;
-  Vector vector() const;
+  double k1() const;
-  Vector k() const;
+  double k2() const;
-  //Matrix K() const; //FIXME: Uppercase
+
+  // Action on Point2
+  gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
+  gtsam::Point2 calibrate(const gtsam::Point2& p, double tol) const;
+  gtsam::Point2 calibrate(const gtsam::Point2& p) const;
 
   // enabling serialization functionality
   void serialize() const;
 };
 
+#include <gtsam/geometry/Cal3DS2.h>
+virtual class Cal3DS2 : gtsam::Cal3DS2_Base {
+  // Standard Constructors
+  Cal3DS2();
+  Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2);
+  Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2, double p1, double p2);
+  Cal3DS2(Vector v);
+
+  // Testable
+  bool equals(const gtsam::Cal3DS2& rhs, double tol) const;
+
+  // Manifold
+  size_t dim() const;
+  static size_t Dim();
+  gtsam::Cal3DS2 retract(Vector v) const;
+  Vector localCoordinates(const gtsam::Cal3DS2& c) const;
+
+  // enabling serialization functionality
+  void serialize() const;
+};
+
+#include <gtsam/geometry/Cal3Unified.h>
+virtual class Cal3Unified : gtsam::Cal3DS2_Base {
+  // Standard Constructors
+  Cal3Unified();
+  Cal3Unified(double fx, double fy, double s, double u0, double v0, double k1, double k2);
+  Cal3Unified(double fx, double fy, double s, double u0, double v0, double k1, double k2, double p1, double p2, double xi);
+  Cal3Unified(Vector v);
+
+  // Testable
+  bool equals(const gtsam::Cal3Unified& rhs, double tol) const;
+
+  // Standard Interface
+  double xi() const;
+  gtsam::Point2 spaceToNPlane(const gtsam::Point2& p) const;
+  gtsam::Point2 nPlaneToSpace(const gtsam::Point2& p) const;
+
+  // Manifold
+  size_t dim() const;
+  static size_t Dim();
+  gtsam::Cal3Unified retract(Vector v) const;
+  Vector localCoordinates(const gtsam::Cal3Unified& c) const;
+
+  // enabling serialization functionality
+  void serialize() const;
+};
+
+#include <gtsam/geometry/Cal3_S2Stereo.h>
 class Cal3_S2Stereo {
   // Standard Constructors
   Cal3_S2Stereo();

gtsam/geometry/Cal3DS2.h

@@ -68,7 +68,7 @@ public:
   /// @{
 
   /// print with optional string
-  void print(const std::string& s = "") const ;
+  virtual void print(const std::string& s = "") const ;
 
   /// assert equality up to a tolerance
   bool equals(const Cal3DS2& K, double tol = 10e-9) const;
@@ -89,10 +89,20 @@ public:
   /// Return dimensions of calibration manifold object
   static size_t Dim() { return 9; }  //TODO: make a final dimension variable
 
+  /// @}
+  /// @name Clone
+  /// @{
+
+  /// @return a deep copy of this object
+  virtual boost::shared_ptr<Base> clone() const {
+    return boost::shared_ptr<Base>(new Cal3DS2(*this));
+  }
+
+  /// @}
+
 
 private:
 
-  /// @}
   /// @name Advanced Interface
   /// @{
 

gtsam/geometry/Cal3DS2_Base.h

@@ -45,9 +45,6 @@ protected:
   double p1_, p2_ ; // tangential distortion
 
 public:
-  Matrix3 K() const ;
-  Vector4 k() const { return Vector4(k1_, k2_, p1_, p2_); }
-  Vector9 vector() const ;
 
   /// @name Standard Constructors
   /// @{
@@ -59,6 +56,8 @@ public:
       double k1, double k2, double p1 = 0.0, double p2 = 0.0) :
   fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0), k1_(k1), k2_(k2), p1_(p1), p2_(p2) {}
 
+  virtual ~Cal3DS2_Base() {}
+
   /// @}
   /// @name Advanced Constructors
   /// @{
@@ -70,7 +69,7 @@ public:
   /// @{
 
   /// print with optional string
-  void print(const std::string& s = "") const ;
+  virtual void print(const std::string& s = "") const ;
 
   /// assert equality up to a tolerance
   bool equals(const Cal3DS2_Base& K, double tol = 10e-9) const;
@@ -106,6 +105,15 @@ public:
   /// Second tangential distortion coefficient
   inline double p2() const { return p2_;}
 
+  /// return calibration matrix -- not really applicable
+  Matrix3 K() const;
+
+  /// return distortion parameter vector
+  Vector4 k() const { return Vector4(k1_, k2_, p1_, p2_); }
+
+  /// Return all parameters as a vector
+  Vector9 vector() const;
+
   /**
    * convert intrinsic coordinates xy to (distorted) image coordinates uv
    * @param p point in intrinsic coordinates
@@ -126,9 +134,19 @@ public:
   /// Derivative of uncalibrate wrpt the calibration parameters
   Matrix29 D2d_calibration(const Point2& p) const ;
 
-private:
+  /// @}
+  /// @name Clone
+  /// @{
+
+  /// @return a deep copy of this object
+  virtual boost::shared_ptr<Cal3DS2_Base> clone() const {
+    return boost::shared_ptr<Cal3DS2_Base>(new Cal3DS2_Base(*this));
+  }
 
   /// @}
+
+private:
+
   /// @name Advanced Interface
   /// @{
 

gtsam/geometry/Cal3Unified.h

@@ -50,9 +50,8 @@ private:
   double xi_;  // mirror parameter
 
 public:
-  enum { dimension = 10 };
 
-    Vector10 vector() const ;
+  enum { dimension = 10 };
 
   /// @name Standard Constructors
   /// @{
@@ -77,7 +76,7 @@ public:
   /// @{
 
   /// print with optional string
-  void print(const std::string& s = "") const ;
+  virtual void print(const std::string& s = "") const ;
 
   /// assert equality up to a tolerance
   bool equals(const Cal3Unified& K, double tol = 10e-9) const;
@@ -125,6 +124,11 @@ public:
   /// Return dimensions of calibration manifold object
   static size_t Dim() { return 10; }  //TODO: make a final dimension variable
 
+  /// Return all parameters as a vector
+  Vector10 vector() const ;
+
+  /// @}
+
 private:
 
   /** Serialization function */

matlab/+gtsam/covarianceEllipse.m

@@ -8,6 +8,8 @@ function h = covarianceEllipse(x,P,color, k)
 % it is assumed x and y are the first two components of state x
 % k is scaling for std deviations, defaults to 1 std
 
+hold on
+
 [e,s] = eig(P(1:2,1:2));
 s1 = s(1,1);
 s2 = s(2,2);

matlab/+gtsam/covarianceEllipse3D.m

@@ -1,4 +1,4 @@
-function covarianceEllipse3D(c,P)
+function sc = covarianceEllipse3D(c,P,scale)
 % covarianceEllipse3D plots a Gaussian as an uncertainty ellipse
 % Based on Maybeck Vol 1, page 366
 % k=2.296 corresponds to 1 std, 68.26% of all probability
@@ -6,10 +6,16 @@ function covarianceEllipse3D(c,P)
 %
 % Modified from http://www.mathworks.com/matlabcentral/newsreader/view_thread/42966
 
+hold on
+
 [e,s] = svd(P);
 k = 11.82; 
 radii = k*sqrt(diag(s));
 
+if exist('scale', 'var')
+    radii = radii * scale;
+end
+
 % generate data for "unrotated" ellipsoid
 [xc,yc,zc] = ellipsoid(0,0,0,radii(1),radii(2),radii(3),8);
 

matlab/+gtsam/cylinderSampleProjection.m

@@ -0,0 +1,89 @@
+function [visiblePoints] = cylinderSampleProjection(K, pose, imageSize, cylinders)
+
+% Input: 
+% Output:
+%   visiblePoints:  data{k} 3D Point in overal point clouds with index k 
+%                   Z{k}    2D measurements in overal point clouds with index k   
+%                   index {i}{j}
+%                   i: the cylinder index;
+%                   j: the point index on the cylinder;
+%                   
+% @Description: Project sampled points on cylinder to camera frame
+% @Authors: Zhaoyang Lv
+
+import gtsam.*
+
+camera = SimpleCamera(pose, K);
+
+%% memory allocation
+cylinderNum = length(cylinders);
+
+%% check visiblity of points on each cylinder
+pointCloudIndex = 0;
+visiblePointIdx = 1;
+for i = 1:cylinderNum
+    
+    pointNum = length(cylinders{i}.Points);
+
+    % to check point visibility        
+    for j = 1:pointNum
+
+        pointCloudIndex  = pointCloudIndex + 1;
+                
+        % Cheirality Exception
+        sampledPoint3 = cylinders{i}.Points{j};
+        sampledPoint3local = pose.transform_to(sampledPoint3);        
+        if sampledPoint3local.z <= 0
+            continue; 
+        end
+        Z2d = camera.project(sampledPoint3);
+
+        % ignore points not visible in the scene
+        if Z2d.x < 0 || Z2d.x >= imageSize.x || Z2d.y < 0 || Z2d.y >= imageSize.y 
+            continue;       
+        end            
+
+        % ignore points occluded
+        % use a simple math hack to check occlusion:
+        %   1. All points in front of cylinders' surfaces are visible
+        %   2. For points behind the cylinders' surfaces, the cylinder
+        visible = true;
+        for k = 1:cylinderNum
+
+            rayCameraToPoint = pose.translation().between(sampledPoint3).vector();
+            rayCameraToCylinder = pose.translation().between(cylinders{k}.centroid).vector();
+            rayCylinderToPoint = cylinders{k}.centroid.between(sampledPoint3).vector();
+
+            % Condition 1: all points in front of the cylinders'
+            % surfaces are visible
+            if dot(rayCylinderToPoint, rayCameraToCylinder) < 0
+               continue;
+            else 
+                projectedRay = dot(rayCameraToCylinder, rayCameraToPoint) / norm(rayCameraToCylinder);
+                if projectedRay > 0
+                    %rayCylinderToProjected = rayCameraToCylinder - norm(projectedRay) / norm(rayCameraToPoint) * rayCameraToPoint;
+                    if rayCylinderToPoint(1) > cylinders{k}.radius && ...
+                            rayCylinderToPoint(2) > cylinders{k}.radius
+                       continue;
+                    else
+                        visible = false;
+                        break;
+                    end
+                end
+            end
+            
+        end
+        
+        if visible
+            visiblePoints.data{visiblePointIdx} = sampledPoint3;
+            visiblePoints.Z{visiblePointIdx} = Z2d;
+            visiblePoints.cylinderIdx{visiblePointIdx} = i;
+            visiblePoints.overallIdx{visiblePointIdx} = pointCloudIndex;
+            visiblePointIdx = visiblePointIdx + 1;
+        end
+
+    end
+    
+end
+    
+end

matlab/+gtsam/cylinderSampleProjectionStereo.m

@@ -0,0 +1,93 @@
+function [visiblePoints] = cylinderSampleProjectionStereo(K, pose, imageSize, cylinders)
+
+import gtsam.*
+
+%% memory allocation
+cylinderNum = length(cylinders);
+
+visiblePoints.data = cell(1);
+visiblePoints.Z = cell(1);
+visiblePoints.cylinderIdx = cell(1);
+visiblePoints.overallIdx = cell(1);
+
+%% check visiblity of points on each cylinder
+pointCloudIndex = 0;
+visiblePointIdx = 1;
+for i = 1:cylinderNum
+    
+    pointNum = length(cylinders{i}.Points);
+
+    % to check point visibility        
+    for j = 1:pointNum
+
+        pointCloudIndex  = pointCloudIndex + 1;
+                
+        % For Cheirality Exception
+        sampledPoint3 = cylinders{i}.Points{j};
+        sampledPoint3local = pose.transform_to(sampledPoint3);
+        if sampledPoint3local.z < 0
+            continue; 
+        end
+        
+        % measurements 
+        Z.du = K.fx() * K.baseline() / sampledPoint3local.z;  
+        Z.uL = K.fx() * sampledPoint3local.x / sampledPoint3local.z + K.px();
+        Z.uR = Z.uL + Z.du;
+        Z.v = K.fy() / sampledPoint3local.z + K.py();
+
+        % ignore points not visible in the scene
+        if Z.uL < 0 || Z.uL >= imageSize.x || ...
+                Z.uR < 0 || Z.uR >= imageSize.x || ...
+                Z.v < 0 || Z.v >= imageSize.y 
+            continue;       
+        end            
+
+        % too small disparity may call indeterminant system exception
+        if Z.du < 0.6
+            continue;
+        end
+        
+        % ignore points occluded
+        % use a simple math hack to check occlusion:
+        %   1. All points in front of cylinders' surfaces are visible
+        %   2. For points behind the cylinders' surfaces, the cylinder
+        visible = true;
+        for k = 1:cylinderNum
+
+            rayCameraToPoint = pose.translation().between(sampledPoint3).vector();
+            rayCameraToCylinder = pose.translation().between(cylinders{k}.centroid).vector();
+            rayCylinderToPoint = cylinders{k}.centroid.between(sampledPoint3).vector();
+
+            % Condition 1: all points in front of the cylinders'
+            % surfaces are visible
+            if dot(rayCylinderToPoint, rayCameraToCylinder) < 0
+               continue;
+            else 
+                projectedRay = dot(rayCameraToCylinder, rayCameraToPoint) / norm(rayCameraToCylinder);
+                if projectedRay > 0
+                    %rayCylinderToProjected = rayCameraToCylinder - norm(projectedRay) / norm(rayCameraToPoint) * rayCameraToPoint;
+                    if rayCylinderToPoint(1) > cylinders{k}.radius && ...
+                            rayCylinderToPoint(2) > cylinders{k}.radius
+                       continue;
+                    else
+                        visible = false;
+                        break;
+                    end
+                end
+            end
+            
+        end
+        
+        if visible
+            visiblePoints.data{visiblePointIdx} = sampledPoint3;
+            visiblePoints.Z{visiblePointIdx} = Z;
+            visiblePoints.cylinderIdx{visiblePointIdx} = i;
+            visiblePoints.overallIdx{visiblePointIdx} = pointCloudIndex;
+            visiblePointIdx = visiblePointIdx + 1;
+        end
+        
+    end
+
+end
+
+end

matlab/+gtsam/cylinderSampling.m

@@ -0,0 +1,26 @@
+function [cylinder] = cylinderSampling(baseCentroid, radius, height, density)
+% 
+% @author: Zhaoyang Lv
+    import gtsam.*
+    % calculate the cylinder area
+    area = 2 * pi * radius * height;
+    
+    pointsNum = round(area * density);
+    
+    points3 = cell(pointsNum, 1);
+    
+    % sample the points
+    for i = 1:pointsNum
+        theta = 2 * pi * rand;
+        x = radius * cos(theta) + baseCentroid.x;
+        y = radius * sin(theta) + baseCentroid.y;
+        z = height * rand;
+        points3{i,1} = Point3([x,y,z]');
+    end
+   
+    cylinder.area = area;
+    cylinder.radius = radius;
+    cylinder.height = height;
+    cylinder.Points = points3;
+    cylinder.centroid = Point3(baseCentroid.x, baseCentroid.y, height/2);
+end

matlab/+gtsam/plotCamera.m

@@ -1,18 +1,20 @@
 function plotCamera(pose, axisLength)
+	hold on
+
     C = pose.translation().vector();
     R = pose.rotation().matrix();
     
     xAxis = C+R(:,1)*axisLength;
     L = [C xAxis]';
-    line(L(:,1),L(:,2),L(:,3),'Color','r');
+    h_x = line(L(:,1),L(:,2),L(:,3),'Color','r');
     
     yAxis = C+R(:,2)*axisLength;
     L = [C yAxis]';
-    line(L(:,1),L(:,2),L(:,3),'Color','g');
+    h_y = line(L(:,1),L(:,2),L(:,3),'Color','g');
     
     zAxis = C+R(:,3)*axisLength;
     L = [C zAxis]';
-    line(L(:,1),L(:,2),L(:,3),'Color','b');
+    h_z = line(L(:,1),L(:,2),L(:,3),'Color','b');
     
     axis equal
 end

matlab/+gtsam/plotCylinderSamples.m

@@ -0,0 +1,35 @@
+function plotCylinderSamples(cylinders, options, figID)
+% plot the cylinders on the given field
+% @author: Zhaoyang Lv
+
+    figure(figID);
+
+    holdstate = ishold;
+    hold on
+    
+    num = size(cylinders, 1);
+
+    sampleDensity = 120;
+    
+    for i = 1:num                
+        [X,Y,Z] = cylinder(cylinders{i}.radius, sampleDensity * cylinders{i}.radius * cylinders{i}.height);
+        
+        X = X + cylinders{i}.centroid.x;
+        Y = Y + cylinders{i}.centroid.y;
+        Z = Z * cylinders{i}.height;
+        
+        cylinderHandle = surf(X,Y,Z);
+        set(cylinderHandle, 'FaceAlpha', 0.5);
+        hold on
+    end
+    
+    axis equal
+    axis([0, options.fieldSize.x, 0, options.fieldSize.y, 0, 20]);
+        
+    grid on
+    
+    if ~holdstate
+        hold off
+    end
+    
+end

matlab/+gtsam/plotFlyingResults.m

@@ -0,0 +1,177 @@
+function plotFlyingResults(pts3d, poses, posesCov, cylinders, options)
+% plot the visible points on the cylinders and trajectories
+%
+% author: Zhaoyang Lv
+
+import gtsam.*
+
+figID = 1;
+figure(figID);
+set(gcf, 'Position', [80,1,1800,1000]);
+
+
+%% plot all the cylinders and sampled points
+
+axis equal
+axis([0, options.fieldSize.x, 0, options.fieldSize.y, 0, options.height + 30]);
+xlabel('X (m)');
+ylabel('Y (m)');
+zlabel('Height (m)');
+
+h = cameratoolbar('Show');
+
+if options.camera.IS_MONO
+    h_title = title('Quadrotor Flight Simulation with Monocular Camera');
+else
+    h_title = title('Quadrotor Flight Simulation with Stereo Camera');
+end
+
+text(100,1750,0, sprintf('Flying Speed: %0.1f\n', options.speed))
+
+view([30, 30]);
+
+hlight = camlight('headlight'); 
+lighting gouraud
+
+if(options.writeVideo)
+    videoObj = VideoWriter('Camera_Flying_Example.avi');
+    videoObj.Quality = 100;
+    videoObj.FrameRate = options.camera.fps;
+    open(videoObj);
+end
+
+
+sampleDensity = 120;
+cylinderNum = length(cylinders);
+h_cylinder = cell(cylinderNum);
+for i = 1:cylinderNum
+    
+    hold on
+    
+    [X,Y,Z] = cylinder(cylinders{i}.radius, sampleDensity * cylinders{i}.radius * cylinders{i}.height);
+
+    X = X + cylinders{i}.centroid.x;
+    Y = Y + cylinders{i}.centroid.y;
+    Z = Z * cylinders{i}.height;
+
+    h_cylinder{i} = surf(X,Y,Z);
+    set(h_cylinder{i}, 'FaceColor', [0 0 1], 'FaceAlpha', 0.2);
+    h_cylinder{i}.AmbientStrength = 0.8;
+    
+end
+
+%% plot trajectories and points 
+posesSize = length(poses);
+pointSize = length(pts3d);
+for i = 1:posesSize
+    if i > 1
+        hold on
+        plot3([poses{i}.x; poses{i-1}.x], [poses{i}.y; poses{i-1}.y], [poses{i}.z; poses{i-1}.z], '-b');
+    end
+    
+    if exist('h_pose_cov', 'var')
+        delete(h_pose_cov);
+    end
+       
+    %plotCamera(poses{i}, 3);
+    
+    gRp = poses{i}.rotation().matrix();  % rotation from pose to global
+    C = poses{i}.translation().vector();
+    axisLength = 3;
+    
+    xAxis = C+gRp(:,1)*axisLength;
+    L = [C xAxis]';
+    line(L(:,1),L(:,2),L(:,3),'Color','r');
+    
+    yAxis = C+gRp(:,2)*axisLength;
+    L = [C yAxis]';
+    line(L(:,1),L(:,2),L(:,3),'Color','g');
+    
+    zAxis = C+gRp(:,3)*axisLength;
+    L = [C zAxis]';
+    line(L(:,1),L(:,2),L(:,3),'Color','b');
+    
+    pPp = posesCov{i}(4:6,4:6); % covariance matrix in pose coordinate frame    
+    gPp = gRp*pPp*gRp'; % convert the covariance matrix to global coordinate frame
+    h_pose_cov = gtsam.covarianceEllipse3D(C, gPp, options.plot.covarianceScale); 
+    
+    if exist('h_point', 'var')
+        for j = 1:pointSize
+            delete(h_point{j});
+        end
+    end
+    if exist('h_point_cov', 'var')
+        for j = 1:pointSize
+            delete(h_point_cov{j});
+        end
+    end
+    
+    h_point = cell(pointSize, 1);
+    h_point_cov = cell(pointSize, 1);
+    for j = 1:pointSize
+        if ~isempty(pts3d{j}.cov{i})
+            hold on
+            h_point{j} = plot3(pts3d{j}.data.x, pts3d{j}.data.y, pts3d{j}.data.z);
+            h_point_cov{j} = gtsam.covarianceEllipse3D([pts3d{j}.data.x; pts3d{j}.data.y; pts3d{j}.data.z], ...
+                pts3d{j}.cov{i}, options.plot.covarianceScale);
+        end
+    end 
+    
+    axis equal
+    axis([0, options.fieldSize.x, 0, options.fieldSize.y, 0, options.height + 30]);
+
+    drawnow
+    
+    if options.writeVideo
+        currFrame = getframe(gcf);
+        writeVideo(videoObj, currFrame);
+    end
+end
+
+
+if exist('h_pose_cov', 'var')
+    delete(h_pose_cov);
+end
+
+% wait for two seconds
+pause(2);
+
+%% change views angle
+for i = 30 : i : 90
+    view([30, i]);
+
+    if options.writeVideo
+        currFrame = getframe(gcf);
+        writeVideo(videoObj, currFrame);
+    end
+    
+    drawnow
+end
+
+% changing perspective
+
+
+%% camera flying through video
+camzoom(0.8);
+for i = 1 : posesSize
+    
+    hold on
+    
+    campos([poses{i}.x, poses{i}.y, poses{i}.z]);
+    camtarget([options.fieldSize.x/2, options.fieldSize.y/2, 0]);
+    camlight(hlight, 'headlight');
+    
+    if options.writeVideo
+        currFrame = getframe(gcf);
+        writeVideo(videoObj, currFrame);
+    end
+    
+    drawnow
+end
+
+%%close video
+if(options.writeVideo)
+    close(videoObj);
+end
+
+end

matlab/+gtsam/points2DTrackMonocular.m

@@ -0,0 +1,113 @@
+function pts2dTracksMono = points2DTrackMonocular(K, cameraPoses, imageSize, cylinders)
+% Assess how accurately we can reconstruct points from a particular monocular camera setup. 
+% After creation of the factor graph for each track, linearize it around ground truth. 
+% There is no optimization
+% @author: Zhaoyang Lv
+
+import gtsam.*
+
+%% create graph
+graph = NonlinearFactorGraph;
+
+%% create the noise factors
+poseNoiseSigmas = [0.001 0.001 0.001 0.1 0.1 0.1]';
+posePriorNoise  = noiseModel.Diagonal.Sigmas(poseNoiseSigmas);
+measurementNoiseSigma = 1.0;
+measurementNoise = noiseModel.Isotropic.Sigma(2, measurementNoiseSigma);
+
+cameraPosesNum = length(cameraPoses);
+
+%% add measurements and initial camera & points values
+pointsNum = 0;
+cylinderNum = length(cylinders);
+points3d = cell(0);
+for i = 1:cylinderNum
+    cylinderPointsNum = length(cylinders{i}.Points);
+    pointsNum = pointsNum + cylinderPointsNum; 
+    for j = 1:cylinderPointsNum
+        points3d{end+1}.data = cylinders{i}.Points{j};
+        points3d{end}.Z = cell(0);
+        points3d{end}.camConstraintIdx = cell(0);
+        points3d{end}.added = cell(0);
+        points3d{end}.visiblity = false;
+        points3d{end}.cov = cell(cameraPosesNum);
+    end
+end
+
+graph.add(PriorFactorPose3(symbol('x', 1), cameraPoses{1}, posePriorNoise));
+
+%% initialize graph and values
+initialEstimate = Values;
+for i = 1:pointsNum
+    point_j = points3d{i}.data.retract(0.1*randn(3,1));
+    initialEstimate.insert(symbol('p', i), point_j); 
+end
+
+pts3d = cell(cameraPosesNum, 1);
+cameraPosesCov = cell(cameraPosesNum, 1);
+marginals = Values;
+for i = 1:cameraPosesNum     
+    cameraPose = cameraPoses{i};    
+    pts3d{i} = cylinderSampleProjection(K, cameraPose, imageSize, cylinders);
+    
+    measurementNum = length(pts3d{i}.Z);
+    for j = 1:measurementNum
+        index = pts3d{i}.overallIdx{j};
+        points3d{index}.Z{end+1} = pts3d{i}.Z{j};
+        points3d{index}.camConstraintIdx{end+1} = i;
+        points3d{index}.added{end+1} = false;
+        
+        if length(points3d{index}.Z) < 2
+            continue;
+        else
+            for k = 1:length(points3d{index}.Z)
+                if ~points3d{index}.added{k}                
+                    graph.add(GenericProjectionFactorCal3_S2(points3d{index}.Z{k}, ...
+                        measurementNoise, symbol('x', points3d{index}.camConstraintIdx{k}), ...
+                        symbol('p', index), K) );
+                    points3d{index}.added{k} = true;
+                end
+            end
+        end 
+        
+        points3d{index}.visiblity = true;    
+    end
+
+    pose_i = cameraPoses{i}.retract(0.1*randn(6,1));
+    initialEstimate.insert(symbol('x', i), pose_i);    
+
+    marginals = Marginals(graph, initialEstimate);
+
+    for j = 1:pointsNum
+        if points3d{j}.visiblity
+            points3d{j}.cov{i} = marginals.marginalCovariance(symbol('p',j));
+        end
+    end
+
+    cameraPosesCov{i} = marginals.marginalCovariance(symbol('x',i));
+
+end
+  
+%% Print the graph
+graph.print(sprintf('\nFactor graph:\n'));
+
+%% Plot the result
+plotFlyingResults(points3d, cameraPoses, cameraPosesCov, cylinders, options);
+
+%% get all the points track information
+for i = 1:pointsNum
+    if ~points3d{i}.visiblity
+        continue;
+    end
+    
+    pts2dTracksMono.pt3d{end+1} = points3d{i}.data;
+    pts2dTracksMono.Z{end+1} = points3d{i}.Z;
+
+    if length(points3d{i}.Z) == 1
+        %pts2dTracksMono.cov{i} singular matrix 
+    else 
+        pts2dTracksMono.cov{end+1} = marginals.marginalCovariance(symbol('p', i));    
+    end
+end
+
+end

matlab/+gtsam/points2DTrackStereo.m

@@ -0,0 +1,101 @@
+function [pts2dTracksStereo, initialEstimate] = points2DTrackStereo(K, cameraPoses, options, cylinders)
+% Assess how accurately we can reconstruct points from a particular monocular camera setup. 
+% After creation of the factor graph for each track, linearize it around ground truth. 
+% There is no optimization
+%
+% @author: Zhaoyang Lv
+
+import gtsam.*
+
+%% create graph
+graph = NonlinearFactorGraph;
+
+%% create the noise factors
+poseNoiseSigmas = [0.001 0.001 0.001 0.1 0.1 0.1]';
+posePriorNoise  = noiseModel.Diagonal.Sigmas(poseNoiseSigmas);
+stereoNoise = noiseModel.Isotropic.Sigma(3, 0.05);
+
+cameraPosesNum = length(cameraPoses);
+
+%% add measurements and initial camera & points values
+pointsNum = 0;
+cylinderNum = length(cylinders);
+points3d = cell(0);
+for i = 1:cylinderNum
+    cylinderPointsNum = length(cylinders{i}.Points);
+    pointsNum = pointsNum + length(cylinders{i}.Points);
+    for j = 1:cylinderPointsNum
+        points3d{end+1}.data = cylinders{i}.Points{j};
+        points3d{end}.Z = cell(0);
+        points3d{end}.cameraConstraint = cell(0);
+        points3d{end}.visiblity = false;
+        points3d{end}.cov = cell(cameraPosesNum);
+    end
+end
+
+graph.add(PriorFactorPose3(symbol('x', 1), cameraPoses{1}, posePriorNoise));
+
+%% initialize graph and values
+initialEstimate = Values;
+for i = 1:pointsNum
+    point_j = points3d{i}.data.retract(0.05*randn(3,1));
+    initialEstimate.insert(symbol('p', i), point_j);    
+end
+
+pts3d = cell(cameraPosesNum, 1);
+cameraPosesCov = cell(cameraPosesNum, 1);
+for i = 1:cameraPosesNum 
+    pts3d{i} = cylinderSampleProjectionStereo(K, cameraPoses{i}, options.camera.resolution, cylinders);
+    
+    if isempty(pts3d{i}.Z)
+        continue;
+    end
+    
+    measurementNum = length(pts3d{i}.Z);
+    for j = 1:measurementNum
+        index = pts3d{i}.overallIdx{j};
+        points3d{index}.Z{end+1} = pts3d{i}.Z{j};
+        points3d{index}.cameraConstraint{end+1} = i;
+        points3d{index}.visiblity = true;
+  
+        graph.add(GenericStereoFactor3D(StereoPoint2(pts3d{i}.Z{j}.uL, pts3d{i}.Z{j}.uR, pts3d{i}.Z{j}.v), ...
+            stereoNoise, symbol('x', i), symbol('p', index), K));    
+    end
+
+    pose_i = cameraPoses{i}.retract(poseNoiseSigmas);
+    initialEstimate.insert(symbol('x', i), pose_i);
+
+    %% linearize the graph
+    marginals = Marginals(graph, initialEstimate);
+
+    for j = 1:pointsNum
+        if points3d{j}.visiblity
+            points3d{j}.cov{i} = marginals.marginalCovariance(symbol('p', j));
+        end       
+    end
+    
+    cameraPosesCov{i} = marginals.marginalCovariance(symbol('x', i));    
+end
+
+%% Plot the result
+plotFlyingResults(points3d, cameraPoses, cameraPosesCov, cylinders, options);
+
+%% get all the 2d points track information
+pts2dTracksStereo.pt3d = cell(0);
+pts2dTracksStereo.Z = cell(0);
+pts2dTracksStereo.cov = cell(0);
+for i = 1:pointsNum
+    if ~points3d{i}.visiblity
+        continue;
+    end
+    
+    pts2dTracksStereo.pt3d{end+1} = points3d{i}.data;
+    pts2dTracksStereo.Z{end+1} = points3d{i}.Z;
+    pts2dTracksStereo.cov{end+1} = marginals.marginalCovariance(symbol('p', i));       
+end
+
+% 
+% %% plot the result with covariance ellipses
+% plotFlyingResults(pts2dTracksStereo.pt3d, pts2dTracksStereo.cov, cameraPoses, cameraPosesCov, cylinders, options);
+
+end

matlab/gtsam_examples/CameraFlyingExample.m

@@ -0,0 +1,179 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+% Atlanta, Georgia 30332-0415
+% All Rights Reserved
+% Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+% 
+% See LICENSE for the license information
+%
+% @brief A camera flying example through a field of cylinder landmarks
+% @author Zhaoyang Lv
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+
+clear all;
+clc;
+clf;
+
+import gtsam.*
+
+% test or run
+options.enableTests = false;
+
+%% cylinder options
+% the number of cylinders in the field
+options.cylinder.cylinderNum = 15; % pls be smaller than 20
+% cylinder size
+options.cylinder.radius = 3;  % pls be smaller than 5
+options.cylinder.height = 10;
+% point density on cylinder
+options.cylinder.pointDensity = 0.1;
+
+%% camera options
+%   parameters set according to the stereo camera:
+%   http://www.matrix-vision.com/USB2.0-single-board-camera-mvbluefox-mlc.html
+
+% set up monocular camera or stereo camera
+options.camera.IS_MONO = false;
+% the field of view of camera
+options.camera.fov = 120;
+% fps for image
+options.camera.fps = 25;
+% camera pixel resolution
+options.camera.resolution = Point2(752, 480);
+% camera horizon
+options.camera.horizon = 60;
+% camera baseline
+options.camera.baseline = 0.05;
+% camera focal length
+options.camera.f = round(options.camera.resolution.x * options.camera.horizon / ...
+    options.camera.fov);
+% camera focal baseline
+options.camera.fB = options.camera.f * options.camera.baseline;
+% camera disparity
+options.camera.disparity = options.camera.fB / options.camera.horizon;
+% Monocular Camera Calibration
+options.camera.monoK = Cal3_S2(options.camera.f, options.camera.f, 0, ...
+    options.camera.resolution.x/2, options.camera.resolution.y/2);
+% Stereo Camera Calibration
+options.camera.stereoK = Cal3_S2Stereo(options.camera.f, options.camera.f, 0, ...
+    options.camera.resolution.x/2, options.camera.resolution.y/2, options.camera.disparity);
+
+% write video output
+options.writeVideo = true;
+% the testing field size (unit: meter)
+options.fieldSize = Point2([100, 100]');
+% camera flying speed (unit: meter / second)
+options.speed = 20;
+% camera flying height
+options.height = 30;
+
+%% ploting options
+% display covariance scaling factor, default to be 1.
+% The covariance visualization default models 99% of all probablity 
+options.plot.covarianceScale = 1;
+% plot the trajectory covariance
+options.plot.DISP_TRAJ_COV = true;
+% plot points covariance
+options.plot.POINTS_COV = true;
+
+%% This is for tests
+if options.enableTests
+    % test1: visibility test in monocular camera 
+    cylinders{1}.centroid = Point3(30, 50, 5);
+    cylinders{2}.centroid = Point3(50, 50, 5);
+    cylinders{3}.centroid = Point3(70, 50, 5);
+
+    for i = 1:3
+        cylinders{i}.radius = 5;
+        cylinders{i}.height = 10;
+
+        cylinders{i}.Points{1} = cylinders{i}.centroid.compose(Point3(-cylinders{i}.radius, 0, 0));
+        cylinders{i}.Points{2} = cylinders{i}.centroid.compose(Point3(cylinders{i}.radius, 0, 0));
+    end
+
+    camera = SimpleCamera.Lookat(Point3(10, 50, 10), ... 
+        Point3(options.fieldSize.x/2, options.fieldSize.y/2, 0), ...
+        Point3([0,0,1]'), options.monoK); 
+
+    pose = camera.pose;
+    prjMonoResult = cylinderSampleProjection(options.camera.monoK, pose, ...
+        options.camera.resolution, cylinders);
+
+    % test2: visibility test in stereo camera  
+    prjStereoResult = cylinderSampleProjectionStereo(options.camera.stereoK, ...
+        pose, options.camera.resolution, cylinders);
+end
+
+%% generate a set of cylinders and point samples on cylinders
+cylinderNum = options.cylinder.cylinderNum;
+cylinders = cell(cylinderNum, 1);
+baseCentroid = cell(cylinderNum, 1);
+theta = 0;
+i = 1;
+while i <= cylinderNum
+    theta = theta + 2*pi/10;
+    x = 40 * rand * cos(theta) + options.fieldSize.x/2;
+    y = 20 * rand * sin(theta) + options.fieldSize.y/2;
+    baseCentroid{i} = Point2([x, y]');
+    
+    % prevent two cylinders interact with each other
+    regenerate = false;
+    for j = 1:i-1
+        if i > 1 && baseCentroid{i}.dist(baseCentroid{j}) < options.cylinder.radius * 2
+            regenerate = true;
+            break;
+        end
+    end
+    if regenerate 
+       continue;
+    end
+    
+    cylinders{i,1} = cylinderSampling(baseCentroid{i}, options.cylinder.radius, ...
+         options.cylinder.height, options.cylinder.pointDensity);
+    i = i+1;
+end
+
+%% generate ground truth camera trajectories: a line
+KMono = Cal3_S2(525,525,0,320,240);
+cameraPoses = cell(0);
+theta = 0;
+t = Point3(5, 5, options.height);
+i = 0;
+while 1
+    i = i+1;
+    distance = options.speed / options.camera.fps;
+    angle = 0.1*pi*(rand-0.5);
+    inc_t = Point3(distance * cos(angle), ...
+        distance * sin(angle), 0);
+    t = t.compose(inc_t);
+    
+    if t.x > options.fieldSize.x - 10 || t.y > options.fieldSize.y - 10;
+        break;
+    end
+    
+    %t = Point3([(i-1)*(options.fieldSize.x - 10)/options.poseNum + 10, ...
+    %    15, 10]');
+    camera = SimpleCamera.Lookat(t, ... 
+        Point3(options.fieldSize.x/2, options.fieldSize.y/2, 0), ...
+        Point3([0,0,1]'), options.camera.monoK);    
+    cameraPoses{end+1} = camera.pose;
+end
+
+%% set up camera and get measurements
+if options.camera.IS_MONO 
+    % use Monocular Camera
+    pts2dTracksMono = points2DTrackMonocular(options.camera.monoK, cameraPoses, ...
+        options.camera.resolution, cylinders);
+else 
+    % use Stereo Camera
+    pts2dTracksStereo = points2DTrackStereo(options.camera.stereoK, ...
+        cameraPoses, options, cylinders);
+end
+
+
+
+
+ 
+
+

matlab/gtsam_tests/testCal3Unified.m

@@ -0,0 +1,7 @@
+% test Cal3Unified
+import gtsam.*;
+
+K = Cal3Unified;
+EXPECT('fx',K.fx()==1);
+EXPECT('fy',K.fy()==1);
+

matlab/gtsam_tests/test_gtsam.m

@@ -1,17 +1,25 @@
 % Test runner script - runs each test 
 
-% display 'Starting: testPriorFactor'
+%% geometry
-% testPriorFactor
+display 'Starting: testCal3Unified'
+testCal3Unified
 
-display 'Starting: testValues'
+%% linear
-testValues
+display 'Starting: testKalmanFilter'
+testKalmanFilter
 
 display 'Starting: testJacobianFactor'
 testJacobianFactor
 
-display 'Starting: testKalmanFilter'
+%% nonlinear
-testKalmanFilter
+display 'Starting: testValues'
+testValues
 
+%% SLAM
+display 'Starting: testPriorFactor'
+testPriorFactor
+
+%% examples
 display 'Starting: testLocalizationExample'
 testLocalizationExample
 
@@ -36,6 +44,7 @@ testStereoVOExample
 display 'Starting: testVisualISAMExample'
 testVisualISAMExample
 
+%% MATLAB specific
 display 'Starting: testUtilities'
 testUtilities
 

matlab/unstable_examples/.gitignore

@@ -1 +1,2 @@
 *.m~
+*.avi