beamer.cpp

#include "../../inc/beamer.h"

using namespace cv;
using namespace std;
#define ESCAPE_CHAR 27

/*
   Calculate the line segment PaPb that is the shortest route between
   two lines P1P2 and P3P4. Calculate also the values of mua and mub where
      Pa = P1 + mua (P2 - P1)
      Pb = P3 + mub (P4 - P3)
   Return FALSE if no solution exists.
   http://paulbourke.net/geometry/pointlineplane/
*/

typedef struct
{
    double x, y, z;
} XYZ;

int LineLineIntersect(
    Point3d p1, Point3d p2, Point3d p3, Point3d p4, Point3d *pa, Point3d *pb,
    double *mua, double *mub)
{

    float EPS = 0.0001;
    XYZ p13, p43, p21;
    double d1343, d4321, d1321, d4343, d2121;
    double numer, denom;

    p13.x = p1.x - p3.x;
    p13.y = p1.y - p3.y;
    p13.z = p1.z - p3.z;
    p43.x = p4.x - p3.x;
    p43.y = p4.y - p3.y;
    p43.z = p4.z - p3.z;
    if (abs(p43.x) < EPS && abs(p43.y) < EPS && abs(p43.z) < EPS)
        return (false);
    p21.x = p2.x - p1.x;
    p21.y = p2.y - p1.y;
    p21.z = p2.z - p1.z;
    if (abs(p21.x) < EPS && abs(p21.y) < EPS && abs(p21.z) < EPS)
        return (true);

    d1343 = p13.x * p43.x + p13.y * p43.y + p13.z * p43.z;
    d4321 = p43.x * p21.x + p43.y * p21.y + p43.z * p21.z;
    d1321 = p13.x * p21.x + p13.y * p21.y + p13.z * p21.z;
    d4343 = p43.x * p43.x + p43.y * p43.y + p43.z * p43.z;
    d2121 = p21.x * p21.x + p21.y * p21.y + p21.z * p21.z;

    denom = d2121 * d4343 - d4321 * d4321;
    if (abs(denom) < EPS)
        return (false);
    numer = d1343 * d4321 - d1321 * d4343;

    *mua = numer / denom;
    *mub = (d1343 + d4321 * (*mua)) / d4343;

    pa->x = p1.x + *mua * p21.x;
    pa->y = p1.y + *mua * p21.y;
    pa->z = p1.z + *mua * p21.z;
    pb->x = p3.x + *mub * p43.x;
    pb->y = p3.y + *mub * p43.y;
    pb->z = p3.z + *mub * p43.z;

    return (true);
}

Beamer::Beamer(){
    //position par défaut
    beamerPosition = Point3f(0.0f, 0.265f, -0.205f);

}

vector<int> Beamer::findCercleZ(Mat &rgb)
{
    Mat src_gray;
    cvtColor(rgb, src_gray, CV_BGR2GRAY);
    /// Reduce the noise so we avoid false circle detection
    GaussianBlur(src_gray, src_gray, Size(9, 9), 2, 2);
    vector<Vec3f> circles;
    circles.clear();
    /// Apply the Hough Transform to find the circles
    //source, output, method, inverse ratio of resolution, Minimum distance between detected centers, threeshold canny, threeshold center, min radius, max radius
    HoughCircles(src_gray, circles, CV_HOUGH_GRADIENT, 1, src_gray.rows / 4, 75, 50, 0, 0);
    //doit tester si le cercle est bon (rayon);

    vector<int> result;
    if (!circles.empty())
    {
        for (int i = 0; i < 3; i++)
        {
            result.push_back(round(circles[0][i]));
        }
    }
    return result;
}

void Beamer::findBeamerFrom(Camera camera)
{
    char wname[] = "FindBeamer";
    namedWindow(wname, CV_WINDOW_NORMAL);
    setWindowProperty(wname, CV_WND_PROP_FULLSCREEN, CV_WINDOW_FULLSCREEN);
    Mat depth;
    Mat rgb;
    Mat frameImage(Size(1400, 1050), CV_8UC3, Scalar(0, 0, 0));
    vector<Point> points;
    points.push_back(Point(500, 500));
    points.push_back(Point(1000, 300));
    points.push_back(Point(300, 800));
    unsigned int nbPoint = 3; //number of point to calculate 1 vector
    vector<Point3d> points1;  //vectors calculate for each point
    vector<Point3d> points2;  //1 point for each vector (to calculate constante d)
    double fact = -20.0;


    camera.startAlign();

    for (int i = 0; i < (int)points.size(); i++)
    {
        vector<Point3f> capturedPoints;
        Point p = points[i];
        while (1)
        {
            camera.captureFramesAlign();
            depth = camera.getDepthFrameAlign();
            rgb = camera.getRGBFrameAlign();
            Scalar color;
            vector<int> crc = findCercleZ(rgb);
            if (!crc.empty())
                color = Scalar(0, 255, 0);
            else
                color = Scalar(0, 0, 255);

            line(frameImage, Point(p.x, 0), Point(p.x, frameImage.rows - 1), color, 4);
            line(frameImage, Point(0, p.y), Point(frameImage.cols - 1, p.y), color, 4);
            putText(frameImage, to_string(capturedPoints.size() + 1) + "/" + to_string(nbPoint), Point(400, 400), FONT_HERSHEY_SIMPLEX, 1, Scalar(255, 255, 255));
            cv::imshow(wname, frameImage);
            char keyCode = waitKey(500);
            if (keyCode == ESCAPE_CHAR)
                exit(0);
            else if (keyCode == ' ' && !crc.empty())
            {
                float coord[2] = {(float)crc[0], (float)crc[1]};
                float z = static_cast<float>(depth.at<uint16_t>(crc[1], crc[0]));
                Point3f p = camera.deprojectPixelToPoint(coord, z / 1000.0);
                capturedPoints.push_back(p);
            }

            if (capturedPoints.size() == nbPoint)
            {
                Vec3f dir(capturedPoints[0].x - capturedPoints[1].x, capturedPoints[0].y - capturedPoints[1].y, capturedPoints[0].z - capturedPoints[1].z);
                Vec6f line;
                fitLine(capturedPoints, line, CV_DIST_L2, 0, 0.01, 0.01);
                points1.push_back(Point3d(line[3] * fact, line[4] * fact, line[5] * fact));
                points2.push_back(Point3d(line[0], line[1], line[2]));
                frameImage.setTo(Scalar(0, 0, 0));
                break;
            }
            frameImage.setTo(Scalar(0, 0, 0));
        }
    }

    camera.stop();

    Point3d pa, pb;
    double mua;
    double mub;
    vector<Point3d> beamerPoints;
    LineLineIntersect(points1[0], points2[0], points1[1], points2[1], &pa, &pb, &mua, &mub);
    beamerPoints.push_back(pa);
    beamerPoints.push_back(pb);
    LineLineIntersect(points1[0], points2[0], points1[2], points2[2], &pa, &pb, &mua, &mub);
    beamerPoints.push_back(pa);
    beamerPoints.push_back(pb);
    LineLineIntersect(points1[1], points2[1], points1[2], points2[2], &pa, &pb, &mua, &mub);
    beamerPoints.push_back(pa);
    beamerPoints.push_back(pb);

    Point3d beamerPoint(0.0, 0.0, 0.0);
    for (unsigned int i = 0; i < beamerPoints.size(); i++)
    {
        beamerPoint += beamerPoints[i];
    }
    beamerPoint /= 6.0;

    //set beamer position
    beamerPosition.x = (float)beamerPoint.x;
    beamerPosition.y = (float)beamerPoint.y;
    beamerPosition.z = (float)beamerPoint.z;
  
    destroyAllWindows();
}


void Beamer::printPosition(){
    cv::Point3f pos = getPosition();
    std::cout << "(" << pos.x << "," << pos.y << "," << pos.z << ")" << std::endl;
}