text: #include "opencv2/opencv.hpp"
  
  #include "AprilTags/TagDetection.h"
  #include "AprilTags/MathUtil.h"
  
  #ifdef PLATFORM_APERIOS
  // missing/broken isnan
  namespace std {
  static bool isnan(float x) {
    const int EXP = 0x7f800000;
    const int FRAC = 0x007fffff;
    const int y = *((int *)(&x));
    return ((y & EXP) == EXP && (y & FRAC) != 0);
  }
  }
  #endif
  
  namespace AprilTags {
  
  TagDetection::TagDetection()
      : good(false),
        obsCode(),
        code(),
        id(),
        hammingDistance(),
        rotation(),
        p(),
        cxy(),
        observedPerimeter(),
        homography(),
        hxy() {
    homography.setZero();
  }
  
  TagDetection::TagDetection(int _id)
      : good(false),
        obsCode(),
        code(),
        id(_id),
        hammingDistance(),
        rotation(),
        p(),
        cxy(),
        observedPerimeter(),
        homography(),
        hxy() {
    homography.setZero();
  }
  
  float TagDetection::getXYOrientation() const {
    // Because the order of segments in a quad is arbitrary, so is the
    // homography's rotation, so we can't determine orientation directly
    // from the homography.  Instead, use the homography to find two
    // bottom corners of a properly oriented tag in pixel coordinates,
    // and then compute orientation from that.
    std::pair<float, float> p0 = interpolate(-1, -1);  // lower left corner of tag
    std::pair<float, float> p1 = interpolate(1, -1);  // lower right corner of tag
    float orient = atan2(p1.second - p0.second, p1.first - p0.first);
    return !std::isnan(float(orient)) ? orient : 0.;
  }
  
  std::pair<float, float> TagDetection::interpolate(float x, float y) const {
    float z = homography(2, 0) * x + homography(2, 1) * y + homography(2, 2);
    if (z == 0)
      return std::pair<float, float>(0, 0);  // prevents returning a pair with a
                                             // -NaN, for which gcc 4.4 flubs
                                             // isnan
    float newx =
        (homography(0, 0) * x + homography(0, 1) * y + homography(0, 2)) / z +
        hxy.first;
    float newy =
        (homography(1, 0) * x + homography(1, 1) * y + homography(1, 2)) / z +
        hxy.second;
    return std::pair<float, float>(newx, newy);
  }
  
  bool TagDetection::overlapsTooMuch(const TagDetection &other) const {
    // Compute a sort of "radius" of the two targets. We'll do this by
    // computing the average length of the edges of the quads (in
    // pixels).
    float radius =
        (MathUtil::distance2D(p[0], p[1]) + MathUtil::distance2D(p[1], p[2]) +
         MathUtil::distance2D(p[2], p[3]) + MathUtil::distance2D(p[3], p[0]) +
         MathUtil::distance2D(other.p[0], other.p[1]) +
         MathUtil::distance2D(other.p[1], other.p[2]) +
         MathUtil::distance2D(other.p[2], other.p[3]) +
         MathUtil::distance2D(other.p[3], other.p[0])) /
        16.0f;
  
    // distance (in pixels) between two tag centers
    float dist = MathUtil::distance2D(cxy, other.cxy);
  
    // reject pairs where the distance between centroids is smaller than
    // the "radius" of one of the tags.
    return (dist < radius);
  }
  
  Eigen::Matrix4d TagDetection::getRelativeTransform(double tag_size, double fx,
                                                     double fy, double px,
                                                     double py) const {
    std::vector<cv::Point3f> objPts;
    std::vector<cv::Point2f> imgPts;
    double s = tag_size / 2.;
    objPts.push_back(cv::Point3f(-s, -s, 0));
    objPts.push_back(cv::Point3f(s, -s, 0));
    objPts.push_back(cv::Point3f(s, s, 0));
    objPts.push_back(cv::Point3f(-s, s, 0));
  
    std::pair<float, float> p1 = p[0];
    std::pair<float, float> p2 = p[1];
    std::pair<float, float> p3 = p[2];
    std::pair<float, float> p4 = p[3];
    imgPts.push_back(cv::Point2f(p1.first, p1.second));
    imgPts.push_back(cv::Point2f(p2.first, p2.second));
    imgPts.push_back(cv::Point2f(p3.first, p3.second));
    imgPts.push_back(cv::Point2f(p4.first, p4.second));
  
    cv::Mat rvec, tvec;
    cv::Matx33f cameraMatrix(fx, 0, px, 0, fy, py, 0, 0, 1);
    cv::Vec4f distParam(0, 0, 0, 0);  // all 0?
    cv::solvePnP(objPts, imgPts, cameraMatrix, distParam, rvec, tvec);
    cv::Matx33d r;
    cv::Rodrigues(rvec, r);
    Eigen::Matrix3d wRo;
    wRo << r(0, 0), r(0, 1), r(0, 2), r(1, 0), r(1, 1), r(1, 2), r(2, 0), r(2, 1),
        r(2, 2);
  
    Eigen::Matrix4d T;
    T.topLeftCorner(3, 3) = wRo;
    T.col(3).head(3) << tvec.at<double>(0), tvec.at<double>(1),
        tvec.at<double>(2);
    T.row(3) << 0, 0, 0, 1;
  
    return T;
  }
  
  void TagDetection::getRelativeTranslationRotation(double tag_size, double fx,
                                                    double fy, double px,
                                                    double py,
                                                    Eigen::Vector3d &trans,
                                                    Eigen::Matrix3d &rot) const {
    Eigen::Matrix4d T = getRelativeTransform(tag_size, fx, fy, px, py);
  
    // converting from camera frame (z forward, x right, y down) to
    // object frame (x forward, y left, z up)
    Eigen::Matrix4d M;
    M << 0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 0, 1;
    Eigen::Matrix4d MT = M * T;
    // translation vector from camera to the April tag
    trans = MT.col(3).head(3);
    // orientation of April tag with respect to camera: the camera
    // convention makes more sense here, because yaw,pitch,roll then
    // naturally agree with the orientation of the object
    rot = T.block(0, 0, 3, 3);
  }
  
  // draw one April tag detection on actual image
  void TagDetection::draw(cv::Mat &image) const {
    // use corner points detected by line intersection
    std::pair<float, float> p1 = p[0];
    std::pair<float, float> p2 = p[1];
    std::pair<float, float> p3 = p[2];
    std::pair<float, float> p4 = p[3];
  
    // plot outline
    cv::line(image, cv::Point2f(p1.first, p1.second),
             cv::Point2f(p2.first, p2.second), cv::Scalar(255, 0, 0, 0));
    cv::line(image, cv::Point2f(p2.first, p2.second),
             cv::Point2f(p3.first, p3.second), cv::Scalar(0, 255, 0, 0));
    cv::line(image, cv::Point2f(p3.first, p3.second),
             cv::Point2f(p4.first, p4.second), cv::Scalar(0, 0, 255, 0));
    cv::line(image, cv::Point2f(p4.first, p4.second),
             cv::Point2f(p1.first, p1.second), cv::Scalar(255, 0, 255, 0));
  
    // mark center
    // cv::circle(image, cv::Point2f(cxy.first, cxy.second), 8,
    // cv::Scalar(0,0,255,0), 2);
  
    // print ID
    std::ostringstream strSt;
    strSt << id;
    cv::putText(image, strSt.str(), cv::Point2f(cxy.first - 5, cxy.second + 5),
                cv::FONT_HERSHEY_PLAIN, 1, cv::Scalar(0, 0, 255), 2);
  }
  
  }  // namespace
  