Skip to content
Snippets Groups Projects
Commit 40c1ae75 authored by simon.fanetti's avatar simon.fanetti
Browse files

fixe color slight changes with margin error

parent a12a9bd5
No related branches found
No related tags found
No related merge requests found
Expand all Hide whitespace changes
Inline Side-by-side
Makefile
+ 4
− 4
View file @ 40c1ae75
...@@ -4,16 +4,16 @@ CFLAGS=-std=c++11 -Wall -Wextra -g ...@@ -4,16 +4,16 @@ CFLAGS=-std=c++11 -Wall -Wextra -g
CCP=g++ CCP=g++
all: all:
$(MAKE) -C app $(MAKE) -C apps
run: run:
$(MAKE) run -C app $(MAKE) run -C apps
setup: setup:
$(API_PATH)/app/SandboxSetup/SandboxSetup $(API_PATH)/app/SandboxSetup/SandboxSetup
debug: debug:
$(MAKE) debug -C app $(MAKE) debug -C apps
clean: clean:
$(MAKE) clean -C app $(MAKE) clean -C apps
apps/Makefile 0 → 100644
+ 16
− 0
View file @ 40c1ae75
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
+ 22
− 0
View file @ 40c1ae75
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 CFLAGS=-std=c++11 -Wall -Wextra -g
CCP=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) $(CCP) $^ -o $@ -L$(API_PATH)/build -lsandbox $(DEP_SANDBOX)
%.o: %.cpp %.o: %.cpp
$(CCP) $(CFLAGS) -I$(API_PATH)/inc -c $< -o $@ $(CCP) $(CFLAGS) -I$(API_PATH)/inc -c $< -o $@
run: run:
./app ./display_levels
debug: 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: clean:
rm -f *.o app rm -f *.o display_levels
app/app.cpp → apps/display_levels/display_levels.cpp
+ 74
− 131
View file @ 40c1ae75
#include "app.h" #include "display_levels.h"
#include <valgrind/callgrind.h> #include <valgrind/callgrind.h>
Sandbox sandbox;
void (*apps[1])() = {showLevel};
int main(int argc, char *argv[]) int main(){
{
if(sandbox.loadConfigFrom("../sandbox_conf.yaml")){ 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; std::cout << "Failed to load the configuration" << std::endl;
return 1; return 1;
} }
sandbox.init();
if(sandbox.init()){
apps[0](); std::cout << "Failed to initilize sandbox" << std::endl;
return 1;
/*cout << "Press: \n 0: Show level \n 1: Show difference \n"; }
int n = 0;
cin >> n;
apps[n]();*/ displayLevels(sandbox);
} }
/* /*
* App n.1 * Show levels of the sandbox in color
* Show levels in color
*/ */
void showLevel(){ void displayLevels(Sandbox &sandbox){
float top = sandbox.getProjection()->getDistanceTopSandbox();
float sandboxHeight = 0.2f; 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"; char windowName[] = "Sandbox";
cv::namedWindow(windowName, CV_WINDOW_NORMAL); cv::namedWindow(windowName, CV_WINDOW_NORMAL);
cv::setWindowProperty(windowName, CV_WND_PROP_FULLSCREEN, CV_WINDOW_FULLSCREEN); cv::setWindowProperty(windowName, CV_WND_PROP_FULLSCREEN, CV_WINDOW_FULLSCREEN);
sandbox.captureFrame();
cv::Mat depth_cache = sandbox.getDepthFrame();
// //
// Debug // Default
// //
/* do{
CALLGRIND_START_INSTRUMENTATION; sandbox.captureFrame();
CALLGRIND_TOGGLE_COLLECT; 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 colorized = colorizeDepth(depth_cache, top-sandboxHeight, sandboxHeight);
cv::Mat depth = sandbox.getDepthFrame(); cv::Mat res;
cv::Mat colored = colorizeDepth(depth, top, sandboxHeight); cv::cvtColor(colorized, res, CV_RGB2BGR);
cv::Mat res;
cv::cvtColor(colored, res, CV_BGR2RGB); res = sandbox.adjustProjection(res);
res = sandbox.adjustProjection(res); cv::imshow(windowName, res);
} while (cv::waitKey(10) != ESCAPE_CHAR);
cv::imshow(windowName, res);
cv::destroyAllWindows(); cv::destroyAllWindows();
CALLGRIND_TOGGLE_COLLECT;
CALLGRIND_STOP_INSTRUMENTATION;
*/
// //
// Default // Debug
// //
/*
CALLGRIND_START_INSTRUMENTATION;
CALLGRIND_TOGGLE_COLLECT;
const float depth_margin = 0.02f;
sandbox.captureFrame(); sandbox.captureFrame();
cv::Mat depth = sandbox.getDepthFrame(); cv::Mat new_depth = sandbox.getDepthFrame();
cv::Mat mask_diff = cv::abs(depth_cache-new_depth) > DEPTH_MARGIN_ERROR;
do{ new_depth.copyTo(depth_cache, mask_diff);
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 colorized = colorizeDepth(depth_cache, top-sandboxHeight, sandboxHeight);
cv::Mat res; cv::Mat res;
cv::cvtColor(colored, res, CV_BGR2RGB); 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(); cv::destroyAllWindows();
CALLGRIND_TOGGLE_COLLECT;
CALLGRIND_STOP_INSTRUMENTATION;
*/
} }
/* /*
* Colorize depth frame * Colorize depth frame
*/ */
cv::Mat colorizeDepth(cv::Mat depth, float sandboxTop, float sandboxHeight){ 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); cv::Mat normalizedDepth = depth * -1.0f + (sandboxTop + sandboxHeight);
normalizedDepth.setTo(0, normalizedDepth < 0); normalizedDepth.setTo(0, normalizedDepth < 0);
normalizedDepth.setTo(sandboxHeight, normalizedDepth > sandboxHeight); normalizedDepth.setTo(sandboxHeight, normalizedDepth > sandboxHeight);
cv::Mat res = cv::Mat(depth.rows, depth.cols, CV_8UC3); cv::Mat res = cv::Mat(depth.rows, depth.cols, CV_8UC3);
for(int j=0; j<depth.rows; j++){ for(int j=0; j<depth.rows; j++){
for(int i=0; i<depth.cols; i++){ 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); 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)[0] = color[0];
res.at<cv::Vec3b>(j,i)[1] = color[1]; res.at<cv::Vec3b>(j,i)[1] = color[1];
...@@ -118,42 +127,47 @@ cv::Mat colorizeDepth(cv::Mat depth, float sandboxTop, float sandboxHeight){ ...@@ -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 * Return the color matching the value in the gradient color between min and max
* Where : * Where :
* red = highest point * red = highest point
* blue = lowest point * blue = lowest point
*
* Color order :
* blue > cyan > green > yellow > red
*/ */
cv::Scalar floatToColor(float value, float min, float max){ cv::Scalar floatToColor(float value, float min, float max){
const uint CHANNEL_MAX = 255; 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 }, uint initColors[4][3] = { { 0, 0, CHANNEL_MAX },
{ 0, CHANNEL_MAX, CHANNEL_MAX }, { 0, CHANNEL_MAX, CHANNEL_MAX },
{ 0, CHANNEL_MAX, 0 }, { 0, CHANNEL_MAX, 0 },
{ CHANNEL_MAX, 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 }, int coeffColors[4][3] = { { 0, 1, 0 },
{ 0, 0, -1 }, { 0, 0, -1 },
{ 1, 0, 0 }, { 1, 0, 0 },
{ 0, -1, 0 } }; { 0, -1, 0 } };
// values go from min to max
float clamped = value; float clamped = value;
clamped = (clamped < min) ? min : clamped; clamped = (clamped < min) ? min : clamped;
clamped = (clamped > max) ? max : clamped; clamped = (clamped > max) ? max : clamped;
// values goes from 0 to relativeMax // values go from 0 to relativeMax
float relative = clamped - min; float relative = clamped - min;
float relativeMax = max - min; float relativeMax = max - min;
const uint COLOR_MAX = 4*(CHANNEL_MAX+1)-1; const uint COLOR_MAX = 4*(CHANNEL_MAX+1)-1;
uint color = static_cast<uint>(relative * COLOR_MAX / relativeMax); uint color = static_cast<uint>(relative * COLOR_MAX / relativeMax);
uint index = static_cast<uint>(color / (CHANNEL_MAX+1)); uint index = static_cast<uint>(color / (CHANNEL_MAX+1));
// red at the highest point, blue at the lowest // 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 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 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){ ...@@ -161,74 +175,3 @@ cv::Scalar floatToColor(float value, float min, float max){
return cv::Scalar(r,g,b); 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
+ 3
− 6
View file @ 40c1ae75
#ifndef MY_APPS_H #ifndef APP_DISPLAY_LEVELS_H
#define MY_APPS_H #define APP_DISPLAY_LEVELS_H
#include <sandbox.h> #include <sandbox.h>
#include <opencv2/opencv.hpp> #include <opencv2/opencv.hpp>
...@@ -7,12 +7,9 @@ ...@@ -7,12 +7,9 @@
#define ESCAPE_CHAR 27 #define ESCAPE_CHAR 27
void showLevel(); void displayLevels(Sandbox &sandbox);
//void showDiff();
cv::Scalar floatToColor(float value, float min, float max); cv::Scalar floatToColor(float value, float min, float max);
cv::Mat colorizeDepth(cv::Mat depth, float sandboxTop, float sandboxHeight); cv::Mat colorizeDepth(cv::Mat depth, float sandboxTop, float sandboxHeight);
//Mat coloredFrame(Mat frameDepth);
#endif #endif
valgrind_profiles/callgrind.out.21305 deleted 100644 → 0
+ 0
− 70377
View file @ a12a9bd5
This diff is collapsed.
valgrind_profiles/callgrind.out.8382 0 → 100644
+ 135457
− 0
View file @ 40c1ae75
This diff is collapsed.
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment