diff --git a/app.cpp b/app.cpp
index 092da011eb46392da070bc4e7cf24fcdb4ca7580..7c7ff9358b1132a090ba6115e4000a413ac25ea3 100644
--- a/app.cpp
+++ b/app.cpp
@@ -1,35 +1,20 @@
-#include "../ar_sandbox_lib/inc/sandbox.h"
-#include <numeric>
-#include <fstream>
-#include <string>
-#include <opencv2/opencv.hpp>
+#include "app.h"
-using namespace std;
-using namespace cv;
-#define ESCAPE_CHAR 27
-
-
-
-Mat coloredFrame(Mat frameDepth);
-void showLevel();
-void showDiff();
-
-
-
-Sandbox client;
-void (*apps[2])() = {showLevel, showDiff};
+Sandbox sandbox;
+void (*apps[1])() = {showLevel};
int main(int argc, char *argv[])
{
-
- if(client.loadConfig()){
+ if(sandbox.loadConfig()){
std::cout << "Failed to load the configuration" << std::endl;
return 1;
}
- //showLevel();
+ sandbox.init();
+
+ apps[0]();
/*cout << "Press: \n 0: Show level \n 1: Show difference \n";
int n = 0;
@@ -38,32 +23,102 @@ int main(int argc, char *argv[])
apps[n]();*/
}
+/*
+* App n.1
+* Show levels in color
+*/
void showLevel(){
- Mat frameData;
- Mat colored;
+ float top = sandbox.getProjection()->getDistanceTopSandbox();
+ float sandboxHeight = 0.1f;
+ char windowName[] = "Sandbox";
+ cv::namedWindow(windowName, CV_WINDOW_NORMAL);
+ cv::setWindowProperty(windowName, CV_WND_PROP_FULLSCREEN, CV_WINDOW_FULLSCREEN);
- //Mat proj_frame;
- //char windowName[] = "Sandbox";
- //cv::namedWindow(windowName, CV_WINDOW_AUTOSIZE);
+ do{
+ cv::Mat depth = sandbox.getDepthFrame();
+ cv::Mat colored = colorizeDepth(depth, top, sandboxHeight);
+ cv::Mat res;
+ cv::cvtColor(colored, res, CV_BGR2RGB);
- client.initWindowsFullScreen();
+ res = sandbox.adjustProjection(res);
+ cv::imshow(windowName, res);
- do{
- client.getDepthFrame().copyTo(frameData);
- colored = coloredFrame(frameData);
+ } while (cv::waitKey(10) != ESCAPE_CHAR);
+
+ cv::destroyAllWindows();
+}
- //colored.copyTo(proj_frame);
- //Mat* res = client.adjustProjection(&proj_frame);
- //cv::imshow(windowName, *res);
+/*
+* Colorize depth frame
+*/
+cv::Mat colorizeDepth(cv::Mat depth, float sandboxTop, float sandboxHeight){
+
+ // matrix with depth from 0 to n, where 0 is the lowest point
+ cv::Mat normalizedDepth = (depth - sandboxTop) * -1.0f + sandboxHeight;
+ normalizedDepth.setTo(0.0f, normalizedDepth < 0.0f);
+ normalizedDepth.setTo(sandboxHeight, normalizedDepth > sandboxHeight);
+ cv::Mat res = cv::Mat(depth.rows, depth.cols, CV_8UC3);
+
+
+
+ for(int j=0; j<depth.rows; j++){
+ for(int i=0; i<depth.cols; i++){
+ //std::cout << "Color" << std::endl;
+ cv::Scalar color = floatToColor(normalizedDepth.at<float>(j,i), 0.0f, sandboxHeight);
+ res.at<cv::Vec3b>(j,i)[0] = color[0];
+ res.at<cv::Vec3b>(j,i)[1] = color[1];
+ res.at<cv::Vec3b>(j,i)[2] = color[2];
+ }
+ }
+
+ return res;
+}
- client.showImage(&colored);
- } while (waitKey(10) != ESCAPE_CHAR);
+/*
+* Return the color matching the value in the gradient color between min and max
+* Where :
+* red = highest point
+* blue = lowest point
+*/
+cv::Scalar floatToColor(float value, float min, float max){
+
+ const uint CHANNEL_MAX = 255;
+
+ // map for the color gradient (rgb channels)
+ uint initColors[4][3] = { { 0, 0, CHANNEL_MAX },
+ { 0, CHANNEL_MAX, CHANNEL_MAX },
+ { 0, CHANNEL_MAX, 0 },
+ { CHANNEL_MAX, CHANNEL_MAX, 0 } };
+
+ int coeffColors[4][3] = { { 0, 1, 0 },
+ { 0, 0, -1 },
+ { 1, 0, 0 },
+ { 0, -1, 0 } };
- cv::destroyAllWindows();
+
+ float clamped = value;
+ clamped = (clamped < min) ? min : clamped;
+ clamped = (clamped > max) ? max : clamped;
+
+ // values goes from 0 to relativeMax
+ float relative = clamped - min;
+ float relativeMax = max - min;
+
+ uint colorMax = 4*CHANNEL_MAX;
+ uint color = static_cast<uint>(relative * colorMax / relativeMax);
+ int index = static_cast<uint>(color / CHANNEL_MAX);
+
+ // red at the highest point, blue at the lowest
+ uint8_t r = static_cast<uint8_t>( initColors[index][0] + coeffColors[index][0] * (color % CHANNEL_MAX) );
+ uint8_t g = static_cast<uint8_t>( initColors[index][1] + coeffColors[index][1] * (color % CHANNEL_MAX) );
+ uint8_t b = static_cast<uint8_t>( initColors[index][2] + coeffColors[index][2] * (color % CHANNEL_MAX) );
+
+ return cv::Scalar(r,g,b);
}
+/*
Mat coloredFrame(Mat frameDepth){
Mat depthFrameColored(frameDepth.size(), CV_8U);
@@ -101,11 +156,12 @@ Mat coloredFrame(Mat frameDepth){
cv::applyColorMap(depthFrameColored, depthFrameColored, cv::COLORMAP_JET);
depthFrameColored.setTo(cv::Scalar(0, 0, 0), (frameDepth == 0));
return depthFrameColored;
+
}
void showDiff(){
-/*
+
Mat frameData;
client.getDepthFrame(&frameData);
Mat frameData;
@@ -129,5 +185,6 @@ void showDiff(){
keyCode = waitKey(10);
} while (keyCode!= ESCAPE_CHAR);
destroyAllWindows();
- */
+
}
+*/
\ No newline at end of file
diff --git a/app.h b/app.h
new file mode 100644
index 0000000000000000000000000000000000000000..0e1b397df92474303c9022102bf391592d462ae6
--- /dev/null
+++ b/app.h
@@ -0,0 +1,18 @@
+#ifndef MY_APPS_H
+#define MY_APPS_H
+
+#include "../ar_sandbox_lib/inc/sandbox.h"
+#include <opencv2/opencv.hpp>
+
+#define ESCAPE_CHAR 27
+
+
+void showLevel();
+//void showDiff();
+
+cv::Scalar floatToColor(float value, float min, float max);
+cv::Mat colorizeDepth(cv::Mat depth, float sandboxTop, float sandboxHeight);
+//Mat coloredFrame(Mat frameDepth);
+
+
+#endif
diff --git a/beamer_config.sh b/beamer_config.sh
index 8955109279bf1650c333a23ac5d631e3a4791e2c..b47b842a5a7d44abd5e913303ec4aa7c907ca15f 100755
--- a/beamer_config.sh
+++ b/beamer_config.sh
@@ -3,5 +3,6 @@ SCN_RES=1920x1080
BEAMER=HDMI-1
BM_RES=1400x1050
+
xrandr --output $SCREEN --rate 60 --mode $SCN_RES --fb $SCN_RES --panning $SCN_RES* \
--output $BEAMER --mode $BM_RES --same-as $SCREEN > /dev/null
diff --git a/sandbox_conf.yaml b/sandbox_conf.yaml
index 7dd1043be56a137175d13c641a7e15521da5de08..b7668c6ad260eb5afae40c8cbed7ada77f309f59 100644
--- a/sandbox_conf.yaml
+++ b/sandbox_conf.yaml
@@ -1,13 +1,26 @@
AdjustingMatrix:
width: 3
height: 2
- matrix: [1, 0, 0, -0, 1, 0]
+ matrix: [0.9998824, -0.0153356194, 3.71817994, 0.0153356194, 0.9998824, -4.87917471]
+DistanceTopSandbox:
+ distance: 1.10
CroppingMask:
- x: 160
- y: 120
- width: 320
- height: 245
+ x: 176
+ y: 121
+ width: 326
+ height: 250
+BeamerResolution:
+ width: 1400
+ height: 1050
BeamerPosition:
- x: 0
- y: 0
- z: 0
+ x: -0.0121798348
+ y: -0.146660045
+ z: 0.0548454896
+FrameProcessProfil:
+ contrast: 2.2999999999999936
+ brightness: -75
+ minDistance: 60
+ cannyEdgeThreshold: 137
+ houghAccThreshold: 30
+ minRadius: 4
+ maxRadius: 9