idea to manipulate matrixes without for loops

inc/camera.h

@@ -42,6 +42,10 @@ class Camera{
         void stop();
         cv::Point3f deprojectPixelToPoint(float coord[], float z1);
         cv::Point2i projectPointToPixel(cv::Point3f point3D);
+
+        void deprojectPixelToPoint(cv::Mat_<float> &depth, cv::Mat_<cv::Point3f> &real);
+        void projectPointToPixel(cv::Mat_<cv::Point3f> &real, cv::Mat_<cv::Point2i> &pixels);
+
         void capture();
         void printCroppingMask();
 

inc/projection.h

@@ -13,9 +13,11 @@ class Projection{
         void deprojectPixelsFromDepth(cv::Mat_<float> &depth, cv::Rect mask, Camera *camera, cv::Point3f beamer_pos, cv::Mat_<cv::Point2i> &deprojectMap);
         void filterLowestDeprojectedPoints(cv::Mat_<float> &depth, cv::Mat_<cv::Point2i> &deprojectMap, cv::Mat_<cv::Point2i> &frameMapMask);
         void buildFrame(cv::Mat_<float> &depth, cv::Mat_<cv::Point2i> &frameMapMask, cv::Mat_<cv::Vec3b> &src, cv::Mat_<cv::Vec3b> &dst);
+        void buildFrame2(cv::Mat_<cv::Point2i> &frameMapMask, cv::Mat_<cv::Vec3b> &src, cv::Mat_<cv::Vec3b> &dst);
         cv::Point2i findMatchingPixel(int i, int j, float z, Camera *camera, cv::Point3f beamer_pos);
         void copyPixelsInto(cv::Point2i pixel_dst, cv::Mat_<cv::Vec3b> &dst, cv::Point2i pixel_src, cv::Mat_<cv::Vec3b> &src, cv::Mat_<float> &depth);
-        cv::Size getMatchingSize(cv::Mat_<cv::Vec3b> &src, cv::Rect base);
+        cv::Size getMatchingSize(cv::Mat &src, cv::Mat &base);
+
 
     public:
         Projection();
@@ -28,6 +30,7 @@ class Projection{
         cv::Point2i rotatePixel(cv::Point2i pixel);
         cv::Point2i revertRotatePixel(cv::Point2i pixel);
         void adjustFrame(cv::Mat_<float> depth, cv::Mat_<cv::Vec3b> src, cv::Mat_<cv::Vec3b> &dst, Camera *camera, cv::Point3f beamer_pos);
+        void adjustFrame2(cv::Mat_<float> depth, cv::Mat_<cv::Vec3b> src, cv::Mat_<cv::Vec3b> &dst, Camera *camera, cv::Point3f beamer_pos);
         void printAdjustingMatrix();
 
 };

src/components/camera.cpp

@@ -114,6 +114,64 @@ cv::Point3f Camera::deprojectPixelToPoint(float coord[], float z){
 }
 
 
+
+void Camera::deprojectPixelToPoint(cv::Mat_<float> &depth, cv::Mat_<cv::Point3f> &real){
+    /*
+    // RS2_DISTORTION_BROWN_CONRADY
+
+    float x = (pixel[0] - intrin->ppx) / intrin->fx;
+    float y = (pixel[1] - intrin->ppy) / intrin->fy;
+
+    point[0] = depth * x;
+    point[1] = depth * y;
+    point[2] = depth;
+
+    */
+
+   float ppx = intr_profile.ppx * depth.size().width / intr_profile.width;
+   float ppy = intr_profile.ppy * depth.size().height / intr_profile.height;
+   float fx = depth.size().width * intr_profile.fx / intr_profile.width;
+   float fy = depth.size().height * intr_profile.fy / intr_profile.height;
+
+    for (int j = 0; j < depth.rows; j++){
+        for (int i = 0; i < depth.cols; i++){
+
+            float x = (i - ppx) / fx;
+            float y = (j - ppy) / fy;
+            float depth_ij = depth.at<float>(j,i);
+
+            real.at<cv::Point3f>(j,i) = cv::Point3f( depth_ij*x, depth_ij*y, depth_ij );
+        }
+    }
+}
+void Camera::projectPointToPixel(cv::Mat_<cv::Point3f> &real, cv::Mat_<cv::Point2i> &pixels){
+    /*
+    // RS2_DISTORTION_BROWN_CONRADY
+
+    float x = point[0] / point[2], y = point[1] / point[2];
+
+    pixel[0] = x * intrin->fx + intrin->ppx;
+    pixel[1] = y * intrin->fy + intrin->ppy;
+    */
+
+    float ppx = intr_profile.ppx * real.size().width / intr_profile.width;
+    float ppy = intr_profile.ppy * real.size().height / intr_profile.height;
+    float fx = real.size().width * intr_profile.fx / intr_profile.width;
+    float fy = real.size().height * intr_profile.fy / intr_profile.height;
+
+    for (int j = 0; j < pixels.rows; j++){
+        for (int i = 0; i < pixels.cols; i++){
+
+            cv::Point3f pt = real.at<cv::Point3f>(j,i);
+            float x = pt.x / pt.z;
+            float y = pt.y / pt.z;
+
+            pixels.at<cv::Point2i>(j,i) = cv::Point2i( x*fx+ppx, y*fy+ppy );
+        }
+    }
+}
+
+
 void Camera::printCroppingMask(){
     cv::Rect mask = getCroppingMask();
     std::cout << "(" << mask.x << "," << mask.y << ") + " << mask.width << "x" << mask.height << std::endl;

src/components/projection.cpp

@@ -31,21 +31,19 @@ cv::Point2i Projection::revertRotatePixel(cv::Point2i pixel){
 // Adjust the projected frame with the topology from the camera to the beamer POV
 void Projection::adjustFrame(cv::Mat_<float> depth, cv::Mat_<cv::Vec3b> src, cv::Mat_<cv::Vec3b> &dst, Camera *camera, cv::Point3f beamer_pos){
 
-    cv::Rect mask = camera->getCroppingMask();
-
     // resize the frames to be a multiple of the camera size :
     //      src.size = n * camera.depth.size , where n is uint > 0
-    static cv::Mat_<cv::Vec3b> resized_dst = cv::Mat_<cv::Vec3b>(getMatchingSize(dst, mask));
+    static cv::Mat_<cv::Vec3b> resized_dst = cv::Mat_<cv::Vec3b>(getMatchingSize(dst, depth));
     cv::resize(dst, resized_dst, resized_dst.size());
     cv::resize(src, src, resized_dst.size());
    
-    static cv::Mat_<cv::Point2i> deprojectMap = cv::Mat_<cv::Point2i>(mask.height, mask.width);
+    static cv::Mat_<cv::Point2i> deprojectMap = cv::Mat_<cv::Point2i>(depth.size().height, depth.size().width);
     deprojectMap = cv::Point2i(-1,-1);
 
-    static cv::Mat_<cv::Point2i> frameMapMask = cv::Mat_<cv::Point2i>(mask.height, mask.width, cv::Point2i(-1,-1));
+    static cv::Mat_<cv::Point2i> frameMapMask = cv::Mat_<cv::Point2i>(depth.size().height, depth.size().width, cv::Point2i(-1,-1));
     frameMapMask = cv::Point2i(-1,-1);
 
-    deprojectPixelsFromDepth(depth, mask, camera, beamer_pos, deprojectMap);
+    deprojectPixelsFromDepth(depth, camera->getCroppingMask(), camera, beamer_pos, deprojectMap);
 
     filterLowestDeprojectedPoints(depth, deprojectMap, frameMapMask);
 
@@ -57,6 +55,54 @@ void Projection::adjustFrame(cv::Mat_<float> depth, cv::Mat_<cv::Vec3b> src, cv:
     cv::warpAffine(dst, dst, adjustingMatrix, dst.size());
 }
 
+void Projection::adjustFrame2(cv::Mat_<float> depth, cv::Mat_<cv::Vec3b> src, cv::Mat_<cv::Vec3b> &dst, Camera *camera, cv::Point3f beamer_pos){
+
+    cv::resize(depth, depth, dst.size());
+    cv::resize(src, src, dst.size());
+   
+    static cv::Mat_<cv::Point2i> deprojectMap = cv::Mat_<cv::Point2i>(depth.size());
+    deprojectMap = cv::Point2i(-1,-1);
+
+    static cv::Mat_<cv::Point2i> frameMapMask = cv::Mat_<cv::Point2i>(depth.size(), cv::Point2i(-1,-1));
+    frameMapMask = cv::Point2i(-1,-1);
+
+
+    cv::Point3f CB = beamer_pos;
+    static cv::Mat_<cv::Point3f> CP = cv::Mat_<cv::Point3f>(depth.size());
+    static cv::Mat_<cv::Point3f> BP = cv::Mat_<cv::Point3f>(depth.size());
+    static cv::Mat_<cv::Point3f> BA = cv::Mat_<cv::Point3f>(depth.size());
+    static cv::Mat_<cv::Point3f> BV = cv::Mat_<cv::Point3f>(depth.size());
+    static cv::Mat_<cv::Point3f> CV = cv::Mat_<cv::Point3f>(depth.size());
+    static cv::Mat_<float> alpha = cv::Mat_<float>(depth.size());
+    
+    camera->deprojectPixelToPoint(depth, CP);
+/*
+    BP = CB;
+    BP -= CP;
+    BP *= -1;
+
+    BA = cv::Point3f(0,0,0);
+    BA.z = BP.z;
+
+    float BE = distanceTopSandbox - CB.z;
+
+    alpha = cv::norm(BA);
+    alpha /= BE;
+
+    BV = alpha * BP;
+    CV = CB + BV;
+*/
+    camera->projectPointToPixel(CV, deprojectMap);
+
+
+    filterLowestDeprojectedPoints(depth, deprojectMap, frameMapMask);
+
+    buildFrame2(frameMapMask, src, dst);
+
+    cv::warpAffine(dst, dst, adjustingMatrix, dst.size());
+}
+
+    
 
 
 /*
@@ -131,11 +177,27 @@ void Projection::buildFrame(cv::Mat_<float> &depth, cv::Mat_<cv::Point2i> &frame
     }
 }
 
+void Projection::buildFrame2(cv::Mat_<cv::Point2i> &frameMapMask, cv::Mat_<cv::Vec3b> &src, cv::Mat_<cv::Vec3b> &dst){
+    
+    for (int j = 0; j < frameMapMask.rows; j++){
+        for (int i = 0; i < frameMapMask.cols; i++){
+
+            cv::Point2i pixel_src = frameMapMask.at<cv::Point2i>(j,i);
+            cv::Point2i pixel_dst = cv::Point2i(i,j);
+
+            if( (0<=pixel_src.x && pixel_src.x<frameMapMask.cols) && (0<=pixel_src.y && pixel_src.y<frameMapMask.rows) ){
+                // src and dst must be of same size
+                dst.at<cv::Vec3b>(pixel_dst) = src.at<cv::Vec3b>(pixel_src);
+            }
+        }
+    }
+}
+
 
-cv::Size Projection::getMatchingSize(cv::Mat_<cv::Vec3b> &src, cv::Rect base){
+cv::Size Projection::getMatchingSize(cv::Mat &src, cv::Mat &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;
+    bigSize.width = (src.size().width % base.size().width == 0) ? src.size().width : src.size().width - (src.size().width % base.size().width) + base.size().width;
+    bigSize.height = (src.size().height % base.size().height == 0) ? src.size().height : src.size().height - (src.size().height % base.size().height) + base.size().height;
     return bigSize;
 }
 
@@ -178,8 +240,8 @@ cv::Point2i Projection::findMatchingPixel(int i, int j, float z, Camera *camera,
    
     float pixel[2] = {static_cast<float>(i), static_cast<float>(j)};
     cv::Point3f CP = camera->deprojectPixelToPoint(pixel, z);
-    cv::Point3f BP = CB - CP;
-    float BAz = CB.z + BP.z;
+    cv::Point3f BP = CP - CB;
+    float BAz = BP.z;
     float BEz = distanceTopSandbox - CB.z;
     float alpha = BEz / BAz;
     cv::Point3f V = (alpha * BP);