摄像机外参数标定位姿估计 pose estimation的经典文章和代码 EPNP

摄像机外参数标定（位姿估计 pose estimation）的经典文章和代码 EPNP

% MAIN Illustrates how to use the EPnP algorithm described in:
%
%       Francesc Moreno-Noguer Vincent Lepetit Pascal Fua.
%       Accurate Non-Iterative O（n） Solution to the PnP Problem. 
%       In Proceedings of ICCV 2007. 
%
% Copyright （C） <2007>  
% 
% 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 .
%
% Francesc Moreno-Noguer CVLab-EPFL September 2007.
% fmorenoguer@gmail.com http://cvlab.epfl.ch/~fmoreno/ 

clear all; close all;

addpath data;
addpath error;
addpath EPnP;


fprintf（‘\n---------EPnP--------------\n‘）;
%1.-Generate simulated input data------------------------------------------
load_points=0;
if ~load_points
    n=50; %number of points
    std_noise=10; %noise in the measurements （in pixels）
    [ApointRt]=generate_noisy_input_data（nstd_noise）;
    save（‘data\input_data_noise.mat‘‘A‘‘point‘‘Rt‘）;
else
    load（‘data\input_data_noise.mat‘‘A‘‘point‘‘Rt‘）;
    n=size（point2）;
    draw_noisy_input_data（point）;
end

%2.-Inputs format--------------------------------
x3d=zeros（n4）;
x2d=zeros（n3）; 
A=A（:1:3）;
for i=1:n
    x3d_h（i:）=[point（i）.Xworld‘1]; 
    x2d_h（i:）=[point（i）.Ximg（1:2）‘1];

    %world and camera coordinates
    X3d_world（i:）=point（i）.Xworld‘;
    X3d_cam（i:）=point（i）.Xcam‘;
end


%3.-EPnP----------------------------------------------------
Xw=x3d_h（:1:3）;
U=x2d_h（:1:2）;

[RpTpXcsol]=efficient_pnp（x3d_hx2d_hA）;

%draw Results
for i=1:n
    point（i）.Xcam_est=Xc（i:）‘;
end
figure; h=gcf;
plot_3d_reconstruction（point‘EPnP （Old）‘h）;
xlim（[-2 2]）; ylim（[-2 2]）;

%compute error
error=reprojection_error_usingRT（XwURpTpA）;
fprintf（‘error EPnP: %.3f\n‘error）;


%3.-EPnP_GAUSS_NEWTON----------------------------------------------------
Xw=x3d_h（:1:3）;
U=x2d_h（:1:2）;

[RpTpXcsol]=efficient_pnp_gauss（x3d_hx2d_hA）;

%draw Results
for i=1:n
    point（i）.Xcam_est=Xc（i:）‘;
end
figure; h=gcf;
plot_3d_reconstruction（point‘EPnP Gauss Newton‘h）;

%compute error
error=reprojection_error_usingRT（XwURpTpA）;
fprintf（‘error EPnP_Gauss_Newton: %.3f\n‘error）;
xlim（[-2 2]）; ylim（[-2 2]）;

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     目录           0  2017-07-03 20:40  EPnP_matlab\
     目录           0  2017-07-03 20:40  EPnP_matlab\EPnP\
     文件        1522  2007-10-27 10:17  EPnP_matlab\EPnP\compute_A_and_b_Gauss_Newton.m
     文件        6792  2007-10-27 10:35  EPnP_matlab\EPnP\compute_L6_10.m
     文件        1466  2007-09-05 15:02  EPnP_matlab\EPnP\compute_M_ver2.m
     文件        1050  2007-09-05 15:02  EPnP_matlab\EPnP\compute_alphas.m
     文件        4130  2007-09-05 15:02  EPnP_matlab\EPnP\compute_constraint_distance_2param_6eq_3unk.m
     文件        7573  2007-09-05 15:02  EPnP_matlab\EPnP\compute_constraint_distance_3param_6eq_6unk.m
     文件       21501  2007-09-05 15:02  EPnP_matlab\EPnP\compute_constraint_distance_orthog_4param_9eq_10unk.m
     文件        1969  2007-10-27 09:54  EPnP_matlab\EPnP\compute_norm_sign_scaling_factor.m
     文件        2639  2007-09-05 15:03  EPnP_matlab\EPnP\compute_permutation_constraint4.m
     文件        1044  2007-10-27 10:20  EPnP_matlab\EPnP\compute_rho.m
     文件         880  2007-09-05 15:03  EPnP_matlab\EPnP\define_control_points.m
     文件        1328  2007-09-05 15:03  EPnP_matlab\EPnP\define_distances_btw_control_points.m
     文件         865  2007-10-27 10:35  EPnP_matlab\EPnP\dist2.m
     文件        6775  2007-10-27 10:21  EPnP_matlab\EPnP\efficient_pnp.m
     文件        8268  2007-10-27 10:04  EPnP_matlab\EPnP\efficient_pnp_gauss.m
     文件        1465  2007-10-27 10:16  EPnP_matlab\EPnP\gauss_newton.m
     文件         537  2007-09-05 12:45  EPnP_matlab\EPnP\getrotT.m
     文件         878  2007-09-05 15:04  EPnP_matlab\EPnP\kernel_noise.m
     文件        1733  2007-10-27 10:14  EPnP_matlab\EPnP\optimize_betas_gauss_newton.m
     文件        1170  2007-09-05 15:04  EPnP_matlab\EPnP\reprojection_error_usingRT.m
     文件         985  2007-10-27 10:23  EPnP_matlab\EPnP\sign_determinant.m
     目录           0  2017-07-03 20:40  EPnP_matlab\data\
     文件        1249  2007-09-05 15:01  EPnP_matlab\data\draw_noisy_input_data.m
     文件        3981  2007-09-05 15:01  EPnP_matlab\data\generate_noisy_input_data.m
     文件        5354  2017-06-30 15:48  EPnP_matlab\data\input_data_noise.mat
     文件         936  2007-09-05 15:01  EPnP_matlab\data\project_3d_2d.m
     文件         889  2007-09-05 15:01  EPnP_matlab\data\random.m
     文件        1307  2007-09-05 15:01  EPnP_matlab\data\return_Rt_matrix.m
     文件         915  2007-09-05 15:01  EPnP_matlab\data\transform_3d.m
............此处省略5个文件信息