In this section, I am explaining the DEMO of ndt matching. If you want to run ndt matching using your new rosbag file and new map, please follow section: "Ndt matching using your custom map and rosbag" !!

The rosbag file I used in this DEMO, is "first.bag". And the PCD file is "190718_DATA_FIRST_Modified_Rotation_v2.pcd". They are in humanlab 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.bag" to your local.
5. Go to folder "Autoware_Data/190718_PCD"
6. Download "190718_DATA_FIRST_Modified_Rotation_v2.pcd" to your local.

This rosbag file contains IMU, GPS, LiDAR data. It was recorded inside of DGIST. And the PCD file is created by ndt mapping using "first.bag". You can see details in Wiki "How to ndt mapping with rotation".

Follow commands in terminal. Clone the repository, checkout branch, build by catkin_make, source it. And by './run' you can run autoware.

cd
git clone https://github.com/sonnet-ai/autoware-nav.git
cd autoware-nav
git checkout issue2-fix/move_PC_coor
cd ros
catkin_make 
source devel/setup.bash
./run

1. Go to Simulation tab and click 'ref'. Select rosbag file 'first.bag' you just downloaded.
2. Click "Play" and click "Pause" to set rosparam "use_sim_time" true.
3. Go to Setup tab, push "TF" and push "Vehicle Model".
4. Go to Map tab. Click the most upper "ref", and select "190718_DATA_FIRST_Modified_Rotation_v2.pcd". And Click "Point Cloud" to load map. Wait until you see "100%".
5. Load a TF named "mat_tf.launch". The location is in "/ros/src/computing/perception/localization/packages/lidar_localizer/launch/map_tf.launch"
6. Go to Sensing tab. Click 'app' beside of "vexel_grid_filter" and change topic name as "velodyne_points". And click "voxel_grid_filter" to run.
7. Go to Computing tab and run nmea2tfpose to convert /nmea_sentence into /gnss_pose. The nmea2tfpose is modified in my repository!!.
8. Click "app" beside of "ndt_matching" and choose GNSS as initial position. If you have IMU, click "use IMU" as true. And click "ndt_matching" to run.
9. Open RVIZ by clicking button "RViz".
10. In RViz, click "File" -> "Open Config". And select "autoware-nav/ros/src/.config/rviz/default.rviz". ".config" is hidden, so you should reveal it by mouse right click and "show hidden files".
11. Go to Simulation tab and click "Pause" to replay rosbag.
12. Wait about 30 secs, you can see the vehicle start matching. If it doesn't start matching more than 1 minutes, "stop" simulation and "play" it again.

Tips!! : If you want to see the current pointcloud of vehicle, you may change the topic name in rviz. In my case, I changed Points_Raw's topic name from "points_raw" to "velodyne_points".

"map_tf.launch" is made for publishing static_tf. "world_to_map" will be published.

I referenced the moriyama quick start demo. Note : map_to_mobility has no usage here. So I deleted it.

<launch>
  <arg name="period_in_ms" default="10"/>

  <!-- setting path parameter -->
  <arg name="get_height" value="true" />

  <node pkg="tf" type="static_transform_publisher" name="world_to_map" args="0 0 0 0 0 0 /world /map $(arg period_in_ms)"/>

</launch>

If you clone and build the repository on issue-2 (which may merged to master), you can successfully localize base_link using ndt matching. You should provides the initial position of /gnss_pose in your BAG file in "ros/src/computing/perception/localization/packages/gnss_localizer/launch/nmea2tfpose.launch".

<launch>

  <arg name="plane" default="7"/>
  <arg name="origin_x" default="-788934.681117"/>
  <arg name="origin_y" default="2362.398556"/>
  <arg name="origin_z" default="81.543000"/>

  <node pkg="gnss_localizer" type="nmea2tfpose" name="nmea2tfpose" output="log">
    <param name="plane" value="$(arg plane)"/>

    <param name="origin_x" value="$(arg origin_x)"/>
    <param name="origin_y" value="$(arg origin_y)"/>
    <param name="origin_z" value="$(arg origin_z)"/>
  </node>

</launch>

Change (origin_x, origin_y, origin_z) as your data. The position data you initialize in nmea2tfpose must be the first position in /gnss_pose in your rosbag file. You can check it by

$ rostopic echo (your topic, in my case /gnss_pose)

in terminal.

1. Go to Simulation tab and select a rosbag which includes /points_raw (or /velodyne_points) and /nmea_sentence(or /gnss_pose). It will be better if you have IMU.
2. Click "Play" and click "Pause" to set rosparam "use_sim_time" true.
3. Go to Setup tab, input parameters related to relative position between the vehicle and localizer, push "TF" and push "Vehicle Model" (if you leave the space blank, the default model will be loaded.).
4. Go to Map tab and load pointcloud map which you had created by ndt mapping. It should contains rotation. To see how to create a map with rotation, go to Wiki page (How to ndt mapping with rotation).
5. Load a TF named "map_tf.launch". The location is in "/ros/src/computing/perception/localization/packages/lidar_localizer/launch/map_tf.launch"
6. Go to Sensing tab and run voxel_grid_filter to downsample scan data. If your LiDAR topic is not /points_raw, you should change the topic name in the 'voxel_grid_filter' application. (In my case, /velodyne_points)
7. Go to Computing tab and run nmea2tfpose to convert /nmea_sentence into /gnss_pose. The nmea2tfpose is modified in my repository!!.
8. Run ndt_mathcing, click "app" and choose GNSS as initial position. If you have IMU, click "use IMU" as true.
9. Go to Simulation tab and click "Pause" to replay rosbag.

ndt_matching and ndt_mapping is not seem to be good for Simultaneously Localization and Mapping in real autonomous vehicle. Although you could transform all the coordinates together by instruction above, the algorithm itself has a fault so that could not converge the base_link position well. So I decided to use another SLAM algorithms, especially GraphSLAM. Because many GraphSLAMs are using NDT (Normal Distribution Transform) as a baseline, I think I could get into it easily.