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;
}
