The main class which implements Monocular SLAM (Mono-SLAM), either from a real camera or offline data. More...
#include <mrpt/monoslam/CMonoSlam.h>
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 ![]() | |
void | OnTransitionJacobian (CMatrixTemplateNumeric< KFTYPE > &out_F) |
Implements the transition Jacobian ![]() | |
void | OnTransitionNoise (CMatrixTemplateNumeric< KFTYPE > &out_Q) |
Implements the transition noise covariance ![]() | |
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 ![]() ![]() ![]() | |
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. |
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:
2007:
The first version of this application, the corresponding classes and config files were developed by Antonio J. Ortiz de Galisteo as part of his Master Thesis at the University of Malaga (2007), based on public sources released by Andrew Davison (Imperial College London).
Definition at line 109 of file CMonoSlam.h.
mrpt::monoslam::CMonoSlam::CMonoSlam | ( | ) |
Constructor.
virtual mrpt::monoslam::CMonoSlam::~CMonoSlam | ( | ) | [virtual] |
Dtor.
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).
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.
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.
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]
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 | ||||
) | [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 | ||||
) | [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.
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).
u | --> odometry information (unused parameter for MonoSLAM) | |
xv | --> camera state |
void mrpt::monoslam::CMonoSlam::function_h | ( | CMatrixDouble & | h, | |
CMatrixDouble & | Hx_Hy | |||
) | [protected] |
Measurement model.
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:
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.
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.
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).
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.
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.
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.
out_z | A ![]() | |
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. |
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".
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 ![]() | |
out_dyn_dxv | The ![]() ![]() | |
out_dyn_dhn | The ![]() ![]() |
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 and the Jacobians
and (when applicable)
.
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: ![]() | |
out_Hx | One or a vertical stack of ![]() | |
out_Hy | An empty matrix, or one or a vertical stack of ![]() |
out_Hx must consist of 1 (in_full=false) or N (in_full=true) vertically stacked matrices, and out_Hy must consist of 1 or N
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 .
out_F | Must return the Jacobian. The returned matrix must be ![]() |
void mrpt::monoslam::CMonoSlam::OnTransitionModel | ( | const CVectorTemplate< KFTYPE > & | in_u, | |
CVectorTemplate< KFTYPE > & | inout_x, | |||
bool & | out_skipPrediction | |||
) | [protected] |
Implements the transition model .
in_u | The vector returned by OnGetAction. | |
inout_x | At input has
, at output must have | |
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 .
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.
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).
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.
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.
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.
CMatrixDouble mrpt::monoslam::CMonoSlam::m_h [protected] |
coordinate of all features the image uv coordinate system. Note: unobserved features has -1 uv values
Definition at line 399 of file CMonoSlam.h.
CMatrixDouble mrpt::monoslam::CMonoSlam::m_HH [protected] |
Full Jacobian matrix of all matched features.
Definition at line 406 of file CMonoSlam.h.
CMatrixDouble mrpt::monoslam::CMonoSlam::m_hh [protected] |
coordinate of precdicted features(includes just matched ones) in the image uv system
Definition at line 402 of file CMonoSlam.h.
CVectorDouble mrpt::monoslam::CMonoSlam::m_hrl_i [protected] |
Position of the ith feature relative to the camera expressed in the image uv coordinate system.
Definition at line 394 of file CMonoSlam.h.
CMatrixDouble mrpt::monoslam::CMonoSlam::m_Hx_Hy [protected] |
Full Jacobian matrix of all features.
Definition at line 405 of file CMonoSlam.h.
int mrpt::monoslam::CMonoSlam::m_iterations_count [protected] |
Definition at line 201 of file CMonoSlam.h.
Definition at line 220 of file CMonoSlam.h.
CMatrixDouble mrpt::monoslam::CMonoSlam::m_MatchCoord [protected] |
coordinate of matched features in the image uv coordinate system based on correlation value
Definition at line 403 of file CMonoSlam.h.
unsigned int mrpt::monoslam::CMonoSlam::m_num_obj_visible [protected] |
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.
CPose3D mrpt::monoslam::CMonoSlam::m_robotPose [protected] |
robot 3D Pose
Definition at line 409 of file CMonoSlam.h.
CMatrixDouble mrpt::monoslam::CMonoSlam::m_RR [protected] |
Full R matrix of all matched features.
Definition at line 407 of file CMonoSlam.h.
Definition at line 157 of file CMonoSlam.h.
CMatrixDouble mrpt::monoslam::CMonoSlam::m_SS [protected] |
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.
CMatrixDouble mrpt::monoslam::CMonoSlam::m_z [protected] |
coordinate of Observed features in the image uv coordinate system
Definition at line 400 of file CMonoSlam.h.
CMatrixDouble mrpt::monoslam::CMonoSlam::m_zz [protected] |
coordinate of matched features in the image uv coordinate system
Definition at line 401 of file CMonoSlam.h.
CMonoSlamMotionModel mrpt::monoslam::CMonoSlam::mmm [protected] |
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.
Definition at line 148 of file CMonoSlam.h.
double mrpt::monoslam::CMonoSlam::prev_speed [protected] |
Definition at line 207 of file CMonoSlam.h.
CVectorDouble mrpt::monoslam::CMonoSlam::prev_xv [protected] |
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.6.1 for MRPT 0.7.1 SVN: at Tue Dec 22 08:29:35 CET 2009 |