From 16046ef41e1bbf06867ce622f42e145c903eb74f Mon Sep 17 00:00:00 2001 From: Dimitris Sgouropoulos Date: Thu, 5 May 2016 16:16:17 +0300 Subject: [PATCH] added vision lib --- chroma/CMakeLists.txt | 14 +- chroma/CMakeLists.txt~ | 13 +- chroma/include/chroma.hpp | 34 +- chroma/src/chroma.cpp | 445 ------------ depth/CMakeLists.txt | 11 +- depth/CMakeLists.txt~ | 193 +---- depth/include/depth.hpp | 57 +- depth/src/depth.cpp | 1390 ------------------------------------ fusion/CMakeLists.txt | 10 +- fusion/CMakeLists.txt~ | 2 +- fusion/include/fusion.hpp | 47 +- fusion/src/fusion.cpp | 791 +-------------------- vision/CMakeLists.txt | 40 ++ vision/include/vision.hpp | 84 +++ vision/src/vision.cpp | 1408 +++++++++++++++++++++++++++++++++++++ 15 files changed, 1602 insertions(+), 2937 deletions(-) create mode 100644 vision/CMakeLists.txt create mode 100644 vision/include/vision.hpp create mode 100644 vision/src/vision.cpp diff --git a/chroma/CMakeLists.txt b/chroma/CMakeLists.txt index f77afa9..c44bb20 100644 --- a/chroma/CMakeLists.txt +++ b/chroma/CMakeLists.txt @@ -3,9 +3,6 @@ project(chroma) set(CMAKE_CXX_FLAGS "-std=c++11") -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages find_package(catkin REQUIRED COMPONENTS cv_bridge image_transport @@ -22,15 +19,22 @@ aux_source_directory(src/ SRC_LIST) include_directories( include + /usr/include/ ${catkin_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS} ) -add_executable(chroma ${SRC_LIST}) -link_directories(/usr/include /usr/lib/x86_64-linux-gnu/) +link_directories( + /usr/lib/ + /usr/local/lib/ + /usr/lib/x86_64-linux-gnu/ +) + +add_executable(chroma ${SRC_LIST}) target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} + libvision.so ) diff --git a/chroma/CMakeLists.txt~ b/chroma/CMakeLists.txt~ index d8da7b4..c44bb20 100644 --- a/chroma/CMakeLists.txt~ +++ b/chroma/CMakeLists.txt~ @@ -3,9 +3,6 @@ project(chroma) set(CMAKE_CXX_FLAGS "-std=c++11") -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages find_package(catkin REQUIRED COMPONENTS cv_bridge image_transport @@ -22,16 +19,22 @@ aux_source_directory(src/ SRC_LIST) include_directories( include + /usr/include/ ${catkin_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS} ) -add_executable(chroma ${SRC_LIST}) +link_directories( + /usr/lib/ + /usr/local/lib/ + /usr/lib/x86_64-linux-gnu/ +) -link_directories(/usr/include /usr/lib/x86_64-linux-gnu/) +add_executable(chroma ${SRC_LIST}) target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} + libvision.so ) diff --git a/chroma/include/chroma.hpp b/chroma/include/chroma.hpp index 899ceeb..79b33fd 100644 --- a/chroma/include/chroma.hpp +++ b/chroma/include/chroma.hpp @@ -21,6 +21,7 @@ #include #include #include +#include using namespace std; using namespace cv; @@ -28,42 +29,12 @@ using namespace cv; class Chroma_processing { public: - - struct Position - { - float x = 0.0; - float y = 0.0; - float z = 0.0; - float height = 0.0; - float distance = 0.0; - }; - - struct People - { - vector< Rect_ > tracked_boxes; - vector< float > tracked_rankings; - vector< Position > tracked_pos; - }; - - + Chroma_processing(); ~Chroma_processing(); //image and depth callbacks void imageCb(const sensor_msgs::ImageConstPtr& msg); - - //Background & foreground estimation, to be used in sequence - void estimateBackground(Mat& src, Mat& dst, vector& storage, int recursion, float ratio = 0.04, int index = 0); - void estimateForeground(Mat& src1, Mat& src2, Mat& dst); - - //Detection and tracking of blobs - void detectBlobs(const Mat& src, vector< Rect_ >& colour_areas, int range); - void track(vector< Rect_ >& current, People& collection, int rank = 3, float threshold = 0.2); - - //helper functions - int threshold(Mat& src, Mat& dst, int thresh); - inline void gammaCorrection(Mat& src); - void frameDif(Mat& src1, Mat& src2, Mat& dst, float threshold); private: @@ -86,7 +57,6 @@ class Chroma_processing vector< Rect_ > rgb_rects; - bool playback_topics; bool display; bool has_image = false; diff --git a/chroma/src/chroma.cpp b/chroma/src/chroma.cpp index e3b704b..1b39772 100644 --- a/chroma/src/chroma.cpp +++ b/chroma/src/chroma.cpp @@ -158,451 +158,6 @@ void Chroma_processing::imageCb(const sensor_msgs::ImageConstPtr& msg) } -/* Detects non-black areas in the image and populates a list of OpenCV Rects - * - * PARAMETERS: -the vector that will be populated - * -screen width - * - * RETURN -- - */ -void Chroma_processing::detectBlobs(const Mat& src, vector< Rect_ >& colour_areas, int range) -{ - float detectionFactor = 0.1; - float mergeFactor = 0.1; - bool flag = false; - int channels = src.channels(); - int cols = src.cols; - int rows = src.rows; - int size = cols*rows*channels; - for(int y = 0; y < 1; y++) - { - const uchar *dif = src.ptr(y); - for(int x = 0; x < size; x = x + channels) - { - if(dif[x] != 0) - { - int i ,j, o; - i = (x/channels)%(cols); - j = floor(x/(cols*channels)); - - Rect_ removal; - if(i + range >= cols) - continue; - if(j + range >= rows) - continue; - - removal = Rect(i, j, range , range); - if(removal.width < 1 || removal.height < 1) - continue; - - if(!colour_areas.empty()) - { - for(int k = 0; k < colour_areas.size(); k++) - { - Rect_ rect = colour_areas[k]; - Rect all = removal | rect; - int temp = removal.y; - removal.y = rect.y; - Rect intersection = removal & rect; - removal.y = temp; - int threshold = intersection.area(); - if(threshold < 1) - continue; - - if(removal.area() < rect.area()) - { - if(threshold > detectionFactor*removal.area()) - { - colour_areas[k] = all; - flag = true; - } - } - else - { - if(threshold > detectionFactor*rect.area()) - { - colour_areas[k] = all; - flag = true; - } - } - } - if(!flag) - { - colour_areas.push_back(removal); - } - else - flag = false; - - int end = colour_areas.size(); - for(int a = 0; a < end; a++) - { - for(int b = a + 1; b < end; b++) - { - Rect_ removal = colour_areas[a]; - Rect_ rect = colour_areas[b]; - - Rect all = removal | rect; - int temp = removal.y; - removal.y = rect.y; - Rect intersection = removal & rect; - removal.y = temp; - int threshold = intersection.area(); - if(threshold < 1) - continue; - - if(removal.area() < rect.area()) - { - if(threshold > mergeFactor*removal.area()) - { - colour_areas[a] = all; - colour_areas[b] = colour_areas.back(); - colour_areas.pop_back(); - b = a + 1; - end--; - } - } - else - { - if(threshold > mergeFactor*rect.area()) - { - colour_areas[a] = all; - colour_areas[b] = colour_areas.back(); - colour_areas.pop_back(); - b = a + 1; - end--; - } - } - - } - } - - - } - else - { - colour_areas.push_back(removal); - } - - } - } - } - int end = colour_areas.size(); - - for(int k = 0; k < end; k++) - { - float width = colour_areas[k].width; - float height = colour_areas[k].height; - float x = colour_areas[k].x; - float y = colour_areas[k].y; - if((x < 0) || (y < 0) || (height < 0) || (width < 0)) - { - //~ rectangle(src, colour_areas[k], 0, CV_FILLED); - cout<<"done"< 1.5) || (area < cols*rows*0.02)) - { - //~ rectangle(src, colour_areas[k], 0, CV_FILLED); - colour_areas[k] = colour_areas.back(); - colour_areas.pop_back(); - k--; - end--; - - } - } - - /* - for(int k = 0; k < end; k++) - { - if(colour_areas[k].width > 250) - { - colour_areas[k].width = colour_areas[k].width/2; - Rect_ rect = colour_areas[k]; - rect.x = rect.x + rect.width; - rect.width = colour_areas[k].width; - colour_areas.push_back(rect); - k--; - end++; - - } - - } - */ - - //~ cout<<"Size: "< >& cur_boxes, People& collection, int rank, float threshold) -{ - bool exists = false; - float step = 1.2; - - - if(!cur_boxes.empty()) - { - - for(int a = 0; a < cur_boxes.size(); a++) - { - exists = false; - for(int b = 0; b < collection.tracked_boxes.size(); b++) - { - Rect intersection = cur_boxes[a] & collection.tracked_boxes[b]; - int area = intersection.area(); - if(cur_boxes[a].area() < collection.tracked_boxes[b].area()) - { - if(area > threshold*cur_boxes[a].area()) - { - collection.tracked_boxes[b] = cur_boxes[a]; - if(collection.tracked_rankings[b] <= 10) - collection.tracked_rankings[b] = collection.tracked_rankings[b] + step; - exists = true; - break; - } - } - else - { - if(area > threshold*collection.tracked_boxes[b].area()) - { - collection.tracked_boxes[b] = cur_boxes[a]; - if(collection.tracked_rankings[b] <= 10) - collection.tracked_rankings[b] = collection.tracked_rankings[b] + step; - exists = true; - break; - } - } - - } - if(!exists) - { - collection.tracked_boxes.push_back(cur_boxes[a]); - collection.tracked_rankings.push_back(rank + step); - } - } - for(int a = 0; a < collection.tracked_boxes.size(); a++) - { - collection.tracked_rankings[a] = collection.tracked_rankings[a] - 1; - } - for(int a = 0; a < collection.tracked_boxes.size(); a++) - { - if(collection.tracked_rankings[a] <= 0 && collection.tracked_boxes.size() > 0) - { - collection.tracked_boxes[a] = collection.tracked_boxes.back(); - collection.tracked_boxes.pop_back(); - collection.tracked_rankings[a] = collection.tracked_rankings.back(); - collection.tracked_rankings.pop_back(); - a--; - } - - } - - } -} - -/* Estimates the foreground by combining static images, works for - * images the contain edge information - * - * @param a mat representing the current image frame, a mat to store the result, - * the number of images to combine - * @return - - */ -void Chroma_processing::estimateForeground(Mat& cur_Mat, Mat& back_Mat, Mat& dst_Mat) -{ - uchar *back, *cur, *dst; - for(int y = 0; y < 1; y++) - { - int rows = cur_Mat.rows; - int cols = cur_Mat.cols; - int channels = cur_Mat.channels(); - int size = rows*cols*channels; - - back = back_Mat.ptr(y); - cur = cur_Mat.ptr(y); - dst = dst_Mat.ptr(y); - for(int x = 0; x < size; x = x + channels) - { - if(back[x] != 0) - { - dst[x] = 0; - } - } - } -} - -/* Estimates the background by combining static images, works for - * images the contain edge information - * - * @param a mat representing the current image frame, a mat to store the result, - * the number of images to combine - * @return - - */ -void Chroma_processing::estimateBackground(Mat& src, Mat& dst, vector& storage, int recursion, float ratio, int index) -{ - int size = storage.size(); - Mat result; - if (index > recursion) - { - dst = storage.at(index - 1); - return; - } - if(size > 0) - { - if((index <= 0) || ((index >= recursion*ratio && (size >= recursion*ratio)))) - { - bitwise_or(src, storage.at(index), result); - } - else - bitwise_and(src, storage.at(index), result); - if((size -1) == index) - { - storage.push_back(result); - dst = result; - } - else - { - storage.at(index) = src; - index++; - estimateBackground(result, dst, storage, recursion, ratio, index); - } - } - else - { - storage.push_back(src); - dst = src.clone(); - } -} - -/*Calculates the absolute difference between the two mats and thresholds the result according to the threshold given - * - * PARAMETERS: - * -Mat first - * -Mat second - * -Mat with the result - * -float threshold to be used - * - * RETURN: -- - */ -void Chroma_processing::frameDif(Mat& src1, Mat& src2, Mat& dst, float threshold) -{ - uchar *src, *temp, *dest; - int channels = src1.channels(); - int cols = src1.cols; - int rows = src1.rows; - int size = cols*rows*channels; - int motionCounter = 0; - - Mat temp_Mat; - // Absolute dif between our current mat and the previous one - absdiff(src1, src2, temp_Mat); - dst = src1.clone(); - for(int y = 0; y < 1; y++) - { - src = src1.ptr(y); - temp = temp_Mat.ptr(y); - dest = dst.ptr(y); - for(int x = 0; x < size; x = x + channels) - { - float all = 0.0; - float color[channels]; - for (int ch = 0; ch < channels; ch++) - color[ch] = (float)temp[x + ch]; - - for (int i = 0; i < channels; i++) - all += color[i]; - all /= channels; - - if(all > threshold) - { - for (int ch = 0; ch < channels; ch++) - dest[x + ch] = src[x + ch]; - motionCounter++; - } - else - { - for (int ch = 0; ch < channels; ch++) - dest[x + ch] = 0; - } - } - } -} - -/* Thresholds an image to zero (i > thresh -> 0) - * - * @param the Mat to be processed, the threshold to use - * @return the number of pixels above the threshold - */ -int Chroma_processing::threshold(Mat& src, Mat& dst, int thresh) -{ - - int motionCounter = 0; - int end = src.cols*src.rows*src.channels(); - int step = src.channels(); - for(int y = 0; y < 1; y++) - { - uchar *cur = src.ptr(y); - uchar *temp = dst.ptr(y); - for(int x = 0; x < end; x = x + step) - { - double bPercent = double(cur[x]) ; - if(bPercent > thresh) - motionCounter++; - else - { - temp[x] = 0; - temp[x + 1] = 0; - temp[x + 2] = 0; - } - } - - } - return motionCounter; - -} - -/* Image gamma correction - * - * @param the Mat to be processed - * @return - - */ -void Chroma_processing::gammaCorrection(Mat& src) -{ - double inverse_gamma = 1.0 / 2.2; - - Mat lut_matrix(1, 256, CV_8UC1 ); - uchar * ptr = lut_matrix.ptr(); - for( int i = 0; i < 256; i++ ) - ptr[i] = (int)( pow( (double) i / 255.0, inverse_gamma ) * 255.0 ); - LUT( src, lut_matrix, src ); -} - - int main(int argc, char** argv) { ros::init(argc, argv, "chroma"); diff --git a/depth/CMakeLists.txt b/depth/CMakeLists.txt index 7571572..729eab3 100644 --- a/depth/CMakeLists.txt +++ b/depth/CMakeLists.txt @@ -22,19 +22,22 @@ aux_source_directory(src/ SRC_LIST) include_directories( include + /usr/include/ ${catkin_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS} ) -set(EXTERNAL_LIBS alglib) +link_directories( + /usr/lib/ + /usr/local/lib/ + /usr/lib/x86_64-linux-gnu/ +) add_executable(depth ${SRC_LIST}) -link_directories(/usr/include /usr/lib/x86_64-linux-gnu/) - target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} - ${EXTERNAL_LIBS} + libvision.so ) diff --git a/depth/CMakeLists.txt~ b/depth/CMakeLists.txt~ index 0b63a4c..f52e11f 100644 --- a/depth/CMakeLists.txt~ +++ b/depth/CMakeLists.txt~ @@ -1,181 +1,42 @@ cmake_minimum_required(VERSION 2.8.3) project(depth) +set(CMAKE_CXX_FLAGS "-std=c++11") + ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## is used, also find other catkin packages -find_package(catkin REQUIRED) - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend tag for "message_generation" -## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependency has been pulled in -## but can be declared for certainty nonetheless: -## * add a run_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) - -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# std_msgs # Or other packages containing msgs -# ) - -################################################ -## Declare ROS dynamic reconfigure parameters ## -################################################ - -## To declare and build dynamic reconfigure parameters within this -## package, follow these steps: -## * In the file package.xml: -## * add a build_depend and a run_depend tag for "dynamic_reconfigure" -## * In this file (CMakeLists.txt): -## * add "dynamic_reconfigure" to -## find_package(catkin REQUIRED COMPONENTS ...) -## * uncomment the "generate_dynamic_reconfigure_options" section below -## and list every .cfg file to be processed - -## Generate dynamic reconfigure parameters in the 'cfg' folder -# generate_dynamic_reconfigure_options( -# cfg/DynReconf1.cfg -# cfg/DynReconf2.cfg -# ) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if you package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( -# INCLUDE_DIRS include -# LIBRARIES depth -# CATKIN_DEPENDS other_catkin_pkg -# DEPENDS system_lib +find_package(catkin REQUIRED COMPONENTS + cv_bridge + image_transport + roscpp + sensor_msgs + std_msgs ) -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -# include_directories(include) - -## Declare a C++ library -# add_library(depth -# src/${PROJECT_NAME}/depth.cpp -# ) - -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -# add_dependencies(depth ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ executable -# add_executable(depth_node src/depth_node.cpp) - -## Add cmake target dependencies of the executable -## same as for the library above -# add_dependencies(depth_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +find_package(OpenCV REQUIRED) -## Specify libraries to link a library or executable target against -# target_link_libraries(depth_node -# ${catkin_LIBRARIES} -# ) +catkin_package() -############# -## Install ## -############# +aux_source_directory(src/ SRC_LIST) -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# install(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables and/or libraries for installation -# install(TARGETS depth depth_node -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) +include_directories( + include + /usr/include/ + ${catkin_INCLUDE_DIRS} + ${OpenCV_INCLUDE_DIRS} +) -## Mark other files for installation (e.g. launch and bag files, etc.) -# install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) +link_directories( + /usr/lib/ + /usr/local/lib/ + /usr/lib/x86_64-linux-gnu/ +) -############# -## Testing ## -############# +add_executable(depth ${SRC_LIST}) -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_depth.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() +target_link_libraries(${PROJECT_NAME} + ${catkin_LIBRARIES} + ${OpenCV_LIBRARIES} +) -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) diff --git a/depth/include/depth.hpp b/depth/include/depth.hpp index 818c3be..117582e 100644 --- a/depth/include/depth.hpp +++ b/depth/include/depth.hpp @@ -21,13 +21,11 @@ #include #include #include +#include -#include -#include using namespace std; using namespace cv; -using namespace alglib; #define DEPTH_MAX 6000.0 /**< Default maximum distance. Only use this for initialization. */ #define DEPTH_MIN 0.0 /**< Default minimum distance. Only use this for initialization. */ @@ -35,62 +33,13 @@ using namespace alglib; class Depth_processing { public: - - struct Position - { - float x = 0.0; - float y = 0.0; - float z = 0.0; - float height = 0.0; - float distance = 0.0; - }; - - struct People - { - vector< Rect_ > tracked_boxes; - vector< float > tracked_rankings; - vector< Position > tracked_pos; - }; - - + Depth_processing(); ~Depth_processing(); //depth callback void depthCb(const sensor_msgs::ImageConstPtr& msg); - - //Region growing algorithms - void upVerticalFill(Mat& src, float threshold, bool flag); - void upVerticalFill2(Mat& src, float threshold, bool scale); - void rightHorizontalFill(Mat& cur_Mat, float threshold, bool scale); - void rectFill(Mat& cur_Mat, float threshold, int range); - - //Background & foreground estimation, to be used in sequence - void estimateBackground(Mat& src, Mat& dst, vector& storage, int recursion, float ratio = 0.04, int index = 0); - void estimateForeground(Mat& src1, Mat& src2, Mat& dst); - - //Detection and tracking of blobs - void detectBlobs(Mat& src, vector< Rect_ >& colour_areas, int range); - void track(vector< Rect_ >& current, People& collection, int rank = 3, float threshold = 0.2); - - //Depth estimation functions - double calculateDepth(const Mat& src); - double minDepth(vector vec, int number); - double centerDepth(const Mat& src, int number); - double combineDepth(double saveMin, double saveCenter, double saveCluster, double min_depth = 0.0, double max_depth = 6.0); - - //Position estimation - void calculatePosition(Rect& rect, Position& pos); - - //helper functions - int threshold(Mat& src, Mat& dst, int thresh); - void fixRects(vector< Rect_ >& rects, int screenW); - void depthToGray(Mat& src, Mat& dst, float min_depth, float max_depth); - void grayToDepth(Mat& src, Mat& dst, float max_depth); - void frameDif(Mat& src1, Mat& src2, Mat& dst, float threshold); - kmeansreport clusterize(const vector& vec, int clusters); - - + private: ros::NodeHandle nh_; diff --git a/depth/src/depth.cpp b/depth/src/depth.cpp index 9540c21..dbf1fac 100644 --- a/depth/src/depth.cpp +++ b/depth/src/depth.cpp @@ -169,1396 +169,6 @@ void Depth_processing::depthCb(const sensor_msgs::ImageConstPtr& msg) } - -/* Detects non-black areas in the image and populates a list of OpenCV Rects - * - * PARAMETERS: -the vector that will be populated - * -screen width - * - * RETURN -- - */ -void Depth_processing::detectBlobs(Mat& src, vector< Rect_ >& colour_areas, int range) -{ - float detectionFactor = 0.1; - float mergeFactor = 0.1; - bool flag = false; - int channels = src.channels(); - int cols = src.cols; - int rows = src.rows; - int size = cols*rows*channels; - for(int y = 0; y < 1; y++) - { - const uchar *dif = src.ptr(y); - for(int x = 0; x < size; x = x + channels) - { - if(dif[x] != 0) - { - int i ,j, o; - i = (x/channels)%(cols); - j = floor(x/(cols*channels)); - - Rect_ removal; - if(i + range >= cols) - continue; - if(j + range >= rows) - continue; - - removal = Rect(i, j, range , range); - if(removal.width < 1 || removal.height < 1) - continue; - - if(!colour_areas.empty()) - { - for(int k = 0; k < colour_areas.size(); k++) - { - Rect_ rect = colour_areas[k]; - Rect all = removal | rect; - int temp = removal.y; - removal.y = rect.y; - Rect intersection = removal & rect; - removal.y = temp; - int threshold = intersection.area(); - if(threshold < 1) - continue; - - if(removal.area() < rect.area()) - { - if(threshold > detectionFactor*removal.area()) - { - colour_areas[k] = all; - flag = true; - } - } - else - { - if(threshold > detectionFactor*rect.area()) - { - colour_areas[k] = all; - flag = true; - } - } - } - if(!flag) - { - colour_areas.push_back(removal); - } - else - flag = false; - - int end = colour_areas.size(); - for(int a = 0; a < end; a++) - { - for(int b = a + 1; b < end; b++) - { - Rect_ removal = colour_areas[a]; - Rect_ rect = colour_areas[b]; - - Rect all = removal | rect; - int temp = removal.y; - removal.y = rect.y; - Rect intersection = removal & rect; - removal.y = temp; - int threshold = intersection.area(); - if(threshold < 1) - continue; - - if(removal.area() < rect.area()) - { - if(threshold > mergeFactor*removal.area()) - { - colour_areas[a] = all; - colour_areas[b] = colour_areas.back(); - colour_areas.pop_back(); - b = a + 1; - end--; - } - } - else - { - if(threshold > mergeFactor*rect.area()) - { - colour_areas[a] = all; - colour_areas[b] = colour_areas.back(); - colour_areas.pop_back(); - b = a + 1; - end--; - } - } - - } - } - - - } - else - { - colour_areas.push_back(removal); - } - - } - } - } - int end = colour_areas.size(); - - for(int k = 0; k < end; k++) - { - float width = colour_areas[k].width; - float height = colour_areas[k].height; - float x = colour_areas[k].x; - float y = colour_areas[k].y; - if((x < 0) || (y < 0) || (height < 0) || (width < 0)) - { - //~ rectangle(src, colour_areas[k], 0, CV_FILLED); - cout<<"done"< 1.5) || (area < cols*rows*0.02)) - { - //~ rectangle(src, colour_areas[k], 0, CV_FILLED); - colour_areas[k] = colour_areas.back(); - colour_areas.pop_back(); - k--; - end--; - - } - } - - /* - for(int k = 0; k < end; k++) - { - if(colour_areas[k].width > 250) - { - colour_areas[k].width = colour_areas[k].width/2; - Rect_ rect = colour_areas[k]; - rect.x = rect.x + rect.width; - rect.width = colour_areas[k].width; - colour_areas.push_back(rect); - k--; - end++; - - } - - } - */ - - //~ cout<<"Size: "< >& cur_boxes, People& collection, int rank, float threshold) -{ - bool exists = false; - float step = 1.2; - - - if(!cur_boxes.empty()) - { - - for(int a = 0; a < cur_boxes.size(); a++) - { - exists = false; - for(int b = 0; b < collection.tracked_boxes.size(); b++) - { - Rect intersection = cur_boxes[a] & collection.tracked_boxes[b]; - int area = intersection.area(); - if(cur_boxes[a].area() < collection.tracked_boxes[b].area()) - { - if(area > threshold*cur_boxes[a].area()) - { - collection.tracked_boxes[b] = cur_boxes[a]; - if(collection.tracked_rankings[b] <= 10) - collection.tracked_rankings[b] = collection.tracked_rankings[b] + step; - exists = true; - break; - } - } - else - { - if(area > threshold*collection.tracked_boxes[b].area()) - { - collection.tracked_boxes[b] = cur_boxes[a]; - if(collection.tracked_rankings[b] <= 10) - collection.tracked_rankings[b] = collection.tracked_rankings[b] + step; - exists = true; - break; - } - } - - } - if(!exists) - { - collection.tracked_boxes.push_back(cur_boxes[a]); - collection.tracked_rankings.push_back(rank + step); - } - } - for(int a = 0; a < collection.tracked_boxes.size(); a++) - { - collection.tracked_rankings[a] = collection.tracked_rankings[a] - 1; - } - for(int a = 0; a < collection.tracked_boxes.size(); a++) - { - if(collection.tracked_rankings[a] <= 0 && collection.tracked_boxes.size() > 0) - { - collection.tracked_boxes[a] = collection.tracked_boxes.back(); - collection.tracked_boxes.pop_back(); - collection.tracked_rankings[a] = collection.tracked_rankings.back(); - collection.tracked_rankings.pop_back(); - a--; - } - - } - - } -} - -/* Estimates the foreground by combining static images, works for - * images the contain edge information - * - * @param a mat representing the current image frame, a mat to store the result, - * the number of images to combine - * @return - - */ -void Depth_processing::estimateForeground(Mat& cur_Mat, Mat& back_Mat, Mat& dst_Mat) -{ - uchar *back, *cur, *dst; - for(int y = 0; y < 1; y++) - { - int rows = cur_Mat.rows; - int cols = cur_Mat.cols; - int channels = cur_Mat.channels(); - int size = rows*cols*channels; - - back = back_Mat.ptr(y); - cur = cur_Mat.ptr(y); - dst = dst_Mat.ptr(y); - for(int x = 0; x < size; x = x + channels) - { - if(back[x] != 0) - { - dst[x] = 0; - } - } - } -} - -/* Estimates the background by combining static images, works for - * images the contain edge information - * - * @param a mat representing the current image frame, a mat to store the result, - * the number of images to combine - * @return - - */ -void Depth_processing::estimateBackground(Mat& src, Mat& dst, vector& storage, int recursion, float ratio, int index) -{ - int size = storage.size(); - Mat result; - if (index > recursion) - { - dst = storage.at(index - 1); - return; - } - if(size > 0) - { - if((index <= 0) || ((index >= recursion*ratio && (size >= recursion*ratio)))) - { - bitwise_or(src, storage.at(index), result); - } - else - bitwise_and(src, storage.at(index), result); - if((size -1) == index) - { - storage.push_back(result); - dst = result; - } - else - { - storage.at(index) = src; - index++; - estimateBackground(result, dst, storage, recursion, ratio, index); - } - } - else - { - storage.push_back(src); - dst = src.clone(); - } -} - -/*Converts a grayscale Mat to a depth Mat by using the maximum depth value - * - * PARAMETERS: - * -Mat depth - * -Mat with the graysclae result - * -float minimum depth - * -float maximum depth - * - * RETURN: -- - */ -void Depth_processing::grayToDepth(Mat& src, Mat& dst, float max_depth) -{ - Mat temp_img(src.rows, src.cols, CV_32FC1); - int cols = src.cols; - int rows = src.rows; - if(src.isContinuous()) - { - cols *= rows; - rows = 1; - } - for(int i = 0; i < rows; i++) - { - uchar* cur = src.ptr(i); - float* Ii = temp_img.ptr(i); - for(int j = 0; j < cols; j++) - { - Ii[j] = (max_depth*(float(cur[j])/(255.0))); - } - } - dst = temp_img.clone(); - -} - -/*Converts a depth Mat to a grayscale one by using the min and maximum depth values - * - * PARAMETERS: - * -Mat depth - * -Mat with the graysclae result - * -float minimum depth - * -float maximum depth - * - * RETURN: -- - */ -void Depth_processing::depthToGray(Mat& src, Mat& dst, float min_depth, float max_depth) -{ - - Mat temp_img(src.rows, src.cols, CV_8UC1); - int cols = src.cols; - int rows = src.rows; - if(src.isContinuous()) - { - cols *= rows; - rows = 1; - } - for(int i = 0; i < rows; i++) - { - float* cur = src.ptr(i); - uchar* Ii = temp_img.ptr(i); - for(int j = 0; j < cols; j++) - { - Ii[j] = (255*((cur[j] - min_depth)/(max_depth - min_depth))); - } - } - dst = temp_img.clone(); - -} - -/*Calculates the absolute difference between the two mats and thresholds the result according to the threshold given - * - * PARAMETERS: - * -Mat first - * -Mat second - * -Mat with the result - * -float threshold to be used - * - * RETURN: -- - */ -void Depth_processing::frameDif(Mat& src1, Mat& src2, Mat& dst, float threshold) -{ - uchar *src, *temp, *dest; - int channels = src1.channels(); - int cols = src1.cols; - int rows = src1.rows; - int size = cols*rows*channels; - int motionCounter = 0; - - Mat temp_Mat; - // Absolute dif between our current mat and the previous one - absdiff(src1, src2, temp_Mat); - dst = src1.clone(); - for(int y = 0; y < 1; y++) - { - src = src1.ptr(y); - temp = temp_Mat.ptr(y); - dest = dst.ptr(y); - for(int x = 0; x < size; x = x + channels) - { - float all = 0.0; - float color[channels]; - for (int ch = 0; ch < channels; ch++) - color[ch] = (float)temp[x + ch]; - - for (int i = 0; i < channels; i++) - all += color[i]; - all /= channels; - - if(all > threshold) - { - for (int ch = 0; ch < channels; ch++) - dest[x + ch] = src[x + ch]; - motionCounter++; - } - else - { - for (int ch = 0; ch < channels; ch++) - dest[x + ch] = 0; - } - } - } -} - -/*Region growing algorithm that fills black holes in the depth image, uses - * rectangular areas - * - * PARAMETERS: - * -Mat to be corrected - * -float threshold of correction - * -int range of each correctin area - * - * RETURN: -- - */ -void Depth_processing::rectFill(Mat& src, float threshold, int range) -{ - uchar *cur, *temp,*vFill; - int channels = src.channels(); - int cols = src.cols; - int rows = src.rows; - int size = cols*rows*channels; - int count = 0; - float all = 0.0; - float bPercent = 0.0; - float gPercent = 0.0; - float rPercent = 0.0; - bool has_zero = false; - bool flag = false; - - for(int y = 0; y < 1; y++) - { - cur = src.ptr(y); - for(int x = channels*range + cols*range; x < size - (channels*range + cols*range); x = x + channels) - { - if ((cur[x] != 0 && channels == 1) || (channels > 1 && (cur[x+1] != 0 || cur[x+2] != 0) )) - { - int top = x - range*cols*channels; - int topLeft = top - range*channels; - int topRight = top + range*cols*channels; - int bottom = x + range*cols*channels; - int bottomLeft = bottom - range*cols*channels; - int bottomRight = bottom + range*cols*channels; - - if(topLeft%(cols*channels) > top%(cols*channels)) - continue; - if(topRight%(cols*channels) < top%(cols*channels)) - continue; - - has_zero = false; - flag = false; - count = 0; - for(int a = topLeft; a < bottomLeft; a = a + cols*channels) - { - for(int b = a; b < a + 2*range*channels ; b += channels) - { - if (cur[b] == 0) - { - has_zero = true; - count++; - } - else - { - if(channels == 1) - { - all = abs( (float(cur[b])/255) - (float(cur[x])/255) ); - if(all >= threshold) - flag = true; - } - } - - } - } - int countThresh = range; - if(!flag && has_zero && (count <= countThresh)) - { - for(int a = topLeft; a < bottomLeft; a = a + cols*channels) - { - for(int b = a; b < a + 2*range*channels ; b += channels) - { - if (cur[b] == 0) - { - cur[b] = cur[x]; - } - } - } - } - //~ x = x - range*channels + channels; - } - } - } -} - -/* Fills the black holes in the Mat horizontally - * - * PARAMETERS: - * -the Mat to be processed - * -the comparison threshold - * - * RETURN: -- - * - */ -void Depth_processing::rightHorizontalFill(Mat& cur_Mat, float threshold, bool scale) -{ - uchar *cur; - int channels = cur_Mat.channels(); - int cols = cur_Mat.cols; - int rows = cur_Mat.rows; - int size = cols*rows*channels; - float all = 0.0, bPercent = 0.0, gPercent = 0.0, rPercent = 0.0; - - for(int y = 0; y < 1; y++) - { - cur = cur_Mat.ptr(y); - for(int x = 0; x < size; x = x + channels) - { - /*If it is the end of the current line, continue*/ - if(x > 0 && (x%(cols*channels) == 0 || x%(cols*channels) == (cols - 1))) - continue; - if(cur[x] != 0 && ((cur[x + channels] == 0 && channels == 1) || (channels > 1 && (cur[x+1] != 0 | cur[x+2] != 0) ))) - { - int start = x + 2*channels; - for(int a = start; a < size; a = a + channels) - { - //check not to change line - if((x%(cols*channels)) > (a%(cols*channels))) - break; - if(cur[a] != 0) - { - if(channels == 1) - { - all = abs( (float(cur[a])/255) - (float(cur[x])/255) ); - if(all < threshold) - { - if(scale) - { - int rowA = a%(cols*channels); - int rowX = x%(cols*channels); - all = (all/abs(rowA - rowX))*255; - int ratio = 1; - for(int i = x + channels; i < a; i = i + channels) - { - cur[i] = cur[x] + ratio*all; - ratio++; - } - } - else - { - for(int i = x + channels; i < a; i = i + channels) - cur[i] = (cur[x] + cur[a])/2; - } - } - } - else if(cur[a + 1] == 0 && cur[a + 2] == 0) - { - - - bPercent = abs( (double(cur[a])/255) - (double(cur[x])/255) ); - gPercent = abs( (double(cur[a + 1])/255) - (double(cur[x + 1])/255) ); - rPercent = abs( (double(cur[a + 2])/255) - (double(cur[x + 2])/255) ); - - all = bPercent + gPercent + rPercent; - - if(all < threshold){ - cur[a] = cur[x]; - cur[a+1] = cur[x+1]; - cur[a+2] = cur[x+2]; - } - else - break; - } - break; - } - } - } - } - } -} - -/* Fills the black holes in the Mat vertically - * from bottom to top - * - * @param the Mat to be processed, the comparison threshold - * @return - - * - */ -void Depth_processing::upVerticalFill(Mat& src, float threshold, bool flag) -{ - uchar *cur; - int channels = src.channels(); - int cols = src.cols; - int rows = src.rows; - int size = cols*rows*channels; - float all = 0.0; - float bPercent = 0.0; - float gPercent = 0.0; - float rPercent = 0.0; - - threshold *= 255; - float threshold2 = 0.01*255; - for(int y = 0; y < 1; y++) - { - cur = src.ptr(y); - for(int x = size; x > cols*channels; x = x - channels) - { - - if(int(cur[x]) != 0 && ((int(cur[x - cols*channels]) == 0 && channels == 1) || (channels > 1 && (cur[x+1] != 0 | cur[x+2] != 0) ))) - { - - int start = ((x - 2*cols*channels) > 0)?(x - 2*cols*channels):0; - //~ int end = ((start + cols*verRange) < size)?(start + cols*verRange):size; - - for(int a = start; a > 0 ; a = a - cols*channels) - { - if((x - a)/(cols*channels) > rows/10) - { - break; - } - if(int(cur[a]) != 0) - { - if(channels == 1) - { - all = abs(int(cur[a]) - int(cur[x])); - if(all < threshold) - { - int ratio = 1; - int rowA = floor(a/(cols*channels)); - int rowX = floor(x/(cols*channels)); - int color = int(cur[x]) - int(cur[a]); - color /= abs(rowA - rowX); - - for(int i = x - cols*channels; i > a; i = i - cols*channels) - { - cur[i] = int(cur[x]) - ratio*color; - ratio++; - } - } - else - { - for(int i = x - cols*channels; i > a; i = i - cols*channels) - { - int left = 0; - int right = 0; - for(int j = i; j < size ; j = j + channels) - { - //check not to change line - if((i%(cols*channels)) > (j%(cols*channels))) - break; - if(cur[j] != 0) - { - if(channels == 1) - { - right = j; - } - break; - } - } - for(int j = i; j > 0 ; j = j - channels) - { - //check not to change line - if((i%(cols*channels)) < (j%(cols*channels))) - break; - if(cur[j] != 0) - { - if(channels == 1) - { - left = j; - } - break; - } - } - if(!flag) - { - if((abs(int(cur[x]) - int(cur[left])) < threshold)) - { - cur[i] = int(cur[left]) - (int(cur[left]) - int(cur[x]))/abs(left - x); - } - else if((abs(int(cur[x]) - int(cur[right])) < threshold)) - { - cur[i] = int(cur[right]) - (int(cur[right]) - int(cur[x]))/abs(right - x); - } - } - else - { - //~ if(abs(int(cur[left]) - int(cur[right])) < threshold) - //~ { - cur[i] = int(cur[x]); - //~ } - } - //~ else - //~ { - //~ cur[i] = int(cur[a]); - //~ } - } - } - } - else if(cur[a + 1] == 0 && cur[a + 2] == 0) - { - bPercent = abs(cur[a] - cur[x]); - gPercent = abs(cur[a + 1] - cur[x + 1]); - rPercent = abs(cur[a + 2] - cur[x + 2]); - - all = (bPercent + gPercent + rPercent)/3; - - if(all < threshold) - { - cur[x] = cur[a]; - cur[x+1] = cur[a+1]; - cur[x+2] = cur[a+2]; - } - else - break; - } - break; - } - } - - } - } - } -} - -/* Fills the black holes in the Mat vertically - * from bottom to top - * - * @param the Mat to be processed, the comparison threshold - * @return - - * - */ -void Depth_processing::upVerticalFill2(Mat& src, float threshold, bool scale) -{ - uchar *cur; - int channels = src.channels(); - int cols = src.cols; - int rows = src.rows; - int size = cols*rows*channels; - int left; - int right; - int opposite; - float all = 0.0; - float bPercent = 0.0; - float gPercent = 0.0; - float rPercent = 0.0; - - for(int y = 0; y < 1; y++) - { - cur = src.ptr(y); - for(int x = size; x > cols*channels; x = x - channels) - { - opposite = 0; - if(cur[x] != 0 && ((cur[x - cols*channels] == 0 && channels == 1) || (channels > 1 && (cur[x+1] != 0 | cur[x+2] != 0) ))) - { - - int start = ((x - 2*cols*channels) > 0)?(x - 2*cols*channels):0; - - for(int a = start; a > 0 ; a = a - cols*channels) - { - if(cur[a] != 0) - { - if(channels == 1) - { - all = abs( (float(cur[a])/255) - (float(cur[x])/255) ); - if(all < threshold) - { - opposite = a; - } - } - break; - } - } - if(opposite == 0) - continue; - start = x - cols*channels; - for(int k = start; k > opposite; k = k - cols*channels) - { - left = 0; - right = 0; - for(int a = k; a < size ; a = a + channels) - { - //check not to change line - if((k%(cols*channels)) > (a%(cols*channels))) - break; - if(cur[a] != 0) - { - if(channels == 1) - { - all = abs( (float(cur[a])/255) - (float(cur[x])/255) ); - if(all < threshold) - { - right = a; - } - } - else if(cur[a + 1] == 0 && cur[a + 2] == 0) - { - bPercent = abs( (float(cur[a])/255) - (float(cur[x])/255) ); - gPercent = abs( (float(cur[a + 1])/255) - (float(cur[x + 1])/255) ); - rPercent = abs( (float(cur[a + 2])/255) - (float(cur[x + 2])/255) ); - - all = (bPercent + gPercent + rPercent)/3; - - if(all < threshold) - { - for(int i = start; i < a; i = i + channels) - { - cur[i] = cur[x]; - cur[i+1] = cur[x+1]; - cur[i+2] = cur[x+2]; - - } - - - } - } - break; - } - } - for(int a = k; a > 0 ; a = a - channels) - { - //check not to change line - if((k%(cols*channels)) < (a%(cols*channels))) - break; - if(cur[a] != 0) - { - if(channels == 1) - { - all = abs( (float(cur[a])/255) - (float(cur[x])/255) ); - if(all < threshold) - { - left = a; - } - } - else if(cur[a + 1] == 0 && cur[a + 2] == 0) - { - bPercent = abs( (float(cur[a])/255) - (float(cur[x])/255) ); - gPercent = abs( (float(cur[a + 1])/255) - (float(cur[x + 1])/255) ); - rPercent = abs( (float(cur[a + 2])/255) - (float(cur[x + 2])/255) ); - - all = (bPercent + gPercent + rPercent)/3; - - if(all < threshold) - { - for(int i = start; i > a; i = i - channels) - { - cur[i] = cur[x]; - cur[i+1] = cur[x+1]; - cur[i+2] = cur[x+2]; - } - } - } - break; - } - } - if(left == 0 || right == 0) - continue; - if(abs(left - right) > 10 ) - continue; - all = abs( (float(cur[left])/255) - (float(cur[right])/255) ); - if(all > threshold) - { - continue; - } - all = float(cur[left]) - float(cur[right]) ; - all = (all/abs(left - right)); - int ratio = 1; - for(int a = left + channels; a < right; a = a + channels) - { - if(scale) - { - cur[a] = cur[left] - ratio*all; - ratio++; - - } - else - { - cur[a] = cur[x]; - } - } - - } - } - } - } -} - -/* Calculates and stores the real world coordinates (x, y, z) of a face - * in respect to the center of the camera. - * - * PARAMETERS: - * -Database object holding the found faces in the image - * - * RETURN: -- - * - */ -void Depth_processing::calculatePosition(Rect_& rect, Depth_processing::Position& pos) -{ - - float hor_x = 0.0; - float hor_y = 0.0; - float ver_x = 0.0; - float ver_y = 0.0; - float hor_focal = 0.0; - float ver_focal = 0.0; - float depth = 0.0; - float distance = 0.0; - float top = 0.0; - float bottom = 0.0; - - - depth = pos.z; - if (depth != 0.0) - { - - //Find the focal length - hor_focal = depth_height / (2 * tan((Vfield/2) * M_PI / 180.0) ); - ver_focal = depth_width / (2 * tan((Hfield/2) * M_PI / 180.0) ); - - //Transform the pixel x, y in respect to - //the camera center - ver_x = rect.x + rect.width/2; - if(ver_x > depth_width/2) - ver_x = (ver_x - depth_width/2); - else - ver_x = (-1)*(depth_width/2 - ver_x); - - ver_y = rect.y + rect.height/2; - if(ver_y > depth_height/2) - ver_y = (-1)*(ver_y - depth_height/2); - else - ver_y = (depth_height/2 - ver_y); - - //Calculate the real world coordinates of the box center - hor_y = depth * ver_y / hor_focal; - hor_x = depth * ver_x / ver_focal; - - if(pos.x != 0) - { - distance = abs(pos.x - hor_x); - pos.distance = distance; - } - pos.x = hor_x; - pos.y = hor_y; - - - - - ver_y = rect.y; - if(ver_y > depth_height/2) - ver_y = (-1)*(ver_y - depth_height/2); - else - ver_y = (depth_height/2 - ver_y); - top = depth * ver_y / hor_focal; - - - ver_y = rect.y + rect.height; - if(ver_y > depth_height/2) - ver_y = (-1)*(ver_y - depth_height/2); - else - ver_y = (depth_height/2 - ver_y); - - bottom = depth * ver_y / hor_focal; - pos.height = abs(top - bottom); - - } - -} - -/* Calculates the depth of the closest object in the specified Mat by using clustering - * - * PARAMETERS: - * -Mat - * -number of values to calculate - * - * RETURN: - * -Double holding the min cluster median value - * - */ -double Depth_processing::calculateDepth(const Mat& src) -{ - - int clusters = 2; - int cols = src.cols; - int rows = src.rows; - int channels = src.channels(); - int size = cols*rows*channels; - double median = 0.0; - double saveMin = 0.0; - double saveCluster = 0.0; - double saveCenter = 0.0; - double depthCombined = 0.0; - vector vec; - vector cluster_vec; - - if(cols > 0 && rows > 0) - { - - if(src.isContinuous()) - { - rows = 1; - cols = size; - } - - cols = src.cols*0.8; - rows = src.rows*0.8; - - for(int i = 0; i < rows; i++) - { - const float* cur = src.ptr(i); - for(int j = 0; j < cols; j++) - { - cluster_vec.push_back(cur[j]); - } - - } - - // clustering - kmeansreport rep = clusterize(cluster_vec, clusters); - - // cluster item count - int counter[clusters]; - for(int i = 0; i < clusters; i++) - { - counter[i] = 0; - } - for(int i = 0; i < rep.cidx.length(); i++) - { - if(rep.cidx[i] > 0 && rep.cidx[i] < clusters) - counter[rep.cidx[i]]++; - } - - //check which centroid has the smallest value - int min_centroid = INT_MAX; - int index [clusters + 1]; - for(int i = 0; i < clusters + 1; i++) - { - index[i] = -1; - } - - if(rep.c.rows() > 0) - { - for(int i = 0; i < clusters; i++) - { - if((min_centroid > rep.c[i][0]) && (rep.c[i][0] > 0)) - { - min_centroid = rep.c[i][0]; - index[0] = i; - } - } - index[index[0] + 1] = INT_MAX; - - //~ cout<(i); - for(int j = 0; j < cols; j++) - { - if(index[0] == rep.cidx[i*cols + j]) - { - cluster_vec.push_back(cur[j]); - } - } - } - - //median - nth_element(cluster_vec.begin(), cluster_vec.begin() + cluster_vec.size()/2, cluster_vec.end()); - median = cluster_vec[cluster_vec.size()/2]; - for(int i = 1; i < clusters + 1; i++) - { - if(index[i] == -1) - { - index[0] = i - 1; - index[i] = INT_MAX; - break; - } - } - } - - //~ printf("%s %2.3f\n","Depth: " ,median); - - /* - depthToGray(src, src, 0, max_depth); - for(int i = 0; i < src.rows; i++) - { - uchar* cur = src.ptr(i); - for(int j = 0; j < src.cols; j++) - { - if(rep.cidx[i*src.cols + j] == index[0]) - { - cur[j] = 0; - } - else - { - cur[j] = 255; - } - } - } - - Mat temp_img(depth_height, depth_width, CV_8UC1); - temp_img = Scalar(0); - - src.copyTo(temp_img(Rect(personRect.x, personRect.y, src.cols, src.rows))); - imshow("Clustered", temp_img); - moveWindow("Clustered", 645, 0); - */ - - } - - } - return median; - - - /* - // Preprocessing step to discard null or unwanted depth values - // and convert Mat to vector - for(int i = 0; i < rows; i++) - { - float* cur = src.ptr(i); - for(int j = 0; j < cols; j++) - { - vec.push_back(cur[j]); - } - - } - // average of minimum vector elements - saveMin = minDepth(vec, 20); - - // find the depth of the mat center area - saveCenter = centerDepth(src, 10); - - - - - //~ int id = -1; - //~ double result = FLT_MAX; - //~ - //~ for(int i = 0; i < rep.c.rows() - 1; i++) - //~ { - //~ double temp = std::min(result, rep.c[i + 1][0]); - //~ if(temp > 0) - //~ { - //~ id = i; - //~ result = temp; - //~ } - //~ } - //~ saveCluster = result; - - //combine the results of the 2 closest - depthCombined = combineDepth(saveMin, saveCenter, median, 0, max_depth); - - - //~ printf("%s %2.3f\n","Depth combined: " ,depthCombined); - return depthCombined; - */ - -} - -/* Finds an average of the minimum - * values of a given vector - * - * PARAMETERS: - * -vector with the values - * -number of values to calculate - * - * RETURN: - * -Double holding the average value - * - */ -double Depth_processing::minDepth(vector vec, int number) -{ - double result = 0; - double min = 0; - int done = 0; - float threshold = number; - partial_sort (vec.begin(), vec.begin() + threshold, vec.end()); - for(int i = 0; i < threshold; i++) - { - result += vec.at(i); - } - result = result/threshold; - cout<<"Save Min: "<(src.rows/2 - number/2 + i, src.cols/2 - number/2 + j); - if(std::isnan(temp) || temp == FLT_MAX) - continue; - result += temp; - counter++; - } - } - - cout<<"Save Center: "< max_depth) - //~ { - //~ return FLT_MAX; - //~ } - return result/counter; -} - -/* Clusterizes a given vector - * - * PARAMETERS: - * -Vec with the values - * -Int with the number of clusters - * - * RETURN: - * -Double holding the minimum cluster value - * - */ -kmeansreport Depth_processing::clusterize(const vector& vec, int clusters) -{ - clusterizerstate s; - kmeansreport rep; - - real_2d_array matrix; - matrix.setlength(vec.size(), 1); - - for (int i = 0; i < vec.size(); i++) - { - matrix(i, 0) = vec.at(i); - } - - try - { - clusterizercreate(s); - clusterizersetpoints(s, matrix, 2); - clusterizersetkmeanslimits(s, 5, 0); - clusterizerrunkmeans(s, clusters, rep); - } - catch(exception& e) - { - printf("%s %s", "Clusterize failed", e.what()); - } - return rep; - - -} - -/* Calculates an average of the 2 closest values - * - * PARAMETERS: - * -Double the min depth approximation - * -Double the center depth approximation - * -Double the cluster depth approximation - * - * - * RETURN: - * -Double holding the average of the 2 closest values - */ -double Depth_processing::combineDepth(double saveMin, double saveCenter, double saveCluster, double min_depth, double max_depth) -{ - double result; - double minClusterDif; - double minCenterDif; - double clusterCenterDif; - - - - - if((saveMin == FLT_MAX || saveMin == 0) && (saveCluster == FLT_MAX || saveCluster == 0)) - minClusterDif = FLT_MAX; - else - minClusterDif = abs(saveMin - saveCluster); - - if((saveMin == FLT_MAX || saveMin == 0) && (saveCenter == FLT_MAX || saveCenter == 0)) - minCenterDif = FLT_MAX; - else - minCenterDif = abs(saveMin - saveCenter); - - if((saveCluster == FLT_MAX || saveCluster == 0) && (saveCenter == FLT_MAX || saveCenter == 0)) - clusterCenterDif = FLT_MAX; - else - clusterCenterDif = abs(saveCluster - saveCenter); - - - if(minClusterDif > max_depth/10) - minClusterDif = FLT_MAX; - if(minCenterDif > max_depth/10) - minCenterDif = FLT_MAX; - if(clusterCenterDif > max_depth/10) - clusterCenterDif = FLT_MAX; - - if(minClusterDif < minCenterDif) - { - if(minClusterDif < clusterCenterDif) - result = (saveMin + saveCluster)/2; - else - result = (saveCluster + saveCenter)/2; - } - else if(minCenterDif < clusterCenterDif) - result = (saveMin + saveCenter )/2; - else if(clusterCenterDif < minClusterDif) - result = (saveCenter + saveCluster)/2; - else - { - result = max(saveMin, saveCenter); - result = max(result, saveCluster); - } - - - return result; -} - int main(int argc, char** argv) { ros::init(argc, argv, "depth"); diff --git a/fusion/CMakeLists.txt b/fusion/CMakeLists.txt index 82b3742..abc9b2d 100644 --- a/fusion/CMakeLists.txt +++ b/fusion/CMakeLists.txt @@ -22,19 +22,23 @@ aux_source_directory(src/ SRC_LIST) include_directories( include + /usr/include/ ${catkin_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS} ) -set(EXTERNAL_LIBS alglib) +link_directories( + /usr/lib/ + /usr/local/lib/ + /usr/lib/x86_64-linux-gnu/ +) add_executable(fusion ${SRC_LIST}) -link_directories(/usr/include /usr/lib/x86_64-linux-gnu/) target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} - ${EXTERNAL_LIBS} + libvision.so ) diff --git a/fusion/CMakeLists.txt~ b/fusion/CMakeLists.txt~ index f095b1f..82b3742 100644 --- a/fusion/CMakeLists.txt~ +++ b/fusion/CMakeLists.txt~ @@ -28,7 +28,7 @@ include_directories( set(EXTERNAL_LIBS alglib) -add_executable(depth ${SRC_LIST}) +add_executable(fusion ${SRC_LIST}) link_directories(/usr/include /usr/lib/x86_64-linux-gnu/) diff --git a/fusion/include/fusion.hpp b/fusion/include/fusion.hpp index cc8f255..a8bb0d0 100644 --- a/fusion/include/fusion.hpp +++ b/fusion/include/fusion.hpp @@ -29,8 +29,7 @@ #include #include -#include -#include +#include using namespace std; using namespace cv; @@ -46,52 +45,12 @@ char timeBuf[80]; class Fusion_processing { public: - - struct Position - { - float x = 0.0; - float y = 0.0; - float z = 0.0; - float top = 0.0; - float height = 0.0; - float distance = 0.0; - }; - - struct People - { - vector< Rect_ > tracked_boxes; - vector< float > tracked_rankings; - vector< Position > tracked_pos; - }; - - - + Fusion_processing(); ~Fusion_processing(); - void callback(const sensor_msgs::Image::ConstPtr& chroma_msg, const sensor_msgs::Image::ConstPtr& chroma_dif_msg, const sensor_msgs::Image::ConstPtr& depth_msg, const sensor_msgs::Image::ConstPtr& depth_dif_msg); - - //Detection and tracking of blobs - void detectBlobs(Mat& src, vector< Rect_ >& colour_areas, int range); - void track(vector< Rect_ >& current, People& collection, int rank = 3, float threshold = 0.2); - - //Depth estimation functions - double calculateDepth(Mat& src, Rect_ personRect); - double minDepth(vector vec, int number); - double centerDepth(const Mat& src, int number); - double combineDepth(double saveMin, double saveCenter, double saveCluster, double min_depth = 0.0, double max_depth = 6.0); - - //Position estimation - void calculatePosition(Rect& rect, Position& pos); - - //helper functions - int threshold(Mat& src, Mat& dst, int thresh); - void fixRects(vector< Rect_ >& rects, int screenW); - void depthToGray(Mat& src, Mat& dst, float min_depth, float max_depth); - void grayToDepth(Mat& src, Mat& dst, float max_depth); - kmeansreport clusterize(const vector& vec, int clusters); - void writeCSV(Fusion_processing::People& collection, string path); + void writeCSV(People& collection, string path); private: diff --git a/fusion/src/fusion.cpp b/fusion/src/fusion.cpp index 814613e..077e310 100644 --- a/fusion/src/fusion.cpp +++ b/fusion/src/fusion.cpp @@ -93,7 +93,7 @@ void Fusion_processing::callback(const sensor_msgs::ImageConstPtr& chroma_msg, c Rect rect; int rank = -1; int index = -1; - Fusion_processing::Position pos; + Position pos; int end = people.tracked_rankings.size(); for(int i = 0; i < end; i++) { @@ -191,7 +191,7 @@ void Fusion_processing::callback(const sensor_msgs::ImageConstPtr& chroma_msg, c } -void Fusion_processing::writeCSV(Fusion_processing::People& collection, string path) +void Fusion_processing::writeCSV(People& collection, string path) { if (!collection.tracked_boxes.empty()) { @@ -207,7 +207,7 @@ void Fusion_processing::writeCSV(Fusion_processing::People& collection, string p for(int i = 0; i < collection.tracked_boxes.size() ; i++) { Rect box = collection.tracked_boxes[i]; - Fusion_processing::Position pos = collection.tracked_pos[i]; + Position pos = collection.tracked_pos[i]; storage < >& colour_areas, int range) -{ - float detectionFactor = 0.1; - float mergeFactor = 0.1; - bool flag = false; - int channels = src.channels(); - int cols = src.cols; - int rows = src.rows; - int size = cols*rows*channels; - for(int y = 0; y < 1; y++) - { - const uchar *dif = src.ptr(y); - for(int x = 0; x < size; x = x + channels) - { - if(dif[x] != 0) - { - int i ,j, o; - i = (x/channels)%(cols); - j = floor(x/(cols*channels)); - - Rect_ removal; - if(i + range >= cols) - continue; - if(j + range >= rows) - continue; - - removal = Rect(i, j, range , range); - if(removal.width < 1 || removal.height < 1) - continue; - - if(!colour_areas.empty()) - { - for(int k = 0; k < colour_areas.size(); k++) - { - Rect_ rect = colour_areas[k]; - Rect all = removal | rect; - int temp = removal.y; - removal.y = rect.y; - Rect intersection = removal & rect; - removal.y = temp; - int threshold = intersection.area(); - if(threshold < 1) - continue; - - if(removal.area() < rect.area()) - { - if(threshold > detectionFactor*removal.area()) - { - colour_areas[k] = all; - flag = true; - } - } - else - { - if(threshold > detectionFactor*rect.area()) - { - colour_areas[k] = all; - flag = true; - } - } - } - if(!flag) - { - colour_areas.push_back(removal); - } - else - flag = false; - - int end = colour_areas.size(); - for(int a = 0; a < end; a++) - { - for(int b = a + 1; b < end; b++) - { - Rect_ removal = colour_areas[a]; - Rect_ rect = colour_areas[b]; - - Rect all = removal | rect; - int temp = removal.y; - removal.y = rect.y; - Rect intersection = removal & rect; - removal.y = temp; - int threshold = intersection.area(); - if(threshold < 1) - continue; - - if(removal.area() < rect.area()) - { - if(threshold > mergeFactor*removal.area()) - { - colour_areas[a] = all; - colour_areas[b] = colour_areas.back(); - colour_areas.pop_back(); - b = a + 1; - end--; - } - } - else - { - if(threshold > mergeFactor*rect.area()) - { - colour_areas[a] = all; - colour_areas[b] = colour_areas.back(); - colour_areas.pop_back(); - b = a + 1; - end--; - } - } - - } - } - - - } - else - { - colour_areas.push_back(removal); - } - - } - } - } - int end = colour_areas.size(); - - for(int k = 0; k < end; k++) - { - float width = colour_areas[k].width; - float height = colour_areas[k].height; - float x = colour_areas[k].x; - float y = colour_areas[k].y; - if((x < 0) || (y < 0) || (height < 0) || (width < 0)) - { - //~ rectangle(src, colour_areas[k], 0, CV_FILLED); - cout<<"done"< 1.5) || (area < cols*rows*0.02)) - { - //~ rectangle(src, colour_areas[k], 0, CV_FILLED); - colour_areas[k] = colour_areas.back(); - colour_areas.pop_back(); - k--; - end--; - - } - } - - /* - for(int k = 0; k < end; k++) - { - if(colour_areas[k].width > 250) - { - colour_areas[k].width = colour_areas[k].width/2; - Rect_ rect = colour_areas[k]; - rect.x = rect.x + rect.width; - rect.width = colour_areas[k].width; - colour_areas.push_back(rect); - k--; - end++; - - } - - } - */ - - //~ cout<<"Size: "< >& cur_boxes, People& collection, int rank, float threshold) -{ - bool exists = false; - float step = 1.2; - - - if(!cur_boxes.empty()) - { - - for(int a = 0; a < cur_boxes.size(); a++) - { - exists = false; - for(int b = 0; b < collection.tracked_boxes.size(); b++) - { - Rect intersection = cur_boxes[a] & collection.tracked_boxes[b]; - int area = intersection.area(); - if(cur_boxes[a].area() < collection.tracked_boxes[b].area()) - { - if(area > threshold*cur_boxes[a].area()) - { - collection.tracked_boxes[b] = cur_boxes[a]; - if(collection.tracked_rankings[b] <= 10) - collection.tracked_rankings[b] = collection.tracked_rankings[b] + step; - exists = true; - break; - } - } - else - { - if(area > threshold*collection.tracked_boxes[b].area()) - { - collection.tracked_boxes[b] = cur_boxes[a]; - if(collection.tracked_rankings[b] <= 10) - collection.tracked_rankings[b] = collection.tracked_rankings[b] + step; - exists = true; - break; - } - } - - } - if(!exists) - { - collection.tracked_boxes.push_back(cur_boxes[a]); - collection.tracked_rankings.push_back(rank + step); - } - } - for(int a = 0; a < collection.tracked_boxes.size(); a++) - { - collection.tracked_rankings[a] = collection.tracked_rankings[a] - 1; - } - for(int a = 0; a < collection.tracked_boxes.size(); a++) - { - if(collection.tracked_rankings[a] <= 0 && collection.tracked_boxes.size() > 0) - { - collection.tracked_boxes[a] = collection.tracked_boxes.back(); - collection.tracked_boxes.pop_back(); - collection.tracked_rankings[a] = collection.tracked_rankings.back(); - collection.tracked_rankings.pop_back(); - a--; - } - - } - - } -} - -/*Converts a grayscale Mat to a depth Mat by using the maximum depth value - * - * PARAMETERS: - * -Mat depth - * -Mat with the graysclae result - * -float minimum depth - * -float maximum depth - * - * RETURN: -- - */ -void Fusion_processing::grayToDepth(Mat& src, Mat& dst, float max_depth) -{ - Mat temp_img(src.rows, src.cols, CV_32FC1); - int cols = src.cols; - int rows = src.rows; - if(src.isContinuous()) - { - cols *= rows; - rows = 1; - } - for(int i = 0; i < rows; i++) - { - uchar* cur = src.ptr(i); - float* Ii = temp_img.ptr(i); - for(int j = 0; j < cols; j++) - { - Ii[j] = (max_depth*(float(cur[j])/(255.0))); - } - } - dst = temp_img.clone(); - -} - -/*Converts a depth Mat to a grayscale one by using the min and maximum depth values - * - * PARAMETERS: - * -Mat depth - * -Mat with the graysclae result - * -float minimum depth - * -float maximum depth - * - * RETURN: -- - */ -void Fusion_processing::depthToGray(Mat& src, Mat& dst, float min_depth, float max_depth) -{ - - Mat temp_img(src.rows, src.cols, CV_8UC1); - int cols = src.cols; - int rows = src.rows; - if(src.isContinuous()) - { - cols *= rows; - rows = 1; - } - for(int i = 0; i < rows; i++) - { - float* cur = src.ptr(i); - uchar* Ii = temp_img.ptr(i); - for(int j = 0; j < cols; j++) - { - Ii[j] = (255*((cur[j] - min_depth)/(max_depth - min_depth))); - } - } - dst = temp_img.clone(); - -} - -/* Calculates and stores the real world coordinates (x, y, z) of a face - * in respect to the center of the camera. - * - * PARAMETERS: - * -Database object holding the found faces in the image - * - * RETURN: -- - * - */ -void Fusion_processing::calculatePosition(Rect_& rect, Fusion_processing::Position& pos) -{ - - float hor_x = 0.0; - float hor_y = 0.0; - float ver_x = 0.0; - float ver_y = 0.0; - float hor_focal = 0.0; - float ver_focal = 0.0; - float depth = 0.0; - float distance = 0.0; - float top = 0.0; - float bottom = 0.0; - - - depth = pos.z; - if (depth != 0.0) - { - - //Find the focal length - hor_focal = depth_height / (2 * tan((Vfield/2) * M_PI / 180.0) ); - ver_focal = depth_width / (2 * tan((Hfield/2) * M_PI / 180.0) ); - - //Transform the pixel x, y in respect to - //the camera center - ver_x = rect.x + rect.width/2; - if(ver_x > depth_width/2) - ver_x = (ver_x - depth_width/2); - else - ver_x = (-1)*(depth_width/2 - ver_x); - - ver_y = rect.y + rect.height/2; - if(ver_y > depth_height/2) - ver_y = (-1)*(ver_y - depth_height/2); - else - ver_y = (depth_height/2 - ver_y); - - //Calculate the real world coordinates of the box center - hor_y = depth * ver_y / hor_focal; - hor_x = depth * ver_x / ver_focal; - - if(pos.x != 0) - { - distance = abs(pos.x - hor_x); - pos.distance = distance; - } - pos.x = hor_x; - pos.y = hor_y; - - - - - ver_y = rect.y; - if(ver_y > depth_height/2) - ver_y = (-1)*(ver_y - depth_height/2); - else - ver_y = (depth_height/2 - ver_y); - top = depth * ver_y / hor_focal; - - - ver_y = rect.y + rect.height; - if(ver_y > depth_height/2) - ver_y = (-1)*(ver_y - depth_height/2); - else - ver_y = (depth_height/2 - ver_y); - - bottom = depth * ver_y / hor_focal; - pos.top = abs(top); - pos.height = abs(top - bottom); - - } - -} - -/* Calculates the depth of the closest object in the specified Mat by using clustering - * - * PARAMETERS: - * -Mat - * -number of values to calculate - * - * RETURN: - * -Double holding the min cluster median value - * - */ -double Fusion_processing::calculateDepth(Mat& src, Rect_ personRect) -{ - - int clusters = 2; - int cols = src.cols; - int rows = src.rows; - int channels = src.channels(); - int size = cols*rows*channels; - double median = 0.0; - double saveMin = 0.0; - double saveCluster = 0.0; - double saveCenter = 0.0; - double depthCombined = 0.0; - vector vec; - vector cluster_vec; - - if(cols > 0 && rows > 0) - { - - if(src.isContinuous()) - { - rows = 1; - cols = size; - } - - cols = src.cols*0.7; - rows = src.rows*0.4; - - for(int i = 0; i < rows; i++) - { - const float* cur = src.ptr(i); - for(int j = 0; j < cols; j++) - { - cluster_vec.push_back(cur[j]); - } - - } - - // clustering - kmeansreport rep = clusterize(cluster_vec, clusters); - - // cluster item count - int counter[clusters]; - for(int i = 0; i < clusters; i++) - { - counter[i] = 0; - } - for(int i = 0; i < rep.cidx.length(); i++) - { - if(rep.cidx[i] > 0 && rep.cidx[i] < clusters) - counter[rep.cidx[i]]++; - } - - //check which centroid has the smallest value - int min_centroid = INT_MAX; - int index [clusters + 1]; - for(int i = 0; i < clusters + 1; i++) - { - index[i] = -1; - } - - if(rep.c.rows() > 0) - { - for(int i = 0; i < clusters; i++) - { - if((min_centroid > rep.c[i][0]) && (rep.c[i][0] > 0)) - { - min_centroid = rep.c[i][0]; - index[0] = i; - } - } - index[index[0] + 1] = INT_MAX; - - //~ cout<(i); - for(int j = 0; j < cols; j++) - { - if(index[0] == rep.cidx[i*cols + j]) - { - cluster_vec.push_back(cur[j]); - } - } - } - - //median - nth_element(cluster_vec.begin(), cluster_vec.begin() + cluster_vec.size()/2, cluster_vec.end()); - median = cluster_vec[cluster_vec.size()/2]; - for(int i = 1; i < clusters + 1; i++) - { - if(index[i] == -1) - { - index[0] = i - 1; - index[i] = INT_MAX; - break; - } - } - } - - //~ printf("%s %2.3f\n","Depth: " ,median); - - /* - Mat src_gray; - depthToGray(src, src_gray, 0, max_depth); - for(int i = 0; i < rows; i++) - { - uchar* cur = src_gray.ptr(i); - for(int j = 0; j < cols; j++) - { - if(rep.cidx[i*cols + j] == index[0]) - { - cur[j] = 0; - } - else - { - cur[j] = 255; - } - } - } - - Mat temp_img(depth_height, depth_width, CV_8UC1); - temp_img = Scalar(0); - - src.copyTo(temp_img(Rect(personRect.x, personRect.y, cols, rows))); - imshow("Clustered", temp_img); - moveWindow("Clustered", 645, 0); - */ - - } - - } - return median; - - - /* - // Preprocessing step to discard null or unwanted depth values - // and convert Mat to vector - for(int i = 0; i < rows; i++) - { - float* cur = src.ptr(i); - for(int j = 0; j < cols; j++) - { - vec.push_back(cur[j]); - } - - } - // average of minimum vector elements - saveMin = minDepth(vec, 20); - - // find the depth of the mat center area - saveCenter = centerDepth(src, 10); - - - - - //~ int id = -1; - //~ double result = FLT_MAX; - //~ - //~ for(int i = 0; i < rep.c.rows() - 1; i++) - //~ { - //~ double temp = std::min(result, rep.c[i + 1][0]); - //~ if(temp > 0) - //~ { - //~ id = i; - //~ result = temp; - //~ } - //~ } - //~ saveCluster = result; - - //combine the results of the 2 closest - depthCombined = combineDepth(saveMin, saveCenter, median, 0, max_depth); - - - //~ printf("%s %2.3f\n","Depth combined: " ,depthCombined); - return depthCombined; - */ - -} - -/* Finds an average of the minimum - * values of a given vector - * - * PARAMETERS: - * -vector with the values - * -number of values to calculate - * - * RETURN: - * -Double holding the average value - * - */ -double Fusion_processing::minDepth(vector vec, int number) -{ - double result = 0; - double min = 0; - int done = 0; - float threshold = number; - partial_sort (vec.begin(), vec.begin() + threshold, vec.end()); - for(int i = 0; i < threshold; i++) - { - result += vec.at(i); - } - result = result/threshold; - cout<<"Save Min: "<(src.rows/2 - number/2 + i, src.cols/2 - number/2 + j); - if(std::isnan(temp) || temp == FLT_MAX) - continue; - result += temp; - counter++; - } - } - - cout<<"Save Center: "< max_depth) - //~ { - //~ return FLT_MAX; - //~ } - return result/counter; -} - -/* Clusterizes a given vector - * - * PARAMETERS: - * -Vec with the values - * -Int with the number of clusters - * - * RETURN: - * -Double holding the minimum cluster value - * - */ -kmeansreport Fusion_processing::clusterize(const vector& vec, int clusters) -{ - clusterizerstate s; - kmeansreport rep; - - real_2d_array matrix; - matrix.setlength(vec.size(), 1); - - for (int i = 0; i < vec.size(); i++) - { - matrix(i, 0) = vec.at(i); - } - - try - { - clusterizercreate(s); - clusterizersetpoints(s, matrix, 2); - clusterizersetkmeanslimits(s, 5, 0); - clusterizerrunkmeans(s, clusters, rep); - } - catch(exception& e) - { - printf("%s %s", "Clusterize failed", e.what()); - } - return rep; - - -} - -/* Calculates an average of the 2 closest values - * - * PARAMETERS: - * -Double the min depth approximation - * -Double the center depth approximation - * -Double the cluster depth approximation - * - * - * RETURN: - * -Double holding the average of the 2 closest values - */ -double Fusion_processing::combineDepth(double saveMin, double saveCenter, double saveCluster, double min_depth, double max_depth) -{ - double result; - double minClusterDif; - double minCenterDif; - double clusterCenterDif; - - - - - if((saveMin == FLT_MAX || saveMin == 0) && (saveCluster == FLT_MAX || saveCluster == 0)) - minClusterDif = FLT_MAX; - else - minClusterDif = abs(saveMin - saveCluster); - - if((saveMin == FLT_MAX || saveMin == 0) && (saveCenter == FLT_MAX || saveCenter == 0)) - minCenterDif = FLT_MAX; - else - minCenterDif = abs(saveMin - saveCenter); - - if((saveCluster == FLT_MAX || saveCluster == 0) && (saveCenter == FLT_MAX || saveCenter == 0)) - clusterCenterDif = FLT_MAX; - else - clusterCenterDif = abs(saveCluster - saveCenter); - - - if(minClusterDif > max_depth/10) - minClusterDif = FLT_MAX; - if(minCenterDif > max_depth/10) - minCenterDif = FLT_MAX; - if(clusterCenterDif > max_depth/10) - clusterCenterDif = FLT_MAX; - - if(minClusterDif < minCenterDif) - { - if(minClusterDif < clusterCenterDif) - result = (saveMin + saveCluster)/2; - else - result = (saveCluster + saveCenter)/2; - } - else if(minCenterDif < clusterCenterDif) - result = (saveMin + saveCenter )/2; - else if(clusterCenterDif < minClusterDif) - result = (saveCenter + saveCluster)/2; - else - { - result = max(saveMin, saveCenter); - result = max(result, saveCluster); - } - - - return result; -} - int main(int argc, char** argv) { diff --git a/vision/CMakeLists.txt b/vision/CMakeLists.txt new file mode 100644 index 0000000..0c90f87 --- /dev/null +++ b/vision/CMakeLists.txt @@ -0,0 +1,40 @@ +cmake_minimum_required(VERSION 2.8.3) +project(vision) + +set(CMAKE_CXX_FLAGS "-std=c++11") +set(CMAKE_BUILD_TYPE Release) + +find_package(OpenCV REQUIRED) + +include_directories( + include + /usr/include/ + ${OpenCV_INCLUDE_DIRS} +) + + +file(GLOB SOURCES "src/*.cpp") +file(GLOB HEADERS "include/*.hpp") + +link_directories( + /usr/lib/ + /usr/lib/x86_64-linux-gnu/ +) + +add_library(vision SHARED ${SOURCES}) + +target_link_libraries(${PROJECT_NAME} + ${catkin_LIBRARIES} + ${OpenCV_LIBRARIES} + alglib +) + +install(TARGETS vision DESTINATION /usr/local/lib) +install(FILES ${HEADERS} DESTINATION include) + + + + + + + diff --git a/vision/include/vision.hpp b/vision/include/vision.hpp new file mode 100644 index 0000000..fb981f2 --- /dev/null +++ b/vision/include/vision.hpp @@ -0,0 +1,84 @@ +#ifndef VISION_HPP +#define VISION_HPP +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + + + using namespace std; + using namespace cv; + using namespace alglib; + + + + struct Position + { + float x = 0.0; + float y = 0.0; + float z = 0.0; + float top = 0.0; + float height = 0.0; + float distance = 0.0; + }; + + struct People + { + vector< Rect_ > tracked_boxes; + vector< float > tracked_rankings; + vector< Position > tracked_pos; + }; + + //Detection and tracking of blobs + void detectBlobs(Mat& src, vector< Rect_ >& colour_areas, int range); + void track(vector< Rect_ >& current, People& collection, int rank = 3, float threshold = 0.2); + + //Depth estimation functions + double calculateDepth(Mat& src, Rect_ personRect); + double minDepth(vector vec, int number); + double centerDepth(const Mat& src, int number); + double combineDepth(double saveMin, double saveCenter, double saveCluster, double min_depth = 0.0, double max_depth = 6.0); + + //Position estimation + void calculatePosition(Rect& rect, Position& pos, int width = 640, int height = 480, int Hfield = 58, int Vfield = 45); + + + //Region growing algorithms + void upVerticalFill(Mat& src, float threshold, bool flag); + void upVerticalFill2(Mat& src, float threshold, bool scale); + void rightHorizontalFill(Mat& cur_Mat, float threshold, bool scale); + void rectFill(Mat& cur_Mat, float threshold, int range); + + //Background & foreground estimation, to be used in sequence + void estimateBackground(Mat& src, Mat& dst, vector& storage, int recursion, float ratio = 0.04, int index = 0); + void estimateForeground(Mat& src1, Mat& src2, Mat& dst); + + + void frameDif(Mat& src1, Mat& src2, Mat& dst, float threshold); + + //helper functions + int threshold(Mat& src, Mat& dst, int thresh); + void gammaCorrection(Mat& src); + void fixRects(vector< Rect_ >& rects, int screenW); + void depthToGray(Mat& src, Mat& dst, float min_depth, float max_depth); + void grayToDepth(Mat& src, Mat& dst, float max_depth); + kmeansreport clusterize(const vector& vec, int clusters); + + +#endif diff --git a/vision/src/vision.cpp b/vision/src/vision.cpp new file mode 100644 index 0000000..26910b7 --- /dev/null +++ b/vision/src/vision.cpp @@ -0,0 +1,1408 @@ +#include + +/* Detects non-black areas in the image and populates a list of OpenCV Rects + * + * PARAMETERS: -the vector that will be populated + * -screen width + * + * RETURN -- + */ +void detectBlobs(Mat& src, vector< Rect_ >& colour_areas, int range) +{ + float detectionFactor = 0.1; + float mergeFactor = 0.1; + bool flag = false; + int channels = src.channels(); + int cols = src.cols; + int rows = src.rows; + int size = cols*rows*channels; + for(int y = 0; y < 1; y++) + { + const uchar *dif = src.ptr(y); + for(int x = 0; x < size; x = x + channels) + { + if(dif[x] != 0) + { + int i ,j, o; + i = (x/channels)%(cols); + j = floor(x/(cols*channels)); + + Rect_ removal; + if(i + range >= cols) + continue; + if(j + range >= rows) + continue; + + removal = Rect(i, j, range , range); + if(removal.width < 1 || removal.height < 1) + continue; + + if(!colour_areas.empty()) + { + for(int k = 0; k < colour_areas.size(); k++) + { + Rect_ rect = colour_areas[k]; + Rect all = removal | rect; + int temp = removal.y; + removal.y = rect.y; + Rect intersection = removal & rect; + removal.y = temp; + int threshold = intersection.area(); + if(threshold < 1) + continue; + + if(removal.area() < rect.area()) + { + if(threshold > detectionFactor*removal.area()) + { + colour_areas[k] = all; + flag = true; + } + } + else + { + if(threshold > detectionFactor*rect.area()) + { + colour_areas[k] = all; + flag = true; + } + } + } + if(!flag) + { + colour_areas.push_back(removal); + } + else + flag = false; + + int end = colour_areas.size(); + for(int a = 0; a < end; a++) + { + for(int b = a + 1; b < end; b++) + { + Rect_ removal = colour_areas[a]; + Rect_ rect = colour_areas[b]; + + Rect all = removal | rect; + int temp = removal.y; + removal.y = rect.y; + Rect intersection = removal & rect; + removal.y = temp; + int threshold = intersection.area(); + if(threshold < 1) + continue; + + if(removal.area() < rect.area()) + { + if(threshold > mergeFactor*removal.area()) + { + colour_areas[a] = all; + colour_areas[b] = colour_areas.back(); + colour_areas.pop_back(); + b = a + 1; + end--; + } + } + else + { + if(threshold > mergeFactor*rect.area()) + { + colour_areas[a] = all; + colour_areas[b] = colour_areas.back(); + colour_areas.pop_back(); + b = a + 1; + end--; + } + } + + } + } + + + } + else + { + colour_areas.push_back(removal); + } + + } + } + } + int end = colour_areas.size(); + + for(int k = 0; k < end; k++) + { + float width = colour_areas[k].width; + float height = colour_areas[k].height; + float x = colour_areas[k].x; + float y = colour_areas[k].y; + if((x < 0) || (y < 0) || (height < 0) || (width < 0)) + { + //~ rectangle(src, colour_areas[k], 0, CV_FILLED); + cout<<"done"< 1.5) || (area < cols*rows*0.02)) + { + //~ rectangle(src, colour_areas[k], 0, CV_FILLED); + colour_areas[k] = colour_areas.back(); + colour_areas.pop_back(); + k--; + end--; + + } + } + + /* + for(int k = 0; k < end; k++) + { + if(colour_areas[k].width > 250) + { + colour_areas[k].width = colour_areas[k].width/2; + Rect_ rect = colour_areas[k]; + rect.x = rect.x + rect.width; + rect.width = colour_areas[k].width; + colour_areas.push_back(rect); + k--; + end++; + + } + + } + */ + + //~ cout<<"Size: "< >& cur_boxes, People& collection, int rank, float threshold) +{ + bool exists = false; + float step = 1.2; + + + if(!cur_boxes.empty()) + { + + for(int a = 0; a < cur_boxes.size(); a++) + { + exists = false; + for(int b = 0; b < collection.tracked_boxes.size(); b++) + { + Rect intersection = cur_boxes[a] & collection.tracked_boxes[b]; + int area = intersection.area(); + if(cur_boxes[a].area() < collection.tracked_boxes[b].area()) + { + if(area > threshold*cur_boxes[a].area()) + { + collection.tracked_boxes[b] = cur_boxes[a]; + if(collection.tracked_rankings[b] <= 10) + collection.tracked_rankings[b] = collection.tracked_rankings[b] + step; + exists = true; + break; + } + } + else + { + if(area > threshold*collection.tracked_boxes[b].area()) + { + collection.tracked_boxes[b] = cur_boxes[a]; + if(collection.tracked_rankings[b] <= 10) + collection.tracked_rankings[b] = collection.tracked_rankings[b] + step; + exists = true; + break; + } + } + + } + if(!exists) + { + collection.tracked_boxes.push_back(cur_boxes[a]); + collection.tracked_rankings.push_back(rank + step); + } + } + for(int a = 0; a < collection.tracked_boxes.size(); a++) + { + collection.tracked_rankings[a] = collection.tracked_rankings[a] - 1; + } + for(int a = 0; a < collection.tracked_boxes.size(); a++) + { + if(collection.tracked_rankings[a] <= 0 && collection.tracked_boxes.size() > 0) + { + collection.tracked_boxes[a] = collection.tracked_boxes.back(); + collection.tracked_boxes.pop_back(); + collection.tracked_rankings[a] = collection.tracked_rankings.back(); + collection.tracked_rankings.pop_back(); + a--; + } + + } + + } +} + +/*Converts a grayscale Mat to a depth Mat by using the maximum depth value + * + * PARAMETERS: + * -Mat depth + * -Mat with the graysclae result + * -float minimum depth + * -float maximum depth + * + * RETURN: -- + */ +void grayToDepth(Mat& src, Mat& dst, float max_depth) +{ + Mat temp_img(src.rows, src.cols, CV_32FC1); + int cols = src.cols; + int rows = src.rows; + if(src.isContinuous()) + { + cols *= rows; + rows = 1; + } + for(int i = 0; i < rows; i++) + { + uchar* cur = src.ptr(i); + float* Ii = temp_img.ptr(i); + for(int j = 0; j < cols; j++) + { + Ii[j] = (max_depth*(float(cur[j])/(255.0))); + } + } + dst = temp_img.clone(); + +} + +/*Converts a depth Mat to a grayscale one by using the min and maximum depth values + * + * PARAMETERS: + * -Mat depth + * -Mat with the graysclae result + * -float minimum depth + * -float maximum depth + * + * RETURN: -- + */ +void depthToGray(Mat& src, Mat& dst, float min_depth, float max_depth) +{ + + Mat temp_img(src.rows, src.cols, CV_8UC1); + int cols = src.cols; + int rows = src.rows; + if(src.isContinuous()) + { + cols *= rows; + rows = 1; + } + for(int i = 0; i < rows; i++) + { + float* cur = src.ptr(i); + uchar* Ii = temp_img.ptr(i); + for(int j = 0; j < cols; j++) + { + Ii[j] = (255*((cur[j] - min_depth)/(max_depth - min_depth))); + } + } + dst = temp_img.clone(); + +} + +/* Calculates and stores the real world coordinates (x, y, z) of a face + * in respect to the center of the camera. + * + * PARAMETERS: + * -Database object holding the found faces in the image + * + * RETURN: -- + * + */ +void calculatePosition(Rect& rect, Position& pos, int width, int height, int Hfield, int Vfield) +{ + + float hor_x = 0.0; + float hor_y = 0.0; + float ver_x = 0.0; + float ver_y = 0.0; + float hor_focal = 0.0; + float ver_focal = 0.0; + float depth = 0.0; + float distance = 0.0; + float top = 0.0; + float bottom = 0.0; + + + depth = pos.z; + if (depth != 0.0) + { + + //Find the focal length + hor_focal = height / (2 * tan((Vfield/2) * M_PI / 180.0) ); + ver_focal = width / (2 * tan((Hfield/2) * M_PI / 180.0) ); + + //Transform the pixel x, y in respect to + //the camera center + ver_x = rect.x + rect.width/2; + if(ver_x > width/2) + ver_x = (ver_x - width/2); + else + ver_x = (-1)*(width/2 - ver_x); + + ver_y = rect.y + rect.height/2; + if(ver_y > height/2) + ver_y = (-1)*(ver_y - height/2); + else + ver_y = (height/2 - ver_y); + + //Calculate the real world coordinates of the box center + hor_y = depth * ver_y / hor_focal; + hor_x = depth * ver_x / ver_focal; + + if(pos.x != 0) + { + distance = abs(pos.x - hor_x); + pos.distance = distance; + } + pos.x = hor_x; + pos.y = hor_y; + + + + + ver_y = rect.y; + if(ver_y > height/2) + ver_y = (-1)*(ver_y - height/2); + else + ver_y = (height/2 - ver_y); + top = depth * ver_y / hor_focal; + + + ver_y = rect.y + rect.height; + if(ver_y > height/2) + ver_y = (-1)*(ver_y - height/2); + else + ver_y = (height/2 - ver_y); + + bottom = depth * ver_y / hor_focal; + pos.top = abs(top); + pos.height = abs(top - bottom); + + } + +} + +/* Calculates the depth of the closest object in the specified Mat by using clustering + * + * PARAMETERS: + * -Mat + * -number of values to calculate + * + * RETURN: + * -Double holding the min cluster median value + * + */ +double calculateDepth(Mat& src, Rect_ personRect) +{ + + int clusters = 2; + int cols = src.cols; + int rows = src.rows; + int channels = src.channels(); + int size = cols*rows*channels; + double median = 0.0; + double saveMin = 0.0; + double saveCluster = 0.0; + double saveCenter = 0.0; + double depthCombined = 0.0; + vector vec; + vector cluster_vec; + + if(cols > 0 && rows > 0) + { + + if(src.isContinuous()) + { + rows = 1; + cols = size; + } + + cols = src.cols*0.7; + rows = src.rows*0.4; + + for(int i = 0; i < rows; i++) + { + const float* cur = src.ptr(i); + for(int j = 0; j < cols; j++) + { + cluster_vec.push_back(cur[j]); + } + + } + + // clustering + kmeansreport rep = clusterize(cluster_vec, clusters); + + // cluster item count + int counter[clusters]; + for(int i = 0; i < clusters; i++) + { + counter[i] = 0; + } + for(int i = 0; i < rep.cidx.length(); i++) + { + if(rep.cidx[i] > 0 && rep.cidx[i] < clusters) + counter[rep.cidx[i]]++; + } + + //check which centroid has the smallest value + int min_centroid = INT_MAX; + int index [clusters + 1]; + for(int i = 0; i < clusters + 1; i++) + { + index[i] = -1; + } + + if(rep.c.rows() > 0) + { + for(int i = 0; i < clusters; i++) + { + if((min_centroid > rep.c[i][0]) && (rep.c[i][0] > 0)) + { + min_centroid = rep.c[i][0]; + index[0] = i; + } + } + index[index[0] + 1] = INT_MAX; + + //~ cout<(i); + for(int j = 0; j < cols; j++) + { + if(index[0] == rep.cidx[i*cols + j]) + { + cluster_vec.push_back(cur[j]); + } + } + } + + //median + nth_element(cluster_vec.begin(), cluster_vec.begin() + cluster_vec.size()/2, cluster_vec.end()); + median = cluster_vec[cluster_vec.size()/2]; + for(int i = 1; i < clusters + 1; i++) + { + if(index[i] == -1) + { + index[0] = i - 1; + index[i] = INT_MAX; + break; + } + } + } + + //~ printf("%s %2.3f\n","Depth: " ,median); + + /* + Mat src_gray; + depthToGray(src, src_gray, 0, max_depth); + for(int i = 0; i < rows; i++) + { + uchar* cur = src_gray.ptr(i); + for(int j = 0; j < cols; j++) + { + if(rep.cidx[i*cols + j] == index[0]) + { + cur[j] = 0; + } + else + { + cur[j] = 255; + } + } + } + + Mat temp_img(height, depth_width, CV_8UC1); + temp_img = Scalar(0); + + src.copyTo(temp_img(Rect(personRect.x, personRect.y, cols, rows))); + imshow("Clustered", temp_img); + moveWindow("Clustered", 645, 0); + */ + + } + + } + return median; + + + /* + // Preprocessing step to discard null or unwanted depth values + // and convert Mat to vector + for(int i = 0; i < rows; i++) + { + float* cur = src.ptr(i); + for(int j = 0; j < cols; j++) + { + vec.push_back(cur[j]); + } + + } + // average of minimum vector elements + saveMin = minDepth(vec, 20); + + // find the depth of the mat center area + saveCenter = centerDepth(src, 10); + + + + + //~ int id = -1; + //~ double result = FLT_MAX; + //~ + //~ for(int i = 0; i < rep.c.rows() - 1; i++) + //~ { + //~ double temp = std::min(result, rep.c[i + 1][0]); + //~ if(temp > 0) + //~ { + //~ id = i; + //~ result = temp; + //~ } + //~ } + //~ saveCluster = result; + + //combine the results of the 2 closest + depthCombined = combineDepth(saveMin, saveCenter, median, 0, max_depth); + + + //~ printf("%s %2.3f\n","Depth combined: " ,depthCombined); + return depthCombined; + */ + +} + +/* Finds an average of the minimum + * values of a given vector + * + * PARAMETERS: + * -vector with the values + * -number of values to calculate + * + * RETURN: + * -Double holding the average value + * + */ +double minDepth(vector vec, int number) +{ + double result = 0; + double min = 0; + int done = 0; + float threshold = number; + partial_sort (vec.begin(), vec.begin() + threshold, vec.end()); + for(int i = 0; i < threshold; i++) + { + result += vec.at(i); + } + result = result/threshold; + cout<<"Save Min: "<(src.rows/2 - number/2 + i, src.cols/2 - number/2 + j); + if(std::isnan(temp) || temp == FLT_MAX) + continue; + result += temp; + counter++; + } + } + + cout<<"Save Center: "< max_depth) + //~ { + //~ return FLT_MAX; + //~ } + return result/counter; +} + +/* Calculates an average of the 2 closest values + * + * PARAMETERS: + * -Double the min depth approximation + * -Double the center depth approximation + * -Double the cluster depth approximation + * + * + * RETURN: + * -Double holding the average of the 2 closest values + */ +double combineDepth(double saveMin, double saveCenter, double saveCluster, double min_depth, double max_depth) +{ + double result; + double minClusterDif; + double minCenterDif; + double clusterCenterDif; + + + + + if((saveMin == FLT_MAX || saveMin == 0) && (saveCluster == FLT_MAX || saveCluster == 0)) + minClusterDif = FLT_MAX; + else + minClusterDif = abs(saveMin - saveCluster); + + if((saveMin == FLT_MAX || saveMin == 0) && (saveCenter == FLT_MAX || saveCenter == 0)) + minCenterDif = FLT_MAX; + else + minCenterDif = abs(saveMin - saveCenter); + + if((saveCluster == FLT_MAX || saveCluster == 0) && (saveCenter == FLT_MAX || saveCenter == 0)) + clusterCenterDif = FLT_MAX; + else + clusterCenterDif = abs(saveCluster - saveCenter); + + + if(minClusterDif > max_depth/10) + minClusterDif = FLT_MAX; + if(minCenterDif > max_depth/10) + minCenterDif = FLT_MAX; + if(clusterCenterDif > max_depth/10) + clusterCenterDif = FLT_MAX; + + if(minClusterDif < minCenterDif) + { + if(minClusterDif < clusterCenterDif) + result = (saveMin + saveCluster)/2; + else + result = (saveCluster + saveCenter)/2; + } + else if(minCenterDif < clusterCenterDif) + result = (saveMin + saveCenter )/2; + else if(clusterCenterDif < minClusterDif) + result = (saveCenter + saveCluster)/2; + else + { + result = max(saveMin, saveCenter); + result = max(result, saveCluster); + } + + + return result; +} + +/* Estimates the foreground by combining static images, works for + * images the contain edge information + * + * @param a mat representing the current image frame, a mat to store the result, + * the number of images to combine + * @return - + */ +void estimateForeground(Mat& cur_Mat, Mat& back_Mat, Mat& dst_Mat) +{ + uchar *back, *cur, *dst; + for(int y = 0; y < 1; y++) + { + int rows = cur_Mat.rows; + int cols = cur_Mat.cols; + int channels = cur_Mat.channels(); + int size = rows*cols*channels; + + back = back_Mat.ptr(y); + cur = cur_Mat.ptr(y); + dst = dst_Mat.ptr(y); + for(int x = 0; x < size; x = x + channels) + { + if(back[x] != 0) + { + dst[x] = 0; + } + } + } +} + +/* Estimates the background by combining static images, works for + * images the contain edge information + * + * @param a mat representing the current image frame, a mat to store the result, + * the number of images to combine + * @return - + */ +void estimateBackground(Mat& src, Mat& dst, vector& storage, int recursion, float ratio, int index) +{ + int size = storage.size(); + Mat result; + if (index > recursion) + { + dst = storage.at(index - 1); + return; + } + if(size > 0) + { + if((index <= 0) || ((index >= recursion*ratio && (size >= recursion*ratio)))) + { + bitwise_or(src, storage.at(index), result); + } + else + bitwise_and(src, storage.at(index), result); + if((size -1) == index) + { + storage.push_back(result); + dst = result; + } + else + { + storage.at(index) = src; + index++; + estimateBackground(result, dst, storage, recursion, ratio, index); + } + } + else + { + storage.push_back(src); + dst = src.clone(); + } +} + +/*Calculates the absolute difference between the two mats and thresholds the result according to the threshold given + * + * PARAMETERS: + * -Mat first + * -Mat second + * -Mat with the result + * -float threshold to be used + * + * RETURN: -- + */ +void frameDif(Mat& src1, Mat& src2, Mat& dst, float threshold) +{ + uchar *src, *temp, *dest; + int channels = src1.channels(); + int cols = src1.cols; + int rows = src1.rows; + int size = cols*rows*channels; + int motionCounter = 0; + + Mat temp_Mat; + // Absolute dif between our current mat and the previous one + absdiff(src1, src2, temp_Mat); + dst = src1.clone(); + for(int y = 0; y < 1; y++) + { + src = src1.ptr(y); + temp = temp_Mat.ptr(y); + dest = dst.ptr(y); + for(int x = 0; x < size; x = x + channels) + { + float all = 0.0; + float color[channels]; + for (int ch = 0; ch < channels; ch++) + color[ch] = (float)temp[x + ch]; + + for (int i = 0; i < channels; i++) + all += color[i]; + all /= channels; + + if(all > threshold) + { + for (int ch = 0; ch < channels; ch++) + dest[x + ch] = src[x + ch]; + motionCounter++; + } + else + { + for (int ch = 0; ch < channels; ch++) + dest[x + ch] = 0; + } + } + } +} + +/*Region growing algorithm that fills black holes in the depth image, uses + * rectangular areas + * + * PARAMETERS: + * -Mat to be corrected + * -float threshold of correction + * -int range of each correctin area + * + * RETURN: -- + */ +void rectFill(Mat& src, float threshold, int range) +{ + uchar *cur, *temp,*vFill; + int channels = src.channels(); + int cols = src.cols; + int rows = src.rows; + int size = cols*rows*channels; + int count = 0; + float all = 0.0; + float bPercent = 0.0; + float gPercent = 0.0; + float rPercent = 0.0; + bool has_zero = false; + bool flag = false; + + for(int y = 0; y < 1; y++) + { + cur = src.ptr(y); + for(int x = channels*range + cols*range; x < size - (channels*range + cols*range); x = x + channels) + { + if ((cur[x] != 0 && channels == 1) || (channels > 1 && (cur[x+1] != 0 || cur[x+2] != 0) )) + { + int top = x - range*cols*channels; + int topLeft = top - range*channels; + int topRight = top + range*cols*channels; + int bottom = x + range*cols*channels; + int bottomLeft = bottom - range*cols*channels; + int bottomRight = bottom + range*cols*channels; + + if(topLeft%(cols*channels) > top%(cols*channels)) + continue; + if(topRight%(cols*channels) < top%(cols*channels)) + continue; + + has_zero = false; + flag = false; + count = 0; + for(int a = topLeft; a < bottomLeft; a = a + cols*channels) + { + for(int b = a; b < a + 2*range*channels ; b += channels) + { + if (cur[b] == 0) + { + has_zero = true; + count++; + } + else + { + if(channels == 1) + { + all = abs( (float(cur[b])/255) - (float(cur[x])/255) ); + if(all >= threshold) + flag = true; + } + } + + } + } + int countThresh = range; + if(!flag && has_zero && (count <= countThresh)) + { + for(int a = topLeft; a < bottomLeft; a = a + cols*channels) + { + for(int b = a; b < a + 2*range*channels ; b += channels) + { + if (cur[b] == 0) + { + cur[b] = cur[x]; + } + } + } + } + //~ x = x - range*channels + channels; + } + } + } +} + +/* Fills the black holes in the Mat horizontally + * + * PARAMETERS: + * -the Mat to be processed + * -the comparison threshold + * + * RETURN: -- + * + */ +void rightHorizontalFill(Mat& cur_Mat, float threshold, bool scale) +{ + uchar *cur; + int channels = cur_Mat.channels(); + int cols = cur_Mat.cols; + int rows = cur_Mat.rows; + int size = cols*rows*channels; + float all = 0.0, bPercent = 0.0, gPercent = 0.0, rPercent = 0.0; + + for(int y = 0; y < 1; y++) + { + cur = cur_Mat.ptr(y); + for(int x = 0; x < size; x = x + channels) + { + /*If it is the end of the current line, continue*/ + if(x > 0 && (x%(cols*channels) == 0 || x%(cols*channels) == (cols - 1))) + continue; + if(cur[x] != 0 && ((cur[x + channels] == 0 && channels == 1) || (channels > 1 && (cur[x+1] != 0 | cur[x+2] != 0) ))) + { + int start = x + 2*channels; + for(int a = start; a < size; a = a + channels) + { + //check not to change line + if((x%(cols*channels)) > (a%(cols*channels))) + break; + if(cur[a] != 0) + { + if(channels == 1) + { + all = abs( (float(cur[a])/255) - (float(cur[x])/255) ); + if(all < threshold) + { + if(scale) + { + int rowA = a%(cols*channels); + int rowX = x%(cols*channels); + all = (all/abs(rowA - rowX))*255; + int ratio = 1; + for(int i = x + channels; i < a; i = i + channels) + { + cur[i] = cur[x] + ratio*all; + ratio++; + } + } + else + { + for(int i = x + channels; i < a; i = i + channels) + cur[i] = (cur[x] + cur[a])/2; + } + } + } + else if(cur[a + 1] == 0 && cur[a + 2] == 0) + { + + + bPercent = abs( (double(cur[a])/255) - (double(cur[x])/255) ); + gPercent = abs( (double(cur[a + 1])/255) - (double(cur[x + 1])/255) ); + rPercent = abs( (double(cur[a + 2])/255) - (double(cur[x + 2])/255) ); + + all = bPercent + gPercent + rPercent; + + if(all < threshold){ + cur[a] = cur[x]; + cur[a+1] = cur[x+1]; + cur[a+2] = cur[x+2]; + } + else + break; + } + break; + } + } + } + } + } +} + +/* Fills the black holes in the Mat vertically + * from bottom to top + * + * @param the Mat to be processed, the comparison threshold + * @return - + * + */ +void upVerticalFill(Mat& src, float threshold, bool flag) +{ + uchar *cur; + int channels = src.channels(); + int cols = src.cols; + int rows = src.rows; + int size = cols*rows*channels; + float all = 0.0; + float bPercent = 0.0; + float gPercent = 0.0; + float rPercent = 0.0; + + threshold *= 255; + float threshold2 = 0.01*255; + for(int y = 0; y < 1; y++) + { + cur = src.ptr(y); + for(int x = size; x > cols*channels; x = x - channels) + { + + if(int(cur[x]) != 0 && ((int(cur[x - cols*channels]) == 0 && channels == 1) || (channels > 1 && (cur[x+1] != 0 | cur[x+2] != 0) ))) + { + + int start = ((x - 2*cols*channels) > 0)?(x - 2*cols*channels):0; + //~ int end = ((start + cols*verRange) < size)?(start + cols*verRange):size; + + for(int a = start; a > 0 ; a = a - cols*channels) + { + if((x - a)/(cols*channels) > rows/10) + { + break; + } + if(int(cur[a]) != 0) + { + if(channels == 1) + { + all = abs(int(cur[a]) - int(cur[x])); + if(all < threshold) + { + int ratio = 1; + int rowA = floor(a/(cols*channels)); + int rowX = floor(x/(cols*channels)); + int color = int(cur[x]) - int(cur[a]); + color /= abs(rowA - rowX); + + for(int i = x - cols*channels; i > a; i = i - cols*channels) + { + cur[i] = int(cur[x]) - ratio*color; + ratio++; + } + } + else + { + for(int i = x - cols*channels; i > a; i = i - cols*channels) + { + int left = 0; + int right = 0; + for(int j = i; j < size ; j = j + channels) + { + //check not to change line + if((i%(cols*channels)) > (j%(cols*channels))) + break; + if(cur[j] != 0) + { + if(channels == 1) + { + right = j; + } + break; + } + } + for(int j = i; j > 0 ; j = j - channels) + { + //check not to change line + if((i%(cols*channels)) < (j%(cols*channels))) + break; + if(cur[j] != 0) + { + if(channels == 1) + { + left = j; + } + break; + } + } + if(!flag) + { + if((abs(int(cur[x]) - int(cur[left])) < threshold)) + { + cur[i] = int(cur[left]) - (int(cur[left]) - int(cur[x]))/abs(left - x); + } + else if((abs(int(cur[x]) - int(cur[right])) < threshold)) + { + cur[i] = int(cur[right]) - (int(cur[right]) - int(cur[x]))/abs(right - x); + } + } + else + { + //~ if(abs(int(cur[left]) - int(cur[right])) < threshold) + //~ { + cur[i] = int(cur[x]); + //~ } + } + //~ else + //~ { + //~ cur[i] = int(cur[a]); + //~ } + } + } + } + else if(cur[a + 1] == 0 && cur[a + 2] == 0) + { + bPercent = abs(cur[a] - cur[x]); + gPercent = abs(cur[a + 1] - cur[x + 1]); + rPercent = abs(cur[a + 2] - cur[x + 2]); + + all = (bPercent + gPercent + rPercent)/3; + + if(all < threshold) + { + cur[x] = cur[a]; + cur[x+1] = cur[a+1]; + cur[x+2] = cur[a+2]; + } + else + break; + } + break; + } + } + + } + } + } +} + +/* Fills the black holes in the Mat vertically + * from bottom to top + * + * @param the Mat to be processed, the comparison threshold + * @return - + * + */ +void upVerticalFill2(Mat& src, float threshold, bool scale) +{ + uchar *cur; + int channels = src.channels(); + int cols = src.cols; + int rows = src.rows; + int size = cols*rows*channels; + int left; + int right; + int opposite; + float all = 0.0; + float bPercent = 0.0; + float gPercent = 0.0; + float rPercent = 0.0; + + for(int y = 0; y < 1; y++) + { + cur = src.ptr(y); + for(int x = size; x > cols*channels; x = x - channels) + { + opposite = 0; + if(cur[x] != 0 && ((cur[x - cols*channels] == 0 && channels == 1) || (channels > 1 && (cur[x+1] != 0 | cur[x+2] != 0) ))) + { + + int start = ((x - 2*cols*channels) > 0)?(x - 2*cols*channels):0; + + for(int a = start; a > 0 ; a = a - cols*channels) + { + if(cur[a] != 0) + { + if(channels == 1) + { + all = abs( (float(cur[a])/255) - (float(cur[x])/255) ); + if(all < threshold) + { + opposite = a; + } + } + break; + } + } + if(opposite == 0) + continue; + start = x - cols*channels; + for(int k = start; k > opposite; k = k - cols*channels) + { + left = 0; + right = 0; + for(int a = k; a < size ; a = a + channels) + { + //check not to change line + if((k%(cols*channels)) > (a%(cols*channels))) + break; + if(cur[a] != 0) + { + if(channels == 1) + { + all = abs( (float(cur[a])/255) - (float(cur[x])/255) ); + if(all < threshold) + { + right = a; + } + } + else if(cur[a + 1] == 0 && cur[a + 2] == 0) + { + bPercent = abs( (float(cur[a])/255) - (float(cur[x])/255) ); + gPercent = abs( (float(cur[a + 1])/255) - (float(cur[x + 1])/255) ); + rPercent = abs( (float(cur[a + 2])/255) - (float(cur[x + 2])/255) ); + + all = (bPercent + gPercent + rPercent)/3; + + if(all < threshold) + { + for(int i = start; i < a; i = i + channels) + { + cur[i] = cur[x]; + cur[i+1] = cur[x+1]; + cur[i+2] = cur[x+2]; + + } + + + } + } + break; + } + } + for(int a = k; a > 0 ; a = a - channels) + { + //check not to change line + if((k%(cols*channels)) < (a%(cols*channels))) + break; + if(cur[a] != 0) + { + if(channels == 1) + { + all = abs( (float(cur[a])/255) - (float(cur[x])/255) ); + if(all < threshold) + { + left = a; + } + } + else if(cur[a + 1] == 0 && cur[a + 2] == 0) + { + bPercent = abs( (float(cur[a])/255) - (float(cur[x])/255) ); + gPercent = abs( (float(cur[a + 1])/255) - (float(cur[x + 1])/255) ); + rPercent = abs( (float(cur[a + 2])/255) - (float(cur[x + 2])/255) ); + + all = (bPercent + gPercent + rPercent)/3; + + if(all < threshold) + { + for(int i = start; i > a; i = i - channels) + { + cur[i] = cur[x]; + cur[i+1] = cur[x+1]; + cur[i+2] = cur[x+2]; + } + } + } + break; + } + } + if(left == 0 || right == 0) + continue; + if(abs(left - right) > 10 ) + continue; + all = abs( (float(cur[left])/255) - (float(cur[right])/255) ); + if(all > threshold) + { + continue; + } + all = float(cur[left]) - float(cur[right]) ; + all = (all/abs(left - right)); + int ratio = 1; + for(int a = left + channels; a < right; a = a + channels) + { + if(scale) + { + cur[a] = cur[left] - ratio*all; + ratio++; + + } + else + { + cur[a] = cur[x]; + } + } + + } + } + } + } +} + +/* Image gamma correction + * + * @param the Mat to be processed + * @return - + */ +void gammaCorrection(Mat& src) +{ + double inverse_gamma = 1.0 / 2.2; + + Mat lut_matrix(1, 256, CV_8UC1 ); + uchar * ptr = lut_matrix.ptr(); + for( int i = 0; i < 256; i++ ) + ptr[i] = (int)( pow( (double) i / 255.0, inverse_gamma ) * 255.0 ); + LUT( src, lut_matrix, src ); +} + +/* Clusterizes a given vector + * + * PARAMETERS: + * -Vec with the values + * -Int with the number of clusters + * + * RETURN: + * -Double holding the minimum cluster value + * + */ +kmeansreport clusterize(const vector& vec, int clusters) +{ + clusterizerstate s; + kmeansreport rep; + + real_2d_array matrix; + matrix.setlength(vec.size(), 1); + + for (int i = 0; i < vec.size(); i++) + { + matrix(i, 0) = vec.at(i); + } + + try + { + clusterizercreate(s); + clusterizersetpoints(s, matrix, 2); + clusterizersetkmeanslimits(s, 5, 0); + clusterizerrunkmeans(s, clusters, rep); + } + catch(exception& e) + { + printf("%s %s", "Clusterize failed", e.what()); + } + return rep; + + +}