diff --git a/inc/camera.h b/inc/camera.h
index 3e0ad99f2dff9189a6f8548890d5a8387cdde5d1..1626d1afea00c362d1e323f1f6149309c1e17d92 100644
--- a/inc/camera.h
+++ b/inc/camera.h
@@ -15,7 +15,8 @@ class Camera{
rs2::video_frame *color_frame;
rs2::depth_frame *depth_frame;
-
+
+ float depth_scale;
cv::Rect croppingMask;
void warmUpDepthLens();
@@ -25,11 +26,11 @@ class Camera{
Camera();
~Camera();
- // return a float matrix of depth in meters
+ // return values from depth matrix to real world (matrix of floats in meter)
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());
+ values.convertTo(meters,CV_32FC1, depth_scale);
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); };
diff --git a/inc/projection.h b/inc/projection.h
index e486fd0b887d36c6f67321c5f1662516ed20ebe8..ff33e10554683e4a4dd2233131450f1482df7d4b 100644
--- a/inc/projection.h
+++ b/inc/projection.h
@@ -10,8 +10,11 @@ class Projection{
cv::Mat adjustingMatrix;
float distanceTopSandbox;
+ void deprojectPixelsFromDepth(cv::Mat &depth, cv::Rect mask, Camera *camera, cv::Point3f beamer_pos, cv::Mat &pixelsDeprojectMap);
+ void filterHighestDeprojectedPoints(cv::Mat &depth, cv::Mat &pixelsDeprojectMap, cv::Mat &pixelsDeprojectHighestMap);
+ void buildFrame(cv::Mat &depth, cv::Mat &pixelsDeprojectHighestMap, cv::Mat &src, cv::Mat &dst);
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);
+ void copyPixelsInto(cv::Point2i pixel_dst, cv::Mat &dst, cv::Point2i pixel_src, cv::Mat &src, cv::Mat &depth);
cv::Size getMatchingSize(cv::Mat &src, cv::Rect base);
public:
diff --git a/inc/sandbox.h b/inc/sandbox.h
index 64311677df1dec2d0d213ea1fcb11d5f4ac36221..51800ee539e991b388753bdf129d33962c2f00b1 100644
--- a/inc/sandbox.h
+++ b/inc/sandbox.h
@@ -23,6 +23,7 @@ class Sandbox{
Projection* getProjection(){ return projection; };
void init();
+ void captureFrame();
cv::Mat getColorFrame();
cv::Mat getDepthFrame();
cv::Mat adjustProjection(cv::Mat frame);
diff --git a/src/components/camera.cpp b/src/components/camera.cpp
index 3442629842e82830d2942981cce96d6d60f4f33f..26d867f03e8a3f1347e68152104f00641271f927 100644
--- a/src/components/camera.cpp
+++ b/src/components/camera.cpp
@@ -10,6 +10,7 @@ Camera::Camera(){
tempFilter = new rs2::temporal_filter();
color_frame = new rs2::video_frame(rs2::frame());
depth_frame = new rs2::depth_frame(rs2::frame());
+ depth_scale = 1.0f;
}
@@ -51,11 +52,11 @@ int Camera::start(){
*/
rs2::depth_sensor sensor = pipe->start(*cfg).get_device().first<rs2::depth_sensor>();
+ depth_scale = sensor.get_depth_scale();
// Doc presets : https://dev.intelrealsense.com/docs/d400-series-visual-presets
sensor.set_option(RS2_OPTION_VISUAL_PRESET, RS2_RS400_VISUAL_PRESET_HIGH_DENSITY);
//sensor.set_option(RS2_OPTION_VISUAL_PRESET, RS2_RS400_VISUAL_PRESET_DEFAULT);
-
// 5 = range mapped to unlimited
spatFilter->set_option(RS2_OPTION_HOLES_FILL, 5);
@@ -83,11 +84,10 @@ void Camera::capture(){
if(depthFrame && colorFrame){
filterDepthFrame(depthFrame);
-
- // Values from depth matrix to real world
- // depth_frame.get_distance(x,y) == ((float)cv::Mat.at<uint16_t>(y, x) * depth_frame.get_units())
- color_frame->swap(colorFrame);
+
+ // Values relative to camera (not in meter)
depth_frame->swap(depthFrame);
+ color_frame->swap(colorFrame);
}
}
diff --git a/src/components/projection.cpp b/src/components/projection.cpp
index ffccf15324030a0f4d1edc443cb1531a5284a04b..fe6632b9f7eca5e9e71776a2dbc3241ae5dd5cf9 100644
--- a/src/components/projection.cpp
+++ b/src/components/projection.cpp
@@ -29,6 +29,19 @@ void Projection::adjustFrame(cv::Mat depth, cv::Mat src, cv::Mat &dst, Camera *c
cv::Mat pixelsDeprojectMap = cv::Mat_<cv::Point2i>(mask.height, mask.width, cv::Point2i(-1,-1));
cv::Mat pixelsDeprojectHighestMap = cv::Mat_<cv::Point2i>(mask.height, mask.width, cv::Point2i(-1,-1));
+ deprojectPixelsFromDepth(depth, mask, camera, beamer_pos, pixelsDeprojectMap);
+
+ filterHighestDeprojectedPoints(depth, pixelsDeprojectMap, pixelsDeprojectHighestMap);
+
+ buildFrame(depth, pixelsDeprojectHighestMap, src, dst);
+
+
+ cv::resize(dst, dst, dst_size);
+ cv::warpAffine(dst, dst, adjustingMatrix, dst.size());
+}
+
+void Projection::deprojectPixelsFromDepth(cv::Mat &depth, cv::Rect mask, Camera *camera, cv::Point3f beamer_pos, cv::Mat &pixelsDeprojectMap){
+
// Browse the depth frame matching the cropping mask
// while adapting pixels's position to the beamer's position
for (int j = 0; j < depth.rows; j++){
@@ -49,6 +62,9 @@ void Projection::adjustFrame(cv::Mat depth, cv::Mat src, cv::Mat &dst, Camera *c
pixelsDeprojectMap.at<cv::Point2i>(j,i) = pixel;
}
}
+}
+
+void Projection::filterHighestDeprojectedPoints(cv::Mat &depth, cv::Mat &pixelsDeprojectMap, cv::Mat &pixelsDeprojectHighestMap){
for (int j = 0; j < pixelsDeprojectMap.rows; j++){
for (int i = 0; i < pixelsDeprojectMap.cols; i++){
@@ -66,8 +82,10 @@ void Projection::adjustFrame(cv::Mat depth, cv::Mat src, cv::Mat &dst, Camera *c
}
}
}
+}
-
+void Projection::buildFrame(cv::Mat &depth, cv::Mat &pixelsDeprojectHighestMap, cv::Mat &src, cv::Mat &dst){
+
for (int j = 0; j < pixelsDeprojectHighestMap.rows; j++){
for (int i = 0; i < pixelsDeprojectHighestMap.cols; i++){
@@ -75,14 +93,10 @@ void Projection::adjustFrame(cv::Mat depth, cv::Mat src, cv::Mat &dst, Camera *c
if( (0<=pixel.x && pixel.x<depth.cols) && (0<=pixel.y && pixel.y<depth.rows) ){
// src and dst must be of same size
- copyPixelsInto(pixel, dst, cv::Point2i(i,j), src, mask);
+ copyPixelsInto(pixel, dst, cv::Point2i(i,j), src, depth);
}
}
}
-
-
- cv::resize(dst, dst, dst_size);
- cv::warpAffine(dst, dst, adjustingMatrix, dst.size());
}
cv::Size Projection::getMatchingSize(cv::Mat &src, cv::Rect base){
@@ -93,14 +107,14 @@ cv::Size Projection::getMatchingSize(cv::Mat &src, cv::Rect base){
}
// pixels coordinates are relative to the camera depth frame
-void Projection::copyPixelsInto(cv::Point2i pixel_dst, cv::Mat &dst, cv::Point2i pixel_src, cv::Mat &src, cv::Rect base){
+void Projection::copyPixelsInto(cv::Point2i pixel_dst, cv::Mat &dst, cv::Point2i pixel_src, cv::Mat &src, cv::Mat &depth){
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;
+ int ratio_w = src.size().width / depth.size().width;
+ int ratio_h = src.size().height / depth.size().height;
for(int j=0; j<ratio_h; j++){
for (int i=0; i<ratio_w; i++){
diff --git a/src/lib/sandbox.cpp b/src/lib/sandbox.cpp
index 6ee882aa61971cbb3bd51b9be75214222cf13bef..22b5865aab0ca4999946dc1ce509b3bdf300cfac 100644
--- a/src/lib/sandbox.cpp
+++ b/src/lib/sandbox.cpp
@@ -26,19 +26,22 @@ void Sandbox::init(){
camera->start();
}
-cv::Mat Sandbox::getColorFrame(){
+void Sandbox::captureFrame(){
camera->capture();
+}
+
+cv::Mat Sandbox::getColorFrame(){
return camera->getColorFrame()(camera->getCroppingMask());
}
cv::Mat Sandbox::getDepthFrame(){
- camera->capture();
return camera->getDepthFrame()(camera->getCroppingMask());
}
cv::Mat Sandbox::adjustProjection(cv::Mat frame){
+ captureFrame();
cv::Mat depth = getDepthFrame();
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());