JeVoisBase  1.2
JeVois Smart Embedded Machine Vision Toolkit Base Modules
FastOpticalFlow.C
Go to the documentation of this file.
1 // ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
2 //
3 // JeVois Smart Embedded Machine Vision Toolkit - Copyright (C) 2016 by Laurent Itti, the University of Southern
4 // California (USC), and iLab at USC. See http://iLab.usc.edu and http://jevois.org for information about this project.
5 //
6 // This file is part of the JeVois Smart Embedded Machine Vision Toolkit. This program is free software; you can
7 // redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software
8 // Foundation, version 2. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
9 // without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public
10 // License for more details. You should have received a copy of the GNU General Public License along with this program;
11 // if not, write to the Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
12 //
13 // Contact information: Laurent Itti - 3641 Watt Way, HNB-07A - Los Angeles, CA 90089-2520 - USA.
14 // Tel: +1 213 740 3527 - itti@pollux.usc.edu - http://iLab.usc.edu - http://jevois.org
15 // ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
16 /*! \file */
17 
19 
20 // This is a simplified version of the main() function of the code here: git clone
21 // https://github.com/tikroeger/OF_DIS.git
22 
23 // See jevoisbase/Contrib for full information including copyright and for the other source files.
24 
25 #include <opencv2/imgproc/imgproc.hpp>
26 #include <jevoisbase/Contrib/OF_DIS/oflow.h>
27 
28 // ##############################################################################################################
29 FastOpticalFlow::FastOpticalFlow(std::string const & instance) :
30  jevois::Component::Component(instance), itsProfiler("FastOpticalFlow"), itsNuke(true),
31  img_bo_pyr(nullptr), img_bo_dx_pyr(nullptr), img_bo_dy_pyr(nullptr), img_bo_fmat_pyr(nullptr),
32  img_bo_dx_fmat_pyr(nullptr), img_bo_dy_fmat_pyr(nullptr), itsPyrDepth(0)
33 { }
34 
35 // ##############################################################################################################
37 {
38  for (int i = 0; i < itsPyrDepth; ++i)
39  {
40  img_bo_fmat_pyr[i].release();
41  img_bo_dx_fmat_pyr[i].release();
42  img_bo_dy_fmat_pyr[i].release();
43  }
44 
45  img_bo_mat.release();
46  img_bo_fmat.release();
47 
48  if (itsPyrDepth)
49  {
50  delete [] img_bo_pyr;
51  delete [] img_bo_dx_pyr;
52  delete [] img_bo_dy_pyr;
53  delete [] img_bo_fmat_pyr;
54  delete [] img_bo_dx_fmat_pyr;
55  delete [] img_bo_dy_fmat_pyr;
56  }
57 }
58 
59 // ##############################################################################################################
60 void FastOpticalFlow::onParamChange(fastopticalflow::opoint const & JEVOIS_UNUSED_PARAM(param), int const & val)
61 {
62  std::lock_guard<std::mutex> _(itsMtx); // prevent changes during critical section of process()
63  itsNuke = true;
64 }
65 
66 // ##############################################################################################################
67 void FastOpticalFlow::onParamChange(fastopticalflow::thetasf const & JEVOIS_UNUSED_PARAM(param), int const & val)
68 {
69  std::lock_guard<std::mutex> _(itsMtx); // prevent changes during critical section of process()
70  itsNuke = true;
71 }
72 
73 // ##############################################################################################################
74 void FastOpticalFlow::onParamChange(fastopticalflow::thetaps const & JEVOIS_UNUSED_PARAM(param), int const & val)
75 {
76  std::lock_guard<std::mutex> _(itsMtx); // prevent changes during critical section of process()
77  itsNuke = true;
78 }
79 
80 // ##############################################################################################################
81 void FastOpticalFlow::onParamChange(fastopticalflow::thetaov const & JEVOIS_UNUSED_PARAM(param), float const & val)
82 {
83  std::lock_guard<std::mutex> _(itsMtx); // prevent changes during critical section of process()
84  itsNuke = true;
85 }
86 
87 // ##############################################################################################################
88 namespace
89 {
90  void ConstructImgPyramide(const cv::Mat & img_ao_fmat, cv::Mat * img_ao_fmat_pyr, cv::Mat * img_ao_dx_fmat_pyr,
91  cv::Mat * img_ao_dy_fmat_pyr, const float ** img_ao_pyr, const float ** img_ao_dx_pyr,
92  const float ** img_ao_dy_pyr, const int lv_f, const int lv_l, const int rpyrtype,
93  const bool getgrad, const int imgpadding, const int padw, const int padh)
94  {
95  for (int i=0; i<=lv_f; ++i) // Construct image and gradient pyramides
96  {
97  if (i==0) // At finest scale: copy directly, for all other: downscale previous scale by .5
98  {
99 #if (SELECTCHANNEL==1 | SELECTCHANNEL==3) // use RGB or intensity image directly
100  img_ao_fmat_pyr[i] = img_ao_fmat.clone();
101 #elif (SELECTCHANNEL==2) // use gradient magnitude image as input
102  cv::Mat dx,dy,dx2,dy2,dmag;
103  cv::Sobel( img_ao_fmat, dx, CV_32F, 1, 0, 1, 1, 0, cv::BORDER_DEFAULT );
104  cv::Sobel( img_ao_fmat, dy, CV_32F, 0, 1, 1, 1, 0, cv::BORDER_DEFAULT );
105  dx2 = dx.mul(dx);
106  dy2 = dy.mul(dy);
107  dmag = dx2+dy2;
108  cv::sqrt(dmag,dmag);
109  img_ao_fmat_pyr[i] = dmag.clone();
110 #endif
111  }
112  else
113  cv::resize(img_ao_fmat_pyr[i-1], img_ao_fmat_pyr[i], cv::Size(), .5, .5, cv::INTER_LINEAR);
114 
115  img_ao_fmat_pyr[i].convertTo(img_ao_fmat_pyr[i], rpyrtype);
116 
117  if ( getgrad )
118  {
119  cv::Sobel( img_ao_fmat_pyr[i], img_ao_dx_fmat_pyr[i], CV_32F, 1, 0, 1, 1, 0, cv::BORDER_DEFAULT );
120  cv::Sobel( img_ao_fmat_pyr[i], img_ao_dy_fmat_pyr[i], CV_32F, 0, 1, 1, 1, 0, cv::BORDER_DEFAULT );
121  img_ao_dx_fmat_pyr[i].convertTo(img_ao_dx_fmat_pyr[i], CV_32F);
122  img_ao_dy_fmat_pyr[i].convertTo(img_ao_dy_fmat_pyr[i], CV_32F);
123  }
124  }
125 
126  // pad images
127  for (int i=0; i<=lv_f; ++i) // Construct image and gradient pyramides
128  {
129  copyMakeBorder(img_ao_fmat_pyr[i],img_ao_fmat_pyr[i],imgpadding,imgpadding,imgpadding,
130  imgpadding,cv::BORDER_REPLICATE); // Replicate border for image padding
131  img_ao_pyr[i] = (float*)img_ao_fmat_pyr[i].data;
132 
133  if ( getgrad )
134  {
135  copyMakeBorder(img_ao_dx_fmat_pyr[i],img_ao_dx_fmat_pyr[i],imgpadding,imgpadding,imgpadding,imgpadding,
136  cv::BORDER_CONSTANT , 0); // Zero padding for gradients
137  copyMakeBorder(img_ao_dy_fmat_pyr[i],img_ao_dy_fmat_pyr[i],imgpadding,imgpadding,imgpadding,imgpadding,
138  cv::BORDER_CONSTANT , 0);
139 
140  img_ao_dx_pyr[i] = (float*)img_ao_dx_fmat_pyr[i].data;
141  img_ao_dy_pyr[i] = (float*)img_ao_dy_fmat_pyr[i].data;
142  }
143  }
144  }
145 
146  int AutoFirstScaleSelect(int imgwidth, int fratio, int patchsize)
147  {
148  return std::max(0,(int)std::floor(log2((2.0f*(float)imgwidth) / ((float)fratio * (float)patchsize))));
149  }
150 
151 }
152 
153 // ##############################################################################################################
154 void FastOpticalFlow::process(cv::Mat const & img_ao_mat, cv::Mat & dst)
155 {
156  itsProfiler.start();
157 
158  // Prevent param changes while we are running:
159  std::unique_lock<std::mutex> lck(itsMtx);
160 
161  if (img_ao_mat.type() != CV_8UC1) LFATAL("Input images must have same size and be CV_8UC1 grayscale");
162  if (dst.cols != img_ao_mat.cols || dst.rows != img_ao_mat.rows * 2 || dst.type() != CV_8UC1)
163  LFATAL("dst image must have same width as inputs, be 2x taller, and have CV_8UC1 pixels");
164 
165  // Nuke all caches if input size changed:
166  if (img_ao_mat.cols != itsWidth || img_ao_mat.rows != itsHeight) itsNuke = true;
167  itsWidth = img_ao_mat.cols; itsHeight = img_ao_mat.rows;
168 
169  int rpyrtype, nochannels;
170 #if (SELECTCHANNEL==1 | SELECTCHANNEL==2) // use Intensity or Gradient image
171  rpyrtype = CV_32FC1;
172  nochannels = 1;
173 #elif (SELECTCHANNEL==3) // use RGB image
174  rpyrtype = CV_32FC3;
175  nochannels = 3;
176 #endif
177  cv::Mat img_ao_fmat;
178  cv::Size sz = img_ao_mat.size();
179  int width_org = sz.width; // unpadded original image size
180  int height_org = sz.height; // unpadded original image size
181 
182  // *** Parse rest of parameters, See oflow.h for definitions.
183  int lv_f, lv_l, maxiter, miniter, patchsz, patnorm, costfct, tv_innerit, tv_solverit, verbosity;
184  float mindprate, mindrrate, minimgerr, poverl, tv_alpha, tv_gamma, tv_delta, tv_sor;
185  bool usefbcon, usetvref;
186 
187  mindprate = 0.05; mindrrate = 0.95; minimgerr = 0.0;
188  usefbcon = 0; patnorm = 1; costfct = 0;
189  tv_alpha = 10.0; tv_gamma = 10.0; tv_delta = 5.0;
190  tv_innerit = 1; tv_solverit = 3; tv_sor = 1.6;
191  verbosity = 0; // Default: 2 = Plot detailed timings
192 
193  int fratio = 5; // For automatic selection of coarsest scale: 1/fratio * width = maximum expected motion magnitude in
194  // image. Set lower to restrict search space.
195 
196  switch (opoint::get())
197  {
198  case 1:
199  patchsz = 8; poverl = 0.3;
200  lv_f = AutoFirstScaleSelect(width_org, fratio, patchsz);
201  lv_l = std::max(lv_f-2,0); maxiter = 16; miniter = 16;
202  usetvref = 0;
203  break;
204  case 3:
205  patchsz = 12; poverl = 0.75;
206  lv_f = AutoFirstScaleSelect(width_org, fratio, patchsz);
207  lv_l = std::max(lv_f-4,0); maxiter = 16; miniter = 16;
208  usetvref = 1;
209  break;
210  case 4:
211  patchsz = 12; poverl = 0.75;
212  lv_f = AutoFirstScaleSelect(width_org, fratio, patchsz);
213  lv_l = std::max(lv_f-5,0); maxiter = 128; miniter = 128;
214  usetvref = 1;
215  break;
216  case 2:
217  default:
218  patchsz = 8; poverl = 0.4;
219  lv_f = AutoFirstScaleSelect(width_org, fratio, patchsz);
220  lv_l = std::max(lv_f-2,0); maxiter = 12; miniter = 12;
221  usetvref = 1;
222  break;
223  }
224 
225  // Nuke any old pyramid data and allocate some new one?
226  if (itsNuke)
227  {
228  for (int i = 0; i < itsPyrDepth; ++i)
229  {
230  img_bo_fmat_pyr[i].release();
231  img_bo_dx_fmat_pyr[i].release();
232  img_bo_dy_fmat_pyr[i].release();
233  }
234  img_bo_mat.release();
235  img_bo_fmat.release();
236 
237  if (itsPyrDepth)
238  {
239  delete [] img_bo_pyr;
240  delete [] img_bo_dx_pyr;
241  delete [] img_bo_dy_pyr;
242  delete [] img_bo_fmat_pyr;
243  delete [] img_bo_dx_fmat_pyr;
244  delete [] img_bo_dy_fmat_pyr;
245  }
246 
247  img_bo_pyr = new const float*[lv_f + 1];
248  img_bo_dx_pyr = new const float*[lv_f + 1];
249  img_bo_dy_pyr = new const float*[lv_f + 1];
250  img_bo_fmat_pyr = new cv::Mat[lv_f + 1];
251  img_bo_dx_fmat_pyr = new cv::Mat[lv_f + 1];
252  img_bo_dy_fmat_pyr = new cv::Mat[lv_f + 1];
253  }
254 
255  // Remember pyramid depth so we can free them later:
256  itsPyrDepth = lv_f + 1;
257 
258  // Possibly override some of the default values obtained by setting an operating point:
259  int par = thetasf::get(); if (par != -1) { lv_f = par; lv_l = std::max(0, par - 2); }
260  par = thetait::get(); if (par != -1) { maxiter = par; miniter = par; }
261  par = thetaps::get(); if (par != -1) patchsz = par;
262  float fpar = thetaov::get(); if (fpar != -1.0F) poverl = fpar;
263  usetvref = usevref::get() ? 1 : 0;
264 
265  // keep track of whether we should nuke the caches or not before we unlock:
266  bool nuke = itsNuke; itsNuke = false;
267  lck.unlock();
268 
269  // *** Pad image such that width and height are restless divisible on all scales (except last)
270  int padw=0, padh=0;
271  int scfct = pow(2,lv_f); // enforce restless division by this number on coarsest scale
272  int div = sz.width % scfct;
273  if (div>0) padw = scfct - div;
274  div = sz.height % scfct;
275  if (div>0) padh = scfct - div;
276  if (padh>0 || padw>0)
277  copyMakeBorder(img_ao_mat,img_ao_mat,floor((float)padh/2.0f),ceil((float)padh/2.0f),
278  floor((float)padw/2.0f),ceil((float)padw/2.0f),cv::BORDER_REPLICATE);
279  sz = img_ao_mat.size(); // padded image size, ensures divisibility by 2 on all scales (except last)
280 
281  // *** Generate scale pyramides
282  img_ao_mat.convertTo(img_ao_fmat, CV_32F); // convert to float
283 
284  itsProfiler.checkpoint("Converted");
285 
286  const float* img_ao_pyr[lv_f+1];
287  const float* img_ao_dx_pyr[lv_f+1];
288  const float* img_ao_dy_pyr[lv_f+1];
289 
290  cv::Mat img_ao_fmat_pyr[lv_f+1];
291  cv::Mat img_ao_dx_fmat_pyr[lv_f+1];
292  cv::Mat img_ao_dy_fmat_pyr[lv_f+1];
293 
294  ConstructImgPyramide(img_ao_fmat, img_ao_fmat_pyr, img_ao_dx_fmat_pyr, img_ao_dy_fmat_pyr, img_ao_pyr,
295  img_ao_dx_pyr, img_ao_dy_pyr, lv_f, lv_l, rpyrtype, 1, patchsz, padw, padh);
296 
297  // Copy ao to bo if first frame after a nuke:
298  if (nuke)
299  {
300  img_bo_mat = img_ao_mat;
301  img_bo_fmat = img_ao_fmat;
302 
303  for (int i = 0; i < itsPyrDepth; ++i)
304  {
305  img_bo_fmat_pyr[i] = img_ao_fmat_pyr[i]; img_bo_pyr[i] = (float*)img_bo_fmat_pyr[i].data;
306  img_bo_dx_fmat_pyr[i] = img_ao_dx_fmat_pyr[i]; img_bo_dx_pyr[i] = (float*)img_bo_dx_fmat_pyr[i].data;
307  img_bo_dy_fmat_pyr[i] = img_ao_dy_fmat_pyr[i]; img_bo_dy_pyr[i] = (float*)img_bo_dy_fmat_pyr[i].data;
308  }
309  }
310 
311  itsProfiler.checkpoint("Pyramid");
312 
313  // *** Run main optical flow / depth algorithm
314  float sc_fct = pow(2,lv_l);
315 #if (SELECTMODE==1)
316  cv::Mat flowout(sz.height / sc_fct , sz.width / sc_fct, CV_32FC2); // Optical Flow
317 #else
318  cv::Mat flowout(sz.height / sc_fct , sz.width / sc_fct, CV_32FC1); // Depth
319 #endif
320 
321  OFC::OFClass ofc(img_ao_pyr, img_ao_dx_pyr, img_ao_dy_pyr,
323  patchsz, // extra image padding to avoid border violation check
324  (float*)flowout.data, // pointer to n-band output float array
325  nullptr, // pointer to n-band input float array of size of first (coarsest) scale, or nullptr
326  sz.width, sz.height,
327  lv_f, lv_l, maxiter, miniter, mindprate, mindrrate, minimgerr, patchsz, poverl,
328  usefbcon, costfct, nochannels, patnorm,
329  usetvref, tv_alpha, tv_gamma, tv_delta, tv_innerit, tv_solverit, tv_sor,
330  verbosity);
331  itsProfiler.checkpoint("Flow");
332 
333  // *** Resize to original scale, if not run to finest level
334  if (lv_l != 0)
335  {
336  flowout *= sc_fct;
337  cv::resize(flowout, flowout, cv::Size(), sc_fct, sc_fct , cv::INTER_LINEAR);
338  }
339 
340  // If image was padded, remove padding before returning:
341  flowout = flowout(cv::Rect((int)floor((float)padw/2.0f),(int)floor((float)padh/2.0f),width_org,height_org));
342 
343  itsProfiler.checkpoint("Resized");
344 
345  // flowout is CV_32FC2, we want 2-up CV_8UC1 for our final output:
346  size_t const npix = width_org * height_org;
347  unsigned char * vxptr = dst.data;
348  unsigned char * vyptr = dst.data + npix;
349  float const * fdata = reinterpret_cast<float const *>(flowout.data);
350  float const fac = factor::get();
351 
352  for (int i = 0; i < npix; ++i)
353  {
354  *vxptr++ = (unsigned char)(128.0F + std::max(-128.0F, std::min(127.0F, fdata[0] * fac)));
355  *vyptr++ = (unsigned char)(128.0F + std::max(-128.0F, std::min(127.0F, fdata[1] * fac)));
356  fdata += 2;
357  }
358 
359  itsProfiler.checkpoint("Output formatted");
360 
361  // Get ready for next frame:
362  img_bo_mat = img_ao_mat;
363  img_bo_fmat = img_ao_fmat;
364 
365  for (int i = 0; i < itsPyrDepth; ++i)
366  {
367  img_bo_fmat_pyr[i] = img_ao_fmat_pyr[i]; img_bo_pyr[i] = (float*)img_bo_fmat_pyr[i].data;
368  img_bo_dx_fmat_pyr[i] = img_ao_dx_fmat_pyr[i]; img_bo_dx_pyr[i] = (float*)img_bo_dx_fmat_pyr[i].data;
369  img_bo_dy_fmat_pyr[i] = img_ao_dy_fmat_pyr[i]; img_bo_dy_pyr[i] = (float*)img_bo_dy_fmat_pyr[i].data;
370  }
371 
372  itsProfiler.stop();
373 }
374 
virtual ~FastOpticalFlow()
Destructor.
jevois::Profiler itsProfiler
void checkpoint(char const *description)
void process(cv::Mat const &src, cv::Mat &dst)
Process a greyscale image and return flow in a pre-allocated greyscale image of same with and 2x heig...
cv::Mat * img_bo_dx_fmat_pyr
const float ** img_bo_dy_pyr
#define LFATAL(msg)
void onParamChange(fastopticalflow::opoint const &param, int const &val)
std::mutex itsMtx
cv::Mat * img_bo_fmat_pyr
const float ** img_bo_pyr
const float ** img_bo_dx_pyr
cv::Mat * img_bo_dy_fmat_pyr
FastOpticalFlow(std::string const &instance)
Constructor.