diff --git a/FremenServer/README.md b/FremenServer/README.md index 8e91b0e..d1cc5e0 100644 --- a/FremenServer/README.md +++ b/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. diff --git a/FremenServer/action/Fremen.action b/FremenServer/action/Fremen.action index e647838..3882224 100644 --- a/FremenServer/action/Fremen.action +++ b/FremenServer/action/Fremen.action @@ -1,11 +1,12 @@ #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 @@ -13,7 +14,7 @@ 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 diff --git a/FremenServer/src/CFrelement.cpp b/FremenServer/src/CFrelement.cpp index ac66108..1cba077 100644 --- a/FremenServer/src/CFrelement.cpp +++ b/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=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) diff --git a/FremenServer/src/CFrelement.h b/FremenServer/src/CFrelement.h index 953d054..cf00083 100644 --- a/FremenServer/src/CFrelement.h +++ b/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); diff --git a/FremenServer/src/CFrelementSet.cpp b/FremenServer/src/CFrelementSet.cpp index e818d33..d010e0f 100644 --- a/FremenServer/src/CFrelementSet.cpp +++ b/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; diff --git a/FremenServer/src/CFrelementSet.h b/FremenServer/src/CFrelementSet.h index fdebc71..91ae6ed 100644 --- a/FremenServer/src/CFrelementSet.h +++ b/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); diff --git a/FremenServer/src/fremenserver.cpp b/FremenServer/src/fremenserver.cpp index 7ae8541..9add31f 100644 --- a/FremenServer/src/fremenserver.cpp +++ b/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); } diff --git a/froctomap/CMakeLists.txt b/froctomap/CMakeLists.txt index a060e15..5dfbdc2 100644 --- a/froctomap/CMakeLists.txt +++ b/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 ## diff --git a/froctomap/src/convert_raw.cpp b/froctomap/src/convert_raw.cpp index a344986..1f81b30 100644 --- a/froctomap/src/convert_raw.cpp +++ b/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; diff --git a/froctomap/src/froctodump_show.cpp b/froctomap/src/froctodump_show.cpp new file mode 100644 index 0000000..08bd569 --- /dev/null +++ b/froctomap/src/froctodump_show.cpp @@ -0,0 +1,176 @@ +#include +#include +#include +#include +#include + +/*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;idata*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("/froctodump/recovered", 100); + markerFuck = n.advertise("/froctodump/test", 100); + ros::Subscriber retrieveTime = n.subscribe("/froctodump/retrieveTime", 1, timeCallback); + + + ros::spin(); + + return 0; +} diff --git a/froctomap/src/froctomap.cpp b/froctomap/src/froctomap.cpp index 9e8e9bc..8c0e7e9 100644 --- a/froctomap/src/froctomap.cpp +++ b/froctomap/src/froctomap.cpp @@ -25,9 +25,9 @@ #define LIM_MIN_X -2.6 #define LIM_MIN_Y -2.0 #define LIM_MIN_Z 0.0 -#define LIM_MAX_X 0.05 -#define LIM_MAX_Y 0.05 -#define LIM_MAX_Z 4.7 +#define LIM_MAX_X 0.050 +#define LIM_MAX_Y 0.050 +#define LIM_MAX_Z 4.70 #define MAX_GRIDS 10 using namespace std; @@ -301,93 +301,94 @@ bool recover_octomap(froctomap::Recover::Request &req, froctomap::Recover::Resp }else{ gridPtr = gridArray[currentMap]; } - - octomap_msgs::Octomap bmap_msg; - OcTree octree (resolution); - ROS_INFO("Service: recover stamp %.3f",req.stamp); - //Create pointcloud: - octomap::Pointcloud octoCloud; - sensor_msgs::PointCloud fremenCloud; - float x = 0*gridPtr->positionX; - float y = 0*gridPtr->positionY; - geometry_msgs::Point32 test_point; - int cnt = 0; - for(double i = LIM_MIN_X; i < LIM_MAX_X; i+=resolution){ - for(double j = LIM_MIN_Y; j < LIM_MAX_Y; j+=resolution){ - for(double w = LIM_MIN_Z; w < LIM_MAX_Z; w+=resolution){ - point3d ptt(x+i+resolution/2,y+j+resolution/2,w+resolution/2); - - if(gridPtr->retrieve(cnt, req.stamp)) - { - octoCloud.push_back(x+i+resolution/2,y+j+resolution/2,w+resolution/2); - octree.updateNode(ptt,true,true); - } - cnt++; - } - } - } - //Update grid - octree.updateInnerOccupancy(); - - //init visualization markers: - visualization_msgs::MarkerArray occupiedNodesVis; - unsigned int m_treeDepth = octree.getTreeDepth(); - - //each array stores all cubes of a different size, one for each depth level: - occupiedNodesVis.markers.resize(m_treeDepth + 1); - geometry_msgs::Point cubeCenter; - - std_msgs::ColorRGBA m_color; - m_color.r = 0.0; - m_color.g = 0.0; - m_color.b = 1.0; - m_color.a = 1.0; - - for (unsigned i = 0; i < occupiedNodesVis.markers.size(); ++i) - { - double size = octree.getNodeSize(i); - occupiedNodesVis.markers[i].header.frame_id = "/map"; - occupiedNodesVis.markers[i].header.stamp = ros::Time::now(); - occupiedNodesVis.markers[i].ns = "map"; - occupiedNodesVis.markers[i].id = i; - occupiedNodesVis.markers[i].type = visualization_msgs::Marker::CUBE_LIST; - occupiedNodesVis.markers[i].scale.x = size; - occupiedNodesVis.markers[i].scale.y = size; - occupiedNodesVis.markers[i].scale.z = size; - occupiedNodesVis.markers[i].color = m_color; - } - x = gridPtr->positionX; - y = gridPtr->positionY; - for(OcTree::leaf_iterator it = octree.begin_leafs(), end = octree.end_leafs(); it != end; ++it) - { - if(it != NULL && octree.isNodeOccupied(*it)) - { - unsigned idx = it.getDepth(); - cubeCenter.x = x+it.getX(); - cubeCenter.y = y+it.getY(); - cubeCenter.z = 1.45+it.getZ(); - occupiedNodesVis.markers[idx].points.push_back(cubeCenter); - double minX, minY, minZ, maxX, maxY, maxZ; - octree.getMetricMin(minX, minY, minZ); - octree.getMetricMax(maxX, maxY, maxZ); - double h = (1.0 - fmin(fmax((cubeCenter.z - minZ) / (maxZ - minZ), 0.0), 1.0)) * m_colorFactor; - occupiedNodesVis.markers[idx].colors.push_back(heightMapColor(h)); - } - } - octomap_msgs::binaryMapToMsg(octree, bmap_msg); - - fremenCloud.header.frame_id = "/map"; - fremenCloud.header.stamp = ros::Time::now(); - - octomap_pub_ptr->publish(bmap_msg); - retrieve_pub_ptr->publish(occupiedNodesVis); - - ROS_INFO("Octomap published!"); - - res.result = true; + octomap_msgs::Octomap bmap_msg; + OcTree octree (resolution); + ROS_INFO("Service: recover stamp %.3f",req.stamp); + + //Create pointcloud: + octomap::Pointcloud octoCloud; + sensor_msgs::PointCloud fremenCloud; + float x = 0*gridPtr->positionX; + float y = 0*gridPtr->positionY; + geometry_msgs::Point32 test_point; + int cnt = 0; + for(double i = LIM_MIN_X; i < LIM_MAX_X; i+=resolution){ + for(double j = LIM_MIN_Y; j < LIM_MAX_Y; j+=resolution){ + for(double w = LIM_MIN_Z; w < LIM_MAX_Z; w+=resolution){ + point3d ptt(x+i+resolution/2,y+j+resolution/2,w+resolution/2); + + if(gridPtr->retrieve(cnt, req.stamp)) + { + octoCloud.push_back(x+i+resolution/2,y+j+resolution/2,w+resolution/2); + octree.updateNode(ptt,true,true); + } + cnt++; + } + } + } + printf("Nodes: %i\n",cnt); + //Update grid + octree.updateInnerOccupancy(); - return true; + //init visualization markers: + visualization_msgs::MarkerArray occupiedNodesVis; + unsigned int m_treeDepth = octree.getTreeDepth(); + + //each array stores all cubes of a different size, one for each depth level: + occupiedNodesVis.markers.resize(m_treeDepth + 1); + geometry_msgs::Point cubeCenter; + + std_msgs::ColorRGBA m_color; + m_color.r = 0.0; + m_color.g = 0.0; + m_color.b = 1.0; + m_color.a = 1.0; + + for (unsigned i = 0; i < occupiedNodesVis.markers.size(); ++i) + { + double size = octree.getNodeSize(i); + occupiedNodesVis.markers[i].header.frame_id = "/map"; + occupiedNodesVis.markers[i].header.stamp = ros::Time::now(); + occupiedNodesVis.markers[i].ns = "map"; + occupiedNodesVis.markers[i].id = i; + occupiedNodesVis.markers[i].type = visualization_msgs::Marker::CUBE_LIST; + occupiedNodesVis.markers[i].scale.x = size; + occupiedNodesVis.markers[i].scale.y = size; + occupiedNodesVis.markers[i].scale.z = size; + occupiedNodesVis.markers[i].color = m_color; + } + x = gridPtr->positionX; + y = gridPtr->positionY; + for(OcTree::leaf_iterator it = octree.begin_leafs(), end = octree.end_leafs(); it != end; ++it) + { + if(it != NULL && octree.isNodeOccupied(*it)) + { + unsigned idx = it.getDepth(); + cubeCenter.x = x+it.getX(); + cubeCenter.y = y+it.getY(); + cubeCenter.z = 1.45+it.getZ(); + occupiedNodesVis.markers[idx].points.push_back(cubeCenter); + double minX, minY, minZ, maxX, maxY, maxZ; + octree.getMetricMin(minX, minY, minZ); + octree.getMetricMax(maxX, maxY, maxZ); + double h = (1.0 - fmin(fmax((cubeCenter.z - minZ) / (maxZ - minZ), 0.0), 1.0)) * m_colorFactor; + occupiedNodesVis.markers[idx].colors.push_back(heightMapColor(h)); + } + } + octomap_msgs::binaryMapToMsg(octree, bmap_msg); + + fremenCloud.header.frame_id = "/map"; + fremenCloud.header.stamp = ros::Time::now(); + + octomap_pub_ptr->publish(bmap_msg); + retrieve_pub_ptr->publish(occupiedNodesVis); + + ROS_INFO("Octomap published!"); + + res.result = true; + + return true; } @@ -628,25 +629,25 @@ int main(int argc,char *argv[]) CFFTPlan plan; CTimer timer; timer.start(); - plan.prepare(200000); + /*plan.prepare(200000); printf("%i\n",timer.getTime()); timer.reset(); plan.prepare(200000); printf("%i\n",timer.getTime()); - return 0; + return 0;*/ ros::init(argc, argv, "froctomap"); ros::NodeHandle n; - ros::NodeHandle nh("~"); - nh.param("resolution", resolution, 0.1); - nh.param("colorFactor", m_colorFactor, 0.8); - nh.param("filename", loadfilename, ""); + //ros::NodeHandle n("~"); + n.param("resolution", resolution, 0.05); + n.param("colorFactor", m_colorFactor, 0.8); + n.param("filename", loadfilename, ""); strcpy(mapNode,"Greg_office"); //Fremen Grid: - gridPtr = NULL; -/* if (strlen(loadfilename.c_str()) > 0){ + /*gridPtr = NULL; + if (strlen(loadfilename.c_str()) > 0){ numMaps = 0; printf("attempt to load\n"); gridArray[numMaps++] = new CFremenGrid(DIM_X,DIM_Y,DIM_Z); diff --git a/froctomap/src/froctomap_util.cpp b/froctomap/src/froctomap_util.cpp index f16d90a..2852c2a 100644 --- a/froctomap/src/froctomap_util.cpp +++ b/froctomap/src/froctomap_util.cpp @@ -6,6 +6,39 @@ using namespace std; +int generateMaps(const char* name,const char *output,int startTime,int endTime,int interval) +{ + /*load the fremen grid*/ + CFremenGrid *grid = new CFremenGrid(1,1,1); + grid->load(name); + + /*prepare output*/ + long int numTimes = (endTime-startTime)/interval; + long int gridSize = grid->numCells; + unsigned char* states = (unsigned char*) malloc(gridSize*numTimes); + + if (states == NULL) + { + printf("Could not allocate %ld MB of memory.\n",gridSize*numTimes/1024/1024); + return -1; + } + + /*perform retrieval*/ + long int cnt = 0; + for (long int i = 0;iretrieve(s, i*interval+startTime); + printf("3D Grid %ld of %ld generated !\n",i,numTimes); + } + + /*save it*/ + FILE* file = fopen(output,"w+"); + fwrite(states,sizeof(unsigned char),gridSize*numTimes,file); + fclose(file); + printf("File %s saved.\n",output); + delete grid; + free(states); +} + int main(int argc,char *argv[]) { unsigned char *signal = (unsigned char*)malloc(10000000); @@ -15,7 +48,11 @@ int main(int argc,char *argv[]) CTimer timer; timer.start(); CFremenGrid grid(1,1,1); - if (strcmp(argv[1],"retrieve")==0){ + if (strcmp(argv[1],"reconstruct")==0) + { + generateMaps(argv[2],argv[3],atoi(argv[4]),atoi(argv[5]),atoi(argv[6])); + } + else if (strcmp(argv[1],"retrieve")==0){ grid.load(argv[2]); grid.reconstruct(atoi(argv[1]),reconstructed); for (int i=0;i