fixed color

app/app.cpp

@@ -30,34 +30,34 @@ int main(int argc, char *argv[])
 void showLevel(){
 
     float top = sandbox.getProjection()->getDistanceTopSandbox();
-    float sandboxHeight = 0.1f;
+    float sandboxHeight = 0.2f;
     char windowName[] = "Sandbox";
     cv::namedWindow(windowName, CV_WINDOW_NORMAL);
     cv::setWindowProperty(windowName, CV_WND_PROP_FULLSCREEN, CV_WINDOW_FULLSCREEN);
-
+    
 
 //
 // Debug
 //
 
 /*
+    CALLGRIND_START_INSTRUMENTATION;
+    CALLGRIND_TOGGLE_COLLECT;
+
     sandbox.captureFrame();
     cv::Mat depth = sandbox.getDepthFrame();
     cv::Mat colored = colorizeDepth(depth, top, sandboxHeight);
     cv::Mat res;
     cv::cvtColor(colored, res, CV_BGR2RGB);
     
-    CALLGRIND_START_INSTRUMENTATION;
-    CALLGRIND_TOGGLE_COLLECT;
-    
     res = sandbox.adjustProjection(res);
+
+    cv::imshow(windowName, res);
     
+    cv::destroyAllWindows();
     CALLGRIND_TOGGLE_COLLECT;
     CALLGRIND_STOP_INSTRUMENTATION;
-
-    cv::imshow(windowName, res);
-
-*/  
+*/ 
 
 
 //
@@ -65,9 +65,18 @@ void showLevel(){
 //
 
 
+    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 colored = colorizeDepth(depth, top, sandboxHeight);
         cv::Mat res;
         cv::cvtColor(colored, res, CV_BGR2RGB);
@@ -77,14 +86,9 @@ void showLevel(){
         cv::imshow(windowName, res);
 
     } while (cv::waitKey(10) != ESCAPE_CHAR);
+    cv::destroyAllWindows();
 
 
-
-//
-// End
-//
-
-    cv::destroyAllWindows();
 }
 
 /*
@@ -93,8 +97,8 @@ void showLevel(){
 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);
+    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);
     
@@ -144,14 +148,16 @@ cv::Scalar floatToColor(float value, float min, float max){
     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);
+    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) );
-    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) );
+    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)) );
+    uint8_t b = static_cast<uint8_t>( initColors[index][2] + coeffColors[index][2] * (color % (CHANNEL_MAX+1)) );
 
     return cv::Scalar(r,g,b);
 }