chore: import upstream snapshot with attribution
This commit is contained in:
wehub-resource-sync
@@ -0,0 +1,30 @@
|
||||
name: Close inactive issues
|
||||
on:
|
||||
schedule:
|
||||
- cron: "35 11 * * 5"
|
||||
|
||||
env:
|
||||
DAYS_BEFORE_ISSUE_STALE: 30
|
||||
DAYS_BEFORE_ISSUE_CLOSE: 14
|
||||
|
||||
jobs:
|
||||
close-issues:
|
||||
runs-on: ubuntu-latest
|
||||
permissions:
|
||||
issues: write
|
||||
pull-requests: write
|
||||
steps:
|
||||
- uses: actions/stale@v5
|
||||
with:
|
||||
days-before-issue-stale: ${{ env.DAYS_BEFORE_ISSUE_STALE }}
|
||||
days-before-issue-close: ${{ env.DAYS_BEFORE_ISSUE_CLOSE }}
|
||||
stale-issue-label: "stale"
|
||||
stale-issue-message: |
|
||||
This issue is stale because it has been open for ${{ env.DAYS_BEFORE_ISSUE_STALE }} days with no activity.
|
||||
It will be closed if no further activity occurs. Let us know if you still need help!
|
||||
close-issue-message: |
|
||||
This issue is being closed because it has been stale for ${{ env.DAYS_BEFORE_ISSUE_CLOSE }} days with no activity.
|
||||
If you still need help, please feel free to leave comments.
|
||||
days-before-pr-stale: -1
|
||||
days-before-pr-close: -1
|
||||
repo-token: ${{ secrets.GITHUB_TOKEN }}
|
||||
+62
@@ -0,0 +1,62 @@
|
||||
# Prerequisites
|
||||
*.d
|
||||
|
||||
# Compiled Object files
|
||||
*.slo
|
||||
*.lo
|
||||
*.o
|
||||
*.obj
|
||||
|
||||
# Precompiled Headers
|
||||
*.gch
|
||||
*.pch
|
||||
|
||||
# Compiled Dynamic libraries
|
||||
*.so
|
||||
*.dylib
|
||||
*.dll
|
||||
|
||||
# Fortran module files
|
||||
*.mod
|
||||
*.smod
|
||||
|
||||
# Compiled Static libraries
|
||||
*.lai
|
||||
*.la
|
||||
*.a
|
||||
*.lib
|
||||
|
||||
# Executables
|
||||
*.exe
|
||||
*.out
|
||||
*.app
|
||||
|
||||
# VS Code
|
||||
.vscode/*
|
||||
|
||||
# CLion
|
||||
.idea/*
|
||||
|
||||
# Tests
|
||||
ufomap_tests/*
|
||||
|
||||
# Build folder
|
||||
build/*
|
||||
|
||||
# Install folder
|
||||
install/*
|
||||
|
||||
# Cache folder
|
||||
.cache/*
|
||||
|
||||
# Data folder
|
||||
data/*
|
||||
|
||||
# Docs
|
||||
docs/html/*
|
||||
|
||||
# coverage report
|
||||
coverage_report/*
|
||||
.coverage/
|
||||
|
||||
*.pcd
|
||||
@@ -0,0 +1,3 @@
|
||||
[submodule "ufomap"]
|
||||
path = ufomap
|
||||
url = https://github.com/UnknownFreeOccupied/ufomap
|
||||
@@ -0,0 +1,50 @@
|
||||
cmake_minimum_required(VERSION 3.17)
|
||||
|
||||
project(dufomap
|
||||
VERSION 1.0.0
|
||||
DESCRIPTION "DUFOMap: Efficient Dynamic Awareness Mapping"
|
||||
LANGUAGES CXX
|
||||
)
|
||||
set(CMAKE_CXX_STANDARD 20)
|
||||
set(CMAKE_CXX_STANDARD_REQUIRED True)
|
||||
if(NOT CMAKE_BUILD_TYPE)
|
||||
set(CMAKE_BUILD_TYPE Release)
|
||||
endif()
|
||||
|
||||
add_subdirectory(ufomap)
|
||||
include_directories(${CMAKE_SOURCE_DIR}/ufomap)
|
||||
|
||||
add_executable(dufomap_run src/dufomap.cpp)
|
||||
set_target_properties(dufomap_run
|
||||
PROPERTIES
|
||||
VERSION ${PROJECT_VERSION}
|
||||
SOVERSION ${PROJECT_VERSION_MAJOR}
|
||||
CXX_STANDARD 20
|
||||
CXX_STANDARD_REQUIRED YES
|
||||
CXX_EXTENSIONS NO
|
||||
)
|
||||
target_link_libraries(dufomap_run PRIVATE UFO::Map)
|
||||
target_compile_features(dufomap_run
|
||||
PUBLIC
|
||||
cxx_std_20
|
||||
)
|
||||
target_compile_options(dufomap_run
|
||||
PRIVATE
|
||||
# -Wall
|
||||
# -Werror
|
||||
# -Wextra
|
||||
# -Wpedantic
|
||||
# -Wunreachable-code
|
||||
# -Wconversion
|
||||
# -Wcast-align
|
||||
# -Wunused
|
||||
# -Wno-unused-parameter
|
||||
# -Wold-style-cast
|
||||
# -Wpointer-arith
|
||||
# -Wcast-qual
|
||||
# -Wno-missing-braces
|
||||
# -Wdouble-promotion
|
||||
|
||||
# -Wshadow
|
||||
# -march=native
|
||||
)
|
||||
+29
@@ -0,0 +1,29 @@
|
||||
FROM ubuntu:focal
|
||||
LABEL maintainer="Qingwen Zhang <https://kin-zhang.github.io/>"
|
||||
|
||||
# Just in case we need it
|
||||
ENV DEBIAN_FRONTEND noninteractive
|
||||
|
||||
# install g++10 and tbb
|
||||
RUN apt update && apt install -y wget git zsh tmux vim g++ curl unzip
|
||||
RUN apt update && apt install -y gcc-10 g++-10 libtbb-dev liblz4-dev liblzf-dev pkg-config
|
||||
|
||||
# install CMAKE
|
||||
RUN apt update && apt install -y gnupg gnupg2 software-properties-common
|
||||
RUN wget -O - https://apt.kitware.com/keys/kitware-archive-latest.asc 2>/dev/null | gpg --dearmor - | tee /etc/apt/trusted.gpg.d/kitware.gpg >/dev/null
|
||||
RUN apt-add-repository 'deb https://apt.kitware.com/ubuntu/ focal main' && apt update && apt install -y cmake
|
||||
|
||||
# since we will output pcd file, don't want to root to lock it. normally 1000 is the first user in our desktop also
|
||||
RUN useradd -ms /bin/bash -u 1000 kin
|
||||
USER kin
|
||||
|
||||
# setup oh-my-zsh
|
||||
RUN sh -c "$(curl -fsSL https://raw.githubusercontent.com/ohmyzsh/ohmyzsh/master/tools/install.sh)" "" --unattended
|
||||
RUN printf "y\ny\ny\n\n" | bash -c "$(curl -fsSL https://raw.githubusercontent.com/Kin-Zhang/Kin-Zhang/main/scripts/setup_ohmyzsh.sh)"
|
||||
|
||||
RUN mkdir -p /home/kin/workspace && mkdir -p /home/kin/data
|
||||
RUN git clone --recurse-submodules -b main --single-branch https://github.com/KTH-RPL/dufomap /home/kin/workspace/dufomap
|
||||
WORKDIR /home/kin/workspace/dufomap
|
||||
|
||||
# Run container: docker run -it --rm --name dufomap -v /home/kin/data:/home/kin/data zhangkin/dufomap /bin/zsh
|
||||
# you can also run with root user in existing container: docker exec -it -u 0 dufomap /bin/zsh
|
||||
@@ -0,0 +1,28 @@
|
||||
BSD 3-Clause License
|
||||
|
||||
Copyright (c) 2024, Daniel Duberg
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
|
||||
1. Redistributions of source code must retain the above copyright notice, this
|
||||
list of conditions and the following disclaimer.
|
||||
|
||||
2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
this list of conditions and the following disclaimer in the documentation
|
||||
and/or other materials provided with the distribution.
|
||||
|
||||
3. Neither the name of the copyright holder nor the names of its
|
||||
contributors may be used to endorse or promote products derived from
|
||||
this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
|
||||
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
|
||||
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
|
||||
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
|
||||
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
||||
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
@@ -0,0 +1,119 @@
|
||||
<p>
|
||||
<h1 align="center">DUFOMap: Efficient Dynamic Awareness Mapping</h1>
|
||||
</p>
|
||||
|
||||
[](https://arxiv.org/abs/2403.01449)
|
||||
[](https://KTH-RPL.github.io/dufomap)
|
||||
[](https://mit-spark.github.io/Longterm-Perception-WS/assets/proceedings/DUFOMap/poster.pdf)
|
||||
[](https://youtu.be/isDnAVoVD5M)
|
||||
|
||||
Quick Demo: Run with the **same parameter setting** without tuning for different sensor (e.g 16, 32, 64, and 128 channel LiDAR and Livox-series mid360), the following shows the data collected from:
|
||||
|
||||
| Leica-RTC360 | 128-channel LiDAR | Livox-mid360 |
|
||||
| ------- | ------- | ------- |
|
||||
|  |  |  |
|
||||
<!-- | ------- | ------- | ------- | -->
|
||||
|
||||
🚀 2024-11-20: Update dufomap Python API from [SeFlow](https://github.com/KTH-RPL/SeFlow) try it now! `pip install dufomap` and run `python main.py --data_dir data/00` to get the cleaned map directly. Support all >=Python 3.8 in Windows and Linux. Please extract your own data to **the unified format** first follow [this wiki page](https://kth-rpl.github.io/DynamicMap_Benchmark/data/creation/#custom-data).
|
||||
|
||||
|
||||
Clone quickly and init submodules:
|
||||
```bash
|
||||
git clone --recursive -b main --single-branch https://github.com/KTH-RPL/dufomap.git
|
||||
|
||||
|
||||
# The easiest way to run DUFOMap:
|
||||
pip install dufomap
|
||||
python main.py --data_dir data/00
|
||||
```
|
||||
|
||||
|
||||
### Dependencies
|
||||
|
||||
If you want to compile the C++ source version, please install the following dependencies:
|
||||
|
||||
```bash
|
||||
sudo apt update && sudo apt install gcc-10 g++-10
|
||||
sudo apt install libtbb-dev liblz4-dev
|
||||
```
|
||||
|
||||
Or you can directly build docker image through our [Dockerfile](Dockerfile):
|
||||
```bash
|
||||
docker build -t dufomap .
|
||||
```
|
||||
|
||||
### 1. Build & Run
|
||||
|
||||
Build:
|
||||
|
||||
```bash
|
||||
cmake -B build -D CMAKE_CXX_COMPILER=g++-10 && cmake --build build
|
||||
```
|
||||
|
||||
Prepare Data: Teaser data (KITTI 00: 384.4Mb) can be downloaded via follow commands, more data detail can be found in the [dataset section](https://kth-rpl.github.io/DynamicMap_Benchmark/data) or format your own dataset follow [custom dataset section](https://kth-rpl.github.io/DynamicMap_Benchmark/data/creation/#custom-data).
|
||||
|
||||
```bash
|
||||
wget https://zenodo.org/records/8160051/files/00.zip -p data
|
||||
unzip data/00.zip -d data
|
||||
```
|
||||
|
||||
Run:
|
||||
|
||||
```bash
|
||||
./build/dufomap_run data/00 assets/config.toml
|
||||
```
|
||||
|
||||
|
||||
|
||||
## 2. Evaluation
|
||||
|
||||
Please reference to [DynamicMap_Benchmark](https://github.com/KTH-RPL/DynamicMap_Benchmark) for the evaluation of DUFOMap and comparison with other dynamic removal methods.
|
||||
|
||||
[Evaluation Section link](https://github.com/KTH-RPL/DynamicMap_Benchmark/blob/master/scripts/README.md#evaluation)
|
||||
|
||||
|
||||
## Acknowledgements
|
||||
|
||||
Thanks to HKUST Ramlab's members: Bowen Yang, Lu Gan, Mingkai Tang, and Yingbing Chen, who help collect additional datasets.
|
||||
|
||||
This work was partially supported by the Wallenberg AI, Autonomous Systems and Software Program ([WASP](https://wasp-sweden.org/)) funded by the Knut and Alice Wallenberg Foundation including the WASP NEST PerCorSo.
|
||||
|
||||
Feel free to explore below projects that use [ufomap](https://github.com/UnknownFreeOccupied/ufomap) (attach code links as follows):
|
||||
- [RA-L'24 DUFOMap, Dynamic Awareness]()
|
||||
- [RA-L'23 SLICT, SLAM](https://github.com/brytsknguyen/slict)
|
||||
- [RA-L'20 UFOMap, Mapping Framework](https://github.com/UnknownFreeOccupied/ufomap)
|
||||
|
||||
### Citation
|
||||
|
||||
Please cite our works if you find these useful for your research.
|
||||
|
||||
```
|
||||
@article{daniel2024dufomap,
|
||||
author={Duberg, Daniel and Zhang, Qingwen and Jia, MingKai and Jensfelt, Patric},
|
||||
journal={IEEE Robotics and Automation Letters},
|
||||
title={{DUFOMap}: Efficient Dynamic Awareness Mapping},
|
||||
year={2024},
|
||||
volume={9},
|
||||
number={6},
|
||||
pages={1-8},
|
||||
doi={10.1109/LRA.2024.3387658}
|
||||
}
|
||||
@article{duberg2020ufomap,
|
||||
author={Duberg, Daniel and Jensfelt, Patric},
|
||||
journal={IEEE Robotics and Automation Letters},
|
||||
title={{UFOMap}: An Efficient Probabilistic 3D Mapping Framework That Embraces the Unknown},
|
||||
year={2020},
|
||||
volume={5},
|
||||
number={4},
|
||||
pages={6411-6418},
|
||||
doi={10.1109/LRA.2020.3013861}
|
||||
}
|
||||
@inproceedings{zhang2023benchmark,
|
||||
author={Zhang, Qingwen and Duberg, Daniel and Geng, Ruoyu and Jia, Mingkai and Wang, Lujia and Jensfelt, Patric},
|
||||
booktitle={IEEE 26th International Conference on Intelligent Transportation Systems (ITSC)},
|
||||
title={A Dynamic Points Removal Benchmark in Point Cloud Maps},
|
||||
year={2023},
|
||||
pages={608-614},
|
||||
doi={10.1109/ITSC57777.2023.10422094}
|
||||
}
|
||||
```
|
||||
@@ -0,0 +1,7 @@
|
||||
# WeHub 来源说明
|
||||
|
||||
- 原始项目：`KTH-RPL/dufomap`
|
||||
- 原始仓库：https://github.com/KTH-RPL/dufomap
|
||||
- 导入方式：上游默认分支的最新快照
|
||||
- 原作者、版权和许可证信息以原始仓库及本仓库 LICENSE 为准
|
||||
- 本文件仅用于记录来源，不代表 WeHub 是原项目作者
|
||||
@@ -0,0 +1,40 @@
|
||||
|
||||
[important]
|
||||
resolution = 0.1 # voxel size (meters), v in the paper, default 0.1
|
||||
inflate_unknown = 1 # d_p in the paper, default 1
|
||||
inflate_hits_dist = 0.2 # d_s in the paper, default 0.2
|
||||
|
||||
|
||||
# ----------- No need to modify if you are not professional dufoer or ufoer.
|
||||
# ----------- Deeper Parameter for debugging potential better result --------
|
||||
|
||||
[integration]
|
||||
inflate_unknown_compensation = true
|
||||
ray_passthrough_hits = false
|
||||
down_sampling_method = "center" # ["none", "center", "centroid", "uniform"]
|
||||
min_range = 0.2 # For filter out the ego points (some SLAM algorithm will include a pose point etc.
|
||||
max_range = -1 # Maximum range to integrate
|
||||
only_valid = false # Only do ray casting for points within 'max_range'
|
||||
hit_depth = 0 # Level of the octree to integrate hits
|
||||
miss_depth = 0 # Level of the octree to integrate misses
|
||||
ray_casting_depth = 0 # Level of the octree to perform ray casting
|
||||
simple_ray_casting = false
|
||||
simple_ray_casting_factor = 1.0
|
||||
parallel = true # Use parallel execution for integration
|
||||
num_threads = 0 # Number of threads to use, 8 times number of physical cores seems to be a good number and it is chosen if 0 is given
|
||||
|
||||
[map]
|
||||
levels = 20 # Levels of the octree
|
||||
|
||||
[dataset]
|
||||
first = 0 # First pcd index
|
||||
last = -1 # Last pcd index (-1 equals all)
|
||||
num = -1 # Number of pcd files to read (-1 equals all)
|
||||
|
||||
[printing]
|
||||
verbose = false
|
||||
debug = false
|
||||
|
||||
[output]
|
||||
filename = "dufomap_output"
|
||||
raycasting = false
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 944 KiB |
Executable
BIN
Binary file not shown.
|
After Width: | Height: | Size: 1.6 MiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.5 MiB |
Executable
BIN
Binary file not shown.
|
After Width: | Height: | Size: 2.1 MiB |
@@ -0,0 +1,82 @@
|
||||
"""
|
||||
# Created: 2024-11-20 13:11
|
||||
# Copyright (C) 2024-now, RPL, KTH Royal Institute of Technology
|
||||
# Author: Qingwen Zhang (https://kin-zhang.github.io/)
|
||||
#
|
||||
# This file is part of DUFOMap (https://github.com/KTH-RPL/dufomap) and
|
||||
# DynamicMap Benchmark (https://github.com/KTH-RPL/DynamicMap_Benchmark) projects.
|
||||
# If you find this repo helpful, please cite the respective publication as
|
||||
# listed on the above website.
|
||||
|
||||
# Description: Output Cleaned Map through Python API.
|
||||
"""
|
||||
from pathlib import Path
|
||||
import os, fire, time
|
||||
import numpy as np
|
||||
from tqdm import tqdm
|
||||
|
||||
from dufomap import dufomap
|
||||
from dufomap.utils import pcdpy3
|
||||
def inv_pose_matrix(pose):
|
||||
inv_pose = np.eye(4)
|
||||
inv_pose[:3, :3] = pose[:3, :3].T
|
||||
inv_pose[:3, 3] = -pose[:3, :3].T.dot(pose[:3, 3])
|
||||
return inv_pose
|
||||
|
||||
MIN_AXIS_RANGE = 0.2 # HARD CODED: remove ego vehicle points
|
||||
MAX_AXIS_RANGE = 50 # HARD CODED: remove far away points
|
||||
|
||||
class DynamicMapData:
|
||||
def __init__(self, directory):
|
||||
self.scene_id = directory.split("/")[-1]
|
||||
self.directory = Path(directory) / "pcd"
|
||||
self.pcd_files = [os.path.join(self.directory, f) for f in sorted(os.listdir(self.directory)) if f.endswith('.pcd')]
|
||||
|
||||
def __len__(self):
|
||||
return len(self.pcd_files)
|
||||
|
||||
def __getitem__(self, index_):
|
||||
res_dict = {
|
||||
'scene_id': self.scene_id,
|
||||
'timestamp': self.pcd_files[index_].split("/")[-1].split(".")[0],
|
||||
}
|
||||
pcd_ = pcdpy3.PointCloud.from_path(self.pcd_files[index_])
|
||||
pc0 = pcd_.np_data[:,:3]
|
||||
res_dict['pc'] = pc0.astype(np.float32)
|
||||
res_dict['pose'] = list(pcd_.viewpoint)
|
||||
return res_dict
|
||||
|
||||
def main_vis(
|
||||
data_dir: str = "/home/kin/data/00",
|
||||
voxel_map: bool = True, # output voxel-level map or raw point-level.
|
||||
):
|
||||
dataset = DynamicMapData(data_dir)
|
||||
|
||||
# STEP 0: initialize
|
||||
mydufo = dufomap(0.1, 0.2, 2, num_threads=12) # resolution, d_s, d_p same with paper.
|
||||
cloud_acc = np.zeros((0, 3), dtype=np.float32)
|
||||
for data_id in (pbar := tqdm(range(0, len(dataset)),ncols=100)):
|
||||
data = dataset[data_id]
|
||||
now_scene_id = data['scene_id']
|
||||
pbar.set_description(f"id: {data_id}, scene_id: {now_scene_id}, timestamp: {data['timestamp']}")
|
||||
norm_pc0 = np.linalg.norm(data['pc'][:, :3] - data['pose'][:3], axis=1)
|
||||
range_mask = (
|
||||
(norm_pc0>MIN_AXIS_RANGE) &
|
||||
(norm_pc0<MAX_AXIS_RANGE)
|
||||
)
|
||||
# STEP 1: integrate point cloud into dufomap
|
||||
mydufo.run(data['pc'][range_mask], data['pose'], cloud_transform = False)
|
||||
# STEP 1: integrate point cloud into dufomap
|
||||
cloud_acc = np.concatenate((cloud_acc, data['pc']), axis=0)
|
||||
|
||||
# STEP 2: propagate
|
||||
mydufo.oncePropagateCluster(if_propagate=True, if_cluster=False)
|
||||
# STEP 3: Map results; You can save the voxel map directly based on the resolution we set before:
|
||||
mydufo.outputMap(cloud_acc, voxel_map=voxel_map)
|
||||
|
||||
mydufo.printDetailTiming()
|
||||
|
||||
if __name__ == "__main__":
|
||||
start_time = time.time()
|
||||
fire.Fire(main_vis)
|
||||
print(f"Time used: {time.time() - start_time:.2f} s")
|
||||
+448
@@ -0,0 +1,448 @@
|
||||
// UFO
|
||||
#include <ufo/map/integration/integration.hpp>
|
||||
#include <ufo/map/integration/integration_parameters.hpp>
|
||||
#include <ufo/map/node.hpp>
|
||||
#include <ufo/map/point.hpp>
|
||||
#include <ufo/map/point_cloud.hpp>
|
||||
#include <ufo/map/points/points_predicate.hpp>
|
||||
#include <ufo/map/predicate/satisfies.hpp>
|
||||
#include <ufo/map/predicate/spatial.hpp>
|
||||
#include <ufo/map/types.hpp>
|
||||
#include <ufo/map/ufomap.hpp>
|
||||
#include <ufo/math/pose6.hpp>
|
||||
#include <ufo/util/timing.hpp>
|
||||
|
||||
// TOML
|
||||
#include "toml.hpp"
|
||||
#include "indicators.hpp"
|
||||
|
||||
// STL
|
||||
#include <cstdio>
|
||||
#include <filesystem>
|
||||
#include <fstream>
|
||||
#include <future>
|
||||
#include <ios>
|
||||
#include <iostream>
|
||||
#include <limits>
|
||||
#include <random>
|
||||
#include <string>
|
||||
#include <type_traits>
|
||||
#include <unordered_map>
|
||||
#include <vector>
|
||||
|
||||
#ifdef UFO_PARALLEL
|
||||
// STL
|
||||
#include <execution>
|
||||
#endif
|
||||
|
||||
struct Dataset {
|
||||
std::size_t first = 0;
|
||||
std::size_t last = -1;
|
||||
std::size_t num = -1;
|
||||
};
|
||||
|
||||
struct Map {
|
||||
ufo::node_size_t resolution = 0.1; // In meters
|
||||
ufo::depth_t levels = 17; // Levels of the octree
|
||||
};
|
||||
|
||||
struct Clustering {
|
||||
bool cluster = false;
|
||||
float max_distance = 0.2f;
|
||||
std::size_t min_points = 0;
|
||||
ufo::depth_t depth = 0;
|
||||
};
|
||||
|
||||
struct Printing {
|
||||
bool verbose = true;
|
||||
bool debug = false;
|
||||
};
|
||||
|
||||
struct Output {
|
||||
std::string filename = "output";
|
||||
bool has_color = false;
|
||||
bool raycasting = false;
|
||||
};
|
||||
|
||||
struct Config {
|
||||
Dataset dataset;
|
||||
Map map;
|
||||
ufo::IntegrationParams integration;
|
||||
bool propagate = false;
|
||||
Clustering clustering;
|
||||
Printing printing;
|
||||
Output output;
|
||||
|
||||
void read(toml::table tbl)
|
||||
{
|
||||
map.resolution = read(tbl["important"]["resolution"], map.resolution);
|
||||
|
||||
dataset.first = read(tbl["dataset"]["first"], dataset.first);
|
||||
dataset.last = read(tbl["dataset"]["last"], dataset.last);
|
||||
dataset.num = read(tbl["dataset"]["num"], dataset.num);
|
||||
map.levels = read(tbl["map"]["levels"], map.levels);
|
||||
|
||||
auto dsm = read(tbl["integration"]["down_sampling_method"], std::string("none"));
|
||||
integration.down_sampling_method =
|
||||
"none" == dsm
|
||||
? ufo::DownSamplingMethod::NONE
|
||||
: ("centroid" == dsm ? ufo::DownSamplingMethod::CENTROID
|
||||
: ("uniform" == dsm ? ufo::DownSamplingMethod::UNIFORM
|
||||
: ufo::DownSamplingMethod::CENTER));
|
||||
integration.hit_depth = read(tbl["integration"]["hit_depth"], integration.hit_depth);
|
||||
integration.miss_depth =
|
||||
read(tbl["integration"]["miss_depth"], integration.miss_depth);
|
||||
integration.min_range = read(tbl["integration"]["min_range"], integration.min_range);
|
||||
integration.max_range = read(tbl["integration"]["max_range"], integration.max_range);
|
||||
integration.inflate_unknown =
|
||||
read(tbl["important"]["inflate_unknown"], integration.inflate_unknown);
|
||||
integration.inflate_unknown_compensation =
|
||||
read(tbl["integration"]["inflate_unknown_compensation"],
|
||||
integration.inflate_unknown_compensation);
|
||||
integration.ray_passthrough_hits = read(tbl["integration"]["ray_passthrough_hits"],
|
||||
integration.ray_passthrough_hits);
|
||||
integration.inflate_hits_dist =
|
||||
read(tbl["important"]["inflate_hits_dist"], integration.inflate_hits_dist);
|
||||
integration.ray_casting_method = read(tbl["integration"]["simple_ray_casting"], true)
|
||||
? ufo::RayCastingMethod::SIMPLE
|
||||
: ufo::RayCastingMethod::PROPER;
|
||||
integration.simple_ray_casting_factor =
|
||||
read(tbl["integration"]["simple_ray_casting_factor"],
|
||||
integration.simple_ray_casting_factor);
|
||||
integration.parallel = tbl["integration"]["parallel"].value_or(integration.parallel);
|
||||
integration.num_threads =
|
||||
read(tbl["integration"]["num_threads"], integration.num_threads);
|
||||
integration.only_valid =
|
||||
read(tbl["integration"]["only_valid"], integration.only_valid);
|
||||
|
||||
propagate = read(tbl["integration"]["propagate"], propagate);
|
||||
|
||||
clustering.cluster = read(tbl["clustering"]["cluster"], clustering.cluster);
|
||||
clustering.max_distance =
|
||||
read(tbl["clustering"]["max_distance"], clustering.max_distance);
|
||||
clustering.min_points = read(tbl["clustering"]["min_points"], clustering.min_points);
|
||||
clustering.depth = read(tbl["clustering"]["depth"], clustering.depth);
|
||||
|
||||
printing.verbose = read(tbl["printing"]["verbose"], printing.verbose);
|
||||
printing.debug = read(tbl["printing"]["debug"], printing.debug);
|
||||
|
||||
output.filename = read(tbl["output"]["filename"], output.filename);
|
||||
output.has_color = read(tbl["output"]["has_color"], output.has_color);
|
||||
output.raycasting = read(tbl["output"]["raycasting"], output.raycasting);
|
||||
}
|
||||
|
||||
void save() const
|
||||
{
|
||||
// TODO: Implement
|
||||
}
|
||||
|
||||
private:
|
||||
template <typename T>
|
||||
std::remove_cvref_t<T> read(toml::node_view<toml::node> node, T&& default_value)
|
||||
{
|
||||
// if (!node.is_value()) {
|
||||
// node.as_array()->push_back("MISSING");
|
||||
// missing_config = true;
|
||||
// std::cout << node << '\n';
|
||||
// return default_value;
|
||||
// }
|
||||
|
||||
return node.value_or(default_value);
|
||||
}
|
||||
|
||||
private:
|
||||
bool missing_config{false};
|
||||
bool wrong_config{false};
|
||||
};
|
||||
|
||||
std::ostream& operator<<(std::ostream& out, Config const& config)
|
||||
{
|
||||
out << "Config\n";
|
||||
|
||||
out << "\tDataset\n";
|
||||
out << "\t\tFirst: " << config.dataset.first << '\n';
|
||||
out << "\t\tLast: ";
|
||||
if (-1 == config.dataset.last) {
|
||||
out << -1 << '\n';
|
||||
} else {
|
||||
out << config.dataset.last << '\n';
|
||||
}
|
||||
out << "\t\tNum: ";
|
||||
if (-1 == config.dataset.num) {
|
||||
out << -1 << '\n';
|
||||
} else {
|
||||
out << config.dataset.num << '\n';
|
||||
}
|
||||
|
||||
out << "\tMap\n";
|
||||
out << "\t\tResolution: " << config.map.resolution << '\n';
|
||||
out << "\t\tLevels: " << +config.map.levels << '\n';
|
||||
|
||||
out << "\tIntegration\n";
|
||||
out << "\t\tDown sampling method: "
|
||||
<< (ufo::DownSamplingMethod::NONE == config.integration.down_sampling_method
|
||||
? "none"
|
||||
: (ufo::DownSamplingMethod::CENTER ==
|
||||
config.integration.down_sampling_method
|
||||
? "center"
|
||||
: (ufo::DownSamplingMethod::CENTROID ==
|
||||
config.integration.down_sampling_method
|
||||
? "centroid"
|
||||
: "uniform")))
|
||||
<< '\n';
|
||||
out << "\t\tHit depth: " << +config.integration.hit_depth << '\n';
|
||||
out << "\t\tMiss depth: " << +config.integration.miss_depth << '\n';
|
||||
out << "\t\tMin range: " << config.integration.min_range << '\n';
|
||||
out << "\t\tMax range: " << config.integration.max_range << '\n';
|
||||
out << "\t\tInflate unknown " << config.integration.inflate_unknown
|
||||
<< '\n';
|
||||
out << "\t\tInflate unknown compensation "
|
||||
<< config.integration.inflate_unknown_compensation << '\n';
|
||||
out << "\t\tRay passthrough hits " << config.integration.ray_passthrough_hits
|
||||
<< '\n';
|
||||
out << "\t\tInflate hits dist " << config.integration.inflate_hits_dist
|
||||
<< '\n';
|
||||
out << "\t\tEarly stop distance: " << config.integration.early_stop_distance
|
||||
<< '\n';
|
||||
out << "\t\tSimple ray casting: " << std::boolalpha
|
||||
<< (ufo::RayCastingMethod::SIMPLE == config.integration.ray_casting_method ? true
|
||||
: false)
|
||||
<< '\n';
|
||||
out << "\t\tSimple ray casting factor: "
|
||||
<< config.integration.simple_ray_casting_factor << '\n';
|
||||
out << "\t\tParallel: " << config.integration.parallel << '\n';
|
||||
out << "\t\tNum threads: " << config.integration.num_threads << '\n';
|
||||
out << "\t\tOnly valid: " << config.integration.only_valid << '\n';
|
||||
out << "\t\tSliding window size: " << config.integration.sliding_window_size
|
||||
<< '\n';
|
||||
out << "\t\tPropagate: " << std::boolalpha << config.propagate
|
||||
<< '\n';
|
||||
|
||||
out << "\tClustering\n";
|
||||
out << "\t\tCluster: " << std::boolalpha << config.clustering.cluster << '\n';
|
||||
out << "\t\tMax distance: " << config.clustering.max_distance << '\n';
|
||||
out << "\t\tMin points: " << config.clustering.min_points << '\n';
|
||||
out << "\t\tDepth: " << +config.clustering.depth << '\n';
|
||||
|
||||
out << "\tPrinting\n";
|
||||
out << "\t\tVerbose: " << std::boolalpha << config.printing.verbose << '\n';
|
||||
out << "\t\tDebug: " << std::boolalpha << config.printing.debug << '\n';
|
||||
|
||||
out << "\tOutput\n";
|
||||
out << "\t\tFilename: " << config.output.filename << '\n';
|
||||
out << "\t\tHas color: " << std::boolalpha << config.output.has_color << '\n';
|
||||
out << "\t\tRaycasting: " << config.output.raycasting << '\n';
|
||||
|
||||
return out;
|
||||
}
|
||||
|
||||
Config readConfig(std::filesystem::path path)
|
||||
{
|
||||
Config config;
|
||||
for (;;) {
|
||||
if (std::filesystem::exists(path)) {
|
||||
toml::table tbl;
|
||||
try {
|
||||
tbl = toml::parse_file((path).string());
|
||||
} catch (toml::parse_error const& err) {
|
||||
std::cerr << "Configuration parsing failed:\n" << err << '\n';
|
||||
exit(1);
|
||||
}
|
||||
|
||||
config.read(tbl);
|
||||
if (config.printing.verbose) {
|
||||
std::cout << "Found: " << (path) << '\n';
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
if (!path.has_parent_path()) {
|
||||
std::cout << "Did not find configuration file, using default.\n";
|
||||
break;
|
||||
}
|
||||
path = path.parent_path();
|
||||
}
|
||||
|
||||
if (config.printing.verbose) {
|
||||
std::cout << config << '\n';
|
||||
}
|
||||
|
||||
return config;
|
||||
}
|
||||
|
||||
ufo::Color randomColor()
|
||||
{
|
||||
static std::random_device rd;
|
||||
static std::mt19937 gen(rd());
|
||||
static std::uniform_int_distribution<ufo::color_t> dis(0, -1);
|
||||
return {dis(gen), dis(gen), dis(gen)};
|
||||
}
|
||||
|
||||
template <class Map>
|
||||
void cluster(Map& map, Clustering const& clustering)
|
||||
{
|
||||
std::unordered_set<ufo::Node> seen;
|
||||
std::vector<ufo::Sphere> queue;
|
||||
|
||||
auto depth = clustering.depth;
|
||||
auto max_distance = clustering.max_distance;
|
||||
auto min_points = clustering.min_points;
|
||||
|
||||
ufo::label_t l{1};
|
||||
for (auto node : map.query(ufo::pred::Leaf(depth) && ufo::pred::SeenFree() &&
|
||||
ufo::pred::HitsMin(1) && ufo::pred::Label(0))) {
|
||||
if (map.label(node.index())) { // FIXME: This is because how the iterator works
|
||||
continue;
|
||||
}
|
||||
|
||||
seen = {node};
|
||||
queue.assign(1, ufo::Sphere(map.center(node), max_distance));
|
||||
|
||||
map.setLabel(node, l, false);
|
||||
|
||||
while (!queue.empty()) {
|
||||
auto p = ufo::pred::Intersects(queue);
|
||||
queue.clear();
|
||||
for (auto const& node : map.query(
|
||||
ufo::pred::Leaf(depth) && ufo::pred::SeenFree() && ufo::pred::HitsMin(1) &&
|
||||
ufo::pred::Label(0) && std::move(p) &&
|
||||
ufo::pred::Satisfies([&seen](auto n) { return seen.insert(n).second; }))) {
|
||||
queue.emplace_back(map.center(node), max_distance);
|
||||
map.setLabel(node, l, false);
|
||||
}
|
||||
}
|
||||
|
||||
if (seen.size() < min_points) {
|
||||
for (auto e : seen) {
|
||||
if (l == map.label(e)) {
|
||||
map.setLabel(e, -1, false);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
l += seen.size() >= min_points;
|
||||
|
||||
map.propagateModified(); // FIXME: Should this be here?
|
||||
}
|
||||
}
|
||||
|
||||
template <class PointCloud>
|
||||
void filterminDistance(PointCloud& cloud, ufo::Point origin, float min_distance)
|
||||
{
|
||||
float sqrt_dist = min_distance * min_distance;
|
||||
std::erase_if(cloud, [origin, sqrt_dist](auto const& point) {
|
||||
return origin.squaredDistance(point) < sqrt_dist;
|
||||
});
|
||||
}
|
||||
|
||||
int main(int argc, char* argv[])
|
||||
{
|
||||
if (1 >= argc) {
|
||||
std::cout << "[ERROR] Please running by: " << argv[0] << " [pcd_folder] [optional: config_file_path]";
|
||||
return 0;
|
||||
}
|
||||
|
||||
std::filesystem::path path(argv[1]);
|
||||
std::string config_file_path;
|
||||
if (argc > 2)
|
||||
config_file_path = argv[2];
|
||||
else
|
||||
config_file_path = std::string(argv[1]) + "/dufomap.toml";
|
||||
|
||||
auto config = readConfig(std::filesystem::path(config_file_path));
|
||||
std::cout << "[LOG] Step 1: Successfully read configuration from: " << config_file_path <<std::endl;
|
||||
ufo::Map<ufo::MapType::SEEN_FREE | ufo::MapType::REFLECTION | ufo::MapType::LABEL> map(
|
||||
config.map.resolution, config.map.levels);
|
||||
map.reserve(100'000'000);
|
||||
|
||||
std::vector<std::filesystem::path> pcds;
|
||||
for (const auto& entry : std::filesystem::directory_iterator(path / "pcd")) {
|
||||
if (!entry.is_regular_file()) {
|
||||
continue;
|
||||
}
|
||||
std::size_t i = std::stoul(entry.path().stem());
|
||||
if (config.dataset.first <= i && config.dataset.last >= i) {
|
||||
pcds.push_back(entry.path().filename());
|
||||
}
|
||||
}
|
||||
std::ranges::sort(pcds);
|
||||
// std::cout << config << std::endl;
|
||||
pcds.resize(std::min(pcds.size(), config.dataset.num));
|
||||
|
||||
ufo::Timing timing;
|
||||
timing.start("Total");
|
||||
|
||||
ufo::PointCloudColor cloud_acc;
|
||||
|
||||
std::cout << "[LOG] Step 2: Starting Processing data from: " << path << '\n';
|
||||
indicators::show_console_cursor(false);
|
||||
indicators::BlockProgressBar bar{
|
||||
indicators::option::BarWidth{50},
|
||||
indicators::option::Start{"["},
|
||||
indicators::option::End{"]"},
|
||||
indicators::option::PrefixText{"[LOG] Running dufomap "},
|
||||
indicators::option::ForegroundColor{indicators::Color::white},
|
||||
indicators::option::ShowElapsedTime{true},
|
||||
indicators::option::ShowRemainingTime{true},
|
||||
indicators::option::FontStyles{std::vector<indicators::FontStyle>{indicators::FontStyle::bold}}
|
||||
};
|
||||
|
||||
for (std::size_t i{}; std::string filename : pcds) {
|
||||
bar.set_progress(100 * i / pcds.size());
|
||||
++i;
|
||||
timing.setTag("Total " + std::to_string(i) + " of " + std::to_string(pcds.size()) +
|
||||
" (" + std::to_string(100 * i / pcds.size()) + "%)");
|
||||
|
||||
ufo::PointCloudColor cloud;
|
||||
ufo::Pose6f viewpoint;
|
||||
timing[1].start("Read");
|
||||
ufo::readPointCloudPCD(path / "pcd" / filename, cloud, viewpoint);
|
||||
timing[1].stop();
|
||||
|
||||
// NOTE(Qingwen): lots of user facing about ego points inside data, we filterminDistance around ego agent.
|
||||
filterminDistance(cloud, viewpoint.translation, config.integration.min_range);
|
||||
|
||||
cloud_acc.insert(std::end(cloud_acc), std::cbegin(cloud), std::cend(cloud));
|
||||
|
||||
ufo::insertPointCloud(map, cloud, viewpoint.translation, config.integration,
|
||||
config.propagate);
|
||||
|
||||
if (config.printing.verbose) {
|
||||
timing[2] = config.integration.timing;
|
||||
timing.print(true, true, 2, 4);
|
||||
}
|
||||
}
|
||||
indicators::show_console_cursor(true);
|
||||
std::cout << "\033[0m\n[LOG] Step 3: Finished Processing data. Start saving map... " << std::endl;
|
||||
// bar.is_completed();
|
||||
if (!config.propagate) {
|
||||
timing[3].start("Propagate");
|
||||
map.propagateModified();
|
||||
timing[3].stop();
|
||||
}
|
||||
|
||||
timing[4].start("Cluster");
|
||||
if (config.clustering.cluster) {
|
||||
cluster(map, config.clustering);
|
||||
}
|
||||
timing[4].stop();
|
||||
|
||||
timing[5].start("Query");
|
||||
ufo::PointCloudColor cloud_static;
|
||||
|
||||
for (auto& p : cloud_acc) {
|
||||
if (!map.seenFree(p))
|
||||
cloud_static.push_back(p);
|
||||
}
|
||||
|
||||
timing[5].stop();
|
||||
|
||||
timing[6].start("write");
|
||||
ufo::writePointCloudPCD(path / (config.output.filename + ".pcd"), cloud_static);
|
||||
timing[6].stop();
|
||||
timing.stop();
|
||||
|
||||
timing[2] = config.integration.timing;
|
||||
timing.print(true, true, 2, 4);
|
||||
std::cout << "[LOG]: Finished! ^v^.. Clean output map with " << cloud_static.size() << " points save in " << path / (config.output.filename + ".pcd") << '\n';
|
||||
}
|
||||
+3248
File diff suppressed because it is too large
Load Diff
+17401
File diff suppressed because it is too large
Load Diff
Reference in New Issue
Block a user