P_TRO

 Constructor of TRO Filters for Pose SLAM.

 This is a sub-type of Pose SLAM filter (P_Filter), and thus it 
 inherits all methods from the P_Filter.

 This type of filter estimates the robot's trajectory representing it as
 Gaussian represented in the information form. 
 However, beside the information matrix and information vector, we need
 to store the mean (it is used as a linearization point for non linear
 transition and observation functions). Moreover, we have to store
 information to retrive the covariances for all poses and the
 cross-correlations between all poses and the last robot's pose.
 
 The possible parameters are
   - Another P_TRO. Copy constructor.
   - A Gaussian given the initial estimation + the state recovery method 
     that can be
          2 -> Our method 
          1 -> Get whole Sigma 
          0 -> Get Sigma column by column

 See also P_Filter, P_AllSensors, P_OneSensor, P_EIF, P_FusionEKF, P_EKF.

0001 function FF=P_TRO(varargin)
0002 % Constructor of TRO Filters for Pose SLAM.
0003 %
0004 % This is a sub-type of Pose SLAM filter (P_Filter), and thus it
0005 % inherits all methods from the P_Filter.
0006 %
0007 % This type of filter estimates the robot's trajectory representing it as
0008 % Gaussian represented in the information form.
0009 % However, beside the information matrix and information vector, we need
0010 % to store the mean (it is used as a linearization point for non linear
0011 % transition and observation functions). Moreover, we have to store
0012 % information to retrive the covariances for all poses and the
0013 % cross-correlations between all poses and the last robot's pose.
0014 %
0015 % The possible parameters are
0016 %   - Another P_TRO. Copy constructor.
0017 %   - A Gaussian given the initial estimation + the state recovery method
0018 %     that can be
0019 %          2 -> Our method
0020 %          1 -> Get whole Sigma
0021 %          0 -> Get Sigma column by column
0022 %
0023 % See also P_Filter, P_AllSensors, P_OneSensor, P_EIF, P_FusionEKF, P_EKF.
0024 
0025   switch nargin
0026     case 1
0027       if isa(varargin{1},'P_TRO')
0028         FF=varargin{1};
0029       else
0030         % default state recovery method 2 (out method)
0031         FF=P_TRO(varargin{1},2);
0032       end
0033       
0034     case 2
0035       if isa(varargin{1},'Gaussian')
0036         % TRO filter can be seen as a sub-type of EIF but the
0037         % implementation as a different type of filter is more efficient
0038         F=P_Filter(varargin{1});
0039         
0040         dim=get(F,'dim');
0041         
0042         if get(varargin{1},'volume')<1e-9
0043           FF.Lambda=sparse(1e3*eye(dim)); % this should be infinite
0044         else
0045           FF.Lambda=sparse(get(varargin{1},'invCovariance'));
0046         end
0047         
0048         FF.mu=get(varargin{1},'mean'); % used as a linealization point
0049         FF.eta=FF.Lambda*FF.mu;
0050         
0051         % Information associated with each pose: mu, marginal sigma, and
0052         % the necessary information to recover the cross covariance with
0053         % respect to last pose (\Phi_i in the paper).
0054         FF.PoseData={PoseData(FF.mu,FF.Lambda\eye(dim),eye(dim),get(F,'nSteps'))};
0055         
0056         % F is the matrix used to correctly interpret the cross
0057         % correlation data
0058         FF.F=eye(dim);
0059         
0060         % State recovery method when closing a loop
0061         %  2 -> Our method
0062         %  1 -> Get whole Sigma
0063         %  0 -> Get Sigma column by column
0064         if isa(varargin{2},'double')
0065           FF.stateRecoveryMethod=varargin{2};
0066         else
0067           error('Wrong parameter type in P_TRO creation');
0068         end
0069         
0070         % To get statistics about cholmod2
0071         FF.cm2ne=[];
0072         FF.cm2t=[];
0073         FF.cm2mb=[];
0074         
0075         FF=class(FF,'P_TRO',F);
0076       else
0077         error('Wrong parameter type in P_TRO creation');
0078       end
0079       
0080     otherwise
0081       error('Wrong number of parameters in P_TRO creation');
0082   end