void ImageTagManagerImpl::FindTagsWorker()
{
	apriltag_family_t* tf(tag36h11_create());
	apriltag_detector_t* td(apriltag_detector_create());
	apriltag_detector_add_family(td, tf);

	td->quad_decimate = 1.0;
	td->quad_sigma = 0.0;
	td->nthreads = 1;
	td->debug = 0;
	td->refine_decode = 0;
	td->refine_pose = 0;

	while (!m_stopping)
	{
		if (m_event.Wait())
		{
			if (m_locator)
			{
				m_locator->FindTags(td);
			}
		}
	}

	apriltag_detector_destroy(td);
	tag36h11_destroy(tf);
}
Пример #2
0
int main(int argc, char **argv){
	printf("one\n");
	apriltag_family_t *tf = tag36h11_create();
	printf("two\n");
	apriltag_detector_t *td = apriltag_detector_create();
	printf("three\n");
	apriltag_detector_add_family(td,tf);
	printf("four\n");
	image_u8_t* img = image_u8_create_from_pnm("../WillTagged.pnm");
	printf("five\n");
	zarray_t* detections = apriltag_detector_detect(td,img);
	printf("%i",zarray_size(detections)); 
	printf("\n");
	
} 
Пример #3
0
int main(int argc, char *argv[])
{
    getopt_t *getopt = getopt_create();

    getopt_add_bool(getopt, 'h', "help", 0, "Show this help");
    getopt_add_bool(getopt, 'd', "debug", 0, "Enable debugging output (slow)");
    getopt_add_bool(getopt, 'q', "quiet", 0, "Reduce output");
    getopt_add_string(getopt, 'f', "family", "tag36h11", "Tag family to use");
    getopt_add_int(getopt, '\0', "border", "1", "Set tag family border size");
    getopt_add_int(getopt, 'i', "iters", "1", "Repeat processing on input set this many times");
    getopt_add_int(getopt, 't', "threads", "4", "Use this many CPU threads");
    getopt_add_double(getopt, 'x', "decimate", "1.0", "Decimate input image by this factor");
    getopt_add_double(getopt, 'b', "blur", "0.0", "Apply low-pass blur to input");
    getopt_add_bool(getopt, '1', "refine-decode", 0, "Spend more time trying to decode tags");
    getopt_add_bool(getopt, '2', "refine-pose", 0, "Spend more time trying to precisely localize tags");

    if (!getopt_parse(getopt, argc, argv, 1) || getopt_get_bool(getopt, "help")) {
        printf("Usage: %s [options] <input files>\n", argv[0]);
        getopt_do_usage(getopt);
        exit(0);
    }

    const zarray_t *inputs = getopt_get_extra_args(getopt);

    apriltag_family_t *tf = NULL;
    const char *famname = getopt_get_string(getopt, "family");
    if (!strcmp(famname, "tag36h11"))
        tf = tag36h11_create();
    else if (!strcmp(famname, "tag36h10"))
        tf = tag36h10_create();
    else if (!strcmp(famname, "tag36artoolkit"))
        tf = tag36artoolkit_create();
    else if (!strcmp(famname, "tag25h9"))
        tf = tag25h9_create();
    else if (!strcmp(famname, "tag25h7"))
        tf = tag25h7_create();
    else {
        printf("Unrecognized tag family name. Use e.g. \"tag36h11\".\n");
        exit(-1);
    }

    tf->black_border = getopt_get_int(getopt, "border");

    apriltag_detector_t *td = apriltag_detector_create();
    apriltag_detector_add_family(td, tf);
    td->quad_decimate = getopt_get_double(getopt, "decimate");
    td->quad_sigma = getopt_get_double(getopt, "blur");
    td->nthreads = getopt_get_int(getopt, "threads");
    td->debug = getopt_get_bool(getopt, "debug");
    td->refine_decode = getopt_get_bool(getopt, "refine-decode");
    td->refine_pose = getopt_get_bool(getopt, "refine-pose");

    int quiet = getopt_get_bool(getopt, "quiet");

    int maxiters = getopt_get_int(getopt, "iters");

    const int hamm_hist_max = 10;

    for (int iter = 0; iter < maxiters; iter++) {

        if (maxiters > 1)
            printf("iter %d / %d\n", iter + 1, maxiters);

        for (int input = 0; input < zarray_size(inputs); input++) {

            int hamm_hist[hamm_hist_max];
            memset(hamm_hist, 0, sizeof(hamm_hist));

            char *path;
            zarray_get(inputs, input, &path);
            if (!quiet)
                printf("loading %s\n", path);

            image_u8_t *im = image_u8_create_from_pnm(path);
            if (im == NULL) {
                printf("couldn't find %s\n", path);
                continue;
            }

            zarray_t *detections = apriltag_detector_detect(td, im);

            for (int i = 0; i < zarray_size(detections); i++) {
                apriltag_detection_t *det;
                zarray_get(detections, i, &det);

                if (!quiet)
                    printf("detection %3d: id (%2dx%2d)-%-4d, hamming %d, goodness %8.3f, margin %8.3f\n",
                           i, det->family->d*det->family->d, det->family->h, det->id, det->hamming, det->goodness, det->decision_margin);

                hamm_hist[det->hamming]++;

                apriltag_detection_destroy(det);
            }

            zarray_destroy(detections);


            if (!quiet) {
                timeprofile_display(td->tp);
                printf("nedges: %d, nsegments: %d, nquads: %d\n", td->nedges, td->nsegments, td->nquads);
            }

            if (!quiet)
                printf("Hamming histogram: ");

            for (int i = 0; i < hamm_hist_max; i++)
                printf("%5d", hamm_hist[i]);

            if (quiet) {
                printf("%12.3f", timeprofile_total_utime(td->tp) / 1.0E3);
            }

            printf("\n");

            image_u8_destroy(im);
        }
    }

    // don't deallocate contents of inputs; those are the argv
    apriltag_detector_destroy(td);

    tag36h11_destroy(tf);
    return 0;
}
int main(){
  
  bool showGradient = true;
  bool found = false;

  VideoCapture cap(0); // open the default camera
  Size size(854,480);  // size of desired frame origionally 1280x720, 1024x576, 854x480
  if(!cap.isOpened())  // check if camera opened
    return -1;
  
  Mat frame;
  Mat src;
  
  /* From apriltag_demo.c */
  
  int maxiters = 5;
  const int hamm_hist_max = 10;
  int quiet = 0;
  
  apriltag_family_t *tf = tag36h11_create();                // Apriltag family 36h11, can change
  tf->black_border = 1;                                     // Set tag family border size
  
  apriltag_detector_t *td = apriltag_detector_create();     // Apriltag detector
  apriltag_detector_add_family(td, tf);                     // Add apriltag family
  
  td->quad_decimate = 1.0;                                  // Decimate input image by factor
  td->quad_sigma = 0.0;                                     // No blur (I think)
  td->nthreads = 4;                                         // 4 treads provided
  td->debug = 0;                                            // No debuging output
  td->refine_decode = 0;                                    // Don't refine decode
  td->refine_pose = 0;                                      // Don't refine pose
  
  // Output variables
  char imgSize[20];
  char renderTime[20];
  char detectString[50];
  char convertTime[50];
  char displayString[120];
  char outputString[120];
  char locationString[120];
  double time_taken = 0.0;
  
  long double totalFPS = 0.0;
  double count = 0.0;
  
  /* End of apriltag_demo.c */
  
  while(1){
    
    clock_t t;
    t = clock();
    
    cap >> src;                                               // Get a new frame from camera
    if(found){ resize(src,frame,size); }                      // Resize to smaller image if tag found
    else{ frame = src; }                                      // Keep standard image if no tag
    
    if(showGradient){
      cvtColor(src, frame, CV_BGR2GRAY);
      cvtColor(frame, frame, CV_GRAY2RGB);
      src = gradientEdges(frame);                               // Show gradient for fun
    }else{
      cvtColor(src, src, CV_BGR2GRAY);
    }
    
    pnm_t *pnm = mat2pnm(&frame);                             // Convert Mat fram to pnm
    image_u8_t *im = pnm_to_image_u8(pnm);                    // Convert pnm to gray image_u8
    if (im == NULL) {                                         // Error - no image created from pnm
      std::cout << "Error, not a proper pnm" << std::endl;
      return -1;
    }
    
    /*** Start from origional Apriltags from apriltag_demo.c ***/

    int hamm_hist[hamm_hist_max];
    memset(hamm_hist, 0, sizeof(hamm_hist));
    zarray_t *detections = apriltag_detector_detect(td, im);
    
    for (int i = 0; i < zarray_size(detections); i++) {
      
      apriltag_detection_t *det;
      zarray_get(detections, i, &det);
      sprintf(locationString, "Tag Center: (%f,%f)", det->c[0], det->c[1]);
      sprintf(detectString, "detection %2d: id (%2dx%2d)-%-4d, hamming %d, goodness %5.3f, margin %5.3f\n",
               i+1, det->family->d*det->family->d, det->family->h, det->id, det->hamming, det->goodness, det->decision_margin);

      hamm_hist[det->hamming]++;
      
      // draws a vertical rectangle around tag, not ideal, but easy to implement
      // det->p[corner][positon], counter clockwise
      Point pt1 = Point(det->p[0][0], det->p[0][1]);
      Point pt2 = Point(det->p[2][0], det->p[2][1]);
      cv::rectangle(src, pt1, pt2, cvScalar(102,255,0));
      
      apriltag_detection_destroy(det);
    }
    
    if(zarray_size(detections) < 1){
      found = false;
      sprintf(detectString, "No tag detected");
      sprintf(locationString, "No tag detected");
    }else{
      found = false;
    }
    
    zarray_destroy(detections);
    image_u8_destroy(im);

    t = clock() - t;
    double time_taken = ((double)t)/(CLOCKS_PER_SEC/1000);
    //printf("ms to render: %5.3f\n", time_taken);
    
    if (!quiet) {
      //timeprofile_display(td->tp);
      totalFPS += (1000.0/time_taken);
      count += 1.0;
      if(count > 30000.0){
        totalFPS = 0.0;
        count = 0.0;
      }
      sprintf(displayString, "fps: %2.2Lf, nquads: %d",totalFPS/count, td->nquads);
      //std::cout << displayString;
    }
    
    //for (int i = 0; i < hamm_hist_max; i++)
      //printf("%5d", hamm_hist[i]);
    
    sprintf(renderTime, "Render: %5.3fms", time_taken);
    sprintf(imgSize, "%dx%d", frame.cols, frame.rows);
    sprintf(outputString, "%s %s %s", renderTime, convertTime, imgSize);
    printf("%s %s\r", detectString, outputString);
    
    if (quiet) {
      printf("%12.3f", timeprofile_total_utime(td->tp) / 1.0E3);
    }
    
    printf("\n");
    
    /*** End of origional Apriltags from apriltag_demo.c ***/
    
    // displays fps, edges, segments, quads
    putText(src, displayString, cvPoint(30,30),
            FONT_HERSHEY_COMPLEX_SMALL, 0.8, cvScalar(150,150,250), 1, CV_AA);
    
    // displays render time, convert time, and image size
    putText(src, outputString, cvPoint(30,50),
            FONT_HERSHEY_COMPLEX_SMALL, 0.8, cvScalar(150,150,250), 1, CV_AA);
    
    // Displays any detections (if any)
    putText(src, detectString, cvPoint(30,70),
            FONT_HERSHEY_COMPLEX_SMALL, 0.8, cvScalar(150,150,250), 1, CV_AA);
    
    // Displays tag location (if any)
    putText(src, locationString, cvPoint(30,90),
            FONT_HERSHEY_COMPLEX_SMALL, 0.8, cvScalar(150,150,250), 1, CV_AA);
    
    imshow("Display Apriltags", src);
    
    if(waitKey(30) >= 0) break;
  }
  
  /* deallocate apriltag constructs */
  apriltag_detector_destroy(td);
  tag36h11_destroy(tf);
  
  return 0;
}
Пример #5
0
int main(int argc, char *argv[])
{
    april_tag_family_t *tf = tag36h11_create();

    april_tag_detector_t *td = april_tag_detector_create(tf);
    td->small_tag_refinement = 0;

    int maxiters = 1;

    zarray_t *inputs = zarray_create(sizeof(char*));
    int waitsec = 0;

    for (int i = 1; i < argc; i++) {
        if (!strcmp(argv[i], "-d"))
            td->debug = 1;
        else if (!strcmp(argv[i], "-t"))
            td->nthreads = atoi(argv[++i]);
        else if (!strcmp(argv[i], "-f"))
            td->seg_decimate = (i+1 < argc && isdigit(argv[i+1][0])) ? atoi(argv[++i]) : 2;
        else if (!strcmp(argv[i], "-i"))
            maxiters = atoi(argv[++i]);
        else if (!strcmp(argv[i], "-r"))
            td->small_tag_refinement = 1;
        else if (!strcmp(argv[i], "-w"))
            waitsec = atoi(argv[++i]);
        else if (!strcmp(argv[i], "-b"))
            td->seg_sigma = atof(argv[++i]);
/*        else if (!strcmp(argv[i], "--family")) {
            char *fam = argv[++i];
            if (!strcmp(fam, "36h11"))
                td->tag_family = tag36h11_create();
            else if (!strcmp(fam, "36h10"))
                td->tag_family = tag36h10_create();
                } */
        else
            zarray_add(inputs, &argv[i]);
    }

    for (int iter = 0; iter < maxiters; iter++) {

        if (maxiters > 1)
            printf("iter %d / %d\n", iter + 1, maxiters);

        for (int input = 0; input < zarray_size(inputs); input++) {

            char *path;
            zarray_get(inputs, input, &path);
            printf("loading %s\n", path);
            image_u8_t *im = image_u8_create_from_pnm(path);
            if (im == NULL) {
                printf("couldn't find %s\n", path);
                continue;
            }

            zarray_t *detections = april_tag_detector_detect(td, im);

            for (int i = 0; i < zarray_size(detections); i++) {
                april_tag_detection_t *det;
                zarray_get(detections, i, &det);

                printf("detection %3d: id %4d, hamming %d, goodness %f\n", i, det->id, det->hamming, det->goodness);
                april_tag_detection_destroy(det);
            }

            zarray_destroy(detections);

            timeprofile_display(td->tp);
            printf("nedges: %d, nsegments: %d, nquads: %d\n", td->nedges, td->nsegments, td->nquads);

            image_u8_destroy(im);

            if (zarray_size(inputs) > 1 || iter > 0)
                sleep(waitsec);
        }
    }

    april_tag_detector_destroy(td);

    tag36h11_destroy(tf);
    return 0;
}
ImageTagManagerImpl::ImageTagManagerImpl()
	: m_tf(tag36h11_create())
	, m_event(false, false)
	, m_stopping(0)
{
}
Пример #7
0
double* getTag(char* path)
{

    apriltag_family_t *tf = NULL;
    tf = tag36h11_create();

    tf->black_border = 1;
    apriltag_detector_t *td = apriltag_detector_create();
    apriltag_detector_add_family(td, tf);
    td->quad_decimate = 1.0;
    td->quad_sigma = 0.0;
    td->nthreads = 4;
    td->debug = 0;
    td->refine_decode = 0;
    td->refine_pose = 0;
    int quiet = 0;
    int maxiters = 1;
    const int hamm_hist_max = 10;


    int hamm_hist[hamm_hist_max];
    memset(hamm_hist, 0, sizeof(hamm_hist));
    image_u8_t *im = image_u8_create_from_pnm(path);
    if (im == NULL) {
	    printf("couldn't find %s\n", path);
	    return NULL;
    }

    zarray_t *detections = apriltag_detector_detect(td, im);

     nrows = zarray_size(detections);
     ncols = 9;
    
     if (nrows == 0)
	return NULL;

    double* output_matrix = new double[nrows*ncols];

    for (int i = 0; i < zarray_size(detections); i++) {
	    apriltag_detection_t *det;
	    zarray_get(detections, i, &det);


	    output_matrix[ncols*i+0] = det->id;
	    for(int j=0; j<4; j++)
	    {
		output_matrix[ncols*i+ 2*j +1] = det->p[j][0];
		output_matrix[ncols*i+ 2*j +2] = det->p[j][1];
	    }
	    
	    hamm_hist[det->hamming]++;

	    apriltag_detection_destroy(det);
    }

    

    zarray_destroy(detections);
    image_u8_destroy(im);

    // don't deallocate contents of inputs; those are the argv
    apriltag_detector_destroy(td);

    tag36h11_destroy(tf);
    return output_matrix;
}
Пример #8
0
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);
}