├── LICENSE.bsd ├── README.md ├── addSpecificPaths.m ├── data ├── .gitignore └── README.md ├── experiment1_aposteriori_bounds_synthetic.m ├── experiment2_check_hypercontractivity_synthetic.m ├── experiment3_check_hypercontractivity_real.m ├── experiment4_hypercontractivity_bounds.m ├── experiment5_visualize_anticoncentration.m ├── experiment6_check_anticoncentration_synthetic_vs_n.m ├── experiment7_check_anticoncentration_synthetic_vs_eta.m ├── experiment8_slide_synthetic.m ├── experiment9_check_anticoncentration_real.m ├── lib ├── arrow3d.m ├── blkIndices.m ├── computeAposterioriBoundWahba.m ├── computeAposterioriBoundWahba_v2.m ├── createImageStitchingData.m ├── createWahbaProblemData.m ├── my_ref_frame.m ├── plotImageStitchingResults.m ├── plotRotations.m ├── rotatePoints.m ├── shaded_plots │ ├── example_shaded_plots.m │ ├── license.txt │ ├── plot_distribution.m │ ├── plot_distribution_prctile.m │ ├── plot_histogram_shaded.m │ └── plot_shaded.m ├── solveWahba.m ├── solveWahba_TLS_01.m ├── solveWahba_TLS_m1p1.m ├── solveWahba_listDecodable_01.m ├── testAntiConcentration.m └── testHypercontractivity.m └── results ├── SLIDE_example1.mat ├── SLIDE_example2.mat ├── log_results_experiment1.mat ├── log_results_experiment2.mat ├── log_results_experiment2_SOSTOOLS400.mat ├── log_results_experiment3.mat ├── log_results_experiment6.mat ├── log_results_experiment7_isModified_0.mat ├── log_results_experiment7_isModified_1.mat ├── log_results_experiment7_isModified_1_SOSTOOLS400.mat ├── log_results_experiment8_isAdversarial_0_recoverAll_0.mat ├── log_results_experiment8_isAdversarial_1_recoverAll_0.mat └── log_results_experiment8_isAdversarial_1_recoverAll_1.mat /LICENSE.bsd: -------------------------------------------------------------------------------- 1 | Copyright 2019 Massachusetts Institute of Technology. 2 | 3 | Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 4 | 5 | 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 6 | 7 | 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 8 | 9 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # README 2 | 3 | # Overview 4 | 5 | This is the code to reproduce the experiments in the paper: 6 | 7 | **L. Carlone, Estimation Contracts for Outlier-Robust Geometric Perception, [arXiv: 2208.10521](https://arxiv.org/pdf/2208.10521.pdf), 2022.** 8 | 9 | # Getting started 10 | 11 | The code has been implemented and tested in Matlab R2020a, using a Macbook Pro with maxOS Monterey. We expect it to run on any other operating system supporting the dependencies below. 12 | 13 | To run the code, please install the following dependencies. Note: it is important to install all the dependencies in the same folder (e.g., all subfolders of a ``code`` folder): 14 | - [CVX with MOSEK](http://cvxr.com/cvx/doc/mosek.html) (tested with CVX Version 2.2) 15 | - [STRIDE and the Certifiably Robust Perception repository](https://github.com/MIT-SPARK/CertifiablyRobustPerception/) (tested with latest commit, December 10, 2022) 16 | - [SOSTOOLS](https://github.com/oxfordcontrol/SOSTOOLS) (tested with Version 3.01/master branch, but also runs with SOSTOOLS400 branch, which is typically faster) 17 | 18 | Clone our repo in the same folder as the other dependencies. 19 | After installing the dependencies and cloning this repo, the folder where you installed the dependencies in should at least have the following sub-folders: 20 | - CertifiablyRobustPerception 21 | - cvx 22 | - estimation-contracts 23 | - SOSTOOLS 24 | 25 | # Running the code 26 | 27 | You can replicate the experiments in the paper by running each of the following experiments: 28 | * **experiment1_aposteriori_bounds_synthetic**: solves synthetic rotation search problems using [QUASAR](https://arxiv.org/pdf/1905.12536.pdf) and compares the estimation errors with the a posteriori bound developed in our paper (Fig. 4 in the paper) (expected runtime: ~15 hours) 29 | * **experiment2_check_hypercontractivity_synthetic**: check certifiable hypercontractivity in synthetic rotation search problems (Fig. 5 in the paper) (expected runtime: ~1.5 hours) 30 | * **experiment3_check_hypercontractivity_real**: check certifiable hypercontractivity in real rotation search problems arising in panorama stitching (Table 1 in the paper). To run the real tests, please download the data from and put them in the **data** subfolder: [link to panorama stitching data](https://drive.google.com/drive/folders/1CppsDdU98PgG939aV0ZaaBcVYRLrgI9O?usp=sharing) 31 | (expected runtime: ~0.5 hours) 32 | * **experiment4_hypercontractivity_bounds**: visualizes the bounds in Theorem 11 (Fig. 6 in the paper) 33 | * **experiment5_visualize_anticoncentration**: visualizes the functions involved in the definition of certifiable anti-concentration (Fig. 7 in the paper) 34 | * **experiment6_check_anticoncentration_synthetic_vs_n**: check certifiable anti-concentration in synthetic rotation search problems for different number of measurements (Fig. 8(b) in the paper) (expected runtime: ~55 hours) 35 | * **experiment7_check_anticoncentration_synthetic_vs_eta**: check certifiable anti-concentration in synthetic rotation search problems for different values of the parameter $\eta$, which obtains: 36 | * Fig. 8(a) when the parameter ``isModified = 0`` (expected runtime: ~40 hours) 37 | * Fig. 8(c) when the parameter ``isModified = 0`` (expected runtime: ~40 hours) 38 | * **experiment8_slide_synthetic**: evaluates SLIDE for list decodable regression in synthetic rotation search problems, which obtains: 39 | * Fig. 9 when the parameter ``isAdversarial = 0`` and ``recoverAllHypotheses = 0`` (expected runtime: ~6 hours) 40 | * Fig. 10 when the parameter ``isAdversarial = 1`` and ``recoverAllHypotheses = 0`` (expected runtime: ~6 hours) 41 | * Fig. 11 when the parameter ``isAdversarial = 1`` and ``recoverAllHypotheses = 1`` (expected runtime: ~6 hours) 42 | * Fig. 12 when the parameter ``isAdversarial = 1`` and ``recoverAllHypotheses = 1`` (for some random instances of the problem, also stored in SLIDE_example1.mat and SLIDE_example2.mat) 43 | * **experiment9_check_anticoncentration_real**: check certifiable anti-concentration in real rotation search problems arising in panorama stitching (not reported in the paper). To run the real tests, please download the data from and put them in the **data** subfolder: [link to panorama stitching data](https://drive.google.com/drive/folders/1CppsDdU98PgG939aV0ZaaBcVYRLrgI9O?usp=sharing) 44 | 45 | The results we obtained by running the examples above are also stored in the **results** folder within this repo. 46 | Most results are obtained using SOSTOOLS v. 3.01, but we re-ran the experiments where we measured the runtime using the SOSTOOLS400 branch in SOSTOOLS, since the latter is typically faster. We store both sets of results and add a label *SOSTOOLS400* to the latter set of results. 47 | 48 | # Reference 49 | 50 | If you found the paper or code useful, please cite: 51 | 52 | ```bibtex 53 | @article{Carlone22arxiv-estimationContracts, 54 | author = {L. Carlone}, 55 | title = {Estimation Contracts for Outlier-Robust Geometric Perception}, 56 | journal = {arXiv preprint arXiv: 2208.10521}, 57 | pdf = {https://arxiv.org/pdf/2208.10521.pdf}, 58 | Year = {2022} 59 | } 60 | ``` 61 | 62 | # Acknowledgments 63 | 64 | This work was partially funded by the NSF CAREER award [Certifiable Perception for Autonomous Cyber-Physical 65 | Systems](https://nsf.gov/awardsearch/showAward?AWD_ID=2044973) and by ARL [DCIST](https://www.dcist.org/) CRA W911NF-17- 2-0181. 66 | 67 | # License 68 | 69 | [BSD License](LICENSE.BSD) 70 | 71 | 72 | 73 | -------------------------------------------------------------------------------- /addSpecificPaths.m: -------------------------------------------------------------------------------- 1 | exampleRepo = '../CertifiablyRobustPerception/RotationSearch/'; 2 | cvxpath = strcat(exampleRepo, '../../cvx'); 3 | mosekpath = strcat(exampleRepo,'../../mosek'); 4 | utilspath = strcat(exampleRepo,'../utils'); 5 | manoptpath = strcat(exampleRepo,'../manopt'); 6 | sdpnalpath = strcat(exampleRepo,'../../SDPNAL+v1.0'); 7 | stridepath = strcat(exampleRepo,'../STRIDE'); 8 | sostools = strcat(exampleRepo,'../../SOSTOOLS'); 9 | 10 | addpath(genpath(exampleRepo)) 11 | addpath(genpath(cvxpath)) 12 | addpath(genpath(mosekpath)) 13 | addpath(genpath(sostools)) 14 | addpath(genpath(utilspath)) 15 | addpath(genpath('./lib')) 16 | addsostools -------------------------------------------------------------------------------- /data/.gitignore: -------------------------------------------------------------------------------- 1 | /stitching_13_19.mat 2 | /stitching_19_25.mat 3 | /stitching_1_7.mat 4 | /stitching_25_31.mat 5 | /stitching_31_37.mat 6 | /stitching_37_43.mat 7 | /stitching_43_49.mat 8 | /stitching_49_55.mat 9 | /stitching_55_61.mat 10 | /stitching_61_67.mat 11 | /stitching_67_1.mat 12 | /stitching_7_13.mat 13 | -------------------------------------------------------------------------------- /data/README.md: -------------------------------------------------------------------------------- 1 | # data folder 2 | 3 | This folder is designed to contain the data for the panorama stitching experiments in the paper **Estimation Contracts for Outlier-Robust Geometric Perception**. Please download the data from: [link to panorama stitching data](https://drive.google.com/drive/folders/1CppsDdU98PgG939aV0ZaaBcVYRLrgI9O?usp=sharing). 4 | 5 | After downloading the data, this folder should look like this: 6 | 7 | - README.md 8 | - stitching_1_7.mat 9 | - stitching_7_13.mat 10 | - stitching_13_19.mat 11 | - stitching_19_25.mat 12 | - stitching_25_31.mat 13 | - stitching_31_37.mat 14 | - stitching_37_43.mat 15 | - stitching_43_49.mat 16 | - stitching_49_55.mat 17 | - stitching_55_61.mat 18 | - stitching_61_67.mat 19 | - stitching_67_1.mat 20 | 21 | -------------------------------------------------------------------------------- /experiment1_aposteriori_bounds_synthetic.m: -------------------------------------------------------------------------------- 1 | %% Test a posteriori bounds in robust rotation search (Wahba Problem) 2 | %% Synthetic data 3 | %% Paper: "Estimation Contracts for Outlier-Robust Geometric Perception" 4 | %% Luca Carlone, Nov 7, 2022 5 | %% example data from: https://github.com/MIT-SPARK/CertifiablyRobustPerception/ 6 | 7 | clc 8 | clear 9 | close all 10 | restoredefaultpath 11 | addSpecificPaths 12 | 13 | %% set parameters for Monte Carlo runs 14 | Nset = [50] % number of measurements 15 | outrateSet = [0 0.1 0.2 0.3 0.4 0.5]; % outlier rate 16 | nrTests = 100; % number of runs for each configuration 17 | isAdversarial = 0; 18 | 19 | %% allocate matrix to store results 20 | results_true_error = zeros(length(Nset),length(outrateSet),nrTests); 21 | results_boundN = zeros(length(Nset),length(outrateSet),nrTests); 22 | results_bound5 = zeros(length(Nset),length(outrateSet),nrTests); 23 | results_eta = zeros(length(Nset),length(outrateSet),nrTests); 24 | 25 | %% generate random problems and test certifiable hyper-contractivity 26 | for ind_N = 1:length(Nset) % general, but we test for a single N here 27 | N = Nset(ind_N); % number of measurements 28 | for ind_outrate = 1:length(outrateSet) 29 | outrate = outrateSet(ind_outrate); % outlier rate 30 | for test = 1:nrTests 31 | fprintf('=== test %d of %d, with N=%d, and outlier rate = %g ===\n',test,nrTests,N,outrate) 32 | [problem, R_gt] = createWahbaProblemData(N,outrate,isAdversarial); 33 | [R_stride,theta_stride,eta] = solveWahba(problem.a,problem.b,problem.betasq,... 34 | R_gt,... % R_gt only used for performance evaluation 35 | sdpnalpath,manoptpath,stridepath); % paths to solvers 36 | 37 | % compute error and bounds 38 | true_error = norm( R_gt(:) - R_stride(:) ); 39 | J = []; % set of correctly selected inliers 40 | for i=1:N 41 | if theta_stride(i) == problem.theta_gt(i) && theta_stride(i) == 1 42 | J = [J i]; 43 | end 44 | end 45 | 46 | boundN = computeAposterioriBoundWahba_v2(J, problem.At, sqrt(problem.betasq)); 47 | bound5 = computeAposterioriBoundWahba(theta_stride, problem.At, sqrt(problem.betasq), 5); 48 | 49 | % store results 50 | results_true_error(ind_N,ind_outrate,test) = true_error; 51 | results_boundN(ind_N,ind_outrate,test) = boundN; 52 | results_bound5(ind_N,ind_outrate,test) = bound5; 53 | results_eta(ind_N,ind_outrate,test) = eta; 54 | 55 | fprintf('+++ eta=%g, true_error = %g <= %g (boundN) <=? %g (bound5) +++\n',eta,true_error,boundN,bound5) 56 | end 57 | end 58 | end 59 | 60 | results_true_error = squeeze(results_true_error); 61 | results_boundN = squeeze(results_boundN); 62 | results_bound5 = squeeze(results_bound5); 63 | results_eta = squeeze(results_eta); 64 | 65 | %% plot actual errors vs bounds 66 | figure; 67 | meanErrorTrue = mean(results_true_error,2); 68 | meanBoundN = mean(results_boundN,2); 69 | meanBound5 = mean(results_bound5,2); 70 | semilogy(outrateSet,meanErrorTrue,'-k','linewidth',4); 71 | hold on; grid on 72 | semilogy(outrateSet,meanBoundN,'-r','linewidth',4); 73 | semilogy(outrateSet,meanBound5,'-b','linewidth',4); 74 | semilogy(outrateSet,2*sqrt(3) * ones(size(outrateSet)),'-.k','linewidth',4); 75 | for test = 1:nrTests 76 | lh = semilogy(outrateSet,results_true_error(:,test),'-k','linewidth',2); 77 | lh.Color(4)=0.2; 78 | lh = semilogy(outrateSet,results_boundN(:,test),'-r','linewidth',2); 79 | lh.Color(4)=0.2; 80 | lh = semilogy(outrateSet,results_bound5(:,test),'-b','linewidth',2); 81 | lh.Color(4)=0.2; 82 | end 83 | semilogy(outrateSet,meanErrorTrue,'-k','linewidth',4); 84 | semilogy(outrateSet,meanBoundN,'-r','linewidth',4); 85 | semilogy(outrateSet,meanBound5,'-b','linewidth',4); 86 | semilogy(outrateSet,2*sqrt(3) * ones(size(outrateSet)),'-.k','linewidth',4); 87 | ylim([1e-4 5e2]) 88 | xlabel('Outlier rate ($\beta$)','interpreter','latex') 89 | ylabel('Estimation error ($\|x-x^\circ\|_2$)','interpreter','latex') 90 | legend('actual error','bound-J','bound-5','trivial bound','Location', 'Best') 91 | set(gca,'fontsize', 18) 92 | 93 | %% check tightness 94 | figure 95 | results_tightness = results_eta < 1e-7; 96 | tightInstances = 100* sum(results_tightness,2) / nrTests; 97 | bar(outrateSet,tightInstances) 98 | 99 | save log_results_experiment1 100 | 101 | 102 | 103 | 104 | 105 | -------------------------------------------------------------------------------- /experiment2_check_hypercontractivity_synthetic.m: -------------------------------------------------------------------------------- 1 | %% Test Certifiable Hypercontractivity in robust rotation search (Wahba Problem) 2 | %% Synthetic data 3 | %% Paper: "Estimation Contracts for Outlier-Robust Geometric Perception" 4 | %% Luca Carlone, Nov 7, 2022 5 | %% example data from: https://github.com/MIT-SPARK/CertifiablyRobustPerception/ 6 | 7 | clc 8 | clear 9 | close all 10 | restoredefaultpath 11 | addSpecificPaths 12 | 13 | %% set parameters for Monte Carlo runs 14 | Nset = [5 10 20 40 60 80 100 200]; % number of measurements % [5 10 20 40 60 80 100 200] 15 | outrate = 0.5; % outlier rate (irrelevant in the computation of the hypercontractivity 16 | Ct_pow_t_set = [1 2 3 4 5 6]; 17 | nrTests = 100; % number of runs for each configuration 18 | 19 | %% allocate matrix to store results 20 | results = zeros(length(Nset),length(Ct_pow_t_set)); 21 | runtime = zeros(length(Nset),length(Ct_pow_t_set)); 22 | 23 | %% generate random problems and test certifiable hyper-contractivity 24 | for ind_N = 1:length(Nset) 25 | N = Nset(ind_N); % number of measurements 26 | for ind_Ct_set = 1:length(Ct_pow_t_set) 27 | Ct_pow_t_input = Ct_pow_t_set(ind_Ct_set); % Ct for hypercontractivity 28 | nrHyperContractive = 0; % count number of certifiable hypercontractive instances 29 | totalTime = 0; % to measure time required by hyper contractivity check 30 | for test = 1:nrTests 31 | fprintf('=== test %d of %d, with N=%d, and outlier rate = %g ===\n',test,nrTests,N,outrate) 32 | [problem, R_gt] = createWahbaProblemData(N,outrate); 33 | [isHyperContractive,Q,Z,timeTest] = testHypercontractivity(problem.At,Ct_pow_t_input); 34 | totalTime = totalTime + timeTest; 35 | if isHyperContractive 36 | nrHyperContractive = nrHyperContractive + 1; 37 | fprintf('=== %d certifiable hypercontractive out of %d tests ===\n',nrHyperContractive,test) 38 | end 39 | end 40 | results(ind_N,ind_Ct_set) = nrHyperContractive; 41 | runtime(ind_N,ind_Ct_set) = totalTime/nrTests; % we record average time 42 | end 43 | end 44 | 45 | %save log_results_experiment2 46 | 47 | %% plot/display results 48 | figure 49 | h = heatmap(Ct_pow_t_set,Nset,results); 50 | % h.Title = 'Hypercontractive instances'; 51 | h.XLabel = '$C(t)^t$'; 52 | h.YLabel = 'Nr. measurements ($n$)'; 53 | h.NodeChildren(3).XAxis.Label.Interpreter = 'latex'; 54 | h.NodeChildren(3).YAxis.Label.Interpreter = 'latex'; 55 | h.NodeChildren(3).Title.Interpreter = 'latex'; 56 | h.FontSize = 18; 57 | 58 | %% timing figure 59 | figure 60 | h = heatmap(Ct_pow_t_set,Nset,runtime); 61 | %h.Title = 'Average runtime'; 62 | h.XLabel = '$C(t)^t$'; 63 | h.YLabel = 'Nr. measurements ($n$)'; 64 | h.CellLabelFormat = '%.2f'; 65 | h.NodeChildren(3).XAxis.Label.Interpreter = 'latex'; 66 | h.NodeChildren(3).YAxis.Label.Interpreter = 'latex'; 67 | h.NodeChildren(3).Title.Interpreter = 'latex'; 68 | h.FontSize = 18; 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | -------------------------------------------------------------------------------- /experiment3_check_hypercontractivity_real.m: -------------------------------------------------------------------------------- 1 | %% Test Certifiable Hypercontractivity in robust rotation search (Wahba Problem) 2 | %% Real data from image stitching application 3 | %% Paper: "Estimation Contracts for Outlier-Robust Geometric Perception" 4 | %% Luca Carlone, Nov 7, 2022 5 | %% example data provided by Heng Yang (same as the one used in the QUASAR paper: https://arxiv.org/pdf/1905.12536.pdf) 6 | 7 | clc 8 | clear 9 | close all 10 | restoredefaultpath 11 | addSpecificPaths 12 | 13 | %% name of mat file containing images and feature data 14 | dataFileNames = {... 15 | 'data/stitching_1_7.mat',... 16 | 'data/stitching_7_13.mat',... %-x 17 | 'data/stitching_13_19.mat',... 18 | 'data/stitching_19_25.mat',... 19 | 'data/stitching_25_31.mat',... 20 | 'data/stitching_31_37.mat',... 21 | 'data/stitching_37_43.mat',... %-x 22 | 'data/stitching_43_49.mat',... %-x 23 | 'data/stitching_49_55.mat',... 24 | 'data/stitching_55_61.mat',...%-x 25 | 'data/stitching_61_67.mat',... 26 | 'data/stitching_67_1.mat'} %-xx 27 | 28 | Ct_pow_t_set = [1 2 3 4 5 6 7 8 9 10 15 20]; 29 | 30 | isHyperContractive_results = zeros(length(dataFileNames),length(Ct_pow_t_set)); 31 | timeTest_results = zeros(length(dataFileNames),length(Ct_pow_t_set)); 32 | eta_results = zeros(length(dataFileNames),1); 33 | 34 | for fileInd=1:length(dataFileNames) 35 | dataFileName = dataFileNames{fileInd} 36 | 37 | %% parse images to get real data 38 | [problem, R_gt, theta_gt] = createImageStitchingData(dataFileName); 39 | 40 | %% contrarily to experiment1, where we know the ground truth inliers, here we estimate them 41 | [R_stride,theta_stride,eta] = solveWahba(problem.a,problem.b,problem.betasq,... 42 | R_gt,... % R_gt only used for performance evaluation 43 | sdpnalpath,manoptpath,stridepath); % paths to solvers 44 | 45 | %% build inliers/outliers (estimated from real data this time) 46 | problem.outliersIds = find(theta_stride < 0); 47 | 48 | %% check results against QUASAR results (marked as "gt") 49 | theta_diff = abs(theta_stride - theta_gt); 50 | diffOutliers = sum(theta_diff>0); % Sdp formulation is slightly different from QUASAR,.. 51 | % ... but number of inliers should roughly match (+/-1) 52 | R_err = R_gt' * R_stride; 53 | axang = rotm2axang(R_err); 54 | angleErrDeg = rad2deg(axang(4)); % small, as expected (seems rotm2axang approximates to zero) 55 | fprintf('diffOutliers = %d, rotError[deg] = %g\n',diffOutliers,angleErrDeg) 56 | 57 | %% visualize image stitching results 58 | plotImageStitchingResults 59 | 60 | %% store tightness 61 | eta_results(fileInd) = eta; 62 | 63 | for j = 1:length(Ct_pow_t_set) 64 | %% test hypercontractivity 65 | Ct_pow_t_input = Ct_pow_t_set(j); 66 | [isHyperContractive,Q,Z,timeTest] = testHypercontractivity(problem.At,Ct_pow_t_input); 67 | isHyperContractive_results(fileInd,j) = isHyperContractive; 68 | timeTest_results(fileInd,j) = timeTest; 69 | end 70 | isHyperContractive_results 71 | timeTest_results 72 | eta_results 73 | end 74 | 75 | % save log_results_experiment3 76 | 77 | %% print results for latex table 78 | percentageHyperTest = 100*sum(isHyperContractive_results,1)/length(dataFileNames); 79 | for i=1:length(percentageHyperTest) 80 | fprintf(' & $%.1f$ ', percentageHyperTest(i)) 81 | end 82 | fprintf(' \n') 83 | 84 | 85 | 86 | 87 | 88 | -------------------------------------------------------------------------------- /experiment4_hypercontractivity_bounds.m: -------------------------------------------------------------------------------- 1 | %% Visualize bounds in Theorem 11 2 | %% Paper: "Estimation Contracts for Outlier-Robust Geometric Perception" 3 | %% Luca Carlone, Nov 7, 2022 4 | 5 | %% plot coefficients C1 and C2: 6 | close all 7 | clc 8 | N = 50; 9 | Ct_pow_t_set = [1 2 4 6]; 10 | 11 | k = 4; 12 | for j=1:length(Ct_pow_t_set) 13 | t = k/2; 14 | Ct_pow_t = Ct_pow_t_set(j); % Ct^t; 15 | betaMax = ( 1/(Ct_pow_t*2^(3*k-1)) )^(1 / (k/2 - 1)) - 1e-6 % minus some numerical margin 16 | beta = linspace(0,betaMax,100000); 17 | C1 = zeros(length(beta),1); 18 | C2 = zeros(length(beta),1); 19 | for i=1:length(beta) 20 | betai = beta(i); 21 | C1(i) = C1_pow_2_over_k(betai, Ct_pow_t, k); 22 | C2(i) = C2_pow_2_over_k(betai, Ct_pow_t, k); 23 | end 24 | 25 | %% plot C1 26 | figure(1) 27 | semilogy(beta,C1,'-r','linewidth',2) 28 | hold on 29 | grid on 30 | ylimits = [2*10^-3 40]; 31 | xlabel('Outlier rate $\beta$','interpreter','latex') 32 | ylabel('$C_1(4,\beta)^{1/2}$','interpreter','latex') 33 | plot([betaMax betaMax],ylimits,'--k','linewidth',1) 34 | s = sprintf(' C(t)^t = %d ',Ct_pow_t); 35 | switch Ct_pow_t 36 | case 1 37 | text(beta(end),C1(end)+6,s,'HorizontalAlignment','right') 38 | case 2 39 | text(beta(end),C1(end)+4,s,'HorizontalAlignment','left') 40 | case 4 41 | text(beta(end),C1(end)+2,s,'HorizontalAlignment','left') 42 | case 6 43 | text(beta(end),C1(end)+2,s,'HorizontalAlignment','right') 44 | end 45 | ylim(ylimits) 46 | xlim([-1e-5 5*1e-4]) 47 | 48 | %% plot C2 49 | figure(2) 50 | semilogy(beta,C2,'-b','linewidth',2) 51 | hold on 52 | grid on 53 | ylimits = [1*10^-3 20]; 54 | xlabel('Outlier rate $\beta$','interpreter','latex') 55 | ylabel('$C_2(4,\beta)^{1/2}$','interpreter','latex') 56 | plot([betaMax betaMax],ylimits,'--k','linewidth',1) 57 | ylim([10^-5 10^4]) 58 | s = sprintf(' C(t)^t = %d ',Ct_pow_t); 59 | switch Ct_pow_t 60 | case 1 61 | text(beta(end),C2(end)+1,s,'HorizontalAlignment','right') 62 | case 2 63 | text(beta(end),C2(end)+1,s,'HorizontalAlignment','left') 64 | case 4 65 | text(beta(end),C2(end)+1,s,'HorizontalAlignment','left') 66 | case 6 67 | text(beta(end),C2(end)+1,s,'HorizontalAlignment','right') 68 | end 69 | ylim(ylimits) 70 | xlim([-1e-5 5*1e-4]) 71 | end 72 | 73 | %% test for increasing k 74 | Ct_pow_t = 6; % Ct^t; 75 | for k=[4 6 8 10] 76 | t = k/2; 77 | betaMax = ( 1/(Ct_pow_t*2^(3*k-1)) )^(1 / (k/2 - 1)) - 1e-6 % minus some numerical margin 78 | beta = linspace(0,betaMax,100000); 79 | C1 = zeros(length(beta),1); 80 | C2 = zeros(length(beta),1); 81 | for i=1:length(beta) 82 | betai = beta(i); 83 | C1(i) = C1_pow_2_over_k(betai, Ct_pow_t, k); 84 | C2(i) = C2_pow_2_over_k(betai, Ct_pow_t, k); 85 | end 86 | 87 | %% plot C1 88 | figure(3) 89 | semilogy(beta,C1,'-r','linewidth',2) 90 | ylimits = [7*1e-5 30]; 91 | hold on 92 | grid on 93 | xlabel('Outlier rate $\beta$','interpreter','latex') 94 | ylabel('$C_1(k,\beta)^{2/k}$','interpreter','latex') 95 | plot([betaMax betaMax],ylimits,'--k','linewidth',1) 96 | s = sprintf(' k = %d ', k); 97 | switch k 98 | case 4 99 | text(beta(end),C1(end)+5,s,'HorizontalAlignment','left') 100 | case 6 101 | text(beta(end),C1(end)+5,s,'HorizontalAlignment','left') 102 | case 8 103 | text(beta(end),C1(end)+5,s,'HorizontalAlignment','left') 104 | case 10 105 | text(beta(end),C1(end)+5,s,'HorizontalAlignment','right') 106 | end 107 | ylim(ylimits) 108 | xlim([-1e-4 4.5*1e-3]) 109 | 110 | %% plot C2 111 | figure(4) 112 | semilogy(beta,C2,'-b','linewidth',2) 113 | ylimits = [3*1e-5 10]; 114 | hold on 115 | grid on 116 | xlabel('Outlier rate $\beta$','interpreter','latex') 117 | ylabel('$C_2(k,\beta)^{2/k}$','interpreter','latex') 118 | plot([betaMax betaMax],ylimits,'--k','linewidth',1) 119 | ylim([10^-5 10^4]) 120 | s = sprintf(' k = %d ',k); 121 | switch k 122 | case 4 123 | text(beta(end),C2(end)+1,s,'HorizontalAlignment','left') 124 | case 6 125 | text(beta(end),C2(end)+1,s,'HorizontalAlignment','left') 126 | case 8 127 | text(beta(end),C2(end)+1,s,'HorizontalAlignment','left') 128 | case 10 129 | text(beta(end),C2(end)+1,s,'HorizontalAlignment','right') 130 | end 131 | ylim(ylimits) 132 | xlim([-1e-4 4.5*1e-3]) 133 | end 134 | 135 | function value = C1_pow_2_over_k(betai, Ct_pow_t, k) 136 | value = (betai^(k/2-1) * Ct_pow_t * 2^( 3*k-1 ) ) / ... 137 | ( 1 - betai^(k/2-1) * Ct_pow_t * 2^( 3*k-1 ) ); 138 | value = value^(2/k); 139 | end 140 | 141 | function value = C2_pow_2_over_k(betai, Ct_pow_t, k) 142 | value = ((2*betai)^(k/2-1) * ( 2^k + Ct_pow_t * 2^( 2*k ) ) ) / ... 143 | ( 1 - betai^(k/2-1) * Ct_pow_t * 2^( 3*k-1 ) ); 144 | value = value^(2/k); 145 | end -------------------------------------------------------------------------------- /experiment5_visualize_anticoncentration.m: -------------------------------------------------------------------------------- 1 | %% Visualize functions involved in certifiable anti-concentration (definition in Theorem 11 2 | %% Paper: "Estimation Contracts for Outlier-Robust Geometric Perception" 3 | %% Luca Carlone, Nov 7, 2022 4 | 5 | clc 6 | clear 7 | close all 8 | restoredefaultpath 9 | addSpecificPaths 10 | 11 | %% set parameters for Monte Carlo runs 12 | Nset = [50] % number of measurements 13 | outrateSet = [0.30]; % outlier rate 14 | nrTests = 5; % number of runs for each configuration 15 | ylimits = [-0.05 1.5]; % for visualization 16 | 17 | %% visualize certifiable anti-concentration 18 | figure(1) 19 | ax1 = axes(); 20 | for ind_N = 1:length(Nset) % general, but we test for a single N here 21 | N = Nset(ind_N); % number of measurements 22 | for ind_outrate = 1:length(outrateSet) 23 | outrate = outrateSet(ind_outrate); % outlier rate 24 | [problem, R_gt] = createWahbaProblemData(N,outrate); % only used to get the beta 25 | for test = 1:nrTests 26 | fprintf('=== test %d of %d, with N=%d, and outlier rate = %g ===\n',test,nrTests,N,outrate) 27 | % [problem, R_gt] = createWahbaProblemData(N,outrate); 28 | % N = problem.N; 29 | %inliersIds = setdiff([1:N], problem.outliersIds); % anti-concentration only involves the inliers 30 | %nrInliers = N - problem.nrOutliers; 31 | alpha = 1-outrate; % inlier rate 32 | barc = sqrt(problem.betasq); % maximum error for inliers 33 | 34 | Mx = sqrt(3); % bound on ||x|| 35 | M = 2*Mx; % bound on || x - x_gt || 36 | 37 | maxSigmaA = 1; 38 | x = linspace(0,M*maxSigmaA,1000); 39 | 40 | %% for bound to be informative 41 | % alpha * eta / 2 + 2 (1-alpha)/alpha < 2 42 | eta = (2/alpha) * (2 - 2* (1-alpha)/(alpha)) 43 | delta = 2*barc; 44 | C = alpha^2 * eta^2 * (1-2*barc)^2 / (32*barc); 45 | C_delta_Msq = C*delta*M^2; 46 | 47 | %% create polynomial 48 | plotStr = '-k'; 49 | switch test 50 | case 1 51 | c2 = -1; 52 | %plotStr = '-r'; 53 | indText = round(length(x)/4.5); 54 | xposText = 0.4; 55 | yposText = 0.1; 56 | case 2 57 | c2 = -0.3; 58 | indText = round(length(x)/2); 59 | %plotStr = '-r'; 60 | xposText = 1.55; 61 | yposText = 0.1; 62 | case 3 63 | c2 = -0.1; 64 | indText = round(length(x)/1.2); 65 | xposText = 3; 66 | yposText = 0.08; 67 | case 4 68 | c2 = -0.05; 69 | indText = round(length(x)/1.2); 70 | xposText = 2.9; 71 | yposText = 0.4; 72 | case 5 73 | c2 = -0.01; 74 | indText = round(length(x)/2); 75 | %plotStr = '-r'; 76 | xposText = 2.9; 77 | yposText = 0.9; 78 | otherwise 79 | error('exceeded maximum number of cases') 80 | end 81 | s = sprintf(' $c_2$ = %g ', c2); 82 | 83 | y = (1 + c2 * x.^2).^2; 84 | 85 | %% create upper bound 86 | for i=1:length(x) 87 | yup(i) = C_delta_Msq / x(i)^2; 88 | end 89 | 90 | %% create lower bounds 91 | for i=1:length(x) 92 | if x(i) < delta 93 | yd1(i) = (1-delta)^2; 94 | else 95 | yd1(i) = 0; 96 | end 97 | end 98 | for i=1:length(x) 99 | if x(i) < delta 100 | yd2(i) = (1+delta)^2; 101 | else 102 | yd2(i) = 2*ylimits(2); 103 | end 104 | end 105 | 106 | %% plot 107 | plot(x,yup,'-.r','linewidth',2); 108 | hold on; grid on 109 | plot(x,y,plotStr,'linewidth',2); 110 | plot(x,yd1,':k','linewidth',2); 111 | %plot(x,yd2,':k','linewidth',2); 112 | text(xposText,yposText,s,'HorizontalAlignment','left', 'Interpreter', 'latex','FontSize',16) 113 | ylim(ylimits) 114 | 115 | xlabel('$\| A_i v\|$', 'Interpreter', 'latex') 116 | ylabel('$p^2(\|A_i v\|)$ and constraints', 'Interpreter', 'latex') 117 | legend('$C \delta M^2 / \|v\|^2$','$p^2(\|A_i v\|)$','$(1 - \delta)^2$','Location', 'ne','interpreter','latex') 118 | set(gca,'fontsize', 18) 119 | end 120 | end 121 | end 122 | text(1.7,1.6,'$\|v\|$','HorizontalAlignment','left','color','r', 'Interpreter', 'latex','FontSize',18) 123 | ax2 = axes('Position', get(ax1,'Position'), ... 124 | 'XAxisLocation','top', ... 125 | 'Color','none', ... 126 | 'XColor','k'); 127 | ax2.YAxis.Visible = 'off'; 128 | ax2.XLim = ax1.XLim; 129 | ax2.XTick = ax1.XTick; 130 | ax2.XColor = 'r'; 131 | 132 | 133 | %% plot increasingly stricter thresholds 134 | % set parameters for Monte Carlo runs 135 | Nset = [50] % number of measurements 136 | outrateSet = [0.2 0.3 0.4 0.45]; % outlier rate 137 | nrTests = 1; % number of runs for each configuration 138 | ylimits = [-0.05 1.5]; % for visualization 139 | 140 | figure(2) 141 | ax1 = axes(); 142 | % generate random problems and visualize certifiable anti-contraction 143 | for ind_N = 1:length(Nset) % general, but we test for a single N here 144 | N = Nset(ind_N); % number of measurements 145 | for ind_outrate = 1:length(outrateSet) 146 | outrate = outrateSet(ind_outrate); % outlier rate 147 | [problem, R_gt] = createWahbaProblemData(N,outrate); % only used to get the beta 148 | 149 | alpha = 1-outrate; % inlier rate 150 | barc = sqrt(problem.betasq); % maximum error for inliers 151 | 152 | Mx = sqrt(3); % bound on ||x|| 153 | M = 2*Mx; % bound on || x - x_gt || 154 | 155 | maxSigmaA = 1; 156 | x = linspace(0,M*maxSigmaA,1000); 157 | 158 | %% for bound to be informative 159 | % alpha * eta / 2 + 2 (1-alpha)/alpha < 2 160 | alpha 161 | eta = (2/alpha) * (2 - 2* (1-alpha)/(alpha)) 162 | delta = 2*barc; 163 | C = alpha^2 * eta^2 * (1-2*barc)^2 / (32*barc); 164 | C_delta_Msq = C*delta*M^2; 165 | 166 | %% create polynomial 167 | plotStr = '-k'; 168 | switch ind_outrate 169 | case 1 170 | xposText = 3; 171 | yposText = 0.66; 172 | case 2 173 | xposText = 3; 174 | yposText = 0.39; 175 | case 3 176 | xposText = 3; 177 | yposText = 0.15; 178 | case 4 179 | xposText = 1.5; 180 | yposText = 0.18; 181 | otherwise 182 | error('exceeded maximum number of cases') 183 | end 184 | 185 | c2 = -0.1; 186 | indText = round(2/3*length(x)); 187 | s = sprintf(' $\\eta$ = %.2f ', eta); 188 | 189 | y = (1 + c2 * x.^2).^2; 190 | 191 | %% create upper bound 192 | for i=1:length(x) 193 | yup(i) = C_delta_Msq / x(i)^2; 194 | end 195 | 196 | %% create lower bounds 197 | for i=1:length(x) 198 | if x(i) < delta 199 | yd1(i) = 1-delta; 200 | else 201 | yd1(i) = 0; 202 | end 203 | end 204 | for i=1:length(x) 205 | if x(i) < delta 206 | yd2(i) = 1+delta; 207 | else 208 | yd2(i) = 2*ylimits(2); 209 | end 210 | end 211 | 212 | 213 | %% plot 214 | plot(x,yup,'-.r','linewidth',2); 215 | hold on; grid on 216 | plot(x,y,plotStr,'linewidth',2); 217 | plot(x,yd1,':k','linewidth',2); 218 | %plot(x,yd2,':k','linewidth',2); 219 | text(xposText,yposText,s,'HorizontalAlignment','left', 'Interpreter', 'latex','FontSize',16) 220 | ylim(ylimits) 221 | 222 | xlabel('$\| A_i v\|$', 'Interpreter', 'latex') 223 | ylabel('$p^2(\|A_i v\|)$ and constraints', 'Interpreter', 'latex') 224 | legend('$C \delta M^2 / \|v\|^2$','$p^2(\|A_i v\|)$','$(1 - \delta)^2$','Location', 'ne','interpreter','latex') 225 | set(gca,'fontsize', 18) 226 | 227 | etaSet(ind_outrate) = eta; 228 | end 229 | end 230 | text(1.7,1.6,'$\|v\|$','HorizontalAlignment','left','color','r', 'Interpreter', 'latex','FontSize',18) 231 | ax2 = axes('Position', get(ax1,'Position'), ... 232 | 'XAxisLocation','top', ... 233 | 'Color','none', ... 234 | 'XColor','k'); 235 | ax2.YAxis.Visible = 'off'; 236 | ax2.XLim = ax1.XLim; 237 | ax2.XTick = ax1.XTick; 238 | ax2.XColor = 'r'; 239 | 240 | %% plot bounds 241 | % set parameters for Monte Carlo runs 242 | N = [50] % number of measurements 243 | nrTests = 4; % number of runs for each configuration 244 | ylimits = [-0.05 1.5]; % for visualization 245 | 246 | alpha = [0.5:0.01:1]; 247 | 248 | outrate = outrateSet(ind_outrate); % outlier rate 249 | [problem, R_gt] = createWahbaProblemData(N,outrate); % only used to get the beta 250 | for etaInd = 1:length(etaSet) 251 | eta = etaSet(etaInd); 252 | fprintf('=== test %d of %d, with N=%d, and outlier rate = %g ===\n',test,nrTests,N,outrate) 253 | for i = 1:length(alpha) 254 | ai = alpha(i); 255 | b(i) = Mx * (ai * eta) / 2 + 2 * Mx * (1-ai) / ai; 256 | end 257 | 258 | switch etaInd 259 | case 1 260 | xposText = 0.93; 261 | yposText = 3.1; 262 | case 2 263 | xposText = 0.93; 264 | yposText = 2.7; 265 | case 3 266 | xposText = 0.93; 267 | yposText = 2.15; 268 | case 4 269 | xposText = 0.93; 270 | yposText = 1.4; 271 | otherwise 272 | error('exceeded maximum number of cases') 273 | end 274 | indText = round(4/5*length(alpha)); 275 | s = sprintf(' $\\eta$ = %.2f ', eta) 276 | 277 | %% plot 278 | figure(3) 279 | semilogy(alpha,b,'-b','linewidth',2) 280 | hold on 281 | semilogy([alpha],2 * Mx * ones(size(alpha)),'--r','linewidth',2) 282 | text(xposText,yposText,s,'HorizontalAlignment','left', 'Interpreter', 'latex','fontsize',16) 283 | grid on 284 | box off 285 | % xticks([0 0.2 0.4 0.5 0.6 0.8 1]) 286 | ylabel('Estimation error bounds','interpreter','latex') 287 | xlabel('Inlier rate $\alpha$','interpreter','latex') 288 | legend('proposed', 'trivial bound','interpreter','latex') 289 | ax = gca; 290 | ax.FontSize = 18; 291 | end 292 | 293 | %% visualize certifiable anti-concentration for higher degree polynomials 294 | Nset = [50] % number of measurements 295 | outrateSet = [0.45]; % outlier rate 296 | nrTests = 3; % number of runs for each configuration 297 | ylimits = [-0.05 1.5]; % for visualization 298 | degreeSet = [2 4 8]; 299 | 300 | figure(4) 301 | ax1 = axes(); 302 | for ind_N = 1:length(Nset) % general, but we test for a single N here 303 | N = Nset(ind_N); % number of measurements 304 | for ind_outrate = 1:length(outrateSet) 305 | outrate = outrateSet(ind_outrate); % outlier rate 306 | [problem, R_gt] = createWahbaProblemData(N,outrate); % only used to get the beta 307 | for test = 1:nrTests 308 | degree = degreeSet(test) 309 | fprintf('=== test %d of %d, with N=%d, and outlier rate = %g ===\n',test,nrTests,N,outrate) 310 | alpha = 1-outrate; % inlier rate 311 | barc = sqrt(problem.betasq); % maximum error for inliers 312 | 313 | Mx = sqrt(3); % bound on ||x|| 314 | M = 2*Mx; % bound on || x - x_gt || 315 | 316 | maxSigmaA = 1; 317 | x = linspace(0,M*maxSigmaA,1000); 318 | 319 | %% for bound to be informative 320 | eta = (2/alpha) * (2 - 2* (1-alpha)/(alpha)) 321 | delta = 2*barc; 322 | C = alpha^2 * eta^2 * (1-2*barc)^2 / (32*barc); 323 | C_delta_Msq = C*delta*M^2; 324 | 325 | %% fit coefficients: 326 | xplot= [0:0.001:1.2*M]; 327 | yindicator = (abs(xplot)<=delta); 328 | nrPoints = length(xplot); 329 | 330 | %% degree 331 | 332 | A = zeros(nrPoints,1); 333 | b = yindicator(:)-ones(nrPoints,1); 334 | for i=1:nrPoints 335 | for d = 2:2:degree 336 | A(i,d/2) = [xplot(i)^(d)]; 337 | end 338 | end 339 | c = inv(A' * A) * A' * b 340 | 341 | %% create polynomial 342 | plotStr = '-k'; 343 | switch test 344 | case 1 345 | y = (1 + c(1) * x.^2).^2; 346 | xposText = 1.7; 347 | yposText = 0.55; 348 | 349 | case 2 350 | y = (1 + c(1) * x.^2 + c(2) * x.^4).^2; 351 | indText = round(length(x)/2); 352 | %plotStr = '-r'; 353 | xposText = 1.25; 354 | yposText = 0.4; 355 | case 3 356 | y = (1 + c(1) * x.^2 + c(2) * x.^4 + c(3) * x.^6 + c(4) * x.^8).^2; 357 | xposText = 0.38; 358 | yposText = 0.2; 359 | otherwise 360 | error('exceeded maximum number of cases') 361 | end 362 | s = sprintf(' deg = %d ', degree); 363 | 364 | %% create upper bound 365 | for i=1:length(x) 366 | yup(i) = C_delta_Msq / x(i)^2; 367 | end 368 | 369 | %% create lower bounds 370 | for i=1:length(x) 371 | if x(i) < delta 372 | yd1(i) = (1-delta)^2; 373 | else 374 | yd1(i) = 0; 375 | end 376 | end 377 | for i=1:length(x) 378 | if x(i) < delta 379 | yd2(i) = (1+delta)^2; 380 | else 381 | yd2(i) = 2*ylimits(2); 382 | end 383 | end 384 | 385 | %% plot 386 | plot(x,yup,'-.r','linewidth',2); 387 | hold on; grid on 388 | plot(x,y,plotStr,'linewidth',2); 389 | plot(x,yd1,':k','linewidth',2); 390 | %plot(x,yd2,':k','linewidth',2); 391 | text(xposText,yposText,s,'HorizontalAlignment','left', 'Interpreter', 'latex','FontSize',16) 392 | ylim(ylimits) 393 | 394 | xlabel('$\| A_i v\|$', 'Interpreter', 'latex') 395 | ylabel('$p^2(\|A_i v\|)$ and constraints', 'Interpreter', 'latex') 396 | legend('$C \delta M^2 / \|v\|^2$','$p^2(\|A_i v\|)$','$(1 - \delta)^2$','Location', 'ne','interpreter','latex') 397 | set(gca,'fontsize', 18) 398 | end 399 | end 400 | end 401 | text(1.7,1.6,'$\|v\|$','HorizontalAlignment','left','color','r', 'Interpreter', 'latex','FontSize',18) 402 | ax2 = axes('Position', get(ax1,'Position'), ... 403 | 'XAxisLocation','top', ... 404 | 'Color','none', ... 405 | 'XColor','k'); 406 | ax2.YAxis.Visible = 'off'; 407 | ax2.XLim = ax1.XLim; 408 | ax2.XTick = ax1.XTick; 409 | ax2.XColor = 'r'; 410 | -------------------------------------------------------------------------------- /experiment6_check_anticoncentration_synthetic_vs_n.m: -------------------------------------------------------------------------------- 1 | %% Test Certifiable Anti-Concentration in robust rotation search (Wahba Problem) 2 | %% Synthetic data 3 | %% Paper: "Estimation Contracts for Outlier-Robust Geometric Perception" 4 | %% Luca Carlone, Nov 7, 2022 5 | %% example data from: https://github.com/MIT-SPARK/CertifiablyRobustPerception/ 6 | 7 | clc 8 | clear 9 | close all 10 | restoredefaultpath 11 | addSpecificPaths 12 | 13 | %% set parameters for Monte Carlo runs 14 | Nset = [10 50 100 200]; % number of measurements 15 | outrateSet = [0 0.1 0.3 0.5 0.7 0.9]; % outlier rate 16 | nrTests = 10; % number of runs for each configuration 17 | isModifiedWahba = 0; 18 | 19 | alphaBar = 0.8; 20 | eta = (2/alphaBar) * (2 - 2* (1-alphaBar)/(alphaBar)) % smallest eta such that bound is informative 21 | 22 | %% matrix storing results 23 | results = zeros(length(Nset),length(outrateSet), nrTests); 24 | runtime1 = zeros(length(Nset),length(outrateSet), nrTests); 25 | runtime2 = zeros(length(Nset),length(outrateSet), nrTests); 26 | 27 | %% generate random problems and test anti-concentration 28 | for N_ind = 1:length(Nset) 29 | N = Nset(N_ind); % number of measurements 30 | for outrate_ind = 1:length(outrateSet) 31 | outrate = outrateSet(outrate_ind); % outlier rate 32 | nrAntiConcentrated = 0; 33 | for test = 1:nrTests 34 | fprintf('=== test %d of %d, with N=%d, and outlier rate = %g ===\n',test,nrTests,N,outrate) 35 | [problem, R_gt] = createWahbaProblemData(N,outrate); 36 | problem.outrate = outrate; 37 | [isAntiConcentrated,timeTest1,timeTest2] = testAntiConcentration(problem,eta,isModifiedWahba); 38 | 39 | if isAntiConcentrated 40 | nrAntiConcentrated = nrAntiConcentrated + 1; 41 | end 42 | 43 | results(N_ind,outrate_ind,test) = isAntiConcentrated; 44 | runtime1(N_ind,outrate_ind,test) = timeTest1; 45 | runtime2(N_ind,outrate_ind,test) = timeTest2; 46 | fprintf('================ %d sos out of %d ====================\n',nrAntiConcentrated,test) 47 | end 48 | end 49 | end 50 | 51 | save log_results_experiment6 52 | 53 | %% plot/display results 54 | resultsPercentage = 100*sum(results,3)/nrTests; 55 | aveRuntime1 = mean(runtime1,3); 56 | aveRuntime2 = mean(runtime2,3); 57 | 58 | aveAveRuntime1 = mean(runtime1(:)) % overall average 59 | aveAveRuntime2 = mean(runtime2(:)) % overall average 60 | 61 | figure 62 | h = heatmap(outrateSet,Nset,resultsPercentage); 63 | % h.Title = 'Hypercontractive instances'; 64 | h.XLabel = 'Outlier rate ($\beta$)'; 65 | h.YLabel = 'Nr. measurements ($n$)'; 66 | h.CellLabelFormat = '%.2f'; 67 | h.NodeChildren(3).XAxis.Label.Interpreter = 'latex'; 68 | h.NodeChildren(3).YAxis.Label.Interpreter = 'latex'; 69 | h.NodeChildren(3).Title.Interpreter = 'latex'; 70 | h.FontSize = 18; 71 | 72 | %% timing figure 73 | figure 74 | h = heatmap(outrateSet,Nset,aveRuntime1); 75 | %h.Title = 'Average runtime'; 76 | h.XLabel = 'Outlier rate $\beta$'; 77 | h.YLabel = 'Nr. measurements ($n$)'; 78 | h.CellLabelFormat = '%.2f'; 79 | h.NodeChildren(3).XAxis.Label.Interpreter = 'latex'; 80 | h.NodeChildren(3).YAxis.Label.Interpreter = 'latex'; 81 | h.NodeChildren(3).Title.Interpreter = 'latex'; 82 | h.FontSize = 18; 83 | 84 | figure 85 | h = heatmap(outrateSet,Nset,aveRuntime2); 86 | %h.Title = 'Average runtime'; 87 | h.XLabel = 'Outlier rate $\beta$'; 88 | h.YLabel = 'Nr. measurements ($n$)'; 89 | h.CellLabelFormat = '%.2f'; 90 | h.NodeChildren(3).XAxis.Label.Interpreter = 'latex'; 91 | h.NodeChildren(3).YAxis.Label.Interpreter = 'latex'; 92 | h.NodeChildren(3).Title.Interpreter = 'latex'; 93 | h.FontSize = 18; 94 | 95 | 96 | -------------------------------------------------------------------------------- /experiment7_check_anticoncentration_synthetic_vs_eta.m: -------------------------------------------------------------------------------- 1 | %% Test Certifiable Anti-Concentration in robust rotation search (Wahba Problem) 2 | %% Synthetic data 3 | %% Paper: "Estimation Contracts for Outlier-Robust Geometric Perception" 4 | %% Luca Carlone, Nov 7, 2022 5 | %% example data from: https://github.com/MIT-SPARK/CertifiablyRobustPerception/ 6 | 7 | clc 8 | clear 9 | close all 10 | restoredefaultpath 11 | addSpecificPaths 12 | 13 | %% set parameters for Monte Carlo runs 14 | N = 50; 15 | alphaBarSet = [0.55 0.6 0.7 0.8]; % inlier rate for outrateSet = [0.2 0.3 0.4 0.45]; % outlier rate 16 | for i=1:length(alphaBarSet) 17 | alphaBar = alphaBarSet(i); 18 | etaSet(i) = (2/alphaBar) * (2 - 2* (1-alphaBar)/(alphaBar)) % smallest eta such that bound is informative 19 | end 20 | outrateSet = [0 0.1 0.3 0.5 0.7 0.9]; % outlier rate 21 | nrTests = 10; % number of runs for each configuration 22 | isModifiedWahba = 0; 23 | 24 | %% matrix storing results 25 | results = zeros(length(etaSet),length(outrateSet), nrTests); 26 | runtime1 = zeros(length(etaSet),length(outrateSet), nrTests); 27 | runtime2 = zeros(length(etaSet),length(outrateSet), nrTests); 28 | 29 | %% generate random problems and test anti-concentration 30 | for eta_ind = 1:length(etaSet) 31 | eta = etaSet(eta_ind); % number of measurements 32 | for outrate_ind = 1:length(outrateSet) 33 | outrate = outrateSet(outrate_ind); % outlier rate 34 | nrAntiConcentrated = 0; 35 | for test = 1:nrTests 36 | fprintf('=== test %d of %d, with eta=%g, and outlier rate = %g ===\n',test,nrTests,eta,outrate) 37 | [problem, R_gt] = createWahbaProblemData(N,outrate); 38 | problem.outrate = outrate; 39 | [isAntiConcentrated,timeTest1,timeTest2] = testAntiConcentration(problem,eta,isModifiedWahba); 40 | 41 | % log results 42 | results(eta_ind,outrate_ind,test) = isAntiConcentrated; 43 | runtime1(eta_ind,outrate_ind,test) = timeTest1; 44 | runtime2(eta_ind,outrate_ind,test) = timeTest2; 45 | 46 | % display partial results 47 | if isAntiConcentrated 48 | nrAntiConcentrated = nrAntiConcentrated + 1; 49 | end 50 | fprintf('================ %d sos out of %d ====================\n',nrAntiConcentrated,test) 51 | end 52 | end 53 | end 54 | 55 | %% plot/display results 56 | resultsPercentage = 100*sum(results,3)/nrTests; 57 | aveRuntime1 = mean(runtime1,3); 58 | aveRuntime2 = mean(runtime2,3); 59 | 60 | aveAveRuntime1 = mean(runtime1(:)) % overall average 61 | aveAveRuntime2 = mean(runtime2(:)) % overall average 62 | 63 | save log_results_experiment7_isModified_0 64 | 65 | figure 66 | h = heatmap(outrateSet,etaSet,resultsPercentage); 67 | % h.Title = 'Hypercontractive instances'; 68 | h.XLabel = 'Outlier rate ($\beta$)'; 69 | h.YLabel = '$\eta$'; 70 | h.YDisplayLabels = compose('%.2f',str2double(h.YDisplayLabels)); 71 | h.CellLabelFormat = '%.2f'; 72 | h.NodeChildren(3).XAxis.Label.Interpreter = 'latex'; 73 | h.NodeChildren(3).YAxis.Label.Interpreter = 'latex'; 74 | h.NodeChildren(3).Title.Interpreter = 'latex'; 75 | h.FontSize = 18; 76 | 77 | %% timing figure 78 | figure 79 | h = heatmap(outrateSet,etaSet,aveRuntime1); 80 | %h.Title = 'Average runtime'; 81 | h.XLabel = 'Outlier rate $\beta$'; 82 | h.YLabel = '$\eta$'; 83 | h.YDisplayLabels = compose('%.2f',str2double(h.YDisplayLabels)); 84 | h.CellLabelFormat = '%.2f'; 85 | h.NodeChildren(3).XAxis.Label.Interpreter = 'latex'; 86 | h.NodeChildren(3).YAxis.Label.Interpreter = 'latex'; 87 | h.NodeChildren(3).Title.Interpreter = 'latex'; 88 | h.FontSize = 18; 89 | 90 | figure 91 | h = heatmap(outrateSet,etaSet,aveRuntime2); 92 | %h.Title = 'Average runtime'; 93 | h.XLabel = 'Outlier rate $\beta$'; 94 | h.YLabel = '$\eta$'; 95 | h.YDisplayLabels = compose('%.2f',str2double(h.YDisplayLabels)); 96 | h.CellLabelFormat = '%.2f'; 97 | h.NodeChildren(3).XAxis.Label.Interpreter = 'latex'; 98 | h.NodeChildren(3).YAxis.Label.Interpreter = 'latex'; 99 | h.NodeChildren(3).Title.Interpreter = 'latex'; 100 | h.FontSize = 18; 101 | 102 | 103 | -------------------------------------------------------------------------------- /experiment8_slide_synthetic.m: -------------------------------------------------------------------------------- 1 | %% Test list decodable regression in robust rotation search (Wahba Problem) 2 | %% Synthetic data 3 | %% Paper: "Estimation Contracts for Outlier-Robust Geometric Perception" 4 | %% Luca Carlone, Nov 7, 2022 5 | %% example data from: https://github.com/MIT-SPARK/CertifiablyRobustPerception/ 6 | 7 | clc 8 | clear 9 | close all 10 | restoredefaultpath 11 | addSpecificPaths 12 | 13 | %% set parameters for Monte Carlo runs 14 | Nset = [50] % number of measurements 15 | outrateSet = [0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9]; % outlier rate 16 | nrTests = 10; % number of runs for each configuration 17 | isAdversarial = 0; recoverAllHypotheses = 0; % Fig. 9 18 | % isAdversarial = 1; recoverAllHypotheses = 0; % Fig. 10 19 | % isAdversarial = 1; recoverAllHypotheses = 1; % Fig. 11 20 | doPlotRotations = 0; 21 | 22 | %% parameters to visualize rotations out of SLIDE (Fig. 12) 23 | % Nset = [50] % number of measurements 24 | % outrateSet = 0.8 % [0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1.0]; % outlier rate 25 | % nrTests = 1; % number of runs for each configuration 26 | % isAdversarial = 3; 27 | % doPlotRotations = 1; 28 | 29 | %% allocate matrix to store results 30 | results_TLS_R_err_ang_deg_obj1 = zeros(length(Nset),length(outrateSet),nrTests); 31 | results_TLS_R_err_diff_norm_obj1 = zeros(length(Nset),length(outrateSet),nrTests); 32 | results_TLS_eta = zeros(length(Nset),length(outrateSet),nrTests); 33 | results_TLS_R_err_ang_deg_obj2 = zeros(length(Nset),length(outrateSet),nrTests); 34 | results_TLS_R_err_diff_norm_obj2 = zeros(length(Nset),length(outrateSet),nrTests); 35 | results_TLS_fsdp = zeros(length(Nset),length(outrateSet),nrTests); 36 | 37 | results_SLIDE_R_err_ang_deg_obj1 = zeros(length(Nset),length(outrateSet),nrTests); 38 | results_SLIDE_R_err_diff_norm_obj1 = zeros(length(Nset),length(outrateSet),nrTests); 39 | results_SLIDE_eta_norm = zeros(length(Nset),length(outrateSet),nrTests); 40 | results_SLIDE_eta = zeros(length(Nset),length(outrateSet),nrTests); 41 | results_SLIDE_R_err_ang_deg_obj2 = zeros(length(Nset),length(outrateSet),nrTests); 42 | results_SLIDE_R_err_diff_norm_obj2 = zeros(length(Nset),length(outrateSet),nrTests); 43 | results_SLIDE_fsdp = zeros(length(Nset),length(outrateSet),nrTests); 44 | results_SLIDE_time = zeros(length(Nset),length(outrateSet),nrTests); 45 | 46 | %% generate random problems and test certifiable hyper-contractivity 47 | for ind_N = 1:length(Nset) % general, but we test for a single N here 48 | N = Nset(ind_N); % number of measurements 49 | for ind_outrate = 1:length(outrateSet) 50 | outrate = outrateSet(ind_outrate); % outlier rate 51 | for test = 1:nrTests 52 | fprintf('=== test %d of %d, with N=%d, and outlier rate = %g ===\n',test,nrTests,N,outrate) 53 | [problem, R_gt, R_gt_list] = createWahbaProblemData(N,outrate,isAdversarial); 54 | 55 | if doPlotRotations==1 56 | for i=1:size(R_gt_list,3) 57 | my_ref_frame(R_gt_list(:,:,i),zeros(3,1),['r','g','b'],0.75,0.05,0.5) 58 | end 59 | end 60 | 61 | %% Different TLS formulation, for testing 62 | solveWahba_TLS_m1p1 63 | 64 | %% store results single hypotheses 65 | results_TLS_fsdp(ind_N,ind_outrate,test) = f_sdp; 66 | results_TLS_R_err_ang_deg_obj1(ind_N,ind_outrate,test) = R_err; 67 | R_err_diff_norm = norm( R_gt(:) - R_est(:) ); 68 | results_TLS_R_err_diff_norm_obj1(ind_N,ind_outrate,test) = R_err_diff_norm; 69 | results_TLS_eta(ind_N,ind_outrate,test) = eta; 70 | fprintf('+++ eta=%g +++\n',eta) 71 | fprintf('+++ TLS obj 1: rot err =%.3g [deg], norm error=%.3g +++\n',R_err,R_err_diff_norm) 72 | if isAdversarial > 0 % compute error for object 2 73 | for j=2:size(R_gt_list,3) 74 | R_err = getAngularError(quat2rot(q),R_gt_list(:,:,j)); 75 | results_TLS_R_err_ang_deg_obj2(ind_N,ind_outrate,test) = R_err; 76 | R_err_diff_norm = norm( vec(quat2rot(q)) - vec(R_gt_list(:,:,j)) ); 77 | if j == 2 % we only log the second one, but display all 78 | results_TLS_R_err_diff_norm_obj2(ind_N,ind_outrate,test) = R_err_diff_norm; 79 | end 80 | fprintf('+++ TLS obj %d: rot err =%.3g [deg], norm error=%.3g +++\n',j, R_err,R_err_diff_norm) 81 | end 82 | end 83 | clear eta R_err q R_est R_err_diff_norm 84 | fprintf('========================================\n') 85 | 86 | %% List decodable 87 | solveWahba_listDecodable_01 88 | 89 | %% store results list decodable 90 | results_SLIDE_time(ind_N,ind_outrate,test) = timeSLIDE; 91 | results_SLIDE_fsdp(ind_N,ind_outrate,test) = f_sdp; 92 | results_SLIDE_R_err_ang_deg_obj1(ind_N,ind_outrate,test) = R_err; 93 | R_err_diff_norm = norm( R_gt(:) - R_est(:) ); 94 | results_SLIDE_R_err_diff_norm_obj1(ind_N,ind_outrate,test) = R_err_diff_norm; 95 | if R_err_diff_norm > 2*sqrt(3) 96 | R_err_diff_norm 97 | error('error exceeds trivial upper bound?') 98 | end 99 | results_SLIDE_eta_norm(ind_N,ind_outrate,test) = eta_norm; 100 | results_SLIDE_eta(ind_N,ind_outrate,test) = eta; 101 | fprintf('+++ eta=%g +++\n',eta) 102 | fprintf('+++ SLIDE obj 1: rot err =%.3g [deg], norm error=%.3g +++\n',R_err,R_err_diff_norm) 103 | if isAdversarial > 0 104 | assert(isnan(R_err) || abs( R_err - min(R_err_rot_deg_list(:,1)) )<1e-7 ) 105 | results_SLIDE_R_err_ang_deg_obj2(ind_N,ind_outrate,test) = min(R_err_rot_deg_list(:,2)); 106 | results_SLIDE_R_err_diff_norm_obj2(ind_N,ind_outrate,test) = min(R_err_norm_list(:,2)); 107 | fprintf('+++ SLIDE obj 2: rot err =%.3g [deg], norm error=%.3g +++\n',min(R_err_rot_deg_list(:,2)),min(R_err_norm_list(:,2))) 108 | end 109 | 110 | if doPlotRotations==1 111 | plotRotations(R_gt_list, R_est_list) 112 | end 113 | clear eta R_err q R_est R_err_diff_norm R_err_list v_list q_list R_est_list X 114 | end 115 | end 116 | end 117 | 118 | clear image1 image2 image1Col image2Col panorama panoramaView warpedImage matchedPtsImg1 matchedPtsImg2 blender imgCell stackedImage mask I % to make file compact 119 | save log_results_experiment8_isAdversarial_0_recoverAll_0 120 | 121 | %% plots 122 | s = [1:length(outrateSet)]; 123 | outrateSet = outrateSet(s); 124 | margin = 0.02; 125 | 126 | results_TLS_R_err_ang_deg_obj1 = squeeze(results_TLS_R_err_ang_deg_obj1); 127 | results_TLS_R_err_ang_deg_obj1 = results_TLS_R_err_ang_deg_obj1(s,:); 128 | results_TLS_R_err_diff_norm_obj1 = squeeze(results_TLS_R_err_diff_norm_obj1); 129 | results_TLS_R_err_diff_norm_obj1 = results_TLS_R_err_diff_norm_obj1(s,:); 130 | results_TLS_eta = squeeze(results_TLS_eta); 131 | results_TLS_eta = results_TLS_eta(s,:); 132 | results_TLS_R_err_ang_deg_obj2 = squeeze(results_TLS_R_err_ang_deg_obj2); 133 | results_TLS_R_err_ang_deg_obj2 = results_TLS_R_err_ang_deg_obj2(s,:); 134 | results_TLS_R_err_diff_norm_obj2 = squeeze(results_TLS_R_err_diff_norm_obj2); 135 | results_TLS_R_err_diff_norm_obj2 = results_TLS_R_err_diff_norm_obj2(s,:); 136 | 137 | results_SLIDE_R_err_ang_deg_obj1 = squeeze(results_SLIDE_R_err_ang_deg_obj1); 138 | results_SLIDE_R_err_ang_deg_obj1 = results_SLIDE_R_err_ang_deg_obj1(s,:); 139 | results_SLIDE_R_err_diff_norm_obj1 = squeeze(results_SLIDE_R_err_diff_norm_obj1); 140 | results_SLIDE_R_err_diff_norm_obj1 = results_SLIDE_R_err_diff_norm_obj1(s,:); 141 | results_SLIDE_eta = squeeze(results_SLIDE_eta); 142 | results_SLIDE_eta = results_SLIDE_eta(s,:); 143 | results_SLIDE_R_err_ang_deg_obj2 = squeeze(results_SLIDE_R_err_ang_deg_obj2); 144 | results_SLIDE_R_err_ang_deg_obj2 = results_SLIDE_R_err_ang_deg_obj2(s,:); 145 | results_SLIDE_R_err_diff_norm_obj2 = squeeze(results_SLIDE_R_err_diff_norm_obj2); 146 | results_SLIDE_R_err_diff_norm_obj2 = results_SLIDE_R_err_diff_norm_obj2(s,:); 147 | results_SLIDE_time = squeeze(results_SLIDE_time); 148 | 149 | % plot errors (norm of difference) 150 | figure 151 | plot(outrateSet, mean(results_TLS_R_err_diff_norm_obj1,2) ,'-b','linewidth',4); 152 | hold on; grid on 153 | plot(outrateSet, mean(results_SLIDE_R_err_diff_norm_obj1,2) ,'-r','linewidth',4); 154 | plot_distribution_prctile(outrateSet,results_TLS_R_err_diff_norm_obj1','color',[0 0 1],'Prctile',[25 50 75 90]); 155 | plot_distribution_prctile(outrateSet,results_SLIDE_R_err_diff_norm_obj1','color',[1 0 0],'Prctile',[25 50 75 90]); 156 | plot(outrateSet, mean(results_TLS_R_err_diff_norm_obj1,2) ,'-b','linewidth',4); 157 | plot(outrateSet, mean(results_SLIDE_R_err_diff_norm_obj1,2) ,'-r','linewidth',4); 158 | % plot(outrateSet,2*sqrt(3) * ones(size(outrateSet)),'-.k','linewidth',4); 159 | xlabel('Outlier rate ($\beta$)','interpreter','latex') 160 | ylabel('Estimation error ($\|x-x^\circ\|_2$)','interpreter','latex') 161 | % legend('TLS','SLIDE','trivial bound','Location', 'Best') 162 | legend('TLS','SLIDE','Location', 'northwest') 163 | set(gca,'fontsize', 18) 164 | xlim([outrateSet(1)-margin outrateSet(end)+margin]) 165 | yl = ylim; ylim([-0.05*(yl(2)-yl(1)) yl(2)]) 166 | 167 | % plot errors (rotation errors) 168 | figure 169 | plot(outrateSet, mean(results_TLS_R_err_ang_deg_obj1,2) ,'-b','linewidth',4); 170 | hold on; grid on 171 | plot(outrateSet, mean(results_SLIDE_R_err_ang_deg_obj1,2) ,'-r','linewidth',4); 172 | plot_distribution_prctile(outrateSet,results_TLS_R_err_ang_deg_obj1','color',[0 0 1],'Prctile',[25 50 75 90]); 173 | plot_distribution_prctile(outrateSet,results_SLIDE_R_err_ang_deg_obj1','color',[1 0 0],'Prctile',[25 50 75 90]); 174 | plot(outrateSet, mean(results_TLS_R_err_ang_deg_obj1,2) ,'-b','linewidth',4); 175 | plot(outrateSet, mean(results_SLIDE_R_err_ang_deg_obj1,2) ,'-r','linewidth',4); 176 | xlabel('Outlier rate ($\beta$)','interpreter','latex') 177 | ylabel('Rotation error [deg]','interpreter','latex') 178 | legend('TLS','SLIDE','Location', 'northwest') 179 | set(gca,'fontsize', 18) 180 | xlim([outrateSet(1)-margin outrateSet(end)+margin]) 181 | yl = ylim; ylim([-0.05*(yl(2)-yl(1)) yl(2)]) 182 | 183 | % relaxation gap 184 | figure 185 | semilogy(outrateSet, mean(results_TLS_eta,2) ,'-b','linewidth',4); 186 | hold on; 187 | semilogy(outrateSet, mean(results_SLIDE_eta,2) ,'-r','linewidth',4); 188 | plot_distribution_prctile(outrateSet,results_TLS_eta','color',[0 0 1],'Prctile',[25 50 75 90]); 189 | plot_distribution_prctile(outrateSet,results_SLIDE_eta','color',[1 0 0],'Prctile',[25 50 75 90]); 190 | semilogy(outrateSet, mean(results_TLS_eta,2) ,'-b','linewidth',4); 191 | semilogy(outrateSet, mean(results_SLIDE_eta,2) ,'-r','linewidth',4); 192 | xlabel('Outlier rate ($\beta$)','interpreter','latex') 193 | ylabel('Relaxation gap','interpreter','latex') 194 | legend('TLS','SLIDE','Location', 'Best') 195 | set(gca,'fontsize', 18) 196 | grid on 197 | xlim([outrateSet(1)-margin outrateSet(end)+margin]) 198 | 199 | if isAdversarial 200 | if exist('recoverAllHypotheses','var') ==0 || recoverAllHypotheses == 0 201 | % plot errors (rotation errors) for each object 202 | figure 203 | plot(outrateSet, mean(results_TLS_R_err_ang_deg_obj1,2) ,'-b','linewidth',4); 204 | hold on; grid on 205 | plot(outrateSet, mean(results_TLS_R_err_ang_deg_obj2,2) ,':b','linewidth',4); 206 | plot(outrateSet, mean(results_SLIDE_R_err_ang_deg_obj1,2) ,'-r','linewidth',4); 207 | plot(outrateSet, mean(results_SLIDE_R_err_ang_deg_obj2,2) ,':r','linewidth',4); 208 | plot_distribution_prctile(outrateSet(5:end),results_SLIDE_R_err_ang_deg_obj2(5:end,:)','color',[1 0 0],'Prctile',[25 50 75 90]); 209 | plot_distribution_prctile(outrateSet(1:5),results_SLIDE_R_err_ang_deg_obj2(1:5,:)','color',[0.5 0.5 0.5],'Prctile',[25 50 75 90]); 210 | plot(outrateSet, mean(results_TLS_R_err_ang_deg_obj1,2) ,'-b','linewidth',4); 211 | plot(outrateSet, mean(results_TLS_R_err_ang_deg_obj2,2) ,':b','linewidth',4); 212 | plot(outrateSet, mean(results_SLIDE_R_err_ang_deg_obj1,2) ,'-r','linewidth',4); 213 | plot(outrateSet, mean(results_SLIDE_R_err_ang_deg_obj2,2) ,':r','linewidth',4); 214 | xlabel('Outlier rate ($\beta$)','interpreter','latex') 215 | ylabel('Rotation error [deg]','interpreter','latex') 216 | legend('TLS - rotation 1','TLS - rotation 2','SLIDE - rotation 1','SLIDE - rotation 2','Location', 'best') 217 | set(gca,'fontsize', 18) 218 | xticks([0.1:0.1:0.9]) 219 | xlim([outrateSet(1)-margin outrateSet(end)+margin]) 220 | yl = ylim; ylim([-0.05*(yl(2)-yl(1)) yl(2)]) 221 | else 222 | % plot errors (rotation errors) for each object 223 | figure 224 | plot(outrateSet, mean(results_TLS_R_err_ang_deg_obj1,2) ,'-b','linewidth',4); 225 | hold on; grid on 226 | plot(outrateSet, mean(results_TLS_R_err_ang_deg_obj2,2) ,':b','linewidth',4); 227 | plot(outrateSet, mean(results_SLIDE_R_err_ang_deg_obj1,2) ,'-r','linewidth',4); 228 | plot(outrateSet, mean(results_SLIDE_R_err_ang_deg_obj2,2) ,':r','linewidth',4); 229 | plot_distribution_prctile(outrateSet(5:end),results_SLIDE_R_err_ang_deg_obj2(5:end,:)','color',[1 0 0],'Prctile',[25 50 75 90]); 230 | plot_distribution_prctile(outrateSet(1:5),results_SLIDE_R_err_ang_deg_obj2(1:5,:)','color',[1 0 0],'Prctile',[25 50 75 90]); 231 | %plot_distribution_prctile(outrateSet,results_SLIDE_R_err_ang_deg_obj1','color',[1 0 0],'Prctile',[25 50 75 90]); 232 | plot(outrateSet, mean(results_TLS_R_err_ang_deg_obj1,2) ,'-b','linewidth',4); 233 | plot(outrateSet, mean(results_TLS_R_err_ang_deg_obj2,2) ,':b','linewidth',4); 234 | plot(outrateSet, mean(results_SLIDE_R_err_ang_deg_obj1,2) ,'-r','linewidth',4); 235 | plot(outrateSet, mean(results_SLIDE_R_err_ang_deg_obj2,2) ,':r','linewidth',4); 236 | xlabel('Outlier rate ($\beta$)','interpreter','latex') 237 | ylabel('Rotation error [deg]','interpreter','latex') 238 | legend('TLS - rotation 1','TLS - rotation 2','SLIDE - rotation 1','SLIDE - rotation 2','Location', 'best') 239 | set(gca,'fontsize', 18) 240 | xticks([0.1:0.1:0.9]) 241 | xlim([outrateSet(1)-margin outrateSet(end)+margin]) 242 | yl = ylim; ylim([-0.05*(yl(2)-yl(1)) yl(2)]) 243 | end 244 | end 245 | -------------------------------------------------------------------------------- /experiment9_check_anticoncentration_real.m: -------------------------------------------------------------------------------- 1 | %% Test Certifiable Hypercontractivity in robust rotation search (Wahba Problem) 2 | %% Real data from image stitching application 3 | %% Paper: "Estimation Contracts for Outlier-Robust Geometric Perception" 4 | %% Luca Carlone, Nov 7, 2022 5 | %% example data provided by Heng Yang (same as the one used in the QUASAR paper: https://arxiv.org/pdf/1905.12536.pdf) 6 | 7 | clc 8 | clear 9 | close all 10 | restoredefaultpath 11 | addSpecificPaths 12 | 13 | %% name of mat file containing images and feature data 14 | dataFileNames = {... 15 | 'data/stitching_1_7.mat',... 16 | 'data/stitching_7_13.mat',... %-x 17 | 'data/stitching_13_19.mat',... 18 | 'data/stitching_19_25.mat',... 19 | 'data/stitching_25_31.mat',... 20 | 'data/stitching_31_37.mat',... 21 | 'data/stitching_37_43.mat',... %-x 22 | 'data/stitching_43_49.mat',... %-x 23 | 'data/stitching_49_55.mat',... 24 | 'data/stitching_55_61.mat',...%-x 25 | 'data/stitching_61_67.mat',... 26 | 'data/stitching_67_1.mat'} %-xx 27 | 28 | alphaBarSet = [0.55 0.6 0.7 0.8]; % inlier rate for outrateSet = [0.2 0.3 0.4 0.45]; % outlier rate 29 | for i=1:length(alphaBarSet) 30 | alphaBar = alphaBarSet(i); 31 | etaSet(i) = (2/alphaBar) * (2 - 2* (1-alphaBar)/(alphaBar)) % smallest eta such that bound is informative 32 | end 33 | 34 | isAnticoncentrated_results = zeros(length(dataFileNames),length(etaSet)); 35 | timeTest1_results = zeros(length(dataFileNames),length(etaSet)); 36 | timeTest2_results = zeros(length(dataFileNames),length(etaSet)); 37 | eta_results = zeros(length(dataFileNames),1); 38 | 39 | for fileInd=1:length(dataFileNames) 40 | dataFileName = dataFileNames{fileInd} 41 | 42 | %% parse images to get real data 43 | [problem, R_gt, theta_gt] = createImageStitchingData(dataFileName); 44 | 45 | %% contrarily to experiment1, where we know the ground truth inliers, here we estimate them 46 | [R_stride,theta_stride,eta] = solveWahba(problem.a,problem.b,problem.betasq,... 47 | R_gt,... % R_gt only used for performance evaluation 48 | sdpnalpath,manoptpath,stridepath); % paths to solvers 49 | 50 | %% build inliers/outliers (estimated from real data this time) 51 | problem.outliersIds = find(theta_stride < 0); 52 | 53 | %% check results against QUASAR results (marked as "gt") 54 | theta_diff = abs(theta_stride - theta_gt); 55 | diffOutliers = sum(theta_diff>0); % Sdp formulation is slightly different from QUASAR,.. 56 | % ... but number of inliers should roughly match (+/-1) 57 | R_err = R_gt' * R_stride; 58 | axang = rotm2axang(R_err); 59 | angleErrDeg = rad2deg(axang(4)); % small, as expected (seems rotm2axang approximates to zero) 60 | fprintf('diffOutliers = %d, rotError[deg] = %g\n',diffOutliers,angleErrDeg) 61 | 62 | %% visualize image stitching results 63 | % plotImageStitchingResults 64 | 65 | %% store tightness 66 | eta_results(fileInd) = eta; 67 | 68 | %% use outliers from stride 69 | problem.outliersIds = find(theta_stride < 0); 70 | problem.nrOutliers = length(problem.outliersIds); 71 | problem.outrate = problem.nrOutliers / problem.N 72 | 73 | for j = 1:length(etaSet) 74 | %% test hypercontractivity 75 | eta = etaSet(j); 76 | [isAntiConcentrated,timeTest1,timeTest2] = testAntiConcentration(problem,eta); 77 | isAnticoncentrated_results(fileInd,j) = isAntiConcentrated; 78 | timeTest1_results(fileInd,j) = timeTest1; 79 | timeTest2_results(fileInd,j) = timeTest2; 80 | end 81 | isAnticoncentrated_results 82 | timeTest1_results 83 | timeTest2_results 84 | eta_results 85 | end 86 | 87 | % save log_results_experiment8 88 | 89 | %% print results for latex table 90 | percentageHyperTest = 100*sum(isAnticoncentrated_results,1)/length(dataFileNames); 91 | for i=1:length(percentageHyperTest) 92 | fprintf(' & $%.1f$ ', percentageHyperTest(i)) 93 | end 94 | fprintf(' \n') 95 | 96 | 97 | 98 | 99 | 100 | -------------------------------------------------------------------------------- /lib/arrow3d.m: -------------------------------------------------------------------------------- 1 | function arrowHandle = arrow3D(pos, deltaValues, colorCode, stemRatio, rhoRatio) 2 | 3 | % arrowHandle = arrow3D(pos, deltaValues, colorCode, stemRatio) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 4 | % 5 | % Used to plot a single 3D arrow with a cylindrical stem and cone arrowhead 6 | % pos = [X,Y,Z] - spatial location of the starting point of the arrow (end of stem) 7 | % deltaValues = [QX,QY,QZ] - delta parameters denoting the magnitude of the arrow along the x,y,z-axes (relative to 'pos') 8 | % colorCode - Color parameters as per the 'surf' command. For example, 'r', 'red', [1 0 0] are all examples of a red-colored arrow 9 | % stemRatio - The ratio of the length of the stem in proportion to the arrowhead. For example, a call of: 10 | % arrow3D([0,0,0], [100,0,0] , 'r', 0.82) will produce a red arrow of magnitude 100, with the arrowstem spanning a distance 11 | % of 82 (note 0.82 ratio of length 100) while the arrowhead (cone) spans 18. 12 | % rhoRatio - The ratio of the cylinder radius (0.05 is the default) 13 | % value) 14 | % 15 | % Example: 16 | % arrow3D([0,0,0], [4,3,7]); %---- arrow with default parameters 17 | % axis equal; 18 | % 19 | % Author: Shawn Arseneau 20 | % Created: September 14, 2006 21 | % Updated: September 18, 2006 22 | % 23 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 24 | if nargin<2 || nargin>5 25 | error('Incorrect number of inputs to arrow3D'); 26 | end 27 | if numel(pos)~=3 || numel(deltaValues)~=3 28 | error('pos and/or deltaValues is incorrect dimensions (should be three)'); 29 | end 30 | if nargin<3 31 | colorCode = 'interp'; 32 | end 33 | if nargin<4 34 | stemRatio = 0.75; 35 | end 36 | if nargin<5 37 | rhoRatio = 0.05; 38 | end 39 | 40 | X = pos(1); %---- with this notation, there is no need to transpose if the user has chosen a row vs col vector 41 | Y = pos(2); 42 | Z = pos(3); 43 | 44 | [sphi, stheta, srho] = cart2sph(deltaValues(1), deltaValues(2), deltaValues(3)); 45 | 46 | %******************************************* CYLINDER == STEM ********************************************* 47 | cylinderRadius = srho*rhoRatio; 48 | cylinderLength = srho*stemRatio; 49 | [CX,CY,CZ] = cylinder(cylinderRadius); 50 | CZ = CZ.*cylinderLength; %---- lengthen 51 | 52 | %----- ROTATE CYLINDER 53 | [row, col] = size(CX); %---- initial rotation to coincide with X-axis 54 | 55 | newEll = rotatePoints([0 0 -1], [CX(:), CY(:), CZ(:)]); 56 | CX = reshape(newEll(:,1), row, col); 57 | CY = reshape(newEll(:,2), row, col); 58 | CZ = reshape(newEll(:,3), row, col); 59 | 60 | [row, col] = size(CX); 61 | newEll = rotatePoints(deltaValues, [CX(:), CY(:), CZ(:)]); 62 | stemX = reshape(newEll(:,1), row, col); 63 | stemY = reshape(newEll(:,2), row, col); 64 | stemZ = reshape(newEll(:,3), row, col); 65 | 66 | %----- TRANSLATE CYLINDER 67 | stemX = stemX + X; 68 | stemY = stemY + Y; 69 | stemZ = stemZ + Z; 70 | 71 | %******************************************* CONE == ARROWHEAD ********************************************* 72 | coneLength = srho*(1-stemRatio); 73 | coneRadius = cylinderRadius*1.5; 74 | incr = 4; %---- Steps of cone increments 75 | coneincr = coneRadius/incr; 76 | [coneX, coneY, coneZ] = cylinder(cylinderRadius*2:-coneincr:0); %---------- CONE 77 | coneZ = coneZ.*coneLength; 78 | 79 | %----- ROTATE CONE 80 | [row, col] = size(coneX); 81 | newEll = rotatePoints([0 0 -1], [coneX(:), coneY(:), coneZ(:)]); 82 | coneX = reshape(newEll(:,1), row, col); 83 | coneY = reshape(newEll(:,2), row, col); 84 | coneZ = reshape(newEll(:,3), row, col); 85 | 86 | newEll = rotatePoints(deltaValues, [coneX(:), coneY(:), coneZ(:)]); 87 | headX = reshape(newEll(:,1), row, col); 88 | headY = reshape(newEll(:,2), row, col); 89 | headZ = reshape(newEll(:,3), row, col); 90 | 91 | %---- TRANSLATE CONE 92 | V = [0, 0, srho*stemRatio]; %---- centerline for cylinder: the multiplier is to set the cone 'on the rim' of the cylinder 93 | Vp = rotatePoints([0 0 -1], V); 94 | Vp = rotatePoints(deltaValues, Vp); 95 | headX = headX + Vp(1) + X; 96 | headY = headY + Vp(2) + Y; 97 | headZ = headZ + Vp(3) + Z; 98 | %************************************************************************************************************ 99 | hStem = surf(stemX, stemY, stemZ, 'FaceColor', colorCode, 'EdgeColor', 'none'); 100 | hold on; 101 | hHead = surf(headX, headY, headZ, 'FaceColor', colorCode, 'EdgeColor', 'none'); 102 | 103 | if nargout==1 104 | arrowHandle = [hStem, hHead]; 105 | end 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | -------------------------------------------------------------------------------- /lib/blkIndices.m: -------------------------------------------------------------------------------- 1 | function matrixIndices = blkIndices(blockIndex, blockSize) 2 | % The function returns the indices corresponding to the blockIndex-th block 3 | % for a matrix or vector with blocks of size blockIndex X blockIndex 4 | % 5 | % Luca Carlone 6 | % Georgia Institute of Technology 7 | % 1/5/2013 8 | 9 | numBlocks = length(blockIndex); 10 | matrixIndices = zeros(numBlocks*blockSize,1); 11 | 12 | for i=1:numBlocks 13 | matrixIndices(blockSize*i-(blockSize-1) : blockSize*i) = [blockSize*blockIndex(i)-(blockSize-1) : blockSize*blockIndex(i)]; 14 | end 15 | 16 | end -------------------------------------------------------------------------------- /lib/computeAposterioriBoundWahba.m: -------------------------------------------------------------------------------- 1 | function bound = computeAposterioriBoundWahba(theta, At, barc, sizeJ) 2 | 3 | if nargin < 4 4 | sizeJ = 3; % size of minimal set 5 | end 6 | 7 | min_sigma_At = inf; 8 | I_MC = find(theta>0); % set of selected inliers 9 | possible_J = nchoosek(I_MC,sizeJ); 10 | A = zeros(3*sizeJ,9); 11 | for i=1:size(possible_J,1) % for each possible subset of size sizeJ 12 | J = possible_J(i,:); 13 | for k=1:length(J) 14 | A(blkIndices(k,3),:) = At( :,:,J(k) ); 15 | end 16 | min_sigma_J = min(svd(A)); 17 | min_sigma_At = min(min_sigma_At , min_sigma_J); 18 | end 19 | 20 | bound = 2 * barc * sqrt(sizeJ) / min_sigma_At; 21 | 22 | -------------------------------------------------------------------------------- /lib/computeAposterioriBoundWahba_v2.m: -------------------------------------------------------------------------------- 1 | function bound = computeAposterioriBoundWahba_v2(J, At, barc) 2 | 3 | A = zeros(3*length(J),9); 4 | for k=1:length(J) 5 | A(blkIndices(k,3),:) = At( :,:,J(k) ); 6 | end 7 | min_sigma_J = min(svd(A)); 8 | 9 | dim_J = length(J); 10 | bound = 2 * barc * sqrt(dim_J) / min_sigma_J; 11 | 12 | -------------------------------------------------------------------------------- /lib/createImageStitchingData.m: -------------------------------------------------------------------------------- 1 | function [problem, R_gt, theta_gt] = createImageStitchingData(dataFileName) 2 | 3 | load(dataFileName) 4 | 5 | N = 70; % we max the points out at 70, as in the QUASAR paper 6 | problem.N = N; % total number of measurements 7 | 8 | %% create problem data 9 | for i=1:N 10 | ai = Kinv * [double(matchedPtsImg1.Location(i,:)) 1]'; 11 | a(:,i) = ai / norm(ai); 12 | bi = Kinv * [double(matchedPtsImg2.Location(i,:)) 1]'; 13 | b(:,i) = bi / norm(bi); 14 | end 15 | 16 | problem.a = a; 17 | problem.b = b; 18 | 19 | %% from QUASAR paper 20 | R_gt = R_quasar; 21 | theta_gt = thetas; 22 | 23 | %% set inlier bound 24 | problem.betasq = 1e-3; 25 | fprintf('betasq = %g\n',problem.betasq) 26 | 27 | %% rearrange in matrix form, as in eq (4) of the paper 28 | problem.At = zeros(3,9,N); 29 | for i=1:N 30 | problem.At(:,:,i) = kron( problem.a(:,i)' , eye(3) ); 31 | end -------------------------------------------------------------------------------- /lib/createWahbaProblemData.m: -------------------------------------------------------------------------------- 1 | function [problem, R_gt, R_gt_list] = createWahbaProblemData(N,outrate,isAdversarial) 2 | % isAdversarial = 1: creates 2 objects and adds similar noise on both 3 | % isAdversarial = 2: creates 2 objects and adds adversarial noise on the first one 4 | problem.N = N; % total number of measurements 5 | problem.Covariance = 1e-4*eye(3); % noise covariance for inliers 6 | problem.v1_distribution = 'uniform'; % sample uniformly from unit sphere 7 | problem.nrOutliers = round(N*outrate); % nr of outliers 8 | problem.boundNoise = true; % bound noise on inliers 9 | problem.normalize = false; % don't normalize measurements to have unit norm (follows eq. (4) in the paper) 10 | 11 | if nargin < 3 12 | isAdversarial = 0; 13 | end 14 | 15 | if isAdversarial == 1 16 | problem.nrObjects = 2; 17 | problem.objectSize = [1-outrate,outrate]; 18 | warning('Adversarial examples with consistent outliers') 19 | end 20 | 21 | if isAdversarial == 2 22 | problem.nrObjects = 2; 23 | problem.objectSize = [1-outrate,outrate]; 24 | problem.Covariance = 1e-8*eye(3); % we make noise tiny, but add it back adversarially later on 25 | warning('Adversarial examples with consistent outliers and worst case noise') 26 | end 27 | 28 | if isAdversarial == 3 29 | problem.nrObjects = 5; 30 | problem.objectSize = 0.2 * ones(1,problem.nrObjects); 31 | warning('Adversarial examples with consistent outliers corresponding to 5 objects') 32 | end 33 | 34 | if isAdversarial > 3 35 | error('invalid choice of isAdversarial parameter') 36 | end 37 | 38 | %% create problem data 39 | [a, b, R_gt, problem] = createWahbaProblem(problem); 40 | 41 | %% potentially add back adversarial inlier noise 42 | if isAdversarial == 2 43 | problem.Covariance = 1e-4*eye(3); % noise covariance for inliers 44 | problem.noiseBound = sqrt(problem.Covariance(2,2)*chi2inv(0.9999,3)); 45 | nrInliers = N - problem.nrOutliers; 46 | for k=1:nrInliers 47 | % add noise to inliers having worst case norm noiseBound (0.99 of max norm) 48 | noiseInliers = mvnrnd(zeros(1,3), problem.Covariance)'; 49 | a(:,k) = a(:,k) + (0.99 * problem.noiseBound) * noiseInliers / norm(noiseInliers); 50 | end 51 | end 52 | 53 | %% set inlier bound 54 | problem.betasq = problem.noiseBound^2 55 | problem.a = a; 56 | problem.b = b; 57 | 58 | %% rearrange in matrix form, as in eq (4) of the paper 59 | problem.At = zeros(3,9,N); 60 | for i=1:N 61 | problem.At(:,:,i) = kron( problem.a(:,i)' , eye(3) ); 62 | end 63 | 64 | %% in the case of multiple objects, we only store 1 65 | R_gt_list = R_gt; 66 | if size(R_gt,3) > 1 67 | R_gt = R_gt(:,:,1); 68 | end -------------------------------------------------------------------------------- /lib/my_ref_frame.m: -------------------------------------------------------------------------------- 1 | function ref_frame(Ref,OriginRef,colorCode,stemRatio,rhoRatio,axscale) 2 | 3 | % ref_frame plots a 3D representation of a reference frame 4 | % given by three right orthogonal unit vectors 5 | % 6 | % ref_frame(Ref,DimSpace,OriginRef) 7 | % 8 | % Ref is a 3 x 3 orthogonal matrix representing the unit vectors 9 | % of the reference frame to be drawn 10 | % OriginRef is the reference frame origin point 11 | % default value = [0 0 0]' 12 | % colorcode is the code color of each axis 13 | % default value = ['r','g','b'] 14 | % rhoRatio is the ratio of the cylinder radius 15 | % default value = 0.05 16 | % stemRatio is the ratio of the stem length wrt the arrowhead length 17 | % default value = 0.75 18 | % axscale is the common scale factor that multiplies the axes lenght 19 | % default value = 1.0 20 | 21 | % Copyright (C) Basilio Bona 2007 22 | 23 | n=nargin; 24 | if n == 1 25 | OriginRef=[0 0 0]'; 26 | colorCode=['r','g','b']; 27 | rhoRatio=0.05; 28 | stemRatio=0.75; 29 | axscale=1.00; 30 | end 31 | if n == 2 32 | colorCode=['r','g','b']; 33 | rhoRatio=0.05; 34 | stemRatio=0.75; 35 | axscale=1.00; 36 | end 37 | if n == 3 38 | rhoRatio=0.05; 39 | stemRatio=0.75; 40 | axscale=1.00; 41 | end 42 | if n == 4 43 | rhoRatio=0.05; 44 | axscale=1.00; 45 | end 46 | if n == 5 47 | axscale=1.00; 48 | end 49 | 50 | % xproj=DimSpace(1,2); yproj=DimSpace(2,2); zproj=DimSpace(3,1); 51 | 52 | Xm=10;Ym=10;Zm=10; 53 | 54 | DimSpace(1,1)=-Xm; DimSpace(1,2)=Xm; 55 | DimSpace(2,1)=-Ym; DimSpace(2,2)=Ym; 56 | DimSpace(3,1)=-Zm; DimSpace(3,2)=Zm; 57 | OriginRef=[0 0 0]'; 58 | 59 | hold on; 60 | 61 | axis square 62 | grid on 63 | 64 | xlabel('x axis') 65 | ylabel('y axis') 66 | zlabel('z axis') 67 | view(-35,35) 68 | 69 | arrow3d(OriginRef, axscale*Ref(:,1), colorCode(1), stemRatio, rhoRatio); 70 | arrow3d(OriginRef, axscale*Ref(:,2), colorCode(2), stemRatio, rhoRatio); 71 | arrow3d(OriginRef, axscale*Ref(:,3), colorCode(3), stemRatio, rhoRatio); 72 | 73 | lighting phong; 74 | camlight left; 75 | 76 | %PlotSpace 77 | axis equal; xlabel('X'); ylabel('Y'); zlabel('Z'); 78 | 79 | -------------------------------------------------------------------------------- /lib/plotImageStitchingResults.m: -------------------------------------------------------------------------------- 1 | %% code based https://github.com/MIT-SPARK/CertifiablyRobustPerception/ 2 | %% quasar_stitching_gen_data 3 | load(dataFileName) 4 | R = R_stride'; 5 | H = K*R*Kinv; 6 | H = H'; 7 | tforms1(2) = projective2d(eye(3)); 8 | tforms1(2) = projective2d(H); 9 | xlim=zeros(2,2); 10 | ylim=zeros(2,2); 11 | for i = 1:numel(tforms1) 12 | [xlim(i,:), ylim(i,:)] = outputLimits(tforms1(i), [1 imageSize(2)], [1 imageSize(1)]); 13 | end 14 | maxImageSize = imageSize; 15 | 16 | % Find the minimum and maximum output limits 17 | xMin = min([1; xlim(:)]); 18 | xMax = max([maxImageSize(2); xlim(:)]); 19 | 20 | yMin = min([1; ylim(:)]); 21 | yMax = max([maxImageSize(1); ylim(:)]); 22 | 23 | % Width and height of panorama. 24 | width = round(xMax - xMin); 25 | height = round(yMax - yMin); 26 | 27 | % Initialize the "empty" panorama. 28 | panorama = zeros([height width 3], 'like', image1); 29 | 30 | blender = vision.AlphaBlender('Operation', 'Binary mask', ... 31 | 'MaskSource', 'Input port'); 32 | 33 | % Create a 2-D spatial reference object defining the size of the panorama. 34 | xLimits = [xMin xMax]; 35 | yLimits = [yMin yMax]; 36 | panoramaView = imref2d([height width], xLimits, yLimits); 37 | numImages=2; 38 | imgCell={image1Col,image2Col}; 39 | for i = 1:numImages 40 | 41 | I = imgCell{i}; 42 | 43 | % Transform I into the panorama. 44 | warpedImage = imwarp(I, tforms1(i), 'OutputView', panoramaView); 45 | 46 | % Generate a binary mask. 47 | mask = imwarp(true(size(I,1),size(I,2)), tforms1(i), 'OutputView', panoramaView); 48 | 49 | % Overlay the warpedImage onto the panorama. 50 | panorama = step(blender, panorama, warpedImage, mask); 51 | end 52 | 53 | figure; 54 | imshow(panorama) 55 | shg 56 | 57 | %% plot inlier and outlier correspondences 58 | figure; 59 | stackedImage = cat(2, image1, image2); % Places the two images side by side 60 | imshow(stackedImage); 61 | width = size(image1, 2); 62 | hold on; 63 | points1=matchedPtsImg1.Location; 64 | points2=matchedPtsImg2.Location; 65 | for i=1:length(theta_stride) 66 | plot(points1(i, 1), points1(i, 2), 'yo', points2(i, 1) + width, ... 67 | points2(i, 2), 'y+'); 68 | if theta_stride(i)>0 69 | line([points1(i, 1) points2(i, 1) + width], [points1(i, 2) points2(i, 2)], ... 70 | 'Color', 'green','linewidth',3); 71 | else 72 | line([points1(i, 1) points2(i, 1) + width], [points1(i, 2) points2(i, 2)], ... 73 | 'Color', 'red','linewidth',3); 74 | end 75 | end 76 | shg -------------------------------------------------------------------------------- /lib/plotRotations.m: -------------------------------------------------------------------------------- 1 | function plotRotations(R_gt_list, R_est_list) 2 | 3 | origin = zeros(3,1); 4 | 5 | %% plot 6 | figure 7 | for i=1:size(R_est_list,3) 8 | my_ref_frame(R_est_list(:,:,i),origin,['r','g','b'],0.9,0.01,0.8) 9 | end 10 | 11 | %% plot 12 | for i=1:size(R_gt_list,3) 13 | my_ref_frame(R_gt_list(:,:,i),origin,['r','g','b'],0.75,0.05,0.5) 14 | end 15 | set(gca,'fontsize', 18) -------------------------------------------------------------------------------- /lib/rotatePoints.m: -------------------------------------------------------------------------------- 1 | function rotatedData = rotatePoints(alignmentVector, originalData) 2 | 3 | % rotatedData = rotatePoints(alignmentVector, originalData) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 4 | % 5 | % Rotate the 'originalData' in the form of Nx2 or Nx3 about the origin by aligning the x-axis with the alignment vector 6 | % 7 | % Rdata = rotatePoints([1,2,-1], [Xpts(:), Ypts(:), Zpts(:)]) - rotate the (X,Y,Z)pts in 3D with respect to the vector [1,2,-1] 8 | % 9 | % Rotating using spherical components can be done by first converting using [dX,dY,dZ] = cart2sph(theta, phi, rho); alignmentVector = [dX,dY,dZ]; 10 | % 11 | % Example: 12 | % %% Rotate the point [3,4,-7] with respect to the following: 13 | % %%%% Original associated vector is always [1,0,0] 14 | % %%%% Calculate the appropriate rotation requested with respect to the x-axis. For example, if only a rotation about the z-axis is 15 | % %%%% sought, alignmentVector = [2,1,0] %% Note that the z-component is zero 16 | % rotData = rotatePoints(alignmentVector, [3,4,-7]); 17 | % 18 | % Author: Shawn Arseneau 19 | % Created: Feb.2, 2006 20 | % 21 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 22 | 23 | alignmentDim = numel(alignmentVector); 24 | DOF = size(originalData,2); %---- DOF = Degrees of Freedom (i.e. 2 for two dimensional and 3 for three dimensional data) 25 | 26 | if alignmentDim~=DOF 27 | error('Alignment vector does not agree with originalData dimensions'); 28 | end 29 | if DOF<2 || DOF>3 30 | error('rotatePoints only does rotation in two or three dimensions'); 31 | end 32 | 33 | 34 | if DOF==2 % 2D rotation... 35 | [rad_theta, rho] = cart2pol(alignmentVector(1), alignmentVector(2)); 36 | deg_theta = -1 * rad_theta * (180/pi); 37 | ctheta = cosd(deg_theta); stheta = sind(deg_theta); 38 | 39 | Rmatrix = [ctheta, -1.*stheta;... 40 | stheta, ctheta]; 41 | rotatedData = originalData*Rmatrix; 42 | 43 | else % 3D rotation... 44 | [rad_theta, rad_phi, rho] = cart2sph(alignmentVector(1), alignmentVector(2), alignmentVector(3)); 45 | rad_theta = rad_theta * -1; 46 | deg_theta = rad_theta * (180/pi); 47 | deg_phi = rad_phi * (180/pi); 48 | ctheta = cosd(deg_theta); stheta = sind(deg_theta); 49 | Rz = [ctheta, -1.*stheta, 0;... 50 | stheta, ctheta, 0;... 51 | 0, 0, 1]; %% First rotate as per theta around the Z axis 52 | rotatedData = originalData*Rz; 53 | 54 | [rotX, rotY, rotZ] = sph2cart(-1* (rad_theta+(pi/2)), 0, 1); %% Second rotation corresponding to phi 55 | rotationAxis = [rotX, rotY, rotZ]; 56 | u = rotationAxis(:)/norm(rotationAxis); %% Code extract from rotate.m from MATLAB 57 | cosPhi = cosd(deg_phi); 58 | sinPhi = sind(deg_phi); 59 | invCosPhi = 1 - cosPhi; 60 | x = u(1); 61 | y = u(2); 62 | z = u(3); 63 | Rmatrix = [cosPhi+x^2*invCosPhi x*y*invCosPhi-z*sinPhi x*z*invCosPhi+y*sinPhi; ... 64 | x*y*invCosPhi+z*sinPhi cosPhi+y^2*invCosPhi y*z*invCosPhi-x*sinPhi; ... 65 | x*z*invCosPhi-y*sinPhi y*z*invCosPhi+x*sinPhi cosPhi+z^2*invCosPhi]'; 66 | 67 | rotatedData = rotatedData*Rmatrix; 68 | end 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | -------------------------------------------------------------------------------- /lib/shaded_plots/example_shaded_plots.m: -------------------------------------------------------------------------------- 1 | %% Shaded line plot 2 | % Example showing the difference between the standard plot routine and the 3 | % shaded routine 4 | 5 | x = -2*pi:pi/100:2*pi; 6 | fx = sin(x); 7 | 8 | figure('Color','w'); 9 | subplot(1,2,1); 10 | hold on 11 | plot(x,fx); 12 | plot(2*x+pi/2,0.5*fx+0.1*x); 13 | hold off 14 | title('plot'); 15 | 16 | subplot(1,2,2); 17 | hold on 18 | plot_shaded(x,fx); 19 | plot_shaded(2*x+pi/2,0.5*fx+0.1*x); 20 | hold off 21 | title('plot\_shaded'); 22 | 23 | %% Histogram plot 24 | % Plots two histograms for two different distributions 25 | 26 | X1 = 3 + 2.0*randn([100000,1]); 27 | X2 = 12 + 4.0*randn([100000,1]); 28 | 29 | figure('Color','w'); 30 | hold on 31 | plot_histogram_shaded(X1,'Alpha',0.3); 32 | plot_histogram_shaded(X2); 33 | hold off 34 | title('plot\_histogram\_shaded'); 35 | 36 | 37 | %% Distribution plots 38 | % Show different plot routines to visualize measurement errors/noise 39 | 40 | X = 1:0.25:10; 41 | Y = sin(X)+0.25*X; 42 | Y_error = randn(1000,numel(Y)); 43 | Y_noisy = Y+Y_error.*repmat(0.1*X,[size(Y_error,1) 1]); 44 | 45 | 46 | figure('Color','w'); 47 | subplot(3,1,1); 48 | plot(X,Y,'LineWidth',1.5); 49 | title('plot (True value y=f(x))'); 50 | ylim([-1 5]); 51 | 52 | subplot(3,1,2); 53 | hold on 54 | plot(X,Y,'LineWidth',1.5); 55 | plot_distribution(X,Y_noisy); 56 | hold off 57 | title('plot\_distribution'); 58 | ylim([-1 5]); 59 | 60 | subplot(3,1,3); 61 | hold on 62 | plot(X,Y,'LineWidth',1.5); 63 | plot_distribution_prctile(X,Y_noisy,'Prctile',[25 50 75 90]); 64 | hold off 65 | title('plot\_distribution\_prctile'); 66 | ylim([-1 5]); 67 | 68 | 69 | -------------------------------------------------------------------------------- /lib/shaded_plots/license.txt: -------------------------------------------------------------------------------- 1 | Copyright (c) 2018, John A. Onofrey 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution 13 | * Neither the name of Yale University nor the names of its 14 | contributors may be used to endorse or promote products derived from this 15 | software without specific prior written permission. 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE 20 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | -------------------------------------------------------------------------------- /lib/shaded_plots/plot_distribution.m: -------------------------------------------------------------------------------- 1 | function plot_distribution(varargin) 2 | % PLOT_DISTRIBUTION(X,Y) - Plots the mean with standard deviation errors as a shaded region in the open 3 | % figure window. 4 | % Inputs: 5 | % X: vector of n domain values (the x-axis). 6 | % Y: mxn matrix of distribution of range values (the y-axis), where m 7 | % corresponds to the number of data samples for each value n. 8 | % 9 | % PLOT_DISTRIBUTION(X,Y,...) 10 | % Parameter options include: 11 | % 'Alpha': the alpha value of the shaded region, default 0.15. 12 | % 'Color': the shaded region color. 13 | % 'LineWidth': the contour line width, default = 2.0. 14 | 15 | 16 | [X,Y,color_value,alpha_value,line_width] = parseinputs(varargin{:}); 17 | 18 | 19 | MU = mean(Y); 20 | SIGMA = std(Y); 21 | 22 | hold on 23 | % Create the polygons for the shaded region 24 | 25 | Ptop = MU+SIGMA; 26 | Pbot = MU-SIGMA; 27 | 28 | for i=1:numel(X)-1 29 | Px = [X(i) X(i+1) X(i+1) X(i)]; 30 | Py = [Ptop(i) Ptop(i+1) Pbot(i+1) Pbot(i)]; 31 | fill(Px,Py,color_value,'FaceAlpha',alpha_value,'EdgeColor','none'); 32 | end 33 | 34 | plot(X,MU,'LineWidth',line_width,'Color',color_value); 35 | hold off 36 | 37 | end 38 | 39 | 40 | 41 | 42 | function [X,Y,color_value,alpha_value,line_width] = parseinputs(varargin) 43 | 44 | % Check the number of input args 45 | minargs = 2; 46 | numopts = 3; 47 | maxargs = minargs + 2*numopts; 48 | narginchk(minargs,maxargs); 49 | 50 | ax = gca; 51 | 52 | % Set the defaults 53 | alpha_value = 0.15; 54 | color_value = ax.ColorOrder(ax.ColorOrderIndex,:); 55 | line_width = 2.0; 56 | 57 | 58 | % Get the inputs and check them 59 | X = varargin{1}; 60 | validateattributes(X,{'numeric'},{'vector','nonnan','finite'},mfilename,'X',2); 61 | Y = varargin{2}; 62 | validateattributes(Y,{'numeric'},{'2d','nonnan','finite','ncols',numel(X)},mfilename,'Y',2); 63 | 64 | 65 | if nargin > minargs 66 | for i=(minargs+1):2:nargin 67 | PNAME = varargin{i}; 68 | PVALUE = varargin{i+1}; 69 | 70 | PNAME = validatestring(PNAME,{'Alpha','Color','LineWidth','Prctile'},... 71 | mfilename,'ParameterName',i); 72 | 73 | switch PNAME 74 | case 'Alpha' 75 | validateattributes(PVALUE,{'numeric'},{'scalar','nonnan','finite','nonnegative','<=',1.0},mfilename,PNAME,i+1); 76 | alpha_value = PVALUE; 77 | case 'Color' 78 | validateattributes(PVALUE,{'numeric'},{'real','nonnegative','nonempty','vector','numel',3,'<=',1.0},mfilename,PNAME,i+1); 79 | color_value = PVALUE; 80 | case 'LineWidth' 81 | validateattributes(PVALUE,{'numeric'},{'scalar','finite','nonnegative'},... 82 | mfilename,PNAME,i+1); 83 | line_width = PVALUE; 84 | end 85 | end 86 | end 87 | 88 | end 89 | 90 | 91 | 92 | 93 | -------------------------------------------------------------------------------- /lib/shaded_plots/plot_distribution_prctile.m: -------------------------------------------------------------------------------- 1 | function plot_distribution_prctile(varargin) 2 | % PLOT_DISTRIBUTION_PRCTILE(X,Y) - Plots the median with percentile errors as a shaded region in the open 3 | % figure window. 4 | % Inputs: 5 | % X: vector of n domain values (the x-axis). 6 | % Y: mxn matrix of distribution of range values (the y-axis), where m 7 | % corresponds to the number of data samples for each value n. 8 | % 9 | % PLOT_DISTRIBUTION_PRCTILE(X,Y,...) 10 | % Parameter options include: 11 | % 'Alpha': the alpha value of the shaded region, default 0.15. 12 | % 'Color': the shaded region color. 13 | % 'LineWidth': the contour line width, default = 2.0. 14 | % 'Prctile': the percentile values to plot, default [50] (the middle 15 | % 50 percentile, or inter-quartile range). 16 | % 17 | 18 | 19 | [X,Y,prctile_value,color_value,alpha_value,line_width] = parseinputs(varargin{:}); 20 | 21 | p_value = 0.5*(100-sort(prctile_value)); 22 | 23 | hold on 24 | % Create the polygons for the shaded region 25 | for j=1:numel(p_value) 26 | Ptop = prctile(Y,100-p_value(j)); 27 | Pbot = prctile(Y,p_value(j)); 28 | for i=1:numel(X)-1 29 | Px = [X(i) X(i+1) X(i+1) X(i)]; 30 | Py = [Ptop(i) Ptop(i+1) Pbot(i+1) Pbot(i)]; 31 | fill(Px,Py,color_value,'FaceAlpha',alpha_value,'EdgeColor','none'); 32 | end 33 | end 34 | % plot(X,median(Y),'LineWidth',line_width,'Color',color_value); 35 | % hold off 36 | 37 | 38 | end 39 | 40 | 41 | 42 | 43 | 44 | function [X,Y,prctile_value,color_value,alpha_value,line_width] = parseinputs(varargin) 45 | 46 | % Check the number of input args 47 | minargs = 2; 48 | numopts = 4; 49 | maxargs = minargs + 2*numopts; 50 | narginchk(minargs,maxargs); 51 | 52 | ax = gca; 53 | 54 | % Set the defaults 55 | prctile_value = 50; 56 | alpha_value = 0.15; 57 | color_value = ax.ColorOrder(ax.ColorOrderIndex,:); 58 | line_width = 2.0; 59 | 60 | 61 | % Get the inputs and check them 62 | X = varargin{1}; 63 | validateattributes(X,{'numeric'},{'vector','nonnan','finite'},mfilename,'X',2); 64 | Y = varargin{2}; 65 | validateattributes(Y,{'numeric'},{'2d','nonnan','finite','ncols',numel(X)},mfilename,'Y',2); 66 | 67 | 68 | if nargin > minargs 69 | for i=3:2:nargin 70 | PNAME = varargin{i}; 71 | PVALUE = varargin{i+1}; 72 | 73 | PNAME = validatestring(PNAME,{'Alpha','Color','LineWidth','Prctile'},... 74 | mfilename,'ParameterName',i); 75 | 76 | switch PNAME 77 | case 'Alpha' 78 | validateattributes(PVALUE,{'numeric'},{'scalar','nonnan','finite','nonnegative','<=',1.0},mfilename,PNAME,i+1); 79 | alpha_value = PVALUE; 80 | case 'Color' 81 | validateattributes(PVALUE,{'numeric'},{'real','nonnegative','nonempty','vector','numel',3,'<=',1.0},mfilename,PNAME,i+1); 82 | color_value = PVALUE; 83 | case 'LineWidth' 84 | validateattributes(PVALUE,{'numeric'},{'scalar','finite','nonnegative'},... 85 | mfilename,PNAME,i+1); 86 | line_width = PVALUE; 87 | case 'Prctile' 88 | validateattributes(PVALUE,{'numeric'},{'vector','finite','nonnan','nonnegative','<=',100},... 89 | mfilename,PNAME,i+1); 90 | prctile_value = PVALUE; 91 | end 92 | end 93 | end 94 | 95 | end 96 | 97 | -------------------------------------------------------------------------------- /lib/shaded_plots/plot_histogram_shaded.m: -------------------------------------------------------------------------------- 1 | function [bin_centers,hist_values,bin_edges] = plot_histogram_shaded(varargin) 2 | % [C,V,E] = PLOT_HISTOGRAM_SHADED(X) - Plots the histogram as a shaded line plot instead of bar plot. 3 | % Outputs: 4 | % C: the histogram bin centers. 5 | % V: the histogram values at each bin. 6 | % E: the histogram bin edges values. 7 | % 8 | % [C,V,E] = PLOT_HISTOGRAM_SHADED(X,_) 9 | % Optional values: 10 | % 'alpha': alpha value of the shaded region, default 0.2. 11 | % 'bins': number of bins, default []. 12 | % 'color': color of the plot, default [0 0 1]. 13 | % 'edges': the bins edges to use, default []. 14 | % 'normalization': the normalization type to use (see histogram doc), 15 | % default 'probability' 16 | % 17 | 18 | [X,n_bins,bin_edges,color_value,alpha_value,norm_type] = parseinputs(varargin{:}); 19 | 20 | 21 | 22 | if isempty(n_bins) && isempty(bin_edges) 23 | [hist_values,bin_edges] = histcounts(X,'Normalization',norm_type); 24 | else 25 | bin_param = n_bins; 26 | if ~isempty(bin_edges) 27 | bin_param = bin_edges; 28 | end 29 | [hist_values,bin_edges] = histcounts(X,bin_param,'Normalization',norm_type); 30 | end 31 | 32 | 33 | x_cat = cat(1,bin_edges,circshift(bin_edges,-1)); 34 | x_mean = mean(x_cat,1); 35 | bin_centers = x_mean(1:end-1); 36 | fprintf(1,'Plotting histogram with %d bins\n',numel(bin_centers)); 37 | 38 | % Use a shaded plot 39 | plot_shaded(bin_centers,hist_values,'Alpha',alpha_value,'Color',color_value,'LineWidth',1.5); 40 | 41 | 42 | end 43 | 44 | 45 | function [X,n_bins,bin_edges,color_value,alpha_value,norm_type] = parseinputs(varargin) 46 | 47 | % Get the number of input args 48 | minargs = 1; 49 | maxargs = minargs+5*2; 50 | narginchk(minargs,maxargs); 51 | 52 | ax = gca; 53 | 54 | % Set the defaults 55 | n_bins = []; 56 | bin_edges = []; 57 | color_value = ax.ColorOrder(ax.ColorOrderIndex,:); 58 | alpha_value = 0.2; 59 | norm_type = 'probability'; 60 | 61 | % Get the inputs and check them 62 | X = varargin{1}; 63 | validateattributes(X,{'numeric'},{'nonnan','finite'},mfilename,'X',1); 64 | 65 | if nargin > 1 66 | for i=2:2:nargin 67 | PNAME = varargin{i}; 68 | PVALUE = varargin{i+1}; 69 | 70 | PNAME = validatestring(PNAME,... 71 | {'alpha','bins','color','edges','normalization'},... 72 | mfilename,'ParameterName',i); 73 | 74 | switch PNAME 75 | case 'alpha' 76 | validateattributes(PVALUE,{'numeric'},{'real','nonnegative','finite','scalar'},... 77 | mfilename,PNAME,i+1); 78 | alpha_value = PVALUE; 79 | case 'bins' 80 | if ~isempty(PVALUE) 81 | validateattributes(PVALUE,{'numeric'},{'integer','positive','finite','scalar'},... 82 | mfilename,PNAME,i+1); 83 | n_bins = PVALUE; 84 | end 85 | case 'color' 86 | validateattributes(PVALUE,{'numeric'},{'real','nonnegative','nonempty','vector','numel',3,'<=',1.0},mfilename,PNAME,i+1); 87 | color_value = PVALUE; 88 | case 'edges' 89 | validateattributes(PVALUE,{'numeric'},{'real','nonempty','vector','nonnan'},mfilename,PNAME,i+1); 90 | bin_edges = PVALUE; 91 | case 'normalization' 92 | validateattributes(PVALUE,{'char'},{'nonempty'},mfilename,PNAME,i+1); 93 | norm_type = PVALUE; 94 | end 95 | end 96 | end 97 | 98 | end 99 | 100 | -------------------------------------------------------------------------------- /lib/shaded_plots/plot_shaded.m: -------------------------------------------------------------------------------- 1 | function plot_shaded(varargin) 2 | % PLOT_SHADED(X,Y) - Plots the line with pretty shaded region under the line in the open 3 | % figure window. 4 | % Inputs: 5 | % X: vector of domain values. 6 | % Y: vector or range values. 7 | % 8 | % PLOT_SHADED(X,Y,...) 9 | % Parameter options include: 10 | % 'Alpha': the alpha value of the shaded region, default 0.15. 11 | % 'Color': the shaded region color. 12 | % 'LineWidth': the contour line width, default = 2.0. 13 | 14 | 15 | [X,Y,color_value,alpha_value,line_width] = parseinputs(varargin{:}); 16 | 17 | 18 | y_min = min(Y); 19 | y_max = max(Y); 20 | V_alpha = alpha_value*((Y-y_min)/y_max); 21 | F = [1 2 3 4]; 22 | 23 | hold on 24 | % Create patches for the shaded region 25 | for i=1:numel(X)-1 26 | V = [ X(i) y_min ; X(i) Y(i); X(i+1) Y(i+1); X(i+1) y_min ]; 27 | A = [0,V_alpha(i),V_alpha(i+1),0]'; 28 | patch('Faces',F,'Vertices',V,'FaceColor',color_value,... 29 | 'EdgeColor','none',... 30 | 'FaceVertexAlphaData',A,'FaceAlpha','interp','AlphaDataMapping','none'); 31 | end 32 | 33 | plot(X,Y,'LineWidth',line_width,'Color',color_value); 34 | hold off 35 | 36 | end 37 | 38 | 39 | 40 | 41 | function [X,Y,color_value,alpha_value,line_width] = parseinputs(varargin) 42 | 43 | % Check the number of input args 44 | minargs = 2; 45 | numopts = 3; 46 | maxargs = minargs + 2*numopts; 47 | narginchk(minargs,maxargs); 48 | 49 | ax = gca; 50 | 51 | % Set the defaults 52 | alpha_value = 0.15; 53 | color_value = ax.ColorOrder(ax.ColorOrderIndex,:); 54 | line_width = 2.0; 55 | 56 | 57 | % Get the inputs and check them 58 | X = varargin{1}; 59 | validateattributes(X,{'numeric'},{'vector','nonnan','finite'},mfilename,'X',2); 60 | Y = varargin{2}; 61 | validateattributes(Y,{'numeric'},{'vector','nonnan','finite','numel',numel(X)},mfilename,'Y',2); 62 | % Ensure that X and Y are column vectors 63 | X = X(:); 64 | Y = Y(:); 65 | 66 | if nargin > minargs 67 | for i=(minargs+1):2:nargin 68 | PNAME = varargin{i}; 69 | PVALUE = varargin{i+1}; 70 | 71 | PNAME = validatestring(PNAME,{'Alpha','Color','LineWidth','Prctile'},... 72 | mfilename,'ParameterName',i); 73 | 74 | switch PNAME 75 | case 'Alpha' 76 | validateattributes(PVALUE,{'numeric'},{'scalar','nonnan','finite','nonnegative','<=',1.0},mfilename,PNAME,i+1); 77 | alpha_value = PVALUE; 78 | case 'Color' 79 | validateattributes(PVALUE,{'numeric'},{'real','nonnegative','nonempty','vector','numel',3,'<=',1.0},mfilename,PNAME,i+1); 80 | color_value = PVALUE; 81 | case 'LineWidth' 82 | validateattributes(PVALUE,{'numeric'},{'scalar','finite','nonnegative'},... 83 | mfilename,PNAME,i+1); 84 | line_width = PVALUE; 85 | end 86 | end 87 | end 88 | 89 | end 90 | 91 | 92 | 93 | 94 | -------------------------------------------------------------------------------- /lib/solveWahba.m: -------------------------------------------------------------------------------- 1 | function [R_stride,theta_stride,eta] = solveWahba(a,b,betasq,R_gt,sdpnalpath,manoptpath,stridepath) 2 | %% solve SDP relaxation using STRIDE solver, scalable and faster 3 | %% see Yang et al. An Inexact Projected Gradient Method with Rounding and Lifting 4 | %% by Nonlinear Programming for Solving Rank-One Semidefinite Relaxation of Polynomial Optimization 5 | %% https://arxiv.org/abs/2105.14033 6 | %% Solve using STRIDE 7 | 8 | % generate standard SDP data (this is different than CVX) 9 | SDP = QUASAR_Problem(a,b,betasq); 10 | 11 | % set parameters for STRIDE 12 | options.pgdStepSize = 10; % step size, default 10 13 | options.maxiterPGD = 5; % maximum outer iterations for STRIDE, default 5-10 14 | options.SDPNALpath = sdpnalpath; % provide path to SDPNAL 15 | options.tolADMM = 5e-6; % tolerance for warmstart, decrease this parameter for a better warmstart (but takes more time) 16 | options.tolPGD = 1e-8; % tolerance on KKT residual of the SDP 17 | options.lbfgseps = false; 18 | % provide implementation to the local search method 19 | options.rrOpt = 1:3; % round the leading 3 eigenvectors to generate hypotheses 20 | options.rrFunName = 'local_search_quasar'; % name of the .m file that implements the local search 21 | 22 | % Primal initialization 23 | [R_gnc,info_gnc] = GNC_Wahba(a,b,betasq,1.4); 24 | q_gnc = rotm2quat(R_gnc); q_gnc = [q_gnc(2:4),q_gnc(1)]'; 25 | v_gnc = kron([1;info_gnc.theta_gnc],q_gnc); 26 | X0 = {v_gnc*v_gnc'}; 27 | 28 | % call STRIDE 29 | addpath(genpath(manoptpath)); 30 | addpath(genpath(sdpnalpath)); 31 | addpath(genpath(stridepath)); 32 | [outPGD,Xopt,yopt,Sopt] = PGDSDP(SDP.blk, SDP.At, SDP.b, SDP.C, X0, options); 33 | 34 | if size(R_gt,3) > 1 35 | R_gt = R_gt(:,:,1); 36 | end 37 | 38 | infostride = get_performance_quasar(Xopt,yopt,Sopt,SDP,R_gt); 39 | R_stride = infostride.R; 40 | theta_stride = infostride.theta(:); 41 | eta = infostride.Rs; % suboptimality gap 42 | 43 | %% ********************************************* 44 | %% ********************************************* 45 | %% helper function 46 | function Q_cost = cost(v1,v2,barc2) 47 | P=[1, 0, 0, 0, 0, -1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 1; 48 | 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0; 49 | 0, 0, 1, 0, 0, 0, 0, -1, 1, 0, 0, 0, 0, -1, 0, 0; 50 | 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, -1, 0, 0, -1, 0; 51 | -1, 0, 0, 0, 0, 1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 1; 52 | 0, 0, 0, 1, 0, 0, 1, 0, 0, 1, 0, 0, 1, 0, 0, 0; 53 | 0, 0, 1, 0, 0, 0, 0, 1, 1, 0, 0, 0, 0, 1, 0, 0; 54 | 0, 0, 0, -1, 0, 0, 1, 0, 0, 1, 0, 0, -1, 0, 0, 0; 55 | -1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1]; 56 | P=sparse(P); 57 | N = size(v1,2); 58 | Npm = 4*N + 4; 59 | Q_1=zeros(Npm,Npm); 60 | for k=1:N 61 | idx = 4+blkIndices(k,4); 62 | P_k = reshape(P'*reshape(v2(:,k)*v1(:,k)',[9,1]),[4,4]); 63 | ck = 0.5 * ( v1(:,k)'*v1(:,k)+v2(:,k)'*v2(:,k) - barc2 ); 64 | Q_1((1:4),idx) = Q_1((1:4),idx)-0.5*P_k+ck/2*eye(4); 65 | Q_1(idx,(1:4)) = Q_1(idx,(1:4))-0.5*P_k+ck/2*eye(4); 66 | end 67 | 68 | Q_2=zeros(Npm,Npm); 69 | for k=1:N 70 | % idx = 4+blkIndices(k,4); 71 | idx = blkIndices(1,4); 72 | P_k = reshape(P'*reshape(v2(:,k)*v1(:,k)',[9,1]),[4,4]); 73 | ck = 0.5 * ( v1(:,k)'*v1(:,k)+v2(:,k)'*v2(:,k) + barc2 ); 74 | Q_2(idx,idx) = Q_2(idx,idx) - P_k + ck*eye(4); 75 | end 76 | Q_cost=Q_1+Q_2; 77 | Q_cost=sparse(Q_cost); 78 | 79 | Q_cost=Q_cost/barc2; 80 | end 81 | 82 | function B = normalize_cols(A) 83 | mag = sum(A.^2,1).^(0.5); 84 | B = A./mag; 85 | end 86 | 87 | function f_est = cost_org(a,b,betasq,q) 88 | R = quat2rot(q); 89 | residuals = sum((b - R*a).^2,1)/betasq; 90 | f_est = sum( min(residuals(:),1) ); 91 | end 92 | 93 | end 94 | -------------------------------------------------------------------------------- /lib/solveWahba_TLS_01.m: -------------------------------------------------------------------------------- 1 | %% solve SDP TLS relaxation using cvx (with w in {0;1}) 2 | 3 | v1=problem.a; 4 | v2=problem.b; 5 | betasq=problem.betasq; 6 | 7 | N = size(v1,2); % nr vectors (measurements) 8 | % X = [q\tran, q_1\tran, q_2\tran, ..., q_N\tran] has size n = 4+4*N 9 | n = 4*N + 4; 10 | 11 | %% solve SDP relaxation 12 | C = cost(v1,v2,betasq); 13 | unitTest(R_gt,problem.theta_gt(:),N,v1,v2,betasq,C); 14 | q_ind = [1:4]; 15 | 16 | cvx_begin % sdp 17 | % cvx_solver mosek 18 | variable X(n,n) symmetric 19 | minimize( trace(C * X) ) % 20 | subject to 21 | X == semidefinite(n); 22 | trace(X(q_ind,q_ind)) == 1; 23 | for k=1:N 24 | % w_i^2 = w_i 25 | idx = blkIndices(k+1,4); 26 | X(idx,idx) == X(q_ind,idx); 27 | end 28 | for k1=1:N 29 | for k2=k1+1:N+1 30 | % symmetric off diagonal 31 | idx1 = blkIndices(k1,4); 32 | idx2 = blkIndices(k2,4); 33 | X(idx1,idx2) == X(idx1,idx2)'; 34 | end 35 | end 36 | cvx_end 37 | 38 | % extract solution 39 | f_sdp = cvx_optval; % lower bound 40 | eig(X) 41 | 42 | %% ROUNDING 43 | [V,~] = sorteig(X); 44 | v = V(:,1); 45 | q = normalize_cols( v(1:4) ); 46 | w_beforeRounding = zeros(N,1); 47 | for i = 1:N 48 | w_beforeRounding(i) = round(q'*v(blkIndices(i+1,4))); 49 | end 50 | w = round(w_beforeRounding); 51 | 52 | R_err = getAngularError(quat2rot(q),R_gt); 53 | f_est = cost_org(v1,v2,betasq,q); % upper bound 54 | subopt = abs(f_est - f_sdp) / (1+abs(f_est)+abs(f_sdp)); 55 | 56 | eta = subopt; 57 | R_est = quat2rot(q); 58 | fprintf('f_sdp: %3.4e, f_est: %3.4e, R_err: %3.2e[deg].\n',f_sdp,f_est,R_err); 59 | fprintf('Relative suboptimality: %3.2e.\n',subopt); 60 | 61 | %% ********************************************* 62 | %% ********************************************* 63 | %% helper function 64 | function Q_cost = cost(v1,v2,barc2) 65 | P=getP(); 66 | P=sparse(P); 67 | N = size(v1,2); 68 | n = 4*N + 4; 69 | Q_cost=zeros(n,n); 70 | for k=1:N 71 | % w_i (||b_i||^2+||a_i||^2 - barcq) + barcq - 2*b_i'*R_i*a_i 72 | % barcq: 73 | Q_cost([1:4],[1:4]) = Q_cost([1:4],[1:4]) + barc2*eye(4); 74 | 75 | % w_i (||b_i||^2+||a_i||^2 - barcq): 76 | idx = blkIndices(k+1,4); 77 | ck = ( v1(:,k)'*v1(:,k)+v2(:,k)'*v2(:,k) - barc2 ); 78 | Q_cost(idx,idx) = Q_cost(idx,idx) + ck*eye(4); 79 | 80 | % - 2*b_i'*R_i*a_i 81 | P_k = reshape(P'*reshape(v2(:,k)*v1(:,k)',[9,1]),[4,4]); 82 | Q_cost([1:4],idx) = Q_cost([1:4],idx)-P_k; 83 | Q_cost(idx,[1:4]) = Q_cost(idx,[1:4])-P_k; 84 | end 85 | Q_cost=sparse(Q_cost); 86 | end 87 | 88 | function B = normalize_cols(A) 89 | mag = sum(A.^2,1).^(0.5); 90 | B = A./mag; 91 | end 92 | 93 | function f_est = cost_org(a,b,betasq,q) 94 | R = quat2rot(q); 95 | residuals = sum((b - R*a).^2,1); 96 | f_est = sum( min(residuals(:),betasq) ); 97 | end 98 | 99 | function P = getP() 100 | P = [1, 0, 0, 0, 0, -1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 1; 101 | 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0; 102 | 0, 0, 1, 0, 0, 0, 0, -1, 1, 0, 0, 0, 0, -1, 0, 0; 103 | 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, -1, 0, 0, -1, 0; 104 | -1, 0, 0, 0, 0, 1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 1; 105 | 0, 0, 0, 1, 0, 0, 1, 0, 0, 1, 0, 0, 1, 0, 0, 0; 106 | 0, 0, 1, 0, 0, 0, 0, 1, 1, 0, 0, 0, 0, 1, 0, 0; 107 | 0, 0, 0, -1, 0, 0, 1, 0, 0, 1, 0, 0, -1, 0, 0, 0; 108 | -1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1]; 109 | end 110 | 111 | function unitTest(R_gt,theta_gt,N,v1,v2,barc2,C) 112 | %% build X 113 | q = rotm2quat(R_gt); q = [q(2:4),q(1)]'; 114 | w = (theta_gt + ones(size(theta_gt))) ./ 2; 115 | v = kron([1;w],q); 116 | X = v*v'; 117 | n = 4*(N+1); 118 | P=getP(); 119 | 120 | %% check A,b 121 | for k=1:N 122 | Q_cost = zeros(n,n); 123 | % w_i (||b_i||^2+||a_i||^2 - barcq) + barcq - 2*b_i'*R_i*a_i 124 | % barcq: 125 | Q_cost([1:4],[1:4]) = Q_cost([1:4],[1:4]) + barc2*eye(4); 126 | 127 | % w_i (||b_i||^2+||a_i||^2 - barcq): 128 | idx = blkIndices(k+1,4); 129 | ck = ( v1(:,k)'*v1(:,k)+v2(:,k)'*v2(:,k) - barc2 ); 130 | Q_cost(idx,idx) = Q_cost(idx,idx) + ck*eye(4); 131 | 132 | % - 2*b_i'*R_i*a_i 133 | P_k = reshape(P'*reshape(v2(:,k)*v1(:,k)',[9,1]),[4,4]); 134 | Q_cost([1:4],idx) = Q_cost([1:4],idx)-P_k; 135 | Q_cost(idx,[1:4]) = Q_cost(idx,[1:4])-P_k; 136 | 137 | cost_expected = w(k)*norm(v2(:,k) - R_gt*v1(:,k))^2 + (1-w(k))*barc2; 138 | 139 | assert(abs( trace(Q_cost * X) - cost_expected )<1e-6) 140 | end 141 | 142 | assert(abs( trace(C * X) - cost_org(v1,v2,barc2,q) )<1e-6) 143 | end 144 | 145 | -------------------------------------------------------------------------------- /lib/solveWahba_TLS_m1p1.m: -------------------------------------------------------------------------------- 1 | %% QUASAR: solve SDP TLS relaxation using cvx and parametrization with theta in {-1,+1} 2 | 3 | v1=problem.a; 4 | v2=problem.b; 5 | betasq=problem.betasq; 6 | 7 | N = size(v1,2); % nr vectors (measurements) 8 | % X = [q\tran, q_1\tran, q_2\tran, ..., q_N\tran] has size 1 by Npm, where Npm = 4+4*N 9 | 10 | %% solve SDP relaxation using CVX interface and MOSEK solver 11 | %% The syntax here is almost exactly the same as the mathematical program 12 | %% in Proposition 8 and eq. (20) of the ICCV paper 13 | C = cost(v1,v2,betasq); 14 | n = 4*N + 4; 15 | 16 | cvx_begin % sdp 17 | cvx_solver mosek 18 | variable X(n,n) symmetric 19 | minimize( trace(C * X) ) 20 | subject to 21 | X == semidefinite(n); 22 | trace(X((1:4),(1:4))) == 1; 23 | for k=1:N 24 | idx = 4+blkIndices(k,4); 25 | X(idx,idx) == X((1:4),(1:4)); 26 | end 27 | for k1=1:N 28 | for k2=k1+1:N+1 29 | idx1 = blkIndices(k1,4); 30 | idx2 = blkIndices(k2,4); 31 | X(idx1,idx2) == X(idx1,idx2)'; 32 | end 33 | end 34 | cvx_end 35 | 36 | % extract solution 37 | f_sdp = cvx_optval; % lower bound 38 | [V,~] = sorteig(X); 39 | v = V(:,1); 40 | q = normalize_cols( v(1:4) ); 41 | theta = zeros(N,1); 42 | for i = 1:N 43 | theta(i) = sign(q'*v(blkIndices(i+1,4))); 44 | end 45 | R_err = getAngularError(quat2rot(q),R_gt); 46 | f_est = cost_org(v1,v2,betasq,q); % upper bound 47 | subopt = abs(f_est - f_sdp) / (1+abs(f_est)+abs(f_sdp)); 48 | 49 | eta = subopt; 50 | R_est = quat2rot(q); 51 | fprintf('f_sdp: %3.4e, f_est: %3.4e, R_err: %3.2e[deg].\n',f_sdp,f_est,R_err); 52 | fprintf('Relative suboptimality: %3.2e.\n',subopt); 53 | 54 | 55 | %% ********************************************* 56 | %% ********************************************* 57 | %% helper function 58 | function Q_cost = cost(v1,v2,barc2) 59 | P=[1, 0, 0, 0, 0, -1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 1; 60 | 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0; 61 | 0, 0, 1, 0, 0, 0, 0, -1, 1, 0, 0, 0, 0, -1, 0, 0; 62 | 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, -1, 0, 0, -1, 0; 63 | -1, 0, 0, 0, 0, 1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 1; 64 | 0, 0, 0, 1, 0, 0, 1, 0, 0, 1, 0, 0, 1, 0, 0, 0; 65 | 0, 0, 1, 0, 0, 0, 0, 1, 1, 0, 0, 0, 0, 1, 0, 0; 66 | 0, 0, 0, -1, 0, 0, 1, 0, 0, 1, 0, 0, -1, 0, 0, 0; 67 | -1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1]; 68 | P=sparse(P); 69 | N = size(v1,2); 70 | Npm = 4*N + 4; 71 | Q_1=zeros(Npm,Npm); 72 | for k=1:N 73 | idx = 4+blkIndices(k,4); 74 | P_k = reshape(P'*reshape(v2(:,k)*v1(:,k)',[9,1]),[4,4]); 75 | ck = 0.5 * ( v1(:,k)'*v1(:,k)+v2(:,k)'*v2(:,k) - barc2 ); 76 | Q_1((1:4),idx) = Q_1((1:4),idx)-0.5*P_k+ck/2*eye(4); 77 | Q_1(idx,(1:4)) = Q_1(idx,(1:4))-0.5*P_k+ck/2*eye(4); 78 | end 79 | 80 | Q_2=zeros(Npm,Npm); 81 | for k=1:N 82 | % idx = 4+blkIndices(k,4); 83 | idx = blkIndices(1,4); 84 | P_k = reshape(P'*reshape(v2(:,k)*v1(:,k)',[9,1]),[4,4]); 85 | ck = 0.5 * ( v1(:,k)'*v1(:,k)+v2(:,k)'*v2(:,k) + barc2 ); 86 | Q_2(idx,idx) = Q_2(idx,idx) - P_k + ck*eye(4); 87 | end 88 | Q_cost=Q_1+Q_2; 89 | Q_cost=sparse(Q_cost); 90 | 91 | Q_cost=Q_cost/barc2; 92 | end 93 | 94 | function B = normalize_cols(A) 95 | mag = sum(A.^2,1).^(0.5); 96 | B = A./mag; 97 | end 98 | 99 | function f_est = cost_org(a,b,betasq,q) 100 | R = quat2rot(q); 101 | residuals = sum((b - R*a).^2,1)/betasq; 102 | f_est = sum( min(residuals(:),1) ); 103 | end 104 | 105 | 106 | -------------------------------------------------------------------------------- /lib/solveWahba_listDecodable_01.m: -------------------------------------------------------------------------------- 1 | %% solve SDP relaxation for List Decodable Regression using cvx (with w in {0;1}) 2 | % Algorithm 5 in the paper "Estimation Contracts for Outlier-Robust Geometric Perception" 3 | % but using a sparse order 2 moment relaxation 4 | 5 | v1=problem.a; 6 | v2=problem.b; 7 | betasq=problem.betasq; 8 | 9 | N = size(v1,2); % nr vectors (measurements) 10 | %% sparse monomial basis 11 | % X = [1 w_1 ... w_N q\tran, q_1\tran, q_2\tran, ..., q_N\tran] has size n = (N+1)+4*(N+1) 12 | n = (N+1)+4*(N+1); 13 | 14 | %% solve SDP relaxation 15 | % w_i || v1 - R*v1 ||^2 <= barc2 constraints 16 | [A,b] = maxResidualConstraints(v1,v2,betasq); 17 | 18 | % sum_i w_i == alpha * N 19 | if recoverAllHypotheses == 0 20 | alpha = 1-outrate; 21 | else 22 | alpha = min(outrate,1-outrate) 23 | end 24 | assert(abs(round(alpha*N) - alpha*N)<1e-7, 'alpha*N is not integer'); % check it's integer 25 | [Aalpha,balpha] = fixedNumberInliers(N,alpha); 26 | 27 | % check that constraints are correct using gt as a feasible solution 28 | unitTest(R_gt,problem.theta_gt(:),N,alpha,v1,v2,betasq,A,b,Aalpha,balpha,isAdversarial); 29 | 30 | offset = N+1; % shortcut to access part of the moment matrix 31 | q_ind = [1:4]+offset; 32 | 33 | tstart = tic; 34 | 35 | %% solve relaxation 36 | cvx_begin % sdp 37 | cvx_solver mosek 38 | variable X(n,n) symmetric 39 | minimize( norm( X(2:N+1,1) ) ) % norm(w) % quad_form( X(2:N+1,1), eye(N) ) ) % norm(w) 40 | subject to 41 | X == semidefinite(n); 42 | X(1,1) == 1; 43 | trace(X(q_ind,q_ind)) == 1; 44 | for k=1:N 45 | %% moment constraints 46 | % top left diagonal (w_i^2 = w_i) 47 | X(k+1,k+1) == X(1,k+1); 48 | % bottom right block diagonal (q_i q_i' = w_i^2 q q' = w_i q q' = q q_i) 49 | idx = blkIndices(k+1,4)+offset; 50 | X(idx,idx) == X(q_ind,idx); 51 | % intermediate entries (q_i' = w_i q') 52 | X(1,idx) == X(k+1,q_ind); 53 | % intermediate entries (w_i q_i' = w_i^2 q = w_i q = q_i) 54 | X(k+1,idx) == X(1,idx); 55 | 56 | %% max residual constraint 57 | trace( A(:,:,k) * X ) <= b(k); % max residual constraints 58 | end 59 | %% structural constraints 60 | for k1=1:N 61 | for k2=k1+1:N+1 62 | % symmetric off diagonal 63 | idx1 = blkIndices(k1,4)+offset; 64 | idx2 = blkIndices(k2,4)+offset; 65 | X(idx1,idx2) == X(idx1,idx2)'; 66 | % tie theta_i * theta_j to trace of q_i * q_j' (including theta_i vs q * q_i 67 | X(k1,k2) == trace( X(idx1,idx2) ); 68 | end 69 | end 70 | %% desired fraction of inliers 71 | trace( Aalpha * X ) == balpha; 72 | cvx_end 73 | 74 | %% inspect optimal solution of SDP 75 | f_sdp = cvx_optval; % lower bound 76 | e = eig(X); 77 | disp('top 5 eigenvalues') 78 | e(end-5:end) 79 | if isnan(f_sdp)==1 80 | warning('cvx failed to solve correctly') 81 | end 82 | 83 | %% Compute relaxation gap 84 | f_est = sqrt(alpha*N); % optimal cost of original problem (with simple norm) is always norm of w and w has apha*N nonzero entries = 1 85 | subopt = abs(f_est - f_sdp) / (1+abs(f_est)+abs(f_sdp)); 86 | eta_norm = subopt; 87 | 88 | % optimal cost of original problem (with squared norm) is always alpha n 89 | eta = abs(f_est^2 - f_sdp^2) / (1+abs(f_est^2)+abs(f_sdp^2)); 90 | 91 | %% Rounding (similar to Algorith 5 in the estimation contracts paper, 92 | % but solution is also normalized here 93 | PExp_w = X([2:N+1],1); 94 | PExp_xw = zeros(4,N); 95 | v = zeros(4,N); % vectors v as in Algorithm 5 96 | p = zeros(N,1); % probabilities 97 | for k=1:N 98 | idx = blkIndices(k+1,4)+offset; 99 | PExp_xw(:,k) = X(idx,1); 100 | if PExp_w(k) > 1e-7 101 | v(:,k) = PExp_xw(:,k) / PExp_w(k); 102 | p(k) = PExp_w(k); 103 | else % if zero or negative 104 | if PExp_w(k) < -1e-7 105 | PExp_w 106 | error('PExp_w should not be negative') 107 | end 108 | v(:,k) = zeros(4,1); 109 | p(k) = 0; 110 | end 111 | end 112 | %% create list 113 | p = p / sum(p); % make into a valid distribution 114 | K = N; % number of hypotheses in the list (we consider N hypotheses here) 115 | % list = randsample(N,K,true,p); 116 | list = [1:N]; 117 | v_list = v(:,list); 118 | q_list = normalize_cols( v_list ); 119 | R_est_list = zeros(3,3,K); 120 | for i=1:K 121 | qi = q_list(:,i); 122 | R_est_list(:,:,i) = quat2rot(qi); 123 | end 124 | 125 | timeSLIDE = toc(tstart); 126 | 127 | %% case of multiple objects in the data 128 | % visualize 129 | q_gt_list = zeros(4,size(R_gt_list,3)); 130 | for j = 1:size(R_gt_list,3) 131 | % get ground truth quaternion for that object 132 | q_gt_j = rotm2quat(R_gt_list(:,:,j))'; 133 | q_gt_list(:,j)= [q_gt_j(2:4); q_gt_j(1)]; % put scalar part last 134 | end 135 | q_gt_list 136 | q_list 137 | 138 | %% compute minimum errors 139 | R_err_rot_deg_list = zeros(K,size(R_gt_list,3)); % nr hypotheses vs nr objects 140 | R_err_norm_list = zeros(K,size(R_gt_list,3)); % nr hypotheses vs nr objects 141 | for j = 1:size(R_gt_list,3) % nr objects 142 | for i=1:K % hypotheses 143 | qi = q_list(:,i); % i-th hypothesis 144 | R_err_rot_deg_list(i,j) = getAngularError(quat2rot(qi),R_gt_list(:,:,j)); 145 | R_err_norm_list(i,j) = norm( vec(quat2rot(qi)) - vec(R_gt_list(:,:,j)) ); 146 | end 147 | fprintf('object nr: %d, R_err_j: %3.2g[deg].\n',j,min(R_err_rot_deg_list(:,j))); 148 | end 149 | 150 | %% get error for desired (first) object 151 | R_errs_obj1 = R_err_rot_deg_list(:,1); % error for first object vs hypotheses 152 | [minVal,minInd] = min(R_errs_obj1); 153 | R_err = minVal; 154 | q = q_list(:,minInd); 155 | R_est = quat2rot(q); 156 | fprintf('f_sdp: %3.4e, f_est: %3.4e, R_err: %3.2g[deg].\n',f_sdp,f_est,R_err); 157 | fprintf('Relative suboptimality: %3.2e.\n',subopt); 158 | 159 | %% ********************************************* 160 | %% ********************************************* 161 | %% helper function 162 | function f_est = cost_org(a,b,betasq,q) 163 | R = quat2rot(q); 164 | residuals = sum((b - R*a).^2,1); 165 | f_est = sum( min(residuals(:),betasq) ); 166 | end 167 | 168 | function P = getP() 169 | P = [1, 0, 0, 0, 0, -1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 1; 170 | 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0; 171 | 0, 0, 1, 0, 0, 0, 0, -1, 1, 0, 0, 0, 0, -1, 0, 0; 172 | 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, -1, 0, 0, -1, 0; 173 | -1, 0, 0, 0, 0, 1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 1; 174 | 0, 0, 0, 1, 0, 0, 1, 0, 0, 1, 0, 0, 1, 0, 0, 0; 175 | 0, 0, 1, 0, 0, 0, 0, 1, 1, 0, 0, 0, 0, 1, 0, 0; 176 | 0, 0, 0, -1, 0, 0, 1, 0, 0, 1, 0, 0, -1, 0, 0, 0; 177 | -1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1]; 178 | end 179 | 180 | function [A,b] = maxResidualConstraints(v1,v2,barc2) 181 | P=getP(); 182 | P=sparse(P); 183 | N = size(v1,2); 184 | extras = N+1; 185 | n = extras + 4*(N+1); 186 | A = zeros(n,n,N); 187 | b = zeros(N,1); 188 | for k=1:N 189 | % w_i (||b_i||^2+||a_i||^2 - 2*b_i'*R_i*a_i) \leq barc2 190 | % w_i (||b_i||^2+||a_i||^2): 191 | idx = blkIndices(k+1,4) + extras; 192 | ck = ( v1(:,k)'*v1(:,k)+v2(:,k)'*v2(:,k) ); 193 | A(idx,idx,k) = + ck*eye(4); 194 | 195 | % - 2*b_i'*R_i*a_i 196 | P_k = reshape(P'*reshape(v2(:,k)*v1(:,k)',[9,1]),[4,4]); 197 | A([1:4]+ extras,idx,k) = -P_k; 198 | A(idx,[1:4]+ extras,k) = -P_k; 199 | 200 | b(k) = barc2; 201 | end 202 | end 203 | 204 | function [Aalpha,balpha] = fixedNumberInliers(N,alpha) % sum w = alpha * N 205 | n = N+1 + 4*(N+1); 206 | Aalpha=zeros(n,n); 207 | Aalpha(1,2:N+1) = ones(1,N); 208 | balpha = alpha * N; 209 | Aalpha = sparse(Aalpha); 210 | end 211 | 212 | function B = normalize_cols(A) 213 | mag = sum(A.^2,1).^(0.5); 214 | B = A./mag; 215 | end 216 | 217 | function unitTest(R_gt,theta_gt,N,alpha,v1,v2,betasq,A,b,Aalpha,balpha,isAdversarial) 218 | %% build X 219 | q = rotm2quat(R_gt); q = [q(2:4),q(1)]'; 220 | w = (theta_gt + ones(size(theta_gt))) ./ 2; 221 | v = [[1;w] ; kron([1;w],q)]; 222 | X = v*v'; 223 | extras = N+1; 224 | q_ind = [1:4]+extras; 225 | n = extras + 4*(N+1); 226 | P=getP(); 227 | 228 | %% check Aalpha,balpha 229 | if isAdversarial == 0 % in this case theta_gt is not reliable 230 | assert( abs( trace(Aalpha * X) - balpha )<1e-7 ) 231 | end 232 | 233 | %% check A,b 234 | for k=1:N 235 | actual = trace( A(:,:,k) * X ); 236 | expected = w(k) * norm(v2(:,k) - R_gt * v1(:,k) )^2; 237 | 238 | assert(abs( actual - expected )<1e-6) 239 | assert(abs( b(k) - betasq )<1e-6) 240 | end 241 | 242 | %% check constraints 243 | assert ( abs( trace(X(q_ind,q_ind)) - 1 ) <= 1e-6 ) ; 244 | assert ( abs( X(1,1) - 1 ) <= 1e-6 ); 245 | for k=1:N 246 | % top left diagonal (w_i^2 = w_i) 247 | assert ( norm( X(k+1,k+1) - X(1,k+1) ) <= 1e-6 ); 248 | % bottom right block diagonal (q_i q_i' = w_i^2 q q' = w_i q q' = q q_i) 249 | idx = blkIndices(k+1,4)+extras; 250 | assert ( norm( X(idx,idx) - X(q_ind,idx) ) <= 1e-6 ); 251 | % intermediate entries (q_i' = w_i q') 252 | assert ( norm( X(1,idx) - X(k+1,q_ind) ) <= 1e-6 ); 253 | % intermediate entries (w_i q_i' = w_i^2 q = w_i q = q_i) 254 | assert ( norm( X(k+1,idx) - X(1,idx) ) <= 1e-6 ); 255 | end 256 | for k1=1:N 257 | for k2=k1+1:N+1 258 | % symmetric off diagonal 259 | idx1 = blkIndices(k1,4)+extras; 260 | idx2 = blkIndices(k2,4)+extras; 261 | assert ( norm( X(idx1,idx2) - X(idx1,idx2)' ) <= 1e-6 ); 262 | % tie theta_i * theta_j to trace of q_i * q_j' (including theta_i vs q * q_i 263 | assert ( norm( X(k1,k2) - trace( X(idx1,idx2) ) ) <= 1e-6 ); 264 | end 265 | end 266 | end 267 | 268 | -------------------------------------------------------------------------------- /lib/testAntiConcentration.m: -------------------------------------------------------------------------------- 1 | function [isAntiConcentrated,timeTest1,timeTest2] = testAntiConcentration(problem, eta_input,isModifiedWahba) 2 | 3 | %% set (C, delta, M)-certifiable anti-concentration parameters according to Prop 12: 4 | N = problem.N; 5 | inliersIds = setdiff([1:N], problem.outliersIds); % anti-concentration only involves the inliers 6 | nrInliers = N - problem.nrOutliers; 7 | alpha = 1-problem.outrate; % inlier rate 8 | barc = sqrt(problem.betasq); % maximum error for inliers 9 | 10 | Mx = sqrt(3); % bound on ||x|| 11 | M = 2*Mx; % bound on || x - x_gt || 12 | 13 | if nargin < 2 14 | alphaBar = 0.7; 15 | eta = (2/alphaBar) * (2 - 2* (1-alphaBar)/(alphaBar)); % smallest eta such that bound is informative 16 | else 17 | warning('using input eta') 18 | eta = eta_input 19 | end 20 | if nargin<3 21 | isModifiedWahba = 0; 22 | end 23 | 24 | delta = 2*barc; 25 | C = alpha^2 * eta^2 * (1-2*barc)^2 / (32*barc); 26 | C_delta_Msq = C*delta*M^2 27 | 28 | %% Matrices that have to satisfy property 29 | At = problem.At; 30 | 31 | if isModifiedWahba 32 | a = problem.a; 33 | At = zeros(9,9,N); 34 | for i=1:N 35 | a2 = (eye(3) - a(:,i) * a(:,i)') * rand(3,1); 36 | a2 = a2 / norm(a2); 37 | % check orthogonality and unit norm: 38 | if abs( norm(a2) - 1) > 1e-4 || abs( a(:,i)' * a2 ) > 1e-4 39 | i 40 | norm(a2) 41 | abs( a(:,i)' * a2 ) 42 | error('error in a2 generation') 43 | end 44 | a3 = cross(a(:,i),a2); 45 | At(:,:,i) = [kron( a(:,i)' , eye(3) ) ; kron( a2' , eye(3) ) ; kron( a3' , eye(3) )]; 46 | 47 | assert( norm(At(:,:,i) * At(:,:,i)' - eye(9)) <1e-6 ); % check that matrix is orthogonal 48 | end 49 | end 50 | 51 | %% coefficients of polynomial p (assumed degree 2): p(x) = 1 + c2*x^2 52 | c2 = -0.1; 53 | 54 | %% prepare variables for sostools (to check sos proofs) 55 | pvar v1 v2 v3 v4 v5 v6 v7 v8 v9 56 | v = [v1;v2;v3;v4;v5;v6;v7;v8;v9]; 57 | 58 | %% check anti-concentration -- condition 1: 59 | A = []; 60 | degree = 4; % for this is typically enough to check deg 4 61 | isAntiConcentrated = 1; 62 | timeTest1 = 0; 63 | nrRuns = 0; 64 | for i=1:length(inliersIds) 65 | ind = inliersIds(i); 66 | normsq_Ai_v(i) = sum( (At(:,:,ind) * v ).^2 ); % ||Ai' v||^2 67 | 68 | %% check anti-concentration -- condition 1: 69 | %p(i) = 1 + c2 * normsq_Ai_v(i) + c4 * normsq_Ai_v(i)^2; % c0 = 1 (by definition) 70 | p(i) = 1 + c2 * normsq_Ai_v(i); 71 | 72 | tstart = tic; 73 | [gam1,vars,opt] = findbound( p(i)^2 - (1-delta)^2, [delta^2-normsq_Ai_v(i)],[],degree); 74 | timeTest1 = timeTest1+toc(tstart); 75 | nrRuns = nrRuns + 1; 76 | if gam1 < 0 77 | isAntiConcentrated = 0; 78 | warning('found an Ai that is not anti-concentrated (condition 1') 79 | end 80 | gam1 81 | 82 | A = [ A;At(:,:,ind) ]; 83 | end 84 | timeTest1 = timeTest1/nrRuns; 85 | 86 | if isAntiConcentrated == 1 % if we haven't found a case already making the condition false 87 | %% check anti-concentration -- condition 2: 88 | degree = 6; % here we need at least degree 6 89 | normsq_v = sum(v.^2); 90 | 91 | tstart = tic; 92 | [gam2,vars,opt] = findbound( C_delta_Msq - normsq_v * (1/nrInliers) * sum(p.^2),[M^2 - normsq_v],[],degree); 93 | timeTest2 = toc(tstart); 94 | gam2 95 | if gam2 < 0 96 | isAntiConcentrated = 0; 97 | warning('not anti-concentrated (condition 2') 98 | end 99 | end 100 | 101 | -------------------------------------------------------------------------------- /lib/testHypercontractivity.m: -------------------------------------------------------------------------------- 1 | function [isHyperContractive,Q,Z,timeTest] = testHypercontractivity(At,Ct_pow_t_input) 2 | 3 | %% create polynomial in definition of hyper-contractivity 4 | N = size(At,3); % nr measurements 5 | 6 | pvar v1 v2 v3 v4 v5 v6 v7 v8 v9 7 | v = [v1;v2;v3;v4;v5;v6;v7;v8;v9]; 8 | for i=1:N 9 | normsq_Ai_v(i) = sum( ( At(:,:,i) * v ).^2 ); % || A_i' v ||^2 10 | end 11 | 12 | t = 2; 13 | % Ct = N^( (t-1) / t ) 14 | 15 | if nargin<2 16 | Ct_pow_t = N^( (t-1) ) % justified in the paper 17 | else 18 | warning('using user-specified Ct') 19 | Ct_pow_t = Ct_pow_t_input; 20 | end 21 | 22 | %% polynomial in eq (26) of the paper 23 | p_hyper = Ct_pow_t * ( (1/N) * sum( normsq_Ai_v ) )^t ... 24 | - (1/N) * sum( normsq_Ai_v.^t ); % degree 4 polynomial for t=2 25 | 26 | %% check if it is sos using SOSTOOLS 27 | tstart = tic; 28 | [Q,Z] = findsos(p_hyper); 29 | timeTest = toc(tstart); 30 | 31 | %% if sos, the decomposition Q, Z is not empty 32 | isHyperContractive = ~isempty(Q) && ~isempty(Z); % true if p_hyper is sos -------------------------------------------------------------------------------- /results/SLIDE_example1.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MIT-SPARK/estimation-contracts/3f07c0ebcb34dc0719b1f5b4dfa8b3d152d895f1/results/SLIDE_example1.mat -------------------------------------------------------------------------------- /results/SLIDE_example2.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MIT-SPARK/estimation-contracts/3f07c0ebcb34dc0719b1f5b4dfa8b3d152d895f1/results/SLIDE_example2.mat -------------------------------------------------------------------------------- /results/log_results_experiment1.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MIT-SPARK/estimation-contracts/3f07c0ebcb34dc0719b1f5b4dfa8b3d152d895f1/results/log_results_experiment1.mat -------------------------------------------------------------------------------- /results/log_results_experiment2.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MIT-SPARK/estimation-contracts/3f07c0ebcb34dc0719b1f5b4dfa8b3d152d895f1/results/log_results_experiment2.mat -------------------------------------------------------------------------------- /results/log_results_experiment2_SOSTOOLS400.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MIT-SPARK/estimation-contracts/3f07c0ebcb34dc0719b1f5b4dfa8b3d152d895f1/results/log_results_experiment2_SOSTOOLS400.mat -------------------------------------------------------------------------------- /results/log_results_experiment3.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MIT-SPARK/estimation-contracts/3f07c0ebcb34dc0719b1f5b4dfa8b3d152d895f1/results/log_results_experiment3.mat -------------------------------------------------------------------------------- /results/log_results_experiment6.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MIT-SPARK/estimation-contracts/3f07c0ebcb34dc0719b1f5b4dfa8b3d152d895f1/results/log_results_experiment6.mat -------------------------------------------------------------------------------- /results/log_results_experiment7_isModified_0.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MIT-SPARK/estimation-contracts/3f07c0ebcb34dc0719b1f5b4dfa8b3d152d895f1/results/log_results_experiment7_isModified_0.mat -------------------------------------------------------------------------------- /results/log_results_experiment7_isModified_1.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MIT-SPARK/estimation-contracts/3f07c0ebcb34dc0719b1f5b4dfa8b3d152d895f1/results/log_results_experiment7_isModified_1.mat -------------------------------------------------------------------------------- /results/log_results_experiment7_isModified_1_SOSTOOLS400.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MIT-SPARK/estimation-contracts/3f07c0ebcb34dc0719b1f5b4dfa8b3d152d895f1/results/log_results_experiment7_isModified_1_SOSTOOLS400.mat -------------------------------------------------------------------------------- /results/log_results_experiment8_isAdversarial_0_recoverAll_0.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MIT-SPARK/estimation-contracts/3f07c0ebcb34dc0719b1f5b4dfa8b3d152d895f1/results/log_results_experiment8_isAdversarial_0_recoverAll_0.mat -------------------------------------------------------------------------------- /results/log_results_experiment8_isAdversarial_1_recoverAll_0.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MIT-SPARK/estimation-contracts/3f07c0ebcb34dc0719b1f5b4dfa8b3d152d895f1/results/log_results_experiment8_isAdversarial_1_recoverAll_0.mat -------------------------------------------------------------------------------- /results/log_results_experiment8_isAdversarial_1_recoverAll_1.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MIT-SPARK/estimation-contracts/3f07c0ebcb34dc0719b1f5b4dfa8b3d152d895f1/results/log_results_experiment8_isAdversarial_1_recoverAll_1.mat --------------------------------------------------------------------------------