void RosDetector::publishDisplayImage(const cv::Mat& source, const std::vector<cv::Point2f>& corners, bool success) { if (display_pub_.getNumSubscribers() == 0) return; detector_.getDisplayImage(source, corners, success, display_img_cv_); cv_bridge::CvImage cv_image(std_msgs::Header(), "bgr8", display_img_cv_); display_pub_.publish(*(cv_image.toImageMsg())); }
/** @brief Process and/or display DavidSDKGrabber image * @param[in] image davidSDK image */ void grabberCallback (const boost::shared_ptr<pcl::PCLImage>& image) { unsigned char *image_array = reinterpret_cast<unsigned char *> (&image->data[0]); int type = getOpenCVType (image->encoding); cv::Mat cv_image (image->height, image->width, type, image_array); cv::imshow ("davidSDK images", cv_image); cv::waitKey (5); }
void cloud_cb_ (const boost::shared_ptr<openni_wrapper::Image>& image) { thrust::host_vector<pcl_cuda::OpenNIRGB> rgb_image(image->getWidth () * image->getHeight ()); cv::Mat cv_image( image->getHeight (), image->getWidth (), CV_8UC3 ); { pcl::ScopeTime t ("computeBilinear+memcpy"); debayering.computeBilinear (image, rgb_image); //debayering.computeEdgeAware (image, rgb_image); // now fill image and show! pcl::ScopeTime t2 ("memcpy"); memcpy (cv_image.data, &rgb_image[0], image->getWidth () * image->getHeight () * 3); } imshow ("test", cv_image); }
void get_pose_and_image(const std::vector<AprilTags::TagDetection> &detections,const cv_bridge::CvImagePtr &cv_ptr, const std_msgs::Header &header) { cv::Mat image_rgb; cv::cvtColor(cv_ptr->image, image_rgb, CV_GRAY2RGB); if (detections.size()) { std::vector<Point2> pi; // Points in image std::vector<Point3> pw; // Points in world for (auto it = detections.begin(); it != detections.end(); it++) { const int id = it->id; const Point2 c2 = Point2(it->cxy.first, it->cxy.second); for (int j = 0; j < 4; j++) { const Point2 p2 = Point2(it->p[j].first, it->p[j].second); pi.push_back(p2); Point3 p3(tagsWorld[id].p[j].x, tagsWorld[id].p[j].y, 0.0); pw.push_back(p3); // Display tag corners cv::circle(image_rgb, p2, 6, colors[j], 2); } // Display tag id std::ostringstream ss; ss << id; auto color = cv::Scalar(0, 255, 255); if (tagsWorld.find(id) != tagsWorld.end()) { color = cv::Scalar(255,255,0); } cv::putText(image_rgb, ss.str(), Point2(c2.x - 5, c2.y + 5), cv::FONT_HERSHEY_PLAIN, 2, color, 2); } // Get pose static cv::Mat r = cv::Mat::zeros(cv::Size(1, 3), CV_64F); static cv::Mat cTw = cv::Mat::zeros(cv::Size(1, 3), CV_64F); cv::Mat wTc(cv::Size(3, 3), CV_64F); cv::Mat cRw(cv::Size(3, 3), CV_64F), wRc(cv::Size(3, 3), CV_64F); cv::solvePnP(pw, pi, K, D, r, cTw, false); cv::Rodrigues(r, cRw); wRc = cRw.inv(); wTc = -wRc * cTw; // ROS_INFO("%f, %f, %f", r.at<double>(0,0), r.at<double>(1,0), r.at<double>(2,0)); cv::Mat q = rodriguesToQuat(r); // Publish geometry_msgs::PoseStamped pose_cam; pose_cam.header.stamp = header.stamp; pose_cam.header.frame_id = "0"; double *pt = wTc.ptr<double>(); pose_cam.pose.position.x = pt[0]; pose_cam.pose.position.y = pt[1]; pose_cam.pose.position.z = pt[2]; double *pq = q.ptr<double>(); pose_cam.pose.orientation.w = pq[0]; pose_cam.pose.orientation.x = pq[1]; pose_cam.pose.orientation.y = pq[2]; pose_cam.pose.orientation.z = pq[3]; pose_pub.publish(pose_cam); } // Publish image cv_bridge::CvImage cv_image(header, sensor_msgs::image_encodings::BGR8, image_rgb); image_pub.publish(cv_image.toImageMsg()); // cv::imshow("image", image_rgb); // cv::waitKey(1); }
void cam_callback(const sensor_msgs::ImageConstPtr &image, const sensor_msgs::CameraInfoConstPtr &cinfo) { // Get camera info static bool init_cam = false; static cv::Mat K = cv::Mat::zeros(cv::Size(3, 3), CV_64F); static cv::Mat D = cv::Mat::zeros(cv::Size(1, 5), CV_64F); // Stop if camera not calibrated if (cinfo->K[0] == 0.0) throw std::runtime_error("Camera not calibrated."); // TODO: convert to function later // Assign camera info only once if (!init_cam) { for (int i = 0; i < 3; ++i) { double *pk = K.ptr<double>(i); for (int j = 0; j < 3; ++j) { pk[j] = cinfo->K[3 * i + j]; } } double *pd = D.ptr<double>(); for (int k = 0; k < 5; k++) { pd[k] = cinfo->D[k]; } init_cam = true; } // use cv_bridge and convert to grayscale image cv_bridge::CvImagePtr cv_ptr; // use toCvCopy because we will modify the image cv_ptr = cv_bridge::toCvCopy(image, sensor_msgs::image_encodings::MONO8); cv::Mat image_rgb; cv::cvtColor(cv_ptr->image, image_rgb, CV_GRAY2RGB); #if defined(BUILD_UMICH) // Use apriltag_umich // Currently not using this version static april_tag_family_t *tf = tag36h11_create(); static april_tag_detector_t *td = april_tag_detector_create(tf); image_u8_t *im = image_u8_create_from_gray( cv_ptr->image.cols, cv_ptr->image.rows, cv_ptr->image.data); zarray_t *detections = april_tag_detector_detect(td, im); ROS_INFO("Tags detected: %d", zarray_size(detections)); for (size_t i = 0; i < zarray_size(detections); i++) { april_tag_detection_t *det; zarray_get(detections, i, &det); for (int j = 0; j < 4; j++) { const Point2 p = Point2(det->p[j][0], det->p[j][1]); } april_tag_detection_destroy(det); } zarray_destroy(detections); image_u8_destroy(im); #elif defined(BUILD_MIT) // Use apriltag_mit static AprilTags::TagDetector tag_detector(AprilTags::tagCodes36h11); std::vector<AprilTags::TagDetection> detections = tag_detector.extractTags(cv_ptr->image); // Check detection size, only do work if there's tag detected if (detections.size()) { std::vector<Point2> pi; // Points in image std::vector<Point3> pw; // Points in world for (auto it = detections.begin(); it != detections.end(); it++) { const int id = it->id; const Point2 c2 = Point2(it->cxy.first, it->cxy.second); for (int j = 0; j < 4; j++) { const Point2 p2 = Point2(it->p[j].first, it->p[j].second); pi.push_back(p2); Point3 p3(tagsWorld[id].p[j].x, tagsWorld[id].p[j].y, 0.0); pw.push_back(p3); // Display tag corners cv::circle(image_rgb, p2, 6, colors[j], 2); } // Display tag id std::ostringstream ss; ss << id; auto color = cv::Scalar(0, 255, 255); if (tagsWorld.find(id) != tagsWorld.end()) { color = cv::Scalar(255, 255, 0); } cv::putText(image_rgb, ss.str(), Point2(c2.x - 5, c2.y + 5), cv::FONT_HERSHEY_PLAIN, 2, color, 2); } // Get pose static cv::Mat r = cv::Mat::zeros(cv::Size(1, 3), CV_64F); static cv::Mat cTw = cv::Mat::zeros(cv::Size(1, 3), CV_64F); cv::Mat wTc(cv::Size(3, 3), CV_64F); cv::Mat cRw(cv::Size(3, 3), CV_64F), wRc(cv::Size(3, 3), CV_64F); cv::solvePnP(pw, pi, K, D, r, cTw, false); cv::Rodrigues(r, cRw); wRc = cRw.inv(); wTc = -wRc * cTw; // ROS_INFO("%f, %f, %f", r.at<double>(0,0), r.at<double>(1,0), // r.at<double>(2,0)); cv::Mat q = rodriguesToQuat(r); // Publish geometry_msgs::PoseStamped pose_cam; pose_cam.header.stamp = image->header.stamp; pose_cam.header.frame_id = "0"; double *pt = wTc.ptr<double>(); pose_cam.pose.position.x = pt[0]; pose_cam.pose.position.y = pt[1]; pose_cam.pose.position.z = pt[2]; double *pq = q.ptr<double>(); pose_cam.pose.orientation.w = pq[0]; pose_cam.pose.orientation.x = pq[1]; pose_cam.pose.orientation.y = pq[2]; pose_cam.pose.orientation.z = pq[3]; pose_pub.publish(pose_cam); } #endif // Publish image cv_bridge::CvImage cv_image(image->header, sensor_msgs::image_encodings::BGR8, image_rgb); image_pub.publish(cv_image.toImageMsg()); // cv::imshow("image", image_rgb); // cv::waitKey(1); }