diff --git a/.gitignore b/.gitignore
index 11e612e..d6dfd57 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,19 +1,4 @@
-# Build and Release Folders
+# Data Folders
 bin/
-bin-debug/
-bin-release/
-[Oo]bj/ # FlashDevelop obj
-[Bb]in/ # FlashDevelop bin
-
-# Other files and folders
-.settings/
-
-# Executables
-*.swf
-*.air
-*.ipa
-*.apk
-
-# Project files, i.e. `.project`, `.actionScriptProperties` and `.flexProperties`
-# should NOT be excluded as they contain compiler settings and other important
-# information for Eclipse / Flash Builder.
+CarTracking/input/
+VisualOdometry/input/
diff --git a/CarTracking/code/main.asv b/CarTracking/code/main.asv
new file mode 100644
index 0000000..4fdef0d
--- /dev/null
+++ b/CarTracking/code/main.asv
@@ -0,0 +1,89 @@
+clear all; close all; clc;
+
+%%
+cd ../input;
+videoFReader = vision.VideoFileReader('challenge.avi');
+carDetector=vision.CascadeObjectDetector('cars.xml');
+cd ../code;
+
+%%
+carDetector.MergeThreshold=1;
+% carDetector.MinSize=[50 50];
+carDetector.MinSize=[100 100];
+videoFrame=videoFReader();
+videoFrame(1:240,:,:)=0;
+boundingBox=step(carDetector, videoFrame);
+carAnotate=insertObjectAnnotation(videoFrame, 'rectangle', boundingBox, 'Car');
+imshow(carAnotate);
+
+%%
+videoPlayer  = vision.VideoPlayer('Position',...
+    [100 100 [size(videoFrame, 2), size(videoFrame, 1)]+30]);
+
+%%
+for i=1:size(boundingBox,1)
+    
+    pointTracker{i} = vision.PointTracker('MaxBidirectionalError',2);
+    points = detectSURFFeatures(rgb2gray(videoFrame),'ROI', boundingBox(i,:), 'MetricThreshold', 300, 'NumOctaves', 20);
+    
+    %         points = detectHarrisFeatures(rgb2gray(videoFrame),'ROI', boundingBox(i,:), 'MinQuality', 0.00005);
+    %         points = detectMSERFeatures(rgb2gray(videoFrame),'ROI', boundingBox(i,:),'MaxAreaVariation',0.8);
+    %     points = detectBRISKFeatures(rgb2gray(videoFrame),'ROI', boundingBox(i,:));
+    
+    initialize(pointTracker{i},points.Location,videoFrame);
+    bboxPoints{i} = bbox2points(boundingBox(i,:));
+    
+    pointsStruct{i} = points.Location;
+    oldPoints{i} = pointsStruct{i};
+end
+
+%%
+while ~isDone(videoFReader)
+    % get the next frame
+    videoFrame = step(videoFReader);
+    visiblePointsList=[];
+    oldInliers=[];
+    % Track the points. Note that some points may be lost.
+    for j=1:length(pointTracker)
+        
+        [points, isFound] = step(pointTracker{j}, videoFrame);
+        visiblePoints{j} = points(isFound, :);
+        visiblePointsList = [visiblePointsList; visiblePoints{j}];
+        oldInliers = [oldInliers; oldPoints{j}(isFound, :)];
+        
+    end
+    %         if size(visiblePoints{j}, 1) >= 2 % need at least 2 points
+    
+    % Estimate the geometric transformation between the old points
+    % and the new points and eliminate outliers
+    [xform, oldInliers, visiblePointsList] = estimateGeometricTransform(...
+        oldInliers, visiblePointsList, 'similarity', 'MaxDistance', 1);
+    for j=1:length(pointTracker)
+        % Apply the transformation to the bounding box points
+        bboxPoints{j} = transformPointsForward(xform, bboxPoints{j});
+        
+        % Insert a bounding box around the object being tracked
+        bboxPolygon = reshape(bboxPoints{j}', 1, []);
+        videoFrame = insertShape(videoFrame, 'Polygon', bboxPolygon, ...
+            'LineWidth', 2, 'Color', [255 255*(j-1) 255*(j-2)]);
+        
+        % Display tracked points
+        videoFrame = insertMarker(videoFrame, visiblePoints{j}, '+', ...
+            'Color', 'white');
+        % Reset the points
+        oldPoints{j} = visiblePoints{j};
+        setPoints(pointTracker{j}, oldPoints{j});
+    end
+    
+    % Display the annotated video frame using the video player object
+    step(videoPlayer, videoFrame);
+end
+
+
+
+% Clean up
+release(videoFReader);
+release(videoPlayer);
+for i=1:length(pointTracker)
+    release(pointTracker{i});
+end
\ No newline at end of file
diff --git a/CarTracking/code/main.m b/CarTracking/code/main.m
new file mode 100644
index 0000000..7521b09
--- /dev/null
+++ b/CarTracking/code/main.m
@@ -0,0 +1,88 @@
+clear all; close all; clc;
+
+%%
+cd ../input;
+videoFReader = vision.VideoFileReader('simple.avi');
+carDetector=vision.CascadeObjectDetector('cars.xml');
+cd ../code;
+
+%%
+carDetector.MergeThreshold=1;
+carDetector.MinSize=[50 50];
+videoFrame=videoFReader();
+videoFrame(1:240,:,:)=0;
+boundingBox=step(carDetector, videoFrame);
+carAnotate=insertObjectAnnotation(videoFrame, 'rectangle', boundingBox, 'Car');
+imshow(carAnotate);
+
+%%
+videoPlayer  = vision.VideoPlayer('Position',...
+    [100 100 [size(videoFrame, 2), size(videoFrame, 1)]+30]);
+
+%%
+for i=1:size(boundingBox,1)
+    
+    pointTracker{i} = vision.PointTracker('MaxBidirectionalError',2);
+    points = detectSURFFeatures(rgb2gray(videoFrame),'ROI', boundingBox(i,:), 'MetricThreshold', 300, 'NumOctaves', 20);
+    
+    %         points = detectHarrisFeatures(rgb2gray(videoFrame),'ROI', boundingBox(i,:), 'MinQuality', 0.00005);
+    %         points = detectMSERFeatures(rgb2gray(videoFrame),'ROI', boundingBox(i,:),'MaxAreaVariation',0.8);
+    %     points = detectBRISKFeatures(rgb2gray(videoFrame),'ROI', boundingBox(i,:));
+    
+    initialize(pointTracker{i},points.Location,videoFrame);
+    bboxPoints{i} = bbox2points(boundingBox(i,:));
+    
+    pointsStruct{i} = points.Location;
+    oldPoints{i} = pointsStruct{i};
+end
+
+%%
+while ~isDone(videoFReader)
+    % get the next frame
+    videoFrame = step(videoFReader);
+    visiblePointsList=[];
+    oldInliers=[];
+    % Track the points. Note that some points may be lost.
+    for j=1:length(pointTracker)
+        
+        [points, isFound] = step(pointTracker{j}, videoFrame);
+        visiblePoints{j} = points(isFound, :);
+        visiblePointsList = [visiblePointsList; visiblePoints{j}];
+        oldInliers = [oldInliers; oldPoints{j}(isFound, :)];
+        
+    end
+    %         if size(visiblePoints{j}, 1) >= 2 % need at least 2 points
+    
+    % Estimate the geometric transformation between the old points
+    % and the new points and eliminate outliers
+    [xform, oldInliers, visiblePointsList] = estimateGeometricTransform(...
+        oldInliers, visiblePointsList, 'similarity', 'MaxDistance', 1);
+    for j=1:length(pointTracker)
+        % Apply the transformation to the bounding box points
+        bboxPoints{j} = transformPointsForward(xform, bboxPoints{j});
+        
+        % Insert a bounding box around the object being tracked
+        bboxPolygon = reshape(bboxPoints{j}', 1, []);
+        videoFrame = insertShape(videoFrame, 'Polygon', bboxPolygon, ...
+            'LineWidth', 2, 'Color', [255 255*(j-1) 255*(j-2)]);
+        
+        % Display tracked points
+        videoFrame = insertMarker(videoFrame, visiblePoints{j}, '+', ...
+            'Color', 'white');
+        % Reset the points
+        oldPoints{j} = visiblePoints{j};
+        setPoints(pointTracker{j}, oldPoints{j});
+    end
+    
+    % Display the annotated video frame using the video player object
+    step(videoPlayer, videoFrame);
+end
+
+
+
+% Clean up
+release(videoFReader);
+release(videoPlayer);
+for i=1:length(pointTracker)
+    release(pointTracker{i});
+end
\ No newline at end of file
diff --git a/VisualOdometry/ENPM673 - Project3_Part2.pdf b/VisualOdometry/ENPM673 - Project3_Part2.pdf
new file mode 100644
index 0000000..ab7d45d
Binary files /dev/null and b/VisualOdometry/ENPM673 - Project3_Part2.pdf differ
diff --git a/VisualOdometry/code/EssentialMatrixFromFundamentalMatrix.m b/VisualOdometry/code/EssentialMatrixFromFundamentalMatrix.m
new file mode 100644
index 0000000..e316933
--- /dev/null
+++ b/VisualOdometry/code/EssentialMatrixFromFundamentalMatrix.m
@@ -0,0 +1,16 @@
+function E = EssentialMatrixFromFundamentalMatrix(F,K)
+% EssentialMatrixFromFundamentalMatrix
+% Use the camera calibration matrix to esimate the Essential matrix
+% Inputs:
+%     K - size (3 x 3) camera calibration (intrinsics) matrix for both
+%     cameras
+%     F - size (3 x 3) fundamental matrix from EstimateFundamentalMatrix
+% Outputs:
+%     E - size (3 x 3) Essential matrix with singular values (1,1,0)
+E=K'*F*K;
+[u d v]=svd(E);
+i=eye(3);
+i(3,3)=0;
+E=u*i*v';
+E=E/norm(E);
+end
\ No newline at end of file
diff --git a/VisualOdometry/code/EstimateFundamentalMatrix.m b/VisualOdometry/code/EstimateFundamentalMatrix.m
new file mode 100644
index 0000000..71006b8
--- /dev/null
+++ b/VisualOdometry/code/EstimateFundamentalMatrix.m
@@ -0,0 +1,30 @@
+
+function F = EstimateFundamentalMatrix(x1, x2)
+
+
+% EstimateFundamentalMatrix
+% Estimate the fundamental matrix from two image point correspondences 
+% Inputs:
+%     x1 - size (N x 2) matrix of points in image 1
+%     x2 - size (N x 2) matrix of points in image 2, each row corresponding
+%       to x1
+% Output:
+%    F - size (3 x 3) fundamental matrix with rank 2
+
+F=[];
+A=[];
+for i=1:size(x1,1)
+   a=[x1(i,1)*x2(i,1) x1(i,1)*x2(i,2) x1(i,1) x1(i,2)*x2(i,1) x1(i,2)*x2(i,2) x1(i,2) x2(i,1) x2(i,2) 1];
+   A=[A;a];
+end
+
+[U,S,V] = svd(A);
+F=V(:,end);
+F=F';
+F=[F(1) F(2) F(3);F(4) F(5) F(6);F(7) F(8) F(9)]; %or reshape(F,3,3);
+%rank clean up to 2
+[u d v]=svd(F);
+d(3,3)=0;
+F=u*d*v';
+F=F/norm(F);
+end
diff --git a/VisualOdometry/code/ReadCameraModel.m b/VisualOdometry/code/ReadCameraModel.m
new file mode 100644
index 0000000..2afe98c
--- /dev/null
+++ b/VisualOdometry/code/ReadCameraModel.m
@@ -0,0 +1,99 @@
+function [fx, fy, cx, cy, G_camera_image, LUT] = ReadCameraModel(image_dir, models_dir)
+  
+% ReadCameraModel - load camera intrisics and undistortion LUT from disk
+%
+% [fx, fy, cx, cy, G_camera_image, LUT] = ReadCameraModel(image_dir, models_dir)
+%
+% INPUTS:
+%   image_dir: directory containing images for which camera model is required
+%   models_dir: directory containing camera models
+%
+% OUTPUTS:
+%   fx: horizontal focal length in pixels
+%   fy: vertical focal length in pixels
+%   cx: horizontal principal point in pixels
+%   cy: vertical principal point in pixels
+%   G_camera_image: transform that maps from image coordinates to the base
+%     frame of the camera. For monocular cameras, this is simply a rotation.
+%     For stereo camera, this is a rotation and a translation to the left-most
+%     lense.
+%   LUT: undistortion lookup table. For an image of size w x h, LUT will be an
+%     array of size [w x h, 2], with a (u,v) pair for each pixel. Maps pixels
+%     in the undistorted image to pixels in the distorted image
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%
+% Copyright (c) 2016 University of Oxford
+% Authors: 
+%  Geoff Pascoe (gmp@robots.ox.ac.uk)
+%  Will Maddern (wm@robots.ox.ac.uk)
+%
+% This work is licensed under the Creative Commons 
+% Attribution-NonCommercial-ShareAlike 4.0 International License. 
+% To view a copy of this license, visit 
+% http://creativecommons.org/licenses/by-nc-sa/4.0/ or send a letter to 
+% Creative Commons, PO Box 1866, Mountain View, CA 94042, USA.
+%
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+  
+  if models_dir(end) ~= '/'
+    models_dir = [models_dir '/'];
+  end
+  
+  camera = regexp(image_dir, '(stereo|mono_left|mono_right|mono_rear)', ...
+      'match');
+  camera = camera{end};
+  
+  if strcmp(camera, 'stereo')
+    sensor = regexp(image_dir, '(left|centre|right)', 'match');
+    sensor = sensor{end};
+    
+    if strcmp(sensor, 'left')
+      model = 'wide_left';
+    elseif strcmp(sensor, 'right')
+      model = 'wide_right'; % narrow_right also applies to this sensor
+    elseif strcmp(sensor, 'centre')
+      model = 'narrow_left';
+    else
+      error('Unknown camera model for given directory');
+    end
+    
+    intrinsics_path = [models_dir camera '_' model '.txt'];
+    lut_path = [models_dir camera '_' model '_distortion_lut.bin'];
+    
+  else
+    intrinsics_path = [models_dir camera '.txt'];
+    lut_path = [models_dir camera '_distortion_lut.bin'];
+  end
+  
+  if ~exist(intrinsics_path, 'file')
+    error(['Camera intrinsics not found at ' intrinsics_path]);
+  end
+  intrinsics = dlmread(intrinsics_path);
+  
+  % First line of intrinsics file: fx fy cx xy
+  fx = intrinsics(1, 1);
+  fy = intrinsics(1, 2);
+  cx = intrinsics(1, 3);
+  cy = intrinsics(1, 4);
+  
+  % Lines 2-5 of intrinsics file: 4x4 matrix that transforms between 
+  % x-forward coordinate frame at camera origin, and image frame for
+  % the specific lense
+  G_camera_image = intrinsics(2:5, 1:4);
+  
+  if nargout > 5
+    if ~exist(lut_path, 'file') == -1
+      error(['Distortion LUT not found at ' lut_path]);
+    end
+    lut_file = fopen(lut_path);
+    LUT = fread(lut_file, 'double');
+    fclose(lut_file);
+    
+    % LUT consists of a (u,v) pair for each pixel
+    LUT = reshape(LUT, [numel(LUT)/2, 2]);
+  end
+  
+  disp(intrinsics_path)
+  
+end
diff --git a/VisualOdometry/code/UndistortImage.m b/VisualOdometry/code/UndistortImage.m
new file mode 100644
index 0000000..ad403be
--- /dev/null
+++ b/VisualOdometry/code/UndistortImage.m
@@ -0,0 +1,49 @@
+function [undistorted] = UndistortImage(image, LUT)
+  
+% UndistortImage - undistort an image using a lookup table
+% 
+% [undistorted] = UndistortImage(image, LUT)
+%
+% eg.
+% [ ~, ~, ~, ~, ~, LUT] = ...
+%     ReadCameraModel('<models_dir>/stereo_wide_left_undistortion.bin');
+% image = imread('<image_dir>/<timestamp>.png');
+% undistorted = UndistortImage(image, LUT);
+%
+% INPUTS:
+%   image: distorted image to be rectified
+%   LUT: lookup table mapping pixels in the undistorted image to pixels in the
+%     distorted image, as returned from ReadCameraModel
+%
+% OUTPUTS:
+%   undistorted: image after undistortion
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%
+% Copyright (c) 2016 University of Oxford
+% Authors: 
+%  Geoff Pascoe (gmp@robots.ox.ac.uk)
+%  Will Maddern (wm@robots.ox.ac.uk)
+%
+% This work is licensed under the Creative Commons 
+% Attribution-NonCommercial-ShareAlike 4.0 International License. 
+% To view a copy of this license, visit 
+% http://creativecommons.org/licenses/by-nc-sa/4.0/ or send a letter to 
+% Creative Commons, PO Box 1866, Mountain View, CA 94042, USA.
+%
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+undistorted = zeros(size(image), class(image));
+
+for channel = 1:size(image,3)
+  % Interpolate pixels from distorted image using lookup table
+  channel_data = cast(reshape(interp2(cast(image(:,:,channel), 'single'), ...
+                               LUT(:,1)+1, ...
+                               LUT(:,2)+1, ...
+                               'bilinear'), ...
+                     fliplr(size(image(:,:,channel)))).', class(image));
+  
+  undistorted(:,:,channel) = channel_data;
+end
+
+end
diff --git a/VisualOdometry/code/data.mat b/VisualOdometry/code/data.mat
new file mode 100644
index 0000000..31dfb41
Binary files /dev/null and b/VisualOdometry/code/data.mat differ
diff --git a/VisualOdometry/code/helperEstimateRelativePose.m b/VisualOdometry/code/helperEstimateRelativePose.m
new file mode 100644
index 0000000..e644e40
--- /dev/null
+++ b/VisualOdometry/code/helperEstimateRelativePose.m
@@ -0,0 +1,18 @@
+function [orientation, location] = ...
+    helperEstimateRelativePose(matchedPoints1, matchedPoints2, cameraParams)
+    
+%    [fMatrix, inlierIdx] = estimateFundamentalMatrix(...
+%         matchedPoints1,matchedPoints2,'Method','RANSAC',...
+%         'NumTrials',2000,...
+%         'DistanceThreshold',1e-4);
+%     point1=matchedPoints1.selectStrongest(25);
+%     point2=matchedPoints2.selectStrongest(25);
+    F = EstimateFundamentalMatrix(matchedPoints1.Location,matchedPoints2.Location);
+    
+%     inlierPoints1 = matchedPoints1(inlierIdx, :);
+%     inlierPoints2 = matchedPoints2(inlierIdx, :);    
+    
+    % Compute the camera pose from the fundamental matrix.
+    [orientation, location, validPointFraction] = ...
+        cameraPose(F, cameraParams, matchedPoints1, matchedPoints2);
+end
\ No newline at end of file
diff --git a/VisualOdometry/code/main.m b/VisualOdometry/code/main.m
new file mode 100644
index 0000000..bf3ae3d
--- /dev/null
+++ b/VisualOdometry/code/main.m
@@ -0,0 +1,27 @@
+%%
+% close all;warning off;load('data.mat');
+clc;clear all;close all;warning off;
+cd ..;
+imgFolder = fullfile('input\Oxford_dataset\stereo\centre\');
+allImages = imageDatastore(imgFolder,'IncludeSubfolders',true);
+model=fullfile('..\input\Oxford_dataset\model');
+cd code;
+[fx, fy, cx, cy, G_camera_image, LUT]=ReadCameraModel(imgFolder,model);clc;
+K=[fx 0 cx;0 fy cy;0 0 1];
+cameraParams = cameraParameters('IntrinsicMatrix', K);
+%%
+start=1;
+for i = start:length(allImages.Files)
+    image = demosaic(readimage(allImages,i),'gbrg');
+    undistortedImg = UndistortImage(image,LUT);
+    if (i > start+1)
+        [point1,point2]=matchPoint(rgb2gray(undistortedImg),rgb2gray(pImage));
+        figure(1); showMatchedFeatures(undistortedImg,pImage,point1,point2);
+        [orient, loc] = helperEstimateRelativePose(...
+            point1, point2, cameraParams)
+    end
+%     figure(1);
+%     imshow(undistortedImg);
+    pImage=undistortedImg;
+%     pause(1/10);
+end
\ No newline at end of file
diff --git a/VisualOdometry/code/matchPoint.m b/VisualOdometry/code/matchPoint.m
new file mode 100644
index 0000000..4eff84d
--- /dev/null
+++ b/VisualOdometry/code/matchPoint.m
@@ -0,0 +1,13 @@
+function [matchedPoints1,matchedPoints2] = matchPoint( undistortedImg,pImage)
+       point1=detectFASTFeatures(undistortedImg);
+       [f1,vpts1] = extractFeatures((undistortedImg),point1);
+       
+       point2=detectFASTFeatures(pImage);
+       [f2,vpts2] = extractFeatures(pImage,point2);
+       
+       indexPairs = matchFeatures(f1,f2) ;
+       
+       matchedPoints1 = vpts1(indexPairs(:,1));
+       matchedPoints2 = vpts2(indexPairs(:,2));
+end
+