Eigen Implementatin (PCL+Eigen)

include/compute_flow/compute_flow.h

@@ -5,27 +5,40 @@
 #include <dvs_msgs/Event.h>
 #include <dvs_msgs/EventArray.h>
 #include "ros/ros.h"
-
-#include <pcl/ModelCoefficients.h>
 #include <pcl/io/pcd_io.h>
 #include <pcl/point_types.h>
 
-// Headers for extractNormals
-#include <pcl/features/normal_3d.h>
-
-// Headers for fit plane
-#include <pcl/sample_consensus/method_types.h>
-#include <pcl/sample_consensus/model_types.h>
-#include <pcl/segmentation/sac_segmentation.h>
+#include <Eigen/Dense>
 
 namespace compute_flow
 {
-		typedef pcl::PointXYZI PointT;
-		void computeFlow(pcl::PointCloud<PointT> &cloud);
-		void pcaModified();
-		void robustSVD();
-		void fitPlane(pcl::PointCloud<PointT> &cloud);
-		void extractNormals(pcl::PointCloud<PointT> cloud);
+	const int NROWS = 340, NCOLS = 340; //Max and Min Value of X and Y (Event Positions)
+	const int N = 5; // TimeStamp Patch Mask Dimension around the Events
+	const double TIME_THRES = 0.99;
+	const double TH2 = 0.1;
+	typedef pcl::PointXYZI PointT;
+	typedef Eigen::MatrixXd Mat;
+
+	//Eigen::MatrixXd pclToMatrix(pcl::PointCloud<PointT>::Ptr cloud);
+	void computeFlow(pcl::PointCloud<PointT>::Ptr cloud);
+	bool fitPlane(Mat &m,Eigen::Vector3d &normal, const double TH2);
+	void pca(Mat &X,Mat &U);
+
+	void find(Mat &m, Mat &X);
+	Mat getPatch(Mat &m,int x, int y, int N);
+	Mat getBlock(Mat &m,int x,int y,int N);
+
+
+	struct CWiseThres {
+		CWiseThres(const double val,const double thres) : val_(val),thres_(thres){}
+		const double operator()(const double& x) const { 
+			if(abs(x - val_)/val_ > thres_)
+				return 0.0;
+			else
+				return x;
+			 };
+		double val_, thres_;
+	};
 } 
 
 #endif //_compute_flow_H_ 

include/dvs_flow/event_loader.h

@@ -30,8 +30,8 @@ class EventLoader {
   virtual ~EventLoader();
 
 private:
-  pcl::PointCloud<PointT> events_pcl_;
-  //pcl::PointCloud<PointT>::Ptr events_pcl_ptr_;
+  //pcl::PointCloud<PointT> events_pcl_;
+  pcl::PointCloud<PointT>::Ptr events_pcl_ptr_;
   dvs_events events_;
   std::vector<dvs_events> events_array_;
 
@@ -41,12 +41,16 @@ class EventLoader {
   ros::NodeHandle nh_;
   float t_first_ = -1;
   ros::Subscriber events_sub_;
-  ros::Publisher events_pub_;
+
+
+
   image_transport::Subscriber image_sub_;
 
   void eventListenerCallback(const dvs_msgs::EventArray::ConstPtr& msg);
   void imageListenerCallback(const sensor_msgs::Image::ConstPtr& msg);
-  void displayEvents(std::vector<dvs_events> &e_array);
+
+  ros::Publisher events_pub_;
+  void displayEvents();
 
 };
 } // namespace