| 266 | // http://research.microsoft.com/en-us/um/people/zhang/INRIA/Publis/Tutorial-Estim/node25.html |
| 267 | template<typename T> |
| 268 | int findBestHomography(Array<T>& bestH, const Array<float>& x_src, |
| 269 | const Array<float>& y_src, const Array<float>& x_dst, |
| 270 | const Array<float>& y_dst, const Array<float>& rnd, |
| 271 | const unsigned iterations, const unsigned nsamples, |
| 272 | const float inlier_thr, const af_homography_type htype) { |
| 273 | const float* x_src_ptr = x_src.get(); |
| 274 | const float* y_src_ptr = y_src.get(); |
| 275 | const float* x_dst_ptr = x_dst.get(); |
| 276 | const float* y_dst_ptr = y_dst.get(); |
| 277 | |
| 278 | Array<T> H = |
| 279 | createValueArray<T>(af::dim4(9, iterations), static_cast<T>(0)); |
| 280 | H.eval(); |
| 281 | getQueue().sync(); |
| 282 | |
| 283 | const af::dim4& rdims = rnd.dims(); |
| 284 | const af::dim4& Hdims = H.dims(); |
| 285 | |
| 286 | unsigned iter = iterations; |
| 287 | unsigned bestIdx = 0; |
| 288 | int bestInliers = 0; |
| 289 | float minMedian = numeric_limits<float>::max(); |
| 290 | |
| 291 | for (unsigned i = 0; i < iter; i++) { |
| 292 | const unsigned Hidx = Hdims[0] * i; |
| 293 | T* H_ptr = H.get() + Hidx; |
| 294 | |
| 295 | const unsigned ridx = rdims[0] * i; |
| 296 | const float* rnd_ptr = rnd.get() + ridx; |
| 297 | |
| 298 | if (computeHomography<T>(H_ptr, rnd_ptr, x_src_ptr, y_src_ptr, |
| 299 | x_dst_ptr, y_dst_ptr)) { |
| 300 | continue; |
| 301 | } |
| 302 | |
| 303 | if (htype == AF_HOMOGRAPHY_RANSAC) { |
| 304 | int inliers_count = 0; |
| 305 | for (unsigned j = 0; j < nsamples; j++) { |
| 306 | float z = H_ptr[6] * x_src_ptr[j] + H_ptr[7] * y_src_ptr[j] + |
| 307 | H_ptr[8]; |
| 308 | float x = (H_ptr[0] * x_src_ptr[j] + H_ptr[1] * y_src_ptr[j] + |
| 309 | H_ptr[2]) / |
| 310 | z; |
| 311 | float y = (H_ptr[3] * x_src_ptr[j] + H_ptr[4] * y_src_ptr[j] + |
| 312 | H_ptr[5]) / |
| 313 | z; |
| 314 | |
| 315 | float dist = sq(x_dst_ptr[j] - x) + sq(y_dst_ptr[j] - y); |
| 316 | if (dist < (inlier_thr * inlier_thr)) { inliers_count++; } |
| 317 | } |
| 318 | iter = |
| 319 | updateIterations(static_cast<float>(nsamples - inliers_count) / |
| 320 | static_cast<float>(nsamples), |
| 321 | iter); |
| 322 | if (inliers_count > bestInliers) { |
| 323 | bestIdx = i; |
| 324 | bestInliers = inliers_count; |
| 325 | } |