in the middle of non-linear pixels debuggingconflicting_master

1 files changed, 67 insertions, 69 deletions

diff --git a/src/image.cpp b/src/image.cpp
index 636ec78..a111d40 100644
--- a/src/image.cpp
+++ b/src/image.cpp
@@ -6,6 +6,7 @@
 
 #include <libcamera/formats.h>
 
+#include "image.h"
 #include "macro.h"
 #include "pixels.h"
 
@@ -20,15 +21,13 @@ uint64_t rot_elapsed_ns = 0;
 uint64_t pix_elapsed_ns = 0;
 uint64_t dropped_count = 0;
 
+float process_column(const Image::column_t &column);
+float process_column_center_of_mass(const Image::column_t &column);
+
 // float process_column(const uint8_t (&column)[])
 // float process_column(const Image::row_t &column)
 float process_column(const Image::column_t &column)
 {
-    // Image::column_t c = column;
-    // start_timer(process_column);
-    // QElapsedTimer t;
-    // t.start();
-
     float result = std::numeric_limits<float>::quiet_NaN();
 
     // constexpr uint32_t signalThreshold = 900; // = SKO * sqrt(patternSize)
@@ -47,40 +46,6 @@ float process_column(const Image::column_t &column)
 
     static_assert((img_height % patternSize) == 0, "img_height % patternSize should be 0");
     // std::array<uint16_t, img_height / patternSize> sums;
-    // uint16_t maxLALASum{0};
-    // size_t maxLALAIdx{0};
-    // uint16_t sum{0};
-
-    // for (size_t i = 0; i < img_height; i += patternSize) {
-    //     // const auto sum = std::accumulate(column.cbegin() + i,
-    //     //                                  column.cbegin() + i + patternSize,
-    //     //                                  0,
-    //     //                                  std::plus<uint16_t>());
-    //     uint16_t sum{0};
-
-    //     for (size_t j = i; j < i + patternSize; ++j)
-    //         sum += column[j];
-    //     // if ((i % patternSize) == 0)
-    //     //     sum = 0;
-    //     // sum += column[i];
-
-    //     // if (sum > 0xff)
-    //     // std::cout << sum << ' ';
-
-    //     if (sum > maxLALASum) {
-    //         maxLALASum = sum;
-    //         maxLALAIdx = i;
-    //     }
-    // }
-
-    // maxLALAIdx = patternSize * 32 - 14; // crash
-    // maxLALAIdx = patternSize * 32 - 14; // no crash
-
-    // maxLALAIdx = std::clamp(uint32_t(maxLALAIdx),
-    //                         uint32_t(patternSize),
-    //                         uint32_t(img_height - patternSize * 2 - 14));
-
-    // std::cout << maxLALAIdx << ' ';
     // memset(correlation, 0, img_height * sizeof(correlation[0]));
     // memset(correlation, 0, patternSize);
     // memset(correlation + correlationSize - 1, 0, patternSize);
@@ -93,29 +58,17 @@ float process_column(const Image::column_t &column)
         integralSum[i] = column[i] + integralSum[i - 1];
     }
 
-    // sum_elapsed_ns += t.nsecsElapsed();
-    // t.restart();
-
     // pixel * <sum of neighbours>
     for (uint32_t i = 0; i < correlationSize; ++i)
         // for (uint32_t i = maxLALAIdx; i < maxLALAIdx + patternSize * 2; ++i)
         correlation[i + patternSize / 2] = column[i + patternSize / 2]
                                            * (integralSum[i + patternOffset] - integralSum[i]);
-    // * (integralSum[i + patternSize] - integralSum[i]);
-
-    // corr_elapsed_ns += t.nsecsElapsed();
-    // t.restart();
 
     uint32_t cPPP = correlation[0];
     uint32_t cPP = correlation[1];
     uint32_t cP = correlation[2];
 
-    // uint32_t c = correlation[3];
-    // uint32_t cN = correlation[4];
-    // uint32_t cNN = correlation[5];
-
     for (uint32_t i = 3; i < img_height - 2; ++i) {
-        // for (uint32_t i = maxLALAIdx + 3; i < maxLALAIdx + patternSize * 3 - 2; ++i) {
         // p - pixel, n - neighbour
         // P - pixel used in sum, N - neighbour used in sum
         // [N P N]
@@ -124,16 +77,6 @@ float process_column(const Image::column_t &column)
         const uint32_t cNN = correlation[i + 2];
         const auto sum = cP + c + cN;
 
-        // const int32_t rioux0 = int32_t(cPPP + cPP) - int32_t(c + cN);
-        // const int32_t rioux1 = int32_t(cPP + cP) - int32_t(cN + cNN);
-
-        // if (sum > maxTripleSum && rioux0 < 0 && rioux1 >= 0) {
-        //     x1 = i - 1;
-        //     y1 = rioux0;
-        //     y2 = rioux1;
-        //     maxTripleSum = sum;
-        // }
-
         if (sum > maxTripleSum) {
             // [N N n p] - [P N]
             const int32_t rioux0 = int32_t(cPPP + cPP) - int32_t(c + cN);
@@ -154,20 +97,71 @@ float process_column(const Image::column_t &column)
         cPPP = cPP;
         cPP = cP;
         cP = c;
-        // c = cN;
-        // cN = cNN;
-        // cNN = correlation[i + 1];
     }
 
-    // value_elapsed_ns += t.nsecsElapsed();
-    // t.restart();
-
     result = (y2 != y1) ? (float(x1) - (float(y1) / (y2 - y1)))
                         : std::numeric_limits<float>::quiet_NaN();
 
     return result;
 }
 
+float process_column_center_of_mass(const Image::column_t &column)
+{
+    static_assert((img_height % patternSize) == 0, "img_height % patternSize should be 0");
+
+    // const auto &c = column;
+    Image::column_t c = column;
+    constexpr size_t win_size = 2;
+    constexpr size_t wins_count{img_height / win_size};
+    using pixel_sum_t = uint16_t;
+    // using img_pixel_t = decltype(Image::row_t)::value_type;
+    using img_pixel_t = uint8_t;
+    static_assert(std::numeric_limits<pixel_sum_t>::max()
+                      > std::numeric_limits<img_pixel_t>::max() * win_size,
+                  "choose bigger type for pixels sum");
+    // std::array<pixel_sum_t, wins_count> sums;
+    pixel_sum_t maxSum{0};
+    size_t maxWinI{0};
+    constexpr img_pixel_t blackLevel{5};
+
+    // std::for_each(c.begin(), c.end(), [](auto &v) {
+    //     if (v > 20) {
+    //         v = 0;
+    //     }
+    // });
+
+    for (size_t w{0}; w < wins_count; ++w) {
+        // option 0: manually sum
+        const auto i = w * win_size;
+        // sums[w] = c[i + 0] * 1 + c[i + 1] * 2 + c[i + 2] * 3 + c[i + 3] * 4 + c[i + 4] * 5
+        //           + c[i + 5] * 6 + c[i + 6] * 7 + c[i + 7] * 8;
+        // const auto sum{c[i + 0] + c[i + 1] + c[i + 2] + c[i + 3] + c[i + 4] + c[i + 5] + c[i + 6]
+        //                + c[i + 7]};
+        const auto sum{c[i + 0] + c[i + 1] + c[i + 2] + c[i + 3]};
+
+        if (sum > maxSum) {
+            maxSum = sum;
+            maxWinI = w;
+        }
+    }
+
+    const auto start = (maxWinI - 1) * win_size;
+    const auto com_size = win_size;
+    float com_sum{0};
+    float com_sum_mass{0};
+    constexpr uint8_t black_level{27};
+    // std::array<uint8_t, win_size * 3>
+
+    for (size_t i{start}; i < start + (win_size * 3); ++i) {
+        const auto value = c[i] > black_level ? c[i] : 0;
+        com_sum += value;
+        com_sum_mass += value * (i);
+    }
+
+    // return com_sum / (win_size * 3);
+    return com_sum_mass / com_sum;
+}
+
 // uint8_t &Image::dataAt(size_t row, size_t col)
 // {
 //     const auto index = img_width * row + col;
@@ -206,12 +200,16 @@ std::shared_ptr<Pixels> Image::sharedPixels()
     result->counters = counters;
 
     std::transform(rotated_cw.cbegin(), rotated_cw.cend(), result->pixels.begin(), process_column);
+    // std::transform(rotated_cw.cbegin(),
+    //                rotated_cw.cend(),
+    //                result->pixels.begin(),
+    // process_column_center_of_mass);
 
     static bool found{false};
 
     for (const auto &p : result->pixels) {
-        if (p > 400) {
-            std::cout << "AAAAAAAAAAAAAAAAAAAAA too big pixel values: " << p << std::endl;
+        if (p > img_height) {
+            // std::cout << "AAAAAAAAAAAAAAAAAAAAA too big pixel values: " << p << std::endl;
             found = true;
         }
     }