├── docs ├── report_alter.pdf ├── presentation_1.pdf └── readme.txt ├── results └── 2000_frames.bmp ├── sample_data ├── image_2 │ ├── 000000.png │ └── 000001.png ├── image_3 │ ├── 000000.png │ └── 000001.png ├── readme.txt └── calib.txt ├── LICENSE ├── README.md └── src ├── findPotentialNodes.m ├── bucketFeatures.m ├── demo.m ├── updateClique.m ├── minimize.m └── visodo.m /docs/report_alter.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/avisingh599/vo-howard08/HEAD/docs/report_alter.pdf -------------------------------------------------------------------------------- /docs/presentation_1.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/avisingh599/vo-howard08/HEAD/docs/presentation_1.pdf -------------------------------------------------------------------------------- /results/2000_frames.bmp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/avisingh599/vo-howard08/HEAD/results/2000_frames.bmp -------------------------------------------------------------------------------- /sample_data/image_2/000000.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/avisingh599/vo-howard08/HEAD/sample_data/image_2/000000.png -------------------------------------------------------------------------------- /sample_data/image_2/000001.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/avisingh599/vo-howard08/HEAD/sample_data/image_2/000001.png -------------------------------------------------------------------------------- /sample_data/image_3/000000.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/avisingh599/vo-howard08/HEAD/sample_data/image_3/000000.png -------------------------------------------------------------------------------- /sample_data/image_3/000001.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/avisingh599/vo-howard08/HEAD/sample_data/image_3/000001.png -------------------------------------------------------------------------------- /docs/readme.txt: -------------------------------------------------------------------------------- 1 | Parts of this report are not exactly the same as the algorithm implemented here. 2 | These parts would soon be updated. -------------------------------------------------------------------------------- /sample_data/readme.txt: -------------------------------------------------------------------------------- 1 | This sample is taken from the KITTI Visual Odometry dataset. 2 | Please cite their research papers if you happen to use it 3 | in your work. 4 | 5 | http://www.cvlibs.net/datasets/kitti/eval_odometry.php 6 | -------------------------------------------------------------------------------- /sample_data/calib.txt: -------------------------------------------------------------------------------- 1 | P0: 7.188560000000e+02 0.000000000000e+00 6.071928000000e+02 0.000000000000e+00 0.000000000000e+00 7.188560000000e+02 1.852157000000e+02 0.000000000000e+00 0.000000000000e+00 0.000000000000e+00 1.000000000000e+00 0.000000000000e+00 2 | P1: 7.188560000000e+02 0.000000000000e+00 6.071928000000e+02 -3.861448000000e+02 0.000000000000e+00 7.188560000000e+02 1.852157000000e+02 0.000000000000e+00 0.000000000000e+00 0.000000000000e+00 1.000000000000e+00 0.000000000000e+00 3 | P2: 7.188560000000e+02 0.000000000000e+00 6.071928000000e+02 4.538225000000e+01 0.000000000000e+00 7.188560000000e+02 1.852157000000e+02 -1.130887000000e-01 0.000000000000e+00 0.000000000000e+00 1.000000000000e+00 3.779761000000e-03 4 | P3: 7.188560000000e+02 0.000000000000e+00 6.071928000000e+02 -3.372877000000e+02 0.000000000000e+00 7.188560000000e+02 1.852157000000e+02 2.369057000000e+00 0.000000000000e+00 0.000000000000e+00 1.000000000000e+00 4.915215000000e-03 5 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | The MIT License 2 | 3 | Copyright (c) 2015 Avi Singh 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in 13 | all copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 21 | THE SOFTWARE. -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | ##Stereo Visual Odometry 2 | 3 | This is a MATLAB-based implementation of Andrew Howard's 2008 paper: 4 | Real-Time Stereo Visual Odometry for Autonomous Ground Vehicles 5 | 6 | [Link to the paper](https://www-robotics.jpl.nasa.gov/publications/Andrew_Howard/howard_iros08_visodom.pdf). 7 | 8 | ##[Blog Post](http://avisingh599.github.io/vision/visual-odometry-full/) 9 | 10 | Note that this is not an exact implementation. A few things, such as the feature detector, 11 | are different. Also, since it is based on MATLAB, and not C/C++ like the original author, 12 | it is not really "real time". In fact, each VO computation takes around 10-15 seconds. 13 | However, the core algorithm is the same. 14 | 15 | ###Requirements: 16 | 17 | MATLAB R2014a or newer, with the following toolbooxes: 18 | 19 | 1. Computer Vision 20 | 2. Optmization 21 | 22 | ###How to run? 23 | A file demo.m is provided which takes in the input images provided in the sample_data folder, 24 | and runs the algorithm on it. 25 | For a better test of the algorithm, it is suggested that you download KITTI's Visual Odometry 26 | dataset, and test the algorithm on their sequences. 27 | 28 | In order to run this algorithm on your own data, you must modify the intrinsic and extrinsic 29 | calibration parameter in the code. 30 | 31 | ###Results 32 | ![Results on the KITTI VO Benchmark Sequence 0 (2000 frames)](https://github.com/avisingh599/vo-howard08/blob/master/results/2000_frames.bmp) 33 | 34 | Results on the KITTI VO Benchmark Sequence 0 (2000 frames). Blue is the ground truth, green is the VO estimation. 35 | 36 | ###Contact 37 | For any queries, contact: avisingh599@gmail.com 38 | 39 | ###License 40 | MIT -------------------------------------------------------------------------------- /src/findPotentialNodes.m: -------------------------------------------------------------------------------- 1 | % The MIT License 2 | % 3 | % Copyright (c) 2015 Avi Singh 4 | % 5 | % Permission is hereby granted, free of charge, to any person obtaining a copy 6 | % of this software and associated documentation files (the "Software"), to deal 7 | % in the Software without restriction, including without limitation the rights 8 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | % copies of the Software, and to permit persons to whom the Software is 10 | % furnished to do so, subject to the following conditions: 11 | % 12 | % The above copyright notice and this permission notice shall be included in 13 | % all copies or substantial portions of the Software. 14 | % 15 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | % FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | % AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | % LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | % OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 21 | % THE SOFTWARE. 22 | 23 | %% this function is used in the clique selection part of the algorithm 24 | % INPUT: 25 | % clique: the current clique (a vector containing node indices) 26 | % M: the graph, represented as an adjacency matrix 27 | 28 | % OUTPUT: 29 | % newSet -> a set of potential nodes that can added to the clique, such 30 | % that is still remains a clique if one of these nodes is added. A vector 31 | % containing the indices of the potential nodes 32 | 33 | function newSet = findPotentialNodes(clique, M) 34 | 35 | newSet = M(:,clique(1)); 36 | if (size(clique)>1) 37 | for i=2:length(clique) 38 | newSet = newSet & M(:,clique(i)); 39 | end 40 | end 41 | 42 | for i=1:length(clique) 43 | newSet(clique(i)) = 0; 44 | end 45 | 46 | 47 | -------------------------------------------------------------------------------- /src/bucketFeatures.m: -------------------------------------------------------------------------------- 1 | % The MIT License 2 | % 3 | % Copyright (c) 2015 Avi Singh 4 | % 5 | % Permission is hereby granted, free of charge, to any person obtaining a copy 6 | % of this software and associated documentation files (the "Software"), to deal 7 | % in the Software without restriction, including without limitation the rights 8 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | % copies of the Software, and to permit persons to whom the Software is 10 | % furnished to do so, subject to the following conditions: 11 | % 12 | % The above copyright notice and this permission notice shall be included in 13 | % all copies or substantial portions of the Software. 14 | % 15 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | % FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | % AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | % LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | % OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 21 | % THE SOFTWARE. 22 | 23 | % this functions returns a list of feature vectors, such that there are at 24 | % most numCorners number of features in every block (roughly ~100x100) of 25 | % the image with which we are working. This ensures a uniform distribution 26 | % of features over the image 27 | 28 | function points = bucketFeatures(I, h, b, h_break, b_break, numCorners) 29 | % input image I should be grayscale 30 | 31 | y = floor(linspace(1, h - h/h_break, h_break)); 32 | x = floor(linspace(1, b - b/b_break, b_break)); 33 | 34 | final_points = []; 35 | for i=1:length(y) 36 | for j=1:length(x) 37 | roi = [x(j),y(i),floor(b/b_break),floor(h/h_break)]; 38 | corners = detectFASTFeatures(I, 'MinQuality', 0.00, 'MinContrast', 0.1, 'ROI',roi ); 39 | corners = corners.selectStrongest(numCorners); 40 | final_points = vertcat(final_points, corners.Location); 41 | end 42 | end 43 | points = cornerPoints(final_points); 44 | -------------------------------------------------------------------------------- /src/demo.m: -------------------------------------------------------------------------------- 1 | % The MIT License 2 | % 3 | % Copyright (c) 2015 Avi Singh 4 | % 5 | % Permission is hereby granted, free of charge, to any person obtaining a copy 6 | % of this software and associated documentation files (the "Software"), to deal 7 | % in the Software without restriction, including without limitation the rights 8 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | % copies of the Software, and to permit persons to whom the Software is 10 | % furnished to do so, subject to the following conditions: 11 | % 12 | % The above copyright notice and this permission notice shall be included in 13 | % all copies or substantial portions of the Software. 14 | % 15 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | % FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | % AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | % LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | % OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 21 | % THE SOFTWARE. 22 | 23 | % read calibration parameters (intrinsic and extrinsic) from the 24 | % calibration file 25 | calibname = '../sample_data/calib.txt'; 26 | T = readtable(calibname, 'Delimiter', 'space', 'ReadRowNames', true, 'ReadVariableNames', false); 27 | A = table2array(T); 28 | 29 | P1 = vertcat(A(1,1:4), A(1,5:8), A(1,9:12)); 30 | P2 = vertcat(A(2,1:4), A(2,5:8), A(2,9:12)); 31 | 32 | I1_l = rgb2gray(imread('../sample_data/image_2/000000.png')); 33 | I1_r = rgb2gray(imread('../sample_data/image_3/000000.png')); 34 | I2_l = rgb2gray(imread('../sample_data/image_2/000001.png')); 35 | I2_r = rgb2gray(imread('../sample_data/image_3/000001.png')); 36 | 37 | tic; 38 | [R1, t1, cliqueSize, outlier, resnorm] = visodo(I1_l, I1_r, I2_l, I2_r, P1, P2); 39 | toc 40 | 41 | %only accept the results in which there are at least three point in the 42 | %clique which is used for the determination of R, t 43 | % if (cliqueSize >= 3) 44 | % R = R1; 45 | % t = t1; 46 | % end 47 | 48 | % use these lines if you want to 49 | % pos = pos + Rpos*t; 50 | % Rpos = R*Rpos; 51 | -------------------------------------------------------------------------------- /src/updateClique.m: -------------------------------------------------------------------------------- 1 | % The MIT License 2 | % 3 | % Copyright (c) 2015 Avi Singh 4 | % 5 | % Permission is hereby granted, free of charge, to any person obtaining a copy 6 | % of this software and associated documentation files (the "Software"), to deal 7 | % in the Software without restriction, including without limitation the rights 8 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | % copies of the Software, and to permit persons to whom the Software is 10 | % furnished to do so, subject to the following conditions: 11 | % 12 | % The above copyright notice and this permission notice shall be included in 13 | % all copies or substantial portions of the Software. 14 | % 15 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | % FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | % AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | % LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | % OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 21 | % THE SOFTWARE. 22 | 23 | %% this function is used in the clique selection part of the algorithm 24 | % INPUT: 25 | % clique: the current clique (a vector containing node indices) 26 | % potentialNodes: list of nodes that can be added to the clique, so that it 27 | % still remains a clique when one such node is added, a vector of node 28 | % indices 29 | % M: the graph, represented as an adjacency matrix 30 | 31 | %OUTPUT: 32 | % cl -> the new clique, after adding one of the nodes in the potentialNodes 33 | 34 | function cl = updateClique(potentialNodes, clique, M) 35 | 36 | maxNumMatches = 0; 37 | curr_max = 0; 38 | for i = 1:length(potentialNodes) 39 | if(potentialNodes(i)==1) 40 | numMatches = 0; 41 | for j = 1:length(potentialNodes) 42 | if (potentialNodes(j) & M(i,j)) 43 | numMatches = numMatches + 1; 44 | end 45 | end 46 | if (numMatches>=maxNumMatches) 47 | curr_max = i; 48 | maxNumMatches = numMatches; 49 | end 50 | end 51 | end 52 | 53 | if (maxNumMatches~=0) 54 | clique(length(clique)+1) = curr_max; 55 | end 56 | 57 | cl = clique; 58 | 59 | -------------------------------------------------------------------------------- /src/minimize.m: -------------------------------------------------------------------------------- 1 | % The MIT License 2 | % 3 | % Copyright (c) 2015 Avi Singh 4 | % 5 | % Permission is hereby granted, free of charge, to any person obtaining a copy 6 | % of this software and associated documentation files (the "Software"), to deal 7 | % in the Software without restriction, including without limitation the rights 8 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | % copies of the Software, and to permit persons to whom the Software is 10 | % furnished to do so, subject to the following conditions: 11 | % 12 | % The above copyright notice and this permission notice shall be included in 13 | % all copies or substantial portions of the Software. 14 | % 15 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | % FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | % AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | % LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | % OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 21 | % THE SOFTWARE. 22 | 23 | %% the objective function (the reprojection error) which is minimized using 24 | % levenberg-marquardt optimization method 25 | % The parameters 26 | function F = minimize(PAR, F1, F2, W1, W2, P1) 27 | r = PAR(1:3); 28 | t = PAR(4:6); 29 | %F1, F2 -> 2d coordinates of features in I1_l, I2_l 30 | %W1, W2 -> 3d coordinates of the features that have been triangulated 31 | %P1, P2 -> Projection matrices for the two cameras 32 | %r, t -> 3x1 vectors, need to be varied for the minimization 33 | F = zeros(2*size(F1,1), 3); 34 | reproj1 = zeros(size(F1,1), 3); 35 | reproj2 = zeros(size(F1,1), 3); 36 | 37 | dcm = angle2dcm( r(1), r(2), r(3), 'ZXZ' ); 38 | tran = [ horzcat(dcm, t); [0 0 0 1]]; 39 | 40 | for k = 1:size(F1,1) 41 | f1 = F1(k, :)'; 42 | f1(3) = 1; 43 | w2 = W2(k, :)'; 44 | w2(4) = 1; 45 | 46 | f2 = F2(k, :)'; 47 | f2(3) = 1; 48 | w1 = W1(k, :)'; 49 | w1(4) = 1; 50 | 51 | f1_repr = P1*(tran)*w2; 52 | f1_repr = f1_repr/f1_repr(3); 53 | f2_repr = P1*pinv(tran)*w1; 54 | f2_repr = f2_repr/f2_repr(3); 55 | 56 | reproj1(k, :) = (f1 - f1_repr); 57 | reproj2(k, :) = (f2 - f2_repr); 58 | end 59 | 60 | F = [reproj1; reproj2]; -------------------------------------------------------------------------------- /src/visodo.m: -------------------------------------------------------------------------------- 1 | % The MIT License 2 | % 3 | % Copyright (c) 2015 Avi Singh 4 | % 5 | % Permission is hereby granted, free of charge, to any person obtaining a copy 6 | % of this software and associated documentation files (the "Software"), to deal 7 | % in the Software without restriction, including without limitation the rights 8 | % to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | % copies of the Software, and to permit persons to whom the Software is 10 | % furnished to do so, subject to the following conditions: 11 | % 12 | % The above copyright notice and this permission notice shall be included in 13 | % all copies or substantial portions of the Software. 14 | % 15 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | % IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | % FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | % AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | % LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | % OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 21 | % THE SOFTWARE. 22 | 23 | function [R, T, cliqueSize, outliers, resnorm] = visodo(I1_l, I1_r, I2_l, I2_r, P1, P2) 24 | %% The core visual odometry function 25 | %INPUT: 26 | % I1_l -> a grayscale image, from camera 1 (left) at some time t 27 | % I1_r -> a grayscale image, from camera 2 (right) at some time t 28 | % I2_l -> a grayscale image, from camera 1 (left) at time t+1 29 | % I2_r -> a grayscale image, from camera 2 (right) at time t+1 30 | % P1 -> the [3x4] projection matrix of camera 1 (left) 31 | % P2 -> the [3x4] projection matrix of camera 2 (right) 32 | % 33 | %OUTPUT: 34 | % R-> The rotation matrix [3x3] describing the change in orientation from t to 35 | % t+1 36 | % T-> The tranlsation vector [3x1] describing the change in cartesian 37 | % coordinates of the vehicle from t to t+1 38 | % cliqueSize -> the size of the cliques used in the Levenberg-Marquardt 39 | % optimization (checkout the documentation/blog for more info) 40 | % 41 | disparityMap1 = disparity(I1_l,I1_r, 'DistanceThreshold', 5); 42 | disparityMap2 = disparity(I2_l,I2_r); 43 | 44 | [h,b] = size(I1_l); 45 | h_break = 4; 46 | b_break = 12; 47 | numCorners = 100; 48 | inlierThresh = 0.01; 49 | 50 | points1_l = bucketFeatures(I1_l, h, b, h_break, b_break, numCorners); 51 | tracker = vision.PointTracker('MaxBidirectionalError', 1); 52 | initialize(tracker, points1_l.Location, I1_l); 53 | [points2_l, validity] = step(tracker, I2_l); 54 | TF1 = validity(:)==0; 55 | points1_l = points1_l.Location; 56 | points1_l(TF1,:) = []; 57 | points2_l(TF1,:) = []; 58 | 59 | M1_l=points1_l; 60 | M2_l=points2_l; 61 | M2_l = round(M2_l); 62 | M1_r = M1_l; 63 | M2_r = M2_l; 64 | counter = 0; 65 | allow = []; 66 | for i = 1:size(M1_l,1) 67 | if ((disparityMap1(M1_l(i,2),M1_l(i,1))>0)&&(disparityMap1(M1_l(i,2),M1_l(i,1))<100)&&(disparityMap2(M2_l(i,2),M2_l(i,1))>0)&&(disparityMap2(M2_l(i,2),M2_l(i,1))<100)) 68 | M1_r(i, 1) = (M1_l(i, 1) - disparityMap1(M1_l(i,2),M1_l(i,1))); 69 | M2_r(i, 1) = (M2_l(i, 1) - disparityMap2(M2_l(i,2),M2_l(i,1))); 70 | allow(i) = 1; 71 | counter = counter + 1; 72 | end 73 | end 74 | 75 | TF1 = allow(:)==0; 76 | M1_r(TF1,:) = []; 77 | M1_l(TF1,:) = []; 78 | M2_r(TF1,:) = []; 79 | M2_l(TF1,:) = []; 80 | 81 | %figure; showMatchedFeatures(I1_l, I2_l, matchedPoints1_l, matchedPoints1_r); 82 | 83 | points3D_1 = ones(size(M1_l,1),4); % store homogeneous world coordinates 84 | points3D_2 = ones(size(M2_l,1),4); % store homogeneous world coordinates 85 | 86 | %For all point correspondences 87 | for i = 1:size(M1_l,1) 88 | % For all image locations from a list of correspondences build an A 89 | pointInImage1 = M1_l(i,:); 90 | pointInImage2 = M1_r(i,:); 91 | 92 | A = [ 93 | pointInImage1(1)*P1(3,:) - P1(1,:); 94 | pointInImage1(2)*P1(3,:) - P1(2,:); 95 | pointInImage2(1)*P2(3,:) - P2(1,:); 96 | pointInImage2(2)*P2(3,:) - P2(2,:)]; 97 | 98 | % Compute the 3-D location using the smallest singular value from the 99 | % singular value decomposition of the matrix A 100 | [~,~,V]=svd(A); 101 | 102 | X = V(:,end); 103 | X = X/X(end); 104 | 105 | % Store location 106 | points3D_1(i,:) = X'; 107 | 108 | end 109 | points3D_1 = points3D_1(:,1:3); 110 | %scatter3(points3D(:,1), points3D(:,2), points3D(:,3)) 111 | 112 | for i = 1:size(M1_l,1) 113 | % For all image locations from a list of correspondences build an A 114 | pointInImage1 = M2_l(i,:); 115 | pointInImage2 = M2_r(i,:); 116 | 117 | A = [ 118 | pointInImage1(1)*P1(3,:) - P1(1,:); 119 | pointInImage1(2)*P1(3,:) - P1(2,:); 120 | pointInImage2(1)*P2(3,:) - P2(1,:); 121 | pointInImage2(2)*P2(3,:) - P2(2,:)]; 122 | 123 | % Compute the 3-D location using the smallest singular value from the 124 | % singular value decomposition of the matrix A 125 | [~,~,V]=svd(A); 126 | 127 | X = V(:,end); 128 | X = X/X(end); 129 | 130 | % Store location 131 | points3D_2(i,:) = X'; 132 | 133 | end 134 | points3D_2 = points3D_2(:,1:3); 135 | 136 | TF1 = (abs(points3D_1(:,1)))>=50 | (abs(points3D_1(:,2)))>=50 | (abs(points3D_1(:,3)))>=50; 137 | TF2 = (abs(points3D_2(:,1)))>=50 | (abs(points3D_2(:,2)))>=50 | (abs(points3D_2(:,3)))>=50; 138 | 139 | Tdelete = TF1 | TF2; 140 | 141 | points3D_1(Tdelete,:) = []; 142 | points3D_2(Tdelete,:) = []; 143 | M1_l(Tdelete,:) = []; 144 | M2_l(Tdelete,:) = []; 145 | M1_r(Tdelete,:) = []; 146 | M2_r(Tdelete,:) = []; 147 | 148 | % Consistency Matrix M 149 | % Number of Row = Number of Columns = Number of Features 150 | 151 | M = zeros(size(M1_l,1)); % generates a square matrix 152 | 153 | for i = 1:size(M1_l,1) 154 | for j= 1:size(M1_l,1) 155 | if (abs(norm(points3D_2(i, :) - points3D_2(j, :)) - norm(points3D_1(i, :) - points3D_1(j, :))) < inlierThresh ) 156 | M(i,j) = 1; 157 | end 158 | end 159 | end 160 | 161 | %We now need to find the maximal set which 162 | edge_max = 0; 163 | edge_count = 0; 164 | curr_max = 0; 165 | 166 | for i = 1:size(M1_l,1) 167 | edge_count = 0; 168 | for j = 1:size(M1_l,1) 169 | if (M(i,j)==1) 170 | edge_count = edge_count + 1; 171 | end 172 | end 173 | if (edge_count>= edge_max) 174 | curr_max = i; 175 | edge_max = edge_count; 176 | end 177 | end 178 | 179 | % intialize the clique to the max_edge node found above 180 | % intialize a new set containg all the neighbours of the above node 181 | % among the elements of the above set, find the node which is connected to 182 | % most of the other nodes in the above initialized set 183 | % update the cliques with this new node 184 | 185 | clique = [curr_max]; 186 | % now find all neighbours of the above node 187 | % We need two functions here: Find Potential Additions to the Clique: this 188 | % function will take the current clique as the input, and compute the set 189 | % of nodes that can be added to the clique 190 | % Find the best addition to the Clique: This will take the set of nodes 191 | % that can be added to the clique as an input, and then use it. 192 | potentialNodes = findPotentialNodes(clique, M); 193 | i=0; 194 | while(sum(potentialNodes)) 195 | clique = updateClique(potentialNodes, clique, M); 196 | potentialNodes = findPotentialNodes(clique, M); 197 | i = i+1; 198 | end 199 | 200 | newCloud1 = ones(length(clique), 3); 201 | newCloud2 = ones(length(clique), 3); 202 | 203 | newfeatures1 = ones(length(clique), 2); 204 | newfeatures2 = ones(length(clique), 2); 205 | 206 | for i=1:length(clique) 207 | newCloud1(i, :) = points3D_1(clique(i), :); 208 | newCloud2(i, :) = points3D_2(clique(i), :); 209 | 210 | newfeatures1(i, :) = M1_l(clique(i), :); 211 | newfeatures2(i, :) = M2_l(clique(i), :); 212 | end 213 | 214 | 215 | %[R, T] = icp(newCloud1',newCloud2'); 216 | 217 | options = optimoptions(@lsqnonlin,'Algorithm','levenberg-marquardt','MaxFunEvals',1500); 218 | lb = []; 219 | ub = []; 220 | PAR0 = [0;0;0;0;0;0]; 221 | PAR = [0;0;0;0;0;0]; 222 | 223 | 224 | [PAR,resnorm,residual,~,output1] = lsqnonlin(@(PAR) minimize(PAR, newfeatures1, newfeatures2, newCloud1, newCloud2, P1),PAR0, lb, ub, options); 225 | 226 | TF11 = residual(1:0.5*size(residual,1), 1)>=1.0; 227 | TF12 = residual(1:0.5*size(residual,1), 2)>=1.0; 228 | TF1 = bitor(TF11, TF12); 229 | TF21 = residual(0.5*size(residual,1)+1:size(residual,1), 1)>=1.0; 230 | TF22 = residual(0.5*size(residual,1)+1:size(residual,1), 2)>=1.0; 231 | TF2 = bitor(TF21, TF22); 232 | 233 | TF = bitor(TF1,TF2); 234 | 235 | newfeatures1(TF,:) = []; 236 | newfeatures2(TF,:) = []; 237 | newCloud1(TF,:) = []; 238 | newCloud2(TF,:) = []; 239 | 240 | [PAR,resnorm,residual,~,output1] = lsqnonlin(@(PAR) minimize(PAR, newfeatures1, newfeatures2, newCloud1, newCloud2, P1),PAR, lb, ub, options); 241 | 242 | 243 | r = PAR(1:3); 244 | t = PAR(4:6); 245 | R = angle2dcm( r(1), r(2), r(3), 'ZXZ' ); 246 | T = t; 247 | %F1, F2 -> 2d 248 | outliers = sum(TF); 249 | cliqueSize = length(clique); 250 | --------------------------------------------------------------------------------