adjustFrame can project frame of higher resolution

inc/beamerProjection.h

@@ -8,17 +8,20 @@
 class BeamerProjection{
     private:
         cv::Mat adjustingMatrix;
+        float topSandboxDistance = 1.0f;
 
-        cv::Point2i adjustPixel(int i, int j, float z, Camera *camera, cv::Point3f beamer_pos);
+        cv::Point2i findMatchingPixel(int i, int j, float z, Camera *camera, cv::Point3f beamer_pos);
+        void copyPixelsInto(cv::Point2i pixel_dst, cv::Mat &dst, cv::Point2i pixel_src, cv::Mat &src, cv::Rect base);
+        cv::Size getMatchingSize(cv::Mat &src, cv::Rect base);
 
     public:
         BeamerProjection();
 
-        void setAdjustingMatrix(cv::Mat matrix){ adjustingMatrix.setTo(matrix); }
+        void setAdjustingMatrix(cv::Mat matrix){ adjustingMatrix = matrix; }
         cv::Mat getAdjustingMatrix(){ return adjustingMatrix.clone(); }
 
         cv::Point2i rotatePixel(cv::Point2i pixel);
-        void adjustFrame(cv::Mat &depth, cv::Mat &src, cv::Mat &dst, Camera *camera, cv::Point3f beamer_pos);
+        void adjustFrame(cv::Mat depth, cv::Mat src, cv::Mat &dst, Camera *camera, cv::Point3f beamer_pos);
         void printAdjustingMatrix();
 
 };

inc/camera.h

@@ -25,7 +25,12 @@ class Camera{
         Camera();
         ~Camera();
 
-        cv::Mat getDepthFrame(){ return cv::Mat(cv::Size(depth_frame->get_width(), depth_frame->get_height()), CV_16UC1, (void *)depth_frame->get_data(), cv::Mat::AUTO_STEP); };
+        cv::Mat getDepthFrame(){
+            cv::Mat meters;
+            cv::Mat values = cv::Mat(cv::Size(depth_frame->get_width(), depth_frame->get_height()), CV_16UC1, (void *)depth_frame->get_data(), cv::Mat::AUTO_STEP);
+            values.convertTo(meters,CV_32FC1, depth_frame->get_units());
+            return meters;
+        };
         cv::Mat getColorFrame(){ return cv::Mat(cv::Size(color_frame->get_width(), color_frame->get_height()), CV_8UC3, (void *)color_frame->get_data(), cv::Mat::AUTO_STEP); };
         void setCroppingMask(cv::Rect mask){ croppingMask = mask; };
         cv::Rect getCroppingMask(){ return croppingMask; };

inc/sandbox.h

@@ -23,7 +23,7 @@ class Sandbox{
 
         cv::Mat getColorFrame();
         cv::Mat getDepthFrame();
-        void adjustProjection(cv::Mat &frame);
+        cv::Mat adjustProjection(cv::Mat frame);
         void showImage(cv::Mat &image);
         int loadConfig();
         int loadConfigFrom(char *path);

src/components/beamerProjection.cpp

@@ -2,7 +2,7 @@
 
 
 BeamerProjection::BeamerProjection(){
-    adjustingMatrix = cv::getRotationMatrix2D(cv::Point(0, 0), 0, 1);
+    //adjustingMatrix = cv::getRotationMatrix2D(cv::Point(0, 0), 0, 1);
 }
 
 
@@ -14,47 +14,100 @@ cv::Point2i BeamerProjection::rotatePixel(cv::Point2i pixel){
 }
 
 
-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){
+
+    cv::Rect mask = camera->getCroppingMask();
+    cv::Size dst_size = dst.size();
+    cv::resize(src, src, dst.size());
+
+    // resize the frames to be a multiple of the camera size :
+    //      src.size = n * camera.depth.size , where n is uint > 0
+    cv::resize(src, src, getMatchingSize(src, mask));
+    cv::resize(dst, dst, src.size());
+    //src.copyTo(dst);
+
+    // Browse the depth frame matching the cropping mask
+    // while adapting pixels's position to the beamer's position
+    for (int j = 0; j < mask.height; j++){
+        for (int i = 0; i < mask.width; i++){
+
+            // coordinates of the pixel relative to the orginial image taken from the camera
+            int base_x = mask.x+i;
+            int base_y = mask.y+j;
+            float z = depth.at<float>(j,i);
+            
+            // pixels based on the original depth frame taken from the camera
+            cv::Point2i pixel = findMatchingPixel( base_x, base_y, z, camera, beamer_pos );
             
+            // pixel relative to the cropping mask
+            pixel.x -= mask.x;
+            pixel.y -= mask.y;
 
-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);
+            if( (0<=pixel.x && pixel.x<mask.width) && (0<=pixel.y && pixel.y<mask.height) ){
+                // src and dst must be of same size
+                copyPixelsInto(pixel, dst, cv::Point2i(i,j), src, mask);
             }
         }
     }
-    //cout << "temps de calcul: " << (getTickCount() - t1) / getTickFrequency() << endl;
+
+    cv::resize(src, src, dst_size);
     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);
+}
+
+cv::Size BeamerProjection::getMatchingSize(cv::Mat &src, cv::Rect base){
+    cv::Size bigSize;
+    bigSize.width = (src.size().width % base.width == 0) ? src.size().width : src.size().width - (src.size().width % base.width) + base.width;
+    bigSize.height = (src.size().height % base.height == 0) ? src.size().height : src.size().height - (src.size().height % base.height) + base.height;
+    return bigSize;
+}
+
+// pixels coordinates are relative to the camera depth frame
+void BeamerProjection::copyPixelsInto(cv::Point2i pixel_dst, cv::Mat &dst, cv::Point2i pixel_src, cv::Mat &src, cv::Rect base){
+
+    if( src.size().width == dst.size().width && src.size().height == dst.size().height ){
+
+        // determines the size of the square of pixels to copy from the source matching the position of the pixel of the depth frame
+        // (since src and dst are multiple of the depth frame's size)
+        int ratio_w = src.size().width / base.width;
+        int ratio_h = src.size().height / base.height;
+
+        for(int j=0; j<ratio_h; j++){
+            for (int i=0; i<ratio_w; i++){
+
+                int x_dst = pixel_dst.x * ratio_w + i;
+                int y_dst = pixel_dst.y * ratio_h + j;
+                int x_src = pixel_src.x * ratio_w + i;
+                int y_src = pixel_src.y * ratio_h + j;
+
+                dst.at<cv::Vec3b>(y_dst, x_dst) = src.at<cv::Vec3b>(y_src, x_src);
+            }
+        }
+    }
+}
+
+/*
+    C : Camera position
+    B : Beamer position
+    P : Point computed by camera depth
+    
+    Where
+        CP : distance from camera to point (value of depth_frame)
+        CB : distance from camera to beamer (beamer's position is relative to the camera)
+*/
+cv::Point2i BeamerProjection::findMatchingPixel(int i, int j, float z, Camera *camera, cv::Point3f CB){
+   
+    float pixel[2] = {static_cast<float>(i), static_cast<float>(j)};
+    cv::Point3f CP = camera->deprojectPixelToPoint(pixel, z);
+    cv::Point3f BP = CB - CP;
+    //float alpha = (topSandboxDistance - CP.z) / BP.z;
+    float BAz = CB.z + BP.z;
+    float BEz = topSandboxDistance - CB.z;
+    float alpha = BEz / BAz;
+    cv::Point3f V = (alpha * BP);
+    cv::Point3f CV = V + CB;
+
+    return camera->projectPointToPixel(CV);
 }
 
 

src/components/camera.cpp

@@ -89,7 +89,7 @@ void Camera::capture(){
     }
 }
 
-// match a pixel of depth frame to color frame
+// get the coordinates of the pixel matching the point relative to the real world
 cv::Point2i Camera::projectPointToPixel(cv::Point3f point3D){
 
     float point[3] = {point3D.x, point3D.y, point3D.z};
@@ -99,12 +99,12 @@ cv::Point2i Camera::projectPointToPixel(cv::Point3f point3D){
     return cv::Point2i(pixel[0], pixel[1]);
 }
 
-// match a pixel of color frame to depth frame
-cv::Point3f Camera::deprojectPixelToPoint(float coord[], float z1){
+// get  the point relative to the real world matching the coordinates of the pixel
+cv::Point3f Camera::deprojectPixelToPoint(float coord[], float z){
 
     float p[3];
     rs2_intrinsics intr = depth_frame->get_profile().as<rs2::video_stream_profile>().get_intrinsics();
-    rs2_deproject_pixel_to_point(p, &intr, coord, z1);
+    rs2_deproject_pixel_to_point(p, &intr, coord, z);
     return cv::Point3f(p[0], p[1], p[2]);
 }
 

src/lib/sandbox.cpp

@@ -16,51 +16,33 @@ Sandbox::Sandbox(){
 
 cv::Mat Sandbox::getColorFrame(){
     camera.capture();
-    return camera.getColorFrame();
+    return camera.getColorFrame()(camera.getCroppingMask());
 }
 
 cv::Mat Sandbox::getDepthFrame(){
     camera.capture();
-    return camera.getDepthFrame();
+    return camera.getDepthFrame()(camera.getCroppingMask());
 }
 
 
-void Sandbox::adjustProjection(cv::Mat &frame){
-
-    static cv::Mat frameBeamer = cv::Mat(cv::Size(beamer.getWidth(), beamer.getHeight()), CV_8UC3);
+cv::Mat Sandbox::adjustProjection(cv::Mat frame){
     
     cv::Mat depth = getDepthFrame();
-    cv::Mat imageCalibrate = cv::Mat(depth.size(), CV_8UC3, cv::Scalar(0, 0, 0));
-    
-    cv::resize(frame, frame, depth.size());
-
+    cv::Mat imageCalibrate = cv::Mat(cv::Size(beamer.getWidth(), beamer.getHeight()), CV_8UC3, cv::Scalar(0, 0, 0));
     projection.adjustFrame(depth, frame, imageCalibrate, &camera, beamer.getPosition());
-    //flip to align frame with beamer
-    //cv::flip(imageCalibrate, imageCalibrate, 1);
-    //cv::flip(imageCalibrate, imageCalibrate, 0);
-    cv::resize(imageCalibrate(camera.getCroppingMask()), frameBeamer, frameBeamer.size());
-    frameBeamer.copyTo(frame);
 
-/*
-    cv::Mat image = *frame;
-    cv::Mat depth = getDepthFrame();
-    cv::Size beamerResolution = cv::Size(beamer.getWidth(), beamer.getHeight());
-    cv::Mat imageCalibrate(beamerResolution, CV_8UC3, cv::Scalar(0, 0, 0));
-    
-    cv::resize(depth, depth, beamerResolution);
-    cv::resize(image, image, beamerResolution);
-    projection.adjustFrame(depth, image, imageCalibrate, camera, beamer.getPosition());
+    // frame after process
+    cv::dilate(imageCalibrate, imageCalibrate, cv::Mat(), cv::Point(-1, -1), 2, 1, 1);
+    cv::erode(imageCalibrate, imageCalibrate, cv::Mat(), cv::Point(-1, -1), 2, 1, 1);
     
-    imageCalibrate.copyTo(*frame);
-    return frame;
-*/
+    return imageCalibrate;
 }
 
 void Sandbox::showImage(cv::Mat &image){
 
     initWindowsFullScreen(defaultWindowsName);
-    adjustProjection(image);
-    cv::imshow(defaultWindowsName, image);
+    cv::Mat res = adjustProjection(image);
+    cv::imshow(defaultWindowsName, res);
 }
 
 int Sandbox::loadConfigFrom(char *path){

src/tools/sandboxConfig.cpp

@@ -162,11 +162,16 @@ int SandboxConfig::loadAdjustingMatrixFrom(char *path, BeamerProjection *project
         for (int j=0; j<width; j++){
             matrix.at<float>(i,j) = values.at(v_i);
             v_i++;
+            std::cout << matrix.at<float>(i,j) << " ";
         }
+        std::cout << std::endl;
     }
     
+    
+    std::cout << "load matrix" << std::endl;
     projection->setAdjustingMatrix(matrix);
 
+    std::cout << "Ok matrix" << std::endl;
     return 0;
 }