search

autowarefoundation / autoware_ai

Apache License 2.0
26 stars 11 forks source link

ndt matching refactoring in head #839

Closed nsakar closed 6 years ago

nsakar commented 8 years ago

Hi

I refreshed latest autoware stack and was running ndt matching on my test car which was working perfectly as of commit April 15, 2016 [sha1: 08ad56ef0b6d59f4769912b35591493fecf76db8]. This was before the refactoring to separate out voxel grid filter as it's own node. In the previous version, ndt matching was consuming /points_raw and applying the voxel grid filter in the callback. With the latest code as of June 17, the car is not computing pose correctly. It works fine in simulation .

I think there is delay introduced because now 2 nodes have to establish a communication channel. Are there any other gotchas that others have encountered with this refactoring that could be responsible for the observed behavior?

yukkysaito commented 8 years ago

Hi @nsakar

After the refactoring to separate out voxel grid filter, ndt matching is computing correctly in our environment. I think ndt matching was not publishing pose because matching score is small, or mistake the operation. If you wanna improve accuracy, please try to reduce voxel size in sensing tab (but a execution time increases). If there is a operation video, I may give more advice... Did you check filter node in sensing tab?

I think there is delay introduced because now 2 nodes have to establish a communication channel. Are there any other gotchas that others have encountered with this refactoring that could be responsible for the observed behavior?

It is true that there is delay, but I guess delay is less than 1 msec because communication data size is small and it communicate in localhost.

nsakar commented 8 years ago

yes filter node is checked

kitsukawa commented 8 years ago

@nsakar In recent specification change, ndt_matching doesn't publish /current_pose. ndt_matching publish /ndt_pose. Please run vel_pose_mux node in computing tab before runnning ndt_matching. vel_pose_mux subscribes /ndt_pose and publishes /current_pose. In our environment, ndt_matching works well both in simulation and in real vehicle.

nsakar commented 8 years ago

Yes. I am already doing that. My functional code was as of April 16 commit where this change was baked in.

Regards

Nandini

From: Yuki Kitsukawa [mailto:notifications@github.com] Sent: Monday, June 20, 2016 12:18 AM To: CPFL/Autoware Autoware@noreply.github.com Cc: Sarkar, Nandini nandini.sarkar@intel.com; Mention mention@noreply.github.com Subject: Re: [CPFL/Autoware] ndt matching refactoring in head (#361)

@nsakarhttps://github.com/nsakar In recent specification change, ndt_matching doesn't publish /current_pose. ndt_matching publish /ndt_pose. Please run vel_pose_mux node in computing tab before runnning ndt_matching. vel_pose_mux subscribes /ndt_pose and publishes /current_pose.

— You are receiving this because you were mentioned. Reply to this email directly, view it on GitHubhttps://github.com/CPFL/Autoware/issues/361#issuecomment-227066944, or mute the threadhttps://github.com/notifications/unsubscribe/AQcgG-8ojJOzvKNz1MBYvl5PP6m7J1Rzks5qNj6egaJpZM4I4_p6.

kitsukawa commented 8 years ago

@nsakar What is the voxel size of voxel_grid_filter in sensing tab? If possible, can you provide the operation video of the screen? I will investigate.

nsakar commented 8 years ago

1.01

From: Yuki Kitsukawa [mailto:notifications@github.com] Sent: Monday, June 20, 2016 7:39 AM To: CPFL/Autoware Autoware@noreply.github.com Cc: Sarkar, Nandini nandini.sarkar@intel.com; Mention mention@noreply.github.com Subject: Re: [CPFL/Autoware] ndt matching refactoring in head (#361)

@nsakarhttps://github.com/nsakar What is the voxel size of voxel_grid_filter in sensing tab? If possible, can you provide the operation video of the screen? I will investigate.

— You are receiving this because you were mentioned. Reply to this email directly, view it on GitHubhttps://github.com/CPFL/Autoware/issues/361#issuecomment-227161622, or mute the threadhttps://github.com/notifications/unsubscribe/AQcgG0a3BLH9_josj-EmNRaSon1MzmnWks5qNqYTgaJpZM4I4_p6.

nsakar commented 8 years ago

After reverting the ndt_matching code to the apr 16 commit, my car drives correctly. I am wondering what changed between the calculations.

  1. Key observations a. After reapplying previous edits, it was found that the latest code works fine in simulation b. When attempt was made to run code on physical car, there were significant issues found in ndt_matching. c. Main refactoring done on ndt_matching included i. Separation of concerns and application of single responsibility principle
  2. Translates to 1 node was split into two which in turn added yet another IPC into the mix d. Experiments run on physical car: i. Latest code with voxel grid filter leaf size configured correctly [ 1.01]
  3. Works fine for part of the trajectory. Fails at portions where rapid changes in angular pose encountered ii. Latest code with voxel grid merged in [ aka one node eliminating IPC]
  4. Same result as above iii. Latest code with NDT matching overlaid with original functional code [ Apr 16 commit]
  5. Works correctly
kitsukawa commented 8 years ago

@nsakar I see your situation. ICP is a different algorithm of scan matching and completely independent of NDT. We always set the voxel size 2.0 so can you try again using this configuration? By the way, was the map data made by ndt_mapping? What kind of place are you conducting experiments? If the map and real conditions are different to some extent due to the traffic or many parking cars, there is possibility that ndt_matching fails localization.

nsakar commented 8 years ago

It was not working at all with voxel size 2.0. So I changed it to 1.01 as before. I am using the ndt_matching node and not the icp node. Also the map was generated using ndt mapping. I am testing on simulation environment where it works fine and also on physical car where I have issues.

Regards

Nandini

From: Yuki Kitsukawa [mailto:notifications@github.com] Sent: Monday, June 20, 2016 5:49 PM To: CPFL/Autoware Autoware@noreply.github.com Cc: Sarkar, Nandini nandini.sarkar@intel.com; Mention mention@noreply.github.com Subject: Re: [CPFL/Autoware] ndt matching refactoring in head (#361)

@nsakarhttps://github.com/nsakar I see your situation. ICP is a different algorithm of scan matching and completely independent of NDT. We always set the voxel size 2.0 so can you try again using this configuration? By the way, was the map data made by ndt_mapping? What kind of place are you conducting experiments? If the map and real conditions are different to some extent due to the traffic or many parking cars, there is possibility that ndt_matching fails localization.

— You are receiving this because you were mentioned. Reply to this email directly, view it on GitHubhttps://github.com/CPFL/Autoware/issues/361#issuecomment-227311579, or mute the threadhttps://github.com/notifications/unsubscribe/AQcgGz8zrtkHP4mnlpz3HZuHMSgCNqZTks5qNzUAgaJpZM4I4_p6.

kitsukawa commented 8 years ago

Can you check the computation time of scan matching? The computation time is published in /time_ndt_matching topic. If the computation is not finished in measurement interval of LiDAR (Velodyne: 100ms), there is possibility that ndt_matching fails localization.

nsakar commented 8 years ago

The computation time is higher and does exceed 100 ms at points in trajectory where the car demonstrates weird behavior. It appears the new code has made the algorithm very sensitive. If you can throw some light on the possible causes it would be very helpful.

kitsukawa commented 8 years ago

What LIDAR are you using?

nsakar commented 8 years ago

velodyne

From: Yuki Kitsukawa [mailto:notifications@github.com] Sent: Saturday, June 25, 2016 2:31 AM To: CPFL/Autoware Autoware@noreply.github.com Cc: Sarkar, Nandini nandini.sarkar@intel.com; Mention mention@noreply.github.com Subject: Re: [CPFL/Autoware] ndt matching refactoring in head (#361)

What LIDAR are you using?

— You are receiving this because you were mentioned. Reply to this email directly, view it on GitHubhttps://github.com/CPFL/Autoware/issues/361#issuecomment-228526292, or mute the threadhttps://github.com/notifications/unsubscribe/AQcgG2kIC8I5pga8f7H3GiylBF3pMkV8ks5qPPVngaJpZM4I4_p6.

kitsukawa commented 8 years ago

Which model? HDL-64e, HDL-32e, VLP-16...?

nsakar commented 8 years ago

Vlp 16

From: Yuki Kitsukawa [mailto:notifications@github.com] Sent: Saturday, June 25, 2016 2:35 AM To: CPFL/Autoware Autoware@noreply.github.com Cc: Sarkar, Nandini nandini.sarkar@intel.com; Mention mention@noreply.github.com Subject: Re: [CPFL/Autoware] ndt matching refactoring in head (#361)

Which model? HDL-64e, HDL-32e, VLP-16...?

— You are receiving this because you were mentioned. Reply to this email directly, view it on GitHubhttps://github.com/CPFL/Autoware/issues/361#issuecomment-228526404, or mute the threadhttps://github.com/notifications/unsubscribe/AQcgG_CusRz69nb9lVD4GfwFM_OJNvW5ks5qPPYtgaJpZM4I4_p6.

kitsukawa commented 8 years ago

Do you mount VLP-16 on on the roof of the vehicle? Is there enough height from the roof? If lower beams of VLP-16 hit the vehicle, there are not enough points for matching.

nsakar commented 8 years ago

It is working fine with apr 16 ndt_matching code

From: Yuki Kitsukawa [mailto:notifications@github.com] Sent: Saturday, June 25, 2016 2:45 AM To: CPFL/Autoware Autoware@noreply.github.com Cc: Sarkar, Nandini nandini.sarkar@intel.com; Mention mention@noreply.github.com Subject: Re: [CPFL/Autoware] ndt matching refactoring in head (#361)

Do you mount VLP-16 on on the roof of the vehicle? Is there enough height from the roof? If lower beams of VLP-16 hit the vehicle, there are not enough points for matching.

— You are receiving this because you were mentioned. Reply to this email directly, view it on GitHubhttps://github.com/CPFL/Autoware/issues/361#issuecomment-228526975, or mute the threadhttps://github.com/notifications/unsubscribe/AQcgG_4tC8sjtMWtwlqLN2izrLp2iOdEks5qPPiQgaJpZM4I4_p6.

kitsukawa commented 8 years ago

I didn't modify the matching algorithm itself. I confirmed the new code works in the same way as before both in simulation and in field operation. So I can't investigate without your rosbag or operation video of the screen.

nsakar commented 8 years ago

Did you use offset = linear, zero or quadratic in field?

From: Yuki Kitsukawa [mailto:notifications@github.com] Sent: Saturday, June 25, 2016 3:03 AM To: CPFL/Autoware Autoware@noreply.github.com Cc: Sarkar, Nandini nandini.sarkar@intel.com; Mention mention@noreply.github.com Subject: Re: [CPFL/Autoware] ndt matching refactoring in head (#361)

I didn't modify the matching algorithm itself. I confirmed the new code works in the same way as before both in simulation and in field operation. So I can't investigate without your rosbag or operation video of the screen.

— You are receiving this because you were mentioned. Reply to this email directly, view it on GitHubhttps://github.com/CPFL/Autoware/issues/361#issuecomment-228528349, or mute the threadhttps://github.com/notifications/unsubscribe/AQcgGyQRj5BzBY2gxk3uXu3sCUy19Ywyks5qPPzsgaJpZM4I4_p6.

kitsukawa commented 8 years ago

"linear" works the best. We always use "linear" both in simulation and field.

nsakar commented 8 years ago

OK Thanks.

From: Yuki Kitsukawa [mailto:notifications@github.com] Sent: Saturday, June 25, 2016 3:08 AM To: CPFL/Autoware Autoware@noreply.github.com Cc: Sarkar, Nandini nandini.sarkar@intel.com; Mention mention@noreply.github.com Subject: Re: [CPFL/Autoware] ndt matching refactoring in head (#361)

"linear" works the best. We always use "linear" both in simulation and field.

— You are receiving this because you were mentioned. Reply to this email directly, view it on GitHubhttps://github.com/CPFL/Autoware/issues/361#issuecomment-228528678, or mute the threadhttps://github.com/notifications/unsubscribe/AQcgGyklwTc-Px4V3aD7KJI6Q97UM220ks5qPP4TgaJpZM4I4_p6.

sgzzgit commented 8 years ago

I update the code and met the same question with you. @nsakar Could you tell me which ndt_matching code are you using now? Before apr 16 or after apr 16? Thanks.

kerimyener commented 6 years ago

Hello all, I am currently using autoware. First of all, I would like to thank you for your spectacular work. I want use ndt macthing with my rosbag file which composes only velodyne point and imu data. I filtered tf topics out from it. However whenever I launch the ndt matching with the rosbag file my car starts to spin around map. I provide transfer from base_link to Velodyne and as I understood ndt matching provide transfer from map to base_link but apperently I am missing something. Could you give some hint about how ndt macthing works?

kitsukawa commented 6 years ago

@kerimyener How do you made the map, ndt_mapping? Did you indicate the initial position when starting ndt_matching?

kerimyener commented 6 years ago

@kitsukawa yes sir. I used ndt mapping to create the map. Actually I followed these steps: 1-download kittidata (odometry data set) 2-used kitti2bag to converts data to bagfile(i changed velodyne frame id from velo_link to velodyne) 3-filter the dynamic transformation(world to base_link) out 4-create map by using ndt mapping 5-for localization in autoware ---without gnss 1-play rosbag and stop(use sim time true) 2-choose localizer(velodyne) 3-set tf between base_link and velodyne 4-upload pcd file 5-set tf between world to map(world map 0 0 0 0 0 0 ) 6-choose voxel filter 7-choose ndt matching(choose initial pose ) you could see result in attachment rviz_screenshot_2018_03_06-17_23_58 with gnss 5-set set tf between world to map(according to first msg of fix2pose(i added plane in geo_pos_conv.cpp) i set x y z r y p) 6-choose voxel filter 7-choose ndt matching but either way i couldnt manage to overlap map and scan. Thank you for your time

kitsukawa commented 6 years ago

@kerimyener First, the map you are using is dense, so can you downsample PCDs using PCD filter in Map tab and load them instead of raw PCDs? I think that 0.2 for voxel size will work. Do you use same rosbag for localization using ndt_matching as mapping using ndt_mapping?

kerimyener commented 6 years ago

@kitsukawa Thank you for the information. It was the problem map was too dense. Now i can run the ndt matching properly

kitsukawa commented 6 years ago

@kerimyener Glad to hear that.