The accuracy of the hdl_graph_slam

[Blackshaddock]

I ran the datasets that you provided, and used the rosservice to save the map, as you can see the map in some where has the thick point cloud. So can you tell me how to adujst the parameters. and I saw the code and found that IMU is only used in the optimization part, so is there no use of IMU for distortion correction of point cloud data in the front end? Look forward to your reply! thanks

[koide3]

Hi @Blackshaddock, You may want to tune the following params which have a big impact on the mapping result:

• registration_method: NDT_OMP or GICP (note that GICP in PCL 1.7 may not work)
• ndt_resolution: larger values are good for outdoor envionments (e.g. 2-10m)

BTW, I'm going to release a new SLAM framework which allows to interactively correct mapping errors like the one you pointed out. If you are interested in it, please check out the following repository in the next week: https://github.com/koide3/interactive_slam

I saw the code and found that IMU is only used in the optimization part, so is there no use of IMU for distortion correction of point cloud data in the front end?

In this package, we use IMU data only in the backend optimization part (I assumed the user moves the LIDAR “gently” during mapping). In the localization package (hdl_localization), we used IMU to deal with quick sensor motion. The frontend of this package may be improved in the same way, but it has not been done yet.

[GLKode]

Hi @koide3 , will the new SLAM framework be used an enhancement to the hdl_graph_slam? And it would be interesting to know how to use these packages together, for example, hdl_graph_slam and hdl_localization can launch them simultaneously do they complete each other or not? If not, then how would it be possible to add the IMU implementation into hdl_graph_slam, what steps need to be taken, and which classes to be modified or added?

[koide3]

Hi @GLkode, The new framework (interactive_slam) is built on top of hdl_graph_slam. You first create a map with hdl_graph_slam (or any other ROS package), then you can edit the created map with interactive_slam.

Since hdl_localization requires a complete map beforehand, you cannot launch them simultaneously. You need to first create a map with hdl_graph_slam (and interactive_slam), and then launch hdl_localization with the created map.

If you want to incorporate IMU information in the frontend of the mapping process, you need to modify scan_matching_odometry_nodelet. You can use PoseEstimater class which performs a UKF-based IMU and range data fusion to estimate the sensor pose and then give it to the scan matching as the initial guess. If you are going to implement it, I would be happy to provide more detailed information.

https://github.com/koide3/hdl_graph_slam/blob/master/apps/scan_matching_odometry_nodelet.cpp https://github.com/koide3/hdl_localization/blob/master/include/hdl_localization/pose_estimator.hpp

[GLKode]

Thank you very much @koide3 for the detailed explanation, I believe there are many requests for implementing IMU in frondend, I would be happy to implement it, could you please provide me with more detailed information, so I can start.

[koide3]

Hi @GLKode , Thanks a lot. I try to provide more information:

In scan_matching_odometry_nodelet, we apply a scan matching method between consecutive input frames to estimate the sensor ego-motion. In the current implementation, we just use the last sensor pose as the initial guess for the scan matching (scan_matching...cpp, L156). So, it doesn't take sensor motion into account so far.

https://github.com/koide3/hdl_graph_slam/blob/c5d5e6720403cb8445725d6eb30ac6e81be6b004/apps/scan_matching_odometry_nodelet.cpp#L156

In hdl_localization_nodelet, we use IMU data to predict the sensor displacement from the last scan matching result. We have a class named PoseEstimater which exploits UKF to predict the currenct sensor pose from IMU data and integrate it with the scan matching result.

https://github.com/koide3/hdl_localization/blob/master/include/hdl_localization/pose_estimator.hpp

This class has two important methods, predict and correct. "predict" method takes an IMU data and its timestamp and updates the estimated sensor pose. "correct" method takes a point cloud and updates the estimation with the scan matching result. What we need to do is to adapt this class to scan_matching_odometry_nodelet. Currently, PoseEstimater performs map-vs-current-frame scan matching, and we need to modify the class so it performs previous-frame-vs-current-frame scan matching.

https://github.com/koide3/hdl_localization/blob/624a7613d1639fefc0f67934e4f57dd2a8c9878f/include/hdl_localization/pose_estimator.hpp#L97 (we need to set previous frame as the input target at this line)

Then, we can put the class in scan_matching_odometry_nodelet. We can just feed IMU and LIDAR data to the class and publish the result as the estimated odometry. The following codes in the localization nodelet may be helpful to know how we can feed data to the class.

https://github.com/koide3/hdl_localization/blob/624a7613d1639fefc0f67934e4f57dd2a8c9878f/apps/hdl_localization_nodelet.cpp#L153 https://github.com/koide3/hdl_localization/blob/624a7613d1639fefc0f67934e4f57dd2a8c9878f/apps/hdl_localization_nodelet.cpp#L160

In case you need more information please let me know. You can also refer to the paper which describes the theoretical side in detail.

Thanks again for your cooperation :)

[GLKode]

Thank you very much @koide3 for the detailed information, by end of the next week I will be having time and will start working on it. will keep you updated.

[GLKode]

Hi @koide3, I am very sorry for the late reply, I have had an accident last month and could not work on the code, today I have access to my PC and went through the code, in the PoseEstimater, I must set the previous_frame as registration->setInputSource(previous_frame);

With frame did you mean keyframe? from the first look i didn't know where to access to the previous frame, can you please give me a hint?

Thank you in advance!

[koide3]

Hi @GLKode , It doesn't matter. I'd be glad if you could do it in your spare time...

in the PoseEstimater, I must set the previous_frame as segistration->setInputSource(previous_frame); With frame did you mean keyframe? from the first look i didn't know where to access to the previous frame, can you please give me a hint?

In the localization method, since the registration target is the globalmap which never changes, setInputTarget is called when the globalmap is loaded. See: https://github.com/koide3/hdl_localization/blob/624a7613d1639fefc0f67934e4f57dd2a8c9878f/apps/hdl_localization_nodelet.cpp#L186

To adapt the PoseEstimater to the SLAM frontend, I suppose we need to change it to the last keyframe.

[rohansingh42]

Was this implemented?