<commit_before>#include <memory>
#include <string>

#include "pfgrasp.hpp"
#include <opencv2/opencv.hpp>
#include <image_io_utils/image_io_utils.hpp> // to simplify jpeg/zlib compression and decompression
#include <ConciseArgs>
#include <bot_lcmgl_client/lcmgl.h>


void
decode_image(const bot_core::image_t* msg, cv::Mat& img)
{
  if (img.empty() || img.rows != msg->height
      || img.cols != msg->width)
    img.create(msg->height, msg->width, CV_8UC3);

  switch (msg->pixelformat) {
    case bot_core::image_t::PIXEL_FORMAT_RGB:
        memcpy(img.data, msg->data.data(), sizeof(uint8_t) * msg->width * msg->height * 3);
        cv::cvtColor(img, img, CV_RGB2BGR);
      break;
    case bot_core::image_t::PIXEL_FORMAT_MJPEG:
      // for some reason msg->row_stride is 0, so we use msg->width instead.
      jpeg_decompress_8u_rgb(msg->data.data(),
                              msg->size,
                              img.data,
                              msg->width,
                              msg->height,
                              msg->row_stride);
      cv::cvtColor(img, img, CV_RGB2BGR);
      break;
    case bot_core::image_t::PIXEL_FORMAT_GRAY:
      //memcpy(img.data, msg->data.data(), sizeof(uint8_t) * msg->width * msg->height);
      fprintf(stderr, "Gray image not supported\n");
      break;
    default:
      fprintf(stderr, "Unrecognized image format\n");
      break;
  }

  return;
}

void
PFGrasp::imageHandler(const lcm::ReceiveBuffer* rbuf,
    const std::string &channel, const bot_core::image_t* msg)
{
  if (!msg->width || !msg->height) return;

  BotTrans tmpBT;
  bot_frames_get_trans_with_utime(botFrames_, "local", options_.cameraChannelName.c_str(),  msg->utime, &tmpBT);

  double tic = bot_timestamp_now();
  decode_image(msg, img_);

  if(wimg_.empty() || wimg_.cols != img_.cols || wimg_.rows != img_.rows || wimg_.channels() != img_.channels() || wimg_.type() != img_.type()){
    wimg_.create(img_.rows, img_.cols, CV_8UC3);
  }

  warper_->unWarp(img_, wimg_);
  // Update step
  std::vector< Eigen::Vector3d > pts;
  Eigen::Isometry3d local_to_camera;

  if (tracker_->initialized)
      tracker_->update(wimg_, pts, local_to_camera);    // ?????? can add real local to camera value here

  if (++counter_ == 10) {
      printf("===> TLD TRACKER: %4.2f ms\n", (bot_timestamp_now() - tic) * 1.f * 1e-3);
      counter_ = 0;
  }

  // Update bearing vector, save the result for particle filter
  // May need lock

  if (tracker_->detection_valid) {
    img_utime_ = msg->utime;
    localToCam_ = tmpBT;

    const cv::Rect& currBB = tracker_->currBB;
    float roix = currBB.x + currBB.width/2, roiy = currBB.y + currBB.height/2;
    float u = -(cameraParams_.cx - roix);
    float v = -(cameraParams_.cy - roiy);
    if (!cameraParams_.fx && !cameraParams_.fy)
      bearing_a_ = bearing_b_ = 0;
    else
      bearing_a_ = u*1.f/cameraParams_.fx, bearing_b_ = v*1.f/cameraParams_.fy;

    perception::image_roi_t trackmsg;

    float sw = cameraParams_.width, sh = cameraParams_.height;
    trackmsg.roi.x = currBB.x / sw;
    trackmsg.roi.y = currBB.y / sh;
    trackmsg.roi.width = currBB.width / sw;
    trackmsg.roi.height = currBB.height / sh;
    lcm_->publish("TLD_OBJECT_ROI_RESULT", &trackmsg);
  }
  else {
    perception::image_roi_t trackmsg;
    trackmsg.roi.x = 0;
    trackmsg.roi.y = 0;
    trackmsg.roi.width = 0;
    trackmsg.roi.height = 0;
    lcm_->publish("TLD_OBJECT_ROI_RESULT", &trackmsg);
  }

  // Viz
  cv::Mat dispimg = wimg_.clone();

  if (tracker_->detection_valid) {
      const cv::Rect& currBB = tracker_->currBB;
      std::cerr << "TLD currBB: " << currBB.tl() << " " << currBB.br() << std::endl;

      rectangle(dispimg, currBB, Scalar(0,0,255), 3, 8, 0);
  }
  Mat scaleddispimg;
  cv::resize(dispimg, scaleddispimg, Size(dispimg.cols/2,dispimg.rows/2), 0,0,INTER_LINEAR);
  cv::imshow( "Display window", scaleddispimg );
  cv::waitKey(1);

  return;
}

void
PFGrasp::segmentHandler(const lcm::ReceiveBuffer* rbuf,
    const std::string &channel, const perception::image_roi_t* msg)
{
  std::cerr << "SEGMENTATION msg: " << msg->utime << " " << msg->roi.x << " "
      << msg->roi.y << " " << msg->roi.width << " " << msg->roi.height
      << std::endl;

  float sw = cameraParams_.width, sh = cameraParams_.height;
  Rect selection(msg->roi.x * sw, msg->roi.y * sh, msg->roi.width * sw,
      msg->roi.height * sh);

  if (msg->feature_id < 0)
    {
      std::cerr << "INVALID feature_id" << std::endl;
      return;
    }

  // Ensure tracker is initialized
  if (!tracker_->initialized)
    {
      std::cerr << "Tracker Not Initialized!" << std::endl;
      assert(0);
    }

  // Initialize with image and mask
  tracker_->initialize(wimg_, selection);

  if (options_.debug)
    {
      cv::Mat imgd = wimg_.clone();
      rectangle(imgd, selection.tl(), selection.br(), CV_RGB(255, 0, 0), 2);
      cv::imshow("Captured Image ROI", imgd);
    }
}


void
PFGrasp::commandHandler(const lcm::ReceiveBuffer* rbuf, const std::string &channel,
    const perception::pfgrasp_command_t* msg){
  switch (msg->command) {
  case perception::pfgrasp_command_t::STOP:   // stop tracking, unsubscribe image, release particles
  break;
  case perception::pfgrasp_command_t::RESTART:   // stop tracking, unsubscribe image, release particles
    initParticleFilter();
  break;
  case perception::pfgrasp_command_t::START:  // start tracking, subscribe image, initialize particles
    initParticleFilter();
  break;  // also run one iteration
  case perception::pfgrasp_command_t::RUN_ONE_ITER:
    runOneIter();
  break;
  }
}

void
PFGrasp::releaseParticleFilter(){
  pf = new ParticleFilter(N_p, rng_seed, resample_threshold, (void*)this);
  delete pf;

  bot_lcmgl_switch_buffer(lcmgl_);
}


void
PFGrasp::initParticleFilter(){<commit_msg>fix release<commit_after>#include <memory>
#include <string>

#include "pfgrasp.hpp"
#include <opencv2/opencv.hpp>
#include <image_io_utils/image_io_utils.hpp> // to simplify jpeg/zlib compression and decompression
#include <ConciseArgs>
#include <bot_lcmgl_client/lcmgl.h>


void
decode_image(const bot_core::image_t* msg, cv::Mat& img)
{
  if (img.empty() || img.rows != msg->height
      || img.cols != msg->width)
    img.create(msg->height, msg->width, CV_8UC3);

  switch (msg->pixelformat) {
    case bot_core::image_t::PIXEL_FORMAT_RGB:
        memcpy(img.data, msg->data.data(), sizeof(uint8_t) * msg->width * msg->height * 3);
        cv::cvtColor(img, img, CV_RGB2BGR);
      break;
    case bot_core::image_t::PIXEL_FORMAT_MJPEG:
      // for some reason msg->row_stride is 0, so we use msg->width instead.
      jpeg_decompress_8u_rgb(msg->data.data(),
                              msg->size,
                              img.data,
                              msg->width,
                              msg->height,
                              msg->row_stride);
      cv::cvtColor(img, img, CV_RGB2BGR);
      break;
    case bot_core::image_t::PIXEL_FORMAT_GRAY:
      //memcpy(img.data, msg->data.data(), sizeof(uint8_t) * msg->width * msg->height);
      fprintf(stderr, "Gray image not supported\n");
      break;
    default:
      fprintf(stderr, "Unrecognized image format\n");
      break;
  }

  return;
}

void
PFGrasp::imageHandler(const lcm::ReceiveBuffer* rbuf,
    const std::string &channel, const bot_core::image_t* msg)
{
  if (!msg->width || !msg->height) return;

  BotTrans tmpBT;
  bot_frames_get_trans_with_utime(botFrames_, "local", options_.cameraChannelName.c_str(),  msg->utime, &tmpBT);

  double tic = bot_timestamp_now();
  decode_image(msg, img_);

  if(wimg_.empty() || wimg_.cols != img_.cols || wimg_.rows != img_.rows || wimg_.channels() != img_.channels() || wimg_.type() != img_.type()){
    wimg_.create(img_.rows, img_.cols, CV_8UC3);
  }

  warper_->unWarp(img_, wimg_);
  // Update step
  std::vector< Eigen::Vector3d > pts;
  Eigen::Isometry3d local_to_camera;

  if (tracker_->initialized)
      tracker_->update(wimg_, pts, local_to_camera);    // ?????? can add real local to camera value here

  if (++counter_ == 10) {
      printf("===> TLD TRACKER: %4.2f ms\n", (bot_timestamp_now() - tic) * 1.f * 1e-3);
      counter_ = 0;
  }

  // Update bearing vector, save the result for particle filter
  // May need lock

  if (tracker_->detection_valid) {
    img_utime_ = msg->utime;
    localToCam_ = tmpBT;

    const cv::Rect& currBB = tracker_->currBB;
    float roix = currBB.x + currBB.width/2, roiy = currBB.y + currBB.height/2;
    float u = -(cameraParams_.cx - roix);
    float v = -(cameraParams_.cy - roiy);
    if (!cameraParams_.fx && !cameraParams_.fy)
      bearing_a_ = bearing_b_ = 0;
    else
      bearing_a_ = u*1.f/cameraParams_.fx, bearing_b_ = v*1.f/cameraParams_.fy;

    perception::image_roi_t trackmsg;

    float sw = cameraParams_.width, sh = cameraParams_.height;
    trackmsg.roi.x = currBB.x / sw;
    trackmsg.roi.y = currBB.y / sh;
    trackmsg.roi.width = currBB.width / sw;
    trackmsg.roi.height = currBB.height / sh;
    lcm_->publish("TLD_OBJECT_ROI_RESULT", &trackmsg);
  }
  else {
    perception::image_roi_t trackmsg;
    trackmsg.roi.x = 0;
    trackmsg.roi.y = 0;
    trackmsg.roi.width = 0;
    trackmsg.roi.height = 0;
    lcm_->publish("TLD_OBJECT_ROI_RESULT", &trackmsg);
  }

  // Viz
  cv::Mat dispimg = wimg_.clone();

  if (tracker_->detection_valid) {
      const cv::Rect& currBB = tracker_->currBB;
      std::cerr << "TLD currBB: " << currBB.tl() << " " << currBB.br() << std::endl;

      rectangle(dispimg, currBB, Scalar(0,0,255), 3, 8, 0);
  }
  Mat scaleddispimg;
  cv::resize(dispimg, scaleddispimg, Size(dispimg.cols/2,dispimg.rows/2), 0,0,INTER_LINEAR);
  cv::imshow( "Display window", scaleddispimg );
  cv::waitKey(1);

  return;
}

void
PFGrasp::segmentHandler(const lcm::ReceiveBuffer* rbuf,
    const std::string &channel, const perception::image_roi_t* msg)
{
  std::cerr << "SEGMENTATION msg: " << msg->utime << " " << msg->roi.x << " "
      << msg->roi.y << " " << msg->roi.width << " " << msg->roi.height
      << std::endl;

  float sw = cameraParams_.width, sh = cameraParams_.height;
  Rect selection(msg->roi.x * sw, msg->roi.y * sh, msg->roi.width * sw,
      msg->roi.height * sh);

  if (msg->feature_id < 0)
    {
      std::cerr << "INVALID feature_id" << std::endl;
      return;
    }

  // Ensure tracker is initialized
  if (!tracker_->initialized)
    {
      std::cerr << "Tracker Not Initialized!" << std::endl;
      assert(0);
    }

  // Initialize with image and mask
  tracker_->initialize(wimg_, selection);

  if (options_.debug)
    {
      cv::Mat imgd = wimg_.clone();
      rectangle(imgd, selection.tl(), selection.br(), CV_RGB(255, 0, 0), 2);
      cv::imshow("Captured Image ROI", imgd);
    }
}


void
PFGrasp::commandHandler(const lcm::ReceiveBuffer* rbuf, const std::string &channel,
    const perception::pfgrasp_command_t* msg){
  switch (msg->command) {
  case perception::pfgrasp_command_t::STOP:   // stop tracking, unsubscribe image, release particles
  break;
  case perception::pfgrasp_command_t::RESTART:   // stop tracking, unsubscribe image, release particles
    releaseParticleFilter();
    initParticleFilter();
  break;
  case perception::pfgrasp_command_t::START:  // start tracking, subscribe image, initialize particles
    initParticleFilter();
  break;  // also run one iteration
  case perception::pfgrasp_command_t::RUN_ONE_ITER:
    runOneIter();
  break;
  }
}

void
PFGrasp::releaseParticleFilter(){
  pf = new ParticleFilter(N_p, rng_seed, resample_threshold, (void*)this);
  delete pf;

  bot_lcmgl_switch_buffer(lcmgl_);
}


void
PFGrasp::initParticleFilter(){
{'fraction_non_alphanumeric': 0.15202326974078192, 'fraction_numerical': 0.008897253828385661, 'mean_word_length': 3.2035239122617765, 'pattern_counts': {'":': 0, '<': 99, '<?xml version=': 0, '>': 107, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '3019499', 'n_tokens_mistral': 4265, 'n_tokens_neox': 4152, 'n_words': 841}