From 9ae7e897719e8bf4e3a8884da1dd9679363ec377 Mon Sep 17 00:00:00 2001 From: Lai Schwe Date: Mon, 17 Aug 2015 21:55:54 +0800 Subject: [PATCH 1/4] Enhanced background learning mechanism Computing accurate background bounds with the help of accelerometer pitch angle. --- .../track_plus/camera_initializer_new.cpp | 11 --- .../track_plus/camera_initializer_new.h | 2 - track_plus_core/track_plus/main.cpp | 10 ++- track_plus_core/track_plus/math_plus.cpp | 16 ++++ track_plus_core/track_plus/math_plus.h | 4 + .../track_plus/motion_processor_new.cpp | 77 +++++++++++++++---- .../track_plus/motion_processor_new.h | 4 +- 7 files changed, 93 insertions(+), 31 deletions(-) diff --git a/track_plus_core/track_plus/camera_initializer_new.cpp b/track_plus_core/track_plus/camera_initializer_new.cpp index 04b44f5..a5329ae 100644 --- a/track_plus_core/track_plus/camera_initializer_new.cpp +++ b/track_plus_core/track_plus/camera_initializer_new.cpp @@ -143,17 +143,6 @@ bool CameraInitializerNew::adjust_exposure(Camera* camera, Mat& image_in, bool r return false; } -float CameraInitializerNew::linear(float x, float m, float c) -{ - return (m * x) + c; -} - -float CameraInitializerNew::exponential(float x, float a, float b, float c) -{ - const float e = 2.718; - return a + (b * pow(e, -c * x)); -} - void CameraInitializerNew::preset0(Camera* camera) { camera->setGlobalGain(0, 1.0); diff --git a/track_plus_core/track_plus/camera_initializer_new.h b/track_plus_core/track_plus/camera_initializer_new.h index fa6bcd5..a89e94e 100644 --- a/track_plus_core/track_plus/camera_initializer_new.h +++ b/track_plus_core/track_plus/camera_initializer_new.h @@ -39,8 +39,6 @@ class CameraInitializerNew static void init(Camera* camera); static bool adjust_exposure(Camera* camera, Mat& image_in, bool reset = false); - static float linear(float x, float m, float c); - static float exponential(float x, float a, float b, float c); static void preset0(Camera* camera); static void preset1(Camera* camera); static void preset2(Camera* camera); diff --git a/track_plus_core/track_plus/main.cpp b/track_plus_core/track_plus/main.cpp index 2296448..ee4bc7f 100644 --- a/track_plus_core/track_plus/main.cpp +++ b/track_plus_core/track_plus/main.cpp @@ -435,8 +435,10 @@ void compute() if (normalized) { - proceed0 = motion_processor0.compute(image_preprocessed0, image_small0, surface_computer.y_reflection, construct_background, "0", true); - proceed1 = motion_processor1.compute(image_preprocessed1, image_small1, surface_computer.y_reflection, construct_background, "1", true); + proceed0 = motion_processor0.compute(image_preprocessed0, image_small0, surface_computer.y_reflection, imu.pitch, + construct_background, "0", true); + proceed1 = motion_processor1.compute(image_preprocessed1, image_small1, surface_computer.y_reflection, imu.pitch, + construct_background, "1", true); } if (first_pass && motion_processor0.both_moving) @@ -463,8 +465,8 @@ void compute() if (proceed) { - proceed0 = foreground_extractor0.compute(image_preprocessed0, motion_processor0, "0", false); - proceed1 = foreground_extractor1.compute(image_preprocessed1, motion_processor1, "1", false); + proceed0 = foreground_extractor0.compute(image_preprocessed0, motion_processor0, "0", true); + proceed1 = foreground_extractor1.compute(image_preprocessed1, motion_processor1, "1", true); proceed = proceed0 && proceed1; } diff --git a/track_plus_core/track_plus/math_plus.cpp b/track_plus_core/track_plus/math_plus.cpp index 1183c07..4ac32c6 100644 --- a/track_plus_core/track_plus/math_plus.cpp +++ b/track_plus_core/track_plus/math_plus.cpp @@ -175,6 +175,22 @@ Point3f normalize(Point3f value) return result; } +float linear(float x, float m, float c) +{ + return (m * x) + c; +} + +float exponential(float x, float a, float b, float c) +{ + const float e = 2.718; + return a + (b * pow(e, -c * x)); +} + +float power(float x, float a, float b) +{ + return a * pow(x, b); +} + float solve_triangle_A_abc(float a, float b, float c) { return acos((-(a * a) + (b * b) + (c * c)) / (2 * b * c)); diff --git a/track_plus_core/track_plus/math_plus.h b/track_plus_core/track_plus/math_plus.h index f2f92f1..c527de9 100644 --- a/track_plus_core/track_plus/math_plus.h +++ b/track_plus_core/track_plus/math_plus.h @@ -40,6 +40,10 @@ Point3f cross_product(Point3f u, Point3f v); float dot_product(Point3f u, Point3f v); Point3f normalize(Point3f value); +float linear(float x, float m, float c); +float exponential(float x, float a, float b, float c); +float power(float x, float a, float b); + //abc are sides, ABC are angles opposing abc sides in radians float solve_triangle_A_abc(float a, float b, float c); float solve_triangle_B_abc(float a, float b, float c); diff --git a/track_plus_core/track_plus/motion_processor_new.cpp b/track_plus_core/track_plus/motion_processor_new.cpp index 2acde9f..f6fbd6d 100644 --- a/track_plus_core/track_plus/motion_processor_new.cpp +++ b/track_plus_core/track_plus/motion_processor_new.cpp @@ -1,6 +1,7 @@ #include "motion_processor_new.h" -bool MotionProcessorNew::compute(Mat& image_in, Mat& image_raw_in, const int y_ref, bool construct_background, const string name, const bool visualize) +bool MotionProcessorNew::compute(Mat& image_in, Mat& image_raw, const int y_ref, float pitch, + bool construct_background, string name, bool visualize) { if (mode == "tool") return false; @@ -496,7 +497,7 @@ bool MotionProcessorNew::compute(Mat& image_in, Mat& image_raw_in, const int y_r //------------------------------------------------------------------------------------------------------------------------ Mat image_canny; - Canny(image_raw_in, image_canny, 50, 50, 3); + Canny(image_raw, image_canny, 50, 50, 3); for (int i = 0; i < WIDTH_SMALL; ++i) for (int j = 0; j < HEIGHT_SMALL; ++j) @@ -560,20 +561,70 @@ bool MotionProcessorNew::compute(Mat& image_in, Mat& image_raw_in, const int y_r //------------------------------------------------------------------------------------------------------------------------ - Mat image_histogram = Mat::zeros(HEIGHT_SMALL, WIDTH_SMALL, CV_8UC1); - for (int i = 0; i < WIDTH_SMALL; ++i) + float width_bottom = x_separator_right - x_separator_left + 10; + float width_top = width_bottom - power(pitch, 0.006834535, 2.199475); + + int x_middle_bottom = x_separator_middle; + int x_middle_top = (x_middle_bottom - (WIDTH_SMALL / 2)) * 0.5 + (WIDTH_SMALL / 2); + + int x_top_left = x_middle_top - (width_top / 2); + int x_top_right = x_middle_top + (width_top / 2); + int x_bottom_left = x_middle_bottom - (width_bottom / 2); + int x_bottom_right = x_middle_bottom + (width_bottom / 2); + + Point pt_top_left = Point(x_top_left, y_separator_up); + Point pt_top_right = Point(x_top_right, y_separator_up); + Point pt_bottom_left = Point(x_bottom_left, y_separator_down); + Point pt_bottom_right = Point(x_bottom_right, y_separator_down); + + Point pt_intersection0; + Point pt_intersection1; + Point pt_intersection2; + Point pt_intersection3; + bool b0 = get_intersection_at_y(pt_top_left, pt_bottom_left, 0, pt_intersection0); + bool b1 = get_intersection_at_y(pt_top_left, pt_bottom_left, HEIGHT_SMALL_MINUS, pt_intersection1); + bool b2 = get_intersection_at_y(pt_top_right, pt_bottom_right, 0, pt_intersection2); + bool b3 = get_intersection_at_y(pt_top_right, pt_bottom_right, HEIGHT_SMALL_MINUS, pt_intersection3); + + if (b0 && b1 && b2 && b3) { - int intensity = 0; - for (int j = 0; j < HEIGHT_SMALL; ++j) - intensity += image_in.ptr(j, i)[0]; + Mat image_borders = Mat::zeros(HEIGHT_SMALL, WIDTH_SMALL, CV_8UC1); - intensity /= 200; - line(image_histogram, Point(i, intensity), Point(i, 0), Scalar(254), 1); - } + line(image_borders, pt_intersection0, pt_intersection1, Scalar(254), 1); + line(image_borders, pt_intersection2, pt_intersection3, Scalar(254), 1); + + BlobDetectorNew* blob_detector_image_borders = value_store.get_blob_detector("blob_detector_image_borders"); + + bool b4 = false; + if (pt_intersection0.x > 0) + { + floodFill(image_borders, Point(0, 0), Scalar(254)); + b4 = true; + } + else if (pt_intersection1.x > 0) + { + floodFill(image_borders, Point(0, HEIGHT_SMALL_MINUS), Scalar(254)); + b4 = true; + } - line(image_histogram, Point(x_separator_left, 0), Point(x_separator_left, 999), Scalar(254), 1); - line(image_histogram, Point(x_separator_right, 0), Point(x_separator_right, 999), Scalar(254), 1); - imshow("image_histogram" + name, image_histogram); + bool b5 = false; + if (pt_intersection2.x < WIDTH_SMALL_MINUS) + { + floodFill(image_borders, Point(WIDTH_SMALL_MINUS, 0), Scalar(254)); + b5 = true; + } + else if (pt_intersection3.x < WIDTH_SMALL_MINUS) + { + floodFill(image_borders, Point(WIDTH_SMALL_MINUS, HEIGHT_SMALL_MINUS), Scalar(254)); + b5 = true; + } + + if (b4 && b5) + for (int i = 0; i < WIDTH_SMALL; ++i) + for (int j = 0; j < HEIGHT_SMALL; ++j) + if (image_borders.ptr(j, i)[0] > 0) + fill_image_background_static(i, j, image_in); + } } } } diff --git a/track_plus_core/track_plus/motion_processor_new.h b/track_plus_core/track_plus/motion_processor_new.h index 22f6d77..d22b1c3 100644 --- a/track_plus_core/track_plus/motion_processor_new.h +++ b/track_plus_core/track_plus/motion_processor_new.h @@ -50,7 +50,9 @@ class MotionProcessorNew ValueStore value_store; - bool compute(Mat& image_in, Mat& image_raw_in, const int y_ref, bool construct_background, const string name, const bool visualize); + bool compute(Mat& image_in, Mat& image_raw, const int y_ref, float pitch, + bool construct_background, string name, bool visualize); + inline void fill_image_background_static(const int x, const int y, Mat& image_in); Mat compute_image_foreground(Mat& image_in); }; \ No newline at end of file From 7b00c00836472c1ad2adc1b4c7e2f0aa23457356 Mon Sep 17 00:00:00 2001 From: Lai Schwe Date: Mon, 17 Aug 2015 23:34:33 +0800 Subject: [PATCH 2/4] Miscellaneous enhancements --- .../track_plus/motion_processor_new.cpp | 101 +++++++++--------- 1 file changed, 52 insertions(+), 49 deletions(-) diff --git a/track_plus_core/track_plus/motion_processor_new.cpp b/track_plus_core/track_plus/motion_processor_new.cpp index f6fbd6d..17798f9 100644 --- a/track_plus_core/track_plus/motion_processor_new.cpp +++ b/track_plus_core/track_plus/motion_processor_new.cpp @@ -468,9 +468,9 @@ bool MotionProcessorNew::compute(Mat& image_in, Mat& image_raw, con for (int i = 0; i < WIDTH_SMALL; ++i) for (int j = 0; j < HEIGHT_SMALL; ++j) if (i < x_separator_middle && image_in.ptr(j, i)[0] > gray_threshold_left) - image_in_thresholded.ptr(j, i)[0] = 254; + image_in_thresholded.ptr(j, i)[0] = 127; else if (i > x_separator_middle && image_in.ptr(j, i)[0] > gray_threshold_right) - image_in_thresholded.ptr(j, i)[0] = 254; + image_in_thresholded.ptr(j, i)[0] = 127; //------------------------------------------------------------------------------------------------------------------------ @@ -497,7 +497,7 @@ bool MotionProcessorNew::compute(Mat& image_in, Mat& image_raw, con //------------------------------------------------------------------------------------------------------------------------ Mat image_canny; - Canny(image_raw, image_canny, 50, 50, 3); + Canny(image_raw, image_canny, 20, 60, 3); for (int i = 0; i < WIDTH_SMALL; ++i) for (int j = 0; j < HEIGHT_SMALL; ++j) @@ -506,15 +506,15 @@ bool MotionProcessorNew::compute(Mat& image_in, Mat& image_raw, con for (BlobNew& blob : *(blob_detector_image_subtraction->blobs)) for (Point& pt : blob.data) - if (image_in_thresholded.ptr(pt.y, pt.x)[0] == 254) - floodFill(image_in_thresholded, pt, Scalar(127)); + if (image_in_thresholded.ptr(pt.y, pt.x)[0] == 127) + floodFill(image_in_thresholded, pt, Scalar(254)); dilate(image_in_thresholded, image_in_thresholded, Mat(), Point(-1, -1), 3); //------------------------------------------------------------------------------------------------------------------------ BlobDetectorNew* blob_detector_image_in_thresholded = value_store.get_blob_detector("blob_detector_image_in_thresholded"); - blob_detector_image_in_thresholded->compute(image_in_thresholded, 254, 0, WIDTH_SMALL, 0, HEIGHT_SMALL, true); + blob_detector_image_in_thresholded->compute(image_in_thresholded, 127, 0, WIDTH_SMALL, 0, HEIGHT_SMALL, true); for (BlobNew& blob : *blob_detector_image_in_thresholded->blobs) if (blob.x < x_separator_left || blob.x > x_separator_right) @@ -561,11 +561,11 @@ bool MotionProcessorNew::compute(Mat& image_in, Mat& image_raw, con //------------------------------------------------------------------------------------------------------------------------ - float width_bottom = x_separator_right - x_separator_left + 10; + float width_bottom = x_separator_right - x_separator_left; float width_top = width_bottom - power(pitch, 0.006834535, 2.199475); int x_middle_bottom = x_separator_middle; - int x_middle_top = (x_middle_bottom - (WIDTH_SMALL / 2)) * 0.5 + (WIDTH_SMALL / 2); + int x_middle_top = (x_separator_middle - (WIDTH_SMALL / 2)) * 0.1 + (WIDTH_SMALL / 2); int x_top_left = x_middle_top - (width_top / 2); int x_top_right = x_middle_top + (width_top / 2); @@ -577,54 +577,57 @@ bool MotionProcessorNew::compute(Mat& image_in, Mat& image_raw, con Point pt_bottom_left = Point(x_bottom_left, y_separator_down); Point pt_bottom_right = Point(x_bottom_right, y_separator_down); - Point pt_intersection0; - Point pt_intersection1; - Point pt_intersection2; - Point pt_intersection3; - bool b0 = get_intersection_at_y(pt_top_left, pt_bottom_left, 0, pt_intersection0); - bool b1 = get_intersection_at_y(pt_top_left, pt_bottom_left, HEIGHT_SMALL_MINUS, pt_intersection1); - bool b2 = get_intersection_at_y(pt_top_right, pt_bottom_right, 0, pt_intersection2); - bool b3 = get_intersection_at_y(pt_top_right, pt_bottom_right, HEIGHT_SMALL_MINUS, pt_intersection3); + if (x_separator_middle < (WIDTH_SMALL / 2) && pt_top_right.x > pt_bottom_right.x) + pt_bottom_right.x = pt_top_right.x; + if (x_separator_middle > (WIDTH_SMALL / 2) && pt_top_left.x < pt_bottom_left.x) + pt_bottom_left.x = pt_top_left.x; - if (b0 && b1 && b2 && b3) - { - Mat image_borders = Mat::zeros(HEIGHT_SMALL, WIDTH_SMALL, CV_8UC1); + Point pt_intersection0 = Point(-1, 0); + Point pt_intersection1 = Point(-1, 0); + Point pt_intersection2 = Point(WIDTH_SMALL, 0); + Point pt_intersection3 = Point(WIDTH_SMALL, 0); + get_intersection_at_y(pt_top_left, pt_bottom_left, 0, pt_intersection0); + get_intersection_at_y(pt_top_left, pt_bottom_left, HEIGHT_SMALL_MINUS, pt_intersection1); + get_intersection_at_y(pt_top_right, pt_bottom_right, 0, pt_intersection2); + get_intersection_at_y(pt_top_right, pt_bottom_right, HEIGHT_SMALL_MINUS, pt_intersection3); - line(image_borders, pt_intersection0, pt_intersection1, Scalar(254), 1); - line(image_borders, pt_intersection2, pt_intersection3, Scalar(254), 1); + Mat image_borders = Mat::zeros(HEIGHT_SMALL, WIDTH_SMALL, CV_8UC1); - BlobDetectorNew* blob_detector_image_borders = value_store.get_blob_detector("blob_detector_image_borders"); + line(image_borders, pt_intersection0, pt_intersection1, Scalar(254), 1); + line(image_borders, pt_intersection2, pt_intersection3, Scalar(254), 1); + line(image_borders, Point(x_middle_top, y_separator_up), Point(x_middle_bottom, y_separator_down), Scalar(254), 1); - bool b4 = false; - if (pt_intersection0.x > 0) - { - floodFill(image_borders, Point(0, 0), Scalar(254)); - b4 = true; - } - else if (pt_intersection1.x > 0) - { - floodFill(image_borders, Point(0, HEIGHT_SMALL_MINUS), Scalar(254)); - b4 = true; - } + BlobDetectorNew* blob_detector_image_borders = value_store.get_blob_detector("blob_detector_image_borders"); - bool b5 = false; - if (pt_intersection2.x < WIDTH_SMALL_MINUS) - { - floodFill(image_borders, Point(WIDTH_SMALL_MINUS, 0), Scalar(254)); - b5 = true; - } - else if (pt_intersection3.x < WIDTH_SMALL_MINUS) - { - floodFill(image_borders, Point(WIDTH_SMALL_MINUS, HEIGHT_SMALL_MINUS), Scalar(254)); - b5 = true; - } + bool proceed0 = false; + if (pt_intersection0.x > 0) + { + floodFill(image_borders, Point(0, 0), Scalar(254)); + proceed0 = true; + } + else if (pt_intersection1.x > 0) + { + floodFill(image_borders, Point(0, HEIGHT_SMALL_MINUS), Scalar(254)); + proceed0 = true; + } - if (b4 && b5) - for (int i = 0; i < WIDTH_SMALL; ++i) - for (int j = 0; j < HEIGHT_SMALL; ++j) - if (image_borders.ptr(j, i)[0] > 0) - fill_image_background_static(i, j, image_in); + bool proceed1 = false; + if (pt_intersection2.x < WIDTH_SMALL_MINUS) + { + floodFill(image_borders, Point(WIDTH_SMALL_MINUS, 0), Scalar(254)); + proceed1 = true; + } + else if (pt_intersection3.x < WIDTH_SMALL_MINUS) + { + floodFill(image_borders, Point(WIDTH_SMALL_MINUS, HEIGHT_SMALL_MINUS), Scalar(254)); + proceed1 = true; } + + if (proceed0 || proceed1) + for (int i = 0; i < WIDTH_SMALL; ++i) + for (int j = 0; j < HEIGHT_SMALL; ++j) + if (image_borders.ptr(j, i)[0] > 0 && image_in_thresholded.ptr(j, i)[0] < 200) + fill_image_background_static(i, j, image_in); } } } From d66a0061b8fc695825d3ea36ca76adaafa22c77d Mon Sep 17 00:00:00 2001 From: Lai Schwe Date: Tue, 18 Aug 2015 10:40:15 +0800 Subject: [PATCH 3/4] Added DTW matching indexes output --- track_plus_core/track_plus/dtw.cpp | 145 ++++++++++++++++++ track_plus_core/track_plus/dtw.h | 28 ++++ track_plus_core/track_plus/main.cpp | 2 +- .../track_plus/mono_processor_new.cpp | 4 + .../track_plus/motion_processor_new.cpp | 24 +-- track_plus_core/track_plus/pose_estimator.cpp | 50 ------ track_plus_core/track_plus/pose_estimator.h | 3 +- .../track_plus/stereo_processor.cpp | 24 +++ track_plus_core/track_plus/stereo_processor.h | 21 +++ .../track_plus/track_plus.vcxproj | 2 + .../track_plus/track_plus.vcxproj.filters | 6 + 11 files changed, 238 insertions(+), 71 deletions(-) create mode 100644 track_plus_core/track_plus/dtw.cpp create mode 100644 track_plus_core/track_plus/dtw.h diff --git a/track_plus_core/track_plus/dtw.cpp b/track_plus_core/track_plus/dtw.cpp new file mode 100644 index 0000000..1c66225 --- /dev/null +++ b/track_plus_core/track_plus/dtw.cpp @@ -0,0 +1,145 @@ +/* + * Touch+ Software + * Copyright (C) 2015 + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the Aladdin Free Public License as + * published by the Aladdin Enterprises, either version 9 of the + * License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * Aladdin Free Public License for more details. + * + * You should have received a copy of the Aladdin Free Public License + * along with this program. If not, see . + */ + +#include "dtw.h" + +Mat compute_cost_mat(vector& vec0, vector& vec1) +{ + Mat cost_mat = Mat(vec1.size(), vec0.size(), CV_32FC1); + + const int vec0_size = vec0.size(); + const int vec1_size = vec1.size(); + for (int i = 0; i < vec0_size; ++i) + for (int j = 0; j < vec1_size; ++j) + cost_mat.ptr(j, i)[0] = get_distance(vec0[i], vec1[j]); + + return cost_mat; +} + +float compute_dtw(Mat& cost_mat) +{ + const int i_max = cost_mat.cols; + const int j_max = cost_mat.rows; + + for (int i = 0; i < i_max; ++i) + for (int j = 0; j < j_max; ++j) + { + float val0; + if (i - 1 < 0) + val0 = FLT_MAX; + else + val0 = cost_mat.ptr(j, i - 1)[0]; + + float val1; + if (i - 1 < 0 || j - i < 0) + val1 = FLT_MAX; + else + val1 = cost_mat.ptr(j - 1, i - 1)[0]; + + float val2; + if (j - 1 < 0) + val2 = FLT_MAX; + else + val2 = cost_mat.ptr(j - 1, i)[0]; + + float val_min = std::min(std::min(val0, val1), val2); + + if (val_min == FLT_MAX) + continue; + + cost_mat.ptr(j, i)[0] += val_min; + } + + return cost_mat.ptr(j_max - 1, i_max - 1)[0]; +} + +vector compute_dtw_indexes(Mat& cost_mat) +{ + const int i_max = cost_mat.cols; + const int j_max = cost_mat.rows; + + for (int i = 0; i < i_max; ++i) + for (int j = 0; j < j_max; ++j) + { + float val0; + if (i - 1 < 0) + val0 = FLT_MAX; + else + val0 = cost_mat.ptr(j, i - 1)[0]; + + float val1; + if (i - 1 < 0 || j - i < 0) + val1 = FLT_MAX; + else + val1 = cost_mat.ptr(j - 1, i - 1)[0]; + + float val2; + if (j - 1 < 0) + val2 = FLT_MAX; + else + val2 = cost_mat.ptr(j - 1, i)[0]; + + float val_min = std::min(std::min(val0, val1), val2); + + if (val_min == FLT_MAX) + continue; + + cost_mat.ptr(j, i)[0] += val_min; + } + + vector seed_vec; + Point seed = Point(i_max - 1, j_max - 1); + seed_vec.push_back(seed); + + while (!(seed.x == 0 && seed.y == 0)) + { + Point seed0 = Point(seed.x - 1, seed.y); + Point seed1 = Point(seed.x - 1, seed.y - 1); + Point seed2 = Point(seed.x, seed.y - 1); + + float seed0_gray; + if (seed0.x >= 0 && seed0.y >= 0) + seed0_gray = cost_mat.ptr(seed0.y, seed0.x)[0]; + else + seed0_gray = FLT_MAX; + + float seed1_gray; + if (seed1.x >= 0 && seed1.y >= 0) + seed1_gray = cost_mat.ptr(seed1.y, seed1.x)[0]; + else + seed1_gray = FLT_MAX; + + float seed2_gray; + if (seed2.x >= 0 && seed2.y >= 0) + seed2_gray = cost_mat.ptr(seed2.y, seed2.x)[0]; + else + seed2_gray = FLT_MAX; + + float seed_gray_min = min(min(seed0_gray, seed1_gray), seed2_gray); + if (seed_gray_min == seed0_gray) + seed = seed0; + else if (seed_gray_min == seed1_gray) + seed = seed1; + else if (seed_gray_min == seed2_gray) + seed = seed2; + + seed_vec.push_back(seed); + } + + return seed_vec; //vec1[seed.y] vec0[seed.x] +} \ No newline at end of file diff --git a/track_plus_core/track_plus/dtw.h b/track_plus_core/track_plus/dtw.h new file mode 100644 index 0000000..be60529 --- /dev/null +++ b/track_plus_core/track_plus/dtw.h @@ -0,0 +1,28 @@ +/* + * Touch+ Software + * Copyright (C) 2015 + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the Aladdin Free Public License as + * published by the Aladdin Enterprises, either version 9 of the + * License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * Aladdin Free Public License for more details. + * + * You should have received a copy of the Aladdin Free Public License + * along with this program. If not, see . + */ + +#pragma once + +#include "math_plus.h" +#include "opencv2/opencv.hpp" + +using namespace cv; + +Mat compute_cost_mat(vector& vec0, vector& vec1); +float compute_dtw(Mat& cost_mat); +vector compute_dtw_indexes(Mat& cost_mat); \ No newline at end of file diff --git a/track_plus_core/track_plus/main.cpp b/track_plus_core/track_plus/main.cpp index ee4bc7f..fc7d580 100644 --- a/track_plus_core/track_plus/main.cpp +++ b/track_plus_core/track_plus/main.cpp @@ -479,7 +479,7 @@ void compute() if (mode == "surface" && proceed) { - proceed0 = mono_processor0.compute(hand_splitter0, "0", false); + proceed0 = mono_processor0.compute(hand_splitter0, "0", true); proceed1 = mono_processor1.compute(hand_splitter1, "1", false); proceed = proceed0 && proceed1; diff --git a/track_plus_core/track_plus/mono_processor_new.cpp b/track_plus_core/track_plus/mono_processor_new.cpp index c693219..3b833d4 100644 --- a/track_plus_core/track_plus/mono_processor_new.cpp +++ b/track_plus_core/track_plus/mono_processor_new.cpp @@ -212,7 +212,11 @@ bool MonoProcessorNew::compute(HandSplitterNew& hand_splitter, const string name sort_contour(contour_approximated_unsorted, contour_approximated, pivot); if (name == "0") + { + // points_unwrapped_result.clear(); + // compute_unwrap2(contour_approximated, pt_palm, points_unwrapped_result) points_unwrapped_result = contour_approximated; + } contour_approximated.insert(contour_approximated.begin(), contour_sorted[0]); contour_approximated.push_back(contour_sorted[contour_sorted.size() - 1]); diff --git a/track_plus_core/track_plus/motion_processor_new.cpp b/track_plus_core/track_plus/motion_processor_new.cpp index 17798f9..44ff157 100644 --- a/track_plus_core/track_plus/motion_processor_new.cpp +++ b/track_plus_core/track_plus/motion_processor_new.cpp @@ -347,26 +347,14 @@ bool MotionProcessorNew::compute(Mat& image_in, Mat& image_raw, con if (both_moving) { y_separator_down = blob_detector_image_histogram->blob_max_size->y_max; - value_store.set_int("y_separator_down_left", y_separator_down); - value_store.set_int("y_separator_down_right", y_separator_down); + value_store.set_int("y_separator_down", y_separator_down); } - else if (left_moving) + else if (left_moving || right_moving) { - int y_separator_down_left = blob_detector_image_histogram->blob_max_size->y_max; - int y_separator_down_right = value_store.get_int("y_separator_down_right", 0); - value_store.set_int("y_separator_down_left", y_separator_down_left); - - if (y_separator_down_left > y_separator_down_right) - y_separator_down = y_separator_down_left; - } - else if (right_moving) - { - int y_separator_down_right = blob_detector_image_histogram->blob_max_size->y_max; - int y_separator_down_left = value_store.get_int("y_separator_down_left", 0); - value_store.set_int("y_separator_down_right", y_separator_down_right); - - if (y_separator_down_right > y_separator_down_left) - y_separator_down = y_separator_down_right; + y_separator_down = value_store.get_int("y_separator_down"); + int y_separator_down_new = blob_detector_image_histogram->blob_max_size->y_max; + if (y_separator_down_new > y_separator_down) + y_separator_down = y_separator_down_new; } //------------------------------------------------------------------------------------------------------------------------ diff --git a/track_plus_core/track_plus/pose_estimator.cpp b/track_plus_core/track_plus/pose_estimator.cpp index fb3ab55..5dad7cf 100644 --- a/track_plus_core/track_plus/pose_estimator.cpp +++ b/track_plus_core/track_plus/pose_estimator.cpp @@ -100,56 +100,6 @@ void PoseEstimator::compute(vector& points_in) waitKey(1); } -Mat PoseEstimator::compute_cost_mat(vector& vec0, vector& vec1) -{ - Mat cost_mat = Mat(vec1.size(), vec0.size(), CV_32FC1); - - const int vec0_size = vec0.size(); - const int vec1_size = vec1.size(); - for (int i = 0; i < vec0_size; ++i) - for (int j = 0; j < vec1_size; ++j) - cost_mat.ptr(j, i)[0] = get_distance(vec0[i], vec1[j]); - - return cost_mat; -} - -float PoseEstimator::compute_dtw(Mat& cost_mat) -{ - const int i_max = cost_mat.cols; - const int j_max = cost_mat.rows; - - for (int i = 0; i < i_max; ++i) - for (int j = 0; j < j_max; ++j) - { - float val0; - if (i - 1 < 0) - val0 = FLT_MAX; - else - val0 = cost_mat.ptr(j, i - 1)[0]; - - float val1; - if (i - 1 < 0 || j - i < 0) - val1 = FLT_MAX; - else - val1 = cost_mat.ptr(j - 1, i - 1)[0]; - - float val2; - if (j - 1 < 0) - val2 = FLT_MAX; - else - val2 = cost_mat.ptr(j - 1, i)[0]; - - float val_min = std::min(std::min(val0, val1), val2); - - if (val_min == FLT_MAX) - continue; - - cost_mat.ptr(j, i)[0] += val_min; - } - - return cost_mat.ptr(j_max - 1, i_max - 1)[0]; -} - bool PoseEstimator::accumulate_pose(const string name_in, const int count_max, string& name_out) { if (count_max > 2) diff --git a/track_plus_core/track_plus/pose_estimator.h b/track_plus_core/track_plus/pose_estimator.h index c1082eb..49a1c39 100644 --- a/track_plus_core/track_plus/pose_estimator.h +++ b/track_plus_core/track_plus/pose_estimator.h @@ -24,6 +24,7 @@ #include "math_plus.h" #include "mat_functions.h" #include "value_store.h" +#include "dtw.h" #include "opencv2/opencv.hpp" #include @@ -43,8 +44,6 @@ class PoseEstimator void init(); void compute(vector& points_in); - Mat compute_cost_mat(vector& vec0, vector& vec1); - float compute_dtw(Mat& cost_mat); bool accumulate_pose(const string name_in, const int count_max, string& name_out); void save(const string name); void load(); diff --git a/track_plus_core/track_plus/stereo_processor.cpp b/track_plus_core/track_plus/stereo_processor.cpp index 54e95d2..a07984b 100644 --- a/track_plus_core/track_plus/stereo_processor.cpp +++ b/track_plus_core/track_plus/stereo_processor.cpp @@ -1,3 +1,21 @@ +/* + * Touch+ Software + * Copyright (C) 2015 + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the Aladdin Free Public License as + * published by the Aladdin Enterprises, either version 9 of the + * License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * Aladdin Free Public License for more details. + * + * You should have received a copy of the Aladdin Free Public License + * along with this program. If not, see . + */ + #include "stereo_processor.h" bool StereoProcessor::compute(MonoProcessorNew& mono_processor0, MonoProcessorNew& mono_processor1, @@ -64,4 +82,10 @@ bool StereoProcessor::compute(MonoProcessorNew& mono_processor0, MonoProcessorNe imshow("image_visualization_stereo_processor", image_visualization_stereo_processor); return true; +} + +void StereoProcessor::compute_dtw_stereo_match(vector& pt_vec0, vector& pt_vec1) +{ + Mat cost_mat = compute_cost_mat(pt_vec0, pt_vec1); + vector dist = compute_dtw_indexes(cost_mat); } \ No newline at end of file diff --git a/track_plus_core/track_plus/stereo_processor.h b/track_plus_core/track_plus/stereo_processor.h index 4e978ee..86cb992 100644 --- a/track_plus_core/track_plus/stereo_processor.h +++ b/track_plus_core/track_plus/stereo_processor.h @@ -1,7 +1,26 @@ +/* + * Touch+ Software + * Copyright (C) 2015 + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the Aladdin Free Public License as + * published by the Aladdin Enterprises, either version 9 of the + * License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * Aladdin Free Public License for more details. + * + * You should have received a copy of the Aladdin Free Public License + * along with this program. If not, see . + */ + #pragma once #include "mono_processor_new.h" #include "overlapping_blob_pair.h" +#include "dtw.h" class StereoProcessor { @@ -10,4 +29,6 @@ class StereoProcessor bool compute(MonoProcessorNew& mono_processor0, MonoProcessorNew& mono_procesosr1, MotionProcessorNew& motion_processor0, MotionProcessorNew& motion_processor1); + + void compute_dtw_stereo_match(vector& pt_vec0, vector& pt_vec1); }; \ No newline at end of file diff --git a/track_plus_visual_studio/track_plus/track_plus.vcxproj b/track_plus_visual_studio/track_plus/track_plus.vcxproj index e195940..6e50812 100644 --- a/track_plus_visual_studio/track_plus/track_plus.vcxproj +++ b/track_plus_visual_studio/track_plus/track_plus.vcxproj @@ -24,6 +24,7 @@ + @@ -74,6 +75,7 @@ + diff --git a/track_plus_visual_studio/track_plus/track_plus.vcxproj.filters b/track_plus_visual_studio/track_plus/track_plus.vcxproj.filters index 8b9e117..58f2608 100644 --- a/track_plus_visual_studio/track_plus/track_plus.vcxproj.filters +++ b/track_plus_visual_studio/track_plus/track_plus.vcxproj.filters @@ -170,6 +170,9 @@ Source Files + + Header Files + @@ -316,5 +319,8 @@ Source Files + + Source Files + \ No newline at end of file From 4a44894a4a82894c42585ba490613a9b85af22fb Mon Sep 17 00:00:00 2001 From: Lai Schwe Date: Tue, 18 Aug 2015 11:29:00 +0800 Subject: [PATCH 4/4] Updated IPC lock file mechanism + misc enhancements --- build/menu_plus/js/ipc.js | 16 +++++++---- build/menu_plus/js/main.js | 5 ++++ track_plus_core/track_plus/ipc.cpp | 16 +++++++++-- track_plus_core/track_plus/main.cpp | 28 +++++++++++++------ .../win_cursor_plus/IPC.cs | 16 +++++++++-- 5 files changed, 62 insertions(+), 19 deletions(-) diff --git a/build/menu_plus/js/ipc.js b/build/menu_plus/js/ipc.js index 3e804eb..287b1bb 100644 --- a/build/menu_plus/js/ipc.js +++ b/build/menu_plus/js/ipc.js @@ -45,6 +45,7 @@ var IPC = function(selfNameIn) IPC.Updated = 0; IPC.FileNameProcessedMap = new Array(); IPC.SentCount = 0; +IPC.LockFileCount = 0; IPC.prototype.selfName = ""; IPC.prototype.responseMap = new Array(); @@ -62,7 +63,11 @@ IPC.prototype.Update = function() for (var fileNameVecIndex in fileNameVec) { var fileNameCurrent = fileNameVec[fileNameVecIndex]; - if (fileNameCurrent == "lock") + var fileNameLock = ""; + if (fileNameCurrent.length >= 4) + fileNameLock = fileNameCurrent.substring(0, 4); + + if (fileNameLock == "lock") { IPC.Updated = 0; return; @@ -159,19 +164,20 @@ IPC.prototype.SendMessage = function(recipient, messageHead, messageBody) { var self = this; - WriteStringToFile(IpcPath + "/lock", ""); + var lockFileName = "lock_" + self.selfName + IPC.LockFileCount.toString(); + WriteStringToFile(IpcPath + "/" + lockFileName, ""); + ++IPC.LockFileCount; + var fileNameVec = ListFilesInDirectory(IpcPath); var found = true; var fileCount = 0; - while (found) { found = false; for (var fileNameVecIndex in fileNameVec) { var fileNameCurrent = fileNameVec[fileNameVecIndex]; - if (fileNameCurrent == recipient + fileCount.toString()) { found = true; @@ -185,7 +191,7 @@ IPC.prototype.SendMessage = function(recipient, messageHead, messageBody) WriteStringToFile(pathNew, messageHead + "!" + messageBody); ++IPC.SentCount; - DeleteFile(IpcPath + "/lock"); + DeleteFile(IpcPath + "/" + lockFileName); console.log("message sent: " + recipient + " " + messageHead + " " + messageBody); }; diff --git a/build/menu_plus/js/main.js b/build/menu_plus/js/main.js index c26fc98..112d1cd 100644 --- a/build/menu_plus/js/main.js +++ b/build/menu_plus/js/main.js @@ -106,6 +106,11 @@ //---------------------------------------------------------------------------------------------------- + ipc.MapFunction("menu_plus_ready", function(messageBody) + { + ipc.SendMessage("track_plus", "menu_plus_ready", ""); + }); + ipc.MapFunction("show window", function(messageBody) { win.show(); diff --git a/track_plus_core/track_plus/ipc.cpp b/track_plus_core/track_plus/ipc.cpp index b2fa028..77734d7 100644 --- a/track_plus_core/track_plus/ipc.cpp +++ b/track_plus_core/track_plus/ipc.cpp @@ -37,11 +37,17 @@ void IPC::update() vector file_name_vec = list_files_in_directory(ipc_path); for (string file_name_current : file_name_vec) - if (file_name_current == "lock") + { + string file_name_lock = ""; + if (file_name_current.size() >= 4) + file_name_lock = file_name_current.substr(0, 4); + + if (file_name_lock == "lock") { updated = true; return; } + } for (string file_name_current : file_name_vec) { @@ -123,7 +129,11 @@ void IPC::clear() void IPC::send_message(const string recipient, const string message_head, const string message_body) { - write_string_to_file(ipc_path + slash + "lock", ""); + static int lock_file_count = 0; + string lock_file_name = "lock_" + self_name + to_string(lock_file_count); + write_string_to_file(ipc_path + slash + lock_file_name, ""); + ++lock_file_count; + vector file_name_vec = list_files_in_directory(ipc_path); bool found = true; @@ -147,7 +157,7 @@ void IPC::send_message(const string recipient, const string message_head, const write_string_to_file(path_new, message_head + "!" + message_body); ++sent_count; - delete_file(ipc_path + slash + "lock"); + delete_file(ipc_path + slash + lock_file_name); COUT << "message sent: " << recipient << " " << message_head << " " << message_body << endl; } diff --git a/track_plus_core/track_plus/main.cpp b/track_plus_core/track_plus/main.cpp index fc7d580..869b517 100644 --- a/track_plus_core/track_plus/main.cpp +++ b/track_plus_core/track_plus/main.cpp @@ -264,10 +264,6 @@ void on_first_frame() // else mode = "surface"; - ipc->send_message("menu_plus", "show notification", "Please wait:Initializing Touch+ Software"); - - COUT << "here" << endl; - reprojector.load(*ipc, true); CameraInitializerNew::init(camera); pose_estimator.init(); @@ -479,7 +475,7 @@ void compute() if (mode == "surface" && proceed) { - proceed0 = mono_processor0.compute(hand_splitter0, "0", true); + proceed0 = mono_processor0.compute(hand_splitter0, "0", false); proceed1 = mono_processor1.compute(hand_splitter1, "1", false); proceed = proceed0 && proceed1; @@ -790,8 +786,8 @@ void guardian_thread_function() if (child_module_name != "" && child_module_path != "" && process_running(child_module_name + ".exe") == 0) create_process(child_module_path, child_module_name + ".exe", true); - // if (process_running("daemon_plus.exe") == 0) - // ipc->send_message("everyone", "exit", ""); + if (process_running("daemon_plus.exe") == 0) + ipc->send_message("everyone", "exit", ""); Sleep(500); } @@ -811,6 +807,8 @@ int main() init_paths(); ipc = new IPC("track_plus"); + thread ipc_thread(ipc_thread_function); + ipc->map_function("exit", [](const string message_body) { if (child_module_name != "") @@ -820,6 +818,21 @@ int main() exit(0); }); + bool menu_plus_ready = false; + bool* menu_plus_ready_ptr = &menu_plus_ready; + ipc->map_function("menu_plus_ready", [menu_plus_ready_ptr](const string message_body) + { + *menu_plus_ready_ptr = true; + }); + + while (!menu_plus_ready) + { + ipc->send_message("menu_plus", "menu_plus_ready", ""); + Sleep(1000); + } + + ipc->send_message("menu_plus", "show notification", "Please wait:Initializing Touch+ Software"); + #ifdef _WIN32 thread keyboard_hook_thread(keyboard_hook_thread_function); #elif __APPLE__ @@ -828,7 +841,6 @@ int main() thread guardian_thread(guardian_thread_function); thread input_thread(input_thread_function); thread pose_estimator_thread(pose_estimator_thread_function); - thread ipc_thread(ipc_thread_function); camera = new Camera(true, 1280, 480, update); diff --git a/track_plus_visual_studio/win_cursor_plus/IPC.cs b/track_plus_visual_studio/win_cursor_plus/IPC.cs index 90375c4..c642975 100644 --- a/track_plus_visual_studio/win_cursor_plus/IPC.cs +++ b/track_plus_visual_studio/win_cursor_plus/IPC.cs @@ -30,6 +30,7 @@ class IPC public static bool Updated = true; public static Dictionary FileNameProcessedMap = new Dictionary(); public static int SentCount = 0; + public static int LockFileCount = 0; private string selfName = ""; private Dictionary> responseMap = new Dictionary>(); @@ -70,11 +71,17 @@ public void Update() List fileNameVec = FileSystem.ListFilesInDirectory(Globals.IpcPath); foreach (string fileNameCurrent in fileNameVec) - if (fileNameCurrent == "lock") + { + string fileNameLock = ""; + if (fileNameCurrent.Length >= 4) + fileNameLock = fileNameCurrent.Substring(0, 4); + + if (fileNameLock == "lock") { IPC.Updated = true; return; } + } foreach (string fileNameCurrent in fileNameVec) { @@ -157,7 +164,10 @@ public void Clear() public void SendMessage(string recipient, string messageHead, string messageBody) { - FileSystem.WriteStringToFile(Globals.IpcPath + "\\lock", ""); + string lockFileName = "lock_" + selfName + LockFileCount.ToString(); + FileSystem.WriteStringToFile(Globals.IpcPath + "\\" + lockFileName, ""); + ++LockFileCount; + List fileNameVec = FileSystem.ListFilesInDirectory(Globals.IpcPath); bool found = true; @@ -179,7 +189,7 @@ public void SendMessage(string recipient, string messageHead, string messageBody FileSystem.WriteStringToFile(pathNew, messageHead + "!" + messageBody); ++IPC.SentCount; - FileSystem.DeleteFile(Globals.IpcPath + "\\lock"); + FileSystem.DeleteFile(Globals.IpcPath + "\\" + lockFileName); Console.WriteLine("message sent: " + recipient + " " + messageHead + " " + messageBody); }