/* ************************************************************************* */
CalibratedCamera CalibratedCamera::Level(const Pose2& pose2, double height) {
	double st = sin(pose2.theta()), ct = cos(pose2.theta());
	Point3 x(st, -ct, 0), y(0, 0, -1), z(ct, st, 0);
	Rot3 wRc(x, y, z);
	Point3 t(pose2.x(), pose2.y(), height);
	Pose3 pose3(wRc, t);
	return CalibratedCamera(pose3);
}
Beispiel #2
0
void OnMenuAbout()
{
    if (gHwndAbout) {
        SetActiveWindow(gHwndAbout);
        return;
    }

    if (!gAtomAbout) {
        WNDCLASSEX  wcex;
        FillWndClassEx(wcex, ghinst, ABOUT_CLASS_NAME, WndProcAbout);
        wcex.hIcon = LoadIcon(ghinst, MAKEINTRESOURCE(IDI_SUMATRAPDF));
        gAtomAbout = RegisterClassEx(&wcex);
        CrashIf(!gAtomAbout);
    }

    gHwndAbout = CreateWindow(
            ABOUT_CLASS_NAME, ABOUT_WIN_TITLE,
            WS_OVERLAPPED | WS_CAPTION | WS_SYSMENU,
            CW_USEDEFAULT, CW_USEDEFAULT,
            CW_USEDEFAULT, CW_USEDEFAULT,
            NULL, NULL,
            ghinst, NULL);
    if (!gHwndAbout)
        return;

    // get the dimensions required for the about box's content
    RectI rc;
    PAINTSTRUCT ps;
    HDC hdc = BeginPaint(gHwndAbout, &ps);
    UpdateAboutLayoutInfo(gHwndAbout, hdc, &rc);
    EndPaint(gHwndAbout, &ps);
    rc.Inflate(ABOUT_RECT_PADDING, ABOUT_RECT_PADDING);

    // resize the new window to just match these dimensions
    WindowRect wRc(gHwndAbout);
    ClientRect cRc(gHwndAbout);
    wRc.dx += rc.dx - cRc.dx;
    wRc.dy += rc.dy - cRc.dy;
    MoveWindow(gHwndAbout, wRc.x, wRc.y, wRc.dx, wRc.dy, FALSE);

    ShowWindow(gHwndAbout, SW_SHOW);
}
Beispiel #3
0
static bool CreatePropertiesWindow(HWND hParent, PropertiesLayout* layoutData)
{
    CrashIf(layoutData->hwnd);
    HWND hwnd = CreateWindow(
           PROPERTIES_CLASS_NAME, PROPERTIES_WIN_TITLE,
           WS_OVERLAPPED | WS_CAPTION | WS_SYSMENU,
           CW_USEDEFAULT, CW_USEDEFAULT,
           CW_USEDEFAULT, CW_USEDEFAULT,
           NULL, NULL,
           ghinst, NULL);
    if (!hwnd)
        return false;

    layoutData->hwnd = hwnd;
    layoutData->hwndParent = hParent;
    ToggleWindowStyle(hwnd, WS_EX_LAYOUTRTL | WS_EX_NOINHERITLAYOUT, IsUIRightToLeft(), GWL_EXSTYLE);

    // get the dimensions required for the about box's content
    RectI rc;
    PAINTSTRUCT ps;
    HDC hdc = BeginPaint(hwnd, &ps);
    UpdatePropertiesLayout(layoutData, hdc, &rc);
    EndPaint(hwnd, &ps);

    // resize the new window to just match these dimensions
    // (as long as they fit into the current monitor's work area)
    WindowRect wRc(hwnd);
    ClientRect cRc(hwnd);
    RectI work = GetWorkAreaRect(WindowRect(hParent));
    wRc.dx = min(rc.dx + wRc.dx - cRc.dx, work.dx);
    wRc.dy = min(rc.dy + wRc.dy - cRc.dy, work.dy);
    MoveWindow(hwnd, wRc.x, wRc.y, wRc.dx, wRc.dy, FALSE);
    CenterDialog(hwnd, hParent);

    ShowWindow(hwnd, SW_SHOW);
    return true;
}
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);
}