Implemented with a mean - not compiling yet

RWR/src/read_event_pub/CMakeLists.txt

@@ -4,6 +4,9 @@ project(read_event_pub)
 ## Compile as C++11, supported in ROS Kinetic and newer
 add_compile_options(-std=c++11)
 
+#
+##add_subdirectory(/home/chris/Robot-With-Reflexes/RWR/src/dlib-19.23 dlib)
+#include(~/home/chris/Robot-With-Reflexes/RWR/src/dlib-19.23/CMakeLists.txt)
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 ## is used, also find other catkin packages
@@ -12,8 +15,11 @@ find_package(catkin REQUIRED COMPONENTS
   rospy
   std_msgs
   dvs_msgs
+  OpenCV
 )
 
+find_package(OpenCV REQUIRED)
+
 catkin_package()
 
 include_directories(
@@ -21,22 +27,20 @@ include_directories(
   ${catkin_INCLUDE_DIRS}
 )
 
-#add_executable(copy src/event_array_sub.cpp)
-add_executable(${PROJECT_NAME}_node src/event_array_sub.cpp src/read_event_pub_node.cpp)
+#add_executable(${PROJECT_NAME}_node src/event_array_sub.cpp src/read_event_pub_node.cpp)
+#add_executable(${PROJECT_NAME}_node src/Mean_variance.cpp src/read_event_pub_node.cpp)
+#add_executable(${PROJECT_NAME}_node src/Kmeans_variance.cpp src/read_event_pub_node.cpp dlib)
+#add_executable(${PROJECT_NAME}_node src/event_array_sub.cpp src/optical_flow.cpp)
+add_executable(optical_flow src/optical_flow.cpp src/read_event_pub_node.cpp)
+
 
-#add_dependencies(copy
-#  ${${PROJECT_NAME}_EXPORTED_TARGETS}
-#  ${catkin_EXPORTED_TARGETS}
-#)
-add_dependencies(${PROJECT_NAME}_node
+add_dependencies(optical_flow
   ${${PROJECT_NAME}_EXPORTED_TARGETS}
   ${catkin_EXPORTED_TARGETS}
 )
 
-#target_link_libraries(copy
-#  ${catkin_LIBRARIES}
-#)
-
-target_link_libraries(${PROJECT_NAME}_node
+target_link_libraries(optical_flow
   ${catkin_LIBRARIES}
+  ${OpenCV_LIBRARIES}
+  ${Boost_LIBRARIES}
 )
\ No newline at end of file

RWR/src/read_event_pub/include/event_array_sub.h

-#ifndef EVENT_ARRAY_SUB_H_
-#define EVENT_ARRAY_SUB_H_
-
-#define EVENT_ARRAY_PUBLISHERS 3
-#define EVENT_ARRAY_SUBSCRIBERS 1
-
-#include <ros/ros.h>
-#include <dvs_msgs/Event.h>
-#include <dvs_msgs/EventArray.h>
-#include <dvs_msgs/stats.h>
-
-namespace event_array_sub {
-
-class EventArraySub
-{
-    public:
-        EventArraySub(ros::NodeHandle& nh);
-        ~EventArraySub();
-
-    private:
-        ros::NodeHandle nh_;
-
-        void eventsCallback(const dvs_msgs::EventArray::ConstPtr& msg);
-
-        void publishEventStats();
-
-        ros::Subscriber event_sub_;
-
-        struct EventDetection {
-            ros::Publisher events_[2];
-            ros::Publisher dirLR_;
-            float xy_means_[1];
-            size_t xy_variance[1];
-            bool traject;
-            size_t msgSz;
-        };
-        size_t votesLeft;
-        size_t votesRight;
-        size_t trajCounter;
-
-        EventDetection event_detection_stats_;
-        uint8_t counter, nCounter;
-};
-
-}  //namespace
-
-#endif //EVENT_ARRAY_SUB_H_
\ No newline at end of file

RWR/src/read_event_pub/include/optical_flow.h

+#ifndef OPTICAL_FLOW_H_
+#define OPTICAL_FLOW_H_
+
+#define TIMESTAMP_THRESH_NS 100000
+#define LOW_TIMESTAMP_THRESH_NS 25000000
+
+#define PSEUDO_FRAME_NS 33000000
+
+#define COMPASS_DIRS 8
+#define COMPASS_AXIS 2
+
+#define SIGNIFICANT_VECTORS 0.005
+
+#define DVSW 180 //128 in sim
+#define DVSH 240 //128 in sim
+
+#include <ros/ros.h>
+#include <dvs_msgs/Event.h>
+#include <dvs_msgs/EventArray.h>
+
+#include <vector>
+#include <deque>
+#include <utility> 
+#include <opencv2/core/core.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+#include <opencv2/core/mat.hpp>
+
+int compassVectors[COMPASS_DIRS][COMPASS_AXIS] ={{-1,-1},
+                                                {0,-1},
+                                                {1,-1},
+                                                {1,1},
+                                                {0,1},
+                                                {-1,1},
+                                                {-1,0}
+                                                };
+
+namespace optical_flow {
+
+class OpticalFlow
+{
+    public:
+        OpticalFlow(ros::NodeHandle& nh);
+        ~OpticalFlow();
+
+        std::deque< std::pair<double, bool> > frames[DVSH][DVSW];
+        std::deque<int> pixelVectors[DVSH][DVSW];
+
+        std::vector< dvs_msgs::EventArrayPtr > preClusterData;
+        int preNDataPoints;
+
+        int32_t startFrameNSec; // psuedo frame holder
+
+    private:
+        ros::NodeHandle nh_;
+        ros::Subscriber event_sub_;
+        ros::Publisher dirAB;
+
+        void eventsCallback(const dvs_msgs::EventArray::ConstPtr& msg);
+        void motionEventsFiter(const dvs_msgs::EventArray::ConstPtr& stream);
+        bool nFilterPrePro(const dvs_msgs::Event event, int x, int y);
+        void loomingDetection(const cv::Mat points, const cv::Point2f center);
+        /*void loomingDetection(const cv::Mat labels,
+                              const std::vector<cv::Point2f> centers, 
+                              std::vector<bool> loomingidx,
+                              int nClusters);*/
+        //void publishMotionEvent();
+};
+
+}  //namespace
+
+#endif //OPTICAL_FLOW_H_
\ No newline at end of file

RWR/src/read_event_pub/src/optical_flow.cpp

 #include <ros/ros.h>
-#include <dvs_msgs/Event.h>
-#include <dvs_msgs/EventArray.h>
-#include <dvs_msgs/stats.h>
 
+#include <std_msgs/String.h>
+#include <cstdint> 
+
+#include "optical_flow.h"
+
+/*  1. Initialise all the data
+    2. call nFilterPrepro and build up the events that are OF events into a cv::mat
+    3. find the mean/kmeans/mscluster ect
+    4. call the looming algorithm with the labels and mean/centers/centroids
+*/
+
+namespace optical_flow {
+
+OpticalFlow::OpticalFlow(ros::NodeHandle& nh) : nh_(nh)
+{
+    dirAB = nh_.advertise<std_msgs::String>("/obj_det", 1);   
+    startFrameNSec = ros::Time::now().toNSec;
+
+    event_sub_ = nh_.subscribe("/AADC_AudiTT/camera_front/events", 1, &OpticalFlow::eventsCallback, this);
+}
+
+OpticalFlow::~OpticalFlow()
+{
+    //clear the contents for the next set of data
+    for(int i = 0; i <= DVSH; ++i)
+    {
+        for(int j = 0; j <= DVSW; ++j)
+        {
+            frames[i][j].clear();
+            pixelVectors[i][j].clear();
+        }
+    }
+    preClusterData.clear();
+}
+
+void OpticalFlow::eventsCallback(const dvs_msgs::EventArray::ConstPtr& msg)
+{
+    motionEventsFiter(const dvs_msgs::EventArray::ConstPtr& stream);
+
+    //Psuedo frame duration is finished
+    if((ros::Time::now().toNSec - durationStart) >= (int32_t)PSEUDO_FRAME_NS)
+    {
+        std::vector< dvs_msgs::EventArrayPtr > clusterData;
+
+        //Pre-process the data
+        for(int i = 0; i < preClusterData.size(); ++i)
+        {  
+            //Get the ith event
+            const dvs_msgs::Event e(preClusterData[i]);
+
+            //Find a neighbour that suggests its not an isolated event pixel
+            for(it=0; it < COMPASS_DIRS; ++it)
+            {
+                const int x = e.x - compassVectors[it][0];
+                const int y = e.y - compassVectors[it][1];
+                const double ts = e.ts.nsec;
+
+                //make sure its not out of bounds
+                if((x > 0) && (x < DVSH) && (y > 0) && (x < DVSW))
+                {
+                    if(nFilterPrePro(x, y, ts))
+                    {
+                        //copy to a vector holding the correct info
+                        clusterData.push_back(preClusterData[i]);
+                    }
+                }
+            }
+
+        }
+
+        //Perform pattern recognition on the data
+        //open cv matrix to process the data and get the mean of it
+        cv::Mat points(clusterData.size(), 1, CV_32FC2, cv::Scalar::all(0.));
+        cv::Point2f meanP;
+        int meanX = 0;
+        int meanY = 0;
+
+        for(int i =0 ; i < clusterData.size(); ++i)
+        {
+            //copy to the mat
+            cv::Point2f p(clusterData[i].x, clusterData[i].y);
+            points.at<cv::Point2f>(i) = p;
+
+            //add to the mean
+            meanX += clusterData[i].x;
+            meanY += clusterData[i].y;
+        }
+        meanP.x = meanX / clusterData.size();
+        meanP.y = meanY / clusterData.size();
+
+        //Find out if the data is looming and make the robot move
+        loomingDetection(const cv::Mat points, const cv::Point2f meanP);
+
+        //clear the contents for the next set of data
+        for(int i = 0; i <= DVSH; ++i)
+        {
+            for(int j = 0; j <= DVSW; ++j)
+            {
+                frames[i][j].clear();
+                pixelVectors[i][j].clear();
+            }
+        }
+        preClusterData.clear();
+
+        //restart the start time of the frame
+        startFrameNSec = ros::Time::now().toNSec;
+    }
+}
+
+void OpticalFlow::motionEventsFiter(const dvs_msgs::EventArray::ConstPtr& stream)
+{
+    uint16_t it(0);
+
+    //Loop through entire event array for algo 1
+    for(uint32_t i(0); i < stream->events.size(); ++i)
+    {
+        const dvs_msgs::Event e(stream->events[i]);
+        const x = e.x; const y = e.y;
+
+        //Find the out of date points
+        while(!(OpticalFlow::frames[x][y].empty()))
+        {
+            const std::pair<double, bool>  temp = OpticalFlow::frames[x][y].front();
+
+            //Check timestamp diff
+            if((e.ts.nsec - temp.first) > (double)TIMESTAMP_THRESH_NS)
+            {
+                //Remove from the dequeue of events and motion vectors
+                OpticalFlow::frames[x][y].pop_front();
+                OpticalFlow::pixelVectors[x][y].pop_front();
+            }
+            else
+            {
+                exit;
+            }
+        }
+
+        //Push most modern event to the back for more efficient searching
+        OpticalFlow::frames[x][y].push_back(std::make_pair(e.ts.nsec, e.polarity));
+
+        //Find motion vectors for each pixel if active
+        for(it=0; it < COMPASS_DIRS; ++it)
+        {
+            size_t x_p = x - compassVectors[it][0];
+            size_t y_p = y - compassVectors[it][1];
+
+            bool found = false;
+            uint16_t j(0);
+
+            while((j <= (OpticalFlow::frames[x_p][y_p].size())) && (!found))
+            {
+                std::deque< std::pair<double, bool> > topEvent(OpticalFlow::frames[x_p][y_p].at(j));
+
+                //Low timestamo threshold to ensure the same event is not counted
+                if (((double)LOW_TIMESTAMP_THRESH_NS < (e.ts.nsec - topEvent.first))
+                    &&((e.ts.nsec - topEvent.first) <= (double)TIMESTAMP_THRESH_NS)
+                    && (e.polarity == topEvent.polarity))
+                {
+                    //Dangerous/wasteful but simpler way to store the vector data for each pixel
+                    pixelVectors[x][y].push_back(it);
+                    preClusterData.push_back(e); //store the event for the clusters. 
+                    found = true;
+                }
+
+                ++j;
+            }
+        }
+    }
+    return;
+}
+
+/**
+ * @brief Algorithm 3
+ * 
+ * @param event Sucessful event from algorithm1 
+ * @return true If the event presents optical flow characterists
+ * @return false If the event is singular and likely to be noise
+ */
+bool OpticalFlow::nFilterPrePro(const int x, const int y, const double ts)
+{
+    uint32_t i(0); //zero-based
+
+    while(i <= OpticalFlow::frames[x][y].size())
+    {
+        //Copy the motion vector and timestamp
+        std::pair<double, bool> iEvent = OpticalFlow::frames[x][y].at(i);
+        std::deque<int> iMotionVector = OpticalFlow::pixelVectors[x][y].at(i);
+
+        //Keep it in date and check the vectors are similar
+        if (((double)LOW_TIMESTAMP_THRESH_NS < (e.ts.nsec - iEvent.first))
+            &&((e.ts.nsec - iEvent.first) <= (double)TIMESTAMP_THRESH_NS)
+            && ((-1 == j - iMotionVector[i]) || (j - iMotionVector[i] == 1)))
+        {
+            //YES they are similar and represent motion
+            return true;
+        }
+        ++i;
+    }
+}
+
+void OpticalFlow::loomingDetection(const cv::Mat points, const cv::Point2f center)
+{
+    uint32_t pos(0);
+    uint32_t neg(0);
+    size_t processedPoints(0);
+
+    for(int j(0); j <= points.size.height; ++j)
+    {
+        int k = 0;
+        while(k <= OpticalFlow::pixelVectors[x][y].size())
+        {
+            cv::Point2f innerVec(center);
+            const cv::Point2f point = points.at<Point2f>(j);
+            const int vec = pixelVectors[point.x][point.y].at(k);
+            const int8_t compass_x = compassVectors[vec][0];
+            const int8_t compass_y = compassVectors[vec][1];
+
+            innerVec.x = innerVec.x - point.x;
+            innerVec.y = innerVec.y - point.y;
+
+            const int32_t  dotProd = ((innerVec.x * compass_x) + (innerVec.y * compass_y));
+
+            dotProd > 0 ? pos++ : neg++;
+
+            ++k;
+            ++processedPoints;
+        }
+    }
+
+    if((pos >= 2 * neg) && (pos >= SIGNIFICANT_VECTORS * processedPoints))
+    {
+        std_msgs::String dir;
+
+        if(c.y < 75) //On DAVIS-240C: 90 + 15 = 105 //IN SIM: 75
+            dir.data = "above";
+        else if (c.y > 45) //On DAVIS-240C: 90 -15 = 75 //IN SIM: 45
+            dir.data = "below";
+        else
+            dir.data = "none";
+        dirAB.publish(dir);
+
+    }
+}
+
+} //namespace
+
+/*int main(int argc, char** argv)
+{
+    ros::init(argc, argv, "read_event_pub_node");
+
+    ros::NodeHandle nh;
+
+    optical_flow::OpticalFlow opticalFlow(nh);
+
+    ros::spin();
+
+    return 0;
+}*/
\ No newline at end of file