adapt to sandbox lib

app.cpp

 
-#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

app.h

+#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

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

sandbox_conf.yaml

 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