25 #include <opencv2/imgproc/imgproc.hpp>
26 #include <jevoisbase/Contrib/OF_DIS/oflow.h>
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)
62 std::lock_guard<std::mutex> _(
itsMtx);
69 std::lock_guard<std::mutex> _(
itsMtx);
76 std::lock_guard<std::mutex> _(
itsMtx);
83 std::lock_guard<std::mutex> _(
itsMtx);
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)
95 for (
int i=0; i<=lv_f; ++i)
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 );
109 img_ao_fmat_pyr[i] = dmag.clone();
113 cv::resize(img_ao_fmat_pyr[i-1], img_ao_fmat_pyr[i], cv::Size(), .5, .5, cv::INTER_LINEAR);
115 img_ao_fmat_pyr[i].convertTo(img_ao_fmat_pyr[i], rpyrtype);
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);
127 for (
int i=0; i<=lv_f; ++i)
129 copyMakeBorder(img_ao_fmat_pyr[i],img_ao_fmat_pyr[i],imgpadding,imgpadding,imgpadding,
130 imgpadding,cv::BORDER_REPLICATE);
131 img_ao_pyr[i] = (
float*)img_ao_fmat_pyr[i].data;
135 copyMakeBorder(img_ao_dx_fmat_pyr[i],img_ao_dx_fmat_pyr[i],imgpadding,imgpadding,imgpadding,imgpadding,
136 cv::BORDER_CONSTANT , 0);
137 copyMakeBorder(img_ao_dy_fmat_pyr[i],img_ao_dy_fmat_pyr[i],imgpadding,imgpadding,imgpadding,imgpadding,
138 cv::BORDER_CONSTANT , 0);
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;
146 int AutoFirstScaleSelect(
int imgwidth,
int fratio,
int patchsize)
148 return std::max(0,(
int)std::floor(log2((2.0f*(
float)imgwidth) / ((
float)fratio * (
float)patchsize))));
159 std::unique_lock<std::mutex> lck(
itsMtx);
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");
169 int rpyrtype, nochannels;
170 #if (SELECTCHANNEL==1 | SELECTCHANNEL==2) // use Intensity or Gradient image
173 #elif (SELECTCHANNEL==3) // use RGB image
178 cv::Size sz = img_ao_mat.size();
179 int width_org = sz.width;
180 int height_org = sz.height;
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;
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;
196 switch (opoint::get())
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;
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;
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;
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;
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.0
F) poverl = fpar;
263 usetvref = usevref::get() ? 1 : 0;
271 int scfct = pow(2,lv_f);
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();
282 img_ao_mat.convertTo(img_ao_fmat, CV_32F);
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];
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];
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);
314 float sc_fct = pow(2,lv_l);
316 cv::Mat flowout(sz.height / sc_fct , sz.width / sc_fct, CV_32FC2);
318 cv::Mat flowout(sz.height / sc_fct , sz.width / sc_fct, CV_32FC1);
321 OFC::OFClass ofc(img_ao_pyr, img_ao_dx_pyr, img_ao_dy_pyr,
324 (
float*)flowout.data,
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,
337 cv::resize(flowout, flowout, cv::Size(), sc_fct, sc_fct , cv::INTER_LINEAR);
341 flowout = flowout(cv::Rect((
int)floor((
float)padw/2.0f),(
int)floor((
float)padh/2.0f),width_org,height_org));
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();
352 for (
int i = 0; i < npix; ++i)
354 *vxptr++ = (
unsigned char)(128.0
F + std::max(-128.0
F, std::min(127.0
F, fdata[0] * fac)));
355 *vyptr++ = (
unsigned char)(128.0
F + std::max(-128.0
F, std::min(127.0
F, fdata[1] * fac)));