Froctomap improvements

FremenServer/README.md

@@ -98,3 +98,19 @@ Deletes a state with a given **id** from the state collection held in the server
 
 ###The 'update' operation 
 Reserved for future use when FreMEn is fused with Gaussian Mixture Models.
+
+###The 'detect' operation (TODO)
+The **detect** operation is meant to detect anomalous observations based on their discrepancy with the model of **order** given a **confidence** level. The user provides a sequence of observed **values** and **times** of observations of a state **id**, model **order** and a **confidence** level. The server performs predictions for the given **times** and **orders**, compares those predictions with the provided **values** and returns **anomTimes**, when the observed states did not match the predictions on a given confidence **level**. Anomaly is defined as a situation where |p(t)-s(t)| > c, where *p(t)* is the prediction, *s(t)* is the observation and *c* is the confidence level.
+
+####Inputs
+- **id** identification of the state that is concerned. If the **id** did not exist, an error is reported.
+- **values** is a sequence of zeros and ones indicating the observed states at  
+- **times** which are in seconds. The length of the fields **times** and **values** has to be the same.
+- **order** is the model order to be used for predictions, 
+- **confidence** is the confidence level considered. 
+
+####Outputs
+- **anomTimes** contains the times of detected anomalies,
+- **anomValues** contains the values of detected anomalies,
+- **values** contains the values predicted for the given **times**,
+- **message** contains a detailed report or an error message.

FremenServer/action/Fremen.action

@@ -1,19 +1,20 @@
 #goal definition
-string   operation 	#the operation can be 'add', 'addvalues', 'predict', 'forecast', 'entropy', 'evaluate', 'update' and 'delete'
+string   operation 	#the operation can be 'add', 'addvalues', 'predict', 'forecast', 'entropy', 'evaluate', 'detect', 'update', 'delete'
 			#'add' adds a sequence of given (by ID) state measurements and the times these have been obtained to the state's model, if ID is given for the first time, a new state is created and added to the model
 			#'addvalues' adds a sequence of given (by ID) value measurements and the times these have been obtained to the state's model, if ID is given for the first time, a new state is created and added to the model
 			#'predict'  calculates a sequence of probabilities of the state ID for the given times (i.e. prediction of one state for multiple time instants)
 			#'forecast'  calculates a sequence of probabilities of the states with given 'ids' for the given time (i.e. prediction of mutiple states for a single time instant)
 			#'entropy'  calculates a sequence of entropies of the state ID for the given times - this is to support exploration
 			#'evaluate' predicts the state for the given times, compares the prediction with ground truth and returns the prediction errors for model orders from 0 to the order specified. Meant to help to decide which order to use for the 'predict' action. 
+			#'detect' detects anomalies by predicting the state for the given times, comparing with ground truth and flagging up discrepancies that were beyond a certain 'confidence' level 
 			#'delete' removes the current state from the collection of states   
 			#'update' is reserved, will be used int the future for recalculating the models
 
 string  id	  	#specifies the ID of the state to be manipulated 
 string[]  ids	  	#specifies the IDs of the states to be manipulated - explusively fot the 'forecast' action
 uint32[]  times 	#the time instants for the 'predict', 'add', 'entropy' and 'evaluate' actions
 
-#specific for the 'predict' and 'evaluate' actions
+#specific for the 'predict', 'evaluate' and 'detect' actions
 int32    order 		#number of periodic processes to use for the purpose of prediction (0 means a static model, typical value is 2), maximal order used for evaluation of the 'evaluate' action
 
 #specific for the 'forecast' actions
@@ -23,13 +24,18 @@ int32[]  orders	  	#same as previous one, but for all states in the **ids**
 bool[]      states   	#sequence of states observed at the particular times - specific to the 'add' action 	(this is for backward compatibility)
 float32[]   values   	#sequence of values observed at the particular times - specific to the 'addvalues' action, (I recommend to normalize the values so they fit 0-1)
 
+#specific for the 'detect' 
+float32   confidence   	#confidence level for the anomaly detection
+
 ---
 #result definition
 int32 success		#action result - positive value indicates success, negative indicates error
 string message 		#detailed message
-float32[] probabilities	#probabilities of the states calculated by the 'predict' action for the particular 'times'
+float32[] probabilities	#probabilities of the states calculated by the 'predict' action for the particular 'times', probability of a given observation for the 'detect' action
 float32[] entropies	#entropies of the states calculated by the 'entropy' action for the particular 'times'
 float32[] errors 	#prediction errors for the various model orders given by the 'evaluate' action
+uint32[]  anomalyTimes 	#the time instants of detected anomalies 
+uint32[]  anomalyValues	#the observed anomaous states 
 ---
 #feedback
 string status

FremenServer/src/CFrelement.cpp

@@ -135,6 +135,25 @@ int CFrelement::evaluate(uint32_t* times,unsigned char* signal,int length,int or
 	return index;
 }
 
+int CFrelement::detect(uint32_t *times,float *probs,int length,int order,float confidence,uint32_t *anomTimes,float *anomVals)
+{
+	float *estimates = (float*)malloc(length*sizeof(float));
+	estimate(times,estimates,length,order);
+	int anomCount = 0;
+	for (int i = 0;i<length;i++)
+	{
+		if (fabs(probs[i] - estimates[i]) >=confidence)
+		{
+			anomTimes[anomCount] = times[i];
+			anomVals[anomCount] = probs[i];
+			anomCount++;
+		}
+		probs[i] = estimates[i];
+	}
+	delete estimates;
+	return anomCount;
+}
+
 
 /*not required in incremental version*/
 void CFrelement::update(int modelOrder)

FremenServer/src/CFrelement.h

@@ -47,6 +47,8 @@ class CFrelement
 		//evaluates the error of the predictions for the given times and measurements
 		int evaluate(uint32_t* times,unsigned char* signal,int length,int orderi,float* evals);
 
+		//performs anomaly detection on the data provided 
+		int detect(uint32_t *times,float *probs,int length,int order,float confidence,uint32_t *anomTimes,float *anomVals);
 		void update(int modelOrder);
 		void print(int order);
 

FremenServer/src/CFrelementSet.cpp

@@ -32,6 +32,12 @@ int CFrelementSet::evaluate(const char *name,uint32_t times[],unsigned char stat
 	return active->evaluate(times,states,length,order,errors);;
 }
 
+int CFrelementSet::detect(const char *name,uint32_t times[],float probs[],int length,int order,float confidence,uint32_t anomTimes[],float anomVals[])
+{
+	if (find(name) == false)return -1;
+	return active->detect(times,probs,length,order,confidence,anomTimes,anomVals);
+}
+
 int CFrelementSet::estimate(const char *name,uint32_t times[],float probs[],int length,int order)
 {
 	if (find(name) == false)return -1;

FremenServer/src/CFrelementSet.h

@@ -41,6 +41,11 @@ class CFrelementSet
 		  otherwise returns the best performing model order and the errors in the eval array*/
 		int evaluate(const char *name,uint32_t times[],unsigned char probs[],int length,int order,float eval[]);
 
+		/*detects anomalies in the data given provided
+		  returns -1 if the state with the given ID is not present in the collection
+		  otherwise returns the observation probabilities in the probs array and the detected anomalies in the anomTimes and anomVals arrays*/
+		int detect(const char *name,uint32_t times[],float probs[],int length,int order,float confidence,uint32_t anomTimes[],float anomVals[]);
+
 		/*remove states from the collection
 		  return the number of remaining states*/
 		int remove(const char *name);

FremenServer/src/fremenserver.cpp

@@ -55,6 +55,30 @@ void actionServerCallback(const fremenserver::FremenGoalConstPtr& goal, Server*
 			server->setAborted(result);
 		}
 	}
+	else if (goal->operation == "detect")
+	{
+		if (goal->times.size() == goal->values.size()){
+			float anomVals[goal->values.size()];
+			uint32_t anomTimes[goal->values.size()];
+			result.success = frelements.detect(goal->id.c_str(),(uint32_t*)goal->times.data(),(float*)goal->values.data(),(int)goal->values.size(),goal->order,goal->confidence,anomTimes,anomVals);
+			if (result.success >=0)
+			{
+				mess << "Detected " << result.success << " anomalies in " << (int)goal->values.size() << " provided measurements to the state " << goal->id;
+				result.message = mess.str(); 
+			}else{
+				mess << "The state " <<  goal->id << " does not exist in the collection of states.";
+				result.message = mess.str(); 
+			}
+			result.anomalyTimes.assign(anomTimes,anomTimes + result.success);
+			result.anomalyValues.assign(anomVals,anomVals + result.success);
+			server->setSucceeded(result);
+		}else{
+			mess << "The length of the 'states' and 'times' arrays does not match.";
+			result.message = mess.str(); 
+			result.success = -2;
+			server->setAborted(result);
+		}
+	}
 	else if (goal->operation == "add")
 	{
 		if (goal->times.size() == goal->states.size()){
@@ -117,6 +141,7 @@ void actionServerCallback(const fremenserver::FremenGoalConstPtr& goal, Server*
 			}
 		}else{
 			mess << "The length of the 'states' and 'times' arrays does not match.";
+			result.message = mess.str(); 
 			result.success = -2;
 			server->setAborted(result);
 		}

froctomap/CMakeLists.txt

@@ -4,7 +4,7 @@ project(froctomap)
 
 find_package(catkin REQUIRED COMPONENTS  pcl_conversions pcl_ros roscpp rospy std_msgs visualization_msgs geometry_msgs genmsg octomap_server sensor_msgs)
 #set ( CMAKE_CXX_FLAGS "-O3 -march=native")
-set ( CMAKE_CXX_FLAGS "-ggdb")
+set ( CMAKE_CXX_FLAGS "-ggdb -D_FILE_OFFSET_BITS=64" )
 
 
 
@@ -39,6 +39,7 @@ catkin_package()
 add_executable(froctomap src/froctomap.cpp)
 add_executable(convert_raw src/convert_raw.cpp)
 add_executable(froctomap_util src/froctomap_util.cpp)
+add_executable(froctodump_show src/froctodump_show.cpp)
 
 add_dependencies(froctomap ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp)
 
@@ -55,6 +56,7 @@ target_link_libraries(froctomap froctomap_grid)
 target_link_libraries(froctomap_util froctomap_grid)
 target_link_libraries(convert_raw ${catkin_LIBRARIES})
 target_link_libraries(froctomap ${catkin_LIBRARIES})
+target_link_libraries(froctodump_show ${catkin_LIBRARIES})
 
 #############
 ## Install ##

froctomap/src/convert_raw.cpp

@@ -94,8 +94,8 @@ int main(int argc, char** argv)
 			  depth_image.data[2*i+1] = (buffer[i])/256;
 		  }
 
-		  //ros::Time test(data_timestamp/1000000,data_timestamp%1000000);
-		  ros::Time test = ros::Time::now();
+		  ros::Time test(data_timestamp/1000000,data_timestamp%1000000);
+		  //ros::Time test = ros::Time::now();
 		  depth_image.header.stamp = test; //ros::Time(,):
 		  camera_info.header.stamp = test;
 

froctomap/src/froctodump_show.cpp

@@ -0,0 +1,176 @@
+#include <ros/ros.h>
+#include <std_msgs/Int32.h>
+#include <std_msgs/ColorRGBA.h>
+#include <visualization_msgs/Marker.h>
+#include <octomap/octomap.h>
+
+/*delimit the OctoMap dimensions here */
+#define DIM_X 54 
+#define DIM_Y 42
+#define DIM_Z 94 
+
+using namespace std;
+using namespace octomap;
+
+long int times;
+unsigned char *grid;
+long int gridSize = DIM_X*DIM_Y*DIM_Z;
+ros::Subscriber retrieveTime;
+ros::Publisher markerPub;
+ros::Publisher markerFuck;
+std_msgs::Int32 kokot;
+
+//Parameters
+double resolution, m_colorFactor;
+string filename;
+string loadfilename;
+
+std_msgs::ColorRGBA heightMapColor(double h)
+{
+	std_msgs::ColorRGBA color;
+	color.a = 1.0;
+	color.g = h;	
+	color.r = 1-h;	
+	color.b = 0;	
+	return color;
+}
+
+std_msgs::ColorRGBA heightMapColorA(double h){
+
+  std_msgs::ColorRGBA color;
+  color.a = 1.0;
+  // blend over HSV-values (more colors)
+
+  double s = 1.0;
+  double v = 1.0;
+
+  h -= floor(h);
+  h *= 6;
+  int i;
+  double m, n, f;
+
+  i = floor(h);
+  f = h - i;
+  if (!(i & 1)) f = 1 - f; // if i is even
+  m = v * (1 - s);
+  n = v * (1 - s * f);
+
+  switch (i) {
+    case 6:
+    case 0:
+      color.r = v;
+      color.g = n;
+      color.b = m;
+      break;
+    case 1:
+      color.r = n;
+      color.g = v;
+      color.b = m;
+      break;
+    case 2:
+      color.r = m;
+      color.g = v;
+      color.b = n;
+      break;
+    case 3:
+      color.r = m;
+      color.g = n;
+      color.b = v;
+      break;
+    case 4:
+      color.r = n;
+      color.g = m;
+      color.b = v;
+      break;
+    case 5:
+      color.r = v;
+      color.g = m;
+      color.b = n;
+      break;
+    default:
+      color.r = 1;
+      color.g = 0.5;
+      color.b = 0.5;
+      break;
+
+  }
+
+  return color;
+}
+
+void timeCallback(const std_msgs::Int32::ConstPtr& msg)
+{
+	printf("Data: %i\n",msg->data);
+	//init visualization markers:
+	visualization_msgs::Marker markers;
+	geometry_msgs::Point cubeCenter;
+	std_msgs::ColorRGBA m_color;
+
+	//set type, frame and size
+	markers.header.frame_id = "/map";
+	markers.header.stamp = ros::Time::now();
+	markers.ns = "Markers";
+	markers.action = visualization_msgs::Marker::ADD;
+	markers.type = visualization_msgs::Marker::CUBE_LIST;
+	markers.scale.x = resolution;
+	markers.scale.y = resolution;
+	markers.scale.z = resolution;
+	markers.color = m_color;
+	markers.points.clear();
+	markers.colors.clear();
+
+	// prepare to iterate over the entire grid
+	for (int i = 0;i<gridSize;i++)
+	{
+		if (grid[i+msg->data*gridSize] == 1){
+			cubeCenter.x = -(i%(DIM_Z+1))*resolution;
+			cubeCenter.y = (i/(DIM_Y-1)/(DIM_Z+1))*resolution;
+			cubeCenter.z = 3.0-((i/(DIM_Z+1))%(DIM_Y-1))*resolution;
+			m_color.r = 0.0;
+			m_color.b = cubeCenter.z/3.0;
+			m_color.g = 1.0 - m_color.b;
+			m_color.a = 1.0;
+			markers.points.push_back(cubeCenter);
+			markers.colors.push_back(m_color);
+		}
+	}
+	//publish results
+	markerPub.publish(markers);
+}
+
+int main(int argc,char *argv[])
+{
+	//*read grid
+	FILE* file = fopen(argv[1],"r");
+	fseek(file,0,SEEK_END);
+	long int fileSize = ftell(file);
+	printf("File %s of size %ld loaded.\n",argv[1],fileSize);
+	long int numTimes = fileSize/gridSize;
+	if (fileSize%gridSize != 0) printf("Grid file size is not divisible by %ld, file probably corrupted.\n",gridSize);
+	grid = (unsigned char*) malloc(fileSize);
+	fseek(file,0,0);
+	fread(grid,sizeof(unsigned char),gridSize*numTimes,file);
+	fclose(file);
+
+	/*plan.prepare(200000);
+	  printf("%i\n",timer.getTime());
+	  timer.reset();
+	  plan.prepare(200000);
+	  printf("%i\n",timer.getTime());
+	  return 0;*/
+
+	ros::init(argc, argv, "froctomap");
+	ros::NodeHandle n;
+
+	//ros::NodeHandle n("~");
+	n.param("resolution", resolution, 0.05);
+
+	markerPub = n.advertise<visualization_msgs::Marker>("/froctodump/recovered", 100);
+	markerFuck = n.advertise<std_msgs::Int32>("/froctodump/test", 100);
+	ros::Subscriber retrieveTime = n.subscribe("/froctodump/retrieveTime", 1, timeCallback);
+
+
+	ros::spin();
+
+	return 0;
+}