MRPT logo

mrpt::monoslam::CMonoSlam Class Reference

The main class which implements Monocular SLAM (Mono-SLAM), either from a real camera or offline data. More...

#include <mrpt/monoslam/CMonoSlam.h>

Inheritance diagram for mrpt::monoslam::CMonoSlam:

mrpt::bayes::CKalmanFilterCapable

List of all members.

Classes

struct  TOptions
 All the options of Mono-SLAM, in a structure which can be changed manually or saved/loaded to/from a .ini file. More...

Public Member Functions

 CMonoSlam ()
 Constructor.
virtual ~CMonoSlam ()
 Dtor.
The main interface methods for the final user
void go_one_step (const CActionCollectionPtr &optional_odometry=CActionCollectionPtr())
 This is the main method from the user viewpoint in Mono-SLAM: it is passed a pair Action/Observation (action can be empty, observation must contains at least 1 image), and one full Kalman Filter step will be executed.
void loadParameters (const mrpt::utils::CConfigFileBase &source)
 Load all the parameters for Mono-SLAM, Kalman-Filter, and the motion model.
void saveParameters (mrpt::utils::CConfigFileBase &config)
 Save all the parameters for Mono-SLAM, Kalman-Filter, and the motion model.
void resetFilterState ()
 Resets the filter state: the camera pose is resetted and the map is emptied.

Public Attributes

mrpt::hwdrivers::CCameraSensorPtr m_camera
 This is the source of images.
TOptions options
 All the options of Mono-SLAM, in a structure which can be changed manually or saved/loaded to/from a .ini file.
CImage m_progress2D
 With each step, "go_one_step" will save the 2D image with the detected features, etc to this image so the external program can show it to the user.
opengl::COpenGLScenePtr m_progress3D
 With each step, "go_one_step" will save the 3D scene with the filter state to this scene object so the external program can show it to the user.
CMatrixDouble m_robotPath
 robot 3D Pose
double m_simTime

Protected Member Functions

void plot_results_2D (mrpt::math::CMatrixDouble &zz, mrpt::math::CMatrixDouble &hh, mrpt::math::CMatrixDouble &h, mrpt::math::CMatrixDouble &S, mrpt::math::CMatrixDouble &XYZ, mrpt::utils::CImage &framecolor, unsigned int type, mrpt::math::CVectorDouble &match, bool verbose, bool savedatas)
 Show results (features selected and tracked) over actual frame.
void plot_results_3D (mrpt::math::CMatrixDouble &XYZ)
 Show the camera and feature estimated position in a 3D world.
void OnGetAction (CVectorTemplate< KFTYPE > &out_u)
 This function is only a short method for plot in 3D windows.
void OnTransitionModel (const CVectorTemplate< KFTYPE > &in_u, CVectorTemplate< KFTYPE > &inout_x, bool &out_skipPrediction)
 Implements the transition model $ \hat{x}_{k|k-1} = f( \hat{x}_{k-1|k-1}, u_k ) $.
void OnTransitionJacobian (CMatrixTemplateNumeric< KFTYPE > &out_F)
 Implements the transition Jacobian $ \frac{\partial f}{\partial x} $.
void OnTransitionNoise (CMatrixTemplateNumeric< KFTYPE > &out_Q)
 Implements the transition noise covariance $ Q_k $.
void OnGetObservations (CMatrixTemplateNumeric< KFTYPE > &out_z, CMatrixTemplateNumeric< KFTYPE > &out_R, vector_int &out_data_association)
 This is called between the KF prediction step and the update step to allow the application to employ the prior distribution of the system state in the detection of observations (data association), where applicable.
void OnObservationModelAndJacobians (const CMatrixTemplateNumeric< KFTYPE > &in_z, const vector_int &data_association, const bool &in_full, const int &in_obsIdx, CVectorTemplate< KFTYPE > &ytilde, CMatrixTemplateNumeric< KFTYPE > &Hx, CMatrixTemplateNumeric< KFTYPE > &Hy)
 Implements the observation prediction $ h_i(x) $ and the Jacobians $ \frac{\partial h_i}{\partial x} $ and (when applicable) $ \frac{\partial h_i}{\partial y_i} $.
virtual void OnInverseObservationModel (const CMatrixTemplateNumeric< KFTYPE > &in_z, const size_t &in_obsIdx, const size_t &in_idxNewFeat, CVectorTemplate< KFTYPE > &out_yn, CMatrixTemplateNumeric< KFTYPE > &out_dyn_dxv, CMatrixTemplateNumeric< KFTYPE > &out_dyn_dhn)
 If applicable to the given problem, this method implements the inverse observation model needed to extend the "map" with a new "element".
virtual void OnNormalizeStateVector ()
 This method is called after the prediction and after the update, to give the user an opportunity to normalize the state vector (eg, keep angles within -pi,pi range) if the application requires it.
virtual void OnPostIteration ()
 This method is called after finishing one KF iteration and before returning from runOneKalmanIteration().
virtual size_t get_vehicle_size () const
 Must return the dimension of the "vehicle state": either the full state vector or the "vehicle" part if applicable.
virtual size_t get_observation_size () const
 Must return the dimension of each observation (eg, 2 for pixel coordinates, 3 for 3D coordinates,etc).
void dh_dxv (CVectorDouble &yi, CMatrixDouble &zi, CMatrixDouble &m_dh_dxv)
 Calculate the Jacobian dh_dxv.
void dh_dyi (CVectorDouble &yi, CMatrixDouble &zi, CMatrixDouble &m_dh_dy)
 Calculate the Jacobian dh_dy.
void dh_drw (CVectorDouble &yi, CMatrixDouble &zi, CMatrixDouble &m_dh_drw)
 Calculate the Jacobian dh_drw.
void dh_dqwr (CVectorDouble &yi, CMatrixDouble &zi, CMatrixDouble &m_dh_dqwr)
 Calculate the Jacobian dh_dqwr.
void dh_dhrl (CVectorDouble &yi, CMatrixDouble &zi, CMatrixDouble &m_dh_dhrl)
 Calculate the Jacobian dh_dhrl.
void dhrl_drw (CVectorDouble &yi, CMatrixDouble &m_dhrl_drw)
 Calculate the Jacobian dhrl_drw.
void dhd_dhu (CMatrixDouble &zi, CMatrixDouble &m_dhd_dhu)
 Calculate the Jacobian dhd_dhu.
void dhu_dhrl (CVectorDouble &yi, CMatrixDouble &m_dhu_dhrl)
 Calculate the Jacobian dhu_dhrl.
void dhrl_dqwr (CVectorDouble &yi, CMatrixDouble &m_dhrl_dqwr)
 Calculate the Jacobian dhrl_dqwr.
void dhrl_dy (CVectorDouble &yi, CMatrixDouble &m_dhrl_dy)
 Calculate the Jacobian dhrl_dy.
CVectorDouble hinv (CMatrixDouble h_LR_rot)
 Calculate and return the feature coordinate in Unified Inverse Depth Parametrization.
CMatrixDouble calculate_ray (const float &col, const float &row)
 Calculate and return the vector which join the camera optic center to the feature.
void addAFeatureCov_newPar (CMatrixDouble &XYZ_w, float &col, float &row)
 This function increment the covariance matrix with a new feature.
void mahalanob_dist (int j_k, int i_k, int halfW_x, int halfW_y, CMatrixDouble &inv_S, CMatrixDouble &correlationScore)
 Calculate the mahalanobis distance in feature innovation (prediction - measurement).
void addNewFeatures_newPar (float &col, float &row)
 This function insert a new feature to the map (state vector and covariance matrix).
void h_calculate (CMatrixDouble &h_Out, CMatrixDouble &H_Out)
 Calculate the feature's position and the transition matrix H = [dh_dxv dh_dy] Measurement, derivatives and texture prediction.
void hi (CVectorDouble &yinit, CMatrixDouble &zi, size_t indx)
 This function return the position,zi (in pixels) of a feature yi.
void hu (const TPoint3D &XYZ, mrpt::vision::TPixelCoordf &out_pixel)
 % Calculate the projection of yi (in the camera reference)
void calculate_Hi (CVectorDouble &yi, CMatrixDouble &zi, unsigned int i, CMatrixDouble &Hi)
 % Calculate h derivates (h = feature's position in pixels).
void MSMatching (CMatrixDouble &h_predicted, const CMatrixDouble &S_predicted, CMatrixDouble &H_predicted, mrpt::utils::CImage &im, CMatrixDouble &hh, CMatrixDouble &zz, CMatrixDouble &H_matched, CMatrixDouble &R_matched, CFeaturePatch &features)
 Matching between actual image and feature patch (FOR EKF).
void MSMatching (CMatrixDouble &h_predicted, CMatrixDouble &S_predicted, mrpt::utils::CImage &im, CMatrixDouble &hh, CMatrixDouble &zz, CMatrixDouble &R_matched, CVectorDouble &match, CFeaturePatch &features)
 Matching between actual image and feature patch (FOR UKF and IEKF).
void InitializeNewFeature (CFeaturePatch &features, mrpt::utils::CImage &im)
 Initialize feature employing the Harris method adding neccesary information to x(k|k) and p(k|k).
void SearchMoreFeature (CFeaturePatch &features, mrpt::utils::CImage &im, CMatrixDouble &h)
 Try to find a image area without features and search it employing the Harris method adding neccesary information to x(k|k) and p(k|k).
void Ambiguity (CMatrixDouble &HH, CMatrixDouble &RR, CMatrixDouble &hh, CMatrixDouble &zz)
 This function lighten the ambiguity problem when measure features for Monocular SLAM when employ the EKF.
void Ambiguity (CMatrixDouble &RR, CMatrixDouble &hh, CMatrixDouble &zz, CVectorDouble &match)
 This function lighten the ambiguity problem when measure features for Monocular SLAM when employ the IEKF or UKF.
void function_fv (const CMatrixDouble &u, CVectorDouble &xv)
 Generic function to acces to the prediction model: fv (it must be implemented in MonoSLAMMotionModel).
void function_F (CMatrixDouble &F_parcial)
 Generic function to acces to the prediction model Jacobian, F due to the linealization process (it must be implemented in MonoSLAMMotionModel).
void function_Q (CMatrixDouble &Q)
 Generic function to acces to the matrix process noise, Q (it must be implemented in MonoSLAMMotionModel).
void function_h (CMatrixDouble &h, CMatrixDouble &Hx_Hy)
 Measurement model.
void function_R (CMatrixDouble &h, CMatrixDouble &R)
 Generic function to acces to the measurement process noise, R (it must be implemented in MonoSLAMMotionModel).
void normalize ()
 This function normalize the state vector and covariance matrix due to the quaternion employ for orientation.
void delete_feature (const unsigned int &index)
 This function delete from the state vector and covariance matrix the feature in the position i.
unsigned int get_state_size () const
 Return the state size.
size_t get_feature_size () const
 Return the feature size.
CMatrixDouble XYZ6D_TO_XYZ3D ()
 Transform the Unified Inverse Depth Parametrization coordinates to XYZ cartesian coordinates.
CMatrixDouble pxyztpl2pxyz ()
 Transform the Unified Inverse Depth Parametrization covariance matrix (6x6) to XYZ covariance (3x3).
void h_UKF (CMatrixDouble &h)
 Measurement model.
void visibility (CMatrixDouble &h)
 Visibility test for determine which features are shown in the image.
CMatrixDouble dqbar_by_dq ()
 Jacobian of quaternion inverse: this is a constant matrix that doesn't depend on the quaternion.
CMatrixDouble dR_by_dq0 (CQuaternionDouble &q)
 Component Jacobians: each returns a 3x3 matrix which is the derivative of the rotation matrix derived from a quaternion w.r.t.
CMatrixDouble dR_by_dqx (CQuaternionDouble &q)
 Component Jacobians: each returns a 3x3 matrix which is the derivative of the rotation matrix derived from a quaternion w.r.t.
CMatrixDouble dR_by_dqy (CQuaternionDouble &q)
 Component Jacobians: each returns a 3x3 matrix which is the derivative of the rotation matrix derived from a quaternion w.r.t.
CMatrixDouble dR_by_dqz (CQuaternionDouble &q)
 Component Jacobians: each returns a 3x3 matrix which is the derivative of the rotation matrix derived from a quaternion w.r.t.
float dqi_by_dqi (float qi, float qq)
 Auxiliary functions used by dqnorm_by_dq().
float dqi_by_dqj (float qi, float qj, float qq)
 Value of off-diagonal element of Jacobian.
CMatrixDouble dqnorm_by_dq (CQuaternionDouble &q)
 Normalising Jacobian.
CMatrixDouble dvnorm_by_dv (CVectorDouble &v)
 Normalising a 3D vector.
CMatrixDouble dRq_times_a_by_dq (CQuaternionDouble &q, CMatrixDouble &aMat)
 Auxiliary function.
void set_xkk (const CVectorDouble &xkk_in)
 This function set the state vector value.
void set_pkk (const CMatrixDouble &pkk_in)
 This function set the covariance matrix value.
void get_xkk (CVectorDouble &xkk_out)
 This function read the state vector value.
void get_pkk (CMatrixDouble &pkk_out)
 This function read the covariance matrix value.

Protected Attributes

int m_iterations_count
unsigned int m_num_obj_visible
CMonoSlamMotionModel mmm
 MonoSLAMMotionModel.
mrpt::vision::CCamModel cam
 Camera model.
double prev_speed
CVectorDouble prev_xv
mrpt::system::TTimeStamp m_dFrameTimeStamp
mrpt::math::CVectorDouble m_match
mrpt::utils::CImage m_frame
 Set up by go_one_step, it contains the GRAYSCALE image grabbed from the video or live camera so it can be used in the KF methods.
mrpt::slam::CActionCollectionPtr m_action
 If given by the user, set up by go_one_step, it may contain odometry.
mrpt::monoslam::CFeaturePatch m_FeatPatch
 The mapping between landmark IDs and indexes in the Pkk cov.
mrpt::system::TTimeStamp m_timeLastFrame
 Used to estimate FPS of the video source.
CVectorDouble m_hrl_i
 Position of the ith feature relative to the camera expressed in the image uv coordinate system.
mrpt::vision::CFeatureList m_featList
 List of all features.
mrpt::vision::CFeatureList m_ObservedfeatList
 List of observed features.
CMatrixDouble m_h
 coordinate of all features the image uv coordinate system. Note: unobserved features has -1 uv values
CMatrixDouble m_z
 coordinate of Observed features in the image uv coordinate system
CMatrixDouble m_zz
 coordinate of matched features in the image uv coordinate system
CMatrixDouble m_hh
 coordinate of precdicted features(includes just matched ones) in the image uv system
CMatrixDouble m_MatchCoord
 coordinate of matched features in the image uv coordinate system based on correlation value
CMatrixDouble m_Hx_Hy
 Full Jacobian matrix of all features.
CMatrixDouble m_HH
 Full Jacobian matrix of all matched features.
CMatrixDouble m_RR
 Full R matrix of all matched features.
CMatrixDouble m_SS
 Full S matrix of all matched features.
CPose3D m_robotPose
 robot 3D Pose

Static Protected Attributes

static const size_t state_size
 State Vector Size.
static const size_t feature_size
 Feature Vector Size.


Detailed Description

The main class which implements Monocular SLAM (Mono-SLAM), either from a real camera or offline data.

This class just contains the algorithm implementation, it's not a GUI. For directly usable programs, see "apps/mono-slam".

The basic usage would be like this:

The configuration file (.ini-like file) may have these sections:

  [MONOSLAM]
  match_threshold_up    =       // Top threshold for feature classification
  match_threshold_down  =               // Botton threshold for feature classification
  ...// All the variables in mrpt::monoslam::CMonoSlam::TOptions

  [MOTION_MODEL]
  sigma_lin_vel    = 6.0  // linear velocity noise in m/s^2
  sigma_ang_vel    = 6.0  // angular velocity noise in rad/s^2

  [KALMAN]
   method = 0     // 0:Full KF, 1:Efficient KF,  2: Full IKF, 3: Efficient IKF
   ... // All the variables in mrpt::bayes::CKalmanFilterCapable::TKF_options

  [CAMERA_MODEL]
   ....

History of changes:

Definition at line 109 of file CMonoSlam.h.


Constructor & Destructor Documentation

mrpt::monoslam::CMonoSlam::CMonoSlam (  ) 

Constructor.

virtual mrpt::monoslam::CMonoSlam::~CMonoSlam (  )  [virtual]

Dtor.


Member Function Documentation

void mrpt::monoslam::CMonoSlam::addAFeatureCov_newPar ( CMatrixDouble XYZ_w,
float &  col,
float &  row 
) [protected]

This function increment the covariance matrix with a new feature.

void mrpt::monoslam::CMonoSlam::addNewFeatures_newPar ( float &  col,
float &  row 
) [protected]

This function insert a new feature to the map (state vector and covariance matrix).

Parameters:
col is the feature column position in pixels over the image
row is the feature row position in pixels over the image

void mrpt::monoslam::CMonoSlam::Ambiguity ( CMatrixDouble RR,
CMatrixDouble hh,
CMatrixDouble zz,
CVectorDouble match 
) [protected]

This function lighten the ambiguity problem when measure features for Monocular SLAM when employ the IEKF or UKF.

Parameters:
RR --> Measured process noise matrix
hh --> (u,v) coordinates of predicted features (rounded) (is a column vector (u0,v0,u1,v1,.,un,vn))
zz --> (u,v) coordinates of measured features (rounded) (is a column vector (u0,v0,u1,v1,.,un,vn))
match --> This vector contain the information about which features has been correctly measured and which not.

void mrpt::monoslam::CMonoSlam::Ambiguity ( CMatrixDouble HH,
CMatrixDouble RR,
CMatrixDouble hh,
CMatrixDouble zz 
) [protected]

This function lighten the ambiguity problem when measure features for Monocular SLAM when employ the EKF.

Parameters:
HH --> Measured transition matrix
RR --> Measured process noise matrix
hh --> (u,v) coordinates of predicted features (rounded) (is a column vector (u0,v0,u1,v1,.,un,vn))
zz --> (u,v) coordinates of measured features (rounded) (is a column vector (u0,v0,u1,v1,.,un,vn))

void mrpt::monoslam::CMonoSlam::calculate_Hi ( CVectorDouble yi,
CMatrixDouble zi,
unsigned int  i,
CMatrixDouble Hi 
) [protected]

% Calculate h derivates (h = feature's position in pixels).

Hi = [dh_dxv dh_dyi]

Parameters:
yi --> feature in Unified Inverse Depth Parametrization
zi --> measurement in pixels coordinates
i --> feature index
Hi --> measurement jacobian

CMatrixDouble mrpt::monoslam::CMonoSlam::calculate_ray ( const float &  col,
const float &  row 
) [protected]

Calculate and return the vector which join the camera optic center to the feature.

void mrpt::monoslam::CMonoSlam::delete_feature ( const unsigned int &  index  )  [protected]

This function delete from the state vector and covariance matrix the feature in the position i.

void mrpt::monoslam::CMonoSlam::dh_dhrl ( CVectorDouble yi,
CMatrixDouble zi,
CMatrixDouble m_dh_dhrl 
) [protected]

Calculate the Jacobian dh_dhrl.

void mrpt::monoslam::CMonoSlam::dh_dqwr ( CVectorDouble yi,
CMatrixDouble zi,
CMatrixDouble m_dh_dqwr 
) [protected]

Calculate the Jacobian dh_dqwr.

void mrpt::monoslam::CMonoSlam::dh_drw ( CVectorDouble yi,
CMatrixDouble zi,
CMatrixDouble m_dh_drw 
) [protected]

Calculate the Jacobian dh_drw.

void mrpt::monoslam::CMonoSlam::dh_dxv ( CVectorDouble yi,
CMatrixDouble zi,
CMatrixDouble m_dh_dxv 
) [protected]

Calculate the Jacobian dh_dxv.

void mrpt::monoslam::CMonoSlam::dh_dyi ( CVectorDouble yi,
CMatrixDouble zi,
CMatrixDouble m_dh_dy 
) [protected]

Calculate the Jacobian dh_dy.

void mrpt::monoslam::CMonoSlam::dhd_dhu ( CMatrixDouble zi,
CMatrixDouble m_dhd_dhu 
) [protected]

Calculate the Jacobian dhd_dhu.

void mrpt::monoslam::CMonoSlam::dhrl_dqwr ( CVectorDouble yi,
CMatrixDouble m_dhrl_dqwr 
) [protected]

Calculate the Jacobian dhrl_dqwr.

void mrpt::monoslam::CMonoSlam::dhrl_drw ( CVectorDouble yi,
CMatrixDouble m_dhrl_drw 
) [protected]

Calculate the Jacobian dhrl_drw.

void mrpt::monoslam::CMonoSlam::dhrl_dy ( CVectorDouble yi,
CMatrixDouble m_dhrl_dy 
) [protected]

Calculate the Jacobian dhrl_dy.

void mrpt::monoslam::CMonoSlam::dhu_dhrl ( CVectorDouble yi,
CMatrixDouble m_dhu_dhrl 
) [protected]

Calculate the Jacobian dhu_dhrl.

CMatrixDouble mrpt::monoslam::CMonoSlam::dqbar_by_dq (  )  [protected]

Jacobian of quaternion inverse: this is a constant matrix that doesn't depend on the quaternion.

float mrpt::monoslam::CMonoSlam::dqi_by_dqi ( float  qi,
float  qq 
) [protected]

Auxiliary functions used by dqnorm_by_dq().

Value of diagonal element of Jacobian

float mrpt::monoslam::CMonoSlam::dqi_by_dqj ( float  qi,
float  qj,
float  qq 
) [protected]

Value of off-diagonal element of Jacobian.

CMatrixDouble mrpt::monoslam::CMonoSlam::dqnorm_by_dq ( CQuaternionDouble q  )  [protected]

Normalising Jacobian.

CMatrixDouble mrpt::monoslam::CMonoSlam::dR_by_dq0 ( CQuaternionDouble q  )  [protected]

Component Jacobians: each returns a 3x3 matrix which is the derivative of the rotation matrix derived from a quaternion w.r.t.

return the real element

CMatrixDouble mrpt::monoslam::CMonoSlam::dR_by_dqx ( CQuaternionDouble q  )  [protected]

Component Jacobians: each returns a 3x3 matrix which is the derivative of the rotation matrix derived from a quaternion w.r.t.

the imaginary element x

CMatrixDouble mrpt::monoslam::CMonoSlam::dR_by_dqy ( CQuaternionDouble q  )  [protected]

Component Jacobians: each returns a 3x3 matrix which is the derivative of the rotation matrix derived from a quaternion w.r.t.

the imaginary element y

CMatrixDouble mrpt::monoslam::CMonoSlam::dR_by_dqz ( CQuaternionDouble q  )  [protected]

Component Jacobians: each returns a 3x3 matrix which is the derivative of the rotation matrix derived from a quaternion w.r.t.

the imaginary element z

CMatrixDouble mrpt::monoslam::CMonoSlam::dRq_times_a_by_dq ( CQuaternionDouble q,
CMatrixDouble aMat 
) [protected]

Auxiliary function.

User must not be used.

Parameters:
q --> state vector quaternion.
a --> (hLhatRi) Not rotate.

CMatrixDouble mrpt::monoslam::CMonoSlam::dvnorm_by_dv ( CVectorDouble v  )  [protected]

Normalising a 3D vector.

void mrpt::monoslam::CMonoSlam::function_F ( CMatrixDouble F_parcial  )  [protected]

Generic function to acces to the prediction model Jacobian, F due to the linealization process (it must be implemented in MonoSLAMMotionModel).

void mrpt::monoslam::CMonoSlam::function_fv ( const CMatrixDouble u,
CVectorDouble xv 
) [protected]

Generic function to acces to the prediction model: fv (it must be implemented in MonoSLAMMotionModel).

Parameters:
u --> odometry information (unused parameter for MonoSLAM)
xv --> camera state

void mrpt::monoslam::CMonoSlam::function_h ( CMatrixDouble h,
CMatrixDouble Hx_Hy 
) [protected]

Measurement model.

Parameters:
h --> Vector to return the measurement prediction coordinates
Hx_Hy --> Jacobian [Hx Hy1...Hyn]

void mrpt::monoslam::CMonoSlam::function_Q ( CMatrixDouble Q  )  [protected]

Generic function to acces to the matrix process noise, Q (it must be implemented in MonoSLAMMotionModel).

void mrpt::monoslam::CMonoSlam::function_R ( CMatrixDouble h,
CMatrixDouble R 
) [protected]

Generic function to acces to the measurement process noise, R (it must be implemented in MonoSLAMMotionModel).

size_t mrpt::monoslam::CMonoSlam::get_feature_size (  )  const [inline, protected, virtual]

Return the feature size.

In Monocular SLAM this value is 6 due to the employ of Unified Inverse Depth Parametrization.

Reimplemented from mrpt::bayes::CKalmanFilterCapable.

Definition at line 628 of file CMonoSlam.h.

virtual size_t mrpt::monoslam::CMonoSlam::get_observation_size (  )  const [inline, protected, virtual]

Must return the dimension of each observation (eg, 2 for pixel coordinates, 3 for 3D coordinates,etc).

Implements mrpt::bayes::CKalmanFilterCapable.

Definition at line 387 of file CMonoSlam.h.

void mrpt::monoslam::CMonoSlam::get_pkk ( CMatrixDouble pkk_out  )  [inline, protected]

This function read the covariance matrix value.

(Employed for debug the program)

Definition at line 723 of file CMonoSlam.h.

References mrpt::math::CMatrixTemplateNumeric< T >::resize().

unsigned int mrpt::monoslam::CMonoSlam::get_state_size (  )  const [inline, protected]

Return the state size.

In Monocular SLAM this value is 13

Definition at line 624 of file CMonoSlam.h.

virtual size_t mrpt::monoslam::CMonoSlam::get_vehicle_size (  )  const [inline, protected, virtual]

Must return the dimension of the "vehicle state": either the full state vector or the "vehicle" part if applicable.

Implements mrpt::bayes::CKalmanFilterCapable.

Definition at line 379 of file CMonoSlam.h.

void mrpt::monoslam::CMonoSlam::get_xkk ( CVectorDouble xkk_out  )  [inline, protected]

This function read the state vector value.

(Employed for debug the program)

Definition at line 715 of file CMonoSlam.h.

void mrpt::monoslam::CMonoSlam::go_one_step ( const CActionCollectionPtr &  optional_odometry = CActionCollectionPtr()  ) 

This is the main method from the user viewpoint in Mono-SLAM: it is passed a pair Action/Observation (action can be empty, observation must contains at least 1 image), and one full Kalman Filter step will be executed.

This method does:

  • Grab a new image frame from the camera (See m_camera).
  • Search Features if we have less than necessaries
  • EKF Prediction Step
  • Matching/Tracking of Features
  • EKF Update Step
  • Show/Save Results

Parameters:
optional_odometry This may contain odometry, if the robot has it. Left as an empty smart pointer to just rely on images (default).

void mrpt::monoslam::CMonoSlam::h_calculate ( CMatrixDouble h_Out,
CMatrixDouble H_Out 
) [protected]

Calculate the feature's position and the transition matrix H = [dh_dxv dh_dy] Measurement, derivatives and texture prediction.

Parameters:
h,output. This vector contain the feature position estimation.
H,output. This matrix is the Jacovian of measurement prediction.

void mrpt::monoslam::CMonoSlam::h_UKF ( CMatrixDouble h  )  [protected]

Measurement model.

Parameters:
h --> Vector to return the measurement prediction coordinates

void mrpt::monoslam::CMonoSlam::hi ( CVectorDouble yinit,
CMatrixDouble zi,
size_t  indx 
) [protected]

This function return the position,zi (in pixels) of a feature yi.

CVectorDouble mrpt::monoslam::CMonoSlam::hinv ( CMatrixDouble  h_LR_rot  )  [protected]

Calculate and return the feature coordinate in Unified Inverse Depth Parametrization.

Where h_LR_rot is the vector which join the camera optic center to the feature.

void mrpt::monoslam::CMonoSlam::hu ( const TPoint3D XYZ,
mrpt::vision::TPixelCoordf out_pixel 
) [protected]

% Calculate the projection of yi (in the camera reference)

void mrpt::monoslam::CMonoSlam::InitializeNewFeature ( CFeaturePatch features,
mrpt::utils::CImage im 
) [protected]

Initialize feature employing the Harris method adding neccesary information to x(k|k) and p(k|k).

Parameters:
features --> Structure to store the visual features
im --> Initial image in the system

void mrpt::monoslam::CMonoSlam::loadParameters ( const mrpt::utils::CConfigFileBase source  ) 

Load all the parameters for Mono-SLAM, Kalman-Filter, and the motion model.

See also:
saveParameters

void mrpt::monoslam::CMonoSlam::mahalanob_dist ( int  j_k,
int  i_k,
int  halfW_x,
int  halfW_y,
CMatrixDouble inv_S,
CMatrixDouble correlationScore 
) [protected]

Calculate the mahalanobis distance in feature innovation (prediction - measurement).

void mrpt::monoslam::CMonoSlam::MSMatching ( CMatrixDouble h_predicted,
CMatrixDouble S_predicted,
mrpt::utils::CImage im,
CMatrixDouble hh,
CMatrixDouble zz,
CMatrixDouble R_matched,
CVectorDouble match,
CFeaturePatch features 
) [protected]

Matching between actual image and feature patch (FOR UKF and IEKF).

                                input parameters
                                h_predicted --> (u,v) coordinates of predicted features (is a column vector (u0,v0,u1,v1,.,un,vn))
                                S_predicted --> predicted covariance innovation matrix
                                im                      --> current frame

                                output parameters
                                z                       --> (u,v) coordinates of measured features  (rounded) (is a column vector (u0,v0,u1,v1,.,un,vn))
                                R_matched       --> measured process noise matrix

                                inout parameter
                                features        --> (As input) features struct to get feature's patch
                                                                (As output)update hits for erase bad features later

void mrpt::monoslam::CMonoSlam::MSMatching ( CMatrixDouble h_predicted,
const CMatrixDouble S_predicted,
CMatrixDouble H_predicted,
mrpt::utils::CImage im,
CMatrixDouble hh,
CMatrixDouble zz,
CMatrixDouble H_matched,
CMatrixDouble R_matched,
CFeaturePatch features 
) [protected]

Matching between actual image and feature patch (FOR EKF).

                                input parameters
                                h_predicted --> (u,v) coordinates of predicted features (is a column vector (u0,v0,u1,v1,.,un,vn))
                                S_predicted --> predicted covariance innovation matrix
                                H_predicted --> predicted transition matrix
                                im                      --> current frame

                                output parameters
                                h                       --> (u,v) coordinates of predicted features (rounded) (is a column vector (u0,v0,u1,v1,.,un,vn))
                                z                       --> (u,v) coordinates of measured features  (rounded) (is a column vector (u0,v0,u1,v1,.,un,vn))
                                H_matched       --> measured transition matrix
                                R_matched       --> measured process noise matrix

                                inout parameter
                                features        --> (As input) features struct to get feature's patch
                                                                (As output)update hits for erase bad features later

void mrpt::monoslam::CMonoSlam::normalize (  )  [protected]

This function normalize the state vector and covariance matrix due to the quaternion employ for orientation.

If the orientation is not represented by a quaternion, this function is empty.

void mrpt::monoslam::CMonoSlam::OnGetAction ( CVectorTemplate< KFTYPE > &  out_u  )  [protected]

This function is only a short method for plot in 3D windows.

Must return the action vector u.

Parameters:
out_u The action vector which will be passed to OnTransitionModel

void mrpt::monoslam::CMonoSlam::OnGetObservations ( CMatrixTemplateNumeric< KFTYPE > &  out_z,
CMatrixTemplateNumeric< KFTYPE > &  out_R,
vector_int out_data_association 
) [protected]

This is called between the KF prediction step and the update step to allow the application to employ the prior distribution of the system state in the detection of observations (data association), where applicable.

Parameters:
out_z A $N \times O$ matrix, with O being get_observation_size() and N the number of "observations": how many observed landmarks for a map, or just one if not applicable.
out_R A matrix of size N·OxO (O being get_observation_size() and N the number of observations). It is the covariance matrix of the sensor noise for each of the returned observations. The order must correspond to that in out_z.
out_data_association An empty vector or, where applicable, a vector where the i'th element corresponds to the position of the observation in the i'th row of out_z within the system state vector, or -1 if it is a new map element and we want to insert it at the end of this KF iteration. This method will be called just once for each complete KF iteration.
Note:
Since MRPT 0.5.5, the method "OnGetObservations" returning R as a matrix is called instead from the Kalman Filter engine. However, for compatibility, a default virtual method calls this method and build the R matrix from the diagonal R2.

virtual void mrpt::monoslam::CMonoSlam::OnInverseObservationModel ( const CMatrixTemplateNumeric< KFTYPE > &  in_z,
const size_t &  in_obsIdx,
const size_t &  in_idxNewFeat,
CVectorTemplate< KFTYPE > &  out_yn,
CMatrixTemplateNumeric< KFTYPE > &  out_dyn_dxv,
CMatrixTemplateNumeric< KFTYPE > &  out_dyn_dhn 
) [protected, virtual]

If applicable to the given problem, this method implements the inverse observation model needed to extend the "map" with a new "element".

Parameters:
in_z This is the same matrix returned by OnGetObservations().
in_obsIndex The index of the observation whose inverse sensor is to be computed. It corresponds to the row in in_z where the observation can be found.
in_idxNewFeat The index that this new feature will have in the state vector (0:just after the vehicle state, 1: after that,...). Save this number so data association can be done according to these indices.
out_yn The F-length vector with the inverse observation model $ y_n=y(x,z_n) $.
out_dyn_dxv The $F \times V$ Jacobian of the inv. sensor model wrt the robot pose $ \frac{\partial y_n}{\partial x_v} $.
out_dyn_dhn The $F \times O$ Jacobian of the inv. sensor model wrt the observation vector $ \frac{\partial y_n}{\partial h_n} $.

virtual void mrpt::monoslam::CMonoSlam::OnNormalizeStateVector (  )  [inline, protected, virtual]

This method is called after the prediction and after the update, to give the user an opportunity to normalize the state vector (eg, keep angles within -pi,pi range) if the application requires it.

Reimplemented from mrpt::bayes::CKalmanFilterCapable.

Definition at line 373 of file CMonoSlam.h.

References mrpt::math::normalize().

void mrpt::monoslam::CMonoSlam::OnObservationModelAndJacobians ( const CMatrixTemplateNumeric< KFTYPE > &  in_z,
const vector_int data_association,
const bool &  in_full,
const int &  in_obsIdx,
CVectorTemplate< KFTYPE > &  ytilde,
CMatrixTemplateNumeric< KFTYPE > &  Hx,
CMatrixTemplateNumeric< KFTYPE > &  Hy 
) [protected]

Implements the observation prediction $ h_i(x) $ and the Jacobians $ \frac{\partial h_i}{\partial x} $ and (when applicable) $ \frac{\partial h_i}{\partial y_i} $.

Parameters:
in_z This is the same matrix returned by OnGetObservations(), passed here for reference.
in_data_association The vector returned by OnGetObservations(), passed here for reference.
in_full If set to true, all the Jacobians and predictions must be computed at once. Otherwise, just those for the observation in_obsIdx.
in_obsIdx If in_full=false, the row of the observation (in in_z and in_data_association) whose innovation & Jacobians are to be returned now.
out_innov The difference between the expected observation and the real one: $ \tilde{y} = z - h(x) $. It must be a vector of length O*N (in_full=true) or O (in_full=false), with O=get_observation_size() and N=number of rows in in_z.
out_Hx One or a vertical stack of $ \frac{\partial h_i}{\partial x} $.
out_Hy An empty matrix, or one or a vertical stack of $ \frac{\partial h_i}{\partial y_i} $.
out_Hx must consist of 1 (in_full=false) or N (in_full=true) vertically stacked $O \times V$ matrices, and out_Hy must consist of 1 or N $O \times F$ matrices, with:

virtual void mrpt::monoslam::CMonoSlam::OnPostIteration (  )  [protected, virtual]

This method is called after finishing one KF iteration and before returning from runOneKalmanIteration().

Reimplemented from mrpt::bayes::CKalmanFilterCapable.

void mrpt::monoslam::CMonoSlam::OnTransitionJacobian ( CMatrixTemplateNumeric< KFTYPE > &  out_F  )  [protected]

Implements the transition Jacobian $ \frac{\partial f}{\partial x} $.

Parameters:
out_F Must return the Jacobian. The returned matrix must be $N \times N$ with N being either the size of the whole state vector or get_vehicle_size().

void mrpt::monoslam::CMonoSlam::OnTransitionModel ( const CVectorTemplate< KFTYPE > &  in_u,
CVectorTemplate< KFTYPE > &  inout_x,
bool &  out_skipPrediction 
) [protected]

Implements the transition model $ \hat{x}_{k|k-1} = f( \hat{x}_{k-1|k-1}, u_k ) $.

Parameters:
in_u The vector returned by OnGetAction.
inout_x At input has

\[ \hat{x}_{k-1|k-1} \]

, at output must have $ \hat{x}_{k|k-1} $ .

out_skip Set this to true if for some reason you want to skip the prediction step (to do not modify either the vector or the covariance). Default:false

void mrpt::monoslam::CMonoSlam::OnTransitionNoise ( CMatrixTemplateNumeric< KFTYPE > &  out_Q  )  [protected]

Implements the transition noise covariance $ Q_k $.

Parameters:
out_Q Must return the covariance matrix. The returned matrix must be of the same size than the jacobian from OnTransitionJacobian

void mrpt::monoslam::CMonoSlam::plot_results_2D ( mrpt::math::CMatrixDouble zz,
mrpt::math::CMatrixDouble hh,
mrpt::math::CMatrixDouble h,
mrpt::math::CMatrixDouble S,
mrpt::math::CMatrixDouble XYZ,
mrpt::utils::CImage framecolor,
unsigned int  type,
mrpt::math::CVectorDouble match,
bool  verbose,
bool  savedatas 
) [protected]

Show results (features selected and tracked) over actual frame.

void mrpt::monoslam::CMonoSlam::plot_results_3D ( mrpt::math::CMatrixDouble XYZ  )  [protected]

Show the camera and feature estimated position in a 3D world.

CMatrixDouble mrpt::monoslam::CMonoSlam::pxyztpl2pxyz (  )  [inline, protected]

Transform the Unified Inverse Depth Parametrization covariance matrix (6x6) to XYZ covariance (3x3).

Definition at line 636 of file CMonoSlam.h.

void mrpt::monoslam::CMonoSlam::resetFilterState (  ) 

Resets the filter state: the camera pose is resetted and the map is emptied.

void mrpt::monoslam::CMonoSlam::saveParameters ( mrpt::utils::CConfigFileBase config  ) 

Save all the parameters for Mono-SLAM, Kalman-Filter, and the motion model.

See also:
loadParameters

void mrpt::monoslam::CMonoSlam::SearchMoreFeature ( CFeaturePatch features,
mrpt::utils::CImage im,
CMatrixDouble h 
) [protected]

Try to find a image area without features and search it employing the Harris method adding neccesary information to x(k|k) and p(k|k).

Parameters:
features --> Structure to store the visual features
im --> Image where search more features
h --> Predicted features over the image

void mrpt::monoslam::CMonoSlam::set_pkk ( const CMatrixDouble pkk_in  )  [inline, protected]

This function set the covariance matrix value.

(Employed for debug the program)

Definition at line 711 of file CMonoSlam.h.

void mrpt::monoslam::CMonoSlam::set_xkk ( const CVectorDouble xkk_in  )  [inline, protected]

This function set the state vector value.

(Employed for debug the program)

Definition at line 707 of file CMonoSlam.h.

void mrpt::monoslam::CMonoSlam::visibility ( CMatrixDouble h  )  [protected]

Visibility test for determine which features are shown in the image.

Parameters:
h --> Vector to return the measurement prediction coordinates

CMatrixDouble mrpt::monoslam::CMonoSlam::XYZ6D_TO_XYZ3D (  )  [inline, protected]

Transform the Unified Inverse Depth Parametrization coordinates to XYZ cartesian coordinates.

Definition at line 632 of file CMonoSlam.h.


Member Data Documentation

Camera model.

Definition at line 204 of file CMonoSlam.h.

const size_t mrpt::monoslam::CMonoSlam::feature_size [static, protected]

Feature Vector Size.

Definition at line 211 of file CMonoSlam.h.

mrpt::slam::CActionCollectionPtr mrpt::monoslam::CMonoSlam::m_action [protected]

If given by the user, set up by go_one_step, it may contain odometry.

Definition at line 226 of file CMonoSlam.h.

This is the source of images.

It MUST be filled before starting execution of Mono-SLAM This smart pointer can be created from:

Definition at line 119 of file CMonoSlam.h.

Definition at line 219 of file CMonoSlam.h.

List of all features.

Definition at line 396 of file CMonoSlam.h.

The mapping between landmark IDs and indexes in the Pkk cov.

matrix: The mapping between indexes in the Pkk cov. matrix and landmark IDs:

Definition at line 237 of file CMonoSlam.h.

Set up by go_one_step, it contains the GRAYSCALE image grabbed from the video or live camera so it can be used in the KF methods.

Definition at line 223 of file CMonoSlam.h.

coordinate of all features the image uv coordinate system. Note: unobserved features has -1 uv values

Definition at line 399 of file CMonoSlam.h.

Full Jacobian matrix of all matched features.

Definition at line 406 of file CMonoSlam.h.

coordinate of precdicted features(includes just matched ones) in the image uv system

Definition at line 402 of file CMonoSlam.h.

Position of the ith feature relative to the camera expressed in the image uv coordinate system.

Definition at line 394 of file CMonoSlam.h.

Full Jacobian matrix of all features.

Definition at line 405 of file CMonoSlam.h.

Definition at line 201 of file CMonoSlam.h.

Definition at line 220 of file CMonoSlam.h.

coordinate of matched features in the image uv coordinate system based on correlation value

Definition at line 403 of file CMonoSlam.h.

Definition at line 202 of file CMonoSlam.h.

List of observed features.

Definition at line 397 of file CMonoSlam.h.

With each step, "go_one_step" will save the 2D image with the detected features, etc to this image so the external program can show it to the user.

Definition at line 151 of file CMonoSlam.h.

opengl::COpenGLScenePtr mrpt::monoslam::CMonoSlam::m_progress3D

With each step, "go_one_step" will save the 3D scene with the filter state to this scene object so the external program can show it to the user.

Definition at line 154 of file CMonoSlam.h.

robot 3D Pose

Definition at line 156 of file CMonoSlam.h.

robot 3D Pose

Definition at line 409 of file CMonoSlam.h.

Full R matrix of all matched features.

Definition at line 407 of file CMonoSlam.h.

Definition at line 157 of file CMonoSlam.h.

Full S matrix of all matched features.

Definition at line 408 of file CMonoSlam.h.

Used to estimate FPS of the video source.

Definition at line 239 of file CMonoSlam.h.

coordinate of Observed features in the image uv coordinate system

Definition at line 400 of file CMonoSlam.h.

coordinate of matched features in the image uv coordinate system

Definition at line 401 of file CMonoSlam.h.

MonoSLAMMotionModel.

Definition at line 203 of file CMonoSlam.h.

All the options of Mono-SLAM, in a structure which can be changed manually or saved/loaded to/from a .ini file.

See also:
KF_options, loadParameters

Definition at line 148 of file CMonoSlam.h.

Definition at line 207 of file CMonoSlam.h.

Definition at line 208 of file CMonoSlam.h.

const size_t mrpt::monoslam::CMonoSlam::state_size [static, protected]

State Vector Size.

Definition at line 210 of file CMonoSlam.h.




Page generated by Doxygen 1.5.9 for MRPT 0.7.1 SVN: at Mon Aug 17 22:27:43 EDT 2009