diff --git a/tutorial/particle-filter-curve-fitting/vpTutoCommonData.h b/tutorial/particle-filter-curve-fitting/vpTutoCommonData.h index f6d6757681..01d422a96c 100644 --- a/tutorial/particle-filter-curve-fitting/vpTutoCommonData.h +++ b/tutorial/particle-filter-curve-fitting/vpTutoCommonData.h @@ -83,14 +83,10 @@ typedef struct vpTutoCommonData VISP_NAMESPACE_ADDRESSING vpImage m_mask; /*!< A binary mask where 255 means that a pixel belongs to the HSV range delimited by the HSV thresholds.*/ VISP_NAMESPACE_ADDRESSING vpImage m_Iskeleton; /*!< The image resulting from the skeletonization of the mask.*/ VISP_NAMESPACE_ADDRESSING vpImage m_IskeletonNoisy; /*!< The image resulting from the skeletonization of the mask, to which is added some salt and pepper noise.*/ -#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) && defined(VISP_HAVE_DISPLAY) +#if defined(VISP_HAVE_DISPLAY) std::shared_ptr m_displayOrig; std::shared_ptr m_displaySegmented; std::shared_ptr m_displayNoisy; -#elif defined(VISP_HAVE_DISPLAY) - VISP_NAMESPACE_ADDRESSING vpDisplay *m_displayOrig; - VISP_NAMESPACE_ADDRESSING vpDisplay *m_displaySegmented; - VISP_NAMESPACE_ADDRESSING vpDisplay *m_displayNoisy; #endif /// Particle filter parameters @@ -106,11 +102,6 @@ typedef struct vpTutoCommonData , m_stepbystep(true) , m_ratioSaltPepperNoise(0.15) , m_degree(2) -#if (VISP_CXX_STANDARD < VISP_CXX_STANDARD_11) && defined(VISP_HAVE_DISPLAY) - , m_displayOrig(nullptr) - , m_displaySegmented(nullptr) - , m_displayNoisy(nullptr) -#endif , m_pfMaxDistanceForLikelihood(40) , m_pfN(300) , m_pfRatiosAmpliMax({ 0.25, 0.25, 0.25 }) @@ -118,25 +109,6 @@ typedef struct vpTutoCommonData , m_pfNbThreads(-1) { } -#if (VISP_CXX_STANDARD < VISP_CXX_STANDARD_11) && defined(VISP_HAVE_DISPLAY) - ~vpTutoCommonData() - { - if (m_displayOrig != nullptr) { - delete m_displayOrig; - m_displayOrig = nullptr; - } - if (m_displaySegmented != nullptr) { - delete m_displaySegmented; - m_displaySegmented = nullptr; - } - - if (m_displayNoisy != nullptr) { - delete m_displayNoisy; - m_displayNoisy = nullptr; - } - } -#endif - /** * \brief Print the help about the program optional parameters. * @@ -291,18 +263,14 @@ typedef struct vpTutoCommonData m_IskeletonNoisy.resize(m_I_orig.getHeight(), m_I_orig.getWidth()); // Resize the edge-map. // Init the displays +#if defined(VISP_HAVE_DISPLAY) const int horOffset = 20, vertOffset = 25; std::string skeletonTitle("Skeletonized image ("); skeletonTitle += (m_ratioSaltPepperNoise == 0 ? "without" : std::to_string(static_cast(m_ratioSaltPepperNoise * 100.)) + "%"); skeletonTitle += " noise)"; -#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) && defined(VISP_HAVE_DISPLAY) m_displayOrig = VISP_NAMESPACE_ADDRESSING vpDisplayFactory::createDisplay(m_I_orig, horOffset, vertOffset, "Original image"); m_displaySegmented = VISP_NAMESPACE_ADDRESSING vpDisplayFactory::createDisplay(m_I_segmented, 2 * horOffset + m_I_orig.getWidth(), vertOffset, "Segmented image"); m_displayNoisy = VISP_NAMESPACE_ADDRESSING vpDisplayFactory::createDisplay(m_IskeletonNoisy, 2 * horOffset + m_I_orig.getWidth(), 2 * vertOffset + m_I_orig.getHeight(), skeletonTitle); -#elif defined(VISP_HAVE_DISPLAY) - m_displayOrig = VISP_NAMESPACE_ADDRESSING vpDisplayFactory::allocateDisplay(m_I_orig, horOffset, vertOffset, "Original image"); - m_displaySegmented = VISP_NAMESPACE_ADDRESSING vpDisplayFactory::allocateDisplay(m_I_segmented, 2 * horOffset + m_I_orig.getWidth(), vertOffset, "Segmented image"); - m_displayNoisy = VISP_NAMESPACE_ADDRESSING vpDisplayFactory::allocateDisplay(m_IskeletonNoisy, 2 * horOffset + m_I_orig.getWidth(), 2 * vertOffset + m_I_orig.getHeight(), skeletonTitle); #endif return SOFTWARE_CONTINUE; } diff --git a/tutorial/particle-filter/tutorial-pf.cpp b/tutorial/particle-filter/tutorial-pf.cpp index 737ea9e21b..bcb5b48de7 100644 --- a/tutorial/particle-filter/tutorial-pf.cpp +++ b/tutorial/particle-filter/tutorial-pf.cpp @@ -740,6 +740,32 @@ int main(const int argc, const char *argv[]) double dtUKF = vpTime::measureTimeMs() - tUKF; //! [UKF_filtering] + // Get the filtered states + vpColVector XestPF = pfFilter.computeFilteredState(); + vpColVector XestUKF = ukf.getXest(); + + // Compute satistics + vpColVector cX_GT = cMw * object_pos; + vpColVector wX_UKF(4, 1.); + vpColVector wX_PF(4, 1.); + for (unsigned int i = 0; i < 3; ++i) { + wX_PF[i] = XestPF[i]; + wX_UKF[i] = XestUKF[i]; + } + vpColVector cX_PF = cMw * wX_PF; + vpColVector cX_UKF = cMw * wX_UKF; + vpColVector error_PF = cX_PF - cX_GT; + vpColVector error_UKF = cX_UKF - cX_GT; + + // Log statistics + std::cout << " [Particle Filter method] " << std::endl; + std::cout << " Norm of the error = " << error_PF.frobeniusNorm() << " m^2" << std::endl; + std::cout << " Fitting duration = " << dtPF << " ms" << std::endl; + + std::cout << " [Unscented Kalman Filter method] " << std::endl; + std::cout << " Norm of the error = " << error_UKF.frobeniusNorm() << " m^2" << std::endl; + std::cout << " Fitting duration = " << dtUKF << " ms" << std::endl; + //! [Update_displays] #ifdef VISP_HAVE_DISPLAY //! [Noisy_pose] @@ -763,37 +789,15 @@ int main(const int argc, const char *argv[]) plot.plot(0, 0, object_pos[0], object_pos[1]); // Plot the PF filtered state - vpColVector XestPF = pfFilter.computeFilteredState(); plot.plot(0, 1, XestPF[0], XestPF[1]); // Plot the UKF filtered state - vpColVector XestUKF = ukf.getXest(); plot.plot(0, 2, XestUKF[0], XestUKF[1]); // Plot the noisy pose plot.plot(0, 3, wXnoisy, wYnoisy); - vpColVector cX_GT = cMw * object_pos; - vpColVector wX_UKF(4, 1.); - vpColVector wX_PF(4, 1.); - for (unsigned int i = 0; i < 3; ++i) { - wX_PF[i] = XestPF[i]; - wX_UKF[i] = XestUKF[i]; - } - vpColVector cX_PF = cMw * wX_PF; - vpColVector cX_UKF = cMw * wX_UKF; - vpColVector error_PF = cX_PF - cX_GT; - vpColVector error_UKF = cX_UKF - cX_GT; - - std::cout << " [Particle Filter method] " << std::endl; - std::cout << " Norm of the error = " << error_PF.frobeniusNorm() << " m^2" << std::endl; - std::cout << " Fitting duration = " << dtPF << " ms" << std::endl; - - std::cout << " [Unscented Kalman Filter method] " << std::endl; - std::cout << " Norm of the error = " << error_UKF.frobeniusNorm() << " m^2" << std::endl; - std::cout << " Fitting duration = " << dtUKF << " ms" << std::endl; - // Plot the PF filtered state error plotError.plot(0, 0, t, error_PF.frobeniusNorm());