fixe color slight changes with margin error

Makefile

@@ -4,16 +4,16 @@ CFLAGS=-std=c++11 -Wall -Wextra -g
 CCP=g++
 
 all: 
-	$(MAKE) -C app
+	$(MAKE) -C apps
 
 run:
-	$(MAKE) run -C app
+	$(MAKE) run -C apps
 
 setup:
 	$(API_PATH)/app/SandboxSetup/SandboxSetup
 
 debug:
-	$(MAKE) debug -C app
+	$(MAKE) debug -C apps
 
 clean:
-	$(MAKE) clean -C app
+	$(MAKE) clean -C apps

apps/Makefile

+
+API_PATH=../../ar_sandbox_lib
+CFLAGS=-std=c++11 -Wall -Wextra -g
+CCP=g++
+
+all: 
+	$(MAKE) -C display_levels
+
+run:
+	$(MAKE) run -C display_levels
+
+debug:
+	$(MAKE) debug -C display_levels
+
+clean:
+	$(MAKE) clean -C display_levels

app/Makefile → apps/display_levels/Makefile

-include ../../ar_sandbox_lib/dep.mk
+include ../../../ar_sandbox_lib/dep.mk
 
-API_PATH=../../ar_sandbox_lib
+API_PATH=../../../ar_sandbox_lib
 CFLAGS=-std=c++11 -Wall -Wextra -g
 CCP=g++
 
-all: app
+all: display_levels
 
-app: app.o
+display_levels: display_levels.o
 	$(CCP) $^ -o $@ -L$(API_PATH)/build -lsandbox $(DEP_SANDBOX)
 
 %.o: %.cpp
 	$(CCP) $(CFLAGS) -I$(API_PATH)/inc -c $< -o $@ 
 
 run:
-	./app
+	./display_levels
 
 debug:
-	$(shell cd ../valgrind_profiles && valgrind --tool=callgrind --dump-instr=yes --simulate-cache=yes --collect-jumps=yes --collect-atstart=no --instr-atstart=no ../app/app)
+	$(shell cd ../../valgrind_profiles && valgrind --tool=callgrind --dump-instr=yes --simulate-cache=yes --collect-jumps=yes --collect-atstart=no --instr-atstart=no ../apps/display_levels/display_levels)
 
 clean:
-	rm -f *.o app
+	rm -f *.o display_levels

app/app.cpp → apps/display_levels/display_levels.cpp

 
-#include "app.h"
+#include "display_levels.h"
 #include <valgrind/callgrind.h>
 
-Sandbox sandbox;
-void (*apps[1])() = {showLevel};
 
-int main(int argc, char *argv[])
-{
-    if(sandbox.loadConfigFrom("../sandbox_conf.yaml")){
+int main(){
+
+    Sandbox sandbox;
+    
+    // Default
+    char *sandbox_conf_file = (char*)"../../sandbox_conf.yaml";
+  
+    // Debug
+//    char * sandbox_conf_file = "../sandbox_conf.yaml";
+
+    if(sandbox.loadConfigFrom(sandbox_conf_file)){
         std::cout << "Failed to load the configuration" << std::endl;
         return 1;
     }
 
-    sandbox.init();
-
-    apps[0]();
-
-    /*cout << "Press: \n 0: Show level \n 1: Show difference \n";
-    int n = 0;
-    cin >> n;
+    
+    if(sandbox.init()){
+        std::cout << "Failed to initilize sandbox" << std::endl;
+        return 1;
+    }
 
-    apps[n]();*/
+    displayLevels(sandbox);
 }
 
+
 /*
-*   App n.1
-*   Show levels in color
+*   Show levels of the sandbox in color
 */
-void showLevel(){
+void displayLevels(Sandbox &sandbox){
 
-    float top = sandbox.getProjection()->getDistanceTopSandbox();
     float sandboxHeight = 0.2f;
+    // top => distance from camera to the top of the sandbox
+    float top = sandbox.getProjection()->getDistanceTopSandbox();
+        
+    // Needed because the values retrives from depth frames can change a bit although nothing has changed physically
+    const float DEPTH_MARGIN_ERROR = 0.01f;
+
     char windowName[] = "Sandbox";
     cv::namedWindow(windowName, CV_WINDOW_NORMAL);
     cv::setWindowProperty(windowName, CV_WND_PROP_FULLSCREEN, CV_WINDOW_FULLSCREEN);
     
+    sandbox.captureFrame();
+    cv::Mat depth_cache = sandbox.getDepthFrame();
 
 //
-// Debug
+// Default
 //
 
-/*
-    CALLGRIND_START_INSTRUMENTATION;
-    CALLGRIND_TOGGLE_COLLECT;
+    do{
+        sandbox.captureFrame();
+        cv::Mat new_depth = sandbox.getDepthFrame();
+        cv::Mat mask_diff = cv::abs(depth_cache-new_depth) > DEPTH_MARGIN_ERROR;
+        new_depth.copyTo(depth_cache, mask_diff);
 
-    sandbox.captureFrame();
-    cv::Mat depth = sandbox.getDepthFrame();
-    cv::Mat colored = colorizeDepth(depth, top, sandboxHeight);
-    cv::Mat res;
-    cv::cvtColor(colored, res, CV_BGR2RGB);
-    
-    res = sandbox.adjustProjection(res);
+        cv::Mat colorized = colorizeDepth(depth_cache, top-sandboxHeight, sandboxHeight);
+        cv::Mat res;
+        cv::cvtColor(colorized, res, CV_RGB2BGR);
+
+        res = sandbox.adjustProjection(res);
+
+        cv::imshow(windowName, res);
+
+    } while (cv::waitKey(10) != ESCAPE_CHAR);
 
-    cv::imshow(windowName, res);
-    
     cv::destroyAllWindows();
-    CALLGRIND_TOGGLE_COLLECT;
-    CALLGRIND_STOP_INSTRUMENTATION;
-*/ 
 
 
 //
-// Default
+// Debug
 //
 
+/*
+    CALLGRIND_START_INSTRUMENTATION;
+    CALLGRIND_TOGGLE_COLLECT;
 
-    const float depth_margin = 0.02f;
     sandbox.captureFrame();
-    cv::Mat depth = sandbox.getDepthFrame();
-
-    do{
-        sandbox.captureFrame();
-        cv::Mat depth = sandbox.getDepthFrame();
-        //cv::Mat new_depth = sandbox.getDepthFrame();
-        //cv::Mat mask_diff = cv::abs(depth-new_depth) > depth_margin;
-        //if(cv::countNonZero(mask_diff))
-        //    depth.setTo(new_depth, mask_diff);
+    cv::Mat new_depth = sandbox.getDepthFrame();
+    cv::Mat mask_diff = cv::abs(depth_cache-new_depth) > DEPTH_MARGIN_ERROR;
+    new_depth.copyTo(depth_cache, mask_diff);
 
-        cv::Mat colored = colorizeDepth(depth, top, sandboxHeight);
-        cv::Mat res;
-        cv::cvtColor(colored, res, CV_BGR2RGB);
+    cv::Mat colorized = colorizeDepth(depth_cache, top-sandboxHeight, sandboxHeight);
+    cv::Mat res;
+    cv::cvtColor(colorized, res, CV_BGR2RGB);
 
-        res = sandbox.adjustProjection(res);
+    res = sandbox.adjustProjection(res);
 
-        cv::imshow(windowName, res);
+    cv::imshow(windowName, res);
 
-    } while (cv::waitKey(10) != ESCAPE_CHAR);
     cv::destroyAllWindows();
+    
+    CALLGRIND_TOGGLE_COLLECT;
+    CALLGRIND_STOP_INSTRUMENTATION;
 
+*/
 
 }
 
+
 /*
 *   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
+    // Matrix with depth from 0 to n, where 0 is the lowest point
     cv::Mat normalizedDepth = depth * -1.0f + (sandboxTop + sandboxHeight);
     normalizedDepth.setTo(0, normalizedDepth < 0);
     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];
@@ -118,42 +127,47 @@ cv::Mat colorizeDepth(cv::Mat depth, float sandboxTop, float sandboxHeight){
 }
 
 
+
 /*
 *   Return the color matching the value in the gradient color between min and max
 *   Where :
 *       red = highest point
 *       blue = lowest point
+*   
+*   Color order :
+*       blue > cyan > green > yellow > red
 */
 cv::Scalar floatToColor(float value, float min, float max){
 
     const uint CHANNEL_MAX = 255;
 
-    // map for the color gradient (rgb channels)
+    // 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 } };
-
+    
+    // Indicates how the value changes :
+    //  - when reducing channel value   : 255 + (-1 * value)
+    //  - when increasing channel value :   0 + ( 1 * value)
     int coeffColors[4][3] = { {  0,  1,  0 },
                               {  0,  0, -1 },
                               {  1,  0,  0 },
                               {  0, -1,  0 } };
     
-
+    // values go from min to max
     float clamped = value;
     clamped = (clamped < min) ? min : clamped;
     clamped = (clamped > max) ? max : clamped;
 
-    // values goes from 0 to relativeMax
+    // values go from 0 to relativeMax
     float relative = clamped - min;
     float relativeMax = max - min;
 
     const uint COLOR_MAX = 4*(CHANNEL_MAX+1)-1;
     uint color = static_cast<uint>(relative * COLOR_MAX / relativeMax);
     uint index = static_cast<uint>(color / (CHANNEL_MAX+1));
-
     
-
     // 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+1)) );
     uint8_t g = static_cast<uint8_t>( initColors[index][1] + coeffColors[index][1] * (color % (CHANNEL_MAX+1)) );
@@ -161,74 +175,3 @@ cv::Scalar floatToColor(float value, float min, float max){
 
     return cv::Scalar(r,g,b);
 }
-
-/*
-Mat coloredFrame(Mat frameDepth){
-
-    Mat depthFrameColored(frameDepth.size(), CV_8U);
-    int width = frameDepth.cols, height = frameDepth.rows;
-    static uint32_t histogram[0x10000];
-    memset(histogram, 0, sizeof(histogram));
-
-    for (int i = 0; i < height; ++i)
-    {
-        for (int j = 0; j < width; ++j)
-        {
-            ++histogram[frameDepth.at<ushort>(i, j)];
-        }
-    }
-
-    for (int i = 2; i < 0x10000; ++i)
-        histogram[i] += histogram[i - 1]; // Build a cumulative histogram for the indices in [1,0xFFFF]
-
-    for (int i = 0; i < height; ++i)
-    {
-        for (int j = 0; j < width; ++j)
-        {
-            if (uint16_t d = frameDepth.at<ushort>(i, j))
-            {
-                int f = histogram[d] * 255 / histogram[0xFFFF]; // 0-255 based on histogram location
-                depthFrameColored.at<uchar>(i, j) = static_cast<uchar>(f);
-            }
-            else
-            {
-                depthFrameColored.at<uchar>(i, j) = 0;
-            }
-        }
-    }
-    cv::bitwise_not(depthFrameColored, depthFrameColored); //reverse colormap
-    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;
-    client.getDepthFrame(&frameData);
-    resize(frameBase, frameBase, frameData.size()); //to match with camera frame
-    Mat diff(frameData.size(), CV_16S);
-    Mat frameColor(frameData.size(), CV_8UC3, Scalar(0, 0, 0));
-    int toBlue[] = {0, 2};
-    int toRed[] = {0, 0};
-    int keyCode;
-    do
-    {
-        client.getDepthFrame(&frameData);
-        subtract(frameBase, frameData, diff, noArray(), CV_16S);
-        Mat isNeg = diff < -5;
-        Mat isPos = diff > 5;
-        //colorize red & blue
-        mixChannels(&isNeg, 1, &frameColor, 1, toBlue, 1);
-        mixChannels(&isPos, 1, &frameColor, 1, toRed, 1);
-        client.showImage(frameColor);
-        keyCode = waitKey(10);
-    } while (keyCode!= ESCAPE_CHAR);
-    destroyAllWindows();
-    
-}
-*/
\ No newline at end of file

app/app.h → apps/display_levels/display_levels.h

-#ifndef MY_APPS_H
-#define MY_APPS_H
+#ifndef APP_DISPLAY_LEVELS_H
+#define APP_DISPLAY_LEVELS_H
 
 #include <sandbox.h>
 #include <opencv2/opencv.hpp>
@@ -7,12 +7,9 @@
 #define ESCAPE_CHAR 27
 
 
-void showLevel();
-//void showDiff();
+void displayLevels(Sandbox &sandbox);
 
 cv::Scalar floatToColor(float value, float min, float max);
 cv::Mat colorizeDepth(cv::Mat depth, float sandboxTop, float sandboxHeight);
-//Mat coloredFrame(Mat frameDepth);
-
 
 #endif