Skip to content

Commit

Permalink
Merge pull request #1462 from rolalaro/fix_compil_warning_pf
Browse files Browse the repository at this point in the history 
Compilation warnings in PF tutorials
  • Loading branch information
@fspindle
fspindle authored Sep 16, 2024
2 parents 544f9a4 + 0a5ef8d commit 7faf0cd
Show file tree
Hide file tree
Showing 2 changed files with 28 additions and 56 deletions.
36 changes: 2 additions & 34 deletions tutorial/particle-filter-curve-fitting/vpTutoCommonData.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -83,14 +83,10 @@ typedef struct vpTutoCommonData
VISP_NAMESPACE_ADDRESSING vpImage<unsigned char> 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<unsigned char> m_Iskeleton; /*!< The image resulting from the skeletonization of the mask.*/
VISP_NAMESPACE_ADDRESSING vpImage<unsigned char> 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<VISP_NAMESPACE_ADDRESSING vpDisplay> m_displayOrig;
std::shared_ptr<VISP_NAMESPACE_ADDRESSING vpDisplay> m_displaySegmented;
std::shared_ptr<VISP_NAMESPACE_ADDRESSING vpDisplay> 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
Expand All @@ -106,37 +102,13 @@ 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 })
, m_pfSeed(4221)
, 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.
*
Expand Down Expand Up @@ -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<unsigned int>(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;
}
Expand Down
48 changes: 26 additions & 22 deletions tutorial/particle-filter/tutorial-pf.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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]
Expand All @@ -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());

Expand Down

0 comments on commit 7faf0cd

Please sign in to comment.