From 824d70a86319b503980ed48523daa37f87993466 Mon Sep 17 00:00:00 2001
From: chirag <chiragamajithia@gmail.com>
Date: Sun, 30 Oct 2016 01:41:33 -0400
Subject: [PATCH 1/3] Eigen Implementatin (PCL+Eigen)

---
 include/compute_flow/compute_flow.h |  43 +++--
 include/dvs_flow/event_loader.h     |  12 +-
 src/compute_flow.cpp                | 256 +++++++++++++++-------------
 src/event_loader.cpp                |  51 +++---
 4 files changed, 197 insertions(+), 165 deletions(-)

diff --git a/include/compute_flow/compute_flow.h b/include/compute_flow/compute_flow.h
index da6d131..1242e90 100644
--- a/include/compute_flow/compute_flow.h
+++ b/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.2;
+	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);
+	void fitPlane(Mat &m, const double TH2);
+	void pca(Mat &X);
+
+	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_ 
\ No newline at end of file
diff --git a/include/dvs_flow/event_loader.h b/include/dvs_flow/event_loader.h
index a91d04d..ebdd794 100644
--- a/include/dvs_flow/event_loader.h
+++ b/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
diff --git a/src/compute_flow.cpp b/src/compute_flow.cpp
index 57fdbc5..9a2fbae 100644
--- a/src/compute_flow.cpp
+++ b/src/compute_flow.cpp
@@ -3,11 +3,66 @@
 namespace compute_flow
 {
 	//[vx, vy, It] = computeFlow(x, y, t, pol, N, TH1, TH2, NCOLS, NROWS)
-	void computeFlow(pcl::PointCloud<PointT> &cloud)
+	void computeFlow(pcl::PointCloud<PointT>::Ptr events_ptr)
 	{
+
+
 		ROS_INFO_STREAM("compute_flow()");
+		
+		// Mat m = Mat::Random(5,6);
+		// getPatch(m,3,3,2);
+		
+
 		/** CONVERT THIS FUNCTION
+		**/
+
+		Mat img_ts_pos = Mat::Zero(NROWS, NROWS); //Matrix of timestamps of last events with positive polarity
+		Mat img_ts_neg =  Mat::Zero(NROWS, NCOLS); //Matrix of timestamps of last events with negative polarity
+		Mat vx= Mat::Zero(NROWS,NCOLS);
+		Mat vy= Mat::Zero(NROWS,NCOLS);
+		Mat m;
+		
+		
+		std::cout <<"mask timestampe pos" <<m<< std::endl;
+
+		for(auto i= 0; i < events_ptr->size(); ++i)
+		{
+			int x = events_ptr->points[i].x;
+			int y = events_ptr->points[i].y;
+			double ts = events_ptr->points[i].z;
+
+			if(events_ptr->points[i].intensity == 255)
+			{
+				std::cout<<"Patch from Positive Polarity"<<std::endl;
+				img_ts_pos(x,y) = ts;
+				m = getBlock(img_ts_pos,x,y,N);
+			}
+			else if(events_ptr->points[i].intensity == 0)
+			{
+				std::cout<<"Patch from Negative Polarity"<<std::endl;
+				img_ts_neg(x,y) = ts;
+				m = getBlock(img_ts_neg,x,y,N);
+			}
+			else
+			{
+				ROS_ERROR("Polarity of the events NOT GOOD");
+				exit (EXIT_FAILURE);
+			}
+
+			if(m.size()!=0)
+			{
+				//Select Events that are close in the time_stamp
+				fitPlane(m,TH2);
 
+			}
+			else
+			{
+				std::cout<<"NULL MATRIX"<<std::endl;
+			}
+			//Mat m = Mat::Zero(2*N+1,2*N+1); //Time Stamp patch 
+
+		}
+		/**
 	    It_pos=zeros(NROWS,NCOLS);
 	    It_neg=zeros(NROWS,NCOLS);
 	    vx=zeros(NROWS,NCOLS); vy=zeros(NROWS,NCOLS);
@@ -36,7 +91,7 @@ namespace compute_flow
 			    	[vvx,vvy]=fitplane(m, TH2);
 			**/
 					
-					fitPlane(cloud);
+					//estimateNormals(cloud);
 			/**    	
 			    	if(isnan(vvx) || isinf(vvx))
 			    	{
@@ -61,139 +116,100 @@ namespace compute_flow
 		 **/
 	}
 
-	//function [vx,vy]=fitplane(mm, TH)
-	void fitPlane(pcl::PointCloud<PointT> &cloud_)
+	void find(Mat &m, Mat &X)
 	{
-		const pcl::PointCloud<PointT>::Ptr cloud(&cloud_);
-		pcl::IndicesPtr indices(new std::vector<int>);
-		float distanceThreshold = 0.01;
-		int maxIterations = 1000;
-		// Extract plane
-		pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
-		pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
-	// Create the segmentation object
-		pcl::SACSegmentation<PointT> seg;
-	// Optional
-		seg.setOptimizeCoefficients (true);
-		seg.setMaxIterations (maxIterations);
-	// Mandatory
-		seg.setModelType (pcl::SACMODEL_PLANE);
-		seg.setMethodType (pcl::SAC_RANSAC);
-		seg.setDistanceThreshold (distanceThreshold);
-
-		seg.setInputCloud (cloud);
-		if(indices->size())
+		int cnt = 0;
+		for(int i = 0; i < m.rows(); ++i)
 		{
-			seg.setIndices(indices);
+			for(int j = 0; j < m.cols(); ++j)
+			{	
+				if(m(i,j)!=0)
+				{	
+					Eigen::Vector3d vec(j,i,m(i,j)); //Check for X and Y Values
+					//std::cout<<" Vector = "<<vec<<std::endl;
+					X.conservativeResize(X.rows() + 1, Eigen::NoChange);
+					X.row(X.rows()-1) = vec;
+					//std::cout<<"X  = "<<X<<std::endl;
+				}
+			}
 		}
-		seg.segment (*inliers, *coefficients);
-
-		// if(coefficientsOut)
-		// {
-		// 	*coefficientsOut = *coefficients;
-		// }
-
-		//return pcl::IndicesPtr(new std::vector<int>(inliers->indices));
 	}
 
-	void estimateNormals(pcl::PointCloud<PointT> cloud)
-	{
-		const pcl::PointCloud<PointT>::Ptr cloud_ptr(&cloud);
-		pcl::NormalEstimation<PointT, pcl::Normal> ne;
-  		ne.setInputCloud(cloud_ptr);
-  		pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT> ());
-  		ne.setSearchMethod (tree);
-  		pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
-		ne.setRadiusSearch (0.03);
-		ne.compute (*cloud_normals);
-	}	
-	//function [U,mu,vars] = pca( X )
-	void pcaModified()
+	void fitPlane(Mat &m, const double TH2)
 	{
-		ROS_INFO_STREAM("pca_modified()");
-		/*
-		d=size(X); n=d(end); d=prod(d(1:end-1));
-		if(~isa(X,'double'))
-		{
-			X=double(X);
-		}
-
-		if(n==1)
-		{
-			mu=X;
-			U=zeros(d,1);
-			vars=0;
-			return;
-		}
-
-		mu = mean(X, ndims(X));
-		X = bsxfun(@minus,X,mu)/sqrt(n-1);
-		X = reshape( X, d, n );
-
-		m=2500;
-
-		if(min(d,n)>m)
+		double c_val = m(N,N);
+		// std::cout<<"central val = "<<c_val<<std::endl;
+		// std::cout<<"Before thresholding \n"<<m<<std::endl;
+		m = m.unaryExpr(CWiseThres(c_val,TIME_THRES));
+		// std::cout<<"After thresholding \n"<<m<<std::endl;
+		if(m.sum() > 0)
 		{
-			X=X(:,randperm(n,m));
-			n=m;
+			Mat X(0,3);
+			find(m,X);
+			ROS_INFO_STREAM("fit plane input \n"<<X);
+			pca(X);
 		}
 
-		if(0)
-		{
-			[U,S]=svd(X,'econ');
-			vars=diag(S).^2;
-		}
-		elseif( d>n )
-		{
-			[~,SS,V]= robustSvd(X'*X); 
-			**/
-			robustSVD();
-			/**
-			vars=diag(SS);
-			U = X * V * diag(1./sqrt(vars));
-		}
-		else
-		{
-			[~,SS,U]=robustSvd(X*X');
-			**/
-			robustSVD();
-			/**
-			vars=diag(SS);
-		}
 
-		K=vars>1e-30;
-		vars=vars(K);
-		U=U(:,K);
-		*/
 	}
-
-	//function [U,S,V] = robustSvd( X, trials )
-	void robustSVD()
+	//function [U,mu,vars] = pca( X )
+	void pca(Mat &X)
 	{
-		ROS_INFO_STREAM("robust_svd()");
-		/**
-		if(nargin<2)
+		if(X.rows() != 1)
 		{
-
-			trials=100;
+		X = (X.rowwise() - X.colwise().mean())/std::sqrt(X.rows()-1);
+		std::cout<<"X_centered"<<X<<std::endl;
+		Eigen::JacobiSVD<Mat> svd(X, Eigen::ComputeFullU | Eigen::ComputeFullV);
+		std::cout << "U: " << svd.matrixU().size() << std::endl;
+		std::cout << "V: " << svd.matrixV() << std::endl;
+		std::cout<<"Press any key to continue";
+		std::cin.ignore();
+		ROS_INFO_STREAM("pca_modified()");
 		}
+	}
 
-		try
-		{
-			[U,S,V] = svd(X)
-		}
-		catch
-		{
-			if(trials<=0)
-			{
-				error('svd did not converge');
-			}
-			n=numel(X);
-			j=randi(n);
-			X(j)=X(j)+eps;
-			[U,S,V]=robustSvd(X,trials-1);
-		}
-		**/
+	Mat getPatch(Mat &m,int x, int y, int N)
+	{
+		//std::cout<<m<<std::endl;
+		Mat patch;
+		int s_x = x-N; //start x index
+		int s_y = y-N; //start y index
+		int e_x = x + N; //end x index
+		int e_y = y + N; //end y index
+		int m_x = m.rows()-1;
+		int m_y = m.cols()-1;
+		s_x = std::max(s_x,0);
+		s_y = std::max(s_y,0);
+		e_x = std::min(e_x,m_x-1);
+		e_y = std::min(e_y,m_y-1);
+		
+
+		patch = m.block(s_x,s_y,e_x-s_x+1,e_y-s_y+1);
+		// DEBUGGING
+		// std::cout<<"x = "<<x<<" y = "<<y<<" max_x = "<<m.rows()<<" max_y = "<<m.cols()<<std::endl; 
+		// std::cout<<"dx = "<<e_x-s_x+1<<" dy = "<<e_y-s_y+1<<std::endl;
+		// std::cout<<"\npatch = \n"<<patch<<std::endl;
+		return patch;
 	}
 
+	Mat getBlock(Mat &m,int x, int y, int N)
+	{
+		Mat patch;
+		int L = (2*N)+1;
+    	int s_x = x-N; //start x index
+  	 	int s_y = y-N; //start y index
+  	 	int e_x = x + L; //end x index
+ 		int e_y = y + L; //end y index
+
+ 		if(s_x < 0 || s_y < 0 || e_x >= m.rows() || e_y >= m.cols())
+  		{
+   	   		return patch; //return patch of zero size
+  		}
+
+ 		patch = m.block(x-N,y-N,L,L);
+
+ 		ROS_INFO_STREAM("\npatch = \n"<<patch<<"\n");
+ 		return patch;
+	}	
+
 }
\ No newline at end of file
diff --git a/src/event_loader.cpp b/src/event_loader.cpp
index 79e097c..9683e61 100644
--- a/src/event_loader.cpp
+++ b/src/event_loader.cpp
@@ -8,9 +8,11 @@ namespace event_loader{
     ROS_INFO("Subscribing to \"events\" Started");
     events_sub_ = nh_.subscribe("/dvs/events",1000, &EventLoader::eventListenerCallback,this);  
     events_pub_= nh_.advertise<pcl::PointCloud<PointT>> ("events", 10);
-    events_pcl_.header.frame_id = "odom";
-    events_pcl_.width = 0;
-    events_pcl_.height = 1;
+    pcl::PointCloud<PointT>::Ptr temp_ptr (new pcl::PointCloud<PointT>);
+    events_pcl_ptr_ = temp_ptr;
+    events_pcl_ptr_->header.frame_id = "odom";
+    events_pcl_ptr_->width = 0;
+    events_pcl_ptr_->height = 1;
     
 
     image_transport::ImageTransport it_(nh_);
@@ -34,12 +36,12 @@ namespace event_loader{
   {
     ROS_INFO_STREAM("Fresh Events Received: "<<msg->events.size()); 
     //ROS_INFO_STREAM("events_array_ size"<<events_array_.size());
-    if(events_pcl_.size()<BATCH_SIZE)
+    if(events_pcl_ptr_->size()<BATCH_SIZE)
     {
       //events_array_[0].insert(events_array_[0].end(),msg->events.begin(),msg->events.end());      
-      // events_pcl_.width += msg->events.size();
-      // events_pcl_.data.insert(events_array_[0].end(),msg->events.begin(),msg->events.end()); 
-      for(int i = 0; i < msg->events.size();++i)
+      // events_pcl_ptr_->width += msg->events.size();
+      // events_pcl_ptr_->data.insert(events_array_[0].end(),msg->events.begin(),msg->events.end()); 
+      for(auto i = 0; i < msg->events.size();++i)
       {
        PointT f_event;
        f_event.x = msg->events[i].x/10;
@@ -47,24 +49,21 @@ namespace event_loader{
        f_event.z = (msg->events[i].ts.toSec() - msg->events[0].ts.toSec())*100;
       //ROS_INFO_STREAM("Z = "<<f_event.z);
        f_event.intensity = (msg->events[i].polarity?0:1) * 255;
-       events_pcl_.push_back(f_event);
+       events_pcl_ptr_->push_back(f_event);
       }
-      ROS_INFO_STREAM("Events Recorded in PCL"<<events_pcl_.size());
+      ROS_INFO_STREAM("Events Recorded in PCL"<<events_pcl_ptr_->size());
       
     }
-    if(events_pcl_.size()>=BATCH_SIZE)
+    if(events_pcl_ptr_->size()>=BATCH_SIZE)
     {
-      ROS_INFO_STREAM(" --- Events Recorded in PCL"<<events_pcl_.size());
-      events_pcl_.header.stamp = ros::Time::now().toNSec();
-      events_pub_.publish(events_pcl_);
-      ROS_INFO_STREAM("PUBLISHED POINT_CLOUD ");
-      //compute_flow::compute_flow();
-      events_pcl_.clear();
-      events_pcl_.height = 1;
-      ROS_INFO_STREAM(" -- Events Erased"<< events_pcl_.size());
+      ROS_INFO_STREAM(" --- Events Recorded in PCL"<<events_pcl_ptr_->size());
+      displayEvents();
+      compute_flow::computeFlow(events_pcl_ptr_);
+      
+      events_pcl_ptr_->clear();
+      events_pcl_ptr_->height = 1;
+      ROS_INFO_STREAM(" -- Events Erased"<< events_pcl_ptr_->size());
     }
-
-      //displayEvents(events_array_);
   }
 
   void EventLoader::imageListenerCallback(const sensor_msgs::Image::ConstPtr& msg)
@@ -96,14 +95,14 @@ namespace event_loader{
       used_last_image_ = false;*/
   }
 
-  void EventLoader::displayEvents(std::vector<dvs_events> &e)
+  void EventLoader::displayEvents()
   {
-      for ( int i = 0; i < e.size(); ++i )
+      if(events_pub_.getNumSubscribers()>0)
       {
-        for (int j = 0; j < e[i].size() ; ++j)
-        {
-          ROS_INFO_STREAM(" x ["<<i<<"]["<<j<<"[= "<<e[i][j].x);
-        }
+        events_pcl_ptr_->header.stamp = ros::Time::now().toNSec();
+        events_pub_.publish(*events_pcl_ptr_);
+        ROS_INFO_STREAM("PUBLISHED POINT_CLOUD ");
       }
   }
+
 }
\ No newline at end of file

From a3af0a80394378f920d29c0b6c51a6a8f7f69859 Mon Sep 17 00:00:00 2001
From: chirag <chiragamajithia@gmail.com>
Date: Sun, 30 Oct 2016 04:09:32 -0400
Subject: [PATCH 2/3] compute flow done -- bug fix required

---
 include/compute_flow/compute_flow.h |  4 ++--
 src/compute_flow.cpp                | 36 ++++++++++++++++++++++-------
 2 files changed, 30 insertions(+), 10 deletions(-)

diff --git a/include/compute_flow/compute_flow.h b/include/compute_flow/compute_flow.h
index 1242e90..58d4b8f 100644
--- a/include/compute_flow/compute_flow.h
+++ b/include/compute_flow/compute_flow.h
@@ -21,8 +21,8 @@ namespace compute_flow
 	
 	//Eigen::MatrixXd pclToMatrix(pcl::PointCloud<PointT>::Ptr cloud);
 	void computeFlow(pcl::PointCloud<PointT>::Ptr cloud);
-	void fitPlane(Mat &m, const double TH2);
-	void pca(Mat &X);
+	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);
diff --git a/src/compute_flow.cpp b/src/compute_flow.cpp
index 9a2fbae..90638d9 100644
--- a/src/compute_flow.cpp
+++ b/src/compute_flow.cpp
@@ -52,8 +52,16 @@ namespace compute_flow
 			if(m.size()!=0)
 			{
 				//Select Events that are close in the time_stamp
-				fitPlane(m,TH2);
-
+				Eigen::Vector3d normal;
+				if(fitPlane(m,normal,TH2))
+				{
+				std::cout<<"Normal computed "<<normal;
+				double vx = -1*normal(2)/(std::pow(normal(1),2)+std::pow(normal(0),2))*normal(0)*1e6;
+				double vy = -1*normal(2)/(std::pow(normal(1),2)+std::pow(normal(0),2))*normal(1)*1e6;
+				std::cout<<"vx = "<<vx<<"vy = "<<vy<<std::endl;
+				std::cout<<"Press any key to continue";
+				std::cin.ignore(); 
+				}
 			}
 			else
 			{
@@ -135,7 +143,7 @@ namespace compute_flow
 		}
 	}
 
-	void fitPlane(Mat &m, const double TH2)
+	bool fitPlane(Mat &m, Eigen::Vector3d &normal, const double TH2)
 	{
 		double c_val = m(N,N);
 		// std::cout<<"central val = "<<c_val<<std::endl;
@@ -145,25 +153,37 @@ namespace compute_flow
 		if(m.sum() > 0)
 		{
 			Mat X(0,3);
+			Mat U;
 			find(m,X);
 			ROS_INFO_STREAM("fit plane input \n"<<X);
-			pca(X);
+			if(X.rows() >= 3)
+			{
+			pca(X,U);
+			normal= U.col(2);
+			std::cout<<"computed Normal"<< normal<<std::endl;
+			return true;
+			}
+			else
+				return false;
 		}
 
 
 	}
 	//function [U,mu,vars] = pca( X )
-	void pca(Mat &X)
+	void pca(Mat &X, Mat &U)
 	{
-		if(X.rows() != 1)
+		if(X.rows() >= 1) //Checking if there is sufficient data to get coeff > 3
 		{
 		X = (X.rowwise() - X.colwise().mean())/std::sqrt(X.rows()-1);
 		std::cout<<"X_centered"<<X<<std::endl;
+		X = X.transpose()*X;
 		Eigen::JacobiSVD<Mat> svd(X, Eigen::ComputeFullU | Eigen::ComputeFullV);
 		std::cout << "U: " << svd.matrixU().size() << std::endl;
+		Mat S = svd.singularValues().asDiagonal();
+		std::cout<<"S: "<< S <<std::endl; 
 		std::cout << "V: " << svd.matrixV() << std::endl;
-		std::cout<<"Press any key to continue";
-		std::cin.ignore();
+		//U = X * svd.matrixV() * S.inverse(); //check if diagnol(1/S) works
+		U = svd.matrixV();
 		ROS_INFO_STREAM("pca_modified()");
 		}
 	}

From 1b338aef344409398e3c48fa63f7a40dc6353eb0 Mon Sep 17 00:00:00 2001
From: chirag <chiragamajithia@gmail.com>
Date: Sun, 30 Oct 2016 04:16:58 -0400
Subject: [PATCH 3/3] compute flow -- bug fix required

---
 include/compute_flow/compute_flow.h |  2 +-
 src/compute_flow.cpp                | 53 +----------------------------
 2 files changed, 2 insertions(+), 53 deletions(-)

diff --git a/include/compute_flow/compute_flow.h b/include/compute_flow/compute_flow.h
index 58d4b8f..2965da8 100644
--- a/include/compute_flow/compute_flow.h
+++ b/include/compute_flow/compute_flow.h
@@ -14,7 +14,7 @@ namespace compute_flow
 {
 	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.2;
+	const double TIME_THRES = 0.99;
 	const double TH2 = 0.1;
 	typedef pcl::PointXYZI PointT;
 	typedef Eigen::MatrixXd Mat;
diff --git a/src/compute_flow.cpp b/src/compute_flow.cpp
index 90638d9..b42dfe6 100644
--- a/src/compute_flow.cpp
+++ b/src/compute_flow.cpp
@@ -70,58 +70,6 @@ namespace compute_flow
 			//Mat m = Mat::Zero(2*N+1,2*N+1); //Time Stamp patch 
 
 		}
-		/**
-	    It_pos=zeros(NROWS,NCOLS);
-	    It_neg=zeros(NROWS,NCOLS);
-	    vx=zeros(NROWS,NCOLS); vy=zeros(NROWS,NCOLS);
-	    for ii=1:1:length(t) 
-	    {
-	       ptx=x(ii)+1;
-	       pty=y(ii)+1;   
-		  
-		    if pol(ii)==1
-			{	  
-	        	It_pos(pty,ptx)=t(ii);
-		    	m=It_pos(max(pty-N,1):min(pty+N, NROWS),max(ptx-N,1):min(ptx+N, NCOLS));
-		    }
-		    else
-		    {
-		        It_neg(pty,ptx)=t(ii);
-		        m=It_neg(max(pty-N,1):min(pty+N, NROWS),max(ptx-N,1):min(ptx+N, NCOLS));
-		    }
-		    
-		    if numel(m) == (2*N+1)*(2*N+1)
-		    {
-				m(abs(m(N+1,N+1)-m)/m(N+1,N+1)>TH1)=0;
-		   	    if (sum(m(:)>0))
-			    {
-			    	
-			    	[vvx,vvy]=fitplane(m, TH2);
-			**/
-					
-					//estimateNormals(cloud);
-			/**    	
-			    	if(isnan(vvx) || isinf(vvx))
-			    	{
-			    		vvx = 0;
-			    	}
-			    	
-			    	if(isnan(vvy) || isinf(vvy))
-			    	{ 
-			    		vvy = 0;
-			        }
-
-			        if (norm([vvx,vvy])>0)
-			        {
-			                    aa=[vvx vvy];
-			                    vy(pty,ptx)=aa(1);
-			                    vx(pty,ptx)=aa(2);
-			        }
-	   			}
-			}
-		}  
-	   It = max(cat(3, It_pos, It_neg), [], 3);
-		 **/
 	}
 
 	void find(Mat &m, Mat &X)
@@ -184,6 +132,7 @@ namespace compute_flow
 		std::cout << "V: " << svd.matrixV() << std::endl;
 		//U = X * svd.matrixV() * S.inverse(); //check if diagnol(1/S) works
 		U = svd.matrixV();
+		std::cout<<"Checking orthonormality"<<U.transpose()*U<<std::endl;
 		ROS_INFO_STREAM("pca_modified()");
 		}
 	}