OS1-128 compatability
Closed this issue · 12 comments
@rsasaki0109 Hi sir. I tried os1-128 bag data but it fails to reproduce map. When I tried os1-32 it worked. what is this issue means sir. could you help me out. Thanks in advance.
[scanmatcher_node-1] [INFO] [1692692868.561236163] [scan_matcher]: create a first map
[imu_preintegration-3] terminate called after throwing an instance of 'gtsam::IndeterminantLinearSystemException'
[imu_preintegration-3] what():
[imu_preintegration-3] Indeterminant linear system detected while working near variable
[imu_preintegration-3] 8646911284551352379 (Symbol: x59).
[imu_preintegration-3]
[imu_preintegration-3] Thrown when a linear system is ill-posed. The most common cause for this
[imu_preintegration-3] error is having underconstrained variables. Mathematically, the system is
[imu_preintegration-3] underdetermined. See the GTSAM Doxygen documentation at
[imu_preintegration-3] http://borg.cc.gatech.edu/ on gtsam::IndeterminantLinearSystemException for
[imu_preintegration-3] more information.
[ERROR] [imu_preintegration-3]: process has died [pid 7624, exit code -6, cmd '/home/praveen/ros2_ws/install/scanmatcher/lib/scanmatcher/imu_preintegration --ros-args --params-file /home/praveen/ros2_ws/install/scanmatcher/share/scanmatcher/param/lio_ouster.yaml -r /odometry:=/odom'].
First, please verify if scan matching is possible using only lidar with lidarslam_ros2.
Given that the 128-line lidar has a large number of point clouds in a single scan, you may need to play back the rosbag at a slower speed.
Additionally, I want to see not only the point clouds but also the trajectory. I'd like a more zoomed-in image.
@rsasaki0109 okey sir let me try
@rsasaki0109 Hi sir I have built lidarslam_ros2 but it says error like this when i tried to play the os128 bag file -r 0.1 playback speed and I have pcl version 1.10 there ndt has 1.12 so replaced this repo ndt to build that.
[scanmatcher_node-1] [ERROR] [1692784512.841998883] [scan_matcher]: Could not find a connection between 'base_link' and 'os_sensor' because they are not part of the same tree.Tf has two or more unconnected trees.
[scanmatcher_node-1] [ERROR] [1692784513.788539192] [scan_matcher]: Could not find a connection between 'base_link' and 'os_sensor' because they are not part of the same tree.Tf has two or more unconnected trees.
Have you not set up the TF configurations?
https://github.com/rsasaki0109/lidarslam_ros2/blob/humble/lidarslam/launch/lidarslam.launch.py#L28
@rsasaki0109 Hi sir. I tried to change as you said. It is working good on lidarslam_ros2. Thanks for the help sir. Now checking with li_slam_ros2.
lidarslam_ros2 Image :
li_slam_ros2 also working but the image has drift in it:
trajectory li_slam_ros2:
(At first, I thought the backend's loop closure was bad, but upon closer inspection, I noticed a mistake, so I corrected the comment.)
The scan matching seems to be diverging. Have you properly measured the IMU parameters?
Is the CPU load on the PC not a problem? If it's high, it might be a good idea to set ndt_num_threads to a value greater than 0.
Since it's indoors, it might be better to reduce the values of ndt_resolution, trans_for_mapupdate, and vg_size_for_input by about half.
@rsasaki0109 Hi sir. I have tried to adjust the loop closure parameter and ndt_num_threads as you have mentioned in the reference link. It still provides same result. Let me try to reduce these params also values of ndt_resolution, trans_for_mapupdate, and vg_size_for_input. I will record once again and check these params and get back to you sir.
@rsasaki0109 Hi sir. I did some mistake. official ouster_ros package has been updated. I didn't pull the repo and recorded previously. Now I have pulled the latest one and did recording. Unfortunately li_slam_ros2 doesnot created map as you said like it may have large number of pointcloud but I have tried with lidaslam_ros2 it started creating map but I don't know why it gives trajectory like this. Could you please help me out sir.
Note:We mounted lidar on the robodog and recorded the data.
@rsasaki0109 Hi sir. I have adjusted parameters by half which was mentioned earlier. The spike lines are reduced but still there are some. How to handle that sir.Could you help me out. Thanks in advance.
Adjusted params until less spike with loop closure. The Final output is
@rsasaki0109 Hi sir when I try to save map it occurs this error
[graph_based_slam_node-2] Received an request to save the map
[graph_based_slam_node-2] terminate called after throwing an instance of 'std::bad_array_new_length'
[graph_based_slam_node-2] what(): std::bad_array_new_length
[ERROR] [graph_based_slam_node-2]: process has died [pid 15746, exit code -6, cmd '/home/praveen/ros2_wspace/install/graph_based_slam/lib/graph_based_slam/graph_based_slam_node --ros-args --params-file /home/praveen/ros2_wspace/install/lidarslam/share/lidarslam/param/ouster.yaml'].
I tried to increase swap memory and reduced ndt resolution also for graph based slam but still it occurs. But the map has completely finished without error.
I'm sorry, I didn't notice until now.
It's a bit unclear. Do you think the issue might be with the savePCDFileASCII()? It might be better to ask PCL
@rsasaki0109 I am not sure whether it is related to PoseAdjustment. so far this the current way to write pointcloud data to .pcd format savePCDFileASCII(). This error causes in lidarslam_ros2 but there also code is in correct format.