#include <mrpt/monoslam/CMonoSlamMotionModel.h>
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:
| |
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). |
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
Noise vector n = V Omega
rnew = r + (v + V) delta_t qnew = q x q((omega + Omega) delta_t) vnew = v + V omeganew = omega + Omega
Definition at line 89 of file CMonoSlamMotionModel.h.
mrpt::monoslam::CMonoSlamMotionModel::CMonoSlamMotionModel | ( | ) |
Default constructor.
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.
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:.
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).
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] |
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] |
double mrpt::monoslam::CMonoSlamMotionModel::get_sigma_lin_vel | ( | ) | const [inline] |
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] |
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:
const size_t mrpt::monoslam::CMonoSlamMotionModel::feature_size = 6 [static] |
Feature size stored in xkk (in monoslam will be 6).
Definition at line 93 of file CMonoSlamMotionModel.h.
const size_t mrpt::monoslam::CMonoSlamMotionModel::state_size = 13 [static] |
State Vector Size "xv" (in monoslam will be 13).
Definition at line 92 of file CMonoSlamMotionModel.h.
Page generated by Doxygen 1.5.9 for MRPT 0.7.1 SVN: at Mon Aug 17 22:20:53 EDT 2009 |