C++ circular_buffer::begin方法代码示例

size_t GestureClassifierByHistogram::matchHistogramByGestureIdPattern(const boost::circular_buffer<size_t> &matchedHistogramIndexes, const std::vector<histogram_type> &gestureIdPatternHistograms, const double histDistThreshold) const
{
	// create matched ID histogram
#if 0
	cv::MatND hist;
	cv::calcHist(
		&cv::Mat(std::vector<unsigned char>(matchedHistogramIndexes.begin(), matchedHistogramIndexes.end())),
		1, phaseHistChannels, cv::Mat(), hist, histDims, phaseHistSize, phaseHistRanges, true, false
	);
#else
	cv::MatND hist = cv::MatND::zeros(local::gesturePatternHistogramBinNum, 1, CV_32F);
	float *binPtr = (float *)hist.data;
	for (boost::circular_buffer<size_t>::const_iterator it = matchedHistogramIndexes.begin(); it != matchedHistogramIndexes.end(); ++it)
		if (*it != (size_t)-1) ++(binPtr[*it]);
#endif

	// match histogram
	double minHistDist = std::numeric_limits<double>::max();
	const size_t &matchedIdx = gestureIdPatternHistograms.empty() ? -1 : HistogramMatcher::match(gestureIdPatternHistograms, hist, minHistDist);

	// FIXME [delete] >>
	//std::cout << "\t\t\t*** " << minHistDist << std::endl;

	return minHistDist < histDistThreshold ? matchedIdx : -1;
}

void GestureClassifierByHistogram::drawMatchedIdPatternHistogram(const boost::circular_buffer<size_t> &matchedHistogramIndexes, const std::string &windowName) const
{
	// calculate matched index histogram
	cv::MatND hist;
#if defined(__GNUC__)
    {
        cv::Mat tmpmat(std::vector<unsigned char>(matchedHistogramIndexes.begin(), matchedHistogramIndexes.end()));
        cv::calcHist(&tmpmat, 1, local::indexHistChannels, cv::Mat(), hist, local::histDims, local::indexHistSize, local::indexHistRanges, true, false);
    }
#else
	cv::calcHist(&cv::Mat(std::vector<unsigned char>(matchedHistogramIndexes.begin(), matchedHistogramIndexes.end())), 1, local::indexHistChannels, cv::Mat(), hist, local::histDims, local::indexHistSize, local::indexHistRanges, true, false);
#endif

	// normalize histogram
	//HistogramUtil::normalizeHistogram(hist, params_.maxMatchedHistogramNum);

	// draw matched index histogram
	cv::Mat histImg(cv::Mat::zeros(local::indexHistMaxHeight, local::indexHistBins*local::indexHistBinWidth, CV_8UC3));
	HistogramUtil::drawHistogram1D(hist, local::indexHistBins, params_.maxMatchedHistogramNum, local::indexHistBinWidth, local::indexHistMaxHeight, histImg);

	std::ostringstream sstream;
	sstream << "count: " << matchedHistogramIndexes.size();
	cv::putText(histImg, sstream.str(), cv::Point(10, 15), cv::FONT_HERSHEY_COMPLEX, 0.5, CV_RGB(255, 0, 255), 1, 8, false);

	cv::imshow(windowName, histImg);
}

void image_obj_callback(const autoware_msgs::image_obj::ConstPtr& image_obj_msg) {
    pthread_mutex_lock(&mutex);
    image_obj_ringbuf.push_front(*image_obj_msg);

    //vscan_image is empty
    if (vscan_image_ringbuf.begin() == vscan_image_ringbuf.end()) {
        pthread_mutex_unlock(&mutex);
        ROS_INFO("vscan_image ring buffer is empty");
        return;
    }

    buf_flag = true;

    // image_obj > vscan_image
    if (get_time(&(image_obj_ringbuf.front().header)) >= get_time(&(vscan_image_ringbuf.front().header))) {
        vscan_image_buf = vscan_image_ringbuf.front();
        boost::circular_buffer<autoware_msgs::image_obj>::iterator it = image_obj_ringbuf.begin();
        if (image_obj_ringbuf.size() == 1) {
            image_obj_buf = *it;
            pthread_mutex_unlock(&mutex);
            return;
        } else {
            for (it++; it != image_obj_ringbuf.end(); it++) {
                if (fabs_time_diff(&(vscan_image_ringbuf.front().header), &((it-1)->header))
                    < fabs_time_diff(&(vscan_image_ringbuf.front().header), &(it->header))) {
                    image_obj_buf = *(it-1);
                    break;
                }
            }
            if (it == image_obj_ringbuf.end()) {
                image_obj_buf = image_obj_ringbuf.back();
            }
        }

    } else {
        image_obj_buf = image_obj_ringbuf.front();
        boost::circular_buffer<autoware_msgs::PointsImage>::iterator it = vscan_image_ringbuf.begin();
        if (vscan_image_ringbuf.size() == 1) {
            vscan_image_buf = *it;
            pthread_mutex_unlock(&mutex);
            return;
        }

        for (it++; it != vscan_image_ringbuf.end(); it++) {
            if (fabs_time_diff(&(image_obj_ringbuf.front().header), &((it-1)->header))
                < fabs_time_diff(&(image_obj_ringbuf.front().header), &(it->header))) {
                vscan_image_buf = *(it-1);
                break;
            }
        }

        if (it == vscan_image_ringbuf.end()) {
            vscan_image_buf = vscan_image_ringbuf.back();
        }
    }
    pthread_mutex_unlock(&mutex);
}

void image_obj_ranged_callback(const cv_tracker::image_obj_ranged::ConstPtr& image_obj_ranged_msg) {
    pthread_mutex_lock(&mutex);
    image_obj_ranged_ringbuf.push_front(*image_obj_ranged_msg);

    //image_raw is empty
    if (image_raw_ringbuf.begin() == image_raw_ringbuf.end()) {
        pthread_mutex_unlock(&mutex);
        ROS_INFO("image_raw ring buffer is empty");
        return;
    }

    buf_flag = true;

    // image_obj_ranged > image_raw
    if (get_time(&(image_obj_ranged_ringbuf.front().header)) >= get_time(&(image_raw_ringbuf.front().header))) {
        image_raw_buf = image_raw_ringbuf.front();
        boost::circular_buffer<cv_tracker::image_obj_ranged>::iterator it = image_obj_ranged_ringbuf.begin();
        if (image_obj_ranged_ringbuf.size() == 1) {
            image_obj_ranged_buf = *it;
            pthread_mutex_unlock(&mutex);
            return;
        } else {
            for (it++; it != image_obj_ranged_ringbuf.end(); it++) {
                if (fabs_time_diff(&(image_raw_ringbuf.front().header), &((it-1)->header))
                    < fabs_time_diff(&(image_raw_ringbuf.front().header), &(it->header))) {
                    image_obj_ranged_buf = *(it-1);
                    break;
                }
            }
            if (it == image_obj_ranged_ringbuf.end()) {
                image_obj_ranged_buf = image_obj_ranged_ringbuf.back();
            }
        }

    } else {
        image_obj_ranged_buf = image_obj_ranged_ringbuf.front();
        boost::circular_buffer<sensor_msgs::Image>::iterator it = image_raw_ringbuf.begin();
        if (image_raw_ringbuf.size() == 1) {
            image_raw_buf = *it;
            pthread_mutex_unlock(&mutex);
            return;
        }

        for (it++; it != image_raw_ringbuf.end(); it++) {
            if (fabs_time_diff(&(image_obj_ranged_ringbuf.front().header), &((it-1)->header))
                < fabs_time_diff(&(image_obj_ranged_ringbuf.front().header), &(it->header))) {
                image_raw_buf = *(it-1);
                break;
            }
        }

        if (it == image_raw_ringbuf.end()) {
            image_raw_buf = image_raw_ringbuf.back();
        }
    }
    pthread_mutex_unlock(&mutex);
}

  /** Try to write a value to the pipe

      \param[in] value is what we want to write

      \param[in] blocking specify if the call wait for the operation
      to succeed

      \return true on success

      \todo provide a && version
  */
  bool write(const T &value, bool blocking = false) {
    // Lock the pipe to avoid being disturbed
    std::unique_lock<std::mutex> ul { cb_mutex };
    TRISYCL_DUMP_T("Write pipe full = " << full()
                   << " value = " << value);

    if (blocking)
      /* If in blocking mode, wait for the not full condition, that
         may be changed when a read is done */
      read_done.wait(ul, [&] { return !full(); });
    else if (full())
      return false;

    cb.push_back(value);
    TRISYCL_DUMP_T("Write pipe front = " << cb.front()
                   << " back = " << cb.back()
                   << " cb.begin() = " << (void *)&*cb.begin()
                   << " cb.size() = " << cb.size()
                   << " cb.end() = " << (void *)&*cb.end()
                   << " reserved_for_reading() = " << reserved_for_reading()
                   << " reserved_for_writing() = " << reserved_for_writing());
    // Notify the clients waiting to read something from the pipe
    write_done.notify_all();
    return true;
  }

  /** Process the read reservations that are ready to be released in the
      reservation queue
  */
  void move_read_reservation_forward() {
    // Lock the pipe to avoid nuisance
    std::lock_guard<std::mutex> lg { cb_mutex };

    for (;;) {
      if (r_rid_q.empty())
        // No pending reservation, so nothing to do
        break;
      if (!r_rid_q.front().ready)
        /* If the first reservation is not ready to be released, stop
           because it is blocking all the following in the queue
           anyway */
        break;
      // Remove the reservation to be released from the queue
      r_rid_q.pop_front();
      std::size_t n_to_pop;
      if (r_rid_q.empty())
        // If it was the last one, remove all the reservation
        n_to_pop = read_reserved_frozen;
      else
        // Else remove everything up to the next reservation
        n_to_pop =  r_rid_q.front().start - cb.begin();
      // No longer take into account these reserved slots
      read_reserved_frozen -= n_to_pop;
      // Release the elements from the FIFO
      while (n_to_pop--)
        cb.pop_front();
      // Notify the clients waiting for some room to write in the pipe
      read_done.notify_all();
      /* ...and process the next reservation to see if it is ready to
         be released too */
    }
  }

//! Dump Legendre polynomial cache data to stream (table and history).
void dumpLegendrePolynomialCacheData( std::ostream& outputStream,
                                      boost::unordered_map< Point, double > cacheTable,
                                      boost::circular_buffer< Point > cacheHistory )
{
    outputStream << "Table:\n";

    for ( boost::unordered_map< Point, double >::iterator iteratorCacheTable = cacheTable.begin( );
          iteratorCacheTable != cacheTable.end( ); iteratorCacheTable++ )
    {
        outputStream << "\t" << writeLegendrePolynomialStructureToString(
                            iteratorCacheTable->first ).c_str( ) << " => "
                     << iteratorCacheTable->second << std::endl;
    }

    outputStream << "History:\n";

    for ( boost::circular_buffer< Point >::iterator iteratorCacheHistory = cacheHistory.begin( );
          iteratorCacheHistory != cacheHistory.end( ); iteratorCacheHistory++ )
    {
        outputStream << "\t"
                     << writeLegendrePolynomialStructureToString( *iteratorCacheHistory ).c_str( )
                     << ", ";
    }

    outputStream << std::endl;
}

  /** Try to read a value from the pipe

      \param[out] value is the reference to where to store what is
      read

      \param[in] blocking specify if the call wait for the operation
      to succeed

      \return true on success
  */
  bool read(T &value, bool blocking = false) {
    // Lock the pipe to avoid being disturbed
    std::unique_lock<std::mutex> ul { cb_mutex };
    TRISYCL_DUMP_T("Read pipe empty = " << empty());

    if (blocking)
      /* If in blocking mode, wait for the not empty condition, that
         may be changed when a write is done */
      write_done.wait(ul, [&] { return !empty(); });
    else if (empty())
      return false;

    TRISYCL_DUMP_T("Read pipe front = " << cb.front()
                   << " back = " << cb.back()
                   << " reserved_for_reading() = " << reserved_for_reading());
    if (read_reserved_frozen)
      /** If there is a pending reservation, read the next element to
          be read and update the number of reserved elements */
      value = cb.begin()[read_reserved_frozen++];
    else {
      /* There is no pending read reservation, so pop the read value
         from the pipe */
      value = cb.front();
      cb.pop_front();
    }

    TRISYCL_DUMP_T("Read pipe value = " << value);
    // Notify the clients waiting for some room to write in the pipe
    read_done.notify_all();
    return true;
  }

  /** Reserve some part of the pipe for reading

      \param[in] s is the number of element to reserve

      \param[out] rid is an iterator to a description of the
      reservation that has been done if successful

      \param[in] blocking specify if the call wait for the operation
      to succeed

      \return true if the reservation was successful
  */
  bool reserve_read(std::size_t s,
                    rid_iterator &rid,
                    bool blocking = false)  {
    // Lock the pipe to avoid being disturbed
    std::unique_lock<std::mutex> ul { cb_mutex };

    TRISYCL_DUMP_T("Before read reservation cb.size() = " << cb.size()
                   << " size() = " << size());
    if (s == 0)
      // Empty reservation requested, so nothing to do
      return false;

    if (blocking)
      /* If in blocking mode, wait for enough elements to read in the
         pipe for the reservation. This condition can change when a
         write is done */
      write_done.wait(ul, [&] { return s <= size(); });
    else if (s > size())
      // Not enough elements to read in the pipe for the reservation
      return false;

    // Compute the location of the first element of the reservation
    auto first = cb.begin() + read_reserved_frozen;
    // Increment the number of frozen elements
    read_reserved_frozen += s;
    /* Add a description of the reservation at the end of the
       reservation queue */
    r_rid_q.emplace_back(first, s);
    // Return the iterator to the last reservation descriptor
    rid = r_rid_q.end() - 1;
    TRISYCL_DUMP_T("After reservation cb.size() = " << cb.size()
                   << " size() = " << size());
    return true;
  }

size_t GestureClassifierByHistogram::matchHistogramByFrequency(const boost::circular_buffer<size_t> &matchedHistogramIndexes, const size_t countThreshold) const
{
	std::map<const size_t, size_t> freqs;
	for (boost::circular_buffer<size_t>::const_iterator it = matchedHistogramIndexes.begin(); it != matchedHistogramIndexes.end(); ++it)
	{
		if (freqs.find(*it) == freqs.end()) freqs[*it] = 1;
		else ++freqs[*it];
	}

	std::map<const size_t, size_t>::const_iterator itMaxFreq = std::max_element(freqs.begin(), freqs.end(), local::MaxFrequencyComparator());
	return (freqs.end() != itMaxFreq && itMaxFreq->second > countThreshold) ? itMaxFreq->first : -1;
}

    // Helper function: compute the median of a circular buffer
    double circ_buff_median(const boost::circular_buffer<double>& cb) const {
        // FIXME: naive implementation; creates a copy as a vector
        std::vector<double> v;
        for (boost::circular_buffer<double>::const_iterator i = cb.begin();
                i != cb.end(); ++i) {
            v.push_back(*i);
        }

        size_t n = v.size() / 2;
        std::nth_element(v.begin(), v.begin()+n, v.end());
        return v[n];
    }

 ros::Time find(ros::Time sensor_time) {
     boost::circular_buffer<ros::Time>::iterator it = sensor.begin();
     for (int i = 0; it != sensor.end(); it++, i++) {
         if (it->sec == sensor_time.sec && it->nsec == sensor_time.nsec) {
             return execution.at(i); // find
         }
     }
     ROS_ERROR("error:not found a pair");
     ros::Time failed;
     failed.sec = 0;
     failed.nsec = 0;
     return failed; // not find
 }

void CSearchDialog::SaveEntry(int comboBoxId, boost::circular_buffer<std::wstring> &buffer)
{
	TCHAR entry[MAX_PATH];
	GetDlgItemText(m_hDlg, comboBoxId, entry, SIZEOF_ARRAY(entry));

	std::wstring strEntry(entry);
	auto itr = std::find_if(buffer.begin(), buffer.end(),
		[strEntry] (const std::wstring Pattern)
	{
		return Pattern.compare(strEntry) == 0;
	});

	HWND hComboBox = GetDlgItem(m_hDlg, comboBoxId);
	ComboBox_SetCurSel(hComboBox, -1);

	if(itr != buffer.end())
	{
		/* Remove the current element from both the list and the
		combo box. It will be reinserted at the front of both below. */
		auto index = std::distance(buffer.begin(), itr);
		SendMessage(hComboBox, CB_DELETESTRING, index, 0);

		buffer.erase(itr);
	}

	buffer.push_front(entry);

	SendMessage(hComboBox, CB_INSERTSTRING, 0, reinterpret_cast<LPARAM>(entry));
	ComboBox_SetCurSel(hComboBox, 0);
	ComboBox_SetEditSel(hComboBox, -1, -1);

	if(ComboBox_GetCount(hComboBox) > buffer.capacity())
	{
		SendMessage(hComboBox, CB_DELETESTRING, ComboBox_GetCount(hComboBox) - 1, 0);
	}
}

void vscan_image_callback(const autoware_msgs::PointsImage::ConstPtr& vscan_image_msg) {
    pthread_mutex_lock(&mutex);
    vscan_image_ringbuf.push_front(*vscan_image_msg);
    //image_obj is empty
    if (image_obj_ringbuf.begin() == image_obj_ringbuf.end()) {
        ROS_INFO("image_obj ring buffer is empty");
        buf_flag = false;
        pthread_mutex_unlock(&mutex);
        return;
    }
    buf_flag = true;
    pthread_mutex_unlock(&mutex);
    if (image_obj_ranged_flag == true) {
        publish();
    }
}

void CTimeSmoother::BinData(const boost::circular_buffer<double> &data, vector<double> &bins, const double threshold, const unsigned int minbinsize)
{
  if (!data.size())
    return;

  bins.clear();
  vector<unsigned int> counts;

  for (boost::circular_buffer<double>::const_iterator i = data.begin(); i != data.end(); ++i)
  {
    bool found = false;
    for (unsigned int j = 0; j < bins.size(); ++j)
    {
      if (fabs(*i - bins[j]) < threshold*bins[j])
      {
        found = true;
        // update our bin mean and count
        bins[j] = (bins[j]*counts[j] + *i)/(counts[j]+1);
        counts[j]++;
        break;
      }
    }
    if (!found)
    {
      bins.push_back(*i);
      counts.push_back(1);
    }
  }
  if (minbinsize)
  {
    assert(counts.size() == bins.size());
    assert(counts.size());
    // filter out any bins that are not large enough (and any bins that aren't positive)
    for (unsigned int j = 0; j < counts.size(); )
    {
      if (counts[j] < minbinsize || bins[j] < 0.05)
      {
        bins.erase(bins.begin() + j);
        counts.erase(counts.begin() + j);
      }
      else
        j++;
    }
  }
}