PnP位姿估计matlab工具包

最新的位姿估计工具箱，对应文献为：Very Fast Solution to the PnP Problem with Algebraic Outlier Rejection。PnP问题是计算机视觉中非常基础的经典问题，可以用来测量目标的三维位置姿态。

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% This toolbox illustrates how to use the REPPnP and EPPnP 
% algorithms described in:
%
%       Luis Ferraz Xavier Binefa Francesc Moreno-Noguer.
%       Very Fast Solution to the PnP Problem with Algebraic Outlier Rejection. 
%       In Proceedings of CVPR 2014. 
%
% Copyright （C） <2014>  
% 
% This program is free software: you can redistribute it and/or modify
% it under the terms of the version 3 of the GNU General Public License
% as published by the Free Software Foundation.
% 
% This program is distributed in the hope that it will be useful but
% WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
% General Public License for more details.       
% You should have received a copy of the GNU General Public License
% along with this program. If not see .
%
% Luis Ferraz CMTech-UPF June 2014.
% luisferrazc@gmail.comhttp://cmtech.upf.edu/user/62
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

clear; clc;
IniToolbox;

%load data 
load ./data/mymodel model
load ./data/mytest test

%inliers-3D points
U = test.modelcorresp（1:3:）; %set of features matched with the model


%inliers-2D points
pt = test.modelcorresp（6:7:）;
u = model.K \ [pt;ones（1size（pt2））];
u = u（1:2:）;

% compared methods
name= {‘RNSC P3P‘‘RNSC RP4P RPnP‘‘RNSC P3P OPnP‘‘RNSC P3P ASPnP‘ ‘REPPnP‘};
f = {@kP3P@RPnP@kP3P@kP3P@REPPnP};
f2 = {[]@RPnP@OPnP@ASPnP[]}; %post ransac method
ransacsamples = {34330}; %number of samples to apply ransac
marker= {‘o‘‘d‘‘>‘‘<‘‘o‘};
color= {‘g‘[10.50]‘c‘‘b‘‘k‘};
markerfacecolor=  {‘g‘[10.50]‘c‘‘b‘‘n‘};

method_list= struct（‘name‘ name ‘f‘ f‘f2‘ f2 ‘ransac‘ransacsamples...
    ‘c‘ 0...
    ‘mask_inliers‘ones（1size（u2））...
    ‘marker‘ marker ‘color‘ color ‘markerfacecolor‘ markerfacecolor）;


%estimate camera pose 
disp（‘Each method is evaluated 100 times to compute the mean cost‘）;
for k = 1:length（name）

    try
        %experiments are evaluated 10 times
        for i = 1:100
        if strcmp（method_list（k）.name ‘REPPnP‘）
            sigmaerr = 10;
            coef     = 1.4;
            minerror = （coef * sigmaerr）/model.K（1）;
            
            tic;
            mU = U - repmat（mean（U2）1size（U2））;
            [R1 t1 mask] = method_list（k）.f（mUuminerror）;
            t1 = t1 - R1 * mean（U2）;
            tcost = toc * 1000;
            method_list（k）.mask_inliers = mask;
        else
          if （method_list（k）.ransac == 0）
              tic;
              [R1t1]= method_list（k）.f（Uu）;
              tcost = toc* 1000;
          else
              thr = 10;
              s = method_list（k）.ransac;
              tic;
              [R1 t1 inliers] = ransac（mo

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     目录           0  2014-05-22 15:47  \PnP_Toolbox\
     目录           0  2014-05-22 15:47  \PnP_Toolbox\ASPnP\
     目录           0  2014-05-22 15:47  \PnP_Toolbox\common_funcs\
     目录           0  2014-05-22 15:47  \PnP_Toolbox\data\
     文件        5290  2014-05-22 15:47  \PnP_Toolbox\demo_box_with_outliers_ransac.m
     目录           0  2014-05-22 15:47  \PnP_Toolbox\dls_pnp_matlab\
     目录           0  2014-05-22 15:47  \PnP_Toolbox\gOp\
     文件          54  2014-05-22 15:47  \PnP_Toolbox\IniToolbox.m
     文件        7217  2014-05-22 15:47  \PnP_Toolbox\main_ordinary_3d.m
     文件        8490  2014-05-22 15:47  \PnP_Toolbox\main_ordinary_3d_outliers.m
     文件       10343  2014-05-22 15:47  \PnP_Toolbox\main_ordinary_3d_outliers_ransac.m
     文件        9610  2014-05-22 15:47  \PnP_Toolbox\main_ordinary_3d_sigma.m
     文件        3570  2014-05-22 15:47  \PnP_Toolbox\main_ordinary_3d_time.m
     文件        7178  2014-05-22 15:47  \PnP_Toolbox\main_planar.m
     文件        8073  2014-05-22 15:47  \PnP_Toolbox\main_planar_outliers.m
     文件       10418  2014-05-22 15:47  \PnP_Toolbox\main_planar_outliers_ransac.m
     文件        7025  2014-05-22 15:47  \PnP_Toolbox\main_planar_sigma.m
     目录           0  2014-05-22 15:47  \PnP_Toolbox\OPnP\
     目录           0  2014-05-22 15:47  \PnP_Toolbox\p3p_code_final\
     目录           0  2014-05-22 15:47  \PnP_Toolbox\plot_funcs\
     文件        3342  2014-05-22 15:47  \PnP_Toolbox\plotEffectOutliersKernel.m
     目录           0  2014-05-22 15:47  \PnP_Toolbox\ppnp\
     文件        2685  2014-05-22 15:47  \PnP_Toolbox\ReadMe.txt
     目录           0  2014-05-22 15:47  \PnP_Toolbox\REPPnP\
     目录           0  2014-05-22 15:47  \PnP_Toolbox\rpnp\
     文件         461  2014-05-22 15:47  \PnP_Toolbox\ASPnP\appendimages.m
     文件        1642  2014-05-22 15:47  \PnP_Toolbox\ASPnP\ASPnP.m
     文件       19168  2014-05-22 15:47  \PnP_Toolbox\ASPnP\ASPnP_Case1_V2.m
     文件        4318  2014-05-22 15:47  \PnP_Toolbox\ASPnP\ASPnP_Case2_V2.m
     文件        1771  2014-05-22 15:47  \PnP_Toolbox\ASPnP\ASPnP_Case3_V2.m
     文件         753  2014-05-22 15:47  \PnP_Toolbox\ASPnP\ASPnP_Case4_V2.m
............此处省略595个文件信息