Skip to content
Snippets Groups Projects
Select Git revision
  • bec861d6fcb2623e4b261bf2a574092d1721c772
  • master default protected
  • opencv4
  • custom_realsense
  • deproject
  • camera
6 results

beamer.cpp

Blame
    • beamer.cpp 7.56 KiB 
    #include "../../inc/beamer.h"

/*
*   Main
*/


Beamer::Beamer(){
    beamerPosition = cv::Point3f(0.0f, 0.265f, -0.205f);
    resolution = cv::Size(256,144);
    profil = new FrameProcessProfil();
}

Beamer::~Beamer(){
    delete profil;
}




/*
*   Setup config purpose
*/


std::vector<cv::Point2i> Beamer::getCrossList(){

    std::vector<cv::Point2i> points;

    points.push_back( cv::Point2i( getWidth() * 2/10.0f, getHeight() * 2/10.0f) );
    points.push_back( cv::Point2i( getWidth() * 8/10.0f, getHeight() * 5/10.0f) );
    points.push_back( cv::Point2i( getWidth() * 4/10.0f, getHeight() * 8/10.0f) );

    return points;
}


cv::Mat Beamer::editContrast(cv::Mat image, double contrast, int brightness){

    cv::Mat new_image = cv::Mat::zeros( image.size(), image.type() );
    double alpha = contrast;
    int beta = brightness;

    // source : https://docs.opencv.org/2.4/doc/tutorials/core/basic_linear_transform/basic_linear_transform.html
    for( int y = 0; y < image.rows; y++ ) {
        for( int x = 0; x < image.cols; x++ ) {
            new_image.at<uchar>(y,x) = cv::saturate_cast<uchar>( alpha*image.at<uchar>(y,x) + beta );
        }
    }

    return new_image;
}

// TODO : modifier pour avoir une matrice gray_scale en param à la place de rgb
std::vector<cv::Point3i> Beamer::findCircles(cv::Mat &rgb, double contrast, int brightness, double centersMinDist, int cannyEdgeThreshold, int houghAccThreshold, double minRadius, double maxRadius){

    cv::Mat src_gray;
    std::vector<cv::Vec3f> circles;
    circles.clear();

    cv::cvtColor(rgb, src_gray, CV_BGR2GRAY);
    /// Reduce the noise so we avoid false circle detection
    cv::GaussianBlur(src_gray, src_gray, cv::Size(9, 9), 2, 2);
    src_gray = editContrast(src_gray, (double)contrast, (double)brightness);
    
    /// 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
    // dp           : Inverse resolution for the accumulator matrixe => image_resolution * dp = acc_resolution
    // min_dist     : Minimal distance between the detected centers
    // param_1      : Upper threshold of the canny edge detector, determines if a pixel is an edge
        // param_2      : Threshold for the center detection, after the accumulator is complet, threshold to keep only the possible centers
    // min_radius   : Min radius of the circles drawn on the accumulator
    // max_radius   : Max radius of the circles drawn on the accumulator
    cv::HoughCircles(src_gray, circles, CV_HOUGH_GRADIENT, 1, centersMinDist, (double)cannyEdgeThreshold, (double)houghAccThreshold, minRadius, maxRadius);

    std::vector<cv::Point3i> result;
    if (!circles.empty())
    {
        for(cv::Vec3f circle: circles){
            result.push_back( cv::Point3i(round(circle[0]), round(circle[1]), round(circle[2])) );
        }
    }
    return result;
}


cv::Mat Beamer::buildCrossFrame(cv::Point projectedCross, int step, int max, bool circlesFound){

    cv::Mat frameImage(resolution, CV_8UC3, cv::Scalar(0, 0, 0));
    cv::Scalar red = cv::Scalar(0, 0, 255);
    cv::Scalar color = (circlesFound) ? cv::Scalar(0, 180, 0) : cv::Scalar(0, 0, 255);

    cv::line(frameImage, cv::Point(projectedCross.x, 0), cv::Point(projectedCross.x, frameImage.rows - 1), red, 4);
    cv::line(frameImage, cv::Point(0, projectedCross.y), cv::Point(frameImage.cols - 1, projectedCross.y), red, 4);

    // draw the little red/green square at the center of the frame
    cv::rectangle( frameImage,
                   cv::Point(getWidth()/2 -30, getHeight()/2 -20),
                   cv::Point(getWidth()/2 -10, getHeight()/2),
                   color, cv::FILLED, 8, 0 );
    
    // draw the steps
    cv::putText( frameImage,
                 std::to_string(step) + "/" + std::to_string(max),
                 cv::Point( getWidth()/2, getHeight()/2 ),
                 cv::FONT_HERSHEY_SIMPLEX,
                 1,
                 cv::Scalar(255, 255, 255));

    return frameImage;
}


void Beamer::findLinearLine(std::vector<cv::Point3f> *capturedPoints, std::vector<cv::Point3d> *bases, std::vector<cv::Point3d> *directions){
    
    cv::Vec6f line;
    cv::fitLine(*capturedPoints, line, CV_DIST_L2, 0, 0.01, 0.01);

    cv::Point3d direction = cv::Point3d(line[0], line[1], line[2]);
    cv::Point3d base = cv::Point3d(line[3], line[4], line[5]);
    
    bases->push_back(base);
    directions->push_back(direction);
}



/*
   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 Beamer::LineLineIntersect(
    cv::Point3d p1, cv::Point3d p2, cv::Point3d p3, cv::Point3d p4, cv::Point3d *pa, cv::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);
}


/*
* Approxime la position du beamer en 3D en ce basant sur 3 droites
*/
cv::Point3d Beamer::approximatePosition(std::vector<cv::Point3d> *bases, std::vector<cv::Point3d> *directions){

    cv::Point3d pa, pb;
    double mua;
    double mub;
    std::vector<cv::Point3d> beamerPoints;
    int selected[3][2] = { {0,1}, {0,2}, {1,2} };

    for(int i=0; i < 3; i++){
        LineLineIntersect( bases->at(selected[i][0]),
                           bases->at(selected[i][0]) + directions->at(selected[i][0]),
                           bases->at(selected[i][1]),
                           bases->at(selected[i][1]) + directions->at(selected[i][1]),
                           &pa, &pb, &mua, &mub);
        beamerPoints.push_back(pa);
        beamerPoints.push_back(pb);
    }
    
    cv::Point3d beamerPoint(0.0, 0.0, 0.0);
    for (unsigned int i = 0; i < beamerPoints.size(); i++){
        beamerPoint += beamerPoints[i];
    }
    beamerPoint /= (double)beamerPoints.size();
    
    return beamerPoint;
}


cv::Point3f Beamer::deprojectPixel(cv::Point2i circle, cv::Mat *depth, Camera *camera){
    float coord[2] = {(float)circle.x, (float)circle.y};
    float z = depth->at<float>(circle.y, circle.x);
    return camera->deprojectPixelToPoint(coord, z);
}



/*
*   Debug
*/

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