Visual Servoing Platform  version 3.6.1 under development (2024-10-15)
tutorial-pf-curve-fitting-all.cpp
/*
* ViSP, open source Visual Servoing Platform software.
* Copyright (C) 2005 - 2024 by Inria. All rights reserved.
*
* This software is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
* For using ViSP with software that can not be combined with the GNU
* GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
* See https://visp.inria.fr for more information.
*
* This software was developed at:
* Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
*
* If you have questions regarding the use of this file, please contact
* Inria at visp@inria.fr
*
* This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
* WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
*/
// System includes
#include <algorithm>
#include <vector>
// ViSP includes
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpException.h>
#include <visp3/core/vpMouseButton.h>
#include <visp3/core/vpTime.h>
#ifdef VISP_HAVE_DISPLAY
#include <visp3/gui/vpPlot.h>
#endif
#include <visp3/core/vpParticleFilter.h>
#include "vpTutoCommonData.h"
#include "vpTutoMeanSquareFitting.h"
#include "vpTutoParabolaModel.h"
#include "vpTutoSegmentation.h"
#ifdef ENABLE_VISP_NAMESPACE
using namespace VISP_NAMESPACE_NAME;
#endif
#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) && defined(VISP_HAVE_DISPLAY)
#ifndef DOXYGEN_SHOULD_SKIP_THIS
namespace tutorial
{
double evaluate(const vpImagePoint &pt, const vpTutoParabolaModel &model)
{
double u = pt.get_u();
double v = pt.get_v();
double v_model = model.eval(u);
double error = v - v_model;
double squareError = error * error;
return squareError;
}
double evaluate(const vpColVector &coeffs, const unsigned int &height, const unsigned int &width, const std::vector<vpImagePoint> &pts)
{
unsigned int nbPts = static_cast<unsigned int>(pts.size());
vpColVector residuals(nbPts);
vpColVector weights(nbPts, 1.);
vpTutoParabolaModel model(coeffs, height, width);
// Compute the residuals
for (unsigned int i = 0; i < nbPts; ++i) {
double squareError = evaluate(pts[i], model);
residuals[i] = squareError;
}
double meanSquareError = residuals.sum() / static_cast<double>(nbPts);
return std::sqrt(meanSquareError);
}
template<typename T>
void display(const vpColVector &coeffs, const vpImage<T> &I, const vpColor &color,
const unsigned int &vertPosLegend, const unsigned int &horPosLegend)
{
#if defined(VISP_HAVE_DISPLAY)
unsigned int width = I.getWidth();
vpTutoParabolaModel model(coeffs, I.getHeight(), I.getWidth());
for (unsigned int u = 0; u < width; ++u) {
unsigned int v = static_cast<unsigned int>(model.eval(u));
vpDisplay::displayPoint(I, v, u, color, 1);
vpDisplay::displayText(I, vertPosLegend, horPosLegend, "Particle Filter model", color);
}
#else
(void)coeffs;
(void)I;
(void)color;
(void)vertPosLegend;
(void)horPosLegend;
#endif
}
std::vector<vpImagePoint> automaticInitialization(tutorial::vpTutoCommonData &data)
{
// Initialization-related variables
const unsigned int minNbPts = data.m_degree + 1;
const unsigned int nbPtsToUse = 10 * minNbPts;
std::vector<vpImagePoint> initPoints;
// Perform HSV segmentation
tutorial::performSegmentationHSV(data);
// Extracting the skeleton of the mask
std::vector<vpImagePoint> edgePoints = tutorial::extractSkeleton(data);
unsigned int nbEdgePoints = static_cast<unsigned int>(edgePoints.size());
if (nbEdgePoints < nbPtsToUse) {
return edgePoints;
}
// Uniformly extract init points
auto ptHasLowerU = [](const vpImagePoint &ptA, const vpImagePoint &ptB) {
return ptA.get_u() < ptB.get_u();
};
std::sort(edgePoints.begin(), edgePoints.end(), ptHasLowerU);
unsigned int idStart, idStop;
if (nbEdgePoints > nbPtsToUse + 20) {
// Avoid extreme points in case it's noise
idStart = 10;
idStop = static_cast<unsigned int>(edgePoints.size()) - 10;
}
else {
// We need to take all the points because we don't have enough
idStart = 0;
idStop = static_cast<unsigned int>(edgePoints.size());
}
// Sample uniformly the points starting from the left of the image to the right
unsigned int sizeWindow = idStop - idStart + 1;
unsigned int step = sizeWindow / (nbPtsToUse - 1);
for (unsigned int id = idStart; id <= idStop; id += step) {
initPoints.push_back(edgePoints[id]);
}
return initPoints;
}
std::vector<vpImagePoint> manualInitialization(const tutorial::vpTutoCommonData &data)
{
// Interaction variables
const bool waitForClick = true;
vpImagePoint ipClick;
// Display variables
const unsigned int sizeCross = 10;
const unsigned int thicknessCross = 2;
const vpColor colorCross = vpColor::red;
// Initialization-related variables
const unsigned int minNbPts = data.m_degree + 1;
std::vector<vpImagePoint> initPoints;
bool notEnoughPoints = true;
while (notEnoughPoints) {
// Initial display of the images
vpDisplay::display(data.m_I_orig);
// Display the how-to
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);
vpDisplay::displayText(data.m_I_orig, data.m_ipLegend + data.m_legendOffset, "A middle click reinitialize the list of init points.", data.m_colorLegend);
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);
// Display the already selected points
unsigned int nbInitPoints = static_cast<unsigned int>(initPoints.size());
for (unsigned int i = 0; i < nbInitPoints; ++i) {
vpDisplay::displayCross(data.m_I_orig, initPoints[i], sizeCross, colorCross, thicknessCross);
}
// Update the display
vpDisplay::flush(data.m_I_orig);
// Get the user input
vpDisplay::getClick(data.m_I_orig, ipClick, button, waitForClick);
// Either add the clicked point to the list of initial points or stop the loop if enough points are available
switch (button) {
case vpMouseButton::vpMouseButtonType::button1:
initPoints.push_back(ipClick);
break;
case vpMouseButton::vpMouseButtonType::button2:
initPoints.clear();
break;
case vpMouseButton::vpMouseButtonType::button3:
(initPoints.size() >= minNbPts ? notEnoughPoints = false : notEnoughPoints = true);
break;
default:
break;
}
}
return initPoints;
}
vpColVector computeInitialGuess(tutorial::vpTutoCommonData &data)
{
// Vector that contains the init points
std::vector<vpImagePoint> initPoints;
#ifdef VISP_HAVE_DISPLAY
// Interaction variables
const bool waitForClick = true;
vpImagePoint ipClick;
// Display variables
const unsigned int sizeCross = 10;
const unsigned int thicknessCross = 2;
const vpColor colorCross = vpColor::red;
bool automaticInit = false;
// Initial display of the images
vpDisplay::display(data.m_I_orig);
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);
// Update the display
vpDisplay::flush(data.m_I_orig);
// Get the user input
vpDisplay::getClick(data.m_I_orig, ipClick, button, waitForClick);
// Either use the automatic initialization or the manual one depending on the user input
switch (button) {
case vpMouseButton::vpMouseButtonType::button1:
automaticInit = false;
break;
case vpMouseButton::vpMouseButtonType::button3:
automaticInit = true;
break;
default:
break;
}
if (automaticInit) {
// Get automatically the init points from the segmented image
initPoints = tutorial::automaticInitialization(data);
}
else {
// Get manually the init points from the original image
initPoints = tutorial::manualInitialization(data);
}
#else
// Get the init points from the segmented image
initPoints = tutorial::automaticInitialization(data);
#endif
// Compute the coefficients of the parabola using Least-Mean-Square minimization.
tutorial::vpTutoMeanSquareFitting lmsFitter(data.m_degree, data.m_I_orig.getHeight(), data.m_I_orig.getWidth());
lmsFitter.fit(initPoints);
vpColVector X0 = lmsFitter.getCoeffs();
std::cout << "---[Initial fit]---" << std::endl;
std::cout << lmsFitter.getModel();
std::cout << "---[Initial fit]---" << std::endl;
// Display info about the initialization
vpDisplay::display(data.m_I_orig);
vpDisplay::displayText(data.m_I_orig, data.m_ipLegend, "Here are the points selected for the initialization.", data.m_colorLegend);
size_t nbInitPoints = initPoints.size();
for (size_t i = 0; i < nbInitPoints; ++i) {
const vpImagePoint &ip = initPoints[i];
vpDisplay::displayCross(data.m_I_orig, ip, sizeCross, colorCross, thicknessCross);
}
// Update display and wait for click
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()));
vpDisplay::displayText(data.m_I_orig, data.m_ipLegend + data.m_legendOffset, "A click to continue.", data.m_colorLegend);
vpDisplay::flush(data.m_I_orig);
vpDisplay::getClick(data.m_I_orig, waitForClick);
return X0;
}
vpColVector fx(const vpColVector &coeffs, const double &/*dt*/)
{
vpColVector updatedCoeffs = coeffs; // We use a constant position model
return updatedCoeffs;
}
class vpTutoAverageFunctor
{
public:
vpTutoAverageFunctor(const unsigned int &degree, const unsigned int &height, const unsigned int &width)
: m_degree(degree)
, m_height(height)
, m_width(width)
{ }
vpColVector averagePolynomials(const std::vector<vpColVector> &particles, const std::vector<double> &weights, const vpParticleFilter<std::vector<vpImagePoint>>::vpStateAddFunction &)
{
const unsigned int nbParticles = static_cast<unsigned int>(particles.size());
const double nbParticlesAsDOuble = static_cast<double>(nbParticles);
// Compute the sum of the weights to be able to determine the "importance" of a particle with regard to the whole set
const double sumWeight = std::accumulate(weights.begin(), weights.end(), 0.);
// Defining the total number of control points we want to generate
const double nbPointsForAverage = 10. * nbParticlesAsDOuble;
std::vector<vpImagePoint> initPoints;
// Creating control points by each particle
for (unsigned int i = 0; i < nbParticles; ++i) {
// The number of control points a particle can generate is proportional to the ratio of its weight w.r.t. the sum of the weights
double nbPoints = std::floor(weights[i] * nbPointsForAverage / sumWeight);
if (nbPoints > 1.) {
// The particle has a weight high enough to deserve more than one points
vpTutoParabolaModel curve(particles[i], m_height, m_width);
double widthAsDouble = static_cast<double>(m_width);
// Uniform sampling of the control points along the polynomial model
double step = widthAsDouble / (nbPoints - 1.);
for (double u = 0.; u < widthAsDouble; u += step) {
double v = curve.eval(u);
vpImagePoint pt(v, u);
initPoints.push_back(pt);
}
}
else if (nbPoints == 1.) {
// The weight of the particle make it have only one control point
// We sample it at the middle of the image
vpTutoParabolaModel curve(particles[i], m_height, m_width);
double u = static_cast<double>(m_width) / 2.;
double v = curve.eval(u);
vpImagePoint pt(v, u);
initPoints.push_back(pt);
}
}
// We use Least-Mean Square minimization to compute the polynomial model that best fits all the control points
vpTutoMeanSquareFitting lms(m_degree, m_height, m_width);
lms.fit(initPoints);
return lms.getCoeffs();
}
private:
unsigned int m_degree;
unsigned int m_height;
unsigned int m_width;
};
class vpTutoLikelihoodFunctor
{
public:
vpTutoLikelihoodFunctor(const double &stdev, const unsigned int &height, const unsigned int &width)
: m_height(height)
, m_width(width)
{
double sigmaDistanceSquared = stdev * stdev;
m_constantDenominator = 1. / std::sqrt(2. * M_PI * sigmaDistanceSquared);
m_constantExpDenominator = -1. / (2. * sigmaDistanceSquared);
}
double likelihood(const vpColVector &coeffs, const std::vector<vpImagePoint> &meas)
{
double likelihood = 0.;
unsigned int nbPoints = static_cast<unsigned int>(meas.size());
// Generate a model from the coefficients stored in the particle state
vpTutoParabolaModel model(coeffs, m_height, m_width);
// Compute the residual between each measurement point and its equivalent in the model
vpColVector residuals(nbPoints);
for (unsigned int i = 0; i < nbPoints; ++i) {
double squareError = tutorial::evaluate(meas[i], model);
residuals[i] = squareError;
}
// Use Tukey M-estimator to be robust against outliers
vpRobust Mestimator;
vpColVector w(nbPoints, 1.);
Mestimator.MEstimator(vpRobust::TUKEY, residuals, w);
double sumError = w.hadamard(residuals).sum();
// Compute the likelihood as a Gaussian function
likelihood = std::exp(m_constantExpDenominator * sumError / w.sum()) * m_constantDenominator;
likelihood = std::min(likelihood, 1.0); // Clamp to have likelihood <= 1.
likelihood = std::max(likelihood, 0.); // Clamp to have likelihood >= 0.
return likelihood;
}
private:
double m_constantDenominator;
double m_constantExpDenominator;
unsigned int m_height;
unsigned int m_width;
};
}
#endif
int main(const int argc, const char *argv[])
{
tutorial::vpTutoCommonData data;
int returnCode = data.init(argc, argv);
if (returnCode != tutorial::vpTutoCommonData::SOFTWARE_CONTINUE) {
return returnCode;
}
tutorial::vpTutoMeanSquareFitting lmsFitter(data.m_degree, data.m_I_orig.getHeight(), data.m_I_orig.getWidth());
const unsigned int vertOffset = static_cast<unsigned int>(data.m_legendOffset.get_i());
const unsigned int horOffset = static_cast<unsigned int>(data.m_ipLegend.get_j());
const unsigned int legendLmsVert = data.m_I_orig.getHeight() - 3 * vertOffset;
const unsigned int legendLmsHor = horOffset;
const unsigned int legendPFVert = data.m_I_orig.getHeight() - 2 * vertOffset, legendPFHor = horOffset;
// Initialize the attributes of the PF
vpColVector X0 = tutorial::computeInitialGuess(data);
const double maxDistanceForLikelihood = data.m_pfMaxDistanceForLikelihood; // The maximum allowed distance between a particle and the measurement, leading to a likelihood equal to 0..
const double sigmaLikelihood = maxDistanceForLikelihood / 3.; // The standard deviation of likelihood function.
const unsigned int nbParticles = data.m_pfN; // Number of particles to use
std::vector<double> stdevsPF; // Standard deviation for each state component
for (unsigned int i = 0; i < data.m_degree + 1; ++i) {
double ampliMax = data.m_pfRatiosAmpliMax[i] * X0[i];
stdevsPF.push_back(ampliMax / 3.);
}
unsigned long seedPF; // Seed for the random generators of the PF
const float period = 33.3f; // 33.3ms i.e. 30Hz
if (data.m_pfSeed < 0) {
seedPF = static_cast<unsigned long>(vpTime::measureTimeMicros());
}
else {
seedPF = data.m_pfSeed;
}
const int nbThread = data.m_pfNbThreads;
vpParticleFilter<std::vector<vpImagePoint>>::vpProcessFunction processFunc = tutorial::fx;
tutorial::vpTutoLikelihoodFunctor likelihoodFtor(sigmaLikelihood, data.m_I_orig.getHeight(), data.m_I_orig.getWidth());
using std::placeholders::_1;
using std::placeholders::_2;
vpParticleFilter<std::vector<vpImagePoint>>::vpLikelihoodFunction likelihoodFunc = std::bind(&tutorial::vpTutoLikelihoodFunctor::likelihood, &likelihoodFtor, _1, _2);
vpParticleFilter<std::vector<vpImagePoint>>::vpResamplingConditionFunction checkResamplingFunc = vpParticleFilter<std::vector<vpImagePoint>>::simpleResamplingCheck;
vpParticleFilter<std::vector<vpImagePoint>>::vpResamplingFunction resamplingFunc = vpParticleFilter<std::vector<vpImagePoint>>::simpleImportanceResampling;
tutorial::vpTutoAverageFunctor averageCpter(data.m_degree, data.m_I_orig.getHeight(), data.m_I_orig.getWidth());
using std::placeholders::_3;
vpParticleFilter<std::vector<vpImagePoint>>::vpFilterFunction meanFunc = std::bind(&tutorial::vpTutoAverageFunctor::averagePolynomials, &averageCpter, _1, _2, _3);
// Initialize the PF
vpParticleFilter<std::vector<vpImagePoint>> filter(nbParticles, stdevsPF, seedPF, nbThread);
filter.init(X0, processFunc, likelihoodFunc, checkResamplingFunc, resamplingFunc, meanFunc);
#ifdef VISP_HAVE_DISPLAY
unsigned int plotHeight = 350, plotWidth = 350;
int plotXpos = static_cast<int>(data.m_legendOffset.get_u());
int plotYpos = static_cast<int>(data.m_I_orig.getHeight() + 4. * data.m_legendOffset.get_v());
vpPlot plot(1, plotHeight, plotWidth, plotXpos, plotYpos, "Root mean-square error");
plot.initGraph(0, 2);
plot.setLegend(0, 0, "LMS estimator");
plot.setColor(0, 0, vpColor::gray);
plot.setLegend(0, 1, "PF estimator");
plot.setColor(0, 1, vpColor::red);
#endif
bool run = true;
unsigned int nbIter = 0;
double meanDtLMS = 0., meanDtPF = 0.;
double meanRootMeanSquareErrorLMS = 0., meanRootMeanSquareErrorPF = 0.;
while (!data.m_grabber.end() && run) {
std::cout << "Iter " << nbIter << std::endl;
data.m_grabber.acquire(data.m_I_orig);
tutorial::performSegmentationHSV(data);
std::vector<vpImagePoint> edgePoints = tutorial::extractSkeleton(data);
std::vector<vpImagePoint> noisyEdgePoints = tutorial::addSaltAndPepperNoise(edgePoints, data);
#ifdef VISP_HAVE_DISPLAY
vpDisplay::display(data.m_I_orig);
vpDisplay::display(data.m_I_segmented);
vpDisplay::display(data.m_IskeletonNoisy);
#endif
double tLms = vpTime::measureTimeMs();
lmsFitter.fit(noisyEdgePoints);
double dtLms = vpTime::measureTimeMs() - tLms;
double lmsRootMeanSquareError = lmsFitter.evaluate(edgePoints);
std::cout << " [Least-Mean Square method] " << std::endl;
std::cout << " Coeffs = [" << lmsFitter.getCoeffs().transpose() << " ]" << std::endl;
std::cout << " Root Mean Square Error = " << lmsRootMeanSquareError << " pixels" << std::endl;
std::cout << " Fitting duration = " << dtLms << " ms" << std::endl;
meanDtLMS += dtLms;
meanRootMeanSquareErrorLMS += lmsRootMeanSquareError;
double tPF = vpTime::measureTimeMs();
filter.filter(noisyEdgePoints, period);
double dtPF = vpTime::measureTimeMs() - tPF;
vpColVector Xest = filter.computeFilteredState();
double pfError = tutorial::evaluate(Xest, data.m_I_orig.getHeight(), data.m_I_orig.getWidth(), edgePoints);
std::cout << " [Particle Filter method] " << std::endl;
std::cout << " Coeffs = [" << Xest.transpose() << " ]" << std::endl;
std::cout << " Root Mean Square Error = " << pfError << " pixels" << std::endl;
std::cout << " Fitting duration = " << dtPF << " ms" << std::endl;
meanDtPF += dtPF;
meanRootMeanSquareErrorPF += pfError;
#ifdef VISP_HAVE_DISPLAY
// Update image overlay
lmsFitter.display<unsigned char>(data.m_IskeletonNoisy, vpColor::gray, legendLmsVert, legendLmsHor);
tutorial::display(Xest, data.m_IskeletonNoisy, vpColor::red, legendPFVert, legendPFHor);
// Update plot
plot.plot(0, 0, nbIter, lmsRootMeanSquareError);
plot.plot(0, 1, nbIter, pfError);
// Display the images with overlayed info
data.displayLegend(data.m_I_orig);
vpDisplay::flush(data.m_I_orig);
vpDisplay::flush(data.m_I_segmented);
vpDisplay::flush(data.m_IskeletonNoisy);
run = data.manageClicks(data.m_I_orig, data.m_stepbystep);
#endif
++nbIter;
}
double iterAsDouble = static_cast<double>(nbIter);
std::cout << std::endl << std::endl << "-----[Statistics summary]-----" << std::endl;
std::cout << " [LMS method] " << std::endl;
std::cout << " Average Root Mean Square Error = " << meanRootMeanSquareErrorLMS / iterAsDouble << " pixels" << std::endl;
std::cout << " Average fitting duration = " << meanDtLMS / iterAsDouble << " ms" << std::endl;
std::cout << " [Particle Filter method] " << std::endl;
std::cout << " Average Root Mean Square Error = " << meanRootMeanSquareErrorPF / iterAsDouble << " pixels" << std::endl;
std::cout << " Average fitting duration = " << meanDtPF / iterAsDouble << " ms" << std::endl;
#ifdef VISP_HAVE_DISPLAY
if (data.m_grabber.end() && (!data.m_stepbystep)) {
vpDisplay::display(data.m_I_orig);
vpDisplay::displayText(data.m_I_orig, data.m_ipLegend, "End of sequence reached. Click to exit.", data.m_colorLegend);
vpDisplay::flush(data.m_I_orig);
vpDisplay::getClick(data.m_I_orig, true);
}
#endif
return 0;
}
#else
int main()
{
std::cerr << "ViSP must be compiled with C++ standard >= C++11 to use this tutorial." << std::endl;
std::cerr << "ViSP must also have a 3rd party enabling display features, such as X11 or OpenCV." << std::endl;
return EXIT_FAILURE;
}
#endif
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
vpColVector hadamard(const vpColVector &v) const
vpRowVector transpose() const
double sum() const
Class to define RGB colors available for display functionalities.
Definition: vpColor.h:157
static const vpColor red
Definition: vpColor.h:217
static const vpColor gray
Definition: vpColor.h:214
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: vpImagePoint.h:82
double get_u() const
Definition: vpImagePoint.h:136
double get_v() const
Definition: vpImagePoint.h:147
Definition of the vpImage class member functions.
Definition: vpImage.h:131
unsigned int getWidth() const
Definition: vpImage.h:242
unsigned int getHeight() const
Definition: vpImage.h:181
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...
Definition: vpPlot.h:112
Contains an M-estimator and various influence function.
Definition: vpRobust.h:84
@ TUKEY
Tukey influence function.
Definition: vpRobust.h:89
void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights)
Definition: vpRobust.cpp:130
VISP_EXPORT double measureTimeMicros()
VISP_EXPORT double measureTimeMs()