diff --git a/inc/projection.h b/inc/projection.h
index dff7b657700815ee1cbf73aaf05aa9887be3f345..db6f922e6525aaba78a7889d4095d9c3d66259fa 100644
--- a/inc/projection.h
+++ b/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);
diff --git a/src/components/camera.cpp b/src/components/camera.cpp
index 0e5b1d041a66c0ce1c7fd09d4f5e449e0116d2e5..5c84c7ffa05e8f9b42bffb5dc0d4d55bc3f4143a 100644
--- a/src/components/camera.cpp
+++ b/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};
diff --git a/src/components/projection.cpp b/src/components/projection.cpp
index e419376a018076aac20872edd2d3c731e38d1bd9..d525da6022842620857c8dd0bde2fca4d0a0fe75 100644
--- a/src/components/projection.cpp
+++ b/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;