hypothesis.cpp 12 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433
  1. /*
  2. * hypothesis.cpp
  3. *
  4. * Created on: Feb 8, 2016
  5. * Author: Janai
  6. */
  7. #include "hypothesis.h"
  8. #include "utils.h"
  9. hypothesis* hypothesis::new_complete(int approach) {
  10. int l = endF - startF + 1;
  11. hypothesis* h = NULL;
  12. if(approach == LINEAR_EXTRAPOLATION) {
  13. if(l < 2) return NULL;
  14. double t[l], x[l], y[l];
  15. // get coordinates for fitting
  16. t[0] = startF; // frame starting from start
  17. x[0] = p.x;
  18. y[0] = p.y;
  19. for(int f = 0; f < (l - 1); f++) {
  20. t[f + 1] = startF + f + 1;
  21. x[f + 1] = p.x + flow_x[startF + f];
  22. y[f + 1] = p.y + flow_y[startF + f];
  23. }
  24. // fit linear function
  25. double y_c0, y_c1, y_cov00, y_cov01, y_cov11, y_sumsq;
  26. double x_c0, x_c1, x_cov00, x_cov01, x_cov11, x_sumsq;
  27. gsl_fit_linear(t, 1, y, 1, l, &y_c0, &y_c1, &y_cov00, &y_cov01, &y_cov11, &y_sumsq);
  28. gsl_fit_linear(t, 1, x, 1, l, &x_c0, &x_c1, &x_cov00, &x_cov01, &x_cov11, &x_sumsq);
  29. // create new hypothesis while replacing the points used to fit!
  30. double *new_flow_y = new double[F];
  31. double *new_flow_x = new double[F];
  32. for(int f = 0; f < F; f++) {
  33. new_flow_y[f] = y_c1 * (f + 1); // + p.y; ONLY INTERESTED IN FLOW
  34. new_flow_x[f] = x_c1 * (f + 1); // + p.x;
  35. }
  36. h = new hypothesis(F, new_flow_x, new_flow_y, x_c0, y_c0);
  37. h->jet_est = jet_est;
  38. } else if(approach == QUADRATIC_EXTRAPOLATION) {
  39. if(l < 3) return NULL;
  40. double chisq_x, chisq_y;
  41. gsl_matrix *T, *cov_x, *cov_y;
  42. gsl_vector *x, *y, *w, *c_x, *c_y;
  43. T = gsl_matrix_alloc(l, 3);
  44. x = gsl_vector_alloc(l);
  45. y = gsl_vector_alloc(l);
  46. w = gsl_vector_alloc(l);
  47. c_x = gsl_vector_alloc(3);
  48. c_y = gsl_vector_alloc(3);
  49. cov_x = gsl_matrix_alloc(3, 3);
  50. cov_y = gsl_matrix_alloc(3, 3);
  51. int t0 = startF; // frame starting from start
  52. gsl_matrix_set(T, 0, 0, 1.0);
  53. gsl_matrix_set(T, 0, 1, t0);
  54. gsl_matrix_set(T, 0, 2, t0 * t0);
  55. gsl_vector_set(x, 0, p.x);
  56. gsl_vector_set(y, 0, p.y);
  57. gsl_vector_set(w, 0, 1.0 / l); // TODO CHANGE WEIGHT
  58. for (int i = 0; i < l - 1; i++) {
  59. int ti = (t0 + i + 1);
  60. gsl_matrix_set(T, i + 1, 0, 1.0);
  61. gsl_matrix_set(T, i + 1, 1, ti);
  62. gsl_matrix_set(T, i + 1, 2, ti * ti);
  63. gsl_vector_set(x, i + 1, p.x + flow_x[startF + i]);
  64. gsl_vector_set(y, i + 1, p.y + flow_y[startF + i]);
  65. gsl_vector_set(w, i + 1, 1.0 / l);
  66. }
  67. {
  68. gsl_multifit_linear_workspace * work = gsl_multifit_linear_alloc(l, 3);
  69. gsl_multifit_wlinear(T, w, x, c_x, cov_x, &chisq_x, work);
  70. gsl_multifit_linear_free(work);
  71. work = gsl_multifit_linear_alloc(l, 3);
  72. gsl_multifit_wlinear(T, w, y, c_y, cov_y, &chisq_y, work);
  73. gsl_multifit_linear_free(work);
  74. }
  75. #define C_X(i) (gsl_vector_get(c_x,(i)))
  76. #define C_Y(i) (gsl_vector_get(c_y,(i)))
  77. // create new hypothesis while replacing the points used to fit!
  78. double *new_flow_y = new double[F];
  79. double *new_flow_x = new double[F];
  80. for(int f = 0; f < F; f++) {
  81. new_flow_x[f] = C_X(1) * (f + 1) + C_X(2) * (f + 1) * (f + 1); // + p.x;
  82. new_flow_y[f] = C_Y(1) * (f + 1) + C_Y(2) * (f + 1) * (f + 1); // + p.y; ONLY INTERESTED IN FLOW
  83. }
  84. h = new hypothesis(F, new_flow_x, new_flow_y, C_X(0), C_Y(0));
  85. h->jet_est = jet_est;
  86. gsl_matrix_free(T);
  87. gsl_vector_free(x);
  88. gsl_vector_free(y);
  89. gsl_vector_free(w);
  90. gsl_vector_free(c_x);
  91. gsl_vector_free(c_y);
  92. gsl_matrix_free(cov_x);
  93. gsl_matrix_free(cov_y);
  94. }
  95. // store extrapolation details
  96. h->not_extrapolated_length = l;
  97. h->extrapolation_err = distance(*h, ADJ);
  98. return h;
  99. }
  100. hypothesis* hypothesis::new_perturbed(double u_p, double v_p) {
  101. hypothesis* perturbed_h = new hypothesis(F);
  102. perturbed_h->jet_est = jet_est;
  103. perturbed_h->p = p;
  104. perturbed_h->not_extrapolated_length = not_extrapolated_length;
  105. perturbed_h->extrapolation_err = extrapolation_err;
  106. for(int f = 0; f < F; f++) {
  107. float scale = (0.9f / F) * f + 0.1; // linear scaling
  108. if(flow_x[f] > 0)
  109. perturbed_h->flow_x[f] = flow_x[f] + scale * u_p;
  110. else
  111. perturbed_h->flow_x[f] = flow_x[f] - scale * u_p;
  112. if(flow_y[f] > 0)
  113. perturbed_h->flow_y[f] = flow_y[f] + scale * v_p;
  114. else
  115. perturbed_h->flow_y[f] = flow_y[f] - scale * v_p;
  116. }
  117. return perturbed_h;
  118. }
  119. hypothesis& hypothesis::operator =(const hypothesis& h) {
  120. F = h.F;
  121. startF = h.startF;
  122. endF = h.endF;
  123. if(this != &h) {
  124. flow_x = new double[h.F];
  125. flow_y = new double[h.F];
  126. memcpy(flow_x, h.flow_x, sizeof(double) * F);
  127. memcpy(flow_y, h.flow_y, sizeof(double) * F);
  128. }
  129. p = h.p;
  130. energy = h.energy;
  131. return *this;
  132. }
  133. void hypothesis::setOcclusions(const Mat* forward_flow, const Mat* backward_flow, float occlusion_threshold, float occlusion_fb_threshold) {
  134. if(occlusions != NULL) delete[] occlusions;
  135. occlusions = new int[F+1];
  136. occlusions[0] = 0; // always visible in reference frame
  137. for(int t = 0; t < F; t++) {
  138. // we can not detect when something becomes visible again!
  139. if( occlusions[t] == 1) {
  140. occlusions[t+1] = 1;
  141. continue;
  142. }
  143. double u_tm1 = 0;
  144. double v_tm1 = 0;
  145. if (t > 0) {
  146. u_tm1 += flow_x[t-1];
  147. v_tm1 += flow_y[t-1]; // x, y
  148. }
  149. double x_tm1 = p.x + u_tm1;
  150. double y_tm1 = p.y + v_tm1;
  151. if (y_tm1 >= 0 && y_tm1 < forward_flow[t].rows && x_tm1 >= 0 && x_tm1 < forward_flow[t].cols) {
  152. double F_x = bilinearInterp<double>(x_tm1, y_tm1, forward_flow[t], 1);
  153. double F_y = bilinearInterp<double>(x_tm1, y_tm1, forward_flow[t], 0);
  154. double ysq = (flow_y[t] - v_tm1 - F_y);
  155. double xsq = (flow_x[t] - u_tm1 - F_x);
  156. double x_t = p.x + flow_x[t] ;
  157. double y_t = p.y + flow_y[t];
  158. if(y_t >= 0 && y_t < forward_flow[t].rows && x_t >= 0 && x_t < forward_flow[t].cols) {
  159. double bF_x = bilinearInterp<double>(x_t, y_t, backward_flow[t], 1);
  160. double bF_y = bilinearInterp<double>(x_t, y_t, backward_flow[t], 0);
  161. double fb_ysq = (bF_y + F_y);
  162. double fb_xsq = (bF_x + F_x);
  163. if(sqrt(fb_ysq*fb_ysq + fb_xsq*fb_xsq) < occlusion_fb_threshold && sqrt(ysq*ysq + xsq*xsq) < occlusion_threshold)
  164. occlusions[t+1] = 0;
  165. else
  166. occlusions[t+1] = 1;
  167. } else {
  168. occlusions[t+1] = 1;
  169. }
  170. } else {
  171. occlusions[t+1] = 1;
  172. }
  173. }
  174. }
  175. double hypothesis::distance(hypothesis& h, int method) {
  176. int first = max(h.startF, startF);
  177. int length = min((endF - first), (h.endF - first));
  178. Vec2d prev_flow = Vec2d(0,0), prev_flow_h = Vec2d(0,0);
  179. if(first > 0) {
  180. if(h.startF < startF)
  181. prev_flow_h = Vec2d(h.flow_y[first - 1], h.flow_x[first - 1]);
  182. else if(h.startF > startF)
  183. prev_flow = Vec2d(flow_y[first - 1], flow_x[first - 1]);
  184. }
  185. double sum = 0;
  186. // difference of final, accumulated or adjacent flow
  187. if(method == FINAL) {
  188. // final flow
  189. int end = first + length;
  190. Vec2d flow_f = Vec2d(flow_y[end], flow_x[end]) - prev_flow;
  191. Vec2d flow_f_h = Vec2d(h.flow_y[end], h.flow_x[end]) - prev_flow_h;
  192. double ysq = (flow_f[0] - flow_f_h[0]);
  193. double xsq = (flow_f[1] - flow_f_h[1]);
  194. sum += sqrt(xsq*xsq + ysq*ysq);
  195. } else {
  196. int l = 1;
  197. for(int f = first; f < first + length; f++,l++) {
  198. double ysq = 0, xsq = 0;
  199. Vec2d flow_f = Vec2d(flow_y[f], flow_x[f]) - prev_flow;
  200. Vec2d flow_f_h = Vec2d(h.flow_y[f], h.flow_x[f]) - prev_flow_h;
  201. // comparison method
  202. if(method == ACC) {
  203. // accumulated flow
  204. ysq = flow_f[0] - flow_f_h[0];
  205. xsq = flow_f[1] - flow_f_h[1];
  206. sum += sqrt(xsq*xsq + ysq*ysq) / l;
  207. } else if(method == ADJ) {
  208. // adjacent flow
  209. Vec2d flow_fm1 = Vec2d(0,0), flow_fm1_h = Vec2d(0,0);
  210. if(f > first) {
  211. flow_fm1 = Vec2d(flow_y[f - 1], flow_x[f - 1]) - prev_flow;
  212. flow_fm1_h = Vec2d(h.flow_y[f -1], h.flow_x[f -1]) - prev_flow_h;
  213. }
  214. ysq = ((flow_f[0] - flow_fm1[0]) - (flow_f_h[0] - flow_fm1_h[0]));
  215. xsq = ((flow_f[1] - flow_fm1[1]) - (flow_f_h[1] - flow_fm1_h[1]));
  216. sum += sqrt(xsq*xsq + ysq*ysq);
  217. }
  218. }
  219. }
  220. if(method != ACC)
  221. sum = sum / length; // normalize by length
  222. return sum;
  223. }
  224. int hypothesis::compare(const hypothesis& h, double thres, int method) {
  225. int first = max(h.startF, startF);
  226. int length = min((endF - first), (h.endF - first));
  227. // flow before the start
  228. Vec2d prev_flow = Vec2d(0,0), prev_flow_h = Vec2d(0,0);
  229. if(first > 0) {
  230. if(h.startF < startF)
  231. prev_flow_h = Vec2d(h.flow_y[first - 1], h.flow_x[first - 1]);
  232. else if(h.startF > startF)
  233. prev_flow = Vec2d(flow_y[first - 1], flow_x[first - 1]);
  234. }
  235. double dist = 0;
  236. // difference of final, accumulated or adjacent flow
  237. if(method == FINAL) {
  238. // final flow
  239. int end = first + length;
  240. Vec2d flow_f = Vec2d(flow_y[end], flow_x[end]) - prev_flow;
  241. Vec2d flow_f_h = Vec2d(h.flow_y[end], h.flow_x[end]) - prev_flow_h;
  242. double ysq = (flow_f[0] - flow_f_h[0]);
  243. double xsq = (flow_f[1] - flow_f_h[1]);
  244. dist += sqrt(xsq*xsq + ysq*ysq);
  245. } else {
  246. int l = 1;
  247. for(int f = first; f < first + length; f++, l++) {
  248. double ysq = 0, xsq = 0;
  249. Vec2d flow_f = Vec2d(flow_y[f], flow_x[f]) - prev_flow;
  250. Vec2d flow_f_h = Vec2d(h.flow_y[f], h.flow_x[f]) - prev_flow_h;
  251. // comparison method
  252. if(method == ACC) {
  253. // accumulated flow
  254. ysq = flow_f[0] - flow_f_h[0];
  255. xsq = flow_f[1] - flow_f_h[1];
  256. dist += sqrt(xsq*xsq + ysq*ysq) / l;
  257. } else if(method == ADJ) {
  258. // adjacent flow
  259. Vec2d flow_fm1 = Vec2d(0,0), flow_fm1_h = Vec2d(0,0);
  260. if(f > first) {
  261. flow_fm1 = Vec2d(flow_y[f - 1], flow_x[f - 1]) - prev_flow;
  262. flow_fm1_h = Vec2d(h.flow_y[first - 1], h.flow_x[first - 1]) - prev_flow_h;
  263. }
  264. ysq = ((flow_f[0] - flow_fm1[0]) - (flow_f_h[0] - flow_fm1_h[0]));
  265. xsq = ((flow_f[1] - flow_fm1[1]) - (flow_f_h[1] - flow_fm1_h[1]));
  266. dist += sqrt(xsq*xsq + ysq*ysq);
  267. }
  268. }
  269. }
  270. if(method != ACC)
  271. dist /= length; // normalize by length
  272. if(dist > thres) // compare distance to linear threshold
  273. return -2;
  274. else if(not_extrapolated_length < h.not_extrapolated_length) // the other trajectory is longer
  275. return -1;
  276. else if(not_extrapolated_length > h.not_extrapolated_length) // this trajectory is longer
  277. return 1;
  278. else if(extrapolation_err > h.extrapolation_err) // the other has a smaller error
  279. return -1;
  280. else if(extrapolation_err < h.extrapolation_err) // this trajectory has a smaller error
  281. return 1;
  282. else // both trajectories are equal
  283. return 0;
  284. }
  285. int hypothesis::compare(int x, int y, const Mat *acc_cons_flow, int start, int end, double thres, int method) {
  286. int first = max(start, startF);
  287. int length = min((endF - first), (end - first));
  288. Vec2d prev_flow = Vec2d(0,0), prev_flow_h = Vec2d(0,0);
  289. if(first > 0) {
  290. if(start < startF)
  291. prev_flow_h = acc_cons_flow[first - 1].at<Vec2d>(y,x);
  292. else if(start > startF)
  293. prev_flow = Vec2d(flow_y[first - 1], flow_x[first - 1]);
  294. }
  295. double dist = 0;
  296. // difference of final, accumulated or adjacent flow
  297. if(method == FINAL) {
  298. // final flow
  299. int end = first + length;
  300. Vec2d flow_f = Vec2d(flow_y[end], flow_x[end]) - prev_flow;
  301. Vec2d flow_f_h = acc_cons_flow[end].at<Vec2d>(y,x) - prev_flow_h;
  302. double ysq = (flow_f[0] - flow_f_h[0]);
  303. double xsq = (flow_f[1] - flow_f_h[1]);
  304. dist += sqrt(xsq*xsq + ysq*ysq);
  305. } else {
  306. int l = 1;
  307. for(int f = first; f < first + length; f++, l++) {
  308. double ysq = 0, xsq = 0;
  309. Vec2d flow_f = Vec2d(flow_y[f], flow_x[f]) - prev_flow;
  310. Vec2d flow_f_h = acc_cons_flow[f].at<Vec2d>(y,x) - prev_flow_h;
  311. // comparison method
  312. if(method == ACC) {
  313. // accumulated flow
  314. ysq = flow_f[0] - flow_f_h[0];
  315. xsq = flow_f[1] - flow_f_h[1];
  316. dist += sqrt(xsq*xsq + ysq*ysq) / l;
  317. } else if(method == ADJ) {
  318. // adjacent flow
  319. Vec2d flow_fm1 = Vec2d(0,0), flow_fm1_h = Vec2d(0,0);
  320. if(f > first) {
  321. flow_fm1 = Vec2d(flow_y[f - 1], flow_x[f - 1]) - prev_flow;
  322. flow_fm1_h = acc_cons_flow[f - 1].at<Vec2d>(y,x) - prev_flow_h;
  323. }
  324. ysq = ((flow_f[0] - flow_fm1[0]) - (flow_f_h[0] - flow_fm1_h[0]));
  325. xsq = ((flow_f[1] - flow_fm1[1]) - (flow_f_h[1] - flow_fm1_h[1]));
  326. dist += sqrt(xsq*xsq + ysq*ysq);
  327. }
  328. }
  329. }
  330. if(method != ACC)
  331. dist /= length; // normalize by length
  332. int diff = (endF - startF) - (end - start); // difference in length
  333. if(dist > thres) // compare distance to linear threshold
  334. return -2;
  335. else if(diff < 0) // the other trajectory is longer
  336. return -1;
  337. else if(diff > 0) // this trajectory is longer
  338. return 1;
  339. else // both trajectories are equal
  340. return 0;
  341. }