1
- /* ***************************************************************************
2
- *
1
+ /*
3
2
* ViSP, open source Visual Servoing Platform software.
4
3
* Copyright (C) 2005 - 2024 by Inria. All rights reserved.
5
4
*
27
26
*
28
27
* This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29
28
* WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30
- *
31
- *****************************************************************************/
29
+ */
32
30
33
31
/* * \example pf-nonlinear-example.cpp
34
32
* Example on how to use a Particle Filter (PF) on a complex non-linear use-case.
38
36
* fixed to the ceiling.
39
37
*
40
38
* The state vector of the PF is:
41
- * \f{eqnarray*}{
42
- \textbf{x}[0] &=& {}^WX_x \\
43
- \textbf{x}[1] &=& {}^WX_y \\
44
- \textbf{x}[2] &=& {}^WX_z \\
45
- \textbf{x}[3] &=& \omega \Delta t
46
- \f}
47
-
48
- The measurement \f$ \textbf{z} \f$ corresponds to the coordinates in pixels of the different markers.
49
- Be \f$ u_i \f$ and \f$ v_i \f$ the horizontal and vertical pixel coordinates of the \f$ i^{th} \f$ marker.
50
- The measurement vector can be written as:
51
- \f{eqnarray*}{
52
- \textbf{z}[2i] &=& u_i \\
53
- \textbf{z}[2i+1] &=& v_i
54
- \f}
55
-
39
+ * \f{eqnarray*}{
40
+ * \textbf{x}[0] &=& {}^WX_x \\
41
+ * \textbf{x}[1] &=& {}^WX_y \\
42
+ * \textbf{x}[2] &=& {}^WX_z \\
43
+ * \textbf{x}[3] &=& \omega \Delta t
44
+ * \f}
45
+ *
46
+ * The measurement \f$ \textbf{z} \f$ corresponds to the coordinates in pixels of the different markers.
47
+ * Be \f$ u_i \f$ and \f$ v_i \f$ the horizontal and vertical pixel coordinates of the \f$ i^{th} \f$ marker.
48
+ * The measurement vector can be written as:
49
+ * \f{eqnarray*}{
50
+ * \textbf{z}[2i] &=& u_i \\
51
+ * \textbf{z}[2i+1] &=& v_i
52
+ * \f}
53
+ *
56
54
* Some noise is added to the measurement vector to simulate measurements which are
57
55
* not perfect.
58
- */
56
+ */
59
57
60
58
// ViSP includes
61
59
#include < visp3/core/vpConfig.h>
@@ -690,8 +688,6 @@ int main(const int argc, const char *argv[])
690
688
// Compute the pose using the noisy markers
691
689
vpHomogeneousMatrix cMo_noisy = computePose (markersAsVpPoint, ip, cam);
692
690
vpHomogeneousMatrix wMo_noisy = cMw.inverse () * cMo_noisy;
693
- double wXnoisy = wMo_noisy[0 ][3 ];
694
- double wYnoisy = wMo_noisy[1 ][3 ];
695
691
// ! [Noisy_pose]
696
692
697
693
// ! [Update_displays]
@@ -705,6 +701,8 @@ int main(const int argc, const char *argv[])
705
701
plot->plot (0 , 1 , Xest[0 ], Xest[1 ]);
706
702
707
703
// Plot the noisy pose
704
+ double wXnoisy = wMo_noisy[0 ][3 ];
705
+ double wYnoisy = wMo_noisy[1 ][3 ];
708
706
plot->plot (0 , 2 , wXnoisy, wYnoisy);
709
707
// ! [Update_plot]
710
708
0 commit comments