diff --git a/CarTracking/Report.pdf b/CarTracking/Report.pdf new file mode 100644 index 0000000..8b0ad5e Binary files /dev/null and b/CarTracking/Report.pdf differ diff --git a/CarTracking/code/main.m b/CarTracking/code/main.m index 7521b09..a880def 100644 --- a/CarTracking/code/main.m +++ b/CarTracking/code/main.m @@ -1,11 +1,10 @@ clear all; close all; clc; - %% cd ../input; videoFReader = vision.VideoFileReader('simple.avi'); -carDetector=vision.CascadeObjectDetector('cars.xml'); +carDetector=vision.CascadeObjectDetector('cascadeClassifier.xml'); cd ../code; - +videoFWriter = vision.VideoFileWriter('../Output/output.mp4','FileFormat','MPEG4'); %% carDetector.MergeThreshold=1; carDetector.MinSize=[50 50]; @@ -14,21 +13,14 @@ 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,:)); @@ -51,12 +43,10 @@ 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}); @@ -64,7 +54,7 @@ % 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)]); + 'LineWidth', 2, 'Color', [255*(j-1) 255*(j-2) 255*(mod(j,2))]); % Display tracked points videoFrame = insertMarker(videoFrame, visiblePoints{j}, '+', ... @@ -73,16 +63,12 @@ oldPoints{j} = visiblePoints{j}; setPoints(pointTracker{j}, oldPoints{j}); end - - % Display the annotated video frame using the video player object step(videoPlayer, videoFrame); + step(videoFWriter,videoFrame); end - - - -% Clean up release(videoFReader); release(videoPlayer); +release(videoFWriter); for i=1:length(pointTracker) release(pointTracker{i}); end \ No newline at end of file diff --git a/CarTracking/code/modifyDataset.m b/CarTracking/code/modifyDataset.m new file mode 100644 index 0000000..8aacded --- /dev/null +++ b/CarTracking/code/modifyDataset.m @@ -0,0 +1,15 @@ +clc; clear all; close all;warning('off'); + +index = 0; +trainFolder = '..\Input\vehicles\myposFIles\'; +negativeFolder = fullfile('..\Input\vehicles'); + +negativeImages = imageDatastore(negativeFolder,'IncludeSubfolders',true); + +negativeStruct = struct(negativeImages); + +for i =1:negativeStruct.NumFiles + img = imread(char(negativeStruct.Files(i))); + rimg=flip(img,2); + imwrite(rimg,fullfile(trainFolder,sprintf('file_%d.png',i)),'png'); +end \ No newline at end of file diff --git a/CarTracking/code/trainingClassifier.m b/CarTracking/code/trainingClassifier.m new file mode 100644 index 0000000..be356f0 --- /dev/null +++ b/CarTracking/code/trainingClassifier.m @@ -0,0 +1,19 @@ +clc;clear all; close all; warning('off'); +%% +negativeFolder = fullfile('..\Input\non-vehicles\Extras\'); + +positiveFolder = fullfile('..\Input\vehicles\'); + +negativeImages = imageDatastore(negativeFolder,'IncludeSubfolders',true); +positiveImages = imageDatastore(positiveFolder,'IncludeSubfolders',true); + +value = [1 1 64 64]; + +positiveStruct = struct(positiveImages); +positiveImg(:,1) = cell2table(positiveStruct.Files); +for i =1:positiveStruct.NumFiles + positiveImg(i,2) = table(value); +end +%% +trainCascadeObjectDetector('cascadeClassifier.xml',positiveImg, ... + negativeFolder,'FalseAlarmRate',0.05,'NumCascadeStages',10); diff --git a/CarTracking/output/output.mp4 b/CarTracking/output/output.mp4 new file mode 100644 index 0000000..7a7cdb5 Binary files /dev/null and b/CarTracking/output/output.mp4 differ diff --git a/VisualOdometry/Report.pdf b/VisualOdometry/Report.pdf new file mode 100644 index 0000000..bcbd3b6 Binary files /dev/null and b/VisualOdometry/Report.pdf differ diff --git a/VisualOdometry/code/DecomposeCameraMatrix.m b/VisualOdometry/code/DecomposeCameraMatrix.m new file mode 100644 index 0000000..26199b9 --- /dev/null +++ b/VisualOdometry/code/DecomposeCameraMatrix.m @@ -0,0 +1,38 @@ +function [k,r,c] = DecomposeCameraMatrix(p) +% Descompose the camera matrix P into K, R, C according P = K [R |-RC] +% +% Input: +% - p is a 3x4 camera matrix. +% +% Output: +% - k is a 3x3 calibration matrix. +% - r is a 3x3 rotation matrix. +% - c is a 3x1 camera coordinates. +% +%---------------------------------------------------------- +% +% From +% Book: "Multiple View Geometry in Computer Vision", +% Authors: Hartley and Zisserman, 2006, [HaZ2006] +% Section: "Decomposition of the camera matrix", +% Chapter: 6 +% Page: 163 +% +%---------------------------------------------------------- +% Author: Diego Cheda +% Affiliation: CVC - UAB +% Date: 03/06/2008 +%---------------------------------------------------------- + +% Finding camera centre +% PC = 0, right null vector of P. +x = det([ p(:,2), p(:,3), p(:,4) ]); +y = -det([ p(:,1), p(:,3), p(:,4) ]); +z = det([ p(:,1), p(:,2), p(:,4) ]); +t = -det([ p(:,1), p(:,2), p(:,3) ]); + +c = [ x/t; y/t; z/t ]; + +% Finding camera orientation and internal parameters +% P = [M | -MC] = K[R | -RC] +[k,r] = RQDecomposition(p(:,1:3)); diff --git a/VisualOdometry/code/DepthOfPoints.m b/VisualOdometry/code/DepthOfPoints.m new file mode 100644 index 0000000..54f7df1 --- /dev/null +++ b/VisualOdometry/code/DepthOfPoints.m @@ -0,0 +1,39 @@ +function d = DepthOfPoints(x3D,rot,t) +% Computes the depth of points +% +% Input: +% - x3D are the 3D points. +% +% - rot is a 3x3 rotation matrix +% +% - t is a 3x1 traslation matrix. +% +% Output: +% - d is a 1xn vector with the depth of points. +% +%---------------------------------------------------------- +% +% From +% Book: "Multiple View Geometry in Computer Vision", +% Authors: Hartley and Zisserman, 2006, [HaZ2006] +% Section: "Depth of points", +% Chapter: 6 +% Page: 162 +% +%---------------------------------------------------------- +% Author: Diego Cheda +% Affiliation: CVC - UAB +% Date: 11/06/2008 +%---------------------------------------------------------- + +[k,r,c] = DecomposeCameraMatrix([rot t]); + +x3D = HomogeneousCoordinates(x3D,'3D'); + +for i=1:size(x3D,2) + w = rot(3,:) * (x3D(1:3,i) - c(1:3,:)); + + depth = (sign(det(rot)) * w) / x3D(4,i) * norm(rot(3,:)); + + d(i) = depth(1,1); +end diff --git a/VisualOdometry/code/DisambiguateCameraPose.m b/VisualOdometry/code/DisambiguateCameraPose.m new file mode 100644 index 0000000..0a97e7c --- /dev/null +++ b/VisualOdometry/code/DisambiguateCameraPose.m @@ -0,0 +1,12 @@ +function [C, R, X] = DisambiguateCameraPose(Cset, Rset, Xset) + +for i = 1 : length(Cset) + D = Rset{i}*(Xset{i}'-Cset{i}*ones(1,size(Xset{i},1))); + n(i) = length(find(D(3,:)>0 & Xset{i}(:,3)'>0)); +end + +[~, maxi] = min(n); +C = Cset{maxi}; +R = Rset{maxi}'; +X = Xset{maxi}; +end \ No newline at end of file diff --git a/VisualOdometry/code/EssentialMatrixFrom2DPoints.m b/VisualOdometry/code/EssentialMatrixFrom2DPoints.m new file mode 100644 index 0000000..54641db --- /dev/null +++ b/VisualOdometry/code/EssentialMatrixFrom2DPoints.m @@ -0,0 +1,79 @@ +function e = EssentialMatrixFrom2DPoints(p1,p2,k) +% +% Input: +% p1 and p2 are 2xN (or 3xN in homogeneous coordiantes) of +% correspondings corners. +% +% k is the camera matrix. +% +% Output: +% e is the essential matrix. +% +%---------------------------------------------------------- +% +% From +% Book: "Multiple View Geometry in Computer Vision", +% Authors: Hartley and Zisserman, 2006, [HaZ2006] +% Section: "Computation of the Fundamental Matrix F", specifically "The +% normalised 8-point algorithm" +% Chapter: 11 +% Page: 279 +% +%---------------------------------------------------------- +% Author: Diego Cheda +% Affiliation: CVC - UAB +% Date: 03/06/2008 +%---------------------------------------------------------- + +% Points in Normalized Coordinates. p = inv(k) * p +p1 = NormalizedCoordinates(p1,k); +p2 = NormalizedCoordinates(p2,k); + +% Normalization +% Transform the image coordinates according to x^_i = Tx_i and x'^_i = +% T'x'_i where T and T' are normalizing transformation consisting of a +% translation and scaling. +% [p1,t1] = Normalise2DPts(p1); +% [p2,t2] = Normalise2DPts(p2); +[p1,t1] = normalise2dpts(p1); +[p2,t2] = normalise2dpts(p2); + + +% (x,y) +x1 = p1(1,:)'; +x2 = p2(1,:)'; + +y1 = p1(2,:)'; +y2 = p2(2,:)'; + + +% Number of points +numPts = size(p1,2); + +% Af = 0 +% Computes A, the constraint matrix of numpts x 9 +a = [x2.*x1 x2.*y1 x2 y2.*x1 y2.*y1 y2 x1 y1 ones(numPts,1)]; +%a = [x1.*x2 x1.*y2 x1 y1.*x2 y1.*y2 y1 x2 y2 ones(numPts,1)]; + +if (rank(a)<8) + disp('rank defficient'); +end + + +% Singular Value Decomposition of A. +% [U,S,V] = SVD(A) produces a diagonal matrix S, of the same dimension as A +% and with nonnegative diagonal elements in decreasing order, and unitary +% matrices U and V so that A = U*S*V'. +[u, d, v] = svd(a); + +% Linear solution. Obtain F from 9th column of V (the smallest singular +% value of A). +e = reshape(v(:,9),3,3)'; + +% Constraint enforcement. +[u, d, v] = svd(e); +d = diag([1 1 0]); +e = u * d * v'; + +% Desnormalization +e = t2' * e * t1; diff --git a/VisualOdometry/code/EssentialMatrixFromFundamentalMatrix.m b/VisualOdometry/code/EssentialMatrixFromFundamentalMatrix.m deleted file mode 100644 index e316933..0000000 --- a/VisualOdometry/code/EssentialMatrixFromFundamentalMatrix.m +++ /dev/null @@ -1,16 +0,0 @@ -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/EssentialMatrixToCameraMatrix.m b/VisualOdometry/code/EssentialMatrixToCameraMatrix.m new file mode 100644 index 0000000..706874b --- /dev/null +++ b/VisualOdometry/code/EssentialMatrixToCameraMatrix.m @@ -0,0 +1,65 @@ +function [rot,t] = EssentialMatrixToCameraMatrix(e) +% Extracts the cameras from the essential matrix +% +% Input: +% - e is the 3x4 essential matrix. +% +% Output: +% - p1 = [I | 0] is the first canonic camera matrix. +% +% - rot and t are the rotation and translation of the +% second camera matrix from 4 possible solutions. One camera matrix +% must be selected. We test with a single point to determine if it is +% in front of both cameras is sufficient to decide between the four +% different solutions for the camera matrix (pag. 259). +% +%---------------------------------------------------------- +% +% From +% Book: "Multiple View Geometry in Computer Vision", +% Authors: Hartley and Zisserman, 2006, [HaZ2006] +% Section: "Extraction of cameras from the essential matrix", +% Chapter: 9 +% Page: 258 +% +%---------------------------------------------------------- +% Author: Diego Cheda +% Affiliation: CVC - UAB +% Date: 03/06/2008 +%---------------------------------------------------------- + + +% Decompose the matrix E by svd +[u, s, v] = svd(e); + +% +w = [0 -1 0; 1 0 0; 0 0 1]; +z = [0 1 0; -1 0 0; 0 0 1]; + +% +% E = SR where S = [t]_x and R is the rotation matrix. +% E can be factorized as: +%s = u * z * u'; + +% Two possibilities: +rot1 = u * w * v'; +rot2 = u * w' * v'; + +% Two possibilities: +t1 = u(:,3) ./max(abs(u(:,3))); +t2 = -u(:,3) ./max(abs(u(:,3))); + + +% 4 possible choices of the camera matrix P2 based on the 2 possible +% choices of R and 2 possible signs of t. +rot(:,:,1) = rot1; +t(:,:,1) = t1; + +rot(:,:,2) = rot2; +t(:,:,2) = t2; + +rot(:,:,3) = rot1; +t(:,:,3) = t2; + +rot(:,:,4) = rot2; +t(:,:,4) = t1; diff --git a/VisualOdometry/code/EstimateFundamentalMatrix.m b/VisualOdometry/code/EstimateFundamentalMatrix.m deleted file mode 100644 index 71006b8..0000000 --- a/VisualOdometry/code/EstimateFundamentalMatrix.m +++ /dev/null @@ -1,30 +0,0 @@ - -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/ExtractCameraPose.m b/VisualOdometry/code/ExtractCameraPose.m new file mode 100644 index 0000000..d7c9026 --- /dev/null +++ b/VisualOdometry/code/ExtractCameraPose.m @@ -0,0 +1,41 @@ + +function [Cset,Rset] = ExtractCameraPose(E) + +W = [0 -1 0;1 0 0;0 0 1]; +[U,~,V] = svd(E); +R = U*W*V'; +C = U(:,3);%./max(U(:,3)); +if det(R) < 0 + R = -R; + C = -C; +end +Rset(:,:,1) = R; +Cset(:,:,1) = (-R'*C); + +R = U*W*V'; +C = -U(:,3);%./max(U(:,3)); +if det(R) < 0 + R = -R; + C = -C; +end +Rset(:,:,2) = R; +Cset(:,:,2) = (-R'*C); + +R = U*W'*V'; +C = U(:,3);%./max(U(:,3)); +if det(R) < 0 + R = -R; + C = -C; +end +Rset(:,:,3) = R; +Cset(:,:,3) =( -R'*C); + +R = U*W'*V'; +C = -U(:,3);%./max(U(:,3)); +if det(R) < 0 + R = -R; + C = -C; +end +Rset(:,:,4) = R; +Cset(:,:,4) = (-R'*C); +end \ No newline at end of file diff --git a/VisualOdometry/code/FtoEmatrix.m b/VisualOdometry/code/FtoEmatrix.m new file mode 100644 index 0000000..c7c41e9 --- /dev/null +++ b/VisualOdometry/code/FtoEmatrix.m @@ -0,0 +1,8 @@ +function E = FtoEmatrix(F,K) +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/GivensRotationMatrix.m b/VisualOdometry/code/GivensRotationMatrix.m new file mode 100644 index 0000000..bcdc0a6 --- /dev/null +++ b/VisualOdometry/code/GivensRotationMatrix.m @@ -0,0 +1,58 @@ +function q = GivensRotationMatrix(matrixA, axis) +% Computes the Givens rotation matrix. +% +% Input: +% - matrixA is matrixA 3x3 matrix. +% +% - axis is the rotation over one of the 3 coordinate axes (1 for x, 2 +% for y, and 3 for z). +% +% Output: +% - q is the rotation matrix. +% +%---------------------------------------------------------- +% +% From +% Book: "Multiple View Geometry in Computer Vision", +% Authors: Hartley and Zisserman, 2006, [HaZ2006] +% Section: "Givens rotations and RQ decomposition", +% Chapter: appendix 4 +% Page: 579 +% +%---------------------------------------------------------- +% Author: Diego Cheda +% Affiliation: CVC - UAB +% Date: 03/06/2008 +%---------------------------------------------------------- + +if (axis == 1) + % over x + % _ _ + % | 1 | + % | c -s | + % | s c | + % - - + c = -matrixA(3,3)/sqrt(matrixA(3,2)^2 + matrixA(3,3)^2); + s = matrixA(3,2)/sqrt(matrixA(3,2)^2 + matrixA(3,3)^2); + q = [1 0 0; 0 c -s; 0 s c]; +elseif (axis == 2) + % over y + % _ _ + % | c s | + % | 1 | + % | -s c | + % - - + c = matrixA(3,3)/sqrt(matrixA(3,1)^2 + matrixA(3,3)^2); + s = matrixA(3,1)/sqrt(matrixA(3,1)^2 + matrixA(3,3)^2); + q = [c 0 s; 0 1 0; -s 0 c]; +elseif (axis == 3) + % over z + % _ _ + % | c -s | + % | s c | + % | 1 | + % - - + c = -matrixA(2,2)/sqrt(matrixA(2,2)^2 + matrixA(2,1)^2); + s = matrixA(2,1)/sqrt(matrixA(2,2)^2 + matrixA(2,1)^2); + q = [c -s 0; s c 0; 0 0 1]; +end diff --git a/VisualOdometry/code/HomogeneousCoordinates.m b/VisualOdometry/code/HomogeneousCoordinates.m new file mode 100644 index 0000000..72609c6 --- /dev/null +++ b/VisualOdometry/code/HomogeneousCoordinates.m @@ -0,0 +1,46 @@ +function h = HomogeneousCoordinates(p,pts2DOr3D) +% Convert points to homogeneous coordinates +% +% Input: +% p is a matrix of 2D or 3D points. +% +% pts2DOr3D is a flag to indicate if the points are 3D or 2D +% coordinates +% '2D' => 2D points. +% '3D' => 3D points. +% '2Dforce' => 3th coordinate is force to 1 +% +% Output: +% h is a matrix of 2D or 3D points in homogeneous coordinates. +% +%---------------------------------------------------------- +% Author: Diego Cheda +% Affiliation: CVC - UAB +% Date: 10/06/2008 +%---------------------------------------------------------- + + +if (strcmp(pts2DOr3D,'2D')) + if (size(p,1) == 2) + h = [p; ones(size(p,2),1)']; + else if (size(p,1) == 3) + h(1,:) = p(1,:)./p(3,:); + h(2,:) = p(2,:)./p(3,:); + h(3,:) = p(3,:)./p(3,:); + end + end +else if (strcmp(pts2DOr3D,'3D')) + if (size(p,1) == 3) + h = [p; ones(size(p,2),1)']; + else if (size(p,1) == 4) + h(1,:) = p(1,:)./(p(4,:)+eps); + h(2,:) = p(2,:)./(p(4,:)+eps); + h(3,:) = p(3,:)./(p(4,:)+eps); + h(4,:) = p(4,:)./(p(4,:)+eps); + end + end + else if (strcmp(pts2DOr3D,'2Dforce')) + h = [p(1:2,:); ones(size(p,2),1)']; + end + end +end \ No newline at end of file diff --git a/VisualOdometry/code/LinearTriangulation.m b/VisualOdometry/code/LinearTriangulation.m new file mode 100644 index 0000000..47f3ce4 --- /dev/null +++ b/VisualOdometry/code/LinearTriangulation.m @@ -0,0 +1,61 @@ +function x3D = LinearTriangulation(x1,x2,rot,t) +% Computes a reconstruction by linear triangulation by Direct Linear +% Transformation (DTL) algorithm. +% +% Input: +% x1 and x2 are 2xN (or 3xN in homogeneous coordinates) set of +% points. +% +% rot is 3x3 rotation matrix. +% +% t is 3x1 translation matrix. +% +% Output: +% x3D is an estimation of 3D points by projective transformation such +% that x3D * A = 0 +% +%---------------------------------------------------------- +% +% From +% Book: "Multiple View Geometry in Computer Vision", +% Authors: Hartley and Zisserman, 2006, [HaZ2006] Section: "Linear +% triangulation methods", Chapter: 12 +% Page: 312 +% +%---------------------------------------------------------- +% Author: Diego Cheda +% Affiliation: CVC - UAB +% Date: 03/06/2008 +%---------------------------------------------------------- + +% Convert to homogeneous coordinates. +x1 = HomogeneousCoordinates(x1,'2D'); +x2 = HomogeneousCoordinates(x2,'2D'); + +% Normalization +% Transform the image coordinates according to x^_i = Tx_i and x'^_i = +% T'x'_i where T and T' are normalizing transformation conssiting of a +% translation and scaling. +%[x1,t1] = Normalise2DPts(x1); +%[x2,t2] = Normalise2DPts(x2); + +p1 = eye(3,4); +p2 = [rot t]; + +% AX = 0 +for i=1:size(x1,2) + a = [ x1(1,i)*p1(3,:) - p1(1,:); ... + x1(2,i)*p1(3,:) - p1(2,:); ... + x2(1,i)*p2(3,:) - p2(1,:); ... + x2(2,i)*p2(3,:) - p2(2,:) ]; + + % Obtain the SVD of A. The unit singular vector corresponding to the + % smallest singular value is the solution h. A = UDV' with D diagonal + % with positive entries, arranged in descending order down the + % diagonal, then X is the last column of V. + [u,d,v] = svd(a); + x3D(:,i) = v(:,4); +end + +% Desnormalization +%x3D = inv(t2) * x3D * t1; \ No newline at end of file diff --git a/VisualOdometry/code/NormalizedCoordinates.m b/VisualOdometry/code/NormalizedCoordinates.m new file mode 100644 index 0000000..2f1bbd9 --- /dev/null +++ b/VisualOdometry/code/NormalizedCoordinates.m @@ -0,0 +1,27 @@ +function nx = NormalizedCoordinates(x,k); +% Convert a point to normalized coordinates. +% +% Input: +% x is a 3xN points to convert. +% +% k is the camera calibration matrix. +% +% Output: +% nx is a 3xN normalized points. +% +%---------------------------------------------------------- +% +% From +% Book: "Multiple View Geometry in Computer Vision", +% Authors: Hartley and Zisserman, 2006, [HaZ2006] +% Section: "Retrieving the camera matrices", +% Chapter: 9 +% Page: 253 +% +%---------------------------------------------------------- +% Author: Diego Cheda +% Affiliation: CVC - UAB +% Date: 03/06/2008 +%---------------------------------------------------------- + +nx = inv(k) * x; \ No newline at end of file diff --git a/VisualOdometry/code/RQDecomposition.m b/VisualOdometry/code/RQDecomposition.m new file mode 100644 index 0000000..9ed2557 --- /dev/null +++ b/VisualOdometry/code/RQDecomposition.m @@ -0,0 +1,46 @@ +function [k,r] = RQDecomposition(matrixA) +% Computes the RQ decomposition. +% +% Input: +% - a is a 3x3 matrix. +% +% Output: +% - k is a 3x3 upper triangular matrix. +% +% - r is a 3x3 rotation matrix. +% +%---------------------------------------------------------- +% +% From +% Book: "Multiple View Geometry in Computer Vision", +% Authors: Hartley and Zisserman, 2006, [HaZ2006] +% Section: "Givens rotations and RQ decomposition", +% Chapter: appendix 4 +% Page: 579 +% +%---------------------------------------------------------- +% Author: Diego Cheda +% Affiliation: CVC - UAB +% Date: 03/06/2008 +%---------------------------------------------------------- + + qx = GivensRotationMatrix(matrixA,1); + + k = matrixA * qx; + + qy = GivensRotationMatrix(k,2); + + k = k * qy; + + qz = GivensRotationMatrix(k,3); + + k = k * qz; + + r = qz' * qy' * qx'; + + if r(1,1) < 0 + d = diag([-1 -1 ones(1,1)]); + r = r * d; + k = d * k; + end +end \ No newline at end of file diff --git a/VisualOdometry/code/SelectCorrectEssentialCameraMatrix.m b/VisualOdometry/code/SelectCorrectEssentialCameraMatrix.m new file mode 100644 index 0000000..fd1ec11 --- /dev/null +++ b/VisualOdometry/code/SelectCorrectEssentialCameraMatrix.m @@ -0,0 +1,59 @@ +function [cRot,cT,correct] = SelectCorrectEssentialCameraMatrix(rot,t,x1,x2,k) +% Extracts the cameras from the essential matrix +% Input: +% - rot and t are the four rotation and traslation matrice retrieved +% form the essential matrix. +% rot is a 3x3x4 matrix where the last dimension corresponds +% to the different camera matrices and 3x3 are rotation matrices. +% t is a 3x1x4 matrix where the last dimension corresponds +% to the different camera matrices and 3x1 are traslation matrices. +% +% - x3D are the 3D points reconstructed from essential matrix. +% x3D is a 3xNx4 where the last dimesion corresponds +% to the different camera matrices. +% +% +% Output: +% - cRot and cT are the correct rotation and translation of the +% second camera matrix from 4 possible solutions. We test with a +% single point to determine if it is in front of both cameras is +% sufficient to decide between the four different solutions for the +% camera matrix (pag. 259). +% +% - correct is the index of the correct solution. +% +%---------------------------------------------------------- +% Author: Diego Cheda +% Affiliation: CVC - UAB +% Date: 16/06/2008 +%---------------------------------------------------------- + +% Computes 3D points +nx1 = NormalizedCoordinates(x1,k); +nx2 = NormalizedCoordinates(x2,k); +for i=1:4 + x3D(:,:,i) = LinearTriangulation(nx1, nx2, rot(:,:,i), t(:,:,i)); + x3D(:,:,i) = HomogeneousCoordinates(x3D(:,:,i),'3D'); +end + +correct = 0; +for i=1:4 + % compute the depth & sum the sign + depth(i,1) = sum(sign(DepthOfPoints(x3D(:,:,i),eye(3),zeros(3,1)))); %using canonical camera + depth(i,2) = sum(sign(DepthOfPoints(x3D(:,:,i),rot(:,:,i),t(:,:,i)))); % using the recovered camera +end + +if(depth(1,1)<0 && depth(1,2)<0) + correct = 1; +elseif(depth(2,1)<0 && depth(2,2)<0) + correct = 2; +elseif(depth(3,1)<0 && depth(3,2)<0) + correct = 3; +elseif(depth(4,1)<0 && depth(4,2)<0) + correct = 4; +end +%correct + +% return the selected solution +cRot = rot(:,:,correct); +cT = t(:,correct); \ No newline at end of file diff --git a/VisualOdometry/code/Vec2Skew.m b/VisualOdometry/code/Vec2Skew.m new file mode 100644 index 0000000..1a0ece1 --- /dev/null +++ b/VisualOdometry/code/Vec2Skew.m @@ -0,0 +1,5 @@ +function skew = Vec2Skew(v) +skew = [0 -v(3) v(2); + v(3) 0 -v(1); + -v(2) v(1) 0]; + \ No newline at end of file diff --git a/VisualOdometry/code/customFeature.m b/VisualOdometry/code/customFeature.m new file mode 100644 index 0000000..053ea98 --- /dev/null +++ b/VisualOdometry/code/customFeature.m @@ -0,0 +1,10 @@ +function [points , feature, vPoints] = customFeature(image) +if size(image,3) == 3 +image=rgb2gray(image); +end +points = detectSURFFeatures(image,'MetricThreshold', 1000); +% points = detectFASTFeatures(image); +points = selectUniform(points, 500, size(image)); +% points = selectStrongest(points, 200);%, size(image)); +[feature, vPoints] = extractFeatures(image, points, 'Upright', true); +end \ No newline at end of file diff --git a/VisualOdometry/code/customMatchFeature.m b/VisualOdometry/code/customMatchFeature.m new file mode 100644 index 0000000..c743fce --- /dev/null +++ b/VisualOdometry/code/customMatchFeature.m @@ -0,0 +1,9 @@ +function [points , feature,idxPair, vPoints]=customMatchFeature(image,pFeature,pImage,pvPoints) +if size(image,3) == 3 +image=rgb2gray(image); +end +[points , feature, vPoints]=customFeature(image); +idxPair = matchFeatures(pFeature, feature, 'Unique', true); +matchedPoints1 = pvPoints(idxPair(:, 1)); +matchedPoints2 = vPoints(idxPair(:, 2)); +end \ No newline at end of file diff --git a/VisualOdometry/code/helperEstimateRelativePos.m b/VisualOdometry/code/helperEstimateRelativePos.m deleted file mode 100644 index 4e3fbe5..0000000 --- a/VisualOdometry/code/helperEstimateRelativePos.m +++ /dev/null @@ -1,18 +0,0 @@ -function [orientation, location] = ... - helperEstimateRelativePos(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 index 40898eb..3138366 100644 --- a/VisualOdometry/code/main.m +++ b/VisualOdometry/code/main.m @@ -1,5 +1,4 @@ %% -% close all;warning off;load('data.mat'); clc;clear all;close all;warning off; cd ..; imgFolder = fullfile('input\Oxford_dataset\stereo\centre\'); @@ -9,23 +8,122 @@ [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'); +player = vision.VideoPlayer('Position', [20, 400, 650, 510]); %% +image = demosaic(readimage(allImages,1),'gbrg'); +pImage = UndistortImage(image,LUT); +step(player, pImage); +[pPoints , pFeature, pvPoints] = customFeature(pImage); -start=1; +estimatedView = viewSet; +estimatedView = addView(estimatedView, 1, 'Points', pPoints, 'Orientation',... + eye(3),'Location', [0 0 0]); +image = demosaic(readimage(allImages, 2),'gbrg'); +undistortedImg = UndistortImage(image,LUT); +[points,feature,indexPairs,vPoints] = customMatchFeature(undistortedImg,pFeature,pImage,pvPoints); +step(player, undistortedImg); +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); +matchedPoints1 = pPoints(indexPairs(:,1)); +matchedPoints2 = points(indexPairs(:,2)); + +[Cset,Rset] = ExtractCameraPose(E); + +[cRot,cT,~] = SelectCorrectEssentialCameraMatrix(Rset,Cset,... + [matchedPoints1.Location ones(matchedPoints1.Count,1)]',... + [matchedPoints2.Location ones(matchedPoints2.Count,1)]',K); + +estimatedView = addView(estimatedView, 2, 'Points', points,... + 'Orientation', cRot,'Location', cT'); +estimatedView = addConnection(estimatedView, 1, 2, 'Matches', indexPairs); + +pImage = undistortedImg; +pPoints = points; +pFeature = feature; +pvPoints = vPoints; +%% +figure +axis([20,1000, -500, 20, 10, 2000]); +% axis([-1000, 20, -30, 500, -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]); + +grid on; +xlabel('X (cm)'); +ylabel('Y (cm)'); +zlabel('Z (cm)'); +hold on; + +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)); +%% +start=3; for i = start:length(allImages.Files) image = demosaic(readimage(allImages,i),'gbrg'); undistortedImg = UndistortImage(image,LUT); - if (i < start+1) - - else - [point1,point2]=matchPoint(rgb2gray(undistortedImg),rgb2gray(pImage)); - figure(1); showMatchedFeatures(undistortedImg,pImage,point1,point2); - [orient, loc] = helperEstimateRelativePos(... - point1, point2, cameraParams) - end -% figure(1); -% imshow(undistortedImg); - pImage=undistortedImg; -% pause(1/10); -end \ No newline at end of file + + step(player, undistortedImg); + [points,feature,indexPairs,vPoints] = customMatchFeature(undistortedImg,pFeature,pImage, pvPoints); + 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); + + [cRot,cT,~] = SelectCorrectEssentialCameraMatrix(Rset,Cset,... + [matchedPoints1.Location ones(matchedPoints1.Count,1)]',... + [matchedPoints2.Location ones(matchedPoints2.Count,1)]',K); + + 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',cRot,'Location', cT); + + updateCamera(i,trajectoryEstimated,camEstimated,poses(estimatedView)); + pImage = undistortedImg; + pPoints = points; + pFeature = feature; + pvPoints = vPoints; + pause(1/10); +end + +%% +% load gong.mat; +% sound(y); +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, 'b-'); +set(trajectoryEstimated, 'XData', locations(:,1), 'YData', ... + zeros(size(locations,1),1), 'ZData', locations(:,3)); \ No newline at end of file diff --git a/VisualOdometry/code/main_final.m b/VisualOdometry/code/main_final.m deleted file mode 100644 index cce1d6b..0000000 --- a/VisualOdometry/code/main_final.m +++ /dev/null @@ -1,283 +0,0 @@ - -images = imageDatastore(fullfile(toolboxdir('vision'), 'visiondata', ... - 'NewTsukuba')); - -K = [615 0 320; 0 615 240; 0 0 1]'; -cameraParams = cameraParameters('IntrinsicMatrix', K); - -% Load ground truth camera poses. -load(fullfile(toolboxdir('vision'), 'visiondata', ... - 'visualOdometryGroundTruth.mat')); - -% Create an empty viewSet object to manage the data associated with each view. -vSet = viewSet; - -% Read and display the first image. -Irgb = readimage(images, 1); -player = vision.VideoPlayer('Position', [20, 400, 650, 510]); -step(player, Irgb); - - -prevI = undistortImage(rgb2gray(Irgb), cameraParams); - -prevPoints = detectSURFFeatures(prevI, 'MetricThreshold', 500); - -numPoints = 150; -prevPoints = selectUniform(prevPoints, numPoints, size(prevI)); - -prevFeatures = extractFeatures(prevI, prevPoints, 'Upright', true); - -viewId = 1; -vSet = addView(vSet, viewId, 'Points', prevPoints, 'Orientation', eye(3),... - 'Location', [0 0 0]); - -%% Plot Initial Camera Pose - -% Setup axes. -figure -axis([-220, 50, -140, 20, -50, 300]); - -% 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]); - -grid on -xlabel('X (cm)'); -ylabel('Y (cm)'); -zlabel('Z (cm)'); -hold on - -% Plot estimated camera pose. -cameraSize = 7; -camEstimated = plotCamera('Size', cameraSize, 'Location',... - vSet.Views.Location{1}, 'Orientation', vSet.Views.Orientation{1},... - 'Color', 'g', 'Opacity', 0); - -% Plot actual camera pose. -camActual = plotCamera('Size', cameraSize, 'Location', ... - groundTruthPoses.Location{1}, 'Orientation', ... - groundTruthPoses.Orientation{1}, 'Color', 'b', 'Opacity', 0); - -% Initialize camera trajectories. -trajectoryEstimated = plot3(0, 0, 0, 'g-'); -trajectoryActual = plot3(0, 0, 0, 'b-'); - -legend('Estimated Trajectory', 'Actual Trajectory'); -title('Camera Trajectory'); - -%% Estimate the Pose of the Second View - -% Read and display the image. -viewId = 2; -Irgb = readimage(images, viewId); -step(player, Irgb); - -% Convert to gray scale and undistort. -I = undistortImage(rgb2gray(Irgb), cameraParams); - -% Match features between the previous and the current image. -[currPoints, currFeatures, indexPairs] = helperDetectAndMatchFeatures(... - prevFeatures, I); - -% Estimate the pose of the current view relative to the previous view. -[orient, loc, inlierIdx] = helperEstimateRelativePose(... - prevPoints(indexPairs(:,1)), currPoints(indexPairs(:,2)), cameraParams); - -% Exclude epipolar outliers. -indexPairs = indexPairs(inlierIdx, :); - -% Add the current view to the view set. -vSet = addView(vSet, viewId, 'Points', currPoints, 'Orientation', orient, ... - 'Location', loc); -% Store the point matches between the previous and the current views. -vSet = addConnection(vSet, viewId-1, viewId, 'Matches', indexPairs); - -%% -% The location of the second view relative to the first view can only be -% recovered up to an unknown scale factor. Compute the scale factor from -% the ground truth using , -% simulating an external sensor, which would be used in a typical monocular -% visual odometry system. - -vSet = helperNormalizeViewSet(vSet, groundTruthPoses); - -%% -% Update camera trajectory plots using -% and -% . - -helperUpdateCameraPlots(viewId, camEstimated, camActual, poses(vSet), ... - groundTruthPoses); -helperUpdateCameraTrajectories(viewId, trajectoryEstimated, trajectoryActual,... - poses(vSet), groundTruthPoses); - -prevI = I; -prevFeatures = currFeatures; -prevPoints = currPoints; - -%% Bootstrap Estimating Camera Trajectory Using Global Bundle Adjustment -% Find 3D-to-2D correspondences between world points triangulated from the -% previous two views and image points from the current view. Use -% -% to find the matches that satisfy the epipolar constraint, and then use -% -% to triangulate 3-D points from the previous two views and find the -% corresponding 2-D points in the current view. - -for viewId = 3:15 - % Read and display the next image - Irgb = readimage(images, viewId); - step(player, Irgb); - - % Convert to gray scale and undistort. - I = undistortImage(rgb2gray(Irgb), cameraParams); - - % Match points between the previous and the current image. - [currPoints, currFeatures, indexPairs] = helperDetectAndMatchFeatures(... - prevFeatures, I); - - % Eliminate outliers from feature matches. - inlierIdx = helperFindEpipolarInliers(prevPoints(indexPairs(:,1)),... - currPoints(indexPairs(:, 2)), cameraParams); - indexPairs = indexPairs(inlierIdx, :); - - % Triangulate points from the previous two views, and find the - % corresponding points in the current view. - [worldPoints, imagePoints] = helperFind3Dto2DCorrespondences(vSet,... - cameraParams, indexPairs, currPoints); - - % Since RANSAC involves a stochastic process, it may sometimes not - % reach the desired confidence level and exceed maximum number of - % trials. Disable the warning when that happens since the outcomes are - % still valid. - warningstate = warning('off','vision:ransac:maxTrialsReached'); - - % Estimate the world camera pose for the current view. - [orient, loc] = estimateWorldCameraPose(imagePoints, worldPoints, ... - cameraParams, 'Confidence', 99.99, 'MaxReprojectionError', 0.8); - - % Restore the original warning state - warning(warningstate) - - % Add the current view to the view set. - vSet = addView(vSet, viewId, 'Points', currPoints, 'Orientation', orient, ... - 'Location', loc); - - % Store the point matches between the previous and the current views. - vSet = addConnection(vSet, viewId-1, viewId, 'Matches', indexPairs); - - tracks = findTracks(vSet); % Find point tracks spanning multiple views. - - camPoses = poses(vSet); % Get camera poses for all views. - - % Triangulate initial locations for the 3-D world points. - xyzPoints = triangulateMultiview(tracks, camPoses, cameraParams); - - % Refine camera poses using bundle adjustment. - [~, camPoses] = bundleAdjustment(xyzPoints, tracks, camPoses, ... - cameraParams, 'PointsUndistorted', true, 'AbsoluteTolerance', 1e-9,... - 'RelativeTolerance', 1e-9, 'MaxIterations', 300); - - vSet = updateView(vSet, camPoses); % Update view set. - - % Bundle adjustment can move the entire set of cameras. Normalize the - % view set to place the first camera at the origin looking along the - % Z-axes and adjust the scale to match that of the ground truth. - vSet = helperNormalizeViewSet(vSet, groundTruthPoses); - - % Update camera trajectory plot. - helperUpdateCameraPlots(viewId, camEstimated, camActual, poses(vSet), ... - groundTruthPoses); - helperUpdateCameraTrajectories(viewId, trajectoryEstimated, ... - trajectoryActual, poses(vSet), groundTruthPoses); - - prevI = I; - prevFeatures = currFeatures; - prevPoints = currPoints; -end - -%% Estimate Remaining Camera Trajectory Using Windowed Bundle Adjustment -% Estimate the remaining camera trajectory by using windowed bundle -% adjustment to only refine the last 15 views, in order to limit the amount -% of computation. Furthermore, bundle adjustment does not have to be called -% for every view, because |estimateWorldCameraPose| computes the pose in the -% same units as the 3-D points. This section calls bundle adjustment for -% every 7th view. The window size and the frequency of calling bundle -% adjustment have been chosen experimentally. - -for viewId = 16:numel(images.Files) - % Read and display the next image - Irgb = readimage(images, viewId); - step(player, Irgb); - - % Convert to gray scale and undistort. - I = undistortImage(rgb2gray(Irgb), cameraParams); - - % Match points between the previous and the current image. - [currPoints, currFeatures, indexPairs] = helperDetectAndMatchFeatures(... - prevFeatures, I); - - % Triangulate points from the previous two views, and find the - % corresponding points in the current view. - [worldPoints, imagePoints] = helperFind3Dto2DCorrespondences(vSet, ... - cameraParams, indexPairs, currPoints); - - % Since RANSAC involves a stochastic process, it may sometimes not - % reach the desired confidence level and exceed maximum number of - % trials. Disable the warning when that happens since the outcomes are - % still valid. - 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); - - % Restore the original warning state - warning(warningstate) - - % Add the current view and connection to the view set. - vSet = addView(vSet, viewId, 'Points', currPoints, 'Orientation', orient, ... - 'Location', loc); - vSet = addConnection(vSet, viewId-1, viewId, 'Matches', indexPairs); - - % Refine estimated camera poses using windowed bundle adjustment. Run - % the optimization every 7th view. - if mod(viewId, 7) == 0 - % Find point tracks in the last 15 views and triangulate. - windowSize = 15; - startFrame = max(1, viewId - windowSize); - tracks = findTracks(vSet, startFrame:viewId); - camPoses = poses(vSet, startFrame:viewId); - [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 < 2; - - [~, camPoses] = bundleAdjustment(xyzPoints(idx, :), tracks(idx), ... - camPoses, cameraParams, 'FixedViewIDs', fixedIds, ... - 'PointsUndistorted', true, 'AbsoluteTolerance', 1e-9,... - 'RelativeTolerance', 1e-9, 'MaxIterations', 300); - - vSet = updateView(vSet, camPoses); % Update view set. - end - - % Update camera trajectory plot. - helperUpdateCameraPlots(viewId, camEstimated, camActual, poses(vSet), ... - groundTruthPoses); - helperUpdateCameraTrajectories(viewId, trajectoryEstimated, ... - trajectoryActual, poses(vSet), groundTruthPoses); - - prevI = I; - prevFeatures = currFeatures; - prevPoints = currPoints; -end - -hold off - -displayEndOfDemoMessage(mfilename) diff --git a/VisualOdometry/code/matchPoint.m b/VisualOdometry/code/matchPoint.m deleted file mode 100644 index 49568b9..0000000 --- a/VisualOdometry/code/matchPoint.m +++ /dev/null @@ -1,15 +0,0 @@ -function [matchedPoints1,matchedPoints2] = matchPoint( undistortedImg,pImage) - point1= detectSURFFeatures(undistortedImg, 'MetricThreshold', 500); -% point1 = selectUniform(point1, 200, size(undistortedImg)); - [f1,vpts1] = extractFeatures((undistortedImg),point1); - - point2 = detectSURFFeatures(pImage, 'MetricThreshold', 500); - % point2 = selectUniform(point2, 200, size(pImage)); - [f2,vpts2] = extractFeatures(pImage,point2); - - indexPairs = matchFeatures(f1,f2) ; - - matchedPoints1 = vpts1(indexPairs(:,1)); - matchedPoints2 = vpts2(indexPairs(:,2)); -end - diff --git a/VisualOdometry/code/data.mat b/VisualOdometry/code/matlab.mat similarity index 81% rename from VisualOdometry/code/data.mat rename to VisualOdometry/code/matlab.mat index 31dfb41..b08e8d4 100644 Binary files a/VisualOdometry/code/data.mat and b/VisualOdometry/code/matlab.mat differ diff --git a/VisualOdometry/code/memory.mat b/VisualOdometry/code/memory.mat new file mode 100644 index 0000000..fbbf8b6 Binary files /dev/null and b/VisualOdometry/code/memory.mat differ diff --git a/VisualOdometry/code/normalise2dpts.m b/VisualOdometry/code/normalise2dpts.m new file mode 100644 index 0000000..db6fe4d --- /dev/null +++ b/VisualOdometry/code/normalise2dpts.m @@ -0,0 +1,70 @@ +% NORMALISE2DPTS - normalises 2D homogeneous points +% +% Function translates and normalises a set of 2D homogeneous points +% so that their centroid is at the origin and their mean distance from +% the origin is sqrt(2). This process typically improves the +% conditioning of any equations used to solve homographies, fundamental +% matrices etc. +% +% Usage: [newpts, T] = normalise2dpts(pts) +% +% Argument: +% pts - 3xN array of 2D homogeneous coordinates +% +% Returns: +% newpts - 3xN array of transformed 2D homogeneous coordinates. The +% scaling parameter is normalised to 1 unless the point is at +% infinity. +% T - The 3x3 transformation matrix, newpts = T*pts +% +% If there are some points at infinity the normalisation transform +% is calculated using just the finite points. Being a scaling and +% translating transform this will not affect the points at infinity. + +% Peter Kovesi +% School of Computer Science & Software Engineering +% The University of Western Australia +% pk at csse uwa edu au +% http://www.csse.uwa.edu.au/~pk +% +% May 2003 - Original version +% February 2004 - Modified to deal with points at infinity. +% December 2008 - meandist calculation modified to work with Octave 3.0.1 +% (thanks to Ron Parr) + + +function [newpts, T] = normalise2dpts(pts) + + if size(pts,1) ~= 3 + error('pts must be 3xN'); + end + + % Find the indices of the points that are not at infinity + finiteind = find(abs(pts(3,:)) > eps); + + if length(finiteind) ~= size(pts,2) + warning('Some points are at infinity'); + end + + % For the finite points ensure homogeneous coords have scale of 1 + pts(1,finiteind) = pts(1,finiteind)./pts(3,finiteind); + pts(2,finiteind) = pts(2,finiteind)./pts(3,finiteind); + pts(3,finiteind) = 1; + + c = mean(pts(1:2,finiteind)')'; % Centroid of finite points + newp(1,finiteind) = pts(1,finiteind)-c(1); % Shift origin to centroid. + newp(2,finiteind) = pts(2,finiteind)-c(2); + + dist = sqrt(newp(1,finiteind).^2 + newp(2,finiteind).^2); + meandist = mean(dist(:)); % Ensure dist is a column vector for Octave 3.0.1 + + scale = sqrt(2)/meandist; + + T = [scale 0 -scale*c(1) + 0 scale -scale*c(2) + 0 0 1 ]; + + newpts = T*pts; + + + \ No newline at end of file diff --git a/VisualOdometry/code/updateCamera.m b/VisualOdometry/code/updateCamera.m new file mode 100644 index 0000000..7740775 --- /dev/null +++ b/VisualOdometry/code/updateCamera.m @@ -0,0 +1,7 @@ +function updateCamera(viewId,trajectoryEstimated,camEstimated,posesEstimated) +camEstimated.Location = posesEstimated.Location{viewId}; +camEstimated.Orientation = posesEstimated.Orientation{viewId}; +locations = cat(1, posesEstimated.Location{:}); +set(trajectoryEstimated, 'XData', locations(:,1), 'YData', ... + locations(:,2), 'ZData', locations(:,3)); +end \ No newline at end of file diff --git a/VisualOdometry/output/trajectory.fig b/VisualOdometry/output/trajectory.fig new file mode 100644 index 0000000..7b1ab18 Binary files /dev/null and b/VisualOdometry/output/trajectory.fig differ diff --git a/VisualOdometry/output/trajectory.jpg b/VisualOdometry/output/trajectory.jpg new file mode 100644 index 0000000..28fd7eb Binary files /dev/null and b/VisualOdometry/output/trajectory.jpg differ diff --git a/VisualOdometry/output/trajectory1.jpg b/VisualOdometry/output/trajectory1.jpg new file mode 100644 index 0000000..6f84b1d Binary files /dev/null and b/VisualOdometry/output/trajectory1.jpg differ