search

MKJia / BeautyMap

[RA-L'24] BeautyMap: Binary-Encoded Adaptable Ground Matrix for Dynamic Points Removal in Global Maps
68 stars 1 forks source link

`IndexError: index 0 is out of bounds for axis 0 with size 0` with custom data #1

Open VRichardJP opened 1 month ago

VRichardJP commented 1 month ago

Hello!

I am trying to use BeautyMap on a custom map. I have a PCD map named data/raw_map.pcd and each frame composing it into files data/pcd/000000.pcd, data/pcd/000001.pcd, etc. Each frame has VIEWPOINT set correctly (e.g VIEWPOINT 5595.09 14392.2 819.099 0.799272 -0.014222 -0.00993263 0.600719).

The overall map is around 2.0GB. I have around 4000 frames of around 600KB each.

Based on you example I have tried the following command: python main.py --data_dir ./data/ --dis_range 40 --xy_resolution 1 --h_res 0.5.

Which leads to the following error:

$ python main.py --data_dir ./data/ --dis_range 40 --xy_resolution 1 --h_res 0.5
We will process the data in folder:  ./data/ 
[LOG] Min xyz: 4869.56689453125, 14181.6611328125, 775.1815795898438 
[LOG] Max/Min value on x: 1116.04150390625/0.0, y: 1298.7529296875/0.0
[LOG] Matrix order: 1299
Generating Map Binary tree: 100%|█████████████████████████| 447494/447494 [02:59<00:00, 2489.03it/s]
Processing Frames:   0%|                                                   | 0/3895 [00:00<?, ?it/s]
Traceback (most recent call last):
  File "main.py", line 136, in <module>
    fire.Fire(main)
  File "/home/vrichard/mambaforge/envs/beautymap/lib/python3.8/site-packages/fire/core.py", line 143, in Fire
    component_trace = _Fire(component, args, parsed_flag_args, context, name)
  File "/home/vrichard/mambaforge/envs/beautymap/lib/python3.8/site-packages/fire/core.py", line 477, in _Fire
    component, remaining_args = _CallAndUpdateTrace(
  File "/home/vrichard/mambaforge/envs/beautymap/lib/python3.8/site-packages/fire/core.py", line 693, in _CallAndUpdateTrace
    component = fn(*varargs, **kwargs)
  File "main.py", line 84, in main
    Qpts.generate_query_binary_tree(minz_matrix_roi)
  File "/home/vrichard/SLAM/tools/BeautyMap/lib/bee_tree.py", line 183, in generate_query_binary_tree
    idx = newidxy[ib][0]
IndexError: index 0 is out of bounds for axis 0 with size 0

Do you have any idea what is the issue here?

Kin-Zhang commented 1 month ago

Hard to tell at this moment without your data, could you try running 100 frames in https://github.com/Kin-Zhang/octomap:

./build/octomap_run /home/kin/data/00 assets/config_fg.toml 100

If no error in the repo, could you upload 100 frames to us to check your data in BeautyMap? Thanks!

VRichardJP commented 1 month ago

Unfortunatelly, I cannot share any part of the map. I have tested several things and it looks like the problem occurs when the points coordinates are "far away".

For example, I have tested 2 versions of the same map: one in "odom" frame (origin is the mapping start point) and one georeferenced. The only difference is thus the "VIEWPOINT" field (0 in the former case). Yet, I can run the tool on the odom map but it crashes with the reported error on the georeferenced one.

I also tested https://github.com/Kin-Zhang/removert and there was no issue.

VRichardJP commented 1 month ago

I have created uploaded another sample here (different map but same issues): https://drive.google.com/file/d/1fwbFG64muPDaw89g7N7TOHNNJ2plQoJ1/view?usp=sharing

image

I have tried to run this sample with several tools. I have only managed to generate a proper output with your removert fork.

Removert with data/odom: ok

$ ./build/removert_run ../../data/odom/ ./config/params_kitti.yaml -1
I0717 00:58:30.636597 132937 removert_PCD.cpp:53] Raw map loaded, size: 734131
I0717 00:58:30.643214 132937 removert_PCD.cpp:68] We will run all the frame in sequence, the total number is: 92
I0717 00:58:30.643226 132937 removert_PCD.cpp:74] rm res len: 3
(92/93) Processing: ../../data/odom/pcd/000091.pcd Time Cost: 0.00889062s          I0717 00:58:31.543788 132937 removert_PCD.cpp:98] 
Finished res: 2.5
(92/93) Processing: ../../data/odom/pcd/000091.pcd Time Cost: 0.00789837s          I0717 00:58:32.408183 132937 removert_PCD.cpp:98] 
Finished res: 2.2
(92/93) Processing: ../../data/odom/pcd/000091.pcd Time Cost: 0.00871475s          I0717 00:58:33.252171 132937 removert_PCD.cpp:98] 
Finished res: 2
I0717 00:58:33.388491 132937 Removerter.cpp:129] Map saved to this folder: "/home/sig/SLAM/data/odom"

Removert Timings:
     Component      Total   Last    Mean    StDev    Min     Max     Steps
    0. Read RawMap       0.01   0.0063  0.0063    nan   0.0063  0.0063       1
    1. Covert DepthI     2.41   0.0083  0.0087  0.0013  0.0062  0.0242     276
    2. Compare Image     0.03   0.0000  0.0001  0.0005  0.0000  0.0091     276
    3. Get DynamicId     0.08   0.0002  0.0003  0.0001  0.0002  0.0012     276
    4. Parse Idx2Pcd     0.13   0.1271  0.1271    nan   0.1271  0.1271       1
    5. Write             0.01   0.0137  0.0137    nan   0.0137  0.0137       1

Removert with data/geo: ok

$ ./build/removert_run ../../data/geo/ ./config/params_kitti.yaml -1
I0717 01:03:49.868062 134990 removert_PCD.cpp:53] Raw map loaded, size: 734131
I0717 01:03:49.874965 134990 removert_PCD.cpp:68] We will run all the frame in sequence, the total number is: 92
I0717 01:03:49.874975 134990 removert_PCD.cpp:74] rm res len: 3
(92/93) Processing: ../../data/geo/pcd/000091.pcd Time Cost: 0.0132964s          I0717 01:03:51.332391 134990 removert_PCD.cpp:98] 
Finished res: 2.5
(92/93) Processing: ../../data/geo/pcd/000091.pcd Time Cost: 0.00897101s          I0717 01:03:52.131143 134990 removert_PCD.cpp:98] 
Finished res: 2.2
(92/93) Processing: ../../data/geo/pcd/000091.pcd Time Cost: 0.00765133s          I0717 01:03:52.898347 134990 removert_PCD.cpp:98] 
Finished res: 2
I0717 01:03:52.920037 134990 Removerter.cpp:129] Map saved to this folder: "/home/sig/SLAM/data/geo"

Removert Timings:
     Component      Total   Last    Mean    StDev    Min     Max     Steps
    0. Read RawMap       0.01   0.0065  0.0065    nan   0.0065  0.0065       1
    1. Covert DepthI     2.87   0.0076  0.0104  0.0043  0.0059  0.0309     276
    2. Compare Image     0.03   0.0006  0.0001  0.0004  0.0000  0.0062     276
    3. Get DynamicId     0.03   0.0002  0.0001  0.0001  0.0001  0.0010     276
    4. Parse Idx2Pcd     0.01   0.0095  0.0095    nan   0.0095  0.0095       1
    5. Write             0.01   0.0122  0.0122    nan   0.0122  0.0122       1

Octomap with data/odom: looks like running but the result is wrong

image

$ ./build/octomap_run ../../data/odom/ assets/config_fg.toml -1
I0717 01:01:26.823325 134568 octomapper.cpp:32] Config file loaded from: assets/config_fg.toml
I0717 01:01:26.823551 134568 octomapper.cpp:51] resolution: 0.1. Ground filter: 1, Noise filter: 1
I0717 01:01:26.823680 134568 octomap_PCD.cpp:53] We will run all the frame in sequence, the total number is: 92
(8/93) Processing: ../../data/odom/pcd/000007.pcd Time Cost: 0.931361s          [pcl::RandomSampleConsensus::computeModel] RANSAC found no model.
[pcl::SACSegmentation::segment] Error segmenting the model! No solution found.
(34/93) Processing: ../../data/odom/pcd/000033.pcd Time Cost: 1.444s          [pcl::RandomSampleConsensus::computeModel] RANSAC found no model.
[pcl::SACSegmentation::segment] Error segmenting the model! No solution found.
(92/93) Processing: ../../data/odom/pcd/000091.pcd Time Cost: 1.41342s          
I0717 01:03:21.302889 134568 octomapper.cpp:266] Saving cleaned map to pcd file, we will save point-level map.
DynamicOctomap Timings in seconds (s)
       Component             Total        Last        Mean      StDev     Min         Max       Steps
Total                       114.7021    114.7021    114.7021    nan     114.7021    114.7021    1    
  0. One Scan Cost          114.4791    1.4130      1.2443      0.2271  0.8431      1.7917      92   
  1. Ground Segmentation    1.5995      0.0157      0.0174      0.0159  0.0000      0.0656      92   
  2. Ray SetFreeOc          17.9892     0.2372      0.1955      0.0454  0.1070      0.3144      92   
  3. Update Octree          60.3203     0.6335      0.6557      0.1053  0.4488      0.9296      92   
  4. Prune Tree             24.1191     0.4306      0.2622      0.1142  0.0167      0.4482      92   
  5. Save Map               0.2229      0.2229      0.2229      nan     0.2229      0.2229      1    
I0717 01:03:21.525857 134568 octomap_PCD.cpp:91] Done! Check the output in ../../data/odom/, file: octomapfg_output.pcd

Octomap with data/geo: failed

$ ./build/octomap_run ../../data/geo/ assets/config_fg.toml -1
I0717 01:03:34.547410 134947 octomapper.cpp:32] Config file loaded from: assets/config_fg.toml
I0717 01:03:34.547645 134947 octomapper.cpp:51] resolution: 0.1. Ground filter: 1, Noise filter: 1
I0717 01:03:34.547778 134947 octomap_PCD.cpp:53] We will run all the frame in sequence, the total number is: 92
W0717 01:03:34.555579 134947 octomapper.cpp:88] Could not generate Key for origin:(75724 31078.7 8.68315)
WARNING: coordinates ( (75724 31078.7 8.68315) -> (-9.24193 -5.57465 -1.45089)) out of bounds in computeRayKeys
WARNING: coordinates ( (75724 31078.7 8.68315) -> (-9.26512 -5.39199 -1.44075)) out of bounds in computeRayKeys
WARNING: coordinates ( (75724 31078.7 8.68315) -> (-9.18245 -5.15295 -1.41486)) out of bounds in computeRayKeys
WARNING: coordinates ( (75724 31078.7 8.68315) -> (-6.31789 -5.04872 -1.51651)) out of bounds in computeRayKeys
WARNING: coordinates ( (75724 31078.7 8.68315) -> (-6.3946 -4.89498 -1.50978)) out of bounds in computeRayKeys
WARNING: coordinates ( (75724 31078.7 8.68315) -> (-7.92737 -4.55927 -1.46875)) out of bounds in computeRayKeys
[...]
WARNING: coordinates ( (75665.2 31071.1 7.53735) -> (21.3872 -19.752 8.19942)) out of bounds in computeRayKeys
WARNING: coordinates ( (75665.2 31071.1 7.53735) -> (21.5246 -19.4698 8.17585)) out of bounds in computeRayKeys
WARNING: coordinates ( (75665.2 31071.1 7.53735) -> (29.0374 8.2494 6.89055)) out of bounds in computeRayKeys

I0717 01:03:51.319249 134947 octomapper.cpp:266] Saving cleaned map to pcd file, we will save point-level map.
DynamicOctomap Timings in seconds (s)
       Component             Total   Last    Mean   StDev     Min     Max   Steps
Total                       16.9489 16.9489 16.9489 nan     16.9489 16.9489 1    
  0. One Scan Cost          16.7714 0.3164  0.1823  0.0336  0.1382  0.3568  92   
  1. Ground Segmentation    1.5785  0.0283  0.0172  0.0121  0.0000  0.0643  92   
  2. Ray SetFreeOc          12.2124 0.2418  0.1327  0.0334  0.0786  0.2947  92   
  3. Update Octree          0.5068  0.0058  0.0055  0.0011  0.0033  0.0093  92   
  4. Prune Tree             1.7435  0.0334  0.0190  0.0093  0.0005  0.0334  92   
  5. Save Map               0.1774  0.1774  0.1774  nan     0.1774  0.1774  1    
I0717 01:03:51.496730 134947 octomap_PCD.cpp:91] Done! Check the output in ../../data/geo/, file: octomapfg_output.pcd

BeautyMap with data/odom: failed after processing 90% of the frames

$ python main.py --data_dir ../../data/odom/ --dis_range 40 --xy_resolution 1 --h_res 0.5
We will process the data in folder:  ../../data/odom/ 
[LOG] Min xyz: -45.78703308105469, -58.09310531616211, -11.745438575744629 
[LOG] Max/Min value on x: 161.17532348632812/0.0, y: 130.41578674316406/0.0
[LOG] Matrix order: 162
Generating Map Binary tree: 100%|█████████████████████████████| 4434/4434 [00:01<00:00, 2984.21it/s]
Processing Frames:  90%|███████████████████████████████████████▋    | 83/92 [00:01<00:00, 55.68it/s]
Traceback (most recent call last):
  File "main.py", line 136, in <module>
    fire.Fire(main)
  File "/home/sig/mambaforge/envs/beautymap/lib/python3.8/site-packages/fire/core.py", line 143, in Fire
    component_trace = _Fire(component, args, parsed_flag_args, context, name)
  File "/home/sig/mambaforge/envs/beautymap/lib/python3.8/site-packages/fire/core.py", line 477, in _Fire
    component, remaining_args = _CallAndUpdateTrace(
  File "/home/sig/mambaforge/envs/beautymap/lib/python3.8/site-packages/fire/core.py", line 693, in _CallAndUpdateTrace
    component = fn(*varargs, **kwargs)
  File "main.py", line 84, in main
    Qpts.generate_query_binary_tree(minz_matrix_roi)
  File "/home/sig/SLAM/tools/BeautyMap/lib/bee_tree.py", line 183, in generate_query_binary_tree
    idx = newidxy[ib][0]
IndexError: index 0 is out of bounds for axis 0 with size 0

BeautyMap with data/geo: failed on first frame

$ python main.py --data_dir ../../data/geo/ --dis_range 40 --xy_resolution 1 --h_res 0.5
We will process the data in folder:  ../../data/geo/ 
[LOG] Min xyz: -115.23182678222656, -71.38360595703125, -14.030510902404785 
[LOG] Max/Min value on x: 161.1808624267578/0.0, y: 129.38636779785156/0.0
[LOG] Matrix order: 162
Generating Map Binary tree: 100%|█████████████████████████████| 4525/4525 [00:01<00:00, 2967.97it/s]
Processing Frames:   0%|                                                     | 0/92 [00:00<?, ?it/s]
Traceback (most recent call last):
  File "main.py", line 136, in <module>
    fire.Fire(main)
  File "/home/sig/mambaforge/envs/beautymap/lib/python3.8/site-packages/fire/core.py", line 143, in Fire
    component_trace = _Fire(component, args, parsed_flag_args, context, name)
  File "/home/sig/mambaforge/envs/beautymap/lib/python3.8/site-packages/fire/core.py", line 477, in _Fire
    component, remaining_args = _CallAndUpdateTrace(
  File "/home/sig/mambaforge/envs/beautymap/lib/python3.8/site-packages/fire/core.py", line 693, in _CallAndUpdateTrace
    component = fn(*varargs, **kwargs)
  File "main.py", line 84, in main
    Qpts.generate_query_binary_tree(minz_matrix_roi)
  File "/home/sig/SLAM/tools/BeautyMap/lib/bee_tree.py", line 183, in generate_query_binary_tree
    idx = newidxy[ib][0]
IndexError: index 0 is out of bounds for axis 0 with size 0
Kin-Zhang commented 1 month ago

Hi About BeautyMap we already know the reason, @MKJia will push a commit to fix.

But for the data you sent to us, I find that you forgot to transform all frames into map axis: image

For example in the bag translate code:

https://github.com/Kin-Zhang/simple_ndt_slam/blob/e12840886641b364580f9b5820164b8b849d7899/tools/src/extract_pcdFbag_tf.cpp#L185-L189

      std::vector<int> indices;
      pcl::removeNaNFromPointCloud(*pcl_cloud_tmp, *pcl_cloud_tmp, indices);

      // Remember we transform! The Point Cloud!!
      pcl::transformPointCloud(*pcl_cloud_tmp, *pcl_cloud, transform);
MKJia commented 1 month ago

I have applied map points transformation by viewpoint params in the latest hotfix commit. It works on your given data/odom and data/geo. And you can then test our fixed code on your own dataset. @VRichardJP

MKJia commented 1 month ago

I have applied map points transformation by viewpoint params in the latest hotfix commit. It works on your given data/odom and data/geo. And you can then test our fixed code on your own dataset. @VRichardJP

Since you did not transform all frames into map axis, please transform them, or note bee_tree.py line:225 to ALLOW viewpoint transformation for query frames for your own data.

VRichardJP commented 1 month ago

Thank you for the quick feedback. I misunderstood how the VIEWPOINT field was used.

I modified my code so that both raw map and individual frames points use the same coordinate frame. Individual pcd scans VIEWPOINT is still set to the sensor origin (is it necessary?).

image

I could run successfully removert, octomap and beautymap. I don't have any dynamic object in my sample data so I cannot judge the quality of the processing. However I observe Beautymap has tendency to delete thin objects such as fences and poles.

For example, here is a chunk of the original map:

image

And the beautymap output at the same location (with --dis_range 40 --xy_resolution 0.2 --h_res 0.5):

image

This is a different issue, but is there any parameter I can tune to avoid this kind of issue?

Kin-Zhang commented 1 month ago

VIEWPOINT is still set to the sensor origin (is it necessary?)

Yes. Since some methods (ray casting based mainly) need sensor origin to know the start position of the ray, like octomap, dufomap etc. The data format aims for all methods.