38 #include <visp3/core/vpConfig.h>
39 #include <visp3/core/vpException.h>
40 #include <visp3/core/vpMath.h>
41 #include <visp3/core/vpMouseButton.h>
42 #include <visp3/core/vpTime.h>
44 #ifdef VISP_HAVE_DISPLAY
45 #include <visp3/gui/vpPlot.h>
49 #include <visp3/core/vpParticleFilter.h>
52 #include "vpTutoCommonData.h"
53 #include "vpTutoMeanSquareFitting.h"
54 #include "vpTutoParabolaModel.h"
55 #include "vpTutoSegmentation.h"
57 #ifdef ENABLE_VISP_NAMESPACE
61 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) && defined(VISP_HAVE_DISPLAY)
62 #ifndef DOXYGEN_SHOULD_SKIP_THIS
73 double evaluate(
const vpImagePoint &pt,
const vpTutoParabolaModel &model)
75 double u = pt.
get_u();
76 double v = pt.
get_v();
77 double v_model = model.eval(u);
78 double error = v - v_model;
79 double squareError = error * error;
93 double evaluate(
const vpColVector &coeffs,
const unsigned int &height,
const unsigned int &width,
const std::vector<vpImagePoint> &pts)
95 unsigned int nbPts =
static_cast<unsigned int>(pts.size());
98 vpTutoParabolaModel model(coeffs, height, width);
100 for (
unsigned int i = 0; i < nbPts; ++i) {
101 double squareError = evaluate(pts[i], model);
102 residuals[i] = squareError;
104 double meanSquareError = residuals.sum() /
static_cast<double>(nbPts);
105 return std::sqrt(meanSquareError);
120 const unsigned int &vertPosLegend,
const unsigned int &horPosLegend)
122 #if defined(VISP_HAVE_DISPLAY)
125 for (
unsigned int u = 0; u < width; ++u) {
126 unsigned int v =
static_cast<unsigned int>(model.eval(u));
148 std::vector<vpImagePoint> automaticInitialization(tutorial::vpTutoCommonData &data)
151 const unsigned int minNbPts = data.m_degree + 1;
152 const unsigned int nbPtsToUse = 10 * minNbPts;
153 std::vector<vpImagePoint> initPoints;
156 tutorial::performSegmentationHSV(data);
159 std::vector<vpImagePoint> edgePoints = tutorial::extractSkeleton(data);
160 unsigned int nbEdgePoints =
static_cast<unsigned int>(edgePoints.size());
162 if (nbEdgePoints < nbPtsToUse) {
168 return ptA.
get_u() < ptB.get_u();
170 std::sort(edgePoints.begin(), edgePoints.end(), ptHasLowerU);
172 unsigned int idStart, idStop;
173 if (nbEdgePoints > nbPtsToUse + 20) {
176 idStop =
static_cast<unsigned int>(edgePoints.size()) - 10;
181 idStop =
static_cast<unsigned int>(edgePoints.size());
185 unsigned int sizeWindow = idStop - idStart + 1;
186 unsigned int step = sizeWindow / (nbPtsToUse - 1);
187 for (
unsigned int id = idStart;
id <= idStop;
id += step) {
188 initPoints.push_back(edgePoints[
id]);
199 std::vector<vpImagePoint> manualInitialization(
const tutorial::vpTutoCommonData &data)
202 const bool waitForClick =
true;
207 const unsigned int sizeCross = 10;
208 const unsigned int thicknessCross = 2;
212 const unsigned int minNbPts = data.m_degree + 1;
213 std::vector<vpImagePoint> initPoints;
215 bool notEnoughPoints =
true;
216 while (notEnoughPoints) {
221 vpDisplay::displayText(data.m_I_orig, data.m_ipLegend,
"Left click to add init point (min.: " + std::to_string(minNbPts) +
"), right click to estimate the initial coefficients of the Particle Filter.", data.m_colorLegend);
222 vpDisplay::displayText(data.m_I_orig, data.m_ipLegend + data.m_legendOffset,
"A middle click reinitialize the list of init points.", data.m_colorLegend);
223 vpDisplay::displayText(data.m_I_orig, data.m_ipLegend + data.m_legendOffset + data.m_legendOffset,
"If not enough points have been selected, a right click has no effect.", data.m_colorLegend);
226 unsigned int nbInitPoints =
static_cast<unsigned int>(initPoints.size());
227 for (
unsigned int i = 0; i < nbInitPoints; ++i) {
239 case vpMouseButton::vpMouseButtonType::button1:
240 initPoints.push_back(ipClick);
242 case vpMouseButton::vpMouseButtonType::button2:
245 case vpMouseButton::vpMouseButtonType::button3:
246 (initPoints.size() >= minNbPts ? notEnoughPoints = false : notEnoughPoints =
true);
264 vpColVector computeInitialGuess(tutorial::vpTutoCommonData &data)
267 std::vector<vpImagePoint> initPoints;
269 #ifdef VISP_HAVE_DISPLAY
271 const bool waitForClick =
true;
276 const unsigned int sizeCross = 10;
277 const unsigned int thicknessCross = 2;
280 bool automaticInit =
false;
284 vpDisplay::displayText(data.m_I_orig, data.m_ipLegend,
"Left click to manually select the init points, right click to automatically initialize the PF", data.m_colorLegend);
294 case vpMouseButton::vpMouseButtonType::button1:
295 automaticInit =
false;
297 case vpMouseButton::vpMouseButtonType::button3:
298 automaticInit =
true;
306 initPoints = tutorial::automaticInitialization(data);
310 initPoints = tutorial::manualInitialization(data);
315 initPoints = tutorial::automaticInitialization(data);
319 tutorial::vpTutoMeanSquareFitting lmsFitter(data.m_degree, data.m_I_orig.getHeight(), data.m_I_orig.getWidth());
320 lmsFitter.fit(initPoints);
322 std::cout <<
"---[Initial fit]---" << std::endl;
323 std::cout << lmsFitter.getModel();
324 std::cout <<
"---[Initial fit]---" << std::endl;
328 vpDisplay::displayText(data.m_I_orig, data.m_ipLegend,
"Here are the points selected for the initialization.", data.m_colorLegend);
329 size_t nbInitPoints = initPoints.size();
330 for (
size_t i = 0; i < nbInitPoints; ++i) {
336 lmsFitter.display(data.m_I_orig,
vpColor::red,
static_cast<unsigned int>(data.m_ipLegend.get_v() + 2 * data.m_legendOffset.get_v()),
static_cast<unsigned int>(data.m_ipLegend.get_u()));
337 vpDisplay::displayText(data.m_I_orig, data.m_ipLegend + data.m_legendOffset,
"A click to continue.", data.m_colorLegend);
349 return updatedCoeffs;
354 class vpTutoAverageFunctor
357 vpTutoAverageFunctor(
const unsigned int °ree,
const unsigned int &height,
const unsigned int &width)
372 vpColVector averagePolynomials(
const std::vector<vpColVector> &particles,
const std::vector<double> &weights,
const vpParticleFilter<std::vector<vpImagePoint>>::vpStateAddFunction &)
374 const unsigned int nbParticles =
static_cast<unsigned int>(particles.size());
375 const double nbParticlesAsDOuble =
static_cast<double>(nbParticles);
377 const double sumWeight = std::accumulate(weights.begin(), weights.end(), 0.);
380 const double nbPointsForAverage = 10. * nbParticlesAsDOuble;
381 std::vector<vpImagePoint> initPoints;
384 for (
unsigned int i = 0; i < nbParticles; ++i) {
386 double nbPoints = std::floor(weights[i] * nbPointsForAverage / sumWeight);
389 vpTutoParabolaModel curve(particles[i], m_height, m_width);
390 double widthAsDouble =
static_cast<double>(m_width);
392 double step = widthAsDouble / (nbPoints - 1.);
393 for (
double u = 0.; u < widthAsDouble; u += step) {
394 double v = curve.eval(u);
396 initPoints.push_back(pt);
402 vpTutoParabolaModel curve(particles[i], m_height, m_width);
403 double u =
static_cast<double>(m_width) / 2.;
404 double v = curve.eval(u);
406 initPoints.push_back(pt);
410 vpTutoMeanSquareFitting lms(m_degree, m_height, m_width);
412 return lms.getCoeffs();
416 unsigned int m_degree;
417 unsigned int m_height;
418 unsigned int m_width;
423 class vpTutoLikelihoodFunctor
433 vpTutoLikelihoodFunctor(
const double &stdev,
const unsigned int &height,
const unsigned int &width)
437 double sigmaDistanceSquared = stdev * stdev;
438 m_constantDenominator = 1. / std::sqrt(2. * M_PI * sigmaDistanceSquared);
439 m_constantExpDenominator = -1. / (2. * sigmaDistanceSquared);
455 double likelihood(
const vpColVector &coeffs,
const std::vector<vpImagePoint> &meas)
457 double likelihood = 0.;
458 unsigned int nbPoints =
static_cast<unsigned int>(meas.size());
461 vpTutoParabolaModel model(coeffs, m_height, m_width);
465 for (
unsigned int i = 0; i < nbPoints; ++i) {
466 double squareError = tutorial::evaluate(meas[i], model);
467 residuals[i] = squareError;
474 double sumError = w.hadamard(residuals).sum();
477 likelihood = std::exp(m_constantExpDenominator * sumError / w.sum()) * m_constantDenominator;
478 likelihood = std::min(likelihood, 1.0);
479 likelihood = std::max(likelihood, 0.);
484 double m_constantDenominator;
485 double m_constantExpDenominator;
486 unsigned int m_height;
487 unsigned int m_width;
493 int main(
const int argc,
const char *argv[])
495 tutorial::vpTutoCommonData data;
496 int returnCode = data.init(argc, argv);
497 if (returnCode != tutorial::vpTutoCommonData::SOFTWARE_CONTINUE) {
500 tutorial::vpTutoMeanSquareFitting lmsFitter(data.m_degree, data.m_I_orig.getHeight(), data.m_I_orig.getWidth());
501 const unsigned int vertOffset =
static_cast<unsigned int>(data.m_legendOffset.get_i());
502 const unsigned int horOffset =
static_cast<unsigned int>(data.m_ipLegend.get_j());
503 const unsigned int legendLmsVert = data.m_I_orig.getHeight() - 3 * vertOffset;
504 const unsigned int legendLmsHor = horOffset;
505 const unsigned int legendPFVert = data.m_I_orig.getHeight() - 2 * vertOffset, legendPFHor = horOffset;
509 vpColVector X0 = tutorial::computeInitialGuess(data);
513 const double maxDistanceForLikelihood = data.m_pfMaxDistanceForLikelihood;
514 const double sigmaLikelihood = maxDistanceForLikelihood / 3.;
515 const unsigned int nbParticles = data.m_pfN;
516 std::vector<double> stdevsPF;
517 for (
unsigned int i = 0; i < data.m_degree + 1; ++i) {
518 double ampliMax = data.m_pfRatiosAmpliMax[i] * X0[i];
519 stdevsPF.push_back(ampliMax / 3.);
521 unsigned long seedPF;
522 const float period = 33.3f;
523 if (data.m_pfSeed < 0) {
527 seedPF = data.m_pfSeed;
529 const int nbThread = data.m_pfNbThreads;
534 tutorial::vpTutoLikelihoodFunctor likelihoodFtor(sigmaLikelihood, data.m_I_orig.getHeight(), data.m_I_orig.getWidth());
535 using std::placeholders::_1;
536 using std::placeholders::_2;
540 tutorial::vpTutoAverageFunctor averageCpter(data.m_degree, data.m_I_orig.getHeight(), data.m_I_orig.getWidth());
541 using std::placeholders::_3;
548 filter.init(X0, processFunc, likelihoodFunc, checkResamplingFunc, resamplingFunc, meanFunc);
552 #ifdef VISP_HAVE_DISPLAY
553 unsigned int plotHeight = 350, plotWidth = 350;
554 int plotXpos =
static_cast<int>(data.m_legendOffset.get_u());
555 int plotYpos =
static_cast<int>(data.m_I_orig.getHeight() + 4. * data.m_legendOffset.get_v());
556 vpPlot plot(1, plotHeight, plotWidth, plotXpos, plotYpos,
"Root mean-square error");
557 plot.initGraph(0, 2);
558 plot.setLegend(0, 0,
"LMS estimator");
560 plot.setLegend(0, 1,
"PF estimator");
566 unsigned int nbIter = 0;
567 double meanDtLMS = 0., meanDtPF = 0.;
568 double meanRootMeanSquareErrorLMS = 0., meanRootMeanSquareErrorPF = 0.;
569 while (!data.m_grabber.end() && run) {
570 std::cout <<
"Iter " << nbIter << std::endl;
571 data.m_grabber.acquire(data.m_I_orig);
573 tutorial::performSegmentationHSV(data);
576 std::vector<vpImagePoint> edgePoints = tutorial::extractSkeleton(data);
579 std::vector<vpImagePoint> noisyEdgePoints = tutorial::addSaltAndPepperNoise(edgePoints, data);
581 #ifdef VISP_HAVE_DISPLAY
590 lmsFitter.fit(noisyEdgePoints);
592 double lmsRootMeanSquareError = lmsFitter.evaluate(edgePoints);
593 std::cout <<
" [Least-Mean Square method] " << std::endl;
594 std::cout <<
" Coeffs = [" << lmsFitter.getCoeffs().transpose() <<
" ]" << std::endl;
595 std::cout <<
" Root Mean Square Error = " << lmsRootMeanSquareError <<
" pixels" << std::endl;
596 std::cout <<
" Fitting duration = " << dtLms <<
" ms" << std::endl;
598 meanRootMeanSquareErrorLMS += lmsRootMeanSquareError;
603 filter.filter(noisyEdgePoints, period);
612 double pfError = tutorial::evaluate(Xest, data.m_I_orig.getHeight(), data.m_I_orig.getWidth(), edgePoints);
614 std::cout <<
" [Particle Filter method] " << std::endl;
615 std::cout <<
" Coeffs = [" << Xest.
transpose() <<
" ]" << std::endl;
616 std::cout <<
" Root Mean Square Error = " << pfError <<
" pixels" << std::endl;
617 std::cout <<
" Fitting duration = " << dtPF <<
" ms" << std::endl;
619 meanRootMeanSquareErrorPF += pfError;
621 #ifdef VISP_HAVE_DISPLAY
623 lmsFitter.display<
unsigned char>(data.m_IskeletonNoisy,
vpColor::gray, legendLmsVert, legendLmsHor);
624 tutorial::display(Xest, data.m_IskeletonNoisy,
vpColor::red, legendPFVert, legendPFHor);
627 plot.plot(0, 0, nbIter, lmsRootMeanSquareError);
628 plot.plot(0, 1, nbIter, pfError);
630 data.displayLegend(data.m_I_orig);
634 run = data.manageClicks(data.m_I_orig, data.m_stepbystep);
639 double iterAsDouble =
static_cast<double>(nbIter);
641 std::cout << std::endl << std::endl <<
"-----[Statistics summary]-----" << std::endl;
643 std::cout <<
" [LMS method] " << std::endl;
644 std::cout <<
" Average Root Mean Square Error = " << meanRootMeanSquareErrorLMS / iterAsDouble <<
" pixels" << std::endl;
645 std::cout <<
" Average fitting duration = " << meanDtLMS / iterAsDouble <<
" ms" << std::endl;
647 std::cout <<
" [Particle Filter method] " << std::endl;
648 std::cout <<
" Average Root Mean Square Error = " << meanRootMeanSquareErrorPF / iterAsDouble <<
" pixels" << std::endl;
649 std::cout <<
" Average fitting duration = " << meanDtPF / iterAsDouble <<
" ms" << std::endl;
651 #ifdef VISP_HAVE_DISPLAY
652 if (data.m_grabber.end() && (!data.m_stepbystep)) {
655 vpDisplay::displayText(data.m_I_orig, data.m_ipLegend,
"End of sequence reached. Click to exit.", data.m_colorLegend);
669 std::cerr <<
"ViSP must be compiled with C++ standard >= C++11 to use this tutorial." << std::endl;
670 std::cerr <<
"ViSP must also have a 3rd party enabling display features, such as X11 or OpenCV." << std::endl;
Implementation of column vector and the associated operations.
vpRowVector transpose() const
Class to define RGB colors available for display functionalities.
static const vpColor gray
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
static void display(const vpImage< unsigned char > &I)
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
static void flush(const vpImage< unsigned char > &I)
static void displayPoint(const vpImage< unsigned char > &I, const vpImagePoint &ip, const vpColor &color, unsigned int thickness=1)
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition of the vpImage class member functions.
unsigned int getWidth() const
unsigned int getHeight() const
static bool equal(double x, double y, double threshold=0.001)
The class permits to use a Particle Filter.
This class enables real time drawing of 2D or 3D graphics. An instance of the class open a window whi...
Contains an M-estimator and various influence function.
@ TUKEY
Tukey influence function.
void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights)
VISP_EXPORT double measureTimeMicros()
VISP_EXPORT double measureTimeMs()