Function detection_cb

tracking/apps/tracker_node.cpp:212–515

* \brief Read the DetectionArray message and use the detections for creating/updating/deleting tracks * * \param[in] msg the DetectionArray message. */

 * \param[in] msg the DetectionArray message.
 */
void
detection_cb(const opt_msgs::DetectionArray::ConstPtr& msg)
{
  // Read message header information:
  std::string frame_id = msg->header.frame_id;
  ros::Time frame_time = msg->header.stamp;

  std::string frame_id_tmp = frame_id;
  int pos = frame_id_tmp.find("_rgb_optical_frame");
  if (pos != std::string::npos)
    frame_id_tmp.replace(pos, std::string("_rgb_optical_frame").size(), "");
  pos = frame_id_tmp.find("_depth_optical_frame");
  if (pos != std::string::npos)
  frame_id_tmp.replace(pos, std::string("_depth_optical_frame").size(), "");
  last_received_detection_[frame_id_tmp] = frame_time;

  // Compute delay of detection message, if any:
  double time_delay = 0.0;
  if (frame_time > latest_time)
  {
    latest_time = frame_time;
    time_delay = 0.0;
  }
  else
  {
    time_delay = (latest_time - frame_time).toSec();
  }

  tf::StampedTransform transform;
  tf::StampedTransform inverse_transform;
  //	cv_bridge::CvImage::Ptr cvPtr;

  try
  {
    // Read transforms between camera frame and world frame:
    if (!extrinsic_calibration)
    {
      static tf::TransformBroadcaster world_to_camera_tf_publisher;
//      world_to_camera_tf_publisher.sendTransform(tf::StampedTransform(camera_frame_to_world_transform, ros::Time::now(), world_frame_id, frame_id));
      world_to_camera_tf_publisher.sendTransform(tf::StampedTransform(world_to_camera_frame_transform, ros::Time::now(), frame_id, world_frame_id));
    }
    //Calculate direct and inverse transforms between camera and world frame:
    tf_listener->lookupTransform(world_frame_id, frame_id, ros::Time(0), transform);
    tf_listener->lookupTransform(frame_id, world_frame_id, ros::Time(0), inverse_transform);
    //		cvPtr = cv_bridge::toCvCopy(msg->image, sensor_msgs::image_encodings::BGR8);

    // Read camera intrinsic parameters:
    Eigen::Matrix3d intrinsic_matrix;
    for(int i = 0; i < 3; i++)
      for(int j = 0; j < 3; j++)
        intrinsic_matrix(i, j) = msg->intrinsic_matrix[i * 3 + j];

    // Add a new DetectionSource or update an existing one:
    if(detection_sources_map.find(frame_id) == detection_sources_map.end())
    {
      detection_sources_map[frame_id] = new open_ptrack::detection::DetectionSource(cv::Mat(0, 0, CV_8UC3),
          transform, inverse_transform, intrinsic_matrix, frame_time, frame_id);
    }