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 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()) {
ROS_INFO("vscan_image 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++;
}
}
}
void image_raw_callback(const sensor_msgs::Image::ConstPtr& image_raw_msg) {
pthread_mutex_lock(&mutex);
image_raw_ringbuf.push_front(*image_raw_msg);
//image_obj_ranged is empty
if (image_obj_ranged_ringbuf.begin() == image_obj_ranged_ringbuf.end()) {
ROS_INFO("image_obj_ranged ring buffer is empty");
buf_flag = false;
pthread_mutex_unlock(&mutex);
return;
}
buf_flag = true;
pthread_mutex_unlock(&mutex);
pthread_mutex_lock(&mutex);
if (image_obj_tracked_flag == true) {
publish();
}
pthread_mutex_unlock(&mutex);
}
void CTimeSmoother::GetIntRepresentation(const boost::circular_buffer<double> &data, vector<unsigned int> &intData, const vector<double> &bins, const vector<unsigned int> &intBins)
{
intData.clear();
for (boost::circular_buffer<double>::const_iterator i = data.begin(); i != data.end(); ++i)
{
double min_r2 = numeric_limits<double>::max();
unsigned int min_j = 0;
for (unsigned int j = 0; j < bins.size(); ++j)
{
double d = MathUtils::round_int(*i/bins[j]);
double r2 = (*i - bins[j]*d)*(*i - bins[j]*d);
if (r2 < min_r2)
{
min_j = j;
min_r2 = r2;
}
}
intData.push_back(MathUtils::round_int(*i/bins[min_j])*intBins[min_j]);
}
}
bool publish() {
if (buf_flag) {
pthread_mutex_lock(&mutex);
//image_obj is empty
if (image_obj_ringbuf.begin() == image_obj_ringbuf.end()) {
pthread_mutex_unlock(&mutex);
ROS_INFO("image_obj ring buffer is empty");
return false;
}
//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 false;
}
// image_obj > vscan_image
if (get_time(&(image_obj_ringbuf.front().header)) >= get_time(&(vscan_image_ringbuf.front().header))) {
p_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) {
p_image_obj_buf = &*it;
publish_msg(p_image_obj_buf, p_vscan_image_buf);
pthread_mutex_unlock(&mutex);
return true;
} 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))) {
p_image_obj_buf = &*(it-1);
break;
}
}
if (it == image_obj_ringbuf.end()) {
p_image_obj_buf = &(image_obj_ringbuf.back());
}
}
}
// image_obj < vscan_image
else {
p_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) {
p_vscan_image_buf = &*it;
publish_msg(p_image_obj_buf, p_vscan_image_buf);
pthread_mutex_unlock(&mutex);
return true;
}
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))) {
p_vscan_image_buf = &*(it-1);
break;
}
}
if (it == vscan_image_ringbuf.end()) {
p_vscan_image_buf = &(vscan_image_ringbuf.back());
}
}
publish_msg(p_image_obj_buf, p_vscan_image_buf);
if (image_obj_ranged_flag == true){
buf_flag = false;
image_obj_ranged_flag = false;
pthread_mutex_unlock(&flag_mutex);
image_obj_ringbuf.clear();
vscan_image_ringbuf.clear();
}
return true;
} else {
return false;
}
}
void callback(const sensor_msgs::ImageConstPtr &img,
const sensor_msgs::CameraInfoConstPtr &info) {
boost::mutex::scoped_lock lock(mutex_);
ros::Time now = ros::Time::now();
static boost::circular_buffer<double> in_times(100);
static boost::circular_buffer<double> out_times(100);
static boost::circular_buffer<double> in_bytes(100);
static boost::circular_buffer<double> out_bytes(100);
ROS_DEBUG("resize: callback");
if ( !publish_once_ || cp_.getNumSubscribers () == 0 ) {
ROS_DEBUG("resize: number of subscribers is 0, ignoring image");
return;
}
in_times.push_front((now - last_subscribe_time_).toSec());
in_bytes.push_front(img->data.size());
//
try {
int width = dst_width_ ? dst_width_ : (resize_x_ * info->width);
int height = dst_height_ ? dst_height_ : (resize_y_ * info->height);
double scale_x = dst_width_ ? ((double)dst_width_)/info->width : resize_x_;
double scale_y = dst_height_ ? ((double)dst_height_)/info->height : resize_y_;
cv_bridge::CvImagePtr cv_img = cv_bridge::toCvCopy(img);
cv::Mat tmpmat(height, width, cv_img->image.type());
cv::resize(cv_img->image, tmpmat, cv::Size(width, height));
cv_img->image = tmpmat;
sensor_msgs::CameraInfo tinfo = *info;
tinfo.height = height;
tinfo.width = width;
tinfo.K[0] = tinfo.K[0] * scale_x; // fx
tinfo.K[2] = tinfo.K[2] * scale_x; // cx
tinfo.K[4] = tinfo.K[4] * scale_y; // fy
tinfo.K[5] = tinfo.K[5] * scale_y; // cy
tinfo.P[0] = tinfo.P[0] * scale_x; // fx
tinfo.P[2] = tinfo.P[2] * scale_x; // cx
tinfo.P[3] = tinfo.P[3] * scale_x; // T
tinfo.P[5] = tinfo.P[5] * scale_y; // fy
tinfo.P[6] = tinfo.P[6] * scale_y; // cy
if ( !use_messages_ || now - last_publish_time_ > period_ ) {
cp_.publish(cv_img->toImageMsg(),
boost::make_shared<sensor_msgs::CameraInfo> (tinfo));
out_times.push_front((now - last_publish_time_).toSec());
out_bytes.push_front(cv_img->image.total()*cv_img->image.elemSize());
last_publish_time_ = now;
}
} catch( cv::Exception& e ) {
ROS_ERROR("%s", e.what());
}
float duration = (now - last_rosinfo_time_).toSec();
if ( duration > 2 ) {
int in_time_n = in_times.size();
int out_time_n = out_times.size();
double in_time_mean = 0, in_time_rate = 1.0, in_time_std_dev = 0.0, in_time_max_delta, in_time_min_delta;
double out_time_mean = 0, out_time_rate = 1.0, out_time_std_dev = 0.0, out_time_max_delta, out_time_min_delta;
std::for_each( in_times.begin(), in_times.end(), (in_time_mean += boost::lambda::_1) );
in_time_mean /= in_time_n;
in_time_rate /= in_time_mean;
std::for_each( in_times.begin(), in_times.end(), (in_time_std_dev += (boost::lambda::_1 - in_time_mean)*(boost::lambda::_1 - in_time_mean) ) );
in_time_std_dev = sqrt(in_time_std_dev/in_time_n);
if ( in_time_n > 1 ) {
in_time_min_delta = *std::min_element(in_times.begin(), in_times.end());
in_time_max_delta = *std::max_element(in_times.begin(), in_times.end());
}
std::for_each( out_times.begin(), out_times.end(), (out_time_mean += boost::lambda::_1) );
out_time_mean /= out_time_n;
out_time_rate /= out_time_mean;
std::for_each( out_times.begin(), out_times.end(), (out_time_std_dev += (boost::lambda::_1 - out_time_mean)*(boost::lambda::_1 - out_time_mean) ) );
out_time_std_dev = sqrt(out_time_std_dev/out_time_n);
if ( out_time_n > 1 ) {
out_time_min_delta = *std::min_element(out_times.begin(), out_times.end());
out_time_max_delta = *std::max_element(out_times.begin(), out_times.end());
}
double in_byte_mean = 0, out_byte_mean = 0;
std::for_each( in_bytes.begin(), in_bytes.end(), (in_byte_mean += boost::lambda::_1) );
std::for_each( out_bytes.begin(), out_bytes.end(), (out_byte_mean += boost::lambda::_1) );
in_byte_mean /= duration;
out_byte_mean /= duration;
ROS_INFO_STREAM(" in bandwidth: " << std::fixed << std::setw(11) << std::setprecision(3) << in_byte_mean/1000*8
<< " Kbps rate:" << std::fixed << std::setw(7) << std::setprecision(3) << in_time_rate
<< " hz min:" << std::fixed << std::setw(7) << std::setprecision(3) << in_time_min_delta
<< " s max: " << std::fixed << std::setw(7) << std::setprecision(3) << in_time_max_delta
<< " s std_dev: "<< std::fixed << std::setw(7) << std::setprecision(3) << in_time_std_dev << "s n: " << in_time_n);
ROS_INFO_STREAM(" out bandwidth: " << std::fixed << std::setw(11) << std::setprecision(3) << out_byte_mean/1000*8
<< " kbps rate:" << std::fixed << std::setw(7) << std::setprecision(3) << out_time_rate
<< " hz min:" << std::fixed << std::setw(7) << std::setprecision(3) << out_time_min_delta
<< " s max: " << std::fixed << std::setw(7) << std::setprecision(3) << out_time_max_delta
<< " s std_dev: "<< std::fixed << std::setw(7) << std::setprecision(3) << out_time_std_dev << "s n: " << out_time_n);
in_times.clear();
in_bytes.clear();
out_times.clear();
out_bytes.clear();
last_rosinfo_time_ = now;
}
last_subscribe_time_ = now;
if(use_snapshot_) {
publish_once_ = false;
}
}
template<typename T> std::vector<T> buf2vec(boost::circular_buffer<T> buf) const
{
std::vector<T> vec(buf.begin(), buf.end());
return vec;
}
void ShowFrameDurationPlot() {
Vec2i windowSize = mainApp->getWindow()->getSize();
size_t maxSamples = size_t(windowSize.x);
if(maxSamples != frameDurationPlotValues.capacity()) {
frameDurationPlotValues.set_capacity(maxSamples);
}
if(maxSamples != frameDurationPlotVertices.size()) {
frameDurationPlotVertices.resize(maxSamples);
}
GRenderer->ResetTexture(0);
frameDurationPlotValues.push_front(toMs(g_platformTime.lastFrameDuration()));
float avg = std::accumulate(frameDurationPlotValues.begin(), frameDurationPlotValues.end(), 0.f) / frameDurationPlotValues.size();
float worst = *std::max_element(frameDurationPlotValues.begin(), frameDurationPlotValues.end());
const float OFFSET_Y = 80.f;
const float SCALE_Y = 4.0f;
for(size_t i = 0; i < frameDurationPlotValues.size(); ++i)
{
float time = frameDurationPlotValues[i];
frameDurationPlotVertices[i].color = Color::white.toRGB();
frameDurationPlotVertices[i].p.x = i;
frameDurationPlotVertices[i].p.y = OFFSET_Y + (time * SCALE_Y);
frameDurationPlotVertices[i].p.z = 1.0f;
frameDurationPlotVertices[i].w = 1.0f;
}
EERIEDRAWPRIM(Renderer::LineStrip, &frameDurationPlotVertices[0], frameDurationPlotValues.size());
Color avgColor = Color::blue * 0.5f + Color::white * 0.5f;
float avgPos = OFFSET_Y + (avg * SCALE_Y);
drawLine(Vec2f(0, avgPos), Vec2f(windowSize.x, avgPos), 1.0f, Color::blue);
Color worstColor = Color::red * 0.5f + Color::white * 0.5f;
float worstPos = OFFSET_Y + (worst * SCALE_Y);
drawLine(Vec2f(0, worstPos), Vec2f(windowSize.x, worstPos), 1.0f, Color::red);
Font * font = hFontDebug;
float lineOffset = font->getLineHeight() + 2;
std::string labels[3] = { "Average: ", "Worst: ", "Current: " };
Color colors[3] = { avgColor, worstColor, Color::white };
float values[3] = { avg, worst, frameDurationPlotValues[0] };
std::string texts[3];
float widths[3];
static float labelWidth = 0.f;
static float valueWidth = 0.f;
for(size_t i = 0; i < 3; i++) {
// Format value
std::ostringstream oss;
oss << std::fixed << std::setprecision(2) << values[i] << " ms ("<< 1.f / (values[i] * 0.001f) << " FPS)";
texts[i] = oss.str();
// Calculate widths (could be done more efficiently for monospace fonts...)
labelWidth = std::max(labelWidth, float(font->getTextSize(labels[i]).width()));
widths[i] = font->getTextSize(texts[i]).width();
valueWidth = std::max(valueWidth, widths[i]);
}
float x = 10;
float y = 10;
float xend = x + labelWidth + 10 + valueWidth;
for(size_t i = 0; i < 3; i++) {
font->draw(Vec2i(x, y), labels[i], Color::gray(0.8f));
font->draw(Vec2i(xend - widths[i], y), texts[i], colors[i]);
y += lineOffset;
}
}
bool publish() {
if (buf_flag) {
//image_obj_ranged is empty
if (image_obj_ranged_ringbuf.begin() == image_obj_ranged_ringbuf.end()) {
ROS_INFO("image_obj_ranged ring buffer is empty");
return false;
}
//image_raw is empty
if (image_raw_ringbuf.begin() == image_raw_ringbuf.end()) {
ROS_INFO("image_raw ring buffer is empty");
return false;
}
// image_obj_ranged > image_raw
if (get_time(&(image_obj_ranged_ringbuf.front().header)) >= get_time(&(image_raw_ringbuf.front().header))) {
p_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) {
p_image_obj_ranged_buf = &*it;
publish_msg(p_image_obj_ranged_buf, p_image_raw_buf);
if (image_obj_tracked_flag == true){
buf_flag = false;
image_obj_tracked_flag = false;
image_obj_ranged_ringbuf.clear();
image_raw_ringbuf.clear();
}
return true;
} 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))) {
p_image_obj_ranged_buf = &*(it-1);
break;
}
}
if (it == image_obj_ranged_ringbuf.end()) {
p_image_obj_ranged_buf = &(image_obj_ranged_ringbuf.back());
}
}
}
// image_obj_ranged < image_raw
else {
p_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) {
p_image_raw_buf = &*it;
publish_msg(p_image_obj_ranged_buf, p_image_raw_buf);
if (image_obj_tracked_flag == true){
buf_flag = false;
image_obj_tracked_flag = false;
image_obj_ranged_ringbuf.clear();
image_raw_ringbuf.clear();
}
return true;
}
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))) {
p_image_raw_buf = &*(it-1);
break;
}
}
if (it == image_raw_ringbuf.end()) {
p_image_raw_buf = &(image_raw_ringbuf.back());
}
}
publish_msg(p_image_obj_ranged_buf, p_image_raw_buf);
if (image_obj_tracked_flag == true){
buf_flag = false;
image_obj_tracked_flag = false;
image_obj_ranged_ringbuf.clear();
image_raw_ringbuf.clear();
}
return true;
} else {
return false;
}
}