00001 /* +---------------------------------------------------------------------------+ 00002 | The Mobile Robot Programming Toolkit (MRPT) C++ library | 00003 | | 00004 | http://mrpt.sourceforge.net/ | 00005 | | 00006 | Copyright (C) 2005-2009 University of Malaga | 00007 | | 00008 | This software was written by the Perception and Robotics | 00009 | research group, University of Malaga (Spain). | 00010 | Contact: Jose-Luis Blanco <jlblanco@ctima.uma.es> | 00011 | | 00012 | This file is part of the MRPT project. | 00013 | | 00014 | MRPT is free software: you can redistribute it and/or modify | 00015 | it under the terms of the GNU General Public License as published by | 00016 | the Free Software Foundation, either version 3 of the License, or | 00017 | (at your option) any later version. | 00018 | | 00019 | MRPT is distributed in the hope that it will be useful, | 00020 | but WITHOUT ANY WARRANTY; without even the implied warranty of | 00021 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | 00022 | GNU General Public License for more details. | 00023 | | 00024 | You should have received a copy of the GNU General Public License | 00025 | along with MRPT. If not, see <http://www.gnu.org/licenses/>. | 00026 | | 00027 +---------------------------------------------------------------------------+ */ 00028 00029 #ifndef CMonoSlamMotionModel_H 00030 #define CMonoSlamMotionModel_H 00031 00032 #include <mrpt/utils/utils_defs.h> 00033 #include <mrpt/utils/CConfigFileBase.h> 00034 #include <mrpt/math/CMatrixTemplateNumeric.h> 00035 #include <mrpt/math/CVectorTemplate.h> 00036 #include <mrpt/math/CQuaternion.h> 00037 #include <mrpt/monoslam/link_pragmas.h> 00038 00039 namespace mrpt 00040 { 00041 namespace monoslam 00042 { 00043 using namespace mrpt::math; 00044 00045 /** General motion model in 3D 00046 * Assumes a random impulse changes the velocity at each time step. 00047 * 00048 * Impulse 3D Motion Model: 00049 * Class to represent a constant-velocity motion model. 00050 * A constant-velocity motion model does not mean that the camera moves at a constant velocity over all time, 00051 * but that the statistical model of its motion in a timestep is that on average we expect undetermined accelerations 00052 * occur with a Gaussian profile. At each timestep there is a random impulse, but these are normally-distributed. 00053 * State vector: 13 elements 00054 \code 00055 x 00056 r y 00057 z 00058 - - 00059 q0 00060 q qx 00061 qy 00062 qz 00063 - - 00064 vx 00065 v vy 00066 vz 00067 - - 00068 omegax 00069 omega omegay 00070 omegaz 00071 \endcode 00072 * Control vector has 3 elements, and represents a measurement of 00073 * acceleration if a linear accelerometer is present. Otherwise set it 00074 * to zero. 00075 \code 00076 Noise vector n = V 00077 Omega 00078 \endcode 00079 * Update: 00080 \code 00081 rnew = r + (v + V) delta_t 00082 qnew = q x q((omega + Omega) delta_t) 00083 vnew = v + V 00084 omeganew = omega + Omega 00085 \endcode 00086 * Here we can use the specific dimension because is a specific class for monocular SLAM with 00087 * Inverse Depth Parametrization 00088 */ 00089 class MONOSLAMDLLIMPEXP CMonoSlamMotionModel 00090 { 00091 public: 00092 static const size_t state_size = 13; //!< State Vector Size "xv" (in monoslam will be 13) 00093 static const size_t feature_size = 6; //!< Feature size stored in xkk (in monoslam will be 6) 00094 00095 double sigma_lin_vel; //!< linear velocity noise sigma in m/s^2 00096 double sigma_ang_vel; //!< angular velocity noise sigma in rad/s^2 00097 double delta_t; //!< time between frames 00098 00099 public: 00100 00101 /** Calculate commonly used Jacobian part dq(omega * delta_t) by domega 00102 */ 00103 void dqomegadt_by_domega(const CVectorDouble &omega, CMatrixDouble &RESdqomegadt_by_domega); 00104 00105 /** Ancillary function to calculate part of Jacobian partial q / partial omega which is repeatable 00106 * due to symmetry. Here omegaA is one of omegax, omegay, omegaz. 00107 */ 00108 double dq0_by_domegaA(double omegaA, double omega); 00109 00110 /** Ancillary function to calculate part of Jacobian which is repeatable due 00111 * to symmetry. Here omegaA is one of omegax, omegay, omegaz and similarly with qA. 00112 */ 00113 double dqA_by_domegaA(double omegaA, double omega); 00114 00115 /** Ancillary function to calculate part of Jacobian which is repeatable due to symmetry. 00116 * Here omegaB is one of omegax, omegay, omegaz and similarly with qA. 00117 */ 00118 double dqA_by_domegaB(double omegaA, double omegaB,double omega); 00119 00120 /** Auxiliar ancillary function to calculate part of Jacobian which is repeatable due to symmetry. 00121 */ 00122 void dq3_dq1(mrpt::math::CQuaternion<double>& q2,CMatrixDouble &M); 00123 00124 /** Auxiliar ancillary function to calculate part of Jacobian which is repeatable due to symmetry. 00125 */ 00126 void dq3_dq2(mrpt::math::CQuaternion<double>& q1,CMatrixDouble &M); 00127 00128 public: 00129 /** Default constructor 00130 */ 00131 CMonoSlamMotionModel(); 00132 00133 /** Return the dimension of a feature (in monoSLAM employing Unified Inverse Depth Parametrization is 6) 00134 */ 00135 size_t get_feature_size()const {return feature_size;} 00136 00137 /** Return the state dimension (in monoSLAM is 13) 00138 */ 00139 size_t get_state_size()const {return state_size;} 00140 00141 /** Set the time between frames 00142 */ 00143 void set_delta_t(const double dt) {delta_t = dt;} 00144 00145 /** Read the variance of linear velocity */ 00146 double get_sigma_lin_vel()const {return sigma_lin_vel;} 00147 00148 /** Read the variance of angular velocity */ 00149 double get_sigma_ang_vel()const {return sigma_ang_vel;} 00150 00151 /** Read time between frames */ 00152 double get_delta_t()const {return delta_t;} 00153 00154 /** Extract from state vector their components: 00155 * \param xv --> State Vector 00156 * \param r --> camera position 00157 * \param q --> quaternion 00158 * \param v --> linear velocity 00159 * \param w --> angular velocity 00160 */ 00161 void extract_components(CVectorDouble &xv, 00162 CVectorDouble &r, 00163 CQuaternionDouble &q, 00164 CVectorDouble &v, 00165 CVectorDouble &w); 00166 00167 /** Form the state vector from their individual components 00168 * \param xv --> State Vector 00169 * \param r --> camera position 00170 * \param q --> quaternion 00171 * \param v --> linear velocity 00172 * \param w --> angular velocity 00173 */ 00174 void compose_xv(CVectorDouble &xv, 00175 const CVectorDouble &r, 00176 const CQuaternionDouble &q, 00177 const CVectorDouble &v, 00178 const CVectorDouble &w); 00179 00180 /** This function predict the state vector: Prediction model (camera position, angle, linear velocity and angular velocity) 00181 * \param xv is the actual state vector 00182 * \param r_{new} = r + (v+V)*Delta t 00183 * \param q_{new} = q * q(omega + Omega) * Delta t) 00184 * \param v_{new} = v + V 00185 * \param w_{new} = w + W 00186 * [x y z qr qx qy qz vx vy vz wx wy wz] 00187 */ 00188 void func_fv(CVectorDouble &xv); 00189 00190 /** Calculate the Jacobian 'dfv/dxv' 00191 \code 00192 Identity is a good place to start since overall structure is like this 00193 I 0 I * delta_t 0 00194 0 dqnew_by_dq 0 dqnew_by_domega 00195 0 0 I 0 00196 0 0 0 I 00197 \endcode 00198 */ 00199 void func_dfv_by_dxv(CVectorDouble &xv, CMatrixDouble &F); 00200 00201 /** Calculate the covariance noise matrix associated to the process 'Q' 00202 * It's necessary to calculate the prediction 'P(k+1|k)' 00203 * Fill noise covariance matrix Pnn: this is the covariance of the noise vector [V Omega]^T 00204 * that gets added to the state. 00205 * Form of this could change later, but for now assume that 00206 * V and Omega are independent, and that each of their components is independent. 00207 */ 00208 void func_Q(CVectorDouble &xv,CMatrixDouble &QRES); 00209 00210 /** In the case of Monocular SLAM this Jacobian is a 7x7 identity matrix 00211 * 00212 */ 00213 void func_dxp_by_dxv(const CVectorDouble &xv,CMatrixDouble &m); 00214 00215 00216 /** Convert coordenates from inverse detph parametrization (xc,yc,zc,theta,phi,lambda) to 3D (x,y,z) coordinates 00217 * Return a matrix where each row is a feature in which: 00218 * - column 0 --> X coordinate 00219 * - column 1 --> Y coordinate 00220 * - column 2 --> Z coordinate 00221 * xc,yc,zc : camera position in world coord system 00222 * theta,phi,lambda : fteature orientation and depth params 00223 */ 00224 CMatrixDouble XYZTPL2XYZ(CVectorDouble &xkk); 00225 00226 /** Convert covariance matrix from inverse detph parametrization diag(xc,yc,zc,theta,phi,lambda) to 3D 00227 * covariance matrix diag(x,y,z). 00228 */ 00229 CMatrixDouble pxyztpl2pxyz(CMatrixDouble &pkk, CVectorDouble &xkk); 00230 00231 }; // end class 00232 } // end namespace 00233 } // end namespace 00234 00235 #endif //__CMonoSlamMotionModel.h
Page generated by Doxygen 1.5.9 for MRPT 0.7.1 SVN: at Mon Aug 17 22:21:34 EDT 2009 |