isodraw count based image (#176)

robotology · Sep 25, 2023 · f3946ed · f3946ed
1 parent 6ca3069
commit f3946ed
Show file tree

Hide file tree

Showing 2 changed files with 44 additions and 28 deletions.
diff --git a/cpp_tools/vFramer/drawers.cpp b/cpp_tools/vFramer/drawers.cpp
@@ -155,7 +155,7 @@ double isoDrawer::updateImage()
     int step = inf.count / max_events_to_draw; // a maximum of events to draw
 
     canvas.setTo(white);
-    iso_drawer.time_draw< window<AE>::iterator >(canvas, input.begin(), input.end(), 0);
+    iso_drawer.time_draw< window<AE>::iterator >(canvas, input.begin(), input.end(), inf.count);
 
     return inf.timestamp;
 }

diff --git a/ev2/event-driven/vis/draw.h b/ev2/event-driven/vis/draw.h
@@ -117,7 +117,7 @@ class isoImager
     }
 
     template <typename T>
-    void time_draw(cv::Mat img, T begin, T end, int step = 1) {
+    void time_draw(cv::Mat img, T begin, T end, int count) {
 
         //if there is nothing to draw, just draw the frame
         if(begin == end) 
@@ -126,42 +126,58 @@ class isoImager
             return;
         }
 
-        //otherwise draw the events
-        double t0 = begin.timestamp();
         double tf = end.timestamp();
-        if(step < 1) step = 1;
-        int counter = 0;
-        for (auto a = begin; a != end; a++) {
+        int remaining = count;
+        int skip = (count - 2e6) / 2e6;
+        if(skip < 1) skip = 1;
 
+        //draw with skipping (2e6)
+        auto a = begin;
+        while(remaining > 1e6) 
+        {
             double dt = tf - (a.timestamp());
-            if (dt < 0)
-                break;
-
-            if (dt < 0.05) {
-                int x = a->x;
-                int y = a->y;
-                double z = 0;
-                ps.pttr(x, y, z);
-                if (x < 0 || x >= img.cols || y < 0 || y >= img.rows)
-                    continue;
-                if (a->p)
-                    img.at<cv::Vec3b>(y, x) = aqua;
-                else
-                    img.at<cv::Vec3b>(y, x) = violet;
-            }
-
-            if(counter++ % step) continue; 
+            int x = a->x;
+            int y = a->y;
+            ps.pttr(x, y, dt);
+            if (a->p)
+                img.at<cv::Vec3b>(y, x) -= naqua;
+            else
+                img.at<cv::Vec3b>(y, x) -= nviolet;
+            std::advance(a, skip);
+            remaining-=skip;
+        }
 
+        //draw the most recent 2e6 without skipping
+        while(remaining > 10000) 
+        {
+            double dt = tf - (a.timestamp());
             int x = a->x;
             int y = a->y;
-            double z = dt;
-            ps.pttr(x, y, z);
-            if (x < 0 || x >= img.cols || y < 0 || y >= img.rows)
-                continue;
+            ps.pttr(x, y, dt);
             if (a->p)
                 img.at<cv::Vec3b>(y, x) -= naqua;
             else
                 img.at<cv::Vec3b>(y, x) -= nviolet;
+            a++;
+            remaining--;
+        }
+
+        //draw while placing an image on the front surface
+        while(a != end) {
+
+            double dt = tf - (a.timestamp()), z = 0.0;
+            int x1 = a->x, x2 = a->x;
+            int y1 = a->y, y2 = a->y;
+            ps.pttr(x1, y1, dt);
+            ps.pttr(x2, y2, z);
+            if (a->p) {
+                img.at<cv::Vec3b>(y1, x1) -= naqua;
+                img.at<cv::Vec3b>(y2, x2) = aqua;
+            } else {
+                img.at<cv::Vec3b>(y1, x1) -= nviolet;
+                img.at<cv::Vec3b>(y2, x2) = violet;
+            }
+            a++;
         }
 
         img -= base_image;