00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022 #ifndef CVD_VISION_H_
00023 #define CVD_VISION_H_
00024
00025 #include <vector>
00026 #include <memory>
00027 #include <algorithm>
00028
00029 #include <cvd/vision_exceptions.h>
00030 #include <cvd/image.h>
00031 #include <cvd/internal/pixel_operations.h>
00032 #include <cvd/utility.h>
00033
00034
00035 #if defined(CVD_HAVE_TOON)
00036 #include <TooN/TooN.h>
00037 #include <TooN/helpers.h>
00038 #endif
00039
00040 namespace CVD{
00047 template<class C> void twoThirdsSample(const SubImage<C>& in, SubImage<C>& out)
00048 {
00049 typedef typename Pixel::traits<C>::wider_type sum_type;
00050 if( (in.size()/3*2) != out.size())
00051 throw Exceptions::Vision::IncompatibleImageSizes(__FUNCTION__);
00052
00053 for(int yy=0, y=0; y < in.size().y-2; y+=3, yy+=2)
00054 for(int xx=0, x=0; x < in.size().x-2; x+=3, xx+=2)
00055 {
00056
00057
00058
00059
00060 sum_type b = in[y][x+1]*2;
00061 sum_type d = in[y+1][x]*2;
00062 sum_type f = in[y+1][x+2]*2;
00063 sum_type h = in[y+2][x+1]*2;
00064 sum_type e = in[y+1][x+1];
00065
00066 out[yy][xx] = static_cast<C>((in[ y][ x]*4+b+d+e)/9);
00067 out[yy][xx+1] = static_cast<C>((in[ y][x+2]*4+b+f+e)/9);
00068 out[yy+1][xx] = static_cast<C>((in[y+2][ x]*4+h+d+e)/9);
00069 out[yy+1][xx+1] = static_cast<C>((in[y+2][x+2]*4+h+f+e)/9);
00070 }
00071 }
00072
00076 void twoThirdsSample(const SubImage<byte>& in, SubImage<byte>& out);
00077
00078 #ifndef DOXYGEN_IGNORE_INTERNAL
00079 namespace Internal
00080 {
00081 template<class C> class twoThirdsSampler{};
00082 template<class C> struct ImagePromise<twoThirdsSampler<C> >
00083 {
00084 ImagePromise(const SubImage<C>& im)
00085 :i(im)
00086 {}
00087
00088 const SubImage<C>& i;
00089 template<class D> void execute(Image<D>& j)
00090 {
00091 j.resize(i.size()/3*2);
00092 twoThirdsSample(i, j);
00093 }
00094 };
00095 };
00096 template<class C> Internal::ImagePromise<Internal::twoThirdsSampler<C> > twoThirdsSample(const SubImage<C>& c)
00097 {
00098 return Internal::ImagePromise<Internal::twoThirdsSampler<C> >(c);
00099 }
00100 #else
00101
00102
00103
00104
00105
00106
00107
00108 template<class C> Image<C> twoThirdsSample(const SubImage<C>& from);
00109
00110 #endif
00111
00117 template <class T>
00118 void halfSample(const BasicImage<T>& in, BasicImage<T>& out)
00119 {
00120 typedef typename Pixel::traits<T>::wider_type sum_type;
00121 if( (in.size()/2) != out.size())
00122 throw Exceptions::Vision::IncompatibleImageSizes("halfSample");
00123 const T* top = in.data();
00124 const T* bottom = top + in.size().x;
00125 const T* end = top + in.totalsize();
00126 int ow = out.size().x;
00127 int skip = in.size().x + (in.size().x % 2);
00128 T* p = out.data();
00129 while (bottom < end) {
00130 for (int j=0; j<ow; j++) {
00131 *p = static_cast<T>((sum_type(top[0]) + top[1] + bottom[0] + bottom[1])/4);
00132 p++;
00133 top += 2;
00134 bottom += 2;
00135 }
00136 top += skip;
00137 bottom += skip;
00138 }
00139 }
00140
00141 void halfSample(const BasicImage<byte>& in, BasicImage<byte>& out);
00142
00148 template <class T>
00149 inline Image<T> halfSample(const BasicImage<T>& in)
00150 {
00151 Image<T> out(in.size()/2);
00152 halfSample(in, out);
00153 return out;
00154 }
00155
00164 template <class T>
00165 inline Image<T> halfSample( Image<T> in, unsigned int octaves){
00166 for( ;octaves > 0; --octaves){
00167 in = halfSample(in);
00168 }
00169 return in;
00170 }
00171
00177 template <class T>
00178 void threshold(BasicImage<T>& im, const T& minimum, const T& hi)
00179 {
00180 typename BasicImage<T>::iterator it = im.begin();
00181 typename BasicImage<T>::iterator end = im.end();
00182 while (it != end) {
00183 if (*it < minimum)
00184 *it = T();
00185 else
00186 *it = hi;
00187 ++it;
00188 }
00189 }
00190
00197 template <class T>
00198 void stats(const BasicImage<T>& im, T& mean, T& stddev)
00199 {
00200 const int c = Pixel::Component<T>::count;
00201 double v;
00202 double sum[c] = {0};
00203 double sumSq[c] = {0};
00204 const T* p = im.data();
00205 const T* end = im.data()+im.totalsize();
00206 while (p != end) {
00207 for (int k=0; k<c; k++) {
00208 v = Pixel::Component<T>::get(*p, k);
00209 sum[k] += v;
00210 sumSq[k] += v*v;
00211 }
00212 ++p;
00213 }
00214 for (int k=0; k<c; k++) {
00215 double m = sum[k]/im.totalsize();
00216 Pixel::Component<T>::get(mean,k) = (typename Pixel::Component<T>::type)m;
00217 sumSq[k] /= im.totalsize();
00218 Pixel::Component<T>::get(stddev,k) = (typename Pixel::Component<T>::type)sqrt(sumSq[k] - m*m);
00219 }
00220 }
00221
00224 template <class T>
00225 struct multiplyBy
00226 {
00227 const T& factor;
00228 multiplyBy(const T& f) : factor(f) {};
00229 template <class S> inline S operator()(const S& s) const {
00230 return s * factor;
00231 };
00232 };
00233
00234 template <class S, class T, int Sn=Pixel::Component<S>::count, int Tn=Pixel::Component<T>::count> struct Gradient;
00235 template <class S, class T> struct Gradient<S,T,1,2> {
00236 typedef typename Pixel::Component<S>::type SComp;
00237 typedef typename Pixel::Component<T>::type TComp;
00238 typedef typename Pixel::traits<SComp>::wider_type diff_type;
00239 static void gradient(const BasicImage<S>& I, BasicImage<T>& grad) {
00240 int w = I.size().x;
00241 typename BasicImage<S>::const_iterator s = I.begin() + w + 1;
00242 typename BasicImage<S>::const_iterator end = I.end() - w - 1;
00243 typename BasicImage<T>::iterator t = grad.begin() + w + 1;
00244 while (s != end) {
00245 Pixel::Component<T>::get(*t, 0) = Pixel::scalar_convert<TComp,SComp,diff_type>(diff_type(*(s+1)) - *(s-1));
00246 Pixel::Component<T>::get(*t, 1) = Pixel::scalar_convert<TComp,SComp,diff_type>(diff_type(*(s+w)) - *(s-w));
00247 s++;
00248 t++;
00249 }
00250 zeroBorders(grad);
00251 }
00252 };
00253
00260 template <class S, class T> void gradient(const BasicImage<S>& im, BasicImage<T>& out)
00261 {
00262 if( im.size() != out.size())
00263 throw Exceptions::Vision::IncompatibleImageSizes("gradient");
00264 Gradient<S,T>::gradient(im,out);
00265 }
00266
00267
00268 #ifndef DOXYGEN_IGNORE_INTERNAL
00269 void gradient(const BasicImage<byte>& im, BasicImage<short[2]>& out);
00270 #endif
00271
00272
00273 template <class T, class S> inline void sample(const BasicImage<S>& im, double x, double y, T& result)
00274 {
00275 typedef typename Pixel::Component<S>::type SComp;
00276 typedef typename Pixel::Component<T>::type TComp;
00277 int lx = (int)x;
00278 int ly = (int)y;
00279 x -= lx;
00280 y -= ly;
00281 for(unsigned int i = 0; i < Pixel::Component<T>::count; i++){
00282 Pixel::Component<T>::get(result,i) = Pixel::scalar_convert<TComp,SComp>(
00283 (1-y)*((1-x)*Pixel::Component<S>::get(im[ly][lx],i) + x*Pixel::Component<S>::get(im[ly][lx+1], i)) +
00284 y * ((1-x)*Pixel::Component<S>::get(im[ly+1][lx],i) + x*Pixel::Component<S>::get(im[ly+1][lx+1],i)));
00285 }
00286 }
00287
00288 template <class T, class S> inline T sample(const BasicImage<S>& im, double x, double y){
00289 T result;
00290 sample( im, x, y, result);
00291 return result;
00292 }
00293
00294 inline void sample(const BasicImage<float>& im, double x, double y, float& result)
00295 {
00296 int lx = (int)x;
00297 int ly = (int)y;
00298 int w = im.size().x;
00299 const float* base = im[ly]+lx;
00300 float a = base[0];
00301 float b = base[1];
00302 float c = base[w];
00303 float d = base[w+1];
00304 float e = a-b;
00305 x-=lx;
00306 y-=ly;
00307 result = (float)(x*(y*(e-c+d)-e)+y*(c-a)+a);
00308 }
00309
00310 #if defined (CVD_HAVE_TOON)
00311
00322 template <class T, class S>
00323 int transform(const BasicImage<S>& in, BasicImage<T>& out, const TooN::Matrix<2>& M, const TooN::Vector<2>& inOrig, const TooN::Vector<2>& outOrig, const T defaultValue = T())
00324 {
00325 const int w = out.size().x, h = out.size().y, iw = in.size().x, ih = in.size().y;
00326 const TooN::Vector<2> across = M.T()[0];
00327 const TooN::Vector<2> down = M.T()[1];
00328
00329 const TooN::Vector<2> p0 = inOrig - M*outOrig;
00330 const TooN::Vector<2> p1 = p0 + w*across;
00331 const TooN::Vector<2> p2 = p0 + h*down;
00332 const TooN::Vector<2> p3 = p0 + w*across + h*down;
00333
00334
00335
00336
00337
00338 double min_x = p0[0], min_y = p0[1];
00339 double max_x = min_x, max_y = min_y;
00340
00341
00342 if (across[0] < 0)
00343 min_x += w*across[0];
00344 else
00345 max_x += w*across[0];
00346 if (down[0] < 0)
00347 min_x += h*down[0];
00348 else
00349 max_x += h*down[0];
00350 if (across[1] < 0)
00351 min_y += w*across[1];
00352 else
00353 max_y += w*across[1];
00354 if (down[1] < 0)
00355 min_y += h*down[1];
00356 else
00357 max_y += h*down[1];
00358
00359
00360 const TooN::Vector<2> carriage_return = down - w*across;
00361
00362
00363
00364 if (min_x >= 0 && min_y >= 0 && max_x < iw-1 && max_y < ih-1)
00365 {
00366 TooN::Vector<2> p = p0;
00367 for (int i=0; i<h; ++i, p+=carriage_return)
00368 for (int j=0; j<w; ++j, p+=across)
00369 sample(in,p[0],p[1],out[i][j]);
00370 return 0;
00371 }
00372 else
00373 {
00374
00375 const double x_bound = iw-1;
00376 const double y_bound = ih-1;
00377 int count = 0;
00378 TooN::Vector<2> p = p0;
00379 for (int i=0; i<h; ++i, p+=carriage_return) {
00380 for (int j=0; j<w; ++j, p+=across) {
00381
00382 if (0 <= p[0] && 0 <= p[1] && p[0] < x_bound && p[1] < y_bound)
00383 sample(in,p[0],p[1],out[i][j]);
00384 else {
00385 out[i][j] = defaultValue;
00386 ++count;
00387 }
00388 }
00389 }
00390 return count;
00391 }
00392 }
00393
00394 template <class T> void transform(const BasicImage<T>& in, BasicImage<T>& out, const TooN::Matrix<3>& Minv )
00395 {
00396 TooN::Vector<3> base = Minv.T()[2];
00397 TooN::Vector<2> offset;
00398 offset[0] = in.size().x/2;
00399 offset[1] = in.size().y/2;
00400 offset -= TooN::project(base);
00401 TooN::Vector<3> across = Minv.T()[0];
00402 TooN::Vector<3> down = Minv.T()[1];
00403 double w = in.size().x-1;
00404 double h = in.size().y-1;
00405 int ow = out.size().x;
00406 int oh = out.size().y;
00407 base -= down*(oh/2) + across*(ow/2);
00408 for (int row = 0; row < oh; row++, base+=down) {
00409 TooN::Vector<3> x = base;
00410 for (int col = 0; col < ow; col++, x += across) {
00411 TooN::Vector<2> p = project(x) + offset;
00412 if (p[0] >= 0 && p[0] <= w-1 && p[1] >=0 && p[1] <= h-1)
00413 sample(in,p[0],p[1], out[row][col]);
00414 else
00415 zeroPixel(out[row][col]);
00416 }
00417 }
00418 }
00419 #endif
00420
00422 template <class T> void flipVertical( Image<T> & in )
00423 {
00424 int w = in.size().x;
00425 std::auto_ptr<T> buffer_auto(new T[w]);
00426 T* buffer = buffer_auto.get();
00427 T * top = in.data();
00428 T * bottom = top + (in.size().y - 1)*w;
00429 while( top < bottom )
00430 {
00431 std::copy(top, top+w, buffer);
00432 std::copy(bottom, bottom+w, top);
00433 std::copy(buffer, buffer+w, bottom);
00434 top += w;
00435 bottom -= w;
00436 }
00437 }
00438
00440 template <class T> void flipHorizontal( Image<T> & in )
00441 {
00442 int w = in.size().x;
00443 int h = in.size().y;
00444 std::auto_ptr<T> buffer_auto(new T[w]);
00445 T* buffer = buffer_auto.get();
00446 T * left = in.data();
00447 T * right = left + w;
00448 int row = 0;
00449 while(row < h)
00450 {
00451 std::copy(left, right, buffer);
00452 std::reverse_copy(buffer, buffer+w-1, left);
00453 row++;
00454 left += w;
00455 right += w;
00456 }
00457 }
00458
00459
00460 namespace median {
00461 template <class T> inline T median3(T a, T b, T c) {
00462 if (b<a)
00463 return std::max(b,std::min(a,c));
00464 else
00465 return std::max(a,std::min(b,c));
00466 }
00467
00468 template <class T> inline void sort3(T& a, T& b, T& c) {
00469 using std::swap;
00470 if (b<a) swap(a,b);
00471 if (c<b) swap(b,c);
00472 if (b<a) swap(a,b);
00473 }
00474
00475 template <class T> T median_3x3(const T* p, const int w) {
00476 T a = p[-w-1], b = p[-w], c = p[-w+1], d=p[-1], e=p[0], f=p[1], g=p[w-1], h=p[w], i=p[w+1];
00477 sort3(a,b,c);
00478 sort3(d,e,f);
00479 sort3(g,h,i);
00480 e = median3(b,e,h);
00481 g = std::max(std::max(a,d),g);
00482 c = std::min(c,std::min(f,i));
00483 return median3(c,e,g);
00484 }
00485
00486 template <class T> void median_filter_3x3(const T* p, const int w, const int n, T* out)
00487 {
00488 T a = p[-w-1], b = p[-w], d=p[-1], e=p[0], g=p[w-1], h=p[w];
00489 sort3(a,d,g);
00490 sort3(b,e,h);
00491 for (int j=0; j<n; ++j, ++p, ++out) {
00492 T c = p[-w+1], f = p[1], i = p[w+1];
00493 sort3(c,f,i);
00494 *out = median3(std::min(std::min(g,h),i),
00495 median3(d,e,f),
00496 std::max(std::max(a,b),c));
00497 a=b; b=c; d=e; e=f; g=h; h=i;
00498 }
00499 }
00500 }
00501
00502 template <class T> void median_filter_3x3(const SubImage<T>& I, SubImage<T> out)
00503 {
00504 assert(out.size() == I.size());
00505 const int s = I.row_stride();
00506 const int n = I.size().x - 2;
00507 for (int i=1; i+1<I.size().y; ++i)
00508 median::median_filter_3x3(I[i]+1, s, n, out[i]+1);
00509 }
00510
00511 void median_filter_3x3(const SubImage<byte>& I, SubImage<byte> out);
00512
00513
00514
00515
00516 };
00517
00518 #endif // CVD_VISION_H_