git.mutantstargoat.com Git - vkeyb/blob - src/motion.cc

   1 /*
   2 vkeyb - camera motion detection virtual keyboard
   3 Copyright (C) 2012 Eleni Maria Stea <elene.mst@gmail.com>
   4
   5 This program is free software: you can redistribute it and/or modify
   6 it under the terms of the GNU General Public License as published by
   7 the Free Software Foundation, either version 3 of the License, or
   8 (at your option) any later version.
   9
  10 This program is distributed in the hope that it will be useful,
  11 but WITHOUT ANY WARRANTY; without even the implied warranty of
  12 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  13 GNU General Public License for more details.
  14
  15 You should have received a copy of the GNU General Public License
  16 along with this program.  If not, see <http://www.gnu.org/licenses/>.
  17 */
  18
  19 #include <unistd.h>
  20 #include <stdio.h>
  21 #include <sys/time.h>
  22 #include "motion.h"
  23
  24 #define NUM_FEATURES 400
  25 #define MHI_DURATION 1000
  26 #define OFFSET 30
  27
  28 static unsigned long get_msec();
  29
  30 bool stop_capture = false;
  31 int pipefd[2];
  32 cv::Mat frm;
  33 pthread_t ptd;
  34
  35 bool start_capture()
  36 {
  37         if(pipe(pipefd) == -1) {
  38                 perror("failed to create synchronization pipe");
  39                 return false;
  40         }
  41
  42         int res = pthread_create(&ptd, 0, capture_thread, 0);
  43         if(res != 0) {
  44                 fprintf(stderr, "Failed to create capturing thread: %s\n", strerror(res));
  45                 return false;
  46         }
  47         return true;
  48 }
  49
  50 void *capture_thread(void *arg)
  51 {
  52         cv::VideoCapture cap(0);
  53         if(!cap.isOpened()) {
  54                 fprintf(stderr, "failed to open video capture device\n");
  55         }
  56
  57         cv::Mat next_frm, prev_frm, frm8b, prev_frm8b, colimg;
  58
  59         cap >> next_frm;
  60         cv::flip(next_frm, next_frm, 1);
  61         colimg = next_frm.clone();
  62         cv::cvtColor(next_frm, next_frm, CV_RGB2GRAY);
  63         next_frm.convertTo(frm8b, CV_8UC1);
  64
  65         while(!stop_capture) {
  66                 prev_frm = next_frm.clone();
  67                 prev_frm.convertTo(prev_frm8b, CV_8UC1);
  68
  69                 cap >> next_frm;
  70                 cv::flip(next_frm, next_frm, 1);
  71                 colimg = next_frm.clone();
  72                 cv::cvtColor(next_frm, next_frm, CV_RGB2GRAY);
  73                 next_frm.convertTo(frm8b, CV_8UC1);
  74
  75                 double direction = calculate_motion_dir(frm8b, prev_frm8b, colimg);
  76                 write(pipefd[1], &direction, sizeof direction);
  77                 frm = colimg.clone();
  78         }
  79         return 0;
  80 }
  81
  82 double calculate_motion_dir(cv::Mat &frm8b, cv::Mat &prev_frm8b, cv::Mat &colimg)
  83 {
  84         std::vector<cv::Point2f> prev_corners;
  85         std::vector<cv::Point2f> corners;
  86         std::vector<unsigned char> status;
  87         std::vector<float> err;
  88
  89         cv::goodFeaturesToTrack(prev_frm8b, prev_corners, (float)NUM_FEATURES, 0.01, 3.0);
  90         cv::goodFeaturesToTrack(frm8b, corners, (float)NUM_FEATURES, 0.01, 3.0);
  91         cv::calcOpticalFlowPyrLK(prev_frm8b, frm8b, prev_corners, corners, status, err);
  92
  93         cv::Point motion_vector = cv::Point((int)((double)colimg.cols / 2.0), (int)((double)colimg.rows / 2.0));
  94
  95         for(size_t i=0; i<status.size(); i++) {
  96                 if(!status[i])
  97                         continue;
  98
  99                 cv::Point p, q;
 100                 p.x = prev_corners[i].x;
 101                 p.y = prev_corners[i].y;
 102
 103                 q.x = corners[i].x;
 104                 q.y = corners[i].y;
 105
 106                 double angle = atan2((double)(q.y - p.y), (double)(q.x - p.x));
 107                 double hypotenuse = sqrt(pow((double)(q.y - p.y), 2.0) + pow((double)(q.x - p.x), 2.0));
 108
 109                 p.x = (int)(q.x - hypotenuse * cos(angle));
 110                 p.y = (int)(q.y - hypotenuse * sin(angle));
 111
 112                 cv::line(colimg, q, p, cv::Scalar(255, 255, 0), 1, CV_AA, 0);
 113
 114                 if(hypotenuse > 3) {
 115                         cv::Point vec2d = cv::Point(q.x - p.x, q.y - p.y);
 116                         motion_vector = cv::Point(motion_vector.x + vec2d.x, motion_vector.y + vec2d.y);
 117                 }
 118
 119 /*
 120                 q.x = (int)(p.x + cos(angle) + M_PI / 4.0);
 121                 q.y = (int)(p.y + sin(angle) + M_PI / 4.0);
 122
 123                 cv::line(colimg, q, p, cv::Scalar(0, 0, 255), 1, CV_AA, 0);
 124
 125                 q.x = (int)(p.x + cos(angle) - M_PI / 4.0);
 126                 q.y = (int)(p.y + sin(angle) - M_PI / 4.0);
 127
 128                 cv::line(colimg, q, p, cv::Scalar(255, 0, 0), 1, CV_AA, 0);*/
 129
 130         }
 131
 132         cv::Point ctr = cv::Point((int)((double)colimg.cols / 2.0), (int)((double)colimg.rows / 2.0));
 133         cv::Point xproj = cv::Point(motion_vector.x, ctr.y);
 134
 135         cv::line(colimg, motion_vector, ctr, cv::Scalar(255, 0, 0), 3, CV_AA, 0);
 136         cv::line(colimg, xproj, ctr, cv::Scalar(0, 0, 255), 3, CV_AA, 0);
 137
 138         return (xproj.x - ctr.x);
 139 }
 140
 141 double calculate_orientation(cv::Mat &frm, cv::Mat &prev_frm)
 142 {
 143         cv::Mat silhouette;
 144         cv::absdiff(frm, prev_frm, silhouette);
 145
 146         cv::Mat sil8;
 147         cv::cvtColor(silhouette, silhouette, CV_RGB2GRAY);
 148         silhouette.convertTo(sil8, CV_8UC1);
 149
 150         double max_val, min_val;
 151         cv::minMaxLoc(sil8, &min_val, &max_val);
 152
 153         cv::threshold(sil8, sil8, 119, max_val, CV_THRESH_BINARY);
 154
 155         cv::Mat mhi;
 156         sil8.convertTo(mhi, CV_32FC1, 1.0 / (float)UCHAR_MAX);
 157
 158         cv::Mat mask = cv::Mat(mhi.size().width, mhi.size().height, CV_8UC1);
 159         cv::Mat orientation = mhi.clone();
 160
 161         double duration = MHI_DURATION;
 162         unsigned long timestamp = get_msec() + duration;
 163
 164         double min_delta = abs(timestamp);
 165         double max_delta = abs(timestamp) + 500.0;
 166
 167         cv::updateMotionHistory(sil8, mhi, timestamp, duration);
 168         cv::calcMotionGradient(mhi, mask, orientation, min_delta, max_delta, 3);
 169
 170         double global_orientation = cv::calcGlobalOrientation(orientation, mask, mhi, timestamp, duration);
 171
 172         return global_orientation;
 173 }
 174
 175
 176 static unsigned long get_msec()
 177 {
 178         static struct timeval tv0;
 179         struct timeval tv;
 180
 181         gettimeofday(&tv, 0);
 182         if(tv0.tv_sec == 0 && tv0.tv_usec == 0) {
 183                 tv0 = tv;
 184                 return 0;
 185         }
 186         return (tv.tv_sec - tv0.tv_sec) * 1000 + (tv.tv_usec - tv0.tv_usec) / 1000;
 187 }