vpHistogram.cpp

/****************************************************************************
 *
 * ViSP, open source Visual Servoing Platform software.
 * Copyright (C) 2005 - 2023 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.
 *
 * Description:
 * Gray level histogram manipulation.
 *
*****************************************************************************/
 
#include <stdlib.h>
#include <visp3/core/vpDisplay.h>
#include <visp3/core/vpHistogram.h>
#include <visp3/core/vpImageConvert.h>
 
#if defined(VISP_HAVE_PTHREAD) || (defined(_WIN32) && !defined(WINRT_8_0))
#include <visp3/core/vpThread.h>
 
namespace
{
struct vpHistogram_Param_t
{
  unsigned int m_start_index;
  unsigned int m_end_index;
 
  unsigned int m_lut[256];
  unsigned int *m_histogram;
  const vpImage<unsigned char> *m_I;
 
  vpHistogram_Param_t() : m_start_index(0), m_end_index(0), m_lut(), m_histogram(nullptr), m_I(nullptr) { }
 
  vpHistogram_Param_t(unsigned int start_index, unsigned int end_index, const vpImage<unsigned char> *const I)
    : m_start_index(start_index), m_end_index(end_index), m_lut(), m_histogram(nullptr), m_I(I)
  { }
 
  ~vpHistogram_Param_t()
  {
    if (m_histogram != nullptr) {
      delete[] m_histogram;
    }
  }
};
 
vpThread::Return computeHistogramThread(vpThread::Args args)
{
  vpHistogram_Param_t *histogram_param = static_cast<vpHistogram_Param_t *>(args);
  unsigned int start_index = histogram_param->m_start_index;
  unsigned int end_index = histogram_param->m_end_index;
 
  const vpImage<unsigned char> *I = histogram_param->m_I;
 
  unsigned char *ptrStart = (unsigned char *)(I->bitmap) + start_index;
  unsigned char *ptrEnd = (unsigned char *)(I->bitmap) + end_index;
  unsigned char *ptrCurrent = ptrStart;
 
  if (end_index - start_index >= 8) {
    // Unroll loop version
    for (; ptrCurrent <= ptrEnd - 8;) {
      histogram_param->m_histogram[histogram_param->m_lut[*ptrCurrent]]++;
      ++ptrCurrent;
 
      histogram_param->m_histogram[histogram_param->m_lut[*ptrCurrent]]++;
      ++ptrCurrent;
 
      histogram_param->m_histogram[histogram_param->m_lut[*ptrCurrent]]++;
      ++ptrCurrent;
 
      histogram_param->m_histogram[histogram_param->m_lut[*ptrCurrent]]++;
      ++ptrCurrent;
 
      histogram_param->m_histogram[histogram_param->m_lut[*ptrCurrent]]++;
      ++ptrCurrent;
 
      histogram_param->m_histogram[histogram_param->m_lut[*ptrCurrent]]++;
      ++ptrCurrent;
 
      histogram_param->m_histogram[histogram_param->m_lut[*ptrCurrent]]++;
      ++ptrCurrent;
 
      histogram_param->m_histogram[histogram_param->m_lut[*ptrCurrent]]++;
      ++ptrCurrent;
    }
  }
 
  for (; ptrCurrent != ptrEnd; ++ptrCurrent) {
    histogram_param->m_histogram[histogram_param->m_lut[*ptrCurrent]]++;
  }
 
  return 0;
}
} // namespace
#endif
 
bool compare_vpHistogramPeak(vpHistogramPeak first, vpHistogramPeak second);
 
// comparison,
bool compare_vpHistogramPeak(vpHistogramPeak first, vpHistogramPeak second)
{
  if (first.getValue() > second.getValue())
    return true;
  else
    return false;
}
 
vpHistogram::vpHistogram() : histogram(nullptr), size(256) { init(); }
 
vpHistogram::vpHistogram(const vpHistogram &h) : histogram(nullptr), size(256)
{
  init(h.size);
  memcpy(histogram, h.histogram, size * sizeof(unsigned));
}
 
vpHistogram::vpHistogram(const vpImage<unsigned char> &I) : histogram(nullptr), size(256)
{
  init();
 
  calculate(I);
}
 
vpHistogram::~vpHistogram()
{
  if (histogram != nullptr) {
    //    vpTRACE("free: %p", &histogram);
    delete[] histogram;
    histogram = nullptr;
    size = 0;
  }
}
 
vpHistogram &vpHistogram::operator=(const vpHistogram &h)
{
  init(h.size);
  memcpy(histogram, h.histogram, size * sizeof(unsigned));
 
  return *this;
}
 
void vpHistogram::init(unsigned size_)
{
  if (histogram != nullptr) {
    delete[] histogram;
    histogram = nullptr;
  }
 
  this->size = size_;
  histogram = new unsigned[size];
 
  memset(histogram, 0, size * sizeof(unsigned));
 
  //  vpTRACE("alloc: %p", &histogram);
}
 
void vpHistogram::calculate(const vpImage<unsigned char> &I, unsigned int nbins, unsigned int nbThreads)
{
  if (size != nbins) {
    if (histogram != nullptr) {
      delete[] histogram;
      histogram = nullptr;
    }
 
    size = nbins > 256 ? 256 : (nbins > 0 ? nbins : 256);
    if (nbins > 256 || nbins == 0) {
      std::cerr << "nbins=" << nbins << " , nbins should be between ]0 ; 256] ; use by default nbins=256" << std::endl;
    }
    histogram = new unsigned int[size];
  }
 
  memset(histogram, 0, size * sizeof(unsigned int));
 
  bool use_single_thread;
#if !defined(VISP_HAVE_PTHREAD) && !defined(_WIN32)
  use_single_thread = true;
#else
  use_single_thread = (nbThreads == 0 || nbThreads == 1);
#endif
 
  if (!use_single_thread && I.getSize() <= nbThreads) {
    use_single_thread = true;
  }
 
  unsigned int lut[256];
  for (unsigned int i = 0; i < 256; i++) {
    lut[i] = (unsigned int)(i * size / 256.0);
  }
 
  if (use_single_thread) {
    // Single thread
 
    unsigned int size_ = I.getWidth() * I.getHeight();
    unsigned char *ptrStart = (unsigned char *)I.bitmap;
    unsigned char *ptrEnd = ptrStart + size_;
    unsigned char *ptrCurrent = ptrStart;
 
    while (ptrCurrent != ptrEnd) {
      histogram[lut[*ptrCurrent]]++;
      ++ptrCurrent;
    }
  }
  else {
#if defined(VISP_HAVE_PTHREAD) || (defined(_WIN32) && !defined(WINRT_8_0))
    // Multi-threads
 
    std::vector<vpThread *> threadpool;
    std::vector<vpHistogram_Param_t *> histogramParams;
 
    unsigned int image_size = I.getSize();
    unsigned int step = image_size / nbThreads;
    unsigned int last_step = image_size - step * (nbThreads - 1);
 
    for (unsigned int index = 0; index < nbThreads; index++) {
      unsigned int start_index = index * step;
      unsigned int end_index = (index + 1) * step;
 
      if (index == nbThreads - 1) {
        end_index = start_index + last_step;
      }
 
      vpHistogram_Param_t *histogram_param = new vpHistogram_Param_t(start_index, end_index, &I);
      histogram_param->m_histogram = new unsigned int[size];
      memset(histogram_param->m_histogram, 0, size * sizeof(unsigned int));
      memcpy(histogram_param->m_lut, lut, 256 * sizeof(unsigned int));
 
      histogramParams.push_back(histogram_param);
 
      // Start the threads
      vpThread *histogram_thread = new vpThread((vpThread::Fn)computeHistogramThread, (vpThread::Args)histogram_param);
      threadpool.push_back(histogram_thread);
    }
 
    for (size_t cpt = 0; cpt < threadpool.size(); cpt++) {
      // Wait until thread ends up
      threadpool[cpt]->join();
    }
 
    for (unsigned int cpt1 = 0; cpt1 < size; cpt1++) {
      unsigned int sum = 0;
 
      for (size_t cpt2 = 0; cpt2 < histogramParams.size(); cpt2++) {
        sum += histogramParams[cpt2]->m_histogram[cpt1];
      }
 
      histogram[cpt1] = sum;
    }
 
    // Delete
    for (size_t cpt = 0; cpt < threadpool.size(); cpt++) {
      delete threadpool[cpt];
    }
 
    for (size_t cpt = 0; cpt < histogramParams.size(); cpt++) {
      delete histogramParams[cpt];
    }
#endif
  }
}
 
void vpHistogram::equalize(const vpImage<unsigned char> &I, vpImage<unsigned char> &Iout)
{
  // Compute the histogram
  calculate(I);
 
  // Calculate the cumulative distribution function
  unsigned int cdf[256];
  unsigned int cdfMin = std::numeric_limits<unsigned int>::max(), cdfMax = 0;
  unsigned int minValue = std::numeric_limits<unsigned int>::max(), maxValue = 0;
  cdf[0] = histogram[0];
 
  if (cdf[0] < cdfMin && cdf[0] > 0) {
    cdfMin = cdf[0];
    minValue = 0;
  }
 
  for (unsigned int i = 1; i < 256; i++) {
    cdf[i] = cdf[i - 1] + histogram[i];
 
    if (cdf[i] < cdfMin && cdf[i] > 0) {
      cdfMin = cdf[i];
      minValue = i;
    }
 
    if (cdf[i] > cdfMax) {
      cdfMax = cdf[i];
      maxValue = i;
    }
  }
 
  unsigned int nbPixels = I.getWidth() * I.getHeight();
  if (nbPixels == cdfMin) {
    // Only one brightness value in the image
    return;
  }
 
  // Construct the look-up table
  unsigned char lut[256];
  for (unsigned int x = minValue; x <= maxValue; x++) {
    lut[x] = vpMath::round((cdf[x] - cdfMin) / (double)(nbPixels - cdfMin) * 255.0);
  }
 
  Iout = I;
  Iout.performLut(lut);
}
 
void vpHistogram::display(const vpImage<unsigned char> &I, const vpColor &color, unsigned int thickness,
                          unsigned int maxValue_)
{
  unsigned int width = I.getWidth(), height = I.getHeight();
  // Minimal width and height are 36 px
  if (width < 36 || height < 36) {
    std::cerr << "Image I must have at least width >= 36 && height >= 36 !" << std::endl;
    return;
  }
 
  unsigned int maxValue = maxValue_;
  if (maxValue == 0) {
    for (unsigned int i = 0; i < size; i++) {
      if (histogram[i] > maxValue) {
        maxValue = histogram[i];
      }
    }
  }
 
  if (maxValue == 0) {
    throw(vpException(vpException::divideByZeroError, "Cannot display histogram; max value=0"));
  }
  // Keep 12 free pixels at the top
  unsigned int max_height = height - 12;
  double ratio_height = max_height / (double)maxValue;
  double ratio_width = (width - 1) / (double)(size - 1.0);
 
  for (unsigned int i = 1; i < size; i++) {
    unsigned int value1 = histogram[i - 1];
    unsigned int value2 = histogram[i];
 
    vpImagePoint startPt((height - 1) - (value1 * ratio_height), (i - 1) * ratio_width);
    vpImagePoint endPt((height - 1) - (value2 * ratio_height), (i * ratio_width));
    vpDisplay::displayLine(I, startPt, endPt, color, thickness);
  }
}
 
void vpHistogram::smooth(unsigned int fsize)
{
  if (histogram == nullptr) {
    vpERROR_TRACE("Histogram array not initialised\n");
    throw(vpImageException(vpImageException::notInitializedError, "Histogram array not initialised"));
  }
 
  vpHistogram h;
  h = *this;
 
  int hsize = (int)fsize / 2; // half filter size
 
  for (unsigned i = 0; i < size; i++) {
    unsigned int sum = 0;
    unsigned int nb = 0;
    for (int j = -hsize; j <= hsize; j++) {
      // exploitation of the overflow to detect negative value...
      if (/*(i + j) >= 0 &&*/ (i + (unsigned int)j) < size) {
        sum += h.histogram[i + (unsigned int)j];
        nb++;
      }
    }
    histogram[i] = sum / nb;
  }
}
 
unsigned vpHistogram::getPeaks(std::list<vpHistogramPeak> &peaks)
{
  if (histogram == nullptr) {
    vpERROR_TRACE("Histogram array not initialised\n");
    throw(vpImageException(vpImageException::notInitializedError, "Histogram array not initialised"));
  }
 
  int prev_slope;    // Previous histogram inclination
  vpHistogramPeak p; // An histogram peak
  unsigned nbpeaks;  // Number of peaks in the histogram (ie local maxima)
 
  peaks.clear();
 
  // Parse the histogram to get the local maxima
  unsigned cpt = 0;
  unsigned sum_level = 0;
  nbpeaks = 0;
  prev_slope = 1;
 
  for (unsigned i = 0; i < size - 1; i++) {
    int next_slope = (int)histogram[i + 1] - (int)histogram[i]; // Next histogram inclination
 
    //     if ((prev_slope < 0) && (next_slope > 0) ) {
    //       sum_level += i;
    //       cpt ++;
    //       continue;
    //     }
 
    if ((prev_slope > 0) && (next_slope == 0)) {
      sum_level += i + 1;
      cpt++;
      continue;
    }
 
    // Peak detection
    if ((prev_slope > 0) && (next_slope < 0)) {
      sum_level += i;
      cpt++;
 
      unsigned int level = sum_level / cpt;
      p.set((unsigned char)level, histogram[level]);
      //      vpTRACE("add %d %d", p.getLevel(), p.getValue());
      peaks.push_back(p);
 
      nbpeaks++;
    }
 
    prev_slope = next_slope;
    sum_level = 0;
    cpt = 0;
  }
  if (prev_slope > 0) {
    p.set((unsigned char)size - 1u, histogram[size - 1]);
    //      vpTRACE("add %d %d", p.getLevel(), p.getValue());
    peaks.push_back(p);
    nbpeaks++;
  }
 
  return nbpeaks;
}
 
unsigned vpHistogram::getPeaks(unsigned char dist, vpHistogramPeak &peak1, vpHistogramPeak &peak2)
{
  std::list<vpHistogramPeak> peaks;
  unsigned nbpeaks; // Number of peaks in the histogram (ie local maxima)
 
  nbpeaks = getPeaks(peaks);
  sort(peaks);
 
  if (nbpeaks == 0) {
    peak1.set(0, 0);
    peak2.set(0, 0);
    return 0;
  }
 
  if (nbpeaks == 1) {
    peak1 = peaks.front();
    peak2.set(0, 0);
    return 1;
  }
 
  // Parse the peaks list to get the peak with a distance greater
  // than dist to the highest
  peak1 = peaks.front();
  for (std::list<vpHistogramPeak>::const_iterator it = peaks.begin(); it != peaks.end(); ++it) {
    vpHistogramPeak p = *it;
    if (abs(p.getLevel() - peak1.getLevel()) > dist) {
      // The second peak is found
      peak2 = p;
      return 2;
    }
  }
 
  // No second peak found
  peak2.set(0, 0);
  return 1;
}
 
bool vpHistogram::getPeaks(unsigned char dist, vpHistogramPeak &peakl, vpHistogramPeak &peakr, vpHistogramValey &valey)
{
  unsigned char *peak;         // Local maxima values
  int prev_slope;              // Previous histogram inclination
  unsigned index_highest_peak; // Index in peak[] array of the highest peak
  unsigned index_second_peak;  // Index in peak[] array of the second peak
 
  unsigned int prof;
  unsigned int maxprof;    // Nb pixels difference between 2 maxi peaks
  unsigned int nbmini;     // Minimum numbers
  unsigned int sumindmini; // Sum
  unsigned int mini;       // current minimum
  unsigned int nbpeaks;    // Number of peaks in the histogram (ie local maxima)
 
  // Init the valey
  valey.set(0, 0);
 
  // Allocation for the
  peak = new unsigned char[size];
 
  // Parse the histogram to get the local maxima
  nbpeaks = 0;
  prev_slope = 1;
  for (unsigned i = 0; i < size - 1; i++) {
    int next_slope = (int)histogram[i + 1] - (int)histogram[i]; // Next histogram inclination
    if (next_slope == 0)
      continue;
    // Peak detection
    if ((prev_slope > 0) && (next_slope < 0))
      peak[nbpeaks++] = (unsigned char)i;
 
    prev_slope = next_slope;
  }
  if (prev_slope > 0)
    peak[nbpeaks++] = (unsigned char)(size - 1);
 
  // Get the global maximum
  index_highest_peak = 0;
  for (unsigned int i = 0; i < nbpeaks; i++) {
    if (histogram[peak[i]] > histogram[peak[index_highest_peak]]) {
      index_highest_peak = i;
    }
  }
 
  maxprof = 0;
  index_second_peak = index_highest_peak;
 
  // Search second local maximum on the left of the global maximum
  for (unsigned i = 0; i < index_highest_peak; i++) {
    if (peak[index_highest_peak] - peak[i] > dist) {
      prof = 0;
      for (int j = peak[i]; j <= peak[index_highest_peak] - dist; j++)
        if ((histogram[peak[i]] - histogram[j]) > prof)
          prof = histogram[peak[i]] - histogram[j];
 
      if (prof > maxprof) {
        maxprof = prof;
        index_second_peak = i;
      }
    }
  }
 
  // Search second local maximum on the right of the global maximum
  for (unsigned i = index_highest_peak + 1; i < nbpeaks; i++) {
    if (peak[i] - peak[index_highest_peak] > dist) {
      prof = 0;
      for (int j = peak[index_highest_peak] + dist; j <= peak[i]; j++)
        if ((histogram[peak[i]] - histogram[j]) > prof)
          prof = histogram[peak[i]] - histogram[j];
 
      if (prof > maxprof) {
        maxprof = prof;
        index_second_peak = i;
      }
    }
  }
 
  // Determine position of the first and second highest peaks
  if (peak[index_highest_peak] < peak[index_second_peak]) {
    peakr.set(peak[index_second_peak], histogram[peak[index_second_peak]]);
    peakl.set(peak[index_highest_peak], histogram[peak[index_highest_peak]]);
  }
  else {
    peakl.set(peak[index_second_peak], histogram[peak[index_second_peak]]);
    peakr.set(peak[index_highest_peak], histogram[peak[index_highest_peak]]);
  }
 
  if (peakl == peakr) {
 
    delete[] peak;
 
    return (false); // Not a bimodal histogram
  }
 
  // Search the valey
  mini = peakl.getValue();
  sumindmini = 0;
  nbmini = 0;
  for (unsigned i = peakl.getLevel(); i <= peakr.getLevel(); i++) {
    if (histogram[i] < mini) {
      mini = histogram[i];
      nbmini = 1;
      sumindmini = i;
      continue;
    }
    if (histogram[i] == mini) {
      sumindmini += i;
      nbmini++;
    }
  }
  // vpTRACE("nbmini %d", nbmini);
  // vpTRACE("sumindmini %d", sumindmini);
  // vpTRACE("mini: indmini: %d", sumindmini/nbmini);
 
  if (nbmini == 0) {
    // no valey found
    valey.set(0, 0);
 
    delete[] peak;
 
    return false;
  }
  else {
    mini = sumindmini / nbmini; // mean
    valey.set((unsigned char)mini, histogram[mini]);
 
    delete[] peak;
 
    return (true);
  }
}
 
unsigned vpHistogram::getValey(std::list<vpHistogramValey> &valey)
{
  if (histogram == nullptr) {
    vpERROR_TRACE("Histogram array not initialised\n");
    throw(vpImageException(vpImageException::notInitializedError, "Histogram array not initialised"));
  }
 
  int prev_slope;     // Previous histogram inclination
  vpHistogramValey p; // An histogram valey
  unsigned nbvaley;   // Number of valey in the histogram (ie local minima)
 
  valey.clear();
 
  // Parse the histogram to get the local minima
  unsigned cpt = 0;
  unsigned sum_level = 0;
  nbvaley = 0;
  prev_slope = -1;
 
  for (unsigned i = 0; i < size - 1; i++) {
    int next_slope = (int)histogram[i + 1] - (int)histogram[i]; // Next histogram inclination
 
    if ((prev_slope < 0) && (next_slope == 0)) {
      sum_level += i + 1;
      cpt++;
      continue;
    }
 
    // Valey detection
    if ((prev_slope < 0) && (next_slope > 0)) {
      sum_level += i;
      cpt++;
 
      unsigned int level = sum_level / cpt;
      p.set((unsigned char)level, histogram[level]);
      //      vpTRACE("add %d %d", p.getLevel(), p.getValue());
      valey.push_back(p);
 
      nbvaley++;
    }
 
    prev_slope = next_slope;
    sum_level = 0;
    cpt = 0;
  }
  if (prev_slope < 0) {
    p.set((unsigned char)size - 1u, histogram[size - 1]);
    //      vpTRACE("add %d %d", p.getLevel(), p.getValue());
    valey.push_back(p);
    nbvaley++;
  }
 
  return nbvaley;
}
 
bool vpHistogram::getValey(const vpHistogramPeak &peak1, const vpHistogramPeak &peak2, vpHistogramValey &valey)
{
 
  // Set the left and right peaks
  vpHistogramPeak peakl, peakr;
  if (peak1.getLevel() > peak2.getLevel()) {
    peakl = peak2;
    peakr = peak1;
  }
  else {
    peakl = peak1;
    peakr = peak2;
  }
 
  // Search the valey
  unsigned int nbmini;     // Minimum numbers
  unsigned int sumindmini; // Sum
  unsigned int mini;       // current minimum
 
  mini = peakl.getValue();
  sumindmini = 0;
  nbmini = 0;
  for (unsigned i = peakl.getLevel(); i <= peakr.getLevel(); i++) {
    if (histogram[i] < mini) {
      mini = histogram[i];
      nbmini = 1;
      sumindmini = i;
      continue;
    }
    if (histogram[i] == mini) {
      sumindmini += i;
      nbmini++;
    }
  }
 
  if (nbmini == 0) {
    // no valey found
    valey.set(0, 0);
 
    return false;
  }
  else {
    unsigned int minipos = sumindmini / nbmini; // position of the minimum
 
    valey.set((unsigned char)minipos, histogram[minipos]);
    return true;
  }
}
unsigned vpHistogram::getValey(unsigned char dist, const vpHistogramPeak &peak, vpHistogramValey &valeyl,
                               vpHistogramValey &valeyr)
{
  unsigned int ret = 0x11;
  unsigned int nbmini;              // Minimum numbers
  unsigned int sumindmini;          // Sum
  unsigned int mini;                // current minimum
  vpHistogramPeak peakr, peakl;     // Left and right peaks of peak
  std::list<vpHistogramPeak> peaks; // list of histogram peaks
  //   unsigned int nbpeaks=0; // Number of peaks in the histogram (ie local
  //   maxima)
 
  if (peak.getLevel() == 0) {
    valeyl.set(0, 0);
    ret &= 0x01;
  }
  if (peak.getLevel() == size - 1) {
    valeyr.set((unsigned char)(size - 1), 0);
    ret &= 0x10;
  }
 
  if (ret >> 1) // consider the left part of the requested peak
  {
    // If the list of peaks is empty, compute it
    if (peaks.empty()) {
      /* nbpeaks = */ getPeaks(peaks);
    }
 
    // Go to the requested peak in the list
    std::list<vpHistogramPeak>::const_iterator it;
    unsigned index = 0;
    for (it = peaks.begin(); it != peaks.end(); ++it) {
      if (peak == *it) {
        // we are on the peak.
        break;
      }
      index++;
    }
 
    bool found = false;
    if (index == 0) {
      // No chance to get a peak on the left
      // should not occur !
      peakl.set(0, 0);
    }
    else {
   // search for the nearest peak on the left that matches the distance
      std::list<vpHistogramPeak>::const_iterator lit; // left iterator
      for (lit = peaks.begin(); lit != it; ++lit) {
        if (abs((*lit).getLevel() - peak.getLevel()) > dist) {
          // peakl found
          peakl = *lit;
          found = true; // we cannot stop here, since the other peaks on the
                        // right may exist
        }
      }
    }
    if (!found)
      peakl.set(0, 0);
 
    // Search the valey on the left
    mini = peak.getValue();
    sumindmini = 0;
    nbmini = 0;
    for (unsigned i = peakl.getLevel(); i < peak.getLevel(); i++) {
      if (histogram[i] < mini) {
        mini = histogram[i];
        nbmini = 1;
        sumindmini = i;
        continue;
      }
      if (histogram[i] == mini) {
        sumindmini += i;
        nbmini++;
      }
    }
    if (nbmini == 0) {
      valeyl.set(0, 0);
      ret &= 0x01;
    }
    else {
      unsigned int minipos = sumindmini / nbmini; // position of the minimum
      valeyl.set((unsigned char)minipos, histogram[minipos]);
      ret &= 0x11;
    }
  }
 
  if (ret << 1) {
    // If the list of peaks is empty, compute it
    if (peaks.empty()) {
      /* nbpeaks = */ getPeaks(peaks);
    }
    // Go to the requested peak in the list
    std::list<vpHistogramPeak>::const_iterator it;
    for (it = peaks.begin(); it != peaks.end(); ++it) {
      if (peak == *it) {
        // we are on the peak.
        break;
      }
    }
 
    bool found = false;
    // search for the nearest peak on the right that matches the distance
    std::list<vpHistogramPeak>::const_iterator rit; // right iterator
    for (rit = it; rit != peaks.end(); ++rit)
 
      if (abs((*rit).getLevel() - peak.getLevel()) > dist) {
        // peakr found
        peakr = *rit;
        found = true;
        break; // we can stop here
      }
 
    if (!found)
      peakr.set((unsigned char)(size - 1), 0);
 
    // Search the valey on the right
    mini = peak.getValue();
    sumindmini = 0;
    nbmini = 0;
    for (unsigned i = (unsigned int)peak.getLevel() + 1; i <= (unsigned int)peakr.getLevel(); i++) {
      if (histogram[i] < mini) {
        mini = histogram[i];
        nbmini = 1;
        sumindmini = i;
        continue;
      }
      if (histogram[i] == mini) {
        sumindmini += i;
        nbmini++;
      }
    }
    if (nbmini == 0) {
      valeyr.set((unsigned char)(size - 1), 0);
      ret &= 0x10;
    }
    else {
      unsigned int minipos = sumindmini / nbmini; // position of the minimum
      valeyr.set((unsigned char)minipos, histogram[minipos]);
      ret &= 0x11;
    }
  }
 
  return ret;
}
 
unsigned vpHistogram::sort(std::list<vpHistogramPeak> &peaks)
{
 
  if (peaks.empty()) {
    return 0;
  }
 
  peaks.sort(compare_vpHistogramPeak);
 
  return (unsigned int)peaks.size();
}
 
bool vpHistogram::write(const std::string &filename) { return (this->write(filename.c_str())); }
 
bool vpHistogram::write(const char *filename)
{
  FILE *fd = fopen(filename, "w");
  if (fd == nullptr)
    return false;
  for (unsigned i = 0; i < size; i++)
    fprintf(fd, "%u %u\n", i, histogram[i]);
  fclose(fd);
 
  return true;
}