mono_slam.asv

移动机器人同时定位与地图创建最前沿技术

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% SLAM Summer School 2006, Oxford.
% Practical 3. SLAM using Monocular Vision.
% Practical exercise.
% J.M.M. Montiel, Javier Civera, Andrew J. Davison.
% {josemari, jcivera}@unizar.es, [e-mail]
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function varargout = mono_slam(varargin)
% MONO_SLAM M-file for mono_slam.fig
%      MONO_SLAM, by itself, creates a new MONO_SLAM or raises the existing
%      singleton*.
%
%      H = MONO_SLAM returns the handle to a new MONO_SLAM or the handle to
%      the existing singleton*.
%
%      MONO_SLAM('CALLBACK',hObject,eventData,handles,...) calls the local
%      function named CALLBACK in MONO_SLAM.M with the given input arguments.
%
%      MONO_SLAM('Property','Value',...) creates a new MONO_SLAM or raises the
%      existing singleton*.  Starting from the left, property value pairs are
%      applied to the GUI before mono_slam_OpeningFunction gets called.  An
%      unrecognized property name or invalid value makes property application
%      stop.  All inputs are passed to mono_slam_OpeningFcn via varargin.
%
%      *See GUI Options on GUIDE's Tools menu.  Choose "GUI allows only one
%      instance to run (singleton)".
%
% See also: GUIDE, GUIDATA, GUIHANDLES

% Edit the above text to modify the response to help mono_slam

% Last Modified by GUIDE v2.5 24-Aug-2006 20:49:20

% Begin initialization code - DO NOT EDIT
gui_Singleton = 1;
gui_State = struct('gui_Name',       mfilename, ...
                   'gui_Singleton',  gui_Singleton, ...
                   'gui_OpeningFcn', @mono_slam_OpeningFcn, ...
                   'gui_OutputFcn',  @mono_slam_OutputFcn, ...
                   'gui_LayoutFcn',  [] , ...
                   'gui_Callback',   []);
if nargin & isstr(varargin{1})
    gui_State.gui_Callback = str2func(varargin{1});
end

if nargout
    [varargout{1:nargout}] = gui_mainfcn(gui_State, varargin{:});
else
    gui_mainfcn(gui_State, varargin{:});
end
% End initialization code - DO NOT EDIT


% --- Executes just before mono_slam is made visible.
function mono_slam_OpeningFcn(hObject, eventdata, handles, varargin)
% This function has no output args, see OutputFcn.
% hObject    handle to figure
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
% varargin   command line arguments to mono_slam (see VARARGIN)

% Choose default command line output for mono_slam
handles.output = hObject;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% MONO_SLAM
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Set the parameters of the simulation,
% storing them in the data structure handles.
setParameters;

% Parameters you have to modify during the practical
% rho history data
handles.rhoHistory_5 = [];
handles.rhoHistory_15 = [];
handles.feature5History = [];
handles.rhoCovarianceHistory_5 = [];
handles.rhoCovarianceHistory_15 = [];
% Covariance definitions
handles.sigma_imageNoise = 1; % pixels
handles.sigma_aNoise     = 6; % m s^-2
handles.sigma_alphaNoise = 6; % rad s^-2
% Time between frames
handles.deltat = 1/30; %s

% Show the image, to select features in the first step
im = getImage( handles.sequencePath, handles.initialImage );
figure( handles.figure_all );
subplot(handles.figure_image);
hold off;
imagesc(im);
colormap gray;
hold on;
title('Pred&Matched:Red. Pred&NOTMatched:Cian. Match:Green');

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% MONO_SLAM
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

set(handles.state_text, 'String', 'Ready...');


% Update handles structure
guidata(hObject, handles);

% UIWAIT makes mono_slam wait for user response (see UIRESUME)
% uiwait(handles.figure1);


% --- Outputs from this function are returned to the command line.
function varargout = mono_slam_OutputFcn(hObject, eventdata, handles)
% varargout  cell array for returning output args (see VARARGOUT);
% hObject    handle to figure
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Get default command line output from handles structure
varargout{1} = handles.output;


% --- Executes on button press in EKF_step.
function EKF_step_Callback(hObject, eventdata, handles)
% hObject    handle to EKF_step (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

set(handles.state_text, 'String', 'Running, please wait...');

% Extract needed data from the data structure handles
sequencePath = handles.sequencePath;
initialImage = handles.initialImage;
step = handles.step;
x_k_k = handles.x_k_k;
p_k_k = handles.p_k_k;
deltat = handles.deltat;
sigma_aNoise = handles.sigma_aNoise;
sigma_alphaNoise = handles.sigma_alphaNoise;
sigma_imageNoise = handles.sigma_imageNoise;
cam = handles.cam;
featuresInfo = handles.featuresInfo;
camHistory = handles.camHistory;
figure_all = handles.figure_all;
figure_image = handles.figure_image;
figure_3D = handles.figure_3D;
randSphere6D = handles.randSphere6D;
nPointsRand = handles.nPointsRand;
chi2inv_table = handles.chi2inv_table;
rhoHistory_5 = handles.rhoHistory_5;
rhoHistory_15 = handles.rhoHistory_15;
rhoCovarianceHistory_5 = handles.rhoCovarianceHistory_5;
rhoCovarianceHistory_15 = handles.rhoCovarianceHistory_15;
feature5History = handles.feature5History;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% MONO_SLAM
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Monocular EKF step

if (initialImage + step-1) <= handles.numberOfImages
    
    % get an image
    im = getImage( sequencePath, initialImage + step-1 );
    
    % prediction
    [ x_km1_k, p_km1_k ] = prediction( x_k_k, p_k_k, deltat, sigma_aNoise, sigma_alphaNoise );
    
    % measurements prediction
    [ h_all, H_predicted, S ] = measurements_prediction( x_km1_k, p_km1_k, sigma_imageNoise, cam );
    
    % matching
    [ z, h, H_matching, R_matching, z_all ] = matching( h_all, S,...
        H_predicted, sigma_imageNoise, featuresInfo, cam, im, chi2inv_table );
    
    % update
    [ x_k_k, p_k_k ] = update( x_km1_k, p_km1_k, H_matching, R_matching, z, h );
    
    % save camera location
    camHistory(:,step) = [x_k_k(1:3); tr2rpy(q2tr(x_k_k(4:7)))'];
    
    % Here, the code to save history information of two features.
    if size( x_k_k, 1 ) >= (13+5*6)
        rho_estimated = x_k_k( (13+5*6) );
        feature5 = x_k_k( (13+5*6-5:13+5*6) );
        sigma_estimated = sqrt(p_k_k( (13+5*6), (13+5*6) ));
        rhoHistory_5 = [ rhoHistory_5, rho_estimated ];
        rhoCovarianceHistory_5 = [ rhoCovarianceHistory_5, sigma_estimated^2 ];
        feature5History = [feature5History, feature5];
        set(handles.feature5estimation, 'String', sprintf( '[%6.3g, %6.3g, %6.3g]',...
            1/(rho_estimated + 2*sigma_estimated),...
            1/rho_estimated,...
            1/(rho_estimated - 2*sigma_estimated) ));
    end
    if size( x_k_k, 1 ) >= (13+15*6)
        rho_estimated = x_k_k( (13+15*6) );
        sigma_estimated = sqrt(p_k_k( (13+15*6), (13+15*6) ));
        rhoHistory_15 = [ rhoHistory_15, rho_estimated ];
        rhoCovarianceHistory_15 = [ rhoCovarianceHistory_15, sigma_estimated^2 ];
        set(handles.feature15estimation, 'String', sprintf( '[%6.3g, %6.3g, %6.3g]',...
            1/(rho_estimated + 2*sigma_estimated),...
            1/rho_estimated,...
            1/(rho_estimated - 2*sigma_estimated) ));
    end
    save history.mat rhoHistory_5 rhoCovarianceHistory_5 rhoHistory_15 rhoCovarianceHistory_15 feature5History
    
    % plots
    plotresults( step, figure_all, figure_image, figure_3D,...
        x_k_k, p_k_k, h_all, S, z_all, featuresInfo, cam, im,...
        camHistory(:,1:step), randSphere6D, nPointsRand,...
        [handles.Xmin handles.Xmax -5 5 handles.Zmin handles.Zmax], chi2inv_table );
    
    step = step+1;
    
else
    
    plotresults( step, figure_all, figure_image, figure_3D,...
        x_k_k, p_k_k, [], [], [], featuresInfo, cam, [],...