merge

README.md

@@ -5,7 +5,7 @@ Using emergency maps to help robots save you in emergencies
 
 ## Description
 
-The goal of this porgram is described [in this blog post](https://malcolmmielle.wordpress.com/2017/08/07/using-emergency-maps-to-help-robots-save-you-in-emergencies/) and can be summarised by this sentence:
+The goal of this program is described [in this blog post](https://malcolmmielle.wordpress.com/2017/08/07/using-emergency-maps-to-help-robots-save-you-in-emergencies/) and can be summarized by this sentence:
 
 > a method to integrate an emergency map into a robot map, so that the robot can plan its way toward places it has not yet explored.
 
@@ -26,11 +26,11 @@ This should all be soon ros parameters but for now here is how you can test the
 
 * Use `rosrun auto_complete_graph acg_node_review` to run the algorithm.
 
-* In Rviz, one can give an approximation of the position of the prior compared to robot map using the Publish point button. Only two links are needed to initialise.
+* In Rviz, one can give an approximation of the position of the prior compared to robot map using the Publish point button. Only two links are needed to initialize.
 
 ### How to use your own prior
 
-The class `PriorLoaderInterface.hpp` is used to load prior image and detected corners. Just inherite from this class to create your own prior loader. See an example in the class `basementFull.hpp`. This only argument needed for PriorLoaderInterface is the name of the file image where the prior is.
+The class `PriorLoaderInterface.hpp` is used to load prior image and detected corners. Just inherit from this class to create your own prior loader. See an example in the class `basementFull.hpp`. This only argument needed for PriorLoaderInterface is the name of the file image where the prior is.
 
 ## Dependencies
 

include/auto_complete_graph/OptimizableAutoCompleteGraph.hpp

@@ -25,8 +25,8 @@ namespace acg{
 		typedef g2o::LinearSolverCSparse<SlamBlockSolver::PoseMatrixType> SlamLinearSolver;
 	protected:
 // 		g2o::SparseOptimizer _optimizer;
-		std::unique_ptr<SlamLinearSolver> _linearSolver;
-		std::unique_ptr<SlamBlockSolver> _blockSolver;
+// 		std::unique_ptr<SlamLinearSolver> _linearSolver;
+// 		std::unique_ptr<SlamBlockSolver> _blockSolver;
 		//TODO : test with LevenbergMarquard instead (maybe doesn't scale as aggressively)
 		g2o::OptimizationAlgorithmGaussNewton* _solver;
 		g2o::SE2 _sensorOffsetTransf;
@@ -51,10 +51,10 @@ namespace acg{
 // 				const Eigen::Vector2d& linkn
 									) : _sensorOffsetTransf(sensoffset), _first(true){
 
-			_linearSolver = g2o::make_unique<SlamLinearSolver>();
-			_linearSolver->setBlockOrdering(false);
-			_blockSolver = g2o::make_unique<SlamBlockSolver>(std::move(_linearSolver));
-			_solver = new g2o::OptimizationAlgorithmGaussNewton(std::move(_blockSolver));
+			auto linearSolver = g2o::make_unique<SlamLinearSolver>();
+			linearSolver->setBlockOrdering(false);
+			auto blockSolver = g2o::make_unique<SlamBlockSolver>(std::move(linearSolver));
+			_solver = new g2o::OptimizationAlgorithmGaussNewton(std::move(blockSolver));
 			//_linearSolver->setBlockOrdering(false);
 			this->setAlgorithm(_solver);
 
@@ -114,8 +114,8 @@ namespace acg{
 
 // 		AutoCompleteGraph* getGraph(){return _graph;}
 		g2o::ParameterSE2Offset* getSensorOffset(){return _sensorOffset;}
-		std::unique_ptr<SlamLinearSolver>& getLinearSolver(){ return _linearSolver;}
-		std::unique_ptr<SlamBlockSolver>& getBlockSolver(){ return _blockSolver;}
+// 		std::unique_ptr<SlamLinearSolver>& getLinearSolver(){ return _linearSolver;}
+// 		std::unique_ptr<SlamBlockSolver>& getBlockSolver(){ return _blockSolver;}
 // 		const g2o::SparseOptimizer& getoptimizer(){return _optimizer;}
 		g2o::OptimizationAlgorithmGaussNewton* getSolver() {return _solver;}
 		const g2o::SE2& getSensorOffsetTransfo(){return _sensorOffsetTransf;}

include/auto_complete_graph/PriorLoaderInterface.hpp

@@ -1,6 +1,8 @@
 #ifndef AUTOCOMPLETEGRAPH_PRIORLOADERINTERFACE_16112016
 #define AUTOCOMPLETEGRAPH_PRIORLOADERINTERFACE_16112016
 
+#include <random>
+
 #include "bettergraph/PseudoGraph.hpp"
 #include "vodigrex/linefollower/SimpleNode.hpp"
 #include "vodigrex/linefollower/LineFollowerGraphCorners.hpp"
@@ -98,10 +100,12 @@ namespace AASS{
 			 * @param[in] pt_slam : points from the robot slam map
 			 * @param[in] pt_prior : equivalent points from the robot slam map
 			 */
+
 			void initialize(const std::vector<cv::Point2f>& pt_slam, const std::vector<cv::Point2f>& pt_prior);			
 
 		protected:
 
+
 			void rotateGraph(const cv::Mat& rot_mat );
 
 			void AffineTransformGraph(const cv::Mat& warp_transfo );

src/PriorLoaderInterface.cpp

@@ -79,6 +79,7 @@ void AASS::acg::PriorLoaderInterface::transformOntoSLAM()
 
 void AASS::acg::PriorLoaderInterface::extractCornerPrior()
 {
+
 
 	_prior_graph.clear();
 
@@ -97,9 +98,9 @@ void AASS::acg::PriorLoaderInterface::extractCornerPrior()
 	//Very convulted code TODO
 	AASS::acg::PriorLoaderInterface::PriorGraph simple_graph;
 	bettergraph::toSimpleGraph<PriorAttr, AASS::vodigrex::SimpleEdge>(prior_out, simple_graph);
-
-
+
 	_prior_graph = simple_graph;
+
 
 }
 
@@ -158,9 +159,10 @@ void AASS::acg::PriorLoaderInterface::initialize(const std::vector< cv::Point2f
 
 	_same_point_prior.push_back(pt_prior[1]);                   
 	_same_point_slam.push_back(slam_point);
-	
+
 }
 
+
 void AASS::acg::PriorLoaderInterface::rotateGraph(const cv::Mat& rot_mat)
 {	
 	auto transf = [](const cv::Mat& rot_mat, const cv::Point2d point) -> cv::Point2d {

src/acg_node_review.cpp

@@ -1,6 +1,14 @@
 #include <ros/ros.h>
 #include <std_msgs/Bool.h>
 
+#include <sys/time.h>
+#include <sys/stat.h>
+
+#include <boost/filesystem.hpp>
+#include <boost/iterator/filter_iterator.hpp>
+#include <boost/filesystem/path.hpp>
+
+
 #include "ndt_feature/NDTGraphMsg.h"
 #include "ndt_feature/ndt_feature_graph.h"
 #include "ndt_feature/ndtgraph_conversion.h"
@@ -20,15 +28,82 @@ ros::Publisher occ_send;
 
 ros::Time timef;
 
-std::vector<double> all_node_times;
-double node_process_time = 0;
-int cycles = 0;
-
 int count = 0;
 
 bool new_node = false;
 bool was_init = false;
 
+double node_process_time = 0;
+int cycles = 0;
+
+std::vector<double> time_extract_corner_ndt;
+std::vector<double> time_opti;
+std::vector<double> all_node_times;
+
+inline bool exists_test3 (const std::string& name) {
+	struct stat buffer;   
+	return (stat (name.c_str(), &buffer) == 0); 
+}
+
+
+inline void exportResultsGnuplot(const std::string& file_out){
+
+		/*** TIMES ***/
+// 	std::vector<double> time_extract_corner_ndt;
+// 	std::vector<double> time_opti;
+// 	std::vector<double> all_node_times;
+
+	boost::filesystem::path p(file_out);
+	std::string name = p.filename().stem().string();
+
+	std::string result_file = file_out;
+	std::ofstream myfile;
+	myfile.open (result_file, std::ios::out | std::ios::app);
+
+	if (myfile.is_open())
+	{
+		myfile << "Full time\n";
+
+		double mean_times = 0 ;
+		for(auto it = all_node_times.begin() ; it != all_node_times.end() ; ++it){
+			mean_times += *it;
+			myfile << *it << "\n";
+		}
+		mean_times = mean_times / all_node_times.size();
+
+		myfile << "\nExtract time\n";
+
+		double mean_extract = 0 ;
+		for(auto it = time_extract_corner_ndt.begin() ; it != time_extract_corner_ndt.end() ; ++it){
+			mean_extract += *it;
+			myfile << *it << "\n";
+		}
+		mean_extract = mean_extract / time_extract_corner_ndt.size();
+
+		myfile << "\nOpti time\n";
+
+		double mean_opti = 0 ;
+		for(auto it = time_opti.begin() ; it != time_opti.end() ; ++it){
+			mean_opti += *it;
+			myfile << *it << "\n";
+		}
+		mean_opti = mean_opti / time_opti.size();
+
+		myfile << "\nMean time, mean extract, mean opti\n" << mean_times << " " << mean_extract << " " << mean_opti << "\n";
+		myfile.close();
+	}
+	else std::cout << "Unable to open file";
+}
+
+inline double getTime() //in millisecond
+{
+	//assuming unix-type systems
+	//timezone tz;
+	timeval  tv;
+	gettimeofday(&tv, NULL);
+	return (tv.tv_sec*1000000+tv.tv_usec)*1.0/1000;
+}
+
 inline void printImages(AASS::acg::AutoCompleteGraph* oacg){
 	nav_msgs::OccupancyGrid* omap_tmpt = new nav_msgs::OccupancyGrid();
 	nav_msgs::OccupancyGrid::Ptr occ_outt(omap_tmpt);
@@ -148,8 +223,12 @@ void gotGraphandOptimize(const ndt_feature::NDTGraphMsg::ConstPtr msg, AASS::acg
 		//ATTENTION: THE BAD GUY !
 		ndt_feature::msgToNDTGraph(*msg, graph, frame);	
 
-
+
+		ros::Time start_corner = ros::Time::now();
 		oacg->updateNDTGraph(graph);
+		ros::Time end_corner = ros::Time::now();	
+		double corner_extract_tt = (start_corner - end_corner).toSec();
+		time_extract_corner_ndt.push_back(corner_extract_tt);
 
 		if(oacg->getRobotNodes().size() > 0){
 
@@ -158,6 +237,7 @@ void gotGraphandOptimize(const ndt_feature::NDTGraphMsg::ConstPtr msg, AASS::acg
 			oacg->optimize();
 			count++;
 
+
 			visu.updateRviz();
 
 
@@ -166,7 +246,7 @@ void gotGraphandOptimize(const ndt_feature::NDTGraphMsg::ConstPtr msg, AASS::acg
 			node_process_time = node_process_time + (timef - start).toSec();
 			all_node_times.push_back((timef - start).toSec());
 			cycles++;
-				
+
 
 		}	
 	}
@@ -300,6 +380,7 @@ int main(int argc, char **argv)
 
 	}
 
+	exportResultsGnuplot("result.txt");
 
     return 0;
 }