optflow() : multiscale version of the image registration algorithm -----------
| 80 | // optflow() : multiscale version of the image registration algorithm |
| 81 | //----------- |
| 82 | CImg<> optflow(const CImg<>& source, const CImg<>& target, |
| 83 | const float smoothness, const float precision, const unsigned int nb_scales, CImgDisplay& disp) { |
| 84 | const unsigned int iteration_max = 100000; |
| 85 | const float _precision = (float)std::pow(10.0,-(double)precision); |
| 86 | const CImg<> |
| 87 | src = source.get_resize(target,3).normalize(0,1), |
| 88 | dest = target.get_normalize(0,1); |
| 89 | CImg<> U; |
| 90 | |
| 91 | const unsigned int _nb_scales = nb_scales>0?nb_scales: |
| 92 | (unsigned int)(2*std::log((double)(std::max(src.width(),src.height())))); |
| 93 | for (int scale = _nb_scales - 1; scale>=0; --scale) { |
| 94 | const float factor = (float)std::pow(1.5,(double)scale); |
| 95 | const unsigned int |
| 96 | _sw = (unsigned int)(src.width()/factor), sw = _sw?_sw:1, |
| 97 | _sh = (unsigned int)(src.height()/factor), sh = _sh?_sh:1; |
| 98 | const CImg<> |
| 99 | I1 = src.get_resize(sw,sh,1,-100,2), |
| 100 | I2 = dest.get_resize(I1,2); |
| 101 | std::fprintf(stderr," * Scale %d\n",scale); |
| 102 | if (U) (U*=1.5f).resize(I2.width(),I2.height(),1,-100,3); |
| 103 | else U.assign(I2.width(),I2.height(),1,2,0); |
| 104 | |
| 105 | float dt = 2, energy = cimg::type<float>::max(); |
| 106 | const CImgList<> dI = I2.get_gradient(); |
| 107 | |
| 108 | for (unsigned int iteration = 0; iteration<iteration_max; ++iteration) { |
| 109 | std::fprintf(stderr,"\r- Iteration %d - E = %g",iteration,energy); std::fflush(stderr); |
| 110 | float _energy = 0; |
| 111 | cimg_for3XY(U,x,y) { |
| 112 | const float |
| 113 | X = x + U(x,y,0), |
| 114 | Y = y + U(x,y,1); |
| 115 | |
| 116 | float deltaI = 0; |
| 117 | cimg_forC(I2,c) deltaI+=(float)(I1(x,y,c) - I2.linear_atXY(X,Y,c)); |
| 118 | |
| 119 | float _energy_regul = 0; |
| 120 | cimg_forC(U,c) { |
| 121 | const float |
| 122 | Ux = 0.5f*(U(_n1x,y,c) - U(_p1x,y,c)), |
| 123 | Uy = 0.5f*(U(x,_n1y,c) - U(x,_p1y,c)), |
| 124 | Uxx = U(_n1x,y,c) + U(_p1x,y,c), |
| 125 | Uyy = U(x,_n1y,c) + U(x,_p1y,c); |
| 126 | U(x,y,c) = (float)( U(x,y,c) + dt*(deltaI*dI[c].linear_atXY(X,Y) + |
| 127 | smoothness* ( Uxx + Uyy )))/(1 + 4*smoothness*dt); |
| 128 | _energy_regul+=Ux*Ux + Uy*Uy; |
| 129 | } |
| 130 | _energy+=deltaI*deltaI + smoothness*_energy_regul; |
| 131 | } |
| 132 | const float d_energy = (_energy - energy)/(sw*sh); |
| 133 | if (d_energy<=0 && -d_energy<_precision) break; |
| 134 | if (d_energy>0) dt*=0.5f; |
| 135 | energy = _energy; |
| 136 | if (disp) disp.resize(); |
| 137 | if (disp && disp.is_closed()) std::exit(0); |
| 138 | if (disp && !(iteration%300)) { |
| 139 | const unsigned char white[] = { 255,255,255 }; |