visual odom complete

VisualOdometry/code/main.m

@@ -51,9 +51,8 @@
 pFeature = feature;
 pvPoints = vPoints;
 %%
-% axis([-20, 1000, -140, 30, 0, 2000]);
 figure
-axis([-1000, 20, -30, 140, -2000, 0]);
+axis([-1000, 20, -30, 500, -2000, 0]);
 % Set Y-axis to be vertical pointing down.
 view(gca, 3);
 set(gca, 'CameraUpVector', [0, -1, 0]);
@@ -64,10 +63,11 @@
 ylabel('Y (cm)');
 zlabel('Z (cm)');
 hold on;
-% Plot estimated camera pose.
+
 camEstimated = plotCamera('Size', 7, 'Location',...
     estimatedView.Views.Location{1}, 'Orientation', estimatedView.Views.Orientation{1},...
     'Color', 'g', 'Opacity', 0);
+
 trajectoryEstimated = plot3(0, 0, 0, 'g-');
 title('Camera Trajectory');
 updateCamera(2,trajectoryEstimated,camEstimated,poses(estimatedView));
@@ -76,84 +76,50 @@
 for i = start:length(allImages.Files)
     image = demosaic(readimage(allImages,i),'gbrg');
     undistortedImg = UndistortImage(image,LUT);
+
     step(player, undistortedImg);
     [points,feature,indexPairs,vPoints] = customMatchFeature(undistortedImg,pFeature,pImage, pvPoints);
-    % Eliminate outliers from feature matches.
-%         inlierIdx = findEpipolarInliers(pPoints(indexPairs(:,1)),...
-%             points(indexPairs(:, 2)), cameraParams);
-    %
-    % %     [~,inlierIdx] = estimateFundamentalMatrix(pPoints(indexPairs(:,1)),points(indexPairs(:, 2)),...
-    % %         'Method','RANSAC','NumTrials',2000);
-    %
-    %     indexPairs = indexPairs(inlierIdx, :);
-    %
-    %     [worldPoints, imagePoints] = helperFind3Dto2DCorrespondences(estimatedView,...
-    %         cameraParams, indexPairs, points);
-    %
-    %     warningstate = warning('off','vision:ransac:maxTrialsReached');
-    %     % Estimate the world camera pose for the current view.
-    %     [orient, loc] = estimateWorldCameraPose(imagePoints, worldPoints, ...
-    %         cameraParams, 'MaxNumTrials', 5000, 'Confidence', 99.99, ...
-    %         'MaxReprojectionError', 0.8);
-    %
     matchedPoints1 = pPoints(indexPairs(:,1));
     matchedPoints2 = points(indexPairs(:,2));
+
     [F,inlierIdx] = estimateFundamentalMatrix(matchedPoints1,matchedPoints2,...
         'Method','RANSAC','NumTrials',2000,'DistanceThreshold',1e-4);
     indexPairs = indexPairs(inlierIdx,:);
 
     E = FtoEmatrix(F,K);
     [Cset,Rset] = ExtractCameraPose(E);
-%     for index=1:4
-%         Xset{index}=LinearTriangulation(K, estimatedView.Views.Location{i-1}',estimatedView.Views.Orientation{i-1},...
-%             Cset{index}, Rset{index},...
-%             pPoints(indexPairs(:,1)).Location, points(indexPairs(:,2)).Location);
-%     end
-%     [C,R,~] = DisambiguateCameraPose(Cset, Rset, Xset);
+
     [cRot,cT,~] = SelectCorrectEssentialCameraMatrix(Rset,Cset,...
     [matchedPoints1.Location ones(matchedPoints1.Count,1)]',...
     [matchedPoints2.Location ones(matchedPoints2.Count,1)]',K);
 
-
-%     C = C + estimatedView.Views.Location{i-1}';
-%     R = R * estimatedView.Views.Orientation{i-1};
     cT = cT'*estimatedView.Views.Orientation{i-1} + estimatedView.Views.Location{i-1};
     cRot = cRot * estimatedView.Views.Orientation{i-1};
-%     
-%     estimatedView = addView(estimatedView, i, 'Points', points,...
-%     'Orientation', R,'Location', C);
+
     estimatedView = addView(estimatedView, i, 'Points', points,...
     'Orientation', cRot,'Location', cT);
-    estimatedView = addConnection(estimatedView, i-1, i, 'Matches', indexPairs);
-%     
-%     % Restore the original warning state
-%     warning(warningstate)
-%     % Add the current view to the view set.
-%     estimatedView = addView(estimatedView, i, 'Points', points, 'Orientation', orient, ...
-%         'Location', loc);
-%     % Store the point matches between the previous and the current views.
-%     estimatedView = addConnection(estimatedView, i-1, i, 'Matches', indexPairs);
-%     if (mod(i,1) == 0)
-%         windowSize = 2;
-%         startFrame = max(1, i - windowSize);
-%         tracks = findTracks(estimatedView, startFrame:i);
-%         camPoses = poses(estimatedView, startFrame:i);
-%         [xyzPoints, reprojErrors] = triangulateMultiview(tracks, camPoses, ...
-%             cameraParams);
-%         
-%         % Hold the first two poses fixed, to keep the same scale.
-%         fixedIds = [startFrame, startFrame+1];
-%         
-%         % Exclude points and tracks with high reprojection errors.
-%         idx = reprojErrors < 10;
-%         
-%         [~, camPoses] = bundleAdjustment(xyzPoints(idx, :), tracks(idx), ...
-%             camPoses, cameraParams, 'FixedViewIDs', fixedIds, ...
-%             'PointsUndistorted', true, 'AbsoluteTolerance', 1e-9,...
-%             'RelativeTolerance', 1e-9, 'MaxIterations', 300);
-%         
-%         estimatedView = updateView(estimatedView, camPoses);
-%     end
+
+    if (mod(i,3) == 0)
+        windowSize = 1;
+        startFrame = max(1, i - windowSize);
+        tracks = findTracks(estimatedView, startFrame:i);
+        camPoses = poses(estimatedView, startFrame:i);
+        [xyzPoints, reprojErrors] = triangulateMultiview(tracks, camPoses, ...
+            cameraParams);
+
+        % Hold the first two poses fixed, to keep the same scale.
+        fixedIds = [startFrame, startFrame+1];
+
+        % Exclude points and tracks with high reprojection errors.
+        idx = reprojErrors < 8;
+
+        [~, camPoses] = bundleAdjustment(xyzPoints(idx, :), tracks(idx), ...
+            camPoses, cameraParams, 'FixedViewIDs', fixedIds, ...
+            'PointsUndistorted', true, 'AbsoluteTolerance', 1e-9,...
+            'RelativeTolerance', 1e-9, 'MaxIterations', 300);
+
+        estimatedView = updateView(estimatedView, camPoses);
+    end
 
     updateCamera(i,trajectoryEstimated,camEstimated,poses(estimatedView));
     pImage = undistortedImg;
@@ -168,18 +134,18 @@
 sound(y);
 
 figure
-axis([-1000, 20, -30, 400, -2000, 0]);
-% Set Y-axis to be vertical pointing down.
-view(gca, 3);
-set(gca, 'CameraUpVector', [0, -1, 0]);
-camorbit(gca, -120, 0, 'data', [0, 1, 0]);
+% view(gca, 3);
+view([0 0]);
+% set(gca, 'CameraUpVector', [0, -1, 0]);
+set(gca,'Xdir','reverse');
+set(gca,'Zdir','reverse');
 locations = cat(1, estimatedView.Views.Location{:});
 grid on;
 xlabel('X (cm)');
 ylabel('Y (cm)');
 zlabel('Z (cm)');
 hold on;
 % Plot estimated camera pose.
-trajectoryEstimated = plot3(0, 0, 0, 'g-');
+trajectoryEstimated = plot3(0,0,0, 'g-');
 set(trajectoryEstimated, 'XData', locations(:,1), 'YData', ...
-    locations(:,2), 'ZData', locations(:,3));
+    zeros(size(locations,1),1), 'ZData', locations(:,3));

VisualOdometry/code/temp.m

@@ -0,0 +1,17 @@
+load memory.mat
+figure
+% view(gca, 3);
+view([0 0]);
+% set(gca, 'CameraUpVector', [0, -1, 0]);
+set(gca,'Xdir','reverse');
+set(gca,'Zdir','reverse');
+locations = cat(1, estimatedView.Views.Location{:});
+grid on;
+xlabel('X (cm)');
+ylabel('Y (cm)');
+zlabel('Z (cm)');
+hold on;
+% Plot estimated camera pose.
+trajectoryEstimated = plot3(0,0,0, 'g-');
+set(trajectoryEstimated, 'XData', locations(:,1), 'YData', ...
+    zeros(size(locations,1),1), 'ZData', locations(:,3));