* \brief Read the DetectionArray message and use the detections for creating/updating/deleting tracks * * \param[in] msg the DetectionArray message. */
| 213 | * \param[in] msg the DetectionArray message. |
| 214 | */ |
| 215 | void |
| 216 | detection_cb(const opt_msgs::DetectionArray::ConstPtr& msg) |
| 217 | { |
| 218 | // Read message header information: |
| 219 | std::string frame_id = msg->header.frame_id; |
| 220 | ros::Time frame_time = msg->header.stamp; |
| 221 | |
| 222 | std::string frame_id_tmp = frame_id; |
| 223 | int pos = frame_id_tmp.find("_rgb_optical_frame"); |
| 224 | if (pos != std::string::npos) |
| 225 | frame_id_tmp.replace(pos, std::string("_rgb_optical_frame").size(), ""); |
| 226 | pos = frame_id_tmp.find("_depth_optical_frame"); |
| 227 | if (pos != std::string::npos) |
| 228 | frame_id_tmp.replace(pos, std::string("_depth_optical_frame").size(), ""); |
| 229 | last_received_detection_[frame_id_tmp] = frame_time; |
| 230 | |
| 231 | // Compute delay of detection message, if any: |
| 232 | double time_delay = 0.0; |
| 233 | if (frame_time > latest_time) |
| 234 | { |
| 235 | latest_time = frame_time; |
| 236 | time_delay = 0.0; |
| 237 | } |
| 238 | else |
| 239 | { |
| 240 | time_delay = (latest_time - frame_time).toSec(); |
| 241 | } |
| 242 | |
| 243 | tf::StampedTransform transform; |
| 244 | tf::StampedTransform inverse_transform; |
| 245 | // cv_bridge::CvImage::Ptr cvPtr; |
| 246 | |
| 247 | try |
| 248 | { |
| 249 | // Read transforms between camera frame and world frame: |
| 250 | if (!extrinsic_calibration) |
| 251 | { |
| 252 | static tf::TransformBroadcaster world_to_camera_tf_publisher; |
| 253 | world_to_camera_tf_publisher.sendTransform( |
| 254 | tf::StampedTransform(world_to_camera_frame_transform, |
| 255 | ros::Time::now(), frame_id, world_frame_id)); |
| 256 | } |
| 257 | //Calculate direct and inverse transforms between camera and world frame: |
| 258 | tf_listener->lookupTransform(world_frame_id, frame_id, ros::Time(0), |
| 259 | transform); |
| 260 | tf_listener->lookupTransform(frame_id, world_frame_id, ros::Time(0), |
| 261 | inverse_transform); |
| 262 | |
| 263 | // Read camera intrinsic parameters: |
| 264 | Eigen::Matrix3d intrinsic_matrix; |
| 265 | for(int i = 0; i < 3; i++) |
| 266 | for(int j = 0; j < 3; j++) |
| 267 | intrinsic_matrix(i, j) = msg->intrinsic_matrix[i * 3 + j]; |
| 268 | |
| 269 | // Add a new DetectionSource or update an existing one: |
| 270 | if(detection_sources_map.find(frame_id) == detection_sources_map.end()) |
| 271 | { |
| 272 | detection_sources_map[frame_id] = new |
nothing calls this directly
no test coverage detected