Commit 0ec8c288 authored by daniel's avatar daniel
Browse files

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

parent 81b05cd0
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
......@@ -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;
......
......@@ -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_++;
......
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
......
......@@ -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>
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment