http://wiki.ros.org/kinetic/Installation/Ubuntu

sudo apt-get install libomp-dev

http://pointclouds.org/documentation/tutorials/compiling_pcl_posix.php

Firstly, install these :

sudo apt-get install libeigen3-dev
sudo apt-get instsall cmake

Then, go to : https://github.com/RainerKuemmerle/g2o

sudo apt-get install python-software-properties
sudo add-apt-repository ppa:jmaye/ethz
sudo apt-get update
sudo apt-get install libsuitesparse-dev

sudo apt-get update
sudo apt-get install ros-kinetic-geodesy ros-kinetic-pcl-ros ros-kinetic-nmea-msgs ros-kinetic-libg2o

mkdir catkin_ws && cd catkin_ws
mkdir src && cd src
catkin_init_workspace

cd /catkin_ws/src
git clone https://github.com/koide3/ndt_omp.git
git clone https://github.com/koide3/hdl_graph_slam.git
cd /catkin_ws
catkin_make
source devel/setup.bash

This section is necessary for running DEMO !! If you using another rosbag file, these errors may not occur.

I found a error when I use my custom rosbag file's GPS data. The topic is /nmea_sentence and includes the data such as "$GPGGA, $GNRMC ......". In hdl_graph_slam they just use (longitude, altitude) so only GNRMC is being used. But, I got an error said "invalid checksum". So I changed a source code "/catkin_ws/src/hdl_graph_slam/include/hdl_graph_slam/nmea_sentence_parser.hpp". In line 94 :

if(checksum != (sum & 0xf)) {
    ....
}

After numerous trial, I changed it as below:

if(checksum != (sum)) {
    ....
}

It will solve the checksum error and compare checksum correctly.

Second, In my rosbag file named "first.bag", nmea_sentence has "$GNRMC" not "$GPRMC". So I changed the source code in line 19:

if(tokens[0] != "$GPRMC" || tokens.size() < 12) {
   status = 'V';
   return;
}

changed it to :

if(tokens[0] != "$GNRMC" || tokens.size() < 12) {
   status = 'V';
   return;
}

"$GNRMC" and "$GPRMC" has no big difference. They have the same format of "Time, date, position, course and speed data." The only difference is GN just means that the message contains GNSS agnostic data, while GP means it’s GPS only.

Reference : http://navspark.mybigcommerce.com/content/NMEA_Format_v0.1.pdf

Lastly, in my "$GNRMC" topics there are some blank data at the tail, but hdl_graph_slam does not use it at all. So I comment the lines parsing the data that my topics do not have.

// in line 38, comment it. And add a new line that makes it zero.
// track_angle_degree = std::stod(tokens[8]);
track_angle_degree = 0;

...

// comment two lines below. And add a new line that makes it zero.
// magnetic_variation = std::stod(tokens[10]);
// magnetic_variation = tokens[11][0] == 'E' ? magnetic_variation : -magnetic_variation;
magnetic_variation = 0;

After that, you should catkin_make again.

cd /catkin_ws
catkin_make
source devel/setup.bash

Be aware to run roscore first. To run hdl_graph_slam with GPS data (not include IMU at here).

rosparam set use_sim_time true
roslaunch hdl_graph_slam hdl_graph_slam_ford.launch

If you can not find package 'hdl_graph_slam', your setup.bash is not sourced correctly. To fix this temporarily, go to '/catkin_ws' and 'source devel/setup.bash' on every terminal.

roscd hdl_graph_slam/rviz
rviz -d hdl_graph_slam.rviz

Then run rosbag file.

rosbag play --clock ($ your_rosbag_file_name).bag

You have to prepare the ($ your_rosbag_file_name).bag. If you want to run DEMO, Go to "HumanDisk/Autoware_Data/190614_MappingData" and find "xxx_modified_imu.bag". And skip the section "ROSBAG Info" below.

If you want to make your own data, see the section "ROSBAG Info" below.

In hdl_graph_slam, the GPS topic name must be in the list:

• /gps/geopoint (geographic_msgs/GeoPointStamped)
• /gps/navsat (sensor_msgs/NavSatFix)
• /gpsimu_driver/nmea_sentence (nmea_msgs/Sentence)

And the IMU topic name must be :

• /gpsimu_driver/imu_data (sensor_msgs/Imu)

Lastly, the IMU topic's frame ID must be '/velodyne'.

You should record the rosbag with the topic names above.

The DEMO is using the rosbag file recorded in DGIST. The file was 'first.bag' and it has different topic names. Furthermore, IMU topic's frame ID is not /velodyne, so it occurs an error. So I modified both of them and had saved it to the NAS:

1. Access NAS. The IP now in July 2019 is (10.40.4.38). You should input ID and password.
2. Go to folder HumanDisk.
3. Go to folder "Autoware_Data/190614_MappingData".
4. Download "first_modified_imu.bag" to your local.

"first.bag", "second.bag" and "third.bag" are the rosbag files recored in DGIST. It has LiDAR(VLP-16), IMU (3DM_GX5_15) and GPS RTK(MBC MRP 2000) sensor data.

Go to "catkin_ws/src/hdl_graph_slam/launch/hdl_graph_slam_ford.launch" and enable the sensors as you want.

<launch>
   <!-- arguments -->
   ...
   <arg name="enable_gps" default="true" />
   <arg name="enable_imu_acc" default="true" />
   <arg name="enable_imu_ori" default="false" />
   ...

I used gps and imu_acc, so set it to "true". imu_ori may harm stability of localization when it is not reliable (such as some magnetic nearby ...), as the repository owner said, I turned it off.

There are crutial parameters need to be changed for loop closure. Go to "catkin_ws/src/hdl_graph_slam/launch/hdl_graph_slam_ford.launch" and fine-tune some of them. The parameters for loop closing start at (line 85).

If the red sphere (which means the boundary of loop detection) is not big enough to reach to the start point, you should increase it to loop detection. For example, using first_modified_imu.bag I changed it to 60.0, using third_modified_imu.bag I changed it to 100.0.

The most important parameter for loop closure. If you failed to close the loop, check the terminal. Find the smallest fitness_score when loop detection is performed. The data will be like this 👍

--- loop detection ---
num_candidates : 23
matching.........................done
best_score: 32.173    time: 0.000[sec]
loop not found...

Find the smallest value of "best_score" among the last part of the terminal !!! And then, set the "fitness_score_thresh" to be a slightly larger than that value. For example, in first.bag, the smallest fitness_score was 4.0 so I tuned it to 5.0. In third.bag, the smallest was 26.0 so I tuned it to 30.0 .

There are two more parameters for loop closure. "accum_distance_thresh" and "mid_edge_interval". I set it as default and didn't change.

When use the pcd file for localization (its on issue 5 hdl_localization), there are nothing wrong with pcd file using "first.bag". But I ran with the pcd file created by "third.bag" seems to be no problem. But, in the middle of the rosbag file, always fail to localize on a same spot.

You can see the video of hdl_localization on "third.bag". It fails to localize around 3:00. Link : https://www.youtube.com/watch?v=pI-KB9ZZ8tA

I found it is because of the sparsity of the pcd map!! So it is important to make a denser map when the localization is always fail at the same point. I will show the changed parameters below :

<?xml version="1.0"?>
<launch>
  ...
  <!-- transformation between lidar and base_link -->
  <node pkg="tf" type="static_transform_publisher" name="lidar2base_publisher" args="0 0 0 0 0 0 base_link velodyne 10" />
  <node pkg="tf" type="static_transform_publisher" name="imu2base_publisher" args="0 0 -1 0 0 0 base_link imu 10" />    <!-- added for viewing tf. It is not necessary, but I recommend to add these tf to yours too. -->

 ...
    <!-- distance filter -->
    ...
    <param name="distance_near_thresh" value="0.1" /> <!-- default : 1.0, decrease it to make denser map -->
    ...
    <param name="downsample_resolution" value="0.1" /> <!-- default : 0.2, decrease it to make denser map -->
    ...
    <param name="radius_min_neighbors" value="2" /> <!-- default : 5, decrease it to make denser map -->
  </node>
  ...
  <!-- hdl_graph_slam_nodelet -->
    ...
    <param name="keyframe_delta_trans" value="2.0" />  <!-- default : 10.0, decrease it to make denser map. important param !!!! -->
    ...
    <param name="keyframe_delta_angle" value="2.0" />  <!-- default : 3.0, decrease it to make denser map important param !!!! -->
    <!-- loop closure params -->    <!-- adjust loop closure params as the tips at section 4-1 !!!! -->
    <param name="distance_thresh" value="100.0" />
    <param name="accum_distance_thresh" value="60.0" />
    <param name="min_edge_interval" value="10.0" /> 
    <param name="fitness_score_thresh" value="35.0" />
    ...
    <!-- update params -->
    ...
    <param name="map_cloud_resolution" value="0.05" />  <!-- default : 0.05, decrease it to make denser map-->
  </node>

  <node pkg="hdl_graph_slam" type="map2odom_publisher.py" name="map2odom_publisher" />
</launch>

• Loop closure params : adjust it every time you want if you are using rosbag offline SLAM.
• the params right above : they are the default params of "hdl_graph_slam.launch". I used them for dense registration.
• the other params : the params I didn't said here, they are default params in "hdl_graph_slam_ford.launch". It is for using GPS data.

The parameters I changed except loop closure params, they are all the defaults in "hdl_graph_slam.launch". But, I want to use GPS, so the other parameters are based on "hdl_graph_slam_ford.launch", which is described to use it when you enable GPS. The modified parameters here are all related to the sparsity of the map. The parameters above will make it registers denser map, and "map_cloud_resolution" 0.05 will contains almost full points. As "hdl_graph_slam_ford.launch" default, "map_cloud_resolution" 0.2 is already small enough to contains full points.

parameters with 4-1	parameters with 4-2 (dense map)

You can see that there are much denser points on the road plane.

You can use rosservice to save the PCD Map you just created, while hdl_graph_slam is still running.

rosservice call /hdl_graph_slam/save_map <TAB><TAB>

If you twice, the format will be automatically filled on your terminal. If you could not call the service, please go to "/catkin_ws" and "source devel/setup.bash". You can see the format as below:

rosservice call /hdl_graph_slam/save_map "utm: false
resolution: 0.0
destination: ''"

Fill the format as you want. For example :

rosservice call /hdl_graph_slam/save_map "utm: false
resolution: 0.1
destination: '/home/ktk/blarblar.pcd'"

The smaller resolution, the denser pointcloud, the bigger file. With the parameters in section 4-1, resolution 0.2 is enough. With section 4-2, resolution 0.05 could earn full points map. If it is too big, adjust resolution from 0.05 ~ 0.2.

You can see the result pcd :

pcl_viewer blarblar.pcd

The graph below shows the result of hdl_graph_slam. You can see the improvement of the map quality when adding GPS and IMU sensor. Without both of them, it failed to close the loop. With GPS, it closed the loop but the map has distortions. With both sensors, I got a best map quality with closed loop.

w/o GPS, w/o IMU	with GPS, w/o IMU

with GPS, with IMU	Loop Closing

The loop-closed PCD map files are saved to "HumanDisk/Autoware_Data/190802_ClosedPCD/". The "second.bag" is failed to loop closure because the start point and end point are not close enough, so the fitness_score is very large. The specific information is in the "README.md" in that directory.