MRPT logo

mrpt::monoslam::CMonoSlamMotionModel Class Reference

General motion model in 3D Assumes a random impulse changes the velocity at each time step. More...

#include <mrpt/monoslam/CMonoSlamMotionModel.h>

List of all members.

Public Member Functions

void dqomegadt_by_domega (const CVectorDouble &omega, CMatrixDouble &RESdqomegadt_by_domega)
 Calculate commonly used Jacobian part dq(omega * delta_t) by domega.
double dq0_by_domegaA (double omegaA, double omega)
 Ancillary function to calculate part of Jacobian partial q / partial omega which is repeatable due to symmetry.
double dqA_by_domegaA (double omegaA, double omega)
 Ancillary function to calculate part of Jacobian which is repeatable due to symmetry.
double dqA_by_domegaB (double omegaA, double omegaB, double omega)
 Ancillary function to calculate part of Jacobian which is repeatable due to symmetry.
void dq3_dq1 (mrpt::math::CQuaternion< double > &q2, CMatrixDouble &M)
 Auxiliar ancillary function to calculate part of Jacobian which is repeatable due to symmetry.
void dq3_dq2 (mrpt::math::CQuaternion< double > &q1, CMatrixDouble &M)
 Auxiliar ancillary function to calculate part of Jacobian which is repeatable due to symmetry.
 CMonoSlamMotionModel ()
 Default constructor.
size_t get_feature_size () const
 Return the dimension of a feature (in monoSLAM employing Unified Inverse Depth Parametrization is 6).
size_t get_state_size () const
 Return the state dimension (in monoSLAM is 13).
void set_delta_t (const double dt)
 Set the time between frames.
double get_sigma_lin_vel () const
 Read the variance of linear velocity.
double get_sigma_ang_vel () const
 Read the variance of angular velocity.
double get_delta_t () const
 Read time between frames.
void extract_components (CVectorDouble &xv, CVectorDouble &r, CQuaternionDouble &q, CVectorDouble &v, CVectorDouble &w)
 Extract from state vector their components:.
void compose_xv (CVectorDouble &xv, const CVectorDouble &r, const CQuaternionDouble &q, const CVectorDouble &v, const CVectorDouble &w)
 Form the state vector from their individual components.
void func_fv (CVectorDouble &xv)
 This function predict the state vector: Prediction model (camera position, angle, linear velocity and angular velocity).
void func_dfv_by_dxv (CVectorDouble &xv, CMatrixDouble &F)
 Calculate the Jacobian 'dfv/dxv'.
void func_Q (CVectorDouble &xv, CMatrixDouble &QRES)
 Calculate the covariance noise matrix associated to the process 'Q' It's necessary to calculate the prediction 'P(k+1|k)' Fill noise covariance matrix Pnn: this is the covariance of the noise vector [V Omega]^T that gets added to the state.
void func_dxp_by_dxv (const CVectorDouble &xv, CMatrixDouble &m)
 In the case of Monocular SLAM this Jacobian is a 7x7 identity matrix.
CMatrixDouble XYZTPL2XYZ (CVectorDouble &xkk)
 Convert coordenates from inverse detph parametrization (xc,yc,zc,theta,phi,lambda) to 3D (x,y,z) coordinates Return a matrix where each row is a feature in which:

  • column 0 --> X coordinate
  • column 1 --> Y coordinate
  • column 2 --> Z coordinate xc,yc,zc : camera position in world coord system theta,phi,lambda : fteature orientation and depth params.

CMatrixDouble pxyztpl2pxyz (CMatrixDouble &pkk, CVectorDouble &xkk)
 Convert covariance matrix from inverse detph parametrization diag(xc,yc,zc,theta,phi,lambda) to 3D covariance matrix diag(x,y,z).

Public Attributes

double sigma_lin_vel
 linear velocity noise sigma in m/s^2
double sigma_ang_vel
 angular velocity noise sigma in rad/s^2
double delta_t
 time between frames

Static Public Attributes

static const size_t state_size = 13
 State Vector Size "xv" (in monoslam will be 13).
static const size_t feature_size = 6
 Feature size stored in xkk (in monoslam will be 6).

Detailed Description

General motion model in 3D Assumes a random impulse changes the velocity at each time step.

Impulse 3D Motion Model: Class to represent a constant-velocity motion model. A constant-velocity motion model does not mean that the camera moves at a constant velocity over all time, but that the statistical model of its motion in a timestep is that on average we expect undetermined accelerations occur with a Gaussian profile. At each timestep there is a random impulse, but these are normally-distributed. State vector: 13 elements

                                                 x
                  r              y
                                                 z
                  -              -
                                                 q0
                  q              qx
                                                 qy
                                                 qz
                  -              -
                                                 vx
                  v              vy
                                                 vz
                  -              -
                                                 omegax
                  omega          omegay
                                                 omegaz

Control vector has 3 elements, and represents a measurement of acceleration if a linear accelerometer is present. Otherwise set it to zero.

                 Noise vector n = V
                                                  Omega

Update:

                 rnew     =   r + (v + V) delta_t
                 qnew     =   q x q((omega + Omega) delta_t)
                 vnew     =   v + V
                 omeganew =   omega + Omega

Here we can use the specific dimension because is a specific class for monocular SLAM with Inverse Depth Parametrization

Definition at line 89 of file CMonoSlamMotionModel.h.


Constructor & Destructor Documentation

mrpt::monoslam::CMonoSlamMotionModel::CMonoSlamMotionModel (  ) 

Default constructor.


Member Function Documentation

void mrpt::monoslam::CMonoSlamMotionModel::compose_xv ( CVectorDouble xv,
const CVectorDouble r,
const CQuaternionDouble q,
const CVectorDouble v,
const CVectorDouble w 
)

Form the state vector from their individual components.

Parameters:
xv --> State Vector
r --> camera position
q --> quaternion
v --> linear velocity
w --> angular velocity
double mrpt::monoslam::CMonoSlamMotionModel::dq0_by_domegaA ( double  omegaA,
double  omega 
)

Ancillary function to calculate part of Jacobian partial q / partial omega which is repeatable due to symmetry.

Here omegaA is one of omegax, omegay, omegaz.

void mrpt::monoslam::CMonoSlamMotionModel::dq3_dq1 ( mrpt::math::CQuaternion< double > &  q2,
CMatrixDouble M 
)

Auxiliar ancillary function to calculate part of Jacobian which is repeatable due to symmetry.

void mrpt::monoslam::CMonoSlamMotionModel::dq3_dq2 ( mrpt::math::CQuaternion< double > &  q1,
CMatrixDouble M 
)

Auxiliar ancillary function to calculate part of Jacobian which is repeatable due to symmetry.

double mrpt::monoslam::CMonoSlamMotionModel::dqA_by_domegaA ( double  omegaA,
double  omega 
)

Ancillary function to calculate part of Jacobian which is repeatable due to symmetry.

Here omegaA is one of omegax, omegay, omegaz and similarly with qA.

double mrpt::monoslam::CMonoSlamMotionModel::dqA_by_domegaB ( double  omegaA,
double  omegaB,
double  omega 
)

Ancillary function to calculate part of Jacobian which is repeatable due to symmetry.

Here omegaB is one of omegax, omegay, omegaz and similarly with qA.

void mrpt::monoslam::CMonoSlamMotionModel::dqomegadt_by_domega ( const CVectorDouble omega,
CMatrixDouble RESdqomegadt_by_domega 
)

Calculate commonly used Jacobian part dq(omega * delta_t) by domega.

void mrpt::monoslam::CMonoSlamMotionModel::extract_components ( CVectorDouble xv,
CVectorDouble r,
CQuaternionDouble q,
CVectorDouble v,
CVectorDouble w 
)

Extract from state vector their components:.

Parameters:
xv --> State Vector
r --> camera position
q --> quaternion
v --> linear velocity
w --> angular velocity
void mrpt::monoslam::CMonoSlamMotionModel::func_dfv_by_dxv ( CVectorDouble xv,
CMatrixDouble F 
)

Calculate the Jacobian 'dfv/dxv'.

                        Identity is a good place to start since overall structure is like this
                        I       0             I * delta_t   0
                        0       dqnew_by_dq   0             dqnew_by_domega
                        0       0             I             0
                        0       0             0             I
void mrpt::monoslam::CMonoSlamMotionModel::func_dxp_by_dxv ( const CVectorDouble xv,
CMatrixDouble m 
)

In the case of Monocular SLAM this Jacobian is a 7x7 identity matrix.

void mrpt::monoslam::CMonoSlamMotionModel::func_fv ( CVectorDouble xv  ) 

This function predict the state vector: Prediction model (camera position, angle, linear velocity and angular velocity).

Parameters:
xv is the actual state vector
r_{new} = r + (v+V)*Delta t
q_{new} = q * q(omega + Omega) * Delta t)
v_{new} = v + V
w_{new} = w + W [x y z qr qx qy qz vx vy vz wx wy wz]
void mrpt::monoslam::CMonoSlamMotionModel::func_Q ( CVectorDouble xv,
CMatrixDouble QRES 
)

Calculate the covariance noise matrix associated to the process 'Q' It's necessary to calculate the prediction 'P(k+1|k)' Fill noise covariance matrix Pnn: this is the covariance of the noise vector [V Omega]^T that gets added to the state.

Form of this could change later, but for now assume that V and Omega are independent, and that each of their components is independent.

double mrpt::monoslam::CMonoSlamMotionModel::get_delta_t (  )  const [inline]

Read time between frames.

Definition at line 152 of file CMonoSlamMotionModel.h.

size_t mrpt::monoslam::CMonoSlamMotionModel::get_feature_size (  )  const [inline]

Return the dimension of a feature (in monoSLAM employing Unified Inverse Depth Parametrization is 6).

Definition at line 135 of file CMonoSlamMotionModel.h.

double mrpt::monoslam::CMonoSlamMotionModel::get_sigma_ang_vel (  )  const [inline]

Read the variance of angular velocity.

Definition at line 149 of file CMonoSlamMotionModel.h.

double mrpt::monoslam::CMonoSlamMotionModel::get_sigma_lin_vel (  )  const [inline]

Read the variance of linear velocity.

Definition at line 146 of file CMonoSlamMotionModel.h.

size_t mrpt::monoslam::CMonoSlamMotionModel::get_state_size (  )  const [inline]

Return the state dimension (in monoSLAM is 13).

Definition at line 139 of file CMonoSlamMotionModel.h.

CMatrixDouble mrpt::monoslam::CMonoSlamMotionModel::pxyztpl2pxyz ( CMatrixDouble pkk,
CVectorDouble xkk 
)

Convert covariance matrix from inverse detph parametrization diag(xc,yc,zc,theta,phi,lambda) to 3D covariance matrix diag(x,y,z).

void mrpt::monoslam::CMonoSlamMotionModel::set_delta_t ( const double  dt  )  [inline]

Set the time between frames.

Definition at line 143 of file CMonoSlamMotionModel.h.

CMatrixDouble mrpt::monoslam::CMonoSlamMotionModel::XYZTPL2XYZ ( CVectorDouble xkk  ) 

Convert coordenates from inverse detph parametrization (xc,yc,zc,theta,phi,lambda) to 3D (x,y,z) coordinates Return a matrix where each row is a feature in which:

  • column 0 --> X coordinate
  • column 1 --> Y coordinate
  • column 2 --> Z coordinate xc,yc,zc : camera position in world coord system theta,phi,lambda : fteature orientation and depth params.


Member Data Documentation

time between frames

Definition at line 97 of file CMonoSlamMotionModel.h.

Feature size stored in xkk (in monoslam will be 6).

Definition at line 93 of file CMonoSlamMotionModel.h.

angular velocity noise sigma in rad/s^2

Definition at line 96 of file CMonoSlamMotionModel.h.

linear velocity noise sigma in m/s^2

Definition at line 95 of file CMonoSlamMotionModel.h.

State Vector Size "xv" (in monoslam will be 13).

Definition at line 92 of file CMonoSlamMotionModel.h.




Page generated by Doxygen 1.6.1 for MRPT 0.7.1 SVN: at Tue Dec 22 08:29:35 CET 2009