diff --git a/doc/tutorial/misc/tutorial-ukf.dox b/doc/tutorial/misc/tutorial-ukf.dox index fb8d43baf8..e0395ac3ad 100644 --- a/doc/tutorial/misc/tutorial-ukf.dox +++ b/doc/tutorial/misc/tutorial-ukf.dox @@ -232,38 +232,115 @@ As the process function is pretty simple, a simple function called here fx is en \subsubsection tuto-ukf-tutorial-explained-markers Details on the class simulating marker measurement +The measurements of the projection of the markers in the image are handled by the following class: + \snippet tutorial-ukf.cpp Markers_class +It takes as input the camera parameters cam, the homogeneous matrix expressing the pose of the world frame +\f$ \mathcal{F}_W \f$ with regard to the camera frame \f$ \mathcal{F}_C \f$ cMw, the rotation matrix that +expresses the rotation between the object frame and world frame wRo and the homogeneous coordinates of the +markers expressed in the object frame markers to be able to convert the 3D position of the object in the +world frame \f$ {}^W \textbf{X} \f$ into 3D positions of the markers in the camera frame \f$ {}^C \textbf{X}^i \f$, where +\f$ i \f$ denotes the i\f$^th\f$ marker sticked on the object. The standard deviation of the noise noise_stdev +and the seed value seed are here to initialized the Gaussian noise generator used to simulate noisy measurements. + +The method state_to_measurement is used to convert the internal state of the UKF into the measurement space +(i.e. the projection in the image of the markers sticked on the object if the object is at this 3D position): + \snippet tutorial-ukf.cpp Measurement_function +The method measureGT is used to convert the ground truth 3D position of the object into ground truth +projections of the markers in the image: + \snippet tutorial-ukf.cpp GT_measurements +The method measureWithNoise adds noise to the ground truth measurements in order to simulate a noisy +measurement process: + \snippet tutorial-ukf.cpp Noisy_measurements \subsubsection tuto-ukf-tutorial-explained-pose Details on the computation of the pose from noisy measurements +The method computePose compute the 3D pose of an object from the 3D coordinates along with their projection +in the image of a plane surface. Here, we use it to convert the noisy measurements in a noisy 3D pose, in order to +compare the 3D position estimated by the UKF with regard to the pose we would have if we computed the pose directly +from the noisy measurements. + \snippet tutorial-ukf.cpp Pose_for_display \subsubsection tuto-ukf-tutorial-explained-constants Details on the constants of the main loop +In the main loop of the program, we first declare some constants that will be used later on: + \snippet tutorial-ukf.cpp Constants_for_simulation +Here is their meanings: +- dt is the sampling period (the time spent between two acquisitions), +- sigmaMeasurements is the standard deviation of the Gaussian noise added to the measurements, +- radius is the radius of the revolution of the object around the world frame origin, +- w is the pulsation of the motion of revolution, +- phi is the phase of the motion of revolution, +- markers is a vector containing the homogeneous coordinates expressed in the object frame of the markers, +- markersAsVpPoint is a vector containing the 3D coordinates of the markers expressed in the object (to compute the noisy pose as explained previously), +- seed is the seed for the Gaussian noise generator that adds noise to the projections of the markers in the image, +- cMw is the homogeneous matrix expressing the pose of the world frame with regard to the camera frame, +- wMo is the homogeneous matrix expressing the pose of the object frame with regard to the world frame, +- wRo is the rotation matrix contained in wMo +- wZ is the z-axis coordinate of the origin of the object frame expressed in the world frame. + +To convert the 3D position of the object into the projection of its markers in the image, we need camera parameters. We +generate camera parameters for a simulated camera as follow: + \snippet tutorial-ukf.cpp Camera_for_measurements \subsubsection tuto-ukf-tutorial-explained-initukf Details on the initialization of the UKF +To initialize the UKF, we need an object that will be able to compute the sigma points and their associated weights. To +do so, we must create an instance of a class inheriting from the class vpUKSigmaDrawerAbstract. In our case, we decided +to use the method proposed by E. A. Wan and R. van der Merwe and implemented in the vpUKSigmaDrawerMerwe class: + \snippet tutorial-ukf.cpp Sigma_points_drawer +The first parameter is the dimension of the state of the UKF. The second parameter is \f$ \alpha \f$: the greater it is +the further of the mean the sigma points are; it is a real and its value must be between 0 and 1. The third parameter is +\f$ \beta \f$: it is a real whose value is set to two for Gaussian problems. Finally, the last parameter is \f$ \kappa \f$: +it is a real whose value must be set to \f$ 3 - n \f$ for most problems. + +The UKF needs the covariance matrix of the measurements \f$ \textbf{R} \f$ that represents the uncertainty of the +measurements: + \snippet tutorial-ukf.cpp Covariance_measurements +The UKF needs the covariance matrix of the process \f$ \textbf{Q} \f$ that represents the uncertainty induced during the +prediction step: + \snippet tutorial-ukf.cpp Covariance_process +The UKF needs the an estimate of the intial state \f$ \textbf{x}_0 \f$ and of its covariance \f$ \textbf{P}_0 \f$: + \snippet tutorial-ukf.cpp Initial_estimates +Next, we initialize the process function and the measurement function: + \snippet tutorial-ukf.cpp Init_functions +Finally, we create the UKF and initialize its state: + \snippet tutorial-ukf.cpp Init_UKF +If the internal state cannot use the standard addition and substraction, it would be necessary to write other methods: +- a method permitting to add two state vectors (see vpUnscentedKalman::setStateAddFunction), +- a method permitting to substract two state vectors (see vpUnscentedKalman::setStateResidualFunction), +- and a method permitting to compute the mean of several state vectors (see vpUnscentedKalman::setStateMeanFunction). + +If some commands are known to have an effect on the internal state, it would be necessary to write other methods: +- a method for the effects of the commands on the state, without knowing the state (see vpUnscentedKalman::setCommandOnlyFunction), +- and/or a method for the effects of the commands on the state, knowing the state (see vpUnscentedKalman::setCommandStateFunction). + +If the measurement space cannot use the standard addition and substraction, it would be necessary to write other methods: +- a method permitting to substract two measurement vectors (see vpUnscentedKalman::setMeasurementResidualFunction), +- and a method permitting to compute the mean of several state vectors (see vpUnscentedKalman::setMeasurementMeanFunction). + \subsubsection tuto-ukf-tutorial-explained-initgui Details on the initialization of the Graphical User Interface \snippet tutorial-ukf.cpp Init_plot