cleaning

inc/projection.h

@@ -19,7 +19,7 @@ class Projection{
         cv::Point2f fxy;
         cv::Point2f ppxy;
 
-        void deprojectPixelsFromDepth(cv::Mat_<float> &depth, Camera *camera, cv::Point3f beamer_pos, cv::Mat_<cv::Point2i> &deprojectMap, cv::Point2f fxy, cv::Point2f ppxy);
+        void deprojectPixelsFromDepth(cv::Mat_<float> &depth, Camera *camera, cv::Rect mask, cv::Point3f beamer_pos, cv::Mat_<cv::Point2i> &deprojectMap, cv::Point2f fxy, cv::Point2f ppxy);
         void filterLowestDeprojectedPoints(cv::Mat_<float> &depth, cv::Mat_<cv::Point2i> &deprojectMap, cv::Mat_<cv::Point2i> &frameMap);
         void buildFrame(cv::Mat_<float> &depth, cv::Mat_<cv::Point2i> &frameMap, 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, cv::Point2f fxy, cv::Point2f ppxy);

src/components/camera.cpp

@@ -83,50 +83,6 @@ int Camera::start(){
     intr_profile = depth_frame->get_profile().as<rs2::video_stream_profile>().get_intrinsics();
 
 
-
-
-
-    // Debug
-
-/*
-    cv::Mat projection(768, 1024, CV_16UC1);
-    //cv::Mat projection(croppingMask.height, croppingMask.width, CV_16UC1);
-    std::vector<cv::Point2f> prof = getAdaptedIntrinsics(projection);
-
-    float mask_scale_x = croppingMask.x * projection.cols / croppingMask.width;
-    float mask_scale_y = croppingMask.y * projection.rows / croppingMask.height;
-
-    float pxc[] = { 0 + croppingMask.x ,0 + croppingMask.y,
-                    croppingMask.width + croppingMask.x, 0 + croppingMask.y,
-                    croppingMask.width/2.0f + croppingMask.x, croppingMask.height/2.0f + croppingMask.y,
-                    0 + croppingMask.x, croppingMask.height + croppingMask.y,
-                    croppingMask.width + croppingMask.x, croppingMask.height + croppingMask.y};
-
-    float pxp[] = { 0 + mask_scale_x, 0 + mask_scale_y,
-                    projection.size().width + mask_scale_x, 0 + mask_scale_y,
-                    projection.size().width/2.0f + mask_scale_x, projection.size().height/2.0f + mask_scale_y,
-                    0 + mask_scale_x, projection.size().height + mask_scale_y,
-                    projection.size().width + mask_scale_x, projection.size().height + mask_scale_y};
-
-
-    std::cout << "Camera " << intr_profile.width << "," << intr_profile.height << std::endl << intr_profile.fx << "," << intr_profile.fy << std::endl << intr_profile.ppx << "," << intr_profile.ppy << std::endl;
-    std::cout << "Proj "  << projection.size().width << "," << projection.size().height << std::endl << prof.at(0).x << "," << prof.at(0).y << std::endl << prof.at(1).x << "," << prof.at(1).y << std::endl;
-
-    for(int i=0; i<5; i++){
-        float c[] = {pxc[0+i], pxc[1+i]};
-        float px[] = {pxp[0+i], pxp[1+i]};
-        cv::Point3f pcam = deprojectPixelToPoint(c, 2.0f);
-        cv::Point3f ppro = deprojectPixelToPoint(px, 2.0f, prof.at(0), prof.at(1));
-        
-        std::cout << "Center" << std::endl;
-        std::cout << "Camera : " << pcam.x << "," << pcam.y << "," << pcam.z << std::endl;
-        std::cout << "Projection : " << ppro.x << "," << ppro.y << "," << ppro.z << std::endl;
-    }
-
-
-*/
-
-
     return 0;
 }
 
@@ -179,6 +135,9 @@ cv::Point3f Camera::deprojectPixelToPoint(float coord[], float z){
     they are related to the camera's profil, but adapted to what we want to display,
     the limits of the pixels match the limits of the camera's frame and 
     the 3D projection match the camera's too
+
+    Works in our case, because our camera's profil is RS2_DISTORTION_BROWN_CONRADY
+    (profil describing what kind of distoration is applied on the frame to adjust on the lens)
 */
 cv::Point2i Camera::projectPointToPixel(cv::Point3f point, cv::Point2f f, cv::Point2f pp){
     
@@ -198,25 +157,10 @@ std::vector<cv::Point2f> Camera::getAdaptedIntrinsics(cv::Mat &projection){
     
     float fx = static_cast<float>(intr_profile.fx * projection.size().width) / croppingMask.width;
     float fy = static_cast<float>(intr_profile.fy * projection.size().height) / croppingMask.height;
-
     cv::Point2f fxy = cv::Point2f(fx, fy);
 
     float ppx = static_cast<float>(intr_profile.ppx * projection.size().width) / croppingMask.width;
     float ppy = static_cast<float>(intr_profile.ppy * projection.size().height) / croppingMask.height;
-
-/*
-    float cam_center_x = croppingMask.width/2.0f;
-    float ppx_cam_relative_to_croppingMask_center = intr_profile.ppx-(croppingMask.x+cam_center_x);
-    float proj_center_x = projection.size().width/2.0f;
-    float ppx_proj_relative_to_proj_center = proj_center_x * ppx_cam_relative_to_croppingMask_center / cam_center_x;
-    float ppx_proj = ppx_proj_relative_to_proj_center + proj_center_x;
-
-    float cam_center_y = croppingMask.height/2.0f;
-    float ppy_cam_relative_to_croppingMask_center = intr_profile.ppy-(croppingMask.y+cam_center_y);
-    float proj_center_y = projection.size().height/2.0f;
-    float ppy_proj_relative_to_proj_center = proj_center_y * ppy_cam_relative_to_croppingMask_center / cam_center_y;
-    float ppy_proj = ppy_proj_relative_to_proj_center + proj_center_y;
-*/
     cv::Point2f ppxy = cv::Point2f(ppx, ppy);
 
     return std::vector<cv::Point2f> {fxy, ppxy};

src/components/projection.cpp

@@ -56,7 +56,7 @@ void Projection::adjustFrame(cv::Mat_<float> depth, cv::Mat_<cv::Vec3b> src, cv:
     cv::resize(src, src, dst.size());
     cv::resize(depth, depth, dst.size());    
 
-    deprojectPixelsFromDepth(depth, camera, beamer_pos, deprojectMap, fxy, ppxy);
+    deprojectPixelsFromDepth(depth, camera, camera->getCroppingMask() , beamer_pos, deprojectMap, fxy, ppxy);
     filterLowestDeprojectedPoints(depth, deprojectMap, frameMap);
     buildFrame(depth, frameMap, src, resized_dst);
 
@@ -75,10 +75,9 @@ void Projection::adjustFrame(cv::Mat_<float> depth, cv::Mat_<cv::Vec3b> src, cv:
     Deproject pixels in 3D, then adapt to Beamer's POV, and go back to 2D
     This gives us the location od pixels adapted to the Beamer projection
 */
-void Projection::deprojectPixelsFromDepth(cv::Mat_<float> &depth, Camera *camera, cv::Point3f beamer_pos, cv::Mat_<cv::Point2i> &deprojectMap, cv::Point2f fxy, cv::Point2f ppxy){
+void Projection::deprojectPixelsFromDepth(cv::Mat_<float> &depth, Camera *camera, cv::Rect mask, cv::Point3f beamer_pos, cv::Mat_<cv::Point2i> &deprojectMap, cv::Point2f fxy, cv::Point2f ppxy){
     
     // scale coord of the mask with the new resolution of the depth frame
-    cv::Rect mask = camera->getCroppingMask();
     float mask_scale_x = mask.x * depth.cols / mask.width;
     float mask_scale_y = mask.y * depth.rows / mask.height;