added feature to save pointclouds and removed blank spaces in evaluation output files

graph_localization/scripts/private/graph_arla_submap_localization

 roslaunch graph_map visualize_graph_fuser.launch localization:=true arla-2012:=true &
-rosrun graph_localization graph_localization_offline --visualize --visualize-map  --map-switching-method overlap --keyframe-min-distance 0.2 --keyframe-min-rot-deg 2 --key-frame-update --disable-unwarp --tf-base-link /odom_base_link --tf-gt-link /state_base_link --forceSIR  --n-particles 450  --localisation-algorithm-name reg_localisation_type --skip-frame 1 --base-name mcl-ndt --bag-file-path $BAG_LOCATION/arla_bags/mapping/2012-09-05-09-44-50_0.bagfix.bag_edited.bag --map-file-path /home/daniel/.ros/maps/arla/offarla-2012_gt=1_submap=1_sizexy=120_Z=15_intrchR=10_compR=0_res=0.4_maxSensd=130_keyF=1_d=0.6_deg=5_alpha=0_dl=0_xyzir=0_mpsu=0_mnpfg=6kmnp0.map   --data-set arla-2012 --velodyne-config-file "$(rospack find graph_map)/config/velo32.yaml" --z-filter-height -1000000   resolution-local-factor 1.5 #--save-results
+rosrun graph_localization graph_localization_offline --visualize --visualize-map  --map-switching-method overlap --keyframe-min-distance 0.2 --keyframe-min-rot-deg 2 --key-frame-update --disable-unwarp --tf-base-link /odom_base_link --tf-gt-link /state_base_link --forceSIR  --n-particles 450  --localisation-algorithm-name reg_localisation_type --skip-frame 1 --base-name mcl-ndt --bag-file-path $BAG_LOCATION/arla_bags/mapping/2012-09-05-09-44-50_0.bagfix.bag_edited.bag --map-file-path /home/daniel/.ros/maps/arla/offarla-2012_gt=1_submap=1_sizexy=120_Z=15_intrchR=10_compR=0_res=0.4_maxSensd=130_keyF=1_d=0.6_deg=5_alpha=0_dl=0_xyzir=0_mpsu=0_mnpfg=6kmnp0.map    --data-set arla-2012 --velodyne-config-file "$(rospack find graph_map)/config/velo32.yaml" --z-filter-height -1000000   resolution-local-factor 1.5 #--save-results

graph_map/include/graph_map/graph_map_fuser.h

@@ -108,11 +108,11 @@ protected:
   RegTypePtr registrator_;
   MotionModel3d motion_model_3d_;
   plotmarker marker_;
-  ndt_generic::PointCloudQueue<pcl::PointXYZ> cloud_queue_;
   bool initialized_=false;
   bool visualize_=false;
 
   unsigned int nr_frames_=0;
+
   bool use_keyframe_=true;
   double min_keyframe_dist_=0.5;
   double min_keyframe_rot_deg_=15;

graph_map/include/graph_map/graph_map_fuser_impl.h

@@ -14,8 +14,6 @@ bool GraphMapFuser::ProcessFrame(pcl::PointCloud<PointT> &cloud, Eigen::Affine3d
  */
 template<class PointT>
 bool GraphMapFuser::ProcessFrame(pcl::PointCloud<PointT> &cloud, Eigen::Affine3d &Tnow,  Eigen::Affine3d &Tmotion, Eigen::MatrixXd &Tcov){
-  if(use_scanmatching)
-    return scanmatching(cloud,Tnow,Tmotion);
 
   if(!initialized_){
     cerr<<"fuser not initialized"<<endl;
@@ -39,19 +37,10 @@ bool GraphMapFuser::ProcessFrame(pcl::PointCloud<PointT> &cloud, Eigen::Affine3d
     if(graph_map_->GetCurrentNode()->GetMap()->Initialized()){
       graph_map_->WorldToLocalMapFrame(Tnow);
       registration_succesfull = registrator_->RegisterScan(graph_map_->GetCurrentNode()->GetMap(),Tnow,cloud,cov);//Tnow will be updated to the actual pose of the robot according to ndt-d2d registration
-      /* if(registration_succesfull){
-        std::string node_link_id=GetNodeLinkName( graph_map_->GetCurrentNode());
-        NDTMap *scan_ndt=boost::dynamic_pointer_cast<NDTD2DRegType>(registrator_)->GetRegisteredMap();
-        pcl::PointCloud<pcl::PointXYZ> cloud_xyz;
-        pcl::copyPointCloud(cloud, cloud_xyz);
-        GraphPlot::PlotNDT(scan_ndt, node_link_id, "ndt_scan_registeted",Tnow);
-        GraphPlot::PlotScan(cloud_xyz, node_link_id, "scan_registered", Tnow);
-      }*/
       graph_map_->LocalToWorldMapFrame(Tnow);
     }
     Eigen::Affine3d Tsensor_pose;
     Tsensor_pose=Tnow*sensorPose_;
-
     graph_map_->AutomaticMapInterchange(Tsensor_pose,motion_cov,map_node_changed,map_node_created);
   }
 
@@ -61,12 +50,14 @@ bool GraphMapFuser::ProcessFrame(pcl::PointCloud<PointT> &cloud, Eigen::Affine3d
     else
       UpdateSingleMap(cloud,Tnow);
 
-    if ( save_merged_cloud_){
-      pcl::PointCloud<pcl::PointXYZ> tmp_cloud;
-      copyPointCloud(cloud, tmp_cloud);
-      cloud_queue_.Push(tmp_cloud);
+   /* if ( save_merged_cloud_){
+      Eigen::Affine3d sens_in_world=Tnow*sensorPose_;
+      pcl::PointCloud<pcl::PointXYZ> tmp_transformed;
+      transformPointCloudInPlace(sens_in_world, tmp);
+      copyPointCloud(tmp,tmp_transformed);
+      cloud_queue_.Push(tmp_transformed);
       cloud_queue_.Save();
-    }
+    }*/
 
     graph_map_->m_graph.unlock();
     nr_frames_++;

graph_map/scripts/private/arla_2012_submap

 roslaunch graph_map visualize_graph_fuser.launch mapping:=true arla-2012:=true &
-rosrun graph_map graph_mapping_offline  --visualize --visualize-map  --T-map 5 --gt-mapping --disable-registration --disable-unwarp  --itrs 50 --consistency-max-dist 0.2 --consistency-max-rot 0.1 --interchange-radius 10 --compound-radius 0  --sensor-time-offset 0.012 --tf-base-link odom_base_link --tf-gt-link state_base_link  --resolution 0.4   --resolution-local-factor 1.0 --map-switching-method node_position   --min-range 1.6 --max-range 130 --base-name off --dir-name $BAG_LOCATION/arla_bags/mapping/ --lidar-topic /velodyne_packets --velodyne-frame-id /velodyne --map-type-name ndt_map  --velodyne-config-file "$(rospack find graph_map)/config/velo32.yaml" --output-dir-name /home/daniel/.ros/maps   --map-size-xy 120 --map-size-z 15.0 --skip-frame 1 --keyframe-min-distance 0.6 --keyframe-min-rot-deg 5   --data-set arla-2012   --save-map #--save-map     --generate-eval-files disable-keyframe-update --step-control  --save-used-merged-clouds --multi-res
+rosrun graph_map graph_mapping_offline  --generate-eval-files --save-used-merged-clouds --visualize --visualize-map  --T-map 5 --gt-mapping --disable-registration --disable-unwarp  --itrs 50 --consistency-max-dist 0.2 --consistency-max-rot 0.1 --interchange-radius 10 --compound-radius 0  --sensor-time-offset 0.012 --tf-base-link odom_base_link --tf-gt-link state_base_link  --resolution 0.4   --resolution-local-factor 1.0 --map-switching-method node_position   --min-range 1.6 --max-range 130 --base-name off --dir-name $BAG_LOCATION/arla_bags/mapping/ --lidar-topic /velodyne_packets --velodyne-frame-id /velodyne --map-type-name ndt_map  --velodyne-config-file "$(rospack find graph_map)/config/velo32.yaml" --output-dir-name /home/daniel/.ros/maps   --map-size-xy 120 --map-size-z 15.0 --skip-frame 1 --keyframe-min-distance 0.6 --keyframe-min-rot-deg 5   --data-set arla-2012   --save-map #--save-map      disable-keyframe-update --step-control  --save-used-merged-clouds --multi-res
 #--gt-mapping --disable-registration
 #--visualize --visualize-map
 # node_position=0,mean_observation=1,closest_observation=2, grid=3,node_position_esg=4,mean_observation_esg=5

graph_map/src/graph_mapping_offline.cpp

@@ -134,6 +134,8 @@ ndt_offline::OdometryType odom_type;
 Eigen::Affine3d Todom_base_prev,Tgt_base_prev, Tgt_base, Todom_base, Tgt_t0, Todom_t0, fuser_pose, fuser_pose_init, Tsensor_offset;
 Eigen::MatrixXd predCov;
 GraphMapFuser *fuser_=NULL;
+ndt_generic::PointCloudQueue<pcl::PointXYZ> *cloud_queue;
+
 template<class T> std::string toString (const T& x)
 {
   std::ostringstream o;
@@ -314,7 +316,7 @@ bool ReadAllParameters(po::options_description &desc,int &argc, char ***argv){
     exit(0);
   }
 
-
+  cloud_queue=new ndt_generic::PointCloudQueue<pcl::PointXYZ>();
   use_pointtype_xyzir=vm.count("use-pointtype-xyzir");
   use_odometry_source = base_link_id!="";
 
@@ -451,6 +453,7 @@ void SaveMap(){
     }
     if(save_graph_cloud)
       fuser_->SavePointCloud(path);
+
   }
 }
 template<class PointT>
@@ -503,12 +506,17 @@ void PlotAll(bool registration_update, pcl::PointCloud<PointT> points2){
     pcl_conversions::toPCL(t, points2.header.stamp);
     Eigen::Affine3d tmp=fuser_pose*Tsensor_offset;
     perception_oru::transformPointCloudInPlace(tmp, points2);
-    cloud_pub->publish(points2);
+    if(save_used_merged_clouds){
+      cloud_pub->publish(points2);
+      pcl::PointCloud<pcl::PointXYZ> tmp_xyz;
+      copyPointCloud(points2,tmp_xyz);
+      cloud_queue->Push(tmp_xyz);
+      cloud_queue->Save();
+     }
     //vis->PlotLinks();
     //fuser_->PlotMapType();
     //vis->PlotCurrentMap(plotmarker::point);
   }
-
 }
 
 template<typename PointT>