Skip to content
GitLab
Projects
Groups
Snippets
Help
Loading...
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
Open sidebar
software
graph_map_public
Commits
0ec8c288
Commit
0ec8c288
authored
Aug 24, 2018
by
daniel
Browse files
added feature to save pointclouds and removed blank spaces in evaluation output files
parent
81b05cd0
Changes
5
Hide whitespace changes
Inline
Side-by-side
Showing
5 changed files
with
21 additions
and
22 deletions
+21
-22
graph_localization/scripts/private/graph_arla_submap_localization
...calization/scripts/private/graph_arla_submap_localization
+1
-1
graph_map/include/graph_map/graph_map_fuser.h
graph_map/include/graph_map/graph_map_fuser.h
+1
-1
graph_map/include/graph_map/graph_map_fuser_impl.h
graph_map/include/graph_map/graph_map_fuser_impl.h
+7
-16
graph_map/scripts/private/arla_2012_submap
graph_map/scripts/private/arla_2012_submap
+1
-1
graph_map/src/graph_mapping_offline.cpp
graph_map/src/graph_mapping_offline.cpp
+11
-3
No files found.
graph_localization/scripts/private/graph_arla_submap_localization
View file @
0ec8c288
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
View file @
0ec8c288
...
...
@@ -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
View file @
0ec8c288
...
...
@@ -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
View file @
0ec8c288
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
View file @
0ec8c288
...
...
@@ -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
>
...
...
Write
Preview
Markdown
is supported
0%
Try again
or
attach a new file
.
Attach a file
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment