function [l, L_rf, L_sf, L_obs, L_n, N] = retroProjLmk(Rob,Sen,Obs,Opt)

% RETROPROJLMK  Retro project landmark.

%   Copyright 2008-2009 Joan Sola @ LAAS-CNRS.


switch Sen.type

    % camera pinHole
    case {'pinHole'}
        % type of lmk to init
        switch Opt.init.initType
            case {'idpPnt'}
                % INIT LMK OF TYPE: Inverse depth point
                [l, L_rf, L_sf, L_k, L_c, L_obs, L_n] = ...
                    retroProjIdpPntFromPinHoleOnRob( ...
                    Rob.frame, ...
                    Sen.frame, ...
                    Sen.par.k, ...
                    Sen.par.c, ...
                    Obs.meas.y, ...
                    Opt.init.idpPnt.nonObsMean) ;

                N = Opt.init.idpPnt.nonObsStd^2 ;

            case {'hmgPnt'}
                % INIT LMK OF TYPE: Homogeneous point
                [l, L_rf, L_sf, L_k, L_c, L_obs, L_n] = ...
                    retroProjHmgPntFromPinHoleOnRob( ...
                    Rob.frame, ...
                    Sen.frame, ...
                    Sen.par.k, ...
                    Sen.par.c, ...
                    Obs.meas.y, ...
                    Opt.init.idpPnt.nonObsMean) ;

                N = Opt.init.idpPnt.nonObsStd^2 ;

            case {'ahmPnt'}
                % INIT LMK OF TYPE: Anchored Homogeneous point
                [l, L_rf, L_sf, L_k, L_c, L_obs, L_n] = ...
                    retroProjAhmPntFromPinHoleOnRob( ...
                    Rob.frame, ...
                    Sen.frame, ...
                    Sen.par.k, ...
                    Sen.par.c, ...
                    Obs.meas.y, ...
                    Opt.init.idpPnt.nonObsMean) ;

                N = Opt.init.idpPnt.nonObsStd^2 ;

            case {'fhmPnt'}
                % INIT LMK OF TYPE: Framed Homogeneous point
                [l, L_rf, L_sf, L_k, L_c, L_obs, L_n] = ...
                    retroProjFhmPntFromPinHoleOnRob( ...
                    Rob.frame, ...
                    Sen.frame, ...
                    Sen.par.k, ...
                    Sen.par.c, ...
                    Obs.meas.y, ...
                    Opt.init.idpPnt.nonObsMean) ;

                N = Opt.init.idpPnt.nonObsStd^2 ;

            case {'plkLin'}
                % INIT LMK OF TYPE: Plucker line
                [hm, HM_obs] = seg2hmgLin(Obs.meas.y);
                [l, L_rf, L_sf, L_k, L_hm, L_n] = ...
                    retroProjPlkLinFromPinHoleOnRob( ...
                    Rob.frame, ...
                    Sen.frame, ...
                    Sen.par.k, ...
                    hm, ...
                    Opt.init.plkLin.nonObsMean) ;
                L_obs = L_hm*HM_obs;

                N = diag(Opt.init.plkLin.nonObsStd.^2) ;
                
            case 'aplLin'
                % INIT LMK OF TYPE: Anchored Plucker line
                [hm, HM_obs] = seg2hmgLin(Obs.meas.y);
                [l, L_rf, L_sf, L_k, L_hm, L_n] = ...
                    retroProjAplLinFromPinHoleOnRob( ...
                    Rob.frame, ...
                    Sen.frame, ...
                    Sen.par.k, ...
                    hm, ...
                    Opt.init.plkLin.nonObsMean) ;
                L_obs = L_hm*HM_obs;

                N = diag(Opt.init.plkLin.nonObsStd.^2) ;
                
            case 'idpLin'
                % INIT LMK TYPE: Inverse-depth line.
                nMean = Opt.init.idpPnt.nonObsMean*[1;1]; % non-measured prior
                nStd  = Opt.init.idpPnt.nonObsStd*[1;1];
                
                [l, L_rf, L_sf, L_k, L_obs, L_n] = ...
                    retroProjIdpLinFromPinHoleOnRob( ...
                    Rob.frame, ...
                    Sen.frame, ...
                    Sen.par.k, ...
                    Obs.meas.y, ...
                    nMean) ;

                N = diag(nStd.^2);

            case 'hmgLin'
                % INIT LMK TYPE: Homogenous points line.
                nMean = Opt.init.idpPnt.nonObsMean*[1;1]; % non-measured prior
                nStd  = Opt.init.idpPnt.nonObsStd*[1;1];
                
                [l, L_rf, L_sf, L_k, L_obs, L_n] = ...
                    retroProjHmgLinFromPinHoleOnRob( ...
                    Rob.frame, ...
                    Sen.frame, ...
                    Sen.par.k, ...
                    Obs.meas.y, ...
                    nMean) ;

                N = diag(nStd.^2);

            case 'ahmLin'
                % INIT LMK TYPE: Anchored Homogenous points line.
                nMean = Opt.init.idpPnt.nonObsMean*[1;1]; % non-measured prior
                nStd  = Opt.init.idpPnt.nonObsStd*[1;1];
                
                [l, L_rf, L_sf, L_k, L_obs, L_n] = ...
                    retroProjAhmLinFromPinHoleOnRob( ...
                    Rob.frame, ...
                    Sen.frame, ...
                    Sen.par.k, ...
                    Obs.meas.y, ...
                    nMean) ;

                N = diag(nStd.^2);

                
            otherwise
                error('??? Unknown landmark type ''%s'' for initialization.',Opt.init.initType)
        end

        % Pin hole camera with depth information
    case 'pinHoleDepth'

        switch Opt.init.initType
            case 'eucPnt'
                % INIT LMK OF TYPE: Euclidean point
                [l, L_rf, L_sf, L_k, L_c, L_obs] = ...
                    retroProjEucPntFromPhdOnRob( ...
                    Rob.frame, ...
                    Sen.frame, ...
                    Sen.par.k, ...
                    Sen.par.c, ...
                    Obs.meas.y) ;
                
                L_n = zeros(3,0);
                N   = [] ;

            otherwise
                error('??? Unknown landmark type ''%s'' for initialization.',Opt.init.initType)
        end
        
        
    % Omnidirectional camera 
    case {'omniCam'}
        % type of lmk to init
        switch Opt.init.initType
            case {'ahmPnt'}
                % INIT LMK OF TYPE: Anchored Homogeneous point
                [l, L_rf, L_sf, L_k, L_c, L_obs, L_n] = ...
                    retroProjAhmPntFromOmniCamOnRob( ...
                    Rob.frame, ...
                    Sen.frame, ...
                    Sen.par.k, ...
                    Sen.par.c, ...
                    Obs.meas.y, ...
                    Opt.init.idpPnt.nonObsMean) ;

                N = Opt.init.idpPnt.nonObsStd^2 ;
            otherwise
                error('??? Unknown landmark type ''%s'' for initialization.',Opt.init.initType)
        end
        
    otherwise % -- Sen.type
        % Print an error and exit
        error('??? Unknown sensor type ''%s''.',Sen.type);
end % -- Sen.type



% ========== End of function - Start GPL license ==========


%   # START GPL LICENSE

%---------------------------------------------------------------------
%
%   This file is part of SLAMTB, a SLAM toolbox for Matlab.
%
%   SLAMTB is free software: you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation, either version 3 of the License, or
%   (at your option) any later version.
%
%   SLAMTB 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 SLAMTB.  If not, see <http://www.gnu.org/licenses/>.
%
%---------------------------------------------------------------------

%   SLAMTB is Copyright:
%   Copyright (c) 2008-2010, Joan Sola @ LAAS-CNRS,
%   Copyright (c) 2010-2013, Joan Sola,
%   Copyright (c) 2014-2015, Joan Sola @ IRI-UPC-CSIC,
%   SLAMTB is Copyright 2009 
%   by Joan Sola, Teresa Vidal-Calleja, David Marquez and Jean Marie Codol
%   @ LAAS-CNRS.
%   See on top of this file for its particular copyright.

%   # END GPL LICENSE

