beamerProjection.cpp

#include "../../inc/beamerProjection.h"


BeamerProjection::BeamerProjection(){
    adjustingMatrix = cv::getRotationMatrix2D(cv::Point(0, 0), 0, 1);
}


cv::Point2i BeamerProjection::rotatePixel(cv::Point2i pixel){

    cv::Mat tmp = (cv::Mat_<cv::Vec2f>(1, 1) << cv::Vec2f(pixel.x, pixel.y));
    cv::transform(tmp, tmp, adjustingMatrix);
    return cv::Point2i(tmp.at<cv::Vec2f>(0, 0));
}


cv::Point2i BeamerProjection::adjustPixel(int i, int j, float z, Camera *camera, cv::Point3f beamer_pos){

    //pixel to point 3d
    float coord[2] = {static_cast<float>(j), static_cast<float>(i)};
    cv::Point3f p = camera->deprojectPixelToPoint(coord, z / 1000.0);
    //calcul vector point to beamer
    cv::Vec3f dir(beamer_pos.x - p.x, beamer_pos.y - p.y, beamer_pos.z - p.z);
    float prop = (0.99 - p.z) / dir[2];
    p.x += dir[0] * prop;
    p.y += dir[1] * prop;
    p.z += dir[2] * prop;
    //point 3d to pixel
    return camera->projectPointToPixel(p);
}


void BeamerProjection::adjustFrame(cv::Mat &depth, cv::Mat &src, cv::Mat &dst, Camera *camera, cv::Point3f beamer_pos){

    int nbChannel = src.channels();
    //transformation on all pixel
    //int64_t t1 = getTickCount();
    for (int i = 0; i < src.rows; i++)
    {
        for (int j = 0; j < src.cols; j++)
        {
            cv::Point pixelIJ(j, i);
            
            cv::Point pixel = adjustPixel(i, j, static_cast<float>(depth.at<uint16_t>(pixelIJ)), camera, beamer_pos);
            if (pixel.x < dst.cols && pixel.y < dst.rows && pixel.x >= 0 && pixel.y >= 0)
            {
                if (nbChannel == 1)
                    dst.at<char>(pixel) = src.at<char>(pixelIJ);
                else if (nbChannel == 3)
                    dst.at<cv::Vec3b>(pixel) = src.at<cv::Vec3b>(pixelIJ);
            }
        }
    }
    //cout << "temps de calcul: " << (getTickCount() - t1) / getTickFrequency() << endl;
    cv::warpAffine(dst, dst, adjustingMatrix, dst.size());
    cv::dilate(dst, dst, cv::Mat(), cv::Point(-1, -1), 2, 1, 1);
    cv::erode(dst, dst, cv::Mat(), cv::Point(-1, -1), 2, 1, 1);
}


void BeamerProjection::printAdjustingMatrix(){

    cv::Mat matrix = getAdjustingMatrix();
    for (int y = 0; y < matrix.rows; y++){
        for (int x = 0; x < matrix.cols; x++){
            std::cout << (float)matrix.at<double>(y, x) << " ";
        }
        std::cout << std::endl;
    }
}