void cbImage(const sensor_msgs::ImageConstPtr& image_msg){ if (not has_camera_info_){ ROS_INFO_STREAM("[StreetNameDetectorNode] Still waiting for CameraInfo."); return; } /* Convert image to cv::Mat */ cv_bridge::CvImageConstPtr rgb_ptr = cv_bridge::toCvShare(image_msg); /* Convert to gray image*/ cv::Mat image = rgb_ptr->image; cv::Mat image_gray; cv::cvtColor(image, image_gray, CV_BGR2GRAY); /* Detect april tag */ vector<AprilTags::TagDetection> detections = tag_detector_.extractTags(image_gray); /* Detect street name */ // cv::Mat im_in = image.clone(); // // pre-processing: keep green channel only for blobs // cv::Mat im_channels[3]; // cv::split(image, im_channels); // // set blue and red channels to 0 // im_channels[0]= cv::Mat::ones(image.rows, image.cols, CV_8UC1); // im_channels[2]= cv::Mat::ones(image.rows, image.cols, CV_8UC1); // im_channels[0] = 255 * im_channels[0]; // im_channels[2] = 255 * im_channels[2]; // //Merging green channel back to image // cv::merge(im_channels, 3, image); vector<Region> regions_all, regions_black, regions_white, regions_black_f, regions_white_f, regions_filtered; this->detect_regions(image, regions_black, regions_white); ROS_INFO_STREAM("Regions Black:\t" << regions_black.size()); ROS_INFO_STREAM("Regions White:\t" << regions_white.size()); // extract keypoints std::vector<cv::KeyPoint> kps1, kps2; // 1.1 detect FAST and SIFT in multi-scale kps1 = this->get_fast_corners(image, 3000); // try to draw something this->draw_regions(image, regions_black); cv::drawKeypoints(image, kps1, image); /* publish visualization */ duckietown_msgs::AprilTags tags_msg; for (int i = 0; i < detections.size(); ++i){ tags_msg.detections.push_back(toMsg(detections[i])); detections[i].draw(image); } tags_msg.header.stamp = image_msg->header.stamp; /* TODO: Should use the id of image frame? or the vehicle frame? */ tags_msg.header.frame_id = image_msg->header.frame_id; pub_image_.publish(rgb_ptr->toImageMsg()); pub_detection_.publish(tags_msg); }
graph_slam_msgs::Edge Conversions::toMsg(const SlamEdge &edge) { graph_slam_msgs::Edge edge_msg; edge_msg.id = edge.id_; edge_msg.type = edge.type_; edge_msg.id_from = edge.id_from_; edge_msg.id_to = edge.id_to_; edge_msg.transformation = toMsg(edge.transform_, edge.information_); edge_msg.error = edge.error_; edge_msg.matching_score = edge.matching_score_; edge_msg.sensor_from = edge.sensor_from_; edge_msg.sensor_to = edge.sensor_to_; edge_msg.displacement_from = toMsg(edge.displacement_from_); edge_msg.displacement_to = toMsg(edge.displacement_to_); edge_msg.error = edge.error_; edge_msg.age = edge.age_; edge_msg.valid = edge.valid_; edge_msg.diff_time = edge.diff_time_; return edge_msg; }
geometry_msgs::PoseWithCovariance Conversions::toMsg(const Eigen::Isometry3d &pose, const Eigen::MatrixXd &Sigma) { // Store the covariance in a ROS pose message. geometry_msgs::PoseWithCovariance pwc; pwc.pose = toMsg(pose); for (int i = 0; i < 6; i++) { for (int j = 0; j < 6; j++) { pwc.covariance[6*i + j] = Sigma(i, j); } } return pwc; }