oriented_gradient.cpp
Go to the documentation of this file.
00001 // @brief calc oriented gradent
00002 // @author Hiroaki Yaguchi, JSK
00003 #include "jsk_perception/oriented_gradient.hpp"
00004 #include <jsk_topic_tools/log_utils.h>
00005 
00006 namespace jsk_perception {
00007 
00008 // @brief calc 8 neighbor oriented gradient image
00009 // @param src source image
00010 // @param dst destination image (HSV, H : orientation, V : intensity)
00011 void calcOrientedGradient(cv::Mat& src, cv::Mat& dst) {
00012   int width, height;
00013   cv::Mat gimg;
00014 
00015   width = src.cols;
00016   height = src.rows;
00017 
00018   cv::cvtColor(src, gimg, CV_BGR2GRAY);
00019 
00020   dst.create(height, width, CV_8UC3);
00021   for (int y = 0; y < height; y++) {
00022     for (int x = 0; x < width; x++) {
00023       cv::Vec3b& px=dst.at<cv::Vec3b>(y, x);
00024       for (int k = 0; k < 3; k++) {
00025         px[k] = 0;
00026       }
00027     }
00028   }
00029 
00030   float r2 = sqrt(2.0);
00031 
00032   for (int y = 1; y < height - 1; y++) {
00033     for (int x = 1; x < width - 1; x++) {
00034       int m,th;
00035       float dx,dy;
00036       float dxx,dyy,dxy,dyx;
00037       double dth;
00038 
00039       dxx = (float)gimg.at<unsigned char>(y, x + 1)
00040         - (float)gimg.at<unsigned char>(y, x - 1);
00041       dyy = (float)gimg.at<unsigned char>(y + 1, x)
00042         - (float)gimg.at<unsigned char>(y - 1, x);
00043       dxy = (float)gimg.at<unsigned char>(y + 1, x + 1)
00044         - (float)gimg.at<unsigned char>(y - 1, x - 1);
00045       dyx = (float)gimg.at<unsigned char>(y + 1, x - 1)
00046         - (float)gimg.at<unsigned char>(y - 1, x + 1);
00047 
00048       dx = 0.5 * (dxx + 0.5 * r2 * (dxy - dyx));
00049       dy = 0.5 * (dyy + 0.5 * r2 * (dxy + dyx));
00050       // dx = dxx; dy = dyy;
00051 
00052       m = (int)(sqrt(0.5 * (dx * dx + dy * dy)));
00053 
00054       dth = atan2(dy, dx) * 180.0 / M_PI;
00055       if (dth < 0.0) dth += 180.0;
00056       if (dth >= 180.0) dth -= 180.0;
00057       th = (int)dth;
00058 
00059       dst.at<cv::Vec3b>(y, x) = cv::Vec3b(th, 255, m);
00060 
00061     }
00062   }
00063 }
00064 
00065 // @brief calc key points from oriented gradient image
00066 // @param src source image
00067 // @param dst destination image (HSV, H : orientation, V : intensity)
00068 // @param result key points
00069 // @param thres (optional) threshold of intensity
00070 // @param bs (optional) block size
00071 void calcOGKeyPoints(cv::Mat& src,
00072                      cv::Mat& dst,
00073                      std::vector<cv::Point>& result,
00074                      int thres,
00075                      int bs) {
00076   calcOrientedGradient(src, dst);
00077   int width, height;
00078   width = src.cols;
00079   height = src.rows;
00080 
00081   result.clear();
00082 
00083   for (int y = bs; y < height - bs; y++) {
00084     for (int x = bs; x < width - bs; x++) {
00085       cv::Vec3b& px0 = dst.at<cv::Vec3b>(y, x);
00086       if (px0[2] > thres) {
00087         bool iskey = true;
00088         for (int dx = -bs; dx <= bs; dx++) {
00089           for (int dy = -bs; dy <= bs; dy++) {
00090             if (dx == 0 && dy == 0) break;
00091             cv::Vec3b& px1 = dst.at<cv::Vec3b>(y + dy, x + dx);
00092             if (px0[2] < px1[2]) iskey = false;
00093           }
00094         }
00095         if (iskey) result.push_back(cv::Point(x, y));
00096       }
00097 
00098     }
00099   }
00100 
00101 }
00102 
00103 // @brief calc 8 neighbor scaled oriented gradient image
00104 // @param src source image
00105 // @param dst destination image (HSV, H : orientation, V : intensity)
00106 // @param scale scale
00107 void calcScaledOrientedGradient(cv::Mat& src, cv::Mat& dst, int scale) {
00108   cv::Mat gimg;
00109   cv::Mat intimg, sqintimg, tintimg;
00110 
00111   int width = src.cols;
00112   int height = src.rows;
00113 
00114   cv::cvtColor(src, gimg, CV_BGR2GRAY);
00115 
00116   dst.create(height, width, CV_8UC3);
00117   unsigned char *gradbuf = dst.ptr();
00118 
00119   intimg.create(height + 1, width + 1, CV_32S);
00120   sqintimg.create(height + 1, width + 1, CV_32S);
00121   tintimg.create(height + 1, width + 1, CV_32S);
00122 
00123   cv::integral(gimg, intimg, sqintimg, tintimg);
00124 
00125   float r2 = sqrt(2.0);
00126   int xidx, idx;
00127   int hscale = scale / 2;
00128   int bsize = hscale * 2 + 1;
00129   int barea0 = scale * scale;
00130   int barea1 = scale * bsize;
00131   int margin = scale;
00132   for (int y = margin; y < height - margin; y++) {
00133     xidx = y * width;
00134     for (int x = margin; x < width - margin; x++) {
00135       int m, th;
00136       float dx, dy;
00137       float dxx, dyy, dxy, dyx;
00138       double dth;
00139 
00140       dxx = ((float)(intimg.at<int>(y + hscale + 1, x + scale + 1)
00141                      + intimg.at<int>(y - hscale, x + 1)
00142                      - intimg.at<int>(y - hscale, x + scale + 1)
00143                      - intimg.at<int>(y + hscale + 1, x + 1))
00144              - (float)(intimg.at<int>(y + hscale + 1, x)
00145                        +intimg.at<int>(y - hscale, x - scale)
00146                        -intimg.at<int>(y - hscale, x)
00147                        -intimg.at<int>(y + hscale + 1, x - scale))) / barea1;
00148       dyy = ((float)(intimg.at<int>(y + scale + 1, x + hscale + 1)
00149                      + intimg.at<int>(y + 1, x - hscale)
00150                      - intimg.at<int>(y + scale + 1, x - hscale)
00151                      - intimg.at<int>(y + 1, x + hscale + 1))
00152              - (float)(intimg.at<int>(y - scale, x - hscale)
00153                        +intimg.at<int>(y, x + hscale + 1)
00154                        -intimg.at<int>(y - scale, x + hscale + 1)
00155                        -intimg.at<int>(y, x - hscale))) / barea1;
00156 
00157       dxy = ((float)(intimg.at<int>(y + scale + 1, x + scale + 1)
00158                      + intimg.at<int>(y + 1, x + 1)
00159                      - intimg.at<int>(y + 1, x + scale + 1)
00160                      - intimg.at<int>(y + scale + 1, x + 1))
00161              - (float)(intimg.at<int>(y, x)
00162                        + intimg.at<int>(y - scale, x - scale)
00163                        - intimg.at<int>(y, x - scale)
00164                        - intimg.at<int>(y - scale, x))) / barea0;
00165       dyx = ((float)(intimg.at<int>(y + scale + 1, x)
00166                      + intimg.at<int>(y + 1, x - scale)
00167                      - intimg.at<int>(y + 1, x)
00168                      - intimg.at<int>(y + scale + 1, x - scale))
00169              - (float)(intimg.at<int>(y, x + scale + 1)
00170                        + intimg.at<int>(y - scale, x + 1)
00171                        - intimg.at<int>(y - scale, x + scale + 1)
00172                        - intimg.at<int>(y, x + 1))) / barea0;
00173 
00174       dx = 0.5 * (dxx + 0.5 * r2 * (dxy - dyx));
00175       dy = 0.5 * (dyy + 0.5 * r2 * (dxy + dyx));
00176 
00177       m = (int)(sqrt(0.5 * (dx * dx + dy * dy)));
00178 
00179       dth = atan2(dy, dx) * 180.0 / M_PI;
00180       if (dth < 0.0) dth += 180.0;
00181       if (dth >= 180.0) dth -= 180.0;
00182       th = (int)dth;
00183 
00184       gradbuf[(xidx + x) * 3] = th;
00185       gradbuf[(xidx + x) * 3 + 1] = 255;
00186       gradbuf[(xidx + x) * 3 + 2] = m;
00187     }
00188   }
00189 }
00190 
00191 // @brief calc key points from scaled oriented gradient image
00192 // @param src source image
00193 // @param dst destination image (HSV, H : orientation, V : intensity)
00194 void calcSOGKeyPoints(cv::Mat& src, cv::Mat& dst) {
00195   cv::Mat gimg;
00196   cv::Mat intimg, sqintimg, tintimg;
00197 
00198   int width = src.cols;
00199   int height = src.rows;
00200 
00201   std::vector<std::vector<float> > gradimglist;
00202   std::vector<int> scalelist;
00203 
00204   cv::cvtColor(src, gimg, CV_BGR2GRAY);
00205 
00206   dst.create(height, width, CV_8UC3);
00207   unsigned char *gradbuf = dst.ptr();
00208 
00209   intimg.create(height + 1, width + 1, CV_32S);
00210   sqintimg.create(height + 1, width + 1, CV_32S);
00211   tintimg.create(height + 1, width + 1, CV_32S);
00212 
00213   cv::integral(gimg, intimg, sqintimg, tintimg);
00214 
00215   // scale=1,1+1*2,1+2*2,...,1+s*2
00216   int maxscale = 10;
00217   scalelist.resize(width * height);
00218 
00219   // gradimglist = g(0) gx(0), gy(0), ...
00220   gradimglist.resize((maxscale + 1) * 3);
00221 
00222   for(int i = 0; i < gradimglist.size(); i++)
00223     gradimglist[i].resize(width * height);
00224 
00225   // s=1
00226   float r2 = sqrt(2.0);
00227   int xidx, idx;
00228   for (int y = 1; y < height - 1; y++) {
00229     xidx = y * width;
00230     for (int x = 1; x < width - 1; x++) {
00231       float dx, dy;
00232       float dxx, dyy, dxy, dyx;
00233 
00234       dxx = (float)gimg.at<unsigned char>(y, x + 1)
00235         - (float)gimg.at<unsigned char>(y, x - 1);
00236       dyy = (float)gimg.at<unsigned char>(y + 1, x)
00237         - (float)gimg.at<unsigned char>(y - 1, x);
00238       dxy = (float)gimg.at<unsigned char>(y + 1, x + 1)
00239         - (float)gimg.at<unsigned char>(y - 1, x - 1);
00240       dyx = (float)gimg.at<unsigned char>(y + 1, x - 1)
00241         - (float)gimg.at<unsigned char>(y - 1, x + 1);
00242 
00243       dx = 0.5 * (dxx + 0.5 * r2 * (dxy - dyx));
00244       dy = 0.5 * (dyy + 0.5 * r2 * (dxy + dyx));
00245 
00246       idx = xidx + x;
00247       gradimglist[0][idx] = dx * dx + dy * dy;
00248       gradimglist[1][idx] = dx;
00249       gradimglist[2][idx] = dy;
00250     }
00251   }
00252 
00253   // s >= 2
00254   for (int scale = 2; scale <= maxscale; scale++) {
00255 
00256     int hscale = scale / 2;
00257     int bsize = hscale * 2 + 1;
00258     int barea0 = scale * scale;
00259     int barea1 = scale * bsize;
00260     int margin = scale;
00261 
00262     int sidx = (scale - 1) * 3;
00263     for (int y = margin; y < height - margin; y++) {
00264       xidx = y * width;
00265       for (int x = margin; x < width - margin; x++) {
00266         float dx, dy;
00267         float dxx, dyy, dxy, dyx;
00268 
00269         dxx = ((float)(intimg.at<int>(y + hscale + 1, x + scale + 1)
00270                        + intimg.at<int>(y - hscale, x + 1)
00271                        - intimg.at<int>(y - hscale, x + scale + 1)
00272                        - intimg.at<int>(y + hscale + 1, x + 1))
00273                - (float)(intimg.at<int>(y + hscale + 1, x)
00274                          + intimg.at<int>(y - hscale, x - scale)
00275                          - intimg.at<int>(y - hscale, x)
00276                          - intimg.at<int>(y + hscale + 1, x - scale)))
00277             / barea1;
00278         dyy = ((float)(intimg.at<int>(y + scale + 1, x + hscale + 1)
00279                        + intimg.at<int>(y + 1, x - hscale)
00280                        - intimg.at<int>(y + scale + 1, x - hscale)
00281                        - intimg.at<int>(y + 1, x + hscale + 1))
00282                -(float)(intimg.at<int>(y - scale, x - hscale)
00283                         +intimg.at<int>(y, x + hscale + 1)
00284                         -intimg.at<int>(y - scale, x + hscale + 1)
00285                         -intimg.at<int>(y, x - hscale))) / barea1;
00286 
00287 
00288         dxy = ((float)(intimg.at<int>(y + scale + 1, x + scale + 1)
00289                        + intimg.at<int>(y + 1, x + 1)
00290                        - intimg.at<int>(y + 1, x + scale + 1)
00291                        - intimg.at<int>(y + scale + 1, x + 1))
00292                - (float)(intimg.at<int>(y, x)
00293                          + intimg.at<int>(y - scale, x - scale)
00294                          - intimg.at<int>(y, x - scale)
00295                          - intimg.at<int>(y - scale, x))) / barea0;
00296         dyx = ((float)(intimg.at<int>(y + scale + 1, x)
00297                        + intimg.at<int>(y + 1, x - scale)
00298                        - intimg.at<int>(y + 1, x)
00299                        - intimg.at<int>(y + scale + 1, x - scale))
00300                - (float)(intimg.at<int>(y, x + scale + 1)
00301                          + intimg.at<int>(y - scale, x + 1)
00302                          - intimg.at<int>(y - scale, x + scale + 1)
00303                          - intimg.at<int>(y, x + 1))) / barea0;
00304 
00305         dx = 0.5 * (dxx + 0.5 * r2 * (dxy - dyx));
00306         dy = 0.5 * (dyy + 0.5 * r2 * (dxy + dyx));
00307 
00308         idx = xidx + x;
00309         gradimglist[sidx][idx] = dx * dx + dy * dy;
00310         gradimglist[sidx + 1][idx] = dx;
00311         gradimglist[sidx + 2][idx] = dy;
00312       }
00313     }
00314   }
00315 
00316   // scale estimation
00317   int ofs = 1 + maxscale;
00318   for (int y = ofs; y < height - ofs; y++) {
00319     xidx = y * width;
00320     for (int x = ofs; x < width - ofs; x++) {
00321       idx = xidx + x;
00322       float m, mmax, th;
00323       int sscale = 0;
00324       int mscale = sscale;
00325       float dx, dy;
00326       mmax = gradimglist[sscale][idx];
00327       for (int scale = sscale + 1; scale <= maxscale; scale++) {
00328         m = gradimglist[scale * 3][idx];
00329         if (mmax < m) {
00330           mmax = m;
00331           mscale = scale;
00332         } else {
00333           break;
00334         }
00335       }
00336       m = sqrt(mmax);
00337       dx = gradimglist[mscale * 3 + 1][idx];
00338       dy = gradimglist[mscale * 3 + 2][idx];
00339       th = atan2(dy, dx) * 180.0 / M_PI;
00340       if (th < 0.0) th += 180.0;
00341       if (th >= 180.0) th -= 180.0;
00342       scalelist[idx] = mscale;
00343 
00344       gradbuf[idx * 3] = (unsigned char)th;
00345       gradbuf[idx * 3 + 1] = 255;
00346       gradbuf[idx * 3 + 2] = (unsigned char)m;
00347     }
00348   }
00349 
00350   // keypoint localization
00351   for (int y = ofs; y < height - ofs; y++) {
00352     xidx = y * width;
00353     for (int x = ofs; x < width - ofs; x++) {
00354       idx = xidx + x;
00355       int scale = scalelist[idx];
00356       float m = gradimglist[scale][idx];
00357       bool iskey = false;
00358       if (sqrt(m) > 32) {
00359         iskey = true;
00360         for (int sc = scale > 0 ? scale - 1 : 0;
00361              sc < (scale < maxscale ? scale + 1 : maxscale);
00362              sc++) {
00363           for (int xx = -1; xx <= 1; xx++) {
00364             if (m < gradimglist[sc][idx + xx]
00365                 || m < gradimglist[sc][idx + xx + width]
00366                 || m < gradimglist[sc][idx + xx - width]) {
00367               iskey = false;
00368               break;
00369             }
00370           }
00371         }
00372       }
00373 
00374       if (iskey) {
00375         cv::circle(src,
00376                    cv::Point(x, y),
00377                    scale + 1,
00378                    cv::Scalar(0, 0, 255),
00379                    1,
00380                    4);
00381       } else {
00382         // gradbuf[idx*3]=0;
00383         // gradbuf[idx*3+1]=0;
00384         // gradbuf[idx*3+2]=0;
00385       }
00386     }
00387   }
00388 }
00389 
00390 
00391 }  // namespace


jsk_perception
Author(s): Manabu Saito, Ryohei Ueda
autogenerated on Tue Jul 2 2019 19:41:07