diff --git a/app/SandboxSetup/beamerlocationgui.cpp b/app/SandboxSetup/beamerlocationgui.cpp
index 51592d686cd30bcad43de3d0919d3ed0291d672d..3b2c2b80da75e4c1aa20a1a67d7299b9d1240b4e 100644
--- a/app/SandboxSetup/beamerlocationgui.cpp
+++ b/app/SandboxSetup/beamerlocationgui.cpp
@@ -133,7 +133,7 @@ void BeamerLocationGui::userValidePoint(){
if(!circle.empty()){
- capturedPoints.push_back( beamer->deprojectPixel(circle.at(0), &depth, camera) );
+ capturedPoints.push_back( beamer->deprojectPixel(cv::Point2i(circle.at(0).x, circle.at(0).y), &depth, camera) );
updateLabelSteps();
// enough points to perform linear regression and move into next step
diff --git a/app/SandboxSetup/beamerlocationgui.h b/app/SandboxSetup/beamerlocationgui.h
index b7e407abc3c4e12076040f358af16218b5f338ec..3010601d5bbe5bcdf4165d74cacc706272c12c9a 100644
--- a/app/SandboxSetup/beamerlocationgui.h
+++ b/app/SandboxSetup/beamerlocationgui.h
@@ -3,6 +3,7 @@
#include <QDialog>
#include <QTimer>
+#include <QMutex>
#include <opencv2/opencv.hpp>
#include "qtfullscreen.h"
#include "monitorgui.h"
diff --git a/app/SandboxSetup/croppingmask.cpp b/app/SandboxSetup/croppingmask.cpp
index f96a754bbb6df7ed1aa01ca9a0f2694a38d62e90..d68e1810372a5e95f71adc68c01c6b4db0169e56 100644
--- a/app/SandboxSetup/croppingmask.cpp
+++ b/app/SandboxSetup/croppingmask.cpp
@@ -28,13 +28,17 @@ CroppingMask::~CroppingMask()
}
void CroppingMask::valideRoutine(){
+
timer->stop();
+ setup->getCamera()->capture();
cv::Size s = setup->getCamera()->getDepthFrame().size();
cv::Point center(s.width / 2, s.height / 2);
std::vector<cv::Point> rectPoints = getRectPoints();
+ cv::Mat depthFrame = setup->getCamera()->getDepthFrame();
setup->setupAdjustMatrix(rectPoints, center);
setup->setupCroppingMask(rectPoints);
+ setup->getProjection()->setDistanceTopSandbox(depthFrame.at<float>(center));
endSuccess = true;
}
diff --git a/app/SandboxSetup/initcamera.cpp b/app/SandboxSetup/initcamera.cpp
index 3985fe0fe9d6c3adeac8893d6b89685da3e26a28..79ee3a69372a9415d6cfa12a684f0603662a1b14 100644
--- a/app/SandboxSetup/initcamera.cpp
+++ b/app/SandboxSetup/initcamera.cpp
@@ -10,12 +10,7 @@ CameraStartThread::CameraStartThread(Camera *c) : QThread() {
void CameraStartThread::run() {
int err = camera->start();
- emit deviceFound(err);
-
- if(!err){
- camera->warmUpDepthLens();
- emit setupReady(0);
- }
+ emit setupReady(err);
}
@@ -27,9 +22,9 @@ InitCamera::InitCamera(SandboxSetup *_setup, QWidget *parent) :
{
setup = _setup;
ui->setupUi(this);
+ ui->label->setText(QString("Initializing device..."));
workerThread = new CameraStartThread(setup->getCamera());
- connect(workerThread, &CameraStartThread::deviceFound, this, &InitCamera::deviceFound);
connect(workerThread, &CameraStartThread::setupReady, this, &InitCamera::setupReady);
connect(workerThread, &CameraStartThread::finished, workerThread, &QObject::deleteLater);
}
@@ -53,15 +48,11 @@ void InitCamera::setupCamera(){
workerThread->start();
}
-void InitCamera::deviceFound(int err){
- QString msg = (err) ? "No device found" : "Initializing device...";
- ui->label->setText(msg);
-
- if(err)
- emit sendNotif(err);
-}
-
void InitCamera::setupReady(int err){
+ if(err){
+ QString msg = "No device found";
+ ui->label->setText(msg);
+ }
emit sendNotif(err);
}
diff --git a/app/SandboxSetup/initcamera.h b/app/SandboxSetup/initcamera.h
index 86862835777611064cec771398bc4d630e523e6e..1ffe6d7dc780a3e8fedec9792723bd34ef8f17be 100644
--- a/app/SandboxSetup/initcamera.h
+++ b/app/SandboxSetup/initcamera.h
@@ -21,8 +21,7 @@ public:
void run();
signals:
- void setupReady(int state);
- void deviceFound(int err);
+ void setupReady(int err);
private:
Camera *camera;
@@ -47,8 +46,7 @@ private:
CameraStartThread *workerThread;
void setupCamera();
- void setupReady(int state);
- void deviceFound(int err);
+ void setupReady(int err);
};
#endif // INITCAMERA_H
diff --git a/inc/beamer.h b/inc/beamer.h
index 08d9f6fec7e3a9ee26a43236ce7d6e8940237193..12c05bdc89a8af77b236f01bc4ec818b4a62a5bf 100644
--- a/inc/beamer.h
+++ b/inc/beamer.h
@@ -84,7 +84,7 @@ class Beamer{
int findBeamerFrom(Camera *camera);
cv::Mat editContrast(cv::Mat image, double contrast, int brightness);
- cv::Point3f deprojectPixel(cv::Point3i circle, cv::Mat *depth, Camera *camera);
+ cv::Point3f deprojectPixel(cv::Point2i circle, cv::Mat *depth, Camera *camera);
std::vector<cv::Point2i> getCrossList();
cv::Mat getCrossFrame(cv::Point2i projectedCross, int step, int max, bool circlesFound);
cv::Point3d approximatePosition(std::vector<cv::Point3d> *bases, std::vector<cv::Point3d> *directions);
diff --git a/inc/beamerProjection.h b/inc/beamerProjection.h
index 5f756ad8d6a54b864ed5a3a35ec7e01ca73cfcee..6d492c71175719e64c3c5b8dec9c9cddb1aa61e9 100644
--- a/inc/beamerProjection.h
+++ b/inc/beamerProjection.h
@@ -8,17 +8,22 @@
class BeamerProjection{
private:
cv::Mat adjustingMatrix;
+ float distanceTopSandbox = 1.0f;
- cv::Point2i adjustPixel(int i, int j, float z, Camera camera, cv::Point3f beamer);
+ 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){ matrix.clone(); }
- cv::Mat getAdjustingMatrix(){ return adjustingMatrix.clone(); }
+ void setAdjustingMatrix(cv::Mat matrix){ adjustingMatrix = matrix; }
+ cv::Mat getAdjustingMatrix(){ return adjustingMatrix; }
+ void setDistanceTopSandbox(float dist){ distanceTopSandbox = dist; };
+ float getDistanceTopSandbox(){ return distanceTopSandbox; };
cv::Point2i rotatePixel(cv::Point2i pixel);
- void adjustFrame(cv::Mat &depth, cv::Mat &src, cv::Mat &dst, Camera camera, cv::Point3f beamer);
+ void adjustFrame(cv::Mat depth, cv::Mat src, cv::Mat &dst, Camera *camera, cv::Point3f beamer_pos);
void printAdjustingMatrix();
};
diff --git a/inc/camera.h b/inc/camera.h
index a1dd321a72ada8541daaa3e147c5e222ddb4f24c..3e0ad99f2dff9189a6f8548890d5a8387cdde5d1 100644
--- a/inc/camera.h
+++ b/inc/camera.h
@@ -5,33 +5,38 @@
#include <librealsense2/rsutil.h>
#include <opencv2/opencv.hpp>
-// camera resolution : 640x480
-
class Camera{
private:
- rs2::spatial_filter spatFilter;
- rs2::temporal_filter tempFilter;
- rs2::config cfg;
- rs2::pipeline pipe;
- rs2::pipeline_profile profile;
- rs2_intrinsics intrinsics;
-
- cv::Mat matDepth;
- cv::Mat matRGB;
+ rs2::spatial_filter *spatFilter;
+ rs2::temporal_filter *tempFilter;
+ rs2::config *cfg;
+ rs2::pipeline *pipe;
+ rs2::align *align_to_depth;
+
+ rs2::video_frame *color_frame;
+ rs2::depth_frame *depth_frame;
+
cv::Rect croppingMask;
+ void warmUpDepthLens();
void filterDepthFrame(rs2::depth_frame &frameDepth);
public:
Camera();
-
- cv::Mat getDepthFrame(){ return matDepth.clone(); };
- cv::Mat getColorFrame(){ return matRGB.clone(); };
+ ~Camera();
+
+ // return a float matrix of depth in meters
+ 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; };
int start();
- void warmUpDepthLens();
void stop();
cv::Point3f deprojectPixelToPoint(float coord[], float z1);
cv::Point2i projectPointToPixel(cv::Point3f point3D);
diff --git a/inc/sandbox.h b/inc/sandbox.h
index 2588e5a37b089d1d306ab318d66d4f3caf236303..182a86aed35beb21caa78b9998cf0c5d096a09a3 100644
--- a/inc/sandbox.h
+++ b/inc/sandbox.h
@@ -21,10 +21,15 @@ class Sandbox{
public:
Sandbox();
+ Camera* getCamera(){ return &camera; };
+ Beamer* getBeamer(){ return &beamer; };
+ BeamerProjection* getProjection(){ return &projection; };
+
+ void init();
cv::Mat getColorFrame();
cv::Mat getDepthFrame();
- cv::Mat* adjustProjection(cv::Mat* frame);
- void showImage(cv::Mat* image);
+ cv::Mat adjustProjection(cv::Mat frame);
+ void showImage(cv::Mat &image);
int loadConfig();
int loadConfigFrom(char *path);
void initWindowsFullScreen();
diff --git a/inc/sandboxConfig.h b/inc/sandboxConfig.h
index 3898d465c1b93ac29c4bf4d647b513f01daceb4d..0cef219e0b4244efa2ea8fee0340549a63ae9997 100644
--- a/inc/sandboxConfig.h
+++ b/inc/sandboxConfig.h
@@ -7,11 +7,12 @@
#include "beamerProjection.h"
#include "beamer.h"
-#define MASK "CroppingMask"
-#define POSITION "BeamerPosition"
-#define MATRIX "AdjustingMatrix"
-#define RESOLUTION "BeamerResolution"
-#define PROCESSPROFIL "FrameProcessProfil"
+#define SCFG_MASK "CroppingMask"
+#define SCFG_POSITION "BeamerPosition"
+#define SCFG_MATRIX "AdjustingMatrix"
+#define SCFG_DISTANCE "DistanceTopSandbox"
+#define SCFG_RESOLUTION "BeamerResolution"
+#define SCFG_PROCESS_PROFIL "FrameProcessProfil"
class SandboxConfig{
private:
@@ -19,12 +20,14 @@ class SandboxConfig{
public:
static int saveAdjustingMatrixInto(char *path, cv::Mat matrix);
+ static int saveDistanceToSandboxTop(char *path, float distance);
static int saveCroppingMaskInto(char *path, cv::Rect mask);
static int saveBeamerPositionInto(char *path, cv::Point3f position);
static int saveBeamerResolutionInto(char *path, cv::Size resolution);
static int saveFrameProcessProfil(char *path, FrameProcessProfil profil);
static int loadAdjustingMatrixFrom(char *path, BeamerProjection *projection);
+ static int loadDistanceToSandboxTop(char *path, BeamerProjection *projection);
static int loadCroppingMaskFrom(char *path, Camera *camera);
static int loadBeamerPositionFrom(char *path, Beamer *beamer);
static int loadBeamerResolutionFrom(char *path, Beamer *beamer);
diff --git a/src/components/beamer.cpp b/src/components/beamer.cpp
index 055151d03c841b1d561a6e6ef46ce3d63663ded2..a1dbd372f78f9b79272439acac6b02f13db7b7ab 100644
--- a/src/components/beamer.cpp
+++ b/src/components/beamer.cpp
@@ -64,7 +64,7 @@ int Beamer::findBeamerFrom(Camera *camera){
}
else if (keyCode == ' '){
if(!circles.empty()){
- capturedPoints.push_back( deprojectPixel(circles.at(0), &depth, camera) );
+ capturedPoints.push_back( deprojectPixel( cv::Point2i(circles.at(0).x, circles.at(0).y ), &depth, camera) );
}
}
}
@@ -81,12 +81,10 @@ int Beamer::findBeamerFrom(Camera *camera){
return 0;
}
-cv::Point3f Beamer::deprojectPixel(cv::Point3i circle, cv::Mat *depth, Camera *camera){
+cv::Point3f Beamer::deprojectPixel(cv::Point2i circle, cv::Mat *depth, Camera *camera){
float coord[2] = {(float)circle.x, (float)circle.y};
- float z = static_cast<float>(depth->at<uint16_t>(circle.y, circle.x));
- // rs2.get_depth_frame().get_units()
- float depth_unit = 1 / 1000.0;
- return camera->deprojectPixelToPoint(coord, z * depth_unit);
+ float z = depth->at<float>(circle.y, circle.x);
+ return camera->deprojectPixelToPoint(coord, z);
}
void Beamer::findLinearLineFrom(std::vector<cv::Point3f> *capturedPoints, std::vector<cv::Point3d> *bases, std::vector<cv::Point3d> *directions){
@@ -243,8 +241,8 @@ std::vector<cv::Point3i> Beamer::findCircles(cv::Mat &rgb, double contrast, int
// min_dist : Minimal distance between the detected centers
// param_1 : Upper threshold of the canny edge detector, determines if a pixel is an edge
// param_2 : Threshold for the center detection, after the accumulator is complet, threshold to keep only the possible centers
- // min_radius : Min radius of the circles drew on the accumulator
- // max_radius : Max radius of the circles drew on the accumulator
+ // min_radius : Min radius of the circles drawn on the accumulator
+ // max_radius : Max radius of the circles drawn on the accumulator
cv::HoughCircles(src_gray, circles, CV_HOUGH_GRADIENT, 1, centersMinDist, (double)cannyEdgeThreshold, (double)houghAccThreshold, minRadius, maxRadius);
std::vector<cv::Point3i> result;
diff --git a/src/components/beamerProjection.cpp b/src/components/beamerProjection.cpp
index b07d6515727b43c70abe21fd4d0f3b9664c1d186..da649fed1b47a358c9e0dc0fa64e86ade0c6693b 100644
--- a/src/components/beamerProjection.cpp
+++ b/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,46 +14,128 @@ cv::Point2i BeamerProjection::rotatePixel(cv::Point2i pixel){
}
-cv::Point2i BeamerProjection::adjustPixel(int i, int j, float z, Camera camera, cv::Point3f beamer){
- //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.x - p.x, beamer.y - p.y, beamer.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();
+
+ // resize the frames to be a multiple of the camera size :
+ // src.size = n * camera.depth.size , where n is uint > 0
+ cv::resize(dst, dst, getMatchingSize(dst, mask));
+ cv::resize(src, src, dst.size());
+
+ 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));
+
+ // 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++){
+ for (int i = 0; i < depth.cols; 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 (the area where the frame is projected)
+ pixel.x -= mask.x;
+ pixel.y -= mask.y;
+
+ pixelsDeprojectMap.at<cv::Point2i>(j,i) = pixel;
+ }
+ }
+
+ for (int j = 0; j < pixelsDeprojectMap.rows; j++){
+ for (int i = 0; i < pixelsDeprojectMap.cols; i++){
+
+ cv::Point2i pixel = pixelsDeprojectMap.at<cv::Point2i>(j,i);
+
+ if(pixel.x != -1 && pixel.y != -1){
+ // check and keep the highest point at the location pointed by pixel
+ cv::Point2i defaultPoint = pixelsDeprojectHighestMap.at<cv::Point2i>(j,i);
+ if(defaultPoint.x != -1 && defaultPoint.y != -1){
+ if(depth.at<float>(defaultPoint) <= depth.at<float>(pixel))
+ pixel = defaultPoint;
+ }
+ pixelsDeprojectHighestMap.at<cv::Point2i>(j,i) = pixel;
+ }
+ }
+ }
+
+
+ for (int j = 0; j < pixelsDeprojectHighestMap.rows; j++){
+ for (int i = 0; i < pixelsDeprojectHighestMap.cols; i++){
+ cv::Point2i pixel = pixelsDeprojectHighestMap.at<cv::Point2i>(j,i);
-void BeamerProjection::adjustFrame(cv::Mat &depth, cv::Mat &src, cv::Mat &dst, Camera camera, cv::Point3f beamer){
-
- 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);
- 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<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);
}
}
}
- //cout << "temps de calcul: " << (getTickCount() - t1) / getTickFrequency() << endl;
+
+
+ cv::resize(dst, dst, 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 BAz = CB.z + BP.z;
+ float BEz = distanceTopSandbox - CB.z;
+ float alpha = BEz / BAz;
+ cv::Point3f V = (alpha * BP);
+ cv::Point3f CV = V + CB;
+
+ return camera->projectPointToPixel(CV);
}
diff --git a/src/components/camera.cpp b/src/components/camera.cpp
index 05100752411533bd57f3d64840e9f1522fe9cc3e..5bbc05581fa45cb89dde392aca4a231d79724fec 100644
--- a/src/components/camera.cpp
+++ b/src/components/camera.cpp
@@ -1,11 +1,28 @@
#include "../../inc/camera.h"
-Camera::Camera() {
-
+Camera::Camera(){
+
+ cfg = new rs2::config();
+ pipe = new rs2::pipeline();
+ align_to_depth = new rs2::align(RS2_STREAM_DEPTH);
+ spatFilter = new rs2::spatial_filter();
+ tempFilter = new rs2::temporal_filter();
+ color_frame = new rs2::video_frame(rs2::frame());
+ depth_frame = new rs2::depth_frame(rs2::frame());
}
+Camera::~Camera(){
+ delete align_to_depth;
+ delete tempFilter;
+ delete spatFilter;
+ delete pipe;
+ delete cfg;
+ delete depth_frame;
+ delete color_frame;
+}
+
/*
* Public
@@ -15,86 +32,88 @@ Camera::Camera() {
int Camera::start(){
// check for a device availible
- if(!cfg.can_resolve(pipe)){
+ if(!cfg->can_resolve(*pipe)){
//std::cout << "Error: No device found" << std::endl;
return 1;
}
+ // Logs
std::remove("./camera.logs");
rs2::log_to_file(RS2_LOG_SEVERITY_DEBUG, "./camera.logs");
- //rs2::log_to_console(RS2_LOG_SEVERITY_DEBUG);//RS2_LOG_SEVERITY_ERROR);
-
- spatFilter.set_option(RS2_OPTION_HOLES_FILL, 5);
- profile = pipe.start(cfg);
- auto sensor = profile.get_device().first<rs2::depth_sensor>();
- // TODO: At the moment the SDK does not offer a closed enum for D400 visual presets
- // We do this to reduce the number of black pixels
- // The hardware can perform hole-filling much better and much more power efficient then our software
- auto range = sensor.get_option_range(RS2_OPTION_VISUAL_PRESET);
- for (auto i = range.min; i < range.max; i += range.step)
- if (std::string(sensor.get_option_value_description(RS2_OPTION_VISUAL_PRESET, i)) == "High Density")
- sensor.set_option(RS2_OPTION_VISUAL_PRESET, i);
+ // Settings
+ // default camera resolution : 640x480
+ cfg->enable_stream(RS2_STREAM_COLOR);
+ cfg->enable_stream(RS2_STREAM_DEPTH);
+ /*
+ cfg->enable_stream(RS2_STREAM_COLOR, 1280, 720, RS2_FORMAT_RGB8, 30);
+ cfg->enable_stream(RS2_STREAM_DEPTH, 1280, 720, RS2_FORMAT_Y16, 30);
+ */
+
+ rs2::depth_sensor sensor = pipe->start(*cfg).get_device().first<rs2::depth_sensor>();
+
+ // 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);
+
- return 0;
-}
+ // 5 = range mapped to unlimited
+ spatFilter->set_option(RS2_OPTION_HOLES_FILL, 5);
+ warmUpDepthLens();
-// Capture 30 frames to give autoexposure, etc. a chance to settle
-void Camera::warmUpDepthLens()
-{
- for (int i = 0; i < 30; ++i)
- {
- rs2::frameset data = pipe.wait_for_frames();
- auto frameDepth = data.get_depth_frame();
- frameDepth = spatFilter.process(frameDepth);
- frameDepth = tempFilter.process(frameDepth);
- }
+ return 0;
}
void Camera::stop(){
- pipe.stop();
+ pipe->stop();
}
void Camera::capture(){
- rs2::align align(RS2_STREAM_DEPTH);
- auto frameset = pipe.wait_for_frames();
- //Get processed aligned frame
- frameset = align.process(frameset);
- // Trying to get color and aligned depth frames
- rs2::video_frame color_frame = frameset.get_color_frame();
- rs2::depth_frame depth_frame = frameset.get_depth_frame();
- filterDepthFrame(depth_frame);
- // intrinsics.model : Brown Conrady
- matDepth = cv::Mat(cv::Size(depth_frame.get_width(), depth_frame.get_height()), CV_16UC1, (void *)depth_frame.get_data(), cv::Mat::AUTO_STEP);
- // depth_frame.get_distance(x,y) == ((float)matDepth.at<uint16_t>(y, x) * depth_frame.get_units())
- cv::Mat(cv::Size(color_frame.get_width(), color_frame.get_height()), CV_8UC3, (void *)color_frame.get_data(), cv::Mat::AUTO_STEP).copyTo(matRGB);
-}
+ auto frameset = pipe->wait_for_frames();
+
+ // Trying to get frames from the depth perspective (aligned on depth camera)
+ frameset = align_to_depth->process(frameset);
+ rs2::video_frame colorFrame = frameset.get_color_frame();
+ rs2::depth_frame depthFrame = frameset.get_depth_frame();
+
+ 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);
+ depth_frame->swap(depthFrame);
+ }
+}
+// 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};
float pixel[2];
- rs2_project_point_to_pixel(pixel, &intrinsics, point);
+ rs2_intrinsics intr = depth_frame->get_profile().as<rs2::video_stream_profile>().get_intrinsics();
+ rs2_project_point_to_pixel(pixel, &intr, point);
return cv::Point2i(pixel[0], pixel[1]);
}
-
-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_deproject_pixel_to_point(p, &intrinsics, coord, z1);
+ rs2_intrinsics intr = depth_frame->get_profile().as<rs2::video_stream_profile>().get_intrinsics();
+ rs2_deproject_pixel_to_point(p, &intr, coord, z);
return cv::Point3f(p[0], p[1], p[2]);
}
void Camera::printCroppingMask(){
cv::Rect mask = getCroppingMask();
- std::cout << "(" << mask.x << "," << mask.y << ") + " << mask.width << "," << mask.height << std::endl;
+ std::cout << "(" << mask.x << "," << mask.y << ") + " << mask.width << "x" << mask.height << std::endl;
}
@@ -104,10 +123,19 @@ void Camera::printCroppingMask(){
* Private
*/
+// Capture 30 frames to give autoexposure, etc. a chance to settle
+void Camera::warmUpDepthLens()
+{
+ for (int i = 0; i < 30; ++i)
+ {
+ rs2::frameset data = pipe->wait_for_frames();
+ auto frameDepth = data.get_depth_frame();
+ filterDepthFrame(frameDepth);
+ }
+}
void Camera::filterDepthFrame(rs2::depth_frame &frameDepth)
{
- frameDepth = spatFilter.process(frameDepth);
- frameDepth = tempFilter.process(frameDepth);
- intrinsics = frameDepth.get_profile().as<rs2::video_stream_profile>().get_intrinsics();
+ frameDepth = spatFilter->process(frameDepth);
+ frameDepth = tempFilter->process(frameDepth);
}
diff --git a/src/lib/sandbox.cpp b/src/lib/sandbox.cpp
index 0284015eb43ecc415e9024655c26697f50b8fdf6..98f22066c95c36a97a414cf9aa9309f4974821a8 100644
--- a/src/lib/sandbox.cpp
+++ b/src/lib/sandbox.cpp
@@ -6,7 +6,7 @@
*/
Sandbox::Sandbox(){
- camera.start();
+
}
@@ -14,6 +14,10 @@ Sandbox::Sandbox(){
* PUBLIC
*/
+void Sandbox::init(){
+ camera.start();
+}
+
cv::Mat Sandbox::getColorFrame(){
camera.capture();
return camera.getColorFrame()(camera.getCroppingMask());
@@ -25,49 +29,32 @@ cv::Mat Sandbox::getDepthFrame(){
}
-cv::Mat* Sandbox::adjustProjection(cv::Mat* frame){
-
- static cv::Mat frameBeamer(cv::Size(beamer.getWidth(), beamer.getHeight()), CV_8UC3);
+cv::Mat Sandbox::adjustProjection(cv::Mat frame){
cv::Mat depth = getDepthFrame();
- cv::Mat imageCalibrate(depth.size(), CV_8UC3, cv::Scalar(0, 0, 0));
-
- cv::resize(*frame, *frame, depth.size());
- 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::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());
- cv::resize(imageCalibrate, frameBeamer, frameBeamer.size());
- frameBeamer.copyTo(*frame);
- return 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));
+ // 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);
- cv::resize(depth, depth, beamerResolution);
- cv::resize(image, image, beamerResolution);
- projection.adjustFrame(depth, image, imageCalibrate, camera, beamer.getPosition());
-
- imageCalibrate.copyTo(*frame);
- return frame;
-*/
+ return imageCalibrate;
}
-void Sandbox::showImage(cv::Mat* image){
+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){
int err = SandboxConfig::loadAdjustingMatrixFrom(path, &projection);
if(err){ return err; }
+ err = SandboxConfig::loadDistanceToSandboxTop(path, &projection);
+ if(err){ return err; }
err = SandboxConfig::loadCroppingMaskFrom(path, &camera);
if(err){ return err; }
err = SandboxConfig::loadBeamerResolutionFrom(path, &beamer);
@@ -79,13 +66,7 @@ int Sandbox::loadConfigFrom(char *path){
int Sandbox::loadConfig(){
- int err = loadConfigFrom(defaultConfigFilePath);
- if(err){
- std::cout << "error config" << std::endl;
- return err;
- }
-
- return 0;
+ return loadConfigFrom(defaultConfigFilePath);
}
void Sandbox::initWindowsFullScreen(){
diff --git a/src/lib/sandboxSetup.cpp b/src/lib/sandboxSetup.cpp
index b78e6da63fb57c5b816cf463eab41bd31c8e4729..ef179b7519fb4484cd01f068d8b1b00bb8fb85c0 100644
--- a/src/lib/sandboxSetup.cpp
+++ b/src/lib/sandboxSetup.cpp
@@ -14,6 +14,9 @@ SandboxSetup::SandboxSetup(){
int SandboxSetup::saveConfigFrom(char *path){
if(SandboxConfig::saveAdjustingMatrixInto(path, projection.getAdjustingMatrix()))
return 1;
+
+ if(SandboxConfig::saveDistanceToSandboxTop(path, projection.getDistanceTopSandbox()))
+ return 1;
if(SandboxConfig::saveCroppingMaskInto(path, camera.getCroppingMask()))
return 1;
diff --git a/src/tools/sandboxConfig.cpp b/src/tools/sandboxConfig.cpp
index bfa736caef0a34541f9119f2e4210926c1a1bb22..c75c292d14da82a49228bbbe7ac30128ce3affe5 100644
--- a/src/tools/sandboxConfig.cpp
+++ b/src/tools/sandboxConfig.cpp
@@ -26,7 +26,26 @@ int SandboxConfig::saveAdjustingMatrixInto(char *path, cv::Mat matrix){
//std::cout << "[Error] No Config File found : " << err.what() << std::endl;
}
- config[MATRIX] = val;
+ config[SCFG_MATRIX] = val;
+
+ return saveConfigIn(path, config);
+}
+
+
+int SandboxConfig::saveDistanceToSandboxTop(char *path, float distance){
+
+ YAML::Node val;
+
+ val["distance"] = distance;
+
+ YAML::Node config;
+ try{
+ config = YAML::LoadFile(path);
+ }catch(YAML::BadFile err){
+ //std::cout << "[Error] No Config File found : " << err.what() << std::endl;
+ }
+
+ config[SCFG_DISTANCE] = val;
return saveConfigIn(path, config);
}
@@ -48,7 +67,7 @@ int SandboxConfig::saveCroppingMaskInto(char *path, cv::Rect mask){
//std::cout << "[Error] No Config File found : " << err.what() << std::endl;
}
- config[MASK] = val;
+ config[SCFG_MASK] = val;
return saveConfigIn(path, config);
}
@@ -69,7 +88,7 @@ int SandboxConfig::saveBeamerPositionInto(char *path, cv::Point3f pos){
//std::cout << "[Error] No Config File found : " << err.what() << std::endl;
}
- config[POSITION] = val;
+ config[SCFG_POSITION] = val;
return saveConfigIn(path, config);
}
@@ -79,8 +98,8 @@ int SandboxConfig::saveBeamerResolutionInto(char *path, cv::Size res){
YAML::Node val;
- val["height"] = res.height;
val["width"] = res.width;
+ val["height"] = res.height;
YAML::Node config;
try{
@@ -89,7 +108,7 @@ int SandboxConfig::saveBeamerResolutionInto(char *path, cv::Size res){
//std::cout << "[Error] No Config File found : " << err.what() << std::endl;
}
- config[RESOLUTION] = val;
+ config[SCFG_RESOLUTION] = val;
return saveConfigIn(path, config);
}
@@ -114,7 +133,7 @@ int SandboxConfig::saveFrameProcessProfil(char *path, FrameProcessProfil profil)
//std::cout << "[Error] No Config File found : " << err.what() << std::endl;
}
- config[PROCESSPROFIL] = val;
+ config[SCFG_PROCESS_PROFIL] = val;
return saveConfigIn(path, config);
}
@@ -132,23 +151,23 @@ int SandboxConfig::loadAdjustingMatrixFrom(char *path, BeamerProjection *project
try{
config = YAML::LoadFile(path);
}catch(YAML::BadFile err){
- std::cout << "[Error] No Config File found : " << err.what() << std::endl;
+ //std::cout << "[Error] No Config File found : " << err.what() << std::endl;
return 1;
}
// no node for adjusting matrix
- if(!config[MATRIX]){
+ if(!config[SCFG_MATRIX]){
return 2;
};
// uncomplet data for adjusting matrix
- if(!(config[MATRIX]["width"] && config[MATRIX]["height"] && config[MATRIX]["matrix"])){
+ if(!(config[SCFG_MATRIX]["width"] && config[SCFG_MATRIX]["height"] && config[SCFG_MATRIX]["matrix"])){
return 2;
}
- int height = config[MATRIX]["height"].as<int>();
- int width = config[MATRIX]["width"].as<int>();
- std::vector<float> values = config[MATRIX]["matrix"].as<std::vector<float>>();
+ int height = config[SCFG_MATRIX]["height"].as<int>();
+ int width = config[SCFG_MATRIX]["width"].as<int>();
+ std::vector<float> values = config[SCFG_MATRIX]["matrix"].as<std::vector<float>>();
// verify the number of values in the matrix
if(height*width != (int)values.size()){
@@ -164,13 +183,40 @@ int SandboxConfig::loadAdjustingMatrixFrom(char *path, BeamerProjection *project
v_i++;
}
}
-
+
projection->setAdjustingMatrix(matrix);
return 0;
}
+int SandboxConfig::loadDistanceToSandboxTop(char *path, BeamerProjection *projection){
+
+ YAML::Node config;
+
+ try{
+ config = YAML::LoadFile(path);
+ }catch(YAML::BadFile err){
+ //std::cout << "[Error] No Config File found : " << err.what() << std::endl;
+ return 1;
+ }
+
+ if(!config[SCFG_DISTANCE]){
+ return 2;
+ };
+
+ if(!(config[SCFG_DISTANCE]["distance"])){
+ return 2;
+ }
+
+ float distance = config[SCFG_DISTANCE]["distance"].as<float>();
+
+ projection->setDistanceTopSandbox(distance);
+
+ return 0;
+}
+
+
int SandboxConfig::loadCroppingMaskFrom(char *path, Camera *camera){
YAML::Node config;
@@ -178,24 +224,24 @@ int SandboxConfig::loadCroppingMaskFrom(char *path, Camera *camera){
try{
config = YAML::LoadFile(path);
}catch(YAML::BadFile err){
- std::cout << "[Error] No Config File found : " << err.what() << std::endl;
+ //std::cout << "[Error] No Config File found : " << err.what() << std::endl;
return 1;
}
// no node for cropping mask
- if(!config[MASK]){
+ if(!config[SCFG_MASK]){
return 2;
};
// uncomplet data for cropping mask
- if(!(config[MASK]["x"] && config[MASK]["y"] && config[MASK]["width"] && config[MASK]["height"])){
+ if(!(config[SCFG_MASK]["x"] && config[SCFG_MASK]["y"] && config[SCFG_MASK]["width"] && config[SCFG_MASK]["height"])){
return 2;
}
- cv::Rect mask( cv::Point( config[MASK]["x"].as<int>(),
- config[MASK]["y"].as<int>()),
- cv::Size( config[MASK]["width"].as<int>(),
- config[MASK]["height"].as<int>()));
+ cv::Rect mask( cv::Point( config[SCFG_MASK]["x"].as<int>(),
+ config[SCFG_MASK]["y"].as<int>()),
+ cv::Size( config[SCFG_MASK]["width"].as<int>(),
+ config[SCFG_MASK]["height"].as<int>()));
camera->setCroppingMask(mask);
@@ -210,23 +256,23 @@ int SandboxConfig::loadBeamerPositionFrom(char *path, Beamer *beamer){
try{
config = YAML::LoadFile(path);
}catch(YAML::BadFile err){
- std::cout << "[Error] No Config File found : " << err.what() << std::endl;
+ //std::cout << "[Error] No Config File found : " << err.what() << std::endl;
return 1;
}
// no node for beamer position
- if(!config[POSITION]){
+ if(!config[SCFG_POSITION]){
return 2;
};
// uncomplet data for beamer position
- if(!(config[POSITION]["x"] && config[POSITION]["y"] && config[POSITION]["z"])){
+ if(!(config[SCFG_POSITION]["x"] && config[SCFG_POSITION]["y"] && config[SCFG_POSITION]["z"])){
return 2;
}
- cv::Point3f pos( config[POSITION]["x"].as<float>(),
- config[POSITION]["y"].as<float>(),
- config[POSITION]["z"].as<float>());
+ cv::Point3f pos( config[SCFG_POSITION]["x"].as<float>(),
+ config[SCFG_POSITION]["y"].as<float>(),
+ config[SCFG_POSITION]["z"].as<float>());
beamer->setPosition(pos);
@@ -241,22 +287,22 @@ int SandboxConfig::loadBeamerResolutionFrom(char *path, Beamer *beamer){
try{
config = YAML::LoadFile(path);
}catch(YAML::BadFile err){
- std::cout << "[Error] No Config File found : " << err.what() << std::endl;
+ //std::cout << "[Error] No Config File found : " << err.what() << std::endl;
return 1;
}
// no node for beamer resolution
- if(!config[RESOLUTION]){
+ if(!config[SCFG_RESOLUTION]){
return 2;
};
// uncomplet data for beamer resolution
- if(!(config[RESOLUTION]["height"] && config[RESOLUTION]["width"])){
+ if(!(config[SCFG_RESOLUTION]["height"] && config[SCFG_RESOLUTION]["width"])){
return 2;
}
- cv::Size res( config[RESOLUTION]["width"].as<int>(),
- config[RESOLUTION]["height"].as<int>());
+ cv::Size res( config[SCFG_RESOLUTION]["width"].as<int>(),
+ config[SCFG_RESOLUTION]["height"].as<int>());
beamer->setWidth(res.width);
beamer->setHeight(res.height);
@@ -272,34 +318,34 @@ int SandboxConfig::loadFrameProcessProfil(char *path, FrameProcessProfil *profil
try{
config = YAML::LoadFile(path);
}catch(YAML::BadFile err){
- std::cout << "[Error] No Config File found : " << err.what() << std::endl;
+ //std::cout << "[Error] No Config File found : " << err.what() << std::endl;
return 1;
}
// no node for frame process profil
- if(!config[PROCESSPROFIL]){
+ if(!config[SCFG_PROCESS_PROFIL]){
return 2;
};
// uncomplet data for frame process profil
- if(!( config[PROCESSPROFIL]["contrast"] &&
- config[PROCESSPROFIL]["brightness"] &&
- config[PROCESSPROFIL]["minDistance"] &&
- config[PROCESSPROFIL]["cannyEdgeThreshold"] &&
- config[PROCESSPROFIL]["houghAccThreshold"] &&
- config[PROCESSPROFIL]["minRadius"] &&
- config[PROCESSPROFIL]["maxRadius"]
+ if(!( config[SCFG_PROCESS_PROFIL]["contrast"] &&
+ config[SCFG_PROCESS_PROFIL]["brightness"] &&
+ config[SCFG_PROCESS_PROFIL]["minDistance"] &&
+ config[SCFG_PROCESS_PROFIL]["cannyEdgeThreshold"] &&
+ config[SCFG_PROCESS_PROFIL]["houghAccThreshold"] &&
+ config[SCFG_PROCESS_PROFIL]["minRadius"] &&
+ config[SCFG_PROCESS_PROFIL]["maxRadius"]
)){
return 2;
}
- profil->setContrast(config[PROCESSPROFIL]["contrast"].as<double>());
- profil->setBrightness(config[PROCESSPROFIL]["brightness"].as<int>());
- profil->setMinDistance(config[PROCESSPROFIL]["minDistance"].as<uint>());
- profil->setCannyEdgeThreshold(config[PROCESSPROFIL]["cannyEdgeThreshold"].as<uint>());
- profil->setHoughAccThreshold(config[PROCESSPROFIL]["houghAccThreshold"].as<uint>());
- profil->setMinRadius(config[PROCESSPROFIL]["minRadius"].as<uint>());
- profil->setMaxRadius(config[PROCESSPROFIL]["maxRadius"].as<uint>());
+ profil->setContrast(config[SCFG_PROCESS_PROFIL]["contrast"].as<double>());
+ profil->setBrightness(config[SCFG_PROCESS_PROFIL]["brightness"].as<int>());
+ profil->setMinDistance(config[SCFG_PROCESS_PROFIL]["minDistance"].as<uint>());
+ profil->setCannyEdgeThreshold(config[SCFG_PROCESS_PROFIL]["cannyEdgeThreshold"].as<uint>());
+ profil->setHoughAccThreshold(config[SCFG_PROCESS_PROFIL]["houghAccThreshold"].as<uint>());
+ profil->setMinRadius(config[SCFG_PROCESS_PROFIL]["minRadius"].as<uint>());
+ profil->setMaxRadius(config[SCFG_PROCESS_PROFIL]["maxRadius"].as<uint>());
return 0;
}