Open VRichardJP opened 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!
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.
I have created uploaded another sample here (different map but same issues): https://drive.google.com/file/d/1fwbFG64muPDaw89g7N7TOHNNJ2plQoJ1/view?usp=sharing
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
$ ./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
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:
For example in the bag translate code:
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);
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
I have applied map points transformation by viewpoint params in the latest hotfix commit. It works on your given
data/odom
anddata/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.
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?).
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:
And the beautymap output at the same location (with --dis_range 40 --xy_resolution 0.2 --h_res 0.5
):
This is a different issue, but is there any parameter I can tune to avoid this kind of issue?
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.
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 filesdata/pcd/000000.pcd
,data/pcd/000001.pcd
, etc. Each frame has VIEWPOINT set correctly (e.gVIEWPOINT 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:
Do you have any idea what is the issue here?