├── data ├── bun000.ply ├── bun045.ply ├── bun090.ply ├── bun180.ply ├── bun270.ply └── bun315.ply ├── demo.m ├── eigReg.m ├── estimateRigidTransform ├── crossTimesMatrix.m ├── demoEstimateRigidTransform.m ├── estimateRigidTransform.m ├── quat2rot.m └── sampleData.mat ├── flann ├── flann_build_index.m ├── flann_free_index.m ├── flann_load_index.m ├── flann_manual-1.8.4.pdf ├── flann_save_index.m ├── flann_search.m ├── flann_set_distance_type.m ├── nearest_neighbors.cpp ├── nearest_neighbors.mexw64 └── test_flann.m ├── matrixCompute.m ├── ransac.cpp ├── ransac.mexw64 ├── readme.txt └── svdCov.m /demo.m: -------------------------------------------------------------------------------- 1 | clc;clear;close all; 2 | 3 | addpath('./flann/'); 4 | addpath('./estimateRigidTransform'); 5 | 6 | gridStep = 0.01; 7 | Rho = 0.01; 8 | datapath = './data'; 9 | 10 | srcFileName = 'data/bun000.ply'; 11 | tarFileName = 'data/bun090.ply'; 12 | srcCloud = pcread(srcFileName); 13 | tarCloud = pcread(tarFileName); 14 | overlap = 0.5; 15 | tic; 16 | [T,src2tarEst] = eigReg(srcCloud,tarCloud,overlap,gridStep,Rho); 17 | Time = toc 18 | figure,pcshow(tarCloud.Location,'r'),hold on 19 | pcshow(src2tarEst','g') 20 | -------------------------------------------------------------------------------- /eigReg.m: -------------------------------------------------------------------------------- 1 | function [T,src2tarEst] = eigReg(srcCloud,tarCloud,overlap,gridStep,Rho) 2 | %This code is the Matlab implimentation of the paper, 3 | %"Fast Descriptors and Correspondence Propagation for Robust Global Point Cloud Registration," 4 | %IEEE transactions on Image Processing, 2017. 5 | %This code should be used only for academic research. 6 | %any other useage of this code should not be allowed without Author agreement. 7 | % If you have any problem or improvement idea about this code, please 8 | % contact Huan LEI with hlei.ziyan@gmail.com. 9 | 10 | %% parameter configuration for flann search 11 | params.algorithm = 'kdtree'; 12 | params.trees = 8; 13 | params.checks = 64; 14 | 15 | srcData = srcCloud.Location'; 16 | tarData = tarCloud.Location'; 17 | radii = (0.5:0.5:2)*gridStep; 18 | 19 | srcCloudDown = pcdownsample(srcCloud, 'gridAverage', Rho); 20 | tarCloudDown = pcdownsample(tarCloud, 'gridAverage', Rho); 21 | srcSeed = srcCloudDown.Location'; 22 | tarSeed = tarCloudDown.Location'; 23 | 24 | %% compute descriptors for seed points in the source point cloud 25 | K = length(radii); 26 | srcIdx = rangesearch(srcData',srcSeed',radii(1)); 27 | idxSz = cellfun(@length,srcIdx,'uni',true); 28 | srcIdx = srcIdx(idxSz>10); 29 | srcSeed = srcSeed(:,idxSz>10); 30 | M = sum(idxSz>10); 31 | idx = num2cell((1:M)'); 32 | [s,n] = cellfun(@(x,y)svdCov(x,y,srcData,srcSeed),srcIdx,idx,'uni',false); 33 | s = cell2mat(s); 34 | n = cell2mat(n); 35 | for k = 2:K 36 | srcIdx = rangesearch(srcData',srcSeed',radii(k)); 37 | [sk,nk] = cellfun(@(x,y)svdCov(x,y,srcData,srcSeed),srcIdx,idx,'uni',false); 38 | s = [s cell2mat(sk)]; 39 | n = [n cell2mat(nk)]; 40 | end 41 | s = s'; 42 | ds = diff(s); 43 | srcDesp = reshape(ds,3*(K-1),[]); 44 | n = mat2cell(n,3*ones(M,1),K); 45 | srcNorm = cellfun(@(x)reshape(x,[],1),n','uni',false); 46 | srcNorm = cell2mat(srcNorm); 47 | 48 | %% compute descriptors for seed points in the target point cloud 49 | tarIdx = rangesearch(tarData',tarSeed',radii(1)); 50 | idxSz = cellfun(@length,tarIdx,'uni',true); 51 | tarIdx = tarIdx(idxSz>10); 52 | tarSeed = tarSeed(:,idxSz>10); 53 | N = sum(idxSz>10); 54 | idx = num2cell((1:N)'); 55 | [s,n] = cellfun(@(x,y)svdCov(x,y,tarData,tarSeed),tarIdx,idx,'uni',false); 56 | s = cell2mat(s); 57 | n = cell2mat(n); 58 | for k = 2:K 59 | tarIdx = rangesearch(tarData',tarSeed',radii(k)); 60 | [sk,nk] = cellfun(@(x,y)svdCov(x,y,tarData,tarSeed),tarIdx,idx,'uni',false); 61 | s = [s cell2mat(sk)]; 62 | n = [n cell2mat(nk)]; 63 | end 64 | s = s'; 65 | ds = diff(s); 66 | tarDesp = reshape(ds,3*(K-1),[]); 67 | n = mat2cell(n,3*ones(N,1),K); 68 | tarNorm = cellfun(@(x)reshape(x,[],1),n','uni',false); 69 | tarNorm = cell2mat(tarNorm); 70 | 71 | [srcIdx,dist] = flann_search(srcDesp,tarDesp,1,params); % match with descriptors 72 | 73 | %% aggregating each pair of correspondence for finding the best match 74 | M = size(srcSeed,2); 75 | N = size(tarSeed,2); 76 | seedIdx = srcIdx; 77 | Err = inf(N,1); 78 | tform = cell(1,N); 79 | ovNum = ceil(overlap*N); 80 | distThr = 0.2/4*length(radii); 81 | thetaThr = 10; 82 | threshold = gridStep*gridStep; 83 | for n = 1:N 84 | seed = srcSeed(:,seedIdx(n)); 85 | seedNorm = srcNorm(:,seedIdx(n)); 86 | 87 | % source point cloud 88 | d = bsxfun(@minus,srcSeed,seed); 89 | d = sqrt(sum(d.^2,1)); % distance 90 | inProd = bsxfun(@times,srcNorm,seedNorm); 91 | inProd = inProd(1:3:end,:) + inProd(2:3:end,:) + inProd(3:3:end,:); 92 | theta = real(acosd(inProd)); % inner product 93 | 94 | % target point cloud 95 | r = bsxfun(@minus,tarSeed,tarSeed(:,n)); 96 | r = sqrt(sum(r.^2,1)); % distance 97 | inProd = bsxfun(@times,tarNorm,tarNorm(:,n)); 98 | inProd = inProd(1:3:end,:) + inProd(2:3:end,:) + inProd(3:3:end,:); 99 | alpha = real(acosd(inProd)); % inner product 100 | 101 | IDX = rangesearch(r',d',gridStep/2,'distance','cityblock'); 102 | 103 | matches = [seedIdx(n) n]; 104 | for m = [1:seedIdx(n)-1 seedIdx(n)+1:M] 105 | idx = IDX{m};%find(abs(r-d(m))10) 126 | match_srcSeed = srcSeed(:,matches(:,1)); 127 | match_tarSeed = tarSeed(:,matches(:,2)); 128 | CS = ransac(double(match_srcSeed),double(match_tarSeed),threshold); 129 | 130 | if(sum(CS)<3) 131 | continue; 132 | end 133 | 134 | match_srcSeed = match_srcSeed(:,CS); 135 | match_tarSeed = match_tarSeed(:,CS); 136 | [T, Eps] = estimateRigidTransform(match_tarSeed, match_srcSeed); 137 | tarEst = T*[srcSeed;ones(1,M)]; 138 | tarEst = tarEst(1:3,:); 139 | tform{n} = T; 140 | 141 | [index,dist] = flann_search(tarEst,tarSeed,1,params); 142 | [dist,ind] = sort(dist); 143 | Err(n) = sum(sum((tarEst(:,index(ind(1:ovNum)))-tarSeed(:,ind(1:ovNum))).^2)); 144 | end 145 | end 146 | [v,idx] = min(Err); 147 | T = tform{idx}; 148 | tarEst = T*[srcData;ones(1,length(srcData))]; 149 | tarEst = tarEst(1:3,:); 150 | 151 | %% trimmed-icp part 152 | %-------------------------------------------------------------------------- 153 | [index,dist] = flann_search(tarEst,tarData,1,params); 154 | [dist,ind] = sort(dist); 155 | ovNum = ceil(overlap*length(dist)); 156 | tmp = mean(sum((tarEst(:,index(ind(1:ovNum)))-tarData(:,ind(1:ovNum))).^2)); 157 | rmsE(1) = sqrt(tmp); 158 | 159 | match_srcData = srcData(:,index(ind(1:ovNum))); 160 | match_tarData = tarData(:,ind(1:ovNum)); 161 | 162 | maxIter = 100; dE = inf; iter = 1; 163 | errThr = 1e-4; rmseThr = 0.001; 164 | while(dE>errThr && iterrmseThr) 165 | [T, Eps] = estimateRigidTransform(match_tarData, match_srcData); 166 | tarEst = T*[srcData;ones(1,length(srcData))]; 167 | tarEst = tarEst(1:3,:); 168 | 169 | iter = iter + 1; 170 | 171 | [index,dist] = flann_search(tarEst,tarData,1,params); 172 | [dist,ind] = sort(dist); 173 | tmp = mean(sum((tarEst(:,index(ind(1:ovNum)))-tarData(:,ind(1:ovNum))).^2)); 174 | rmsE(iter) = sqrt(tmp); 175 | 176 | match_srcData = srcData(:,index(ind(1:ovNum))); 177 | match_tarData = tarData(:,ind(1:ovNum)); 178 | dE = rmsE(iter - 1) - rmsE(iter); 179 | end 180 | %-------------------------------------------------------------------------- 181 | 182 | src2tarEst = tarEst; 183 | -------------------------------------------------------------------------------- /estimateRigidTransform/crossTimesMatrix.m: -------------------------------------------------------------------------------- 1 | function V_times = crossTimesMatrix(V) 2 | % CROSSTIMESMATRIX 3 | % V_TIMES = CROSSTIMESMATRIX(V) returns a 3x3 (or a series of 3x3) cross times matrices of input vector(s) V 4 | % 5 | % Input: 6 | % V a 3xN matrix, rpresenting a series of 3x1 vectors 7 | % 8 | % Output: 9 | % V_TIMES (Vx) a series of 3x3 matrices where V_times(:,:,i) is the Vx matrix for the vector V(:,i) 10 | % 11 | % Babak Taati, 2003 12 | % (revised 2009) 13 | 14 | [a,b] = size(V); 15 | V_times = zeros(a, 3, b); 16 | 17 | % V_times(1,1,:) = 0; 18 | V_times(1,2,:) = - V(3,:); 19 | V_times(1,3,:) = V(2,:); 20 | 21 | V_times(2,1,:) = V(3,:); 22 | % V_times(2,2,:) = 0; 23 | V_times(2,3,:) = - V(1,:); 24 | 25 | V_times(3,1,:) = - V(2,:); 26 | V_times(3,2,:) = V(1,:); 27 | % V_times(3,3,:) = 0; 28 | -------------------------------------------------------------------------------- /estimateRigidTransform/demoEstimateRigidTransform.m: -------------------------------------------------------------------------------- 1 | % a short script to desmonstrate estimateRigidTransform 2 | % Babak Taati 3 | % 2010 4 | 5 | %% load sample data 6 | load sampleData; % loads two 3-D points clouds (P1h, and P2h, in homogeneous coordinates) and the ground truth alignment between them (T, 4x4) 7 | 8 | alignedP1h = T * P1h; 9 | 10 | subplot(1,3,1); 11 | cameratoolbar 12 | plot3(alignedP1h(1,:), alignedP1h(2,:), alignedP1h(3,:), 'b.'); 13 | hold on 14 | plot3(P2h(1,:), P2h(2,:), P2h(3,:), 'ro'); 15 | title('original points, ground truth alignment'); 16 | 17 | %% add a bit of noise to the second point cloud and estimate the alignment 18 | 19 | noisyP1h = P1h + randn(size(P1h))/20; % add some zero mean Gaussian noise 20 | noisyP2h = P2h + randn(size(P2h))/20; % add some zero mean Gaussian noise 21 | % since we have lots of point matches, estimating the rigid transformation works even if the data is very noisy. Try replacing /20 with /5 and see! 22 | 23 | noisyP1h(:,4) = 1; % force the last homogenous coordiate to one 24 | noisyP2h(:,4) = 1; % force the last homogenous coordiate to one 25 | 26 | [estT, Eps] = estimateRigidTransform(noisyP2h(1:3,:), noisyP1h(1:3,:)); 27 | 28 | alignedP1h = estT * P1h; 29 | subplot(1,3,2); 30 | plot3(alignedP1h(1,:), alignedP1h(2,:), alignedP1h(3,:), 'b.'); 31 | hold on 32 | plot3(P2h(1,:), P2h(2,:), P2h(3,:), 'ro'); 33 | title('original points, estimated alignment from noisy points'); 34 | 35 | 36 | alignedNoisyP1h = estT * noisyP1h; 37 | subplot(1,3,3); 38 | plot3(alignedNoisyP1h(1,:), alignedNoisyP1h(2,:), alignedNoisyP1h(3,:), 'b.'); 39 | hold on 40 | plot3(noisyP2h(1,:), noisyP2h(2,:), noisyP2h(3,:), 'ro'); 41 | title('noisy points, estimated alignment from noisy points'); 42 | -------------------------------------------------------------------------------- /estimateRigidTransform/estimateRigidTransform.m: -------------------------------------------------------------------------------- 1 | function [T, Eps] = estimateRigidTransform(x, y) 2 | % ESTIMATERIGIDTRANSFORM 3 | % [T, EPS] = ESTIMATERIGIDTRANSFORM(X, Y) estimates the rigid transformation 4 | % that best aligns x with y (in the least-squares sense). 5 | % 6 | % Reference: "Estimating Rigid Transformations" in 7 | % "Computer Vision, a modern approach" by Forsyth and Ponce (1993), page 480 8 | % (page 717(?) of the newer edition) 9 | % 10 | % Input: 11 | % X: 3xN, N 3-D points (N>=3) 12 | % Y: 3xN, N 3-D points (N>=3) 13 | % 14 | % Output 15 | % T: the rigid transformation that aligns x and y as: xh = T * yh 16 | % (h denotes homogenous coordinates) 17 | % (corrspondence between points x(:,i) and y(:,i) is assumed) 18 | % 19 | % EPS: the smallest singular value. The closer this value it is 20 | % to 0, the better the estimate is. (large values mean that the 21 | % transform between the two data sets cannot be approximated 22 | % well with a rigid transform. 23 | % 24 | % Babak Taati, 2003 25 | % (revised 2009) 26 | 27 | if nargin ~= 2 28 | error('Requires two input arguments.') 29 | end 30 | 31 | if size(x,1)~=3 || size(y,1)~=3 32 | error('Input point clouds must be a 3xN matrix.'); 33 | end 34 | 35 | if size(x, 2) ~= size(y,2) 36 | error('Input point clouds must be of the same size'); 37 | end 38 | 39 | if size(x,2)<3 || size(y,2)<3 40 | error('At least 3 point matches are needed'); 41 | end 42 | 43 | pointCount = length(x); % since x has N=3+ points, length shows the number of points 44 | 45 | x_centroid = sum(x,2) / pointCount; 46 | y_centroid = sum(y,2) / pointCount; 47 | 48 | x_centrized = [x(1,:)-x_centroid(1) ; x(2,:)-x_centroid(2); x(3,:)-x_centroid(3)]; 49 | y_centrized = [y(1,:)-y_centroid(1) ; y(2,:)-y_centroid(2); y(3,:)-y_centroid(3)]; 50 | 51 | R12 = y_centrized' - x_centrized'; 52 | R21 = x_centrized - y_centrized; 53 | R22_1 = y_centrized + x_centrized; 54 | R22 = crossTimesMatrix(R22_1(1:3,:)); 55 | 56 | B = zeros(4, 4); 57 | A = zeros(4, 4, pointCount); 58 | for ii=1:pointCount 59 | A(1:4,1:4,ii) = [0, R12(ii,1:3); R21(1:3,ii), R22(1:3,1:3,ii)]; 60 | B = B + A(:,:,ii)' * A(:,:,ii); 61 | end 62 | 63 | [~, S, V] = svd(B); 64 | quat = V(:,4); 65 | rot = quat2rot(quat); 66 | 67 | T1 = [eye(3,3), -y_centroid ; 0 0 0 1]; 68 | T2 = [rot, [0; 0; 0]; 0 0 0 1]; 69 | T3 = [eye(3,3), x_centroid ; 0 0 0 1]; 70 | 71 | T = T3 * T2 * T1; 72 | Eps = S(4,4); 73 | 74 | 75 | 76 | -------------------------------------------------------------------------------- /estimateRigidTransform/quat2rot.m: -------------------------------------------------------------------------------- 1 | function R = quat2rot(Q) 2 | % QUAT2ROT 3 | % R = QUAT2ROT(Q) converts a quaternion (4x1 or 1x4) into a 3x3 rotation mattrix 4 | % 5 | % reference: google! 6 | % 7 | % Babak Taati, 2003 8 | % (revised 2009) 9 | 10 | q0 = Q(1); 11 | q1 = Q(2); 12 | q2 = Q(3); 13 | q3 = Q(4); 14 | %% SECTION TITLE 15 | % DESCRIPTIVE TEXT 16 | 17 | R(1,1) = q0*q0 + q1*q1 - q2*q2 - q3*q3; 18 | R(1,2) = 2 * (q1*q2 - q0*q3); 19 | R(1,3) = 2 * (q1*q3 + q0*q2); 20 | 21 | R(2,1) = 2 * (q1*q2 + q0*q3); 22 | R(2,2) = q0*q0 - q1*q1 + q2*q2 - q3*q3; 23 | R(2,3) = 2 * (q2*q3 - q0*q1); 24 | 25 | R(3,1) = 2 * (q1*q3 - q0*q2); 26 | R(3,2) = 2 * (q2*q3 + q0*q1); 27 | R(3,3) = q0*q0 - q1*q1 - q2*q2 + q3*q3; 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /estimateRigidTransform/sampleData.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/EnyaHermite/fastDesp-corrProp/7da2d6af077afa845c906446ce663cb6fc4503a9/estimateRigidTransform/sampleData.mat -------------------------------------------------------------------------------- /flann/flann_build_index.m: -------------------------------------------------------------------------------- 1 | function [index, params, speedup] = flann_build_index(dataset, build_params) 2 | %FLANN_BUILD_INDEX Builds an index for fast approximate nearest neighbors search 3 | % 4 | % [index, params, speedup] = flann_build_index(dataset, build_params) - Constructs the 5 | % index from the provided 'dataset' and (optionally) computes the optimal parameters. 6 | 7 | % Marius Muja, January 2008 8 | 9 | algos = struct( 'linear', 0, 'kdtree', 1, 'kmeans', 2, 'composite', 3, 'kdtree_single', 4, 'hierarchical', 5, 'lsh', 6, 'saved', 254, 'autotuned', 255 ); 10 | center_algos = struct('random', 0, 'gonzales', 1, 'kmeanspp', 2 ); 11 | log_levels = struct('none', 0, 'fatal', 1, 'error', 2, 'warning', 3, 'info', 4); 12 | 13 | default_params = struct('algorithm', 'kdtree' ,'checks', 32, 'eps', 0.0, 'sorted', 1, 'max_neighbors', -1, 'cores', 1, 'trees', 4, 'branching', 32, 'iterations', 5, 'centers_init', 'random', 'cb_index', 0.4, 'target_precision', 0.9,'build_weight', 0.01, 'memory_weight', 0, 'sample_fraction', 0.1, 'table_number', 12, 'key_size', 20, 'multi_probe_level', 2, 'log_level', 'warning', 'random_seed', 0); 14 | 15 | if ~isstruct(build_params) 16 | error('The "build_params" argument must be a structure'); 17 | end 18 | 19 | params = default_params; 20 | fn = fieldnames(build_params); 21 | for i = [1:length(fn)], 22 | name = cell2mat(fn(i)); 23 | params.(name) = build_params.(name); 24 | end 25 | if ~isnumeric(params.algorithm), 26 | params.algorithm = value2id(algos,params.algorithm); 27 | end 28 | if ~isnumeric(params.centers_init), 29 | params.centers_init = value2id(center_algos,params.centers_init); 30 | end 31 | if ~isnumeric(params.log_level), 32 | params.log_level = value2id(log_levels,params.log_level); 33 | end 34 | 35 | [index, params, speedup] = nearest_neighbors('build_index',dataset, params); 36 | 37 | if isnumeric(params.algorithm), 38 | params.algorithm = id2value(algos,params.algorithm); 39 | end 40 | if isnumeric(params.centers_init), 41 | params.centers_init = id2value(center_algos,params.centers_init); 42 | end 43 | end 44 | 45 | function value = id2value(map, id) 46 | fields = fieldnames(map); 47 | for i = 1:length(fields), 48 | val = cell2mat(fields(i)); 49 | if map.(val) == id 50 | value = val; 51 | break; 52 | end 53 | end 54 | end 55 | function id = value2id(map,value) 56 | id = map.(value); 57 | end 58 | -------------------------------------------------------------------------------- /flann/flann_free_index.m: -------------------------------------------------------------------------------- 1 | %Copyright 2008-2009 Marius Muja (mariusm@cs.ubc.ca). All rights reserved. 2 | %Copyright 2008-2009 David G. Lowe (lowe@cs.ubc.ca). All rights reserved. 3 | % 4 | %THE BSD LICENSE 5 | % 6 | %Redistribution and use in source and binary forms, with or without 7 | %modification, are permitted provided that the following conditions 8 | %are met: 9 | % 10 | %1. Redistributions of source code must retain the above copyright 11 | % notice, this list of conditions and the following disclaimer. 12 | %2. Redistributions in binary form must reproduce the above copyright 13 | % notice, this list of conditions and the following disclaimer in the 14 | % documentation and/or other materials provided with the distribution. 15 | % 16 | %THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR 17 | %IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES 18 | %OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. 19 | %IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, 20 | %INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT 21 | %NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 22 | %DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 23 | %THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 24 | %(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 25 | %THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | function flann_free_index(index_id) 28 | %FLANN_FREE_INDEX Deletes the nearest-neighbors index 29 | % 30 | % Deletes an index constructed using flann_build_index. 31 | 32 | % Marius Muja, January 2008 33 | 34 | nearest_neighbors('free_index',index_id); 35 | end -------------------------------------------------------------------------------- /flann/flann_load_index.m: -------------------------------------------------------------------------------- 1 | %Copyright 2008-2009 Marius Muja (mariusm@cs.ubc.ca). All rights reserved. 2 | %Copyright 2008-2009 David G. Lowe (lowe@cs.ubc.ca). All rights reserved. 3 | % 4 | %THE BSD LICENSE 5 | % 6 | %Redistribution and use in source and binary forms, with or without 7 | %modification, are permitted provided that the following conditions 8 | %are met: 9 | % 10 | %1. Redistributions of source code must retain the above copyright 11 | % notice, this list of conditions and the following disclaimer. 12 | %2. Redistributions in binary form must reproduce the above copyright 13 | % notice, this list of conditions and the following disclaimer in the 14 | % documentation and/or other materials provided with the distribution. 15 | % 16 | %THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR 17 | %IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES 18 | %OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. 19 | %IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, 20 | %INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT 21 | %NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 22 | %DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 23 | %THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 24 | %(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 25 | %THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | function index = flann_load_index(filename, dataset) 28 | %FLANN_LOAD_INDEX Loads an index from disk 29 | % 30 | % Marius Muja, March 2009 31 | 32 | index = nearest_neighbors('load_index', filename, dataset); 33 | end -------------------------------------------------------------------------------- /flann/flann_manual-1.8.4.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/EnyaHermite/fastDesp-corrProp/7da2d6af077afa845c906446ce663cb6fc4503a9/flann/flann_manual-1.8.4.pdf -------------------------------------------------------------------------------- /flann/flann_save_index.m: -------------------------------------------------------------------------------- 1 | %Copyright 2008-2009 Marius Muja (mariusm@cs.ubc.ca). All rights reserved. 2 | %Copyright 2008-2009 David G. Lowe (lowe@cs.ubc.ca). All rights reserved. 3 | % 4 | %THE BSD LICENSE 5 | % 6 | %Redistribution and use in source and binary forms, with or without 7 | %modification, are permitted provided that the following conditions 8 | %are met: 9 | % 10 | %1. Redistributions of source code must retain the above copyright 11 | % notice, this list of conditions and the following disclaimer. 12 | %2. Redistributions in binary form must reproduce the above copyright 13 | % notice, this list of conditions and the following disclaimer in the 14 | % documentation and/or other materials provided with the distribution. 15 | % 16 | %THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR 17 | %IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES 18 | %OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. 19 | %IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, 20 | %INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT 21 | %NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 22 | %DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 23 | %THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 24 | %(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 25 | %THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | function flann_save_index(index_id, filename) 28 | %FLANN_SAVE_INDEX Saves an index to disk 29 | % 30 | 31 | % Marius Muja, March 2010 32 | 33 | nearest_neighbors('save_index',index_id, filename); 34 | end -------------------------------------------------------------------------------- /flann/flann_search.m: -------------------------------------------------------------------------------- 1 | %Copyright 2008-2009 Marius Muja (mariusm@cs.ubc.ca). All rights reserved. 2 | %Copyright 2008-2009 David G. Lowe (lowe@cs.ubc.ca). All rights reserved. 3 | % 4 | %THE BSD LICENSE 5 | % 6 | %Redistribution and use in source and binary forms, with or without 7 | %modification, are permitted provided that the following conditions 8 | %are met: 9 | % 10 | %1. Redistributions of source code must retain the above copyright 11 | % notice, this list of conditions and the following disclaimer. 12 | %2. Redistributions in binary form must reproduce the above copyright 13 | % notice, this list of conditions and the following disclaimer in the 14 | % documentation and/or other materials provided with the distribution. 15 | % 16 | %THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR 17 | %IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES 18 | %OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. 19 | %IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, 20 | %INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT 21 | %NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 22 | %DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 23 | %THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 24 | %(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 25 | %THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | function [indices, dists] = flann_search(data, testset, n, search_params) 28 | %NN_SEARCH Fast approximate nearest neighbors search 29 | % 30 | % Performs a fast approximate nearest neighbor search using an 31 | % index constructed using flann_build_index or directly a 32 | % dataset. 33 | 34 | % Marius Muja, January 2008 35 | 36 | 37 | algos = struct( 'linear', 0, 'kdtree', 1, 'kmeans', 2, 'composite', 3, 'lsh', 6, 'saved', 254, 'autotuned', 255 ); 38 | center_algos = struct('random', 0, 'gonzales', 1, 'kmeanspp', 2 ); 39 | log_levels = struct('none', 0, 'fatal', 1, 'error', 2, 'warning', 3, 'info', 4); 40 | 41 | default_params = struct('algorithm', 'kdtree' ,'checks', 32, 'eps', 0.0, 'sorted', 1, 'max_neighbors', -1, 'cores', 1, 'trees', 4, 'branching', 32, 'iterations', 5, 'centers_init', 'random', 'cb_index', 0.4, 'target_precision', 0.9,'build_weight', 0.01, 'memory_weight', 0, 'sample_fraction', 0.1, 'table_number', 12, 'key_size', 20, 'multi_probe_level', 2, 'log_level', 'warning', 'random_seed', 0); 42 | 43 | if ~isstruct(search_params) 44 | error('The "search_params" argument must be a structure'); 45 | end 46 | 47 | params = default_params; 48 | fn = fieldnames(search_params); 49 | for i = [1:length(fn)], 50 | name = cell2mat(fn(i)); 51 | params.(name) = search_params.(name); 52 | end 53 | if ~isnumeric(params.algorithm), 54 | params.algorithm = value2id(algos,params.algorithm); 55 | end 56 | if ~isnumeric(params.centers_init), 57 | params.centers_init = value2id(center_algos,params.centers_init); 58 | end 59 | if ~isnumeric(params.log_level), 60 | params.log_level = value2id(log_levels,params.log_level); 61 | end 62 | 63 | if (size(data,1)==1 && size(data,2)==1) 64 | % we already have an index 65 | [indices,dists] = nearest_neighbors('index_find_nn', data, testset, n, params); 66 | else 67 | % create the index and search 68 | [indices,dists] = nearest_neighbors('find_nn', data, testset, n, params); 69 | end 70 | end 71 | 72 | function value = id2value(map, id) 73 | fields = fieldnames(map); 74 | for i = 1:length(fields), 75 | val = cell2mat(fields(i)); 76 | if map.(val) == id 77 | value = val; 78 | break; 79 | end 80 | end 81 | end 82 | function id = value2id(map,value) 83 | id = map.(value); 84 | end 85 | -------------------------------------------------------------------------------- /flann/flann_set_distance_type.m: -------------------------------------------------------------------------------- 1 | %Copyright 2008-2009 Marius Muja (mariusm@cs.ubc.ca). All rights reserved. 2 | %Copyright 2008-2009 David G. Lowe (lowe@cs.ubc.ca). All rights reserved. 3 | % 4 | %THE BSD LICENSE 5 | % 6 | %Redistribution and use in source and binary forms, with or without 7 | %modification, are permitted provided that the following conditions 8 | %are met: 9 | % 10 | %1. Redistributions of source code must retain the above copyright 11 | % notice, this list of conditions and the following disclaimer. 12 | %2. Redistributions in binary form must reproduce the above copyright 13 | % notice, this list of conditions and the following disclaimer in the 14 | % documentation and/or other materials provided with the distribution. 15 | % 16 | %THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR 17 | %IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES 18 | %OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. 19 | %IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, 20 | %INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT 21 | %NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 22 | %DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 23 | %THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 24 | %(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 25 | %THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | function flann_set_distance_type(type, order) 28 | %FLANN_LOAD_INDEX Loads an index from disk 29 | % 30 | % Marius Muja, March 2009 31 | 32 | distances = struct('euclidean', 1, 'manhattan', 2, 'minkowski', 3, 'max_dist', 4, 'hik', 5, 'hellinger', 6, 'chi_square', 7, 'cs', 7, 'kullback_leibler', 8, 'kl', 8); 33 | 34 | if ~isnumeric(type), 35 | type = value2id(distances,type); 36 | end 37 | if type~=3 38 | order = 0; 39 | end 40 | nearest_neighbors('set_distance_type', type, order); 41 | end 42 | 43 | function id = value2id(map,value) 44 | id = map.(value); 45 | end 46 | -------------------------------------------------------------------------------- /flann/nearest_neighbors.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2008-2009 Marius Muja (mariusm@cs.ubc.ca). All rights reserved. 3 | Copyright 2008-2009 David G. Lowe (lowe@cs.ubc.ca). All rights reserved. 4 | 5 | THE BSD LICENSE 6 | 7 | Redistribution and use in source and binary forms, with or without 8 | modification, are permitted provided that the following conditions 9 | are met: 10 | 11 | 1. Redistributions of source code must retain the above copyright 12 | notice, this list of conditions and the following disclaimer. 13 | 2. Redistributions in binary form must reproduce the above copyright 14 | notice, this list of conditions and the following disclaimer in the 15 | documentation and/or other materials provided with the distribution. 16 | 17 | THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR 18 | IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES 19 | OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. 20 | IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, 21 | INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT 22 | NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 23 | DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 24 | THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 26 | THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | */ 28 | 29 | /* Workaround for MSVC 10, Matlab incompatibility */ 30 | #if (_MSC_VER >= 1600) 31 | #include 32 | #define __STDC_UTF_16__ 33 | #endif 34 | #include 35 | #include 36 | #include 37 | #include 38 | 39 | 40 | 41 | struct TypedIndex 42 | { 43 | flann_index_t index; 44 | flann_datatype_t type; 45 | void* dataset; 46 | }; 47 | 48 | 49 | template 50 | static mxArray* to_mx_array(T value) 51 | { 52 | mxArray* mat = mxCreateDoubleMatrix(1,1,mxREAL); 53 | double* ptr = mxGetPr(mat); 54 | *ptr = value; 55 | 56 | return mat; 57 | } 58 | 59 | 60 | static void matlabStructToFlannStruct( const mxArray* mexParams, FLANNParameters& flannParams ) 61 | { 62 | flannParams.algorithm = (flann_algorithm_t)(int)*(mxGetPr(mxGetField(mexParams, 0,"algorithm"))); 63 | 64 | // kdtree 65 | flannParams.trees = (int)*(mxGetPr(mxGetField(mexParams, 0,"trees"))); 66 | 67 | // kmeans 68 | flannParams.branching = (int)*(mxGetPr(mxGetField(mexParams, 0,"branching"))); 69 | flannParams.iterations = (int)*(mxGetPr(mxGetField(mexParams, 0,"iterations"))); 70 | flannParams.centers_init = (flann_centers_init_t)(int)*(mxGetPr(mxGetField(mexParams, 0,"centers_init"))); 71 | flannParams.cb_index = (float)*(mxGetPr(mxGetField(mexParams, 0,"cb_index"))); 72 | 73 | // autotuned 74 | flannParams.target_precision = (float)*(mxGetPr(mxGetField(mexParams, 0,"target_precision"))); 75 | flannParams.build_weight = (float)*(mxGetPr(mxGetField(mexParams, 0,"build_weight"))); 76 | flannParams.memory_weight = (float)*(mxGetPr(mxGetField(mexParams, 0,"memory_weight"))); 77 | flannParams.sample_fraction = (float)*(mxGetPr(mxGetField(mexParams, 0,"sample_fraction"))); 78 | 79 | // misc 80 | flannParams.log_level = (flann_log_level_t)(int)*(mxGetPr(mxGetField(mexParams, 0,"log_level"))); 81 | flannParams.random_seed = (int)*(mxGetPr(mxGetField(mexParams, 0,"random_seed"))); 82 | 83 | // search 84 | flannParams.checks = (int)*(mxGetPr(mxGetField(mexParams, 0,"checks"))); 85 | flannParams.eps = (float)*(mxGetPr(mxGetField(mexParams, 0,"eps"))); 86 | flannParams.sorted = (int)*(mxGetPr(mxGetField(mexParams, 0,"sorted"))); 87 | flannParams.max_neighbors = (int)*(mxGetPr(mxGetField(mexParams, 0,"max_neighbors"))); 88 | flannParams.cores = (int)*(mxGetPr(mxGetField(mexParams, 0,"cores"))); 89 | 90 | // lsh 91 | flannParams.table_number_ = (unsigned int)*(mxGetPr(mxGetField(mexParams, 0, "table_number"))); 92 | flannParams.key_size_ = (unsigned int)*(mxGetPr(mxGetField(mexParams, 0, "key_size"))); 93 | flannParams.multi_probe_level_ = (unsigned int)*(mxGetPr(mxGetField(mexParams, 0, "multi_probe_level"))); 94 | } 95 | 96 | static mxArray* flannStructToMatlabStruct( const FLANNParameters& flannParams ) 97 | { 98 | const char* fieldnames[] = {"algorithm", "checks", "eps", "sorted", "max_neighbors", "cores", "trees", "leaf_max_size", "branching", "iterations", "centers_init", "cb_index", "table_number", "key_size", "multi_probe_level"}; 99 | mxArray* mexParams = mxCreateStructMatrix(1, 1, sizeof(fieldnames)/sizeof(const char*), fieldnames); 100 | 101 | mxSetField(mexParams, 0, "algorithm", to_mx_array(flannParams.algorithm)); 102 | mxSetField(mexParams, 0, "checks", to_mx_array(flannParams.checks)); 103 | mxSetField(mexParams, 0, "eps", to_mx_array(flannParams.eps)); 104 | mxSetField(mexParams, 0, "sorted", to_mx_array(flannParams.sorted)); 105 | mxSetField(mexParams, 0, "max_neighbors", to_mx_array(flannParams.max_neighbors)); 106 | mxSetField(mexParams, 0, "cores", to_mx_array(flannParams.cores)); 107 | 108 | mxSetField(mexParams, 0, "trees", to_mx_array(flannParams.trees)); 109 | mxSetField(mexParams, 0, "leaf_max_size", to_mx_array(flannParams.trees)); 110 | 111 | mxSetField(mexParams, 0, "branching", to_mx_array(flannParams.branching)); 112 | mxSetField(mexParams, 0, "iterations", to_mx_array(flannParams.iterations)); 113 | mxSetField(mexParams, 0, "centers_init", to_mx_array(flannParams.centers_init)); 114 | mxSetField(mexParams, 0, "cb_index", to_mx_array(flannParams.cb_index)); 115 | 116 | mxSetField(mexParams, 0, "table_number", to_mx_array(flannParams.table_number_)); 117 | mxSetField(mexParams, 0, "key_size", to_mx_array(flannParams.key_size_)); 118 | mxSetField(mexParams, 0, "multi_probe_level", to_mx_array(flannParams.multi_probe_level_)); 119 | 120 | return mexParams; 121 | } 122 | 123 | 124 | static void check_allowed_type(const mxArray* datasetMat) 125 | { 126 | if (!mxIsSingle(datasetMat) && 127 | !mxIsDouble(datasetMat) && 128 | !mxIsUint8(datasetMat) && 129 | !mxIsInt32(datasetMat)) { 130 | mexErrMsgTxt("Data type must be floating point single precision, floating point double precision, " 131 | "8 bit unsigned integer or 32 bit signed integer"); 132 | } 133 | } 134 | 135 | 136 | /** 137 | * Input arguments: dataset (matrix), testset (matrix), n (int), params (struct) 138 | * Output arguments: indices(matrix), dists(matrix) 139 | */ 140 | static void _find_nn(int nOutArray, mxArray* OutArray[], int nInArray, const mxArray* InArray[]) 141 | { 142 | /* Check the number of input arguments */ 143 | if(nInArray != 4) { 144 | mexErrMsgTxt("Incorrect number of input arguments, expecting:\n" 145 | "dataset, testset, nearest_neighbors, params"); 146 | } 147 | 148 | /* Check the number of output arguments */ 149 | if(nOutArray > 2) { 150 | mexErrMsgTxt("One or two outputs required."); 151 | } 152 | const mxArray* datasetMat = InArray[0]; 153 | const mxArray* testsetMat = InArray[1]; 154 | check_allowed_type(datasetMat); 155 | check_allowed_type(testsetMat); 156 | 157 | int dcount = mxGetN(datasetMat); 158 | int length = mxGetM(datasetMat); 159 | int tcount = mxGetN(testsetMat); 160 | 161 | if (mxGetM(testsetMat) != length) { 162 | mexErrMsgTxt("Dataset and testset features should have the same size."); 163 | } 164 | 165 | const mxArray* nnMat = InArray[2]; 166 | 167 | if ((mxGetM(nnMat)!=1)||(mxGetN(nnMat)!=1)|| !mxIsNumeric(nnMat)) { 168 | mexErrMsgTxt("Number of nearest neighbors should be a scalar."); 169 | } 170 | int nn = (int)(*mxGetPr(nnMat)); 171 | 172 | const mxArray* pStruct = InArray[3]; 173 | 174 | if (!mxIsStruct(pStruct)) { 175 | mexErrMsgTxt("Params must be a struct object."); 176 | } 177 | 178 | FLANNParameters p; 179 | matlabStructToFlannStruct(pStruct, p); 180 | 181 | int* result = (int*)malloc(tcount*nn*sizeof(int)); 182 | float* dists = NULL; 183 | double* ddists = NULL; 184 | 185 | /* do the search */ 186 | if (mxIsSingle(datasetMat)) { 187 | float* dataset = (float*) mxGetData(datasetMat); 188 | float* testset = (float*) mxGetData(testsetMat); 189 | dists = (float*)malloc(tcount*nn*sizeof(float)); 190 | flann_find_nearest_neighbors_float(dataset,dcount,length,testset, tcount, result, dists, nn, &p); 191 | } 192 | else if (mxIsDouble(datasetMat)) { 193 | double* dataset = (double*) mxGetData(datasetMat); 194 | double* testset = (double*) mxGetData(testsetMat); 195 | ddists = (double*)malloc(tcount*nn*sizeof(double)); 196 | flann_find_nearest_neighbors_double(dataset,dcount,length,testset, tcount, result, ddists, nn, &p); 197 | } 198 | else if (mxIsUint8(datasetMat)) { 199 | unsigned char* dataset = (unsigned char*) mxGetData(datasetMat); 200 | unsigned char* testset = (unsigned char*) mxGetData(testsetMat); 201 | dists = (float*)malloc(tcount*nn*sizeof(float)); 202 | flann_find_nearest_neighbors_byte(dataset,dcount,length,testset, tcount, result, dists, nn, &p); 203 | } 204 | else if (mxIsInt32(datasetMat)) { 205 | int* dataset = (int*) mxGetData(datasetMat); 206 | int* testset = (int*) mxGetData(testsetMat); 207 | dists = (float*)malloc(tcount*nn*sizeof(float)); 208 | flann_find_nearest_neighbors_int(dataset,dcount,length,testset, tcount, result, dists, nn, &p); 209 | } 210 | 211 | /* Allocate memory for Output Matrix */ 212 | OutArray[0] = mxCreateDoubleMatrix(nn, tcount, mxREAL); 213 | 214 | /* Get pointer to Output matrix and store result */ 215 | double* pOut = mxGetPr(OutArray[0]); 216 | for (int i=0; i 1) { 222 | /* Allocate memory for Output Matrix */ 223 | OutArray[1] = mxCreateDoubleMatrix(nn, tcount, mxREAL); 224 | 225 | /* Get pointer to Output matrix and store result*/ 226 | double* pDists = mxGetPr(OutArray[1]); 227 | if (dists!=NULL) { 228 | for (int i=0; i 2) { 254 | mexErrMsgTxt("One or two outputs required."); 255 | } 256 | 257 | const mxArray* indexMat = InArray[0]; 258 | TypedIndex* typedIndex = *(TypedIndex**)mxGetData(indexMat); 259 | 260 | const mxArray* testsetMat = InArray[1]; 261 | check_allowed_type(testsetMat); 262 | 263 | int tcount = mxGetN(testsetMat); 264 | 265 | const mxArray* nnMat = InArray[2]; 266 | 267 | if ((mxGetM(nnMat)!=1)||(mxGetN(nnMat)!=1)) { 268 | mexErrMsgTxt("Number of nearest neighbors should be a scalar."); 269 | } 270 | int nn = (int)(*mxGetPr(nnMat)); 271 | 272 | int* result = (int*)malloc(tcount*nn*sizeof(int)); 273 | float* dists = NULL; 274 | double* ddists = NULL; 275 | 276 | const mxArray* pStruct = InArray[3]; 277 | 278 | FLANNParameters p; 279 | matlabStructToFlannStruct(pStruct, p); 280 | 281 | if (mxIsSingle(testsetMat)) { 282 | if (typedIndex->type != FLANN_FLOAT32) { 283 | mexErrMsgTxt("Index type must match testset type"); 284 | } 285 | float* testset = (float*) mxGetData(testsetMat); 286 | dists = (float*)malloc(tcount*nn*sizeof(float)); 287 | flann_find_nearest_neighbors_index_float(typedIndex->index,testset, tcount, result, dists, nn, &p); 288 | } 289 | else if (mxIsDouble(testsetMat)) { 290 | if (typedIndex->type != FLANN_FLOAT64) { 291 | mexErrMsgTxt("Index type must match testset type"); 292 | } 293 | double* testset = (double*) mxGetData(testsetMat); 294 | ddists = (double*)malloc(tcount*nn*sizeof(double)); 295 | flann_find_nearest_neighbors_index_double(typedIndex->index,testset, tcount, result, ddists, nn, &p); 296 | } 297 | else if (mxIsUint8(testsetMat)) { 298 | if (typedIndex->type != FLANN_UINT8) { 299 | mexErrMsgTxt("Index type must match testset type"); 300 | } 301 | unsigned char* testset = (unsigned char*) mxGetData(testsetMat); 302 | dists = (float*)malloc(tcount*nn*sizeof(float)); 303 | flann_find_nearest_neighbors_index_byte(typedIndex->index,testset, tcount, result, dists, nn, &p); 304 | } 305 | else if (mxIsInt32(testsetMat)) { 306 | if (typedIndex->type != FLANN_INT32) { 307 | mexErrMsgTxt("Index type must match testset type"); 308 | } 309 | int* testset = (int*) mxGetData(testsetMat); 310 | dists = (float*)malloc(tcount*nn*sizeof(float)); 311 | flann_find_nearest_neighbors_index_int(typedIndex->index,testset, tcount, result, dists, nn, &p); 312 | } 313 | 314 | /* Allocate memory for Output Matrix */ 315 | OutArray[0] = mxCreateDoubleMatrix(nn, tcount, mxREAL); 316 | 317 | /* Get pointer to Output matrix and store result*/ 318 | double* pOut = mxGetPr(OutArray[0]); 319 | for (int i=0; i 1) { 324 | /* Allocate memory for Output Matrix */ 325 | OutArray[1] = mxCreateDoubleMatrix(nn, tcount, mxREAL); 326 | 327 | /* Get pointer to Output matrix and store result*/ 328 | double* pDists = mxGetPr(OutArray[1]); 329 | if (dists!=NULL) { 330 | for (int i=0; i 346 | T* copy_array(const mxArray* array) 347 | { 348 | size_t mem_size = mxGetN(array)*mxGetM(array)*sizeof(T); 349 | void* data = malloc(mem_size); 350 | memcpy(data,mxGetData(array),mem_size); 351 | return (T*)data; 352 | } 353 | 354 | /** 355 | * Input arguments: dataset (matrix), params (struct) 356 | * Output arguments: index (pointer to index), params (struct), speedup(double) 357 | */ 358 | static void _build_index(int nOutArray, mxArray* OutArray[], int nInArray, const mxArray* InArray[]) 359 | { 360 | /* Check the number of input arguments */ 361 | if(nInArray != 2) { 362 | mexErrMsgTxt("Incorrect number of input arguments"); 363 | } 364 | /* Check the number of output arguments */ 365 | if ((nOutArray == 0)||(nOutArray > 3)) { 366 | mexErrMsgTxt("Incorrect number of outputs."); 367 | } 368 | const mxArray* datasetMat = InArray[0]; 369 | check_allowed_type(datasetMat); 370 | 371 | 372 | 373 | int dcount = mxGetN(datasetMat); 374 | int length = mxGetM(datasetMat); 375 | 376 | 377 | const mxArray* pStruct = InArray[1]; 378 | 379 | /* get index parameters */ 380 | FLANNParameters p; 381 | matlabStructToFlannStruct(pStruct, p); 382 | 383 | float speedup = -1; 384 | 385 | TypedIndex* typedIndex = new TypedIndex(); 386 | 387 | if (mxIsSingle(datasetMat)) { 388 | float* dataset = copy_array(datasetMat); 389 | typedIndex->index = flann_build_index_float(dataset,dcount,length, &speedup, &p); 390 | typedIndex->type = FLANN_FLOAT32; 391 | typedIndex->dataset = dataset; 392 | } 393 | else if (mxIsDouble(datasetMat)) { 394 | double* dataset = copy_array(datasetMat); 395 | typedIndex->index = flann_build_index_double(dataset,dcount,length, &speedup, &p); 396 | typedIndex->type = FLANN_FLOAT64; 397 | typedIndex->dataset = dataset; 398 | } 399 | else if (mxIsUint8(datasetMat)) { 400 | unsigned char* dataset = copy_array(datasetMat); 401 | typedIndex->index = flann_build_index_byte(dataset,dcount,length, &speedup, &p); 402 | typedIndex->type = FLANN_UINT8; 403 | typedIndex->dataset = dataset; 404 | } 405 | else if (mxIsInt32(datasetMat)) { 406 | int* dataset = copy_array(datasetMat); 407 | typedIndex->index = flann_build_index_int(dataset,dcount,length, &speedup, &p); 408 | typedIndex->type = FLANN_INT32; 409 | typedIndex->dataset = dataset; 410 | } 411 | 412 | mxClassID classID; 413 | if (sizeof(flann_index_t)==4) { 414 | classID = mxUINT32_CLASS; 415 | } 416 | else if (sizeof(flann_index_t)==8) { 417 | classID = mxUINT64_CLASS; 418 | } 419 | 420 | /* Allocate memory for Output Matrix */ 421 | OutArray[0] = mxCreateNumericMatrix(1, 1, classID, mxREAL); 422 | 423 | /* Get pointer to Output matrix and store result*/ 424 | TypedIndex** pOut = (TypedIndex**)mxGetData(OutArray[0]); 425 | pOut[0] = typedIndex; 426 | 427 | if (nOutArray > 1) { 428 | OutArray[1] = flannStructToMatlabStruct(p); 429 | } 430 | if (nOutArray > 2) { 431 | OutArray[2] = mxCreateDoubleMatrix(1, 1, mxREAL); 432 | double* pSpeedup = mxGetPr(OutArray[2]); 433 | 434 | *pSpeedup = speedup; 435 | } 436 | } 437 | 438 | /** 439 | * Inputs: index (index pointer) 440 | */ 441 | static void _free_index(int nOutArray, mxArray* OutArray[], int nInArray, const mxArray* InArray[]) 442 | { 443 | /* Check the number of input arguments */ 444 | if(!((nInArray == 1)&&((mxGetN(InArray[0])*mxGetM(InArray[0]))==1))) { 445 | mexErrMsgTxt("Expecting a single scalar argument: the index ID"); 446 | } 447 | TypedIndex* typedIndex = *(TypedIndex**)mxGetData(InArray[0]); 448 | if (typedIndex->type==FLANN_FLOAT32) { 449 | flann_free_index_float(typedIndex->index, NULL); 450 | } 451 | else if (typedIndex->type==FLANN_FLOAT64) { 452 | flann_free_index_double(typedIndex->index, NULL); 453 | } 454 | else if (typedIndex->type==FLANN_UINT8) { 455 | flann_free_index_byte(typedIndex->index, NULL); 456 | } 457 | else if (typedIndex->type==FLANN_INT32) { 458 | flann_free_index_int(typedIndex->index, NULL); 459 | } 460 | free(typedIndex->dataset); 461 | delete typedIndex; 462 | } 463 | 464 | /** 465 | * Inputs: level 466 | */ 467 | static void _set_log_level(int nOutArray, mxArray* OutArray[], int nInArray, const mxArray* InArray[]) 468 | { 469 | if (nInArray != 1) { 470 | mexErrMsgTxt("Incorrect number of input arguments: expecting log_level"); 471 | } 472 | 473 | const mxArray* llMat = InArray[0]; 474 | 475 | if ((mxGetM(llMat)!=1)||(mxGetN(llMat)!=1)|| !mxIsNumeric(llMat)) { 476 | mexErrMsgTxt("Log Level should be a scalar."); 477 | } 478 | int log_level = (int)(*mxGetPr(llMat)); 479 | 480 | flann_log_verbosity(log_level); 481 | 482 | } 483 | 484 | /** 485 | * Inputs: type (flann_distance_t), order(int) 486 | */ 487 | static void _set_distance_type(int nOutArray, mxArray* OutArray[], int nInArray, const mxArray* InArray[]) 488 | { 489 | if( ((nInArray != 1)&&(nInArray != 2))) { 490 | mexErrMsgTxt("Incorrect number of input arguments"); 491 | } 492 | 493 | const mxArray* distMat = InArray[0]; 494 | 495 | if ((mxGetM(distMat)!=1)||(mxGetN(distMat)!=1)|| !mxIsNumeric(distMat)) { 496 | mexErrMsgTxt("Distance type should be a scalar."); 497 | } 498 | int distance_type = (int)(*mxGetPr(distMat)); 499 | 500 | int order = 0; 501 | if (nInArray==2) { 502 | const mxArray* ordMat = InArray[1]; 503 | if ((mxGetM(ordMat)!=1)||(mxGetN(ordMat)!=1)|| !mxIsNumeric(ordMat)) { 504 | mexErrMsgTxt("Distance order should be a scalar."); 505 | } 506 | 507 | order = (int)(*mxGetPr(ordMat)); 508 | } 509 | flann_set_distance_type((flann_distance_t)distance_type, order); 510 | } 511 | 512 | 513 | /** 514 | * Inputs: index (index pointer), filename (string) 515 | */ 516 | static void _save_index(int nOutArray, mxArray* OutArray[], int nInArray, const mxArray* InArray[]) 517 | { 518 | /* Check the number of input arguments */ 519 | if(nInArray != 2) { 520 | mexErrMsgTxt("Incorrect number of input arguments"); 521 | } 522 | 523 | const mxArray* indexMat = InArray[0]; 524 | TypedIndex* typedIndex = *(TypedIndex**)mxGetData(indexMat); 525 | 526 | // get the selector 527 | if(!mxIsChar(InArray[1])) { 528 | mexErrMsgTxt("'filename' should be a string"); 529 | } 530 | char filename[128]; 531 | mxGetString(InArray[1],filename,128); 532 | 533 | if (typedIndex->type==FLANN_FLOAT32) { 534 | flann_save_index_float(typedIndex->index, filename); 535 | } 536 | else if (typedIndex->type==FLANN_FLOAT64) { 537 | flann_save_index_double(typedIndex->index, filename); 538 | } 539 | else if (typedIndex->type==FLANN_UINT8) { 540 | flann_save_index_byte(typedIndex->index, filename); 541 | } 542 | else if (typedIndex->type==FLANN_INT32) { 543 | flann_save_index_int(typedIndex->index, filename); 544 | } 545 | } 546 | 547 | 548 | /** 549 | * Inputs: filename (string), matrix 550 | */ 551 | static void _load_index(int nOutArray, mxArray* OutArray[], int nInArray, const mxArray* InArray[]) 552 | { 553 | if(nInArray != 2) { 554 | mexErrMsgTxt("Incorrect number of input arguments"); 555 | } 556 | // get the selector 557 | if(!mxIsChar(InArray[0])) { 558 | mexErrMsgTxt("'filename' should be a string"); 559 | } 560 | char filename[128]; 561 | mxGetString(InArray[0],filename,128); 562 | 563 | const mxArray* datasetMat = InArray[1]; 564 | check_allowed_type(datasetMat); 565 | 566 | int dcount = mxGetN(datasetMat); 567 | int length = mxGetM(datasetMat); 568 | 569 | TypedIndex* typedIndex = new TypedIndex(); 570 | 571 | if (mxIsSingle(datasetMat)) { 572 | float* dataset = copy_array(datasetMat); 573 | typedIndex->index = flann_load_index_float(filename, dataset,dcount,length); 574 | typedIndex->type = FLANN_FLOAT32; 575 | typedIndex->dataset = dataset; 576 | } 577 | else if (mxIsDouble(datasetMat)) { 578 | double* dataset = copy_array(datasetMat); 579 | typedIndex->index = flann_load_index_double(filename, dataset,dcount,length); 580 | typedIndex->type = FLANN_FLOAT64; 581 | typedIndex->dataset = dataset; 582 | } 583 | else if (mxIsUint8(datasetMat)) { 584 | unsigned char* dataset = copy_array(datasetMat); 585 | typedIndex->index = flann_load_index_byte(filename, dataset,dcount,length); 586 | typedIndex->type = FLANN_UINT8; 587 | typedIndex->dataset = dataset; 588 | } 589 | else if (mxIsInt32(datasetMat)) { 590 | int* dataset = copy_array(datasetMat); 591 | typedIndex->index = flann_load_index_int(filename, dataset,dcount,length); 592 | typedIndex->type = FLANN_INT32; 593 | typedIndex->dataset = dataset; 594 | } 595 | 596 | mxClassID classID; 597 | if (sizeof(flann_index_t)==4) { 598 | classID = mxUINT32_CLASS; 599 | } 600 | else if (sizeof(flann_index_t)==8) { 601 | classID = mxUINT64_CLASS; 602 | } 603 | 604 | /* Allocate memory for Output Matrix */ 605 | OutArray[0] = mxCreateNumericMatrix(1, 1, classID, mxREAL); 606 | 607 | /* Get pointer to Output matrix and store result*/ 608 | TypedIndex** pOut = (TypedIndex**)mxGetData(OutArray[0]); 609 | pOut[0] = typedIndex; 610 | } 611 | 612 | 613 | struct mexFunctionEntry 614 | { 615 | const char* name; 616 | void (* function)(int, mxArray**, int, const mxArray**); 617 | }; 618 | 619 | static mexFunctionEntry __functionTable[] = { 620 | { "find_nn", &_find_nn}, 621 | { "build_index", &_build_index}, 622 | { "index_find_nn", &_index_find_nn}, 623 | { "free_index", &_free_index}, 624 | { "save_index", &_save_index}, 625 | { "load_index", &_load_index}, 626 | { "set_log_level", &_set_log_level}, 627 | { "set_distance_type", &_set_distance_type}, 628 | }; 629 | 630 | 631 | static void print_selector_error() 632 | { 633 | char buf[512]; 634 | char* msg = buf; 635 | 636 | sprintf(msg, "%s", "Expecting first argument to be one of: "); 637 | msg = buf+strlen(buf); 638 | for (int i=0; i0))/n; 94 | assert(precision>0.9); 95 | assert(sum(~(match(1,:)-result(1,:)).*(dists(1,:)-ndists(1,:)))==0); 96 | end 97 | run_test('kd-tree search',@test_kdtree_search); 98 | 99 | function test_kmeans_search 100 | [result, ndists] = flann_search(dataset, testset, 10, struct('algorithm','kmeans',... 101 | 'branching',32,... 102 | 'iterations',3,... 103 | 'checks',120)); 104 | n = size(match,2); 105 | precision = (n-sum(abs(result(1,:)-match(1,:))>0))/n; 106 | assert(precision>0.9); 107 | assert(sum(~(match(1,:)-result(1,:)).*(dists(1,:)-ndists(1,:)))==0); 108 | end 109 | run_test('k-means search',@test_kmeans_search); 110 | 111 | 112 | 113 | function test_composite_search 114 | [result, ndists] = flann_search(dataset, testset, 10, struct('algorithm','composite',... 115 | 'branching',32,... 116 | 'iterations',3,... 117 | 'trees', 1,... 118 | 'checks',64)); 119 | n = size(match,2); 120 | precision = (n-sum(abs(result(1,:)-match(1,:))>0))/n; 121 | assert(precision>0.9); 122 | assert(sum(~(match(1,:)-result(1,:)).*(dists(1,:)-ndists(1,:)))==0); 123 | end 124 | run_test('composite search',@test_composite_search); 125 | 126 | function test_autotune_search 127 | [result, ndists] = flann_search(dataset, testset, 10, struct('algorithm','autotuned',... 128 | 'target_precision',0.95,... 129 | 'build_weight',0.01,... 130 | 'memory_weight',0)); 131 | n = size(match,2); 132 | precision = (n-sum(abs(result(1,:)-match(1,:))>0))/n; 133 | assert(precision>0.9); 134 | assert(sum(~(match(1,:)-result(1,:)).*(dists(1,:)-ndists(1,:)))==0); 135 | end 136 | run_test('search with autotune',@test_autotune_search); 137 | 138 | function test_index_kdtree_search 139 | [index, search_params ] = flann_build_index(dataset, struct('algorithm','kdtree', 'trees',8,... 140 | 'checks',64)); 141 | [result, ndists] = flann_search(index, testset, 10, search_params); 142 | n = size(match,2); 143 | precision = (n-sum(abs(result(1,:)-match(1,:))>0))/n; 144 | assert(precision>0.9); 145 | assert(sum(~(match(1,:)-result(1,:)).*(dists(1,:)-ndists(1,:)))==0); 146 | end 147 | run_test('index kd-tree search',@test_index_kdtree_search); 148 | 149 | function test_index_kmeans_search 150 | [index, search_params ] = flann_build_index(dataset, struct('algorithm','kmeans',... 151 | 'branching',32,... 152 | 'iterations',3,... 153 | 'checks',120)); 154 | [result, ndists] = flann_search(index, testset, 10, search_params); 155 | n = size(match,2); 156 | precision = (n-sum(abs(result(1,:)-match(1,:))>0))/n; 157 | assert(precision>0.9); 158 | assert(sum(~(match(1,:)-result(1,:)).*(dists(1,:)-ndists(1,:)))==0); 159 | end 160 | run_test('index kmeans search',@test_index_kmeans_search); 161 | 162 | function test_index_kmeans_search_gonzales 163 | [index, search_params ] = flann_build_index(dataset, struct('algorithm','kmeans',... 164 | 'branching',32,... 165 | 'iterations',3,... 166 | 'checks',120,... 167 | 'centers_init','gonzales')); 168 | [result, ndists] = flann_search(index, testset, 10, search_params); 169 | n = size(match,2); 170 | precision = (n-sum(abs(result(1,:)-match(1,:))>0))/n; 171 | assert(precision>0.9); 172 | assert(sum(~(match(1,:)-result(1,:)).*(dists(1,:)-ndists(1,:)))==0); 173 | end 174 | run_test('index kmeans search gonzales',@test_index_kmeans_search_gonzales); 175 | 176 | function test_index_kmeans_search_kmeanspp 177 | [index, search_params ] = flann_build_index(dataset, struct('algorithm','kmeans',... 178 | 'branching',32,... 179 | 'iterations',3,... 180 | 'checks',120,... 181 | 'centers_init','kmeanspp')); 182 | [result, ndists] = flann_search(index, testset, 10, search_params); 183 | n = size(match,2); 184 | precision = (n-sum(abs(result(1,:)-match(1,:))>0))/n; 185 | assert(precision>0.9); 186 | assert(sum(~(match(1,:)-result(1,:)).*(dists(1,:)-ndists(1,:)))==0); 187 | end 188 | run_test('index kmeans search kmeanspp',@test_index_kmeans_search_kmeanspp); 189 | 190 | function test_index_composite_search 191 | [index, search_params ] = flann_build_index(dataset,struct('algorithm','composite',... 192 | 'branching',32,... 193 | 'iterations',3,... 194 | 'trees', 1,... 195 | 'checks',64)); 196 | [result, ndists] = flann_search(index, testset, 10, search_params); 197 | n = size(match,2); 198 | precision = (n-sum(abs(result(1,:)-match(1,:))>0))/n; 199 | assert(precision>0.9); 200 | assert(sum(~(match(1,:)-result(1,:)).*(dists(1,:)-ndists(1,:)))==0); 201 | end 202 | run_test('index composite search',@test_index_composite_search); 203 | 204 | function test_index_autotune_search 205 | [index, search_params, speedup ] = flann_build_index(dataset,struct('algorithm','autotuned',... 206 | 'target_precision',0.95,... 207 | 'build_weight',0.01,... 208 | 'memory_weight',0)); 209 | [result, ndists] = flann_search(index, testset, 10, search_params); 210 | n = size(match,2); 211 | precision = (n-sum(abs(result(1,:)-match(1,:))>0))/n; 212 | assert(precision>0.9); 213 | assert(sum(~(match(1,:)-result(1,:)).*(dists(1,:)-ndists(1,:)))==0); 214 | end 215 | run_test('index autotune search',@test_index_autotune_search); 216 | 217 | status(); 218 | end 219 | -------------------------------------------------------------------------------- /matrixCompute.m: -------------------------------------------------------------------------------- 1 | function M = matrixCompute(A,xc) 2 | 3 | % perform svd on the convarance matrix of the point set A 4 | if(size(A,1)~=3) 5 | A = A'; 6 | end 7 | 8 | % construct the convariance matrix 9 | K = size(A,2); 10 | B = bsxfun(@minus, A, xc(:)); 11 | M = 1/K*(B*B'); 12 | 13 | -------------------------------------------------------------------------------- /ransac.cpp: -------------------------------------------------------------------------------- 1 | // ransac.cpp : Defines the initialization routines for the DLL. 2 | // 3 | 4 | 5 | #include 6 | #include 7 | #include 8 | #include "mex.h" 9 | 10 | 11 | #define M 3 12 | #define maxIter 200 13 | 14 | using namespace std; 15 | 16 | // generate random indices for the minimal sample set 17 | vector generate(int N) 18 | { 19 | vector index(N); //the whole indices 20 | for (int i = 0; i < N; i++) 21 | { 22 | index[i] = i; 23 | } 24 | 25 | vector vektor(M); 26 | 27 | int in, im = 0; 28 | for (in = N; in > N - M; in--) 29 | { 30 | int r = rand() % in; /* generate a random number 'r' */ 31 | vektor[im++] = index[r]; /* the range begins from 0 */ 32 | index.erase(index.begin() + r); 33 | } 34 | 35 | return vektor; 36 | } 37 | 38 | 39 | 40 | double* estimateTform(double* srcPts, double* tarPts, vector &Idx) 41 | { 42 | mxArray *rhs[2], *lhs[2]; 43 | rhs[0] = mxCreateDoubleMatrix(3, Idx.size(), mxREAL); 44 | rhs[1] = mxCreateDoubleMatrix(3, Idx.size(), mxREAL); 45 | 46 | double *X, *Y; 47 | X = mxGetPr(rhs[0]); 48 | Y = mxGetPr(rhs[1]); 49 | 50 | for (int j = 0; j < Idx.size(); j++) 51 | { 52 | X[3 * j] = tarPts[3 * Idx[j]]; 53 | X[3 * j + 1] = tarPts[3 * Idx[j] + 1]; 54 | X[3 * j + 2] = tarPts[3 * Idx[j] + 2]; 55 | 56 | Y[3 * j] = srcPts[3 * Idx[j]]; 57 | Y[3 * j + 1] = srcPts[3 * Idx[j] + 1]; 58 | Y[3 * j + 2] = srcPts[3 * Idx[j] + 2]; 59 | } 60 | 61 | lhs[0] = mxCreateDoubleMatrix(4, 4, mxREAL); 62 | lhs[1] = mxCreateDoubleMatrix(1, 1, mxREAL); 63 | mexCallMATLAB(2,lhs,2,rhs,"estimateRigidTransform"); 64 | 65 | double* T = mxGetPr(lhs[0]); 66 | return T; 67 | } 68 | 69 | 70 | 71 | void mexFunction(int nlhs, mxArray *plhs[], 72 | int nrhs, const mxArray *prhs[]) 73 | { 74 | double *srcPts, *tarPts, threshold; 75 | 76 | if (mxGetM(prhs[0]) != 3 || mxGetM(prhs[1]) != 3 || mxGetN(prhs[0]) != mxGetN(prhs[1])) 77 | mexErrMsgTxt("The input point matrix should be with size 3-by-N!"); 78 | 79 | srcPts = mxGetPr(prhs[0]); 80 | tarPts = mxGetPr(prhs[1]); 81 | threshold = mxGetScalar(prhs[2]); 82 | int N = mxGetN(prhs[0]); 83 | 84 | plhs[0] = mxCreateLogicalMatrix(1,N); // indicate either a match is inlier or not 85 | bool* CS = (bool*)mxGetData(plhs[0]); 86 | 87 | // Main loop 88 | //--------------------------------------------------------------- 89 | // initializations 90 | int iter = 0; //number of iterations 91 | int bestSz = 3; //initial threshold for inlier size of a better model 92 | vector randIdx(M, 0); 93 | vector x(3, 0), y_hat(3, 0), y(3, 0); 94 | vector thisCS(N, false); 95 | 96 | srand((unsigned)time(NULL)); //set the seed to the current time 97 | //srand((unsigned)time(0)); //set the seed to 0 98 | while (iter <= maxIter) 99 | { 100 | randIdx = generate(N); 101 | double* T = estimateTform(srcPts, tarPts, randIdx); 102 | 103 | // to get size of the consensus set 104 | int inlierSz = 0; 105 | for (int i = 0; i < N; i++) 106 | { 107 | x[0] = srcPts[3 * i]; x[1] = srcPts[3 * i + 1]; x[2] = srcPts[3 * i + 2]; 108 | y[0] = tarPts[3 * i]; y[1] = tarPts[3 * i + 1]; y[2] = tarPts[3 * i + 2]; 109 | 110 | y_hat[0] = T[0] * x[0] + T[4] * x[1] + T[8] * x[2] + T[12]; 111 | y_hat[1] = T[1] * x[0] + T[5] * x[1] + T[9] * x[2] + T[13]; 112 | y_hat[2] = T[2] * x[0] + T[6] * x[1] + T[10] * x[2] + T[14]; 113 | 114 | double thisErr = (y[0] - y_hat[0])*(y[0] - y_hat[0]) + 115 | (y[1] - y_hat[1])*(y[1] - y_hat[1]) + 116 | (y[2] - y_hat[2])*(y[2] - y_hat[2]); 117 | 118 | thisCS[i] = false; 119 | if (thisErr < threshold) 120 | { 121 | inlierSz++; 122 | thisCS[i] = true; 123 | } 124 | } 125 | 126 | if (inlierSz>bestSz) 127 | { 128 | bestSz = inlierSz; // update the best model size 129 | 130 | //update the consensus set 131 | for (int i = 0; i < N; i++) 132 | { 133 | CS[i] = thisCS[i]; 134 | } 135 | } 136 | 137 | if (bestSz == N) 138 | break; 139 | 140 | iter++; 141 | } 142 | //-------------------------------------------------------------- 143 | 144 | return; 145 | } 146 | -------------------------------------------------------------------------------- /ransac.mexw64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/EnyaHermite/fastDesp-corrProp/7da2d6af077afa845c906446ce663cb6fc4503a9/ransac.mexw64 -------------------------------------------------------------------------------- /readme.txt: -------------------------------------------------------------------------------- 1 | The code is released for academic use only. 2 | 3 | We use Matlab2015b to implement our algorithm. If you 4 | use lower version than Matlab2015b, please make sure 5 | the functions such as pcread, pcdownsample are still 6 | supported. If there is something wrong in your use of 7 | the ransac.mex64 file, please re-mex the ransac.cpp on 8 | your machine to make it compatible. 9 | -------------------------------------------------------------------------------- /svdCov.m: -------------------------------------------------------------------------------- 1 | function [s,n] = svdCov(nnIdx, idx, Data, Seed) 2 | 3 | nnPt = Data(:,nnIdx); 4 | C = matrixCompute(nnPt,Seed(:,idx)); 5 | [U,S,~] = svd(C); 6 | s = diag(S)/sum(diag(S)); 7 | n = sign(dot(U(:,3),-Seed(:,idx)))*U(:,3); --------------------------------------------------------------------------------