1 files changed, 46 insertions, 62 deletions
diff --git a/meowpp/gra/FeaturePointsDetector_Harris.h b/meowpp/gra/FeaturePointsDetector_Harris.h
index 8f0f8fe..fb3e2be 100644
--- a/meowpp/gra/FeaturePointsDetector_Harris.h
+++ b/meowpp/gra/FeaturePointsDetector_Harris.h
@@ -8,10 +8,12 @@
 #include "FeaturePointsDetector.h"
 
 #include "../dsa/DisjointSet.h"
+#include "../math/utility.h"
 
 #include "../Self.h"
 
 #include <vector>
+#include <algorithm>
 
 namespace meow {
 
@@ -56,9 +58,10 @@ private:
   };
 
   Self<Myself> const self;
-public:
+
   typedef FeaturePoint<double, double> MyFeaturePoint;
   typedef std::vector<MyFeaturePoint>  MyFeaturePoints;
+public:
   //! @brief constructor 使用預設參數
   FPD_Harris(): self() {
   }
@@ -160,6 +163,10 @@ public:
     return paramB();
   }
 
+  size_t descriptionDimension() const {
+    return (squ(self->boundB_ * 2 + 1) - 1) * 2;
+  }
+
   /*! @brief 找出特徵點
    *
    *  @param [in] bmp 要抓特徵點的點陣圖
@@ -172,60 +179,45 @@ public:
     Bitmap<Pixel> input_gx(input.gradianceX(0, self->noiseN_));
     Bitmap<Pixel> input_gy(input.gradianceY(self->noiseN_, 0));
 
-    // get Matrix I for each pixel
-    Bitmap<double> Ixx(input.height(), input.width(), 0.0);
-    Bitmap<double> Iyy(input.height(), input.width(), 0.0);
-    Bitmap<double> Ixy(input.height(), input.width(), 0.0);
-    for (ssize_t y = 0, Y = input.height(); y < Y; y++) {
+    // get Matrix Ixx, Iyy, Ixy for each pixel
+    Bitmap<Vector3D<double> > Ixys(input.height(), input.width(),
+                                   Vector3D<double>(0.0));
+    for (ssize_t y = 0, Y = input.height(); y < Y; y++)
       for (ssize_t x = 0, X = input.width(); x < X; x++) {
         Pixel gx(input_gx(y, x));
         Pixel gy(input_gy(y, x));
-        Ixx.pixel(y, x, gx * gx);
-        Iyy.pixel(y, x, gy * gy);
-        Ixy.pixel(y, x, gx * gy);
+        Ixys.pixel(y, x, Vector3D<double>(gx * gx, gy * gy, gx * gy));
       }
-    }
 
-    // blur
-    Ixx.gaussianed(self->sizeW_, self->sizeW_);
-    Iyy.gaussianed(self->sizeW_, self->sizeW_);
-    Ixy.gaussianed(self->sizeW_, self->sizeW_);
+    // blur for window size
+    Ixys.gaussianed(self->sizeW_, self->sizeW_);
     input_gx.clear();
     input_gy.clear();
 
     // filter too flat or on edge
     Bitmap<double>    R(input.height(), input.width(),   0.0);
     Bitmap<bool>   good(input.height(), input.width(), false);
-    ssize_t b = self->boundB_;
-    for (ssize_t y = b, Y = -b + input.height(); y < Y; y++) {
-      for (ssize_t x = b, X = -b + input.width(); x < X; x++) {
-        double det = Ixx(y, x) * Iyy(y, x) - squ(Ixy(y, x));
-        double tra = Ixx(y, x) + Iyy(y, x);
+    for (ssize_t y = 0, Y = input.height(); y < Y; y++)
+      for (ssize_t x = 0, X = input.width(); x < X; x++) {
+        double det = Ixys(y, x)(0) * Ixys(y, x)(1) - squ(Ixys(y, x)(2));
+        double tra = Ixys(y, x)(0) + Ixys(y, x)(1);
         double r = det - self->ratioK_ * squ(tra);
         R.pixel(y, x, r);
         good.pixel(y, x, (r >= self->thresholdR_));
       }
-    }
-    Ixx.clear();
-    Iyy.clear();
-    Ixy.clear();
+    Ixys.clear();
 
     // find union neighbor
     DisjointSet dsj(input.size());
     ssize_t dy[2] = {0, 1};
     ssize_t dx[2] = {1, 0};
-    for (ssize_t y = b, Y = -b + input.height(); y < Y; y++) {
-      for (ssize_t x = b, X = -b + input.width(); x < X; x++) {
-        if(good.pixel((size_t)y, (size_t)x)){
-          for (size_t k = 0; k < 2u; k++) {
-            if (good.pixel((size_t)(y + dy[k]), (size_t)(x + dx[k]))) {
+    for (ssize_t y = 0, Y = input.height(); y + 1 < Y; y++)
+      for (ssize_t x = 0, X = input.width(); x + 1 < X; x++)
+        if(good.pixel((size_t)y, (size_t)x))
+          for (size_t k = 0; k < 2u; k++)
+            if (good.pixel((size_t)(y + dy[k]), (size_t)(x + dx[k])))
               dsj.merge( y          * input.width() +  x,
                         (y + dy[k]) * input.width() + (x + dx[k]));
-            }
-          }
-        }
-      }
-    }
 
     // find local maximum
     std::vector<size_t> max_i(input.size());
@@ -243,46 +235,38 @@ public:
     // blur before get description
     input.gaussianed(self->featureG_, self->featureG_);
 
+    // Ignore side
+    ssize_t b = std::max<int>(std::max<int>(self->boundB_,
+                                            2 * self->sizeW_),
+                                            2 * self->noiseN_);
     MyFeaturePoints ret;
-    for (ssize_t y = b, Y = -b + input.height(); y < Y; y++) {
+    Vector<double> desc(descriptionDimension(), 0.0); // description
+    for (ssize_t y = b, Y = -b + input.height(); y < Y; y++)
       for (ssize_t x = b, X = -b + input.width(); x < X; x++) {
-        if (!good.pixel((size_t)y, (size_t)x)) {
-          continue;
-        }
-        size_t  i = y * input.width() + x;
-        if (max_i[dsj.root(i)] != i) {
-          continue;
-        }
+        if (!good.pixel((size_t)y, (size_t)x)) continue;
+        size_t i = y * input.width() + x;
+        if (max_i[dsj.root(i)] != i) continue;
         ssize_t dx[4] = {1, 0, -1, 0};
         ssize_t dy[4] = {0, 1, 0, -1};
-        std::vector<double> desc; // description
-        for (ssize_t d = 1; d <= (ssize_t)self->boundB_; d++) {
+        size_t ct = 0;
+        for (ssize_t d = 1; d <= (ssize_t)self->boundB_; ++d) {
           std::vector<double> light;
-          size_t max_id = 0;
-          size_t x0 = x - d, y0 = y - d;
-          for (size_t k = 0; k < 4; k++) {
-            for (ssize_t n = 0;
-                 n < (ssize_t)b * 2;
-                 n++, x0 += dx[k], y0 += dy[k]){
+          size_t max_id = 0, x0 = x - d, y0 = y - d;
+          for (size_t k = 0; k < 4; k++)
+            for (ssize_t n = 0; n < (ssize_t)d * 2; n++,
+                 x0 += dx[k], y0 += dy[k]) {
               Pixel diff = input.pixel(y0, x0) - input.pixel(y, x) * 0.2;
               light.push_back(diff * diff * self->lightL_);
-              if (light[max_id] < light[-1 + light.size()]) {
-                max_id = -1 + (ssize_t)light.size();
-              }
+              if (light[max_id] < light[(ssize_t)light.size() - 1])
+                max_id = (ssize_t)light.size() - 1;
             }
-          }
           for (ssize_t n = 0, N = light.size(); n < N; n++) {
-            desc.push_back((max_id + n) % N);
-            desc.push_back(light[(max_id + n) % N]);
+            desc.scalar(ct++,       (max_id + n) % N );
+            desc.scalar(ct++, light[(max_id + n) % N]);
           }
         }
-        MyFeaturePoint now(2, desc.size());
-        now.position(0, x);
-        now.position(1, y);
-        now.description(Vector<double>(desc));
-        ret.push_back(now);
+        ret.push_back(MyFeaturePoint(Vector2D<double>(x, y).matrix(), desc));
       }
-    }
     return ret;
   }
 
@@ -353,6 +337,6 @@ public:
 # undef FPD_Harris
 };
 
-}
+} // meow
 
 #endif // gra_FeaturePointsDetector_Harris