Implemented a real-time optical flow visualization.

CMakeLists.txt

@@ -53,7 +53,7 @@ add_message_files(
   State.msg
   StateGroundTruth.msg
   UkfStats.msg
-
+  OpticalFlow.msg
 )
 
 ## Generate services in the 'srv' folder

msg/OpticalFlow.msg

@@ -0,0 +1,10 @@
+# Message to be used by the optical flow code to republish them for
+# displaying in the web dashboard.
+
+Header header
+
+uint32 rows         # a fraction of the corresponding image's height
+uint32 columns       # a fraction of the corresponding image's width
+
+float64[] flow_x      # x components of the optical flow field stored in a 1D array, the index of a vector at (x, y) is (y * width + x)
+float64[] flow_y      # the corresponding y components of the vectors

scripts/analyze_flow.py

@@ -4,6 +4,7 @@
 import picamera.array
 from pidrone_pkg.msg import State
 from geometry_msgs.msg import TwistStamped
+from pidrone_pkg.msg import OpticalFlow
 
 
 class AnalyzeFlow(picamera.array.PiMotionAnalysis):
@@ -28,6 +29,7 @@ def setup(self, camera_wh):
         ############
         # Publisher:
         self.twistpub = rospy.Publisher('/pidrone/picamera/twist', TwistStamped, queue_size=1)
+        self.flowpub = rospy.Publisher('/pidrone/picamera/flow', OpticalFlow, queue_size=1)
         # Subscriber:
         rospy.Subscriber("/pidrone/state", State, self.state_callback)
 
@@ -50,8 +52,18 @@ def analyse(self, a):
         twist_msg.twist.linear.x = self.near_zero(x_motion)
         twist_msg.twist.linear.y = - self.near_zero(y_motion)
 
+        # create the optical flow message
+        shape = np.shape(x)
+        flow_msg = OpticalFlow()
+        flow_msg.header.stamp = rospy.Time.now()
+        flow_msg.rows = shape[0]
+        flow_msg.columns = shape[1]
+        flow_msg.flow_x = x.flatten()
+        flow_msg.flow_y = y.flatten()
+
         # Update and publish the twist message
         self.twistpub.publish(twist_msg)
+        self.flowpub.publish(flow_msg)
 
     def near_zero(self, n):
         return 0 if abs(n) < 0.001 else n

web/index.html

@@ -80,7 +80,12 @@ <h4>Height Readings</h4>
 
                 <div class="col-sm text-center" style="margin-top:20px;">
                     <h4>Camera Image</h4>
-                    <img id="cameraImage" alt="Camera image" src="nocamera.jpg" width=320/>
+                    <input class="btn btn-primary my-2 my-sm-0" type="submit" value="Show Optical Flow" name="flowToggleBtn" onclick="toggleOpticalFlow(this)"/>
+                    <br>
+                    <div style="width: 320px; height: 240px; display: inline-block; position: relative; margin-top: 0.5rem;">
+                        <img id="cameraImage" alt="Camera image" src="nocamera.jpg" width=320/>
+                        <canvas id="cameraFlowCanvas" width=320 height="240" style="position: absolute; top: 0; left: 0;"></canvas>
+                    </div>
                     <div class="form-group">
                         <small id="imagetopicHelp" class="form-text text-muted">Make sure you run <code>rosrun web_video_server web_video_server</code> and the image stream.</small>
                     </div>

web/js/main.js

@@ -39,6 +39,7 @@ var startTime;
 var heightChartPaused = false;
 var showingUkfAnalysis = false;
 var spanningFullWindow = false;
+var displayOpticalFlow = false;
 
 function closeSession(){
   console.log("Closing connections.");
@@ -235,6 +236,14 @@ function init() {
         throttle_rate : 80
     });
 
+    cameraFlowSub = new ROSLIB.Topic({
+      ros : ros,
+      name : '/pidrone/picamera/flow',
+      messageType : 'pidrone_pkg/OpticalFlow',
+      queue_length : 2,
+      throttle_rate : 100
+    });
+
     emaIrSub = new ROSLIB.Topic({
       ros : ros,
       name : '/pidrone/state/ema',
@@ -447,6 +456,8 @@ function init() {
         updateCameraPoseXYChart(message);
     });
 
+    cameraFlowSub.subscribe(updateVisionFlow);
+
     function updateGroundTruthXYChart(msg) {
         xPos = msg.pose.position.x;
         yPos = msg.pose.position.y;
@@ -505,6 +516,57 @@ function init() {
         }
     }
 
+    function updateVisionFlow(msg) {
+        if(displayOpticalFlow) {
+            var rows = msg.rows;
+            var cols = msg.columns; // TODO: why is there an extra one...?
+
+            // Get the [x, y] components of a flow vector at x, y with some scaling, flipping, constraining, etc...
+            function getVector(x, y) {
+                return [-constrain(2*msg.flow_x[(y * cols) + x], -12, 12), -constrain(2*msg.flow_y[(y * cols) + x], -12, 12)]
+            }
+
+            var ctx = document.getElementById('cameraFlowCanvas').getContext('2d');
+
+            ctx.clearRect(0, 0, 320, 240);
+
+            ctx.strokeStyle = "#ccff00"; // Draw direction indicator
+            ctx.beginPath();
+            for(var ix = 0; ix < cols; ix++) {
+                for(var iy = 0; iy < rows; iy++) {
+                    var x = (ix * 16) + 8;
+                    var y = (iy * 16) + 8;
+                    var vec = getVector(ix, iy);
+                    ctx.moveTo(x, y);
+                    ctx.lineTo(x + vec[0], y + vec[1]);
+                    ctx.moveTo(x-1, y);
+                    ctx.lineTo(x+1, y);
+                    ctx.moveTo(x, y-1);
+                    ctx.lineTo(x, y+1);
+                }
+            }
+            ctx.stroke();
+
+            ctx.strokeStyle = "#000"; // Draw center marker
+            ctx.beginPath();
+            for(var ix = 0; ix < cols; ix++) {
+                for(var iy = 0; iy < rows; iy++) {
+                    var x = (ix * 16) + 8;
+                    var y = (iy * 16) + 8;
+                    ctx.moveTo(x-1, y);
+                    ctx.lineTo(x+1, y);
+                    ctx.moveTo(x, y-1);
+                    ctx.lineTo(x, y+1);
+                }
+            }
+            ctx.stroke();
+        }
+    }
+
+    function constrain(num, min, max) {
+      return num > max ? max : (num < min ? min : num);
+    }
+
     function updateCameraPoseXYChart(msg) {
         xPos = msg.pose.position.x;
         yPos = msg.pose.position.y;
@@ -1328,6 +1390,16 @@ function togglePauseXYChart(btn) {
     console.log('Pause button pressed')
 }
 
+function toggleOpticalFlow(btn) {
+  displayOpticalFlow = !displayOpticalFlow;
+  if (displayOpticalFlow) {
+      btn.value = 'Hide Optical Flow'
+  } else {
+      btn.value = 'Show Optical Flow'
+      document.getElementById('cameraFlowCanvas').getContext('2d').clearRect(0, 0, 320, 240);
+  }
+}
+
 function publishVelocityMode() {
     positionMsg.data = false;
     positionPub.publish(positionMsg)