Repository: PRBonn/lidar-visualizer Branch: main Commit: 621656f89300 Files: 20 Total size: 65.4 KB Directory structure: gitextract_9bl9syd_/ ├── .github/ │ └── workflows/ │ ├── pre-commit.yml │ ├── pypi.yml │ └── python.yml ├── .gitignore ├── .pre-commit-config.yaml ├── CITATION.cff ├── LICENSE ├── Makefile ├── README.md ├── pyproject.toml └── src/ └── lidar_visualizer/ ├── __init__.py ├── datasets/ │ ├── __init__.py │ ├── generic.py │ ├── helipr.py │ ├── mcap.py │ ├── ouster.py │ ├── point_cloud2.py │ └── rosbag.py ├── lidar_visualizer.py └── visualizer.py ================================================ FILE CONTENTS ================================================ ================================================ FILE: .github/workflows/pre-commit.yml ================================================ name: Style Check on: push: branches: ["main"] pull_request: branches: ["main"] jobs: pre-commit: name: Pre-commit checks runs-on: ubuntu-latest steps: - uses: actions/checkout@v3 - uses: actions/setup-python@v4 with: python-version: "3.10" - name: set PY run: echo "PY=$(python -VV | sha256sum | cut -d' ' -f1)" >> $GITHUB_ENV - uses: actions/cache@v1 with: path: ~/.cache/pre-commit key: pre-commit|${{ env.PY }}|${{ hashFiles('.pre-commit-config.yaml') }} - uses: pre-commit/action@v3.0.0 ================================================ FILE: .github/workflows/pypi.yml ================================================ name: Publish to PyPI.org on: release: types: [published] push: branches: ["main"] pull_request: branches: ["main"] jobs: pypi: if: github.event_name == 'release' runs-on: ubuntu-latest steps: - uses: actions/checkout@v3 - name: Build sdist run: pipx run build --sdist - name: Build wheel run: pipx run build --wheel - uses: pypa/gh-action-pypi-publish@release/v1 with: password: ${{ secrets.PYPI_API_TOKEN }} ================================================ FILE: .github/workflows/python.yml ================================================ name: Python API on: push: branches: ["main"] pull_request: branches: ["main"] jobs: python_package: runs-on: ${{ matrix.os }} strategy: matrix: os: [ubuntu-22.04, ubuntu-20.04, windows-2022, macos-14] steps: - uses: actions/checkout@v3 - name: Set up Python3 uses: actions/setup-python@v4 with: python-version: "3.10" - name: Install dependencies run: | python -m pip install --upgrade pip - name: Build pip package run: | python -m pip install --verbose . - name: Test installation run: | lidar_visualizer --version ================================================ FILE: .gitignore ================================================ .polyscope.ini imgui.ini # Created by https://www.toptal.com/developers/gitignore/api/python # Edit at https://www.toptal.com/developers/gitignore?templates=python ### Python ### # Byte-compiled / optimized / DLL files __pycache__/ *.py[cod] *$py.class # C extensions *.so # Distribution / packaging .Python build/ develop-eggs/ dist/ downloads/ eggs/ .eggs/ lib/ lib64/ parts/ sdist/ var/ wheels/ share/python-wheels/ *.egg-info/ .installed.cfg *.egg MANIFEST # PyInstaller # Usually these files are written by a python script from a template # before PyInstaller builds the exe, so as to inject date/other infos into it. *.manifest *.spec # Installer logs pip-log.txt pip-delete-this-directory.txt # Unit test / coverage reports htmlcov/ .tox/ .nox/ .coverage .coverage.* .cache nosetests.xml coverage.xml *.cover *.py,cover .hypothesis/ .pytest_cache/ cover/ # Translations *.mo *.pot # Django stuff: *.log local_settings.py db.sqlite3 db.sqlite3-journal # Flask stuff: instance/ .webassets-cache # Scrapy stuff: .scrapy # Sphinx documentation docs/_build/ # PyBuilder .pybuilder/ target/ # Jupyter Notebook .ipynb_checkpoints # IPython profile_default/ ipython_config.py # pyenv # For a library or package, you might want to ignore these files since the code is # intended to run in multiple environments; otherwise, check them in: # .python-version # pipenv # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. # However, in case of collaboration, if having platform-specific dependencies or dependencies # having no cross-platform support, pipenv may install dependencies that don't work, or not # install all needed dependencies. #Pipfile.lock # poetry # Similar to Pipfile.lock, it is generally recommended to include poetry.lock in version control. # This is especially recommended for binary packages to ensure reproducibility, and is more # commonly ignored for libraries. # https://python-poetry.org/docs/basic-usage/#commit-your-poetrylock-file-to-version-control #poetry.lock # pdm # Similar to Pipfile.lock, it is generally recommended to include pdm.lock in version control. #pdm.lock # pdm stores project-wide configurations in .pdm.toml, but it is recommended to not include it # in version control. # https://pdm.fming.dev/#use-with-ide .pdm.toml # PEP 582; used by e.g. github.com/David-OConnor/pyflow and github.com/pdm-project/pdm __pypackages__/ # Celery stuff celerybeat-schedule celerybeat.pid # SageMath parsed files *.sage.py # Environments .env .venv env/ venv/ ENV/ env.bak/ venv.bak/ # Spyder project settings .spyderproject .spyproject # Rope project settings .ropeproject # mkdocs documentation /site # mypy .mypy_cache/ .dmypy.json dmypy.json # Pyre type checker .pyre/ # pytype static type analyzer .pytype/ # Cython debug symbols cython_debug/ # PyCharm # JetBrains specific template is maintained in a separate JetBrains.gitignore that can # be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore # and can be added to the global gitignore or merged into this file. For a more nuclear # option (not recommended) you can uncomment the following to ignore the entire idea folder. #.idea/ ### Python Patch ### # Poetry local configuration file - https://python-poetry.org/docs/configuration/#local-configuration poetry.toml # ruff .ruff_cache/ # LSP config files pyrightconfig.json # End of https://www.toptal.com/developers/gitignore/api/python n ================================================ FILE: .pre-commit-config.yaml ================================================ repos: - repo: https://github.com/pre-commit/pre-commit-hooks rev: v4.4.0 hooks: - id: trailing-whitespace - id: end-of-file-fixer - id: check-yaml - repo: https://github.com/psf/black rev: 23.1.0 hooks: - id: black - repo: https://github.com/pycqa/isort rev: 5.12.0 hooks: - id: isort ================================================ FILE: CITATION.cff ================================================ cff-version: 1.2.0 preferred-citation: title: "KISS-ICP: In Defense of Point-to-Point ICP - Simple, Accurate, and Robust Registration If Done the Right Way" doi: "10.1109/LRA.2023.3236571" year: "2023" type: article journal: "IEEE Robotics and Automation Letters (RA-L)" url: https://www.ipb.uni-bonn.de/wp-content/papercite-data/pdf/vizzo2023ral.pdf codeurl: https://github.com/PRBonn/kiss-icp authors: - family-names: Vizzo given-names: Ignacio - family-names: Guadagnino given-names: Tiziano - family-names: Mersch given-names: Benedikt - family-names: Wiesmann given-names: Louis - family-names: Behley given-names: Jens - family-names: Stachniss given-names: Cyrill ================================================ FILE: LICENSE ================================================ MIT License Copyright (c) 2023 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill Stachniss. Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. ================================================ FILE: Makefile ================================================ install: @pip install --verbose . uninstall: @pip -v uninstall lidar_visualizer clean: @git clean -xff . editable: @pip install --verbose --editable . ================================================ FILE: README.md ================================================ # LiDAR Visualizer 🚀 A flexible, easy-to-use, LiDAR (or any point cloud) visualizer for Linux, Windows, and macOS. ![out](https://user-images.githubusercontent.com/21349875/234777083-eeb4ec57-cb50-4c69-babd-4cc8e63cff86.png) If you also need to obtain poses from your dataset, consider checking out [KISS-ICP](https://github.com/PRBonn/kiss-icp). ## Install (\*) ```sh pip install lidar-visualizer ``` (\*) This package relies on the power of [Open3D](https://www.open3d.org) but does not list it as a dependency. If you haven't installed `open3d` then `pip install open3d` or check [the official instructions](https://www.open3d.org/docs/release/getting_started.html) ## Optional dependencies Depending on the [dataloaders](./src/lidar_visualizer/datasets/) you plan to use you might need to install optional dependencies. The tool will prompt which tools is the one you are requesting and is not accessible, but if you want to go for brute force and install all of it just run: ```sh pip install lidar-visualizer[all] ``` ## Usage ```sh lidar_visualizer --help ``` ## Citation If you use this visualizer for any academic work, please cite our original [paper](https://www.ipb.uni-bonn.de/wp-content/papercite-data/pdf/vizzo2023ral.pdf). ```bibtex @article{vizzo2023ral, author = {Vizzo, Ignacio and Guadagnino, Tiziano and Mersch, Benedikt and Wiesmann, Louis and Behley, Jens and Stachniss, Cyrill}, title = {{KISS-ICP: In Defense of Point-to-Point ICP -- Simple, Accurate, and Robust Registration If Done the Right Way}}, journal = {IEEE Robotics and Automation Letters (RA-L)}, pages = {1029--1036}, doi = {10.1109/LRA.2023.3236571}, volume = {8}, number = {2}, year = {2023}, codeurl = {https://github.com/PRBonn/kiss-icp}, } ``` ================================================ FILE: pyproject.toml ================================================ [project] name = "lidar_visualizer" description = "A LiDAR visualization tool for all your datasets" version = "0.1.4" readme = "README.md" license = {file = "LICENSE"} keywords = ["LiDAR", "Robotics", "Visualization"] authors = [ {name = "Ignacio Vizzo", email = "ignaciovizzo@gmail.com"}, ] classifiers = [ "Operating System :: Unix", "Operating System :: MacOS", "Operating System :: Microsoft :: Windows", "Programming Language :: C++", "Programming Language :: Python :: 3", "Programming Language :: Python :: 3.7", "Programming Language :: Python :: 3.8", "Programming Language :: Python :: 3.9", "Programming Language :: Python :: 3.10", "Programming Language :: Python :: 3.11", "Intended Audience :: Developers", "Intended Audience :: Education", "Intended Audience :: Other Audience", "Intended Audience :: Science/Research", "License :: OSI Approved :: MIT License", ] requires-python = ">=3.7" dependencies = [ "natsort", "numpy", "tqdm", "polyscope>=2.2.1", "typer[all]>=0.6.0", ] [project.optional-dependencies] all = [ "pyntcloud", "trimesh", "ouster-sdk", ] [project.urls] homepage = "https://github.com/PRBonn/lidar-visualizer" [project.scripts] lidar_visualizer = "lidar_visualizer.lidar_visualizer:main" [tool.black] line-length = 100 [tool.isort] profile = "black" ================================================ FILE: src/lidar_visualizer/__init__.py ================================================ # MIT License # # Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill # Stachniss. # # Permission is hereby granted, free of charge, to any person obtaining a copy # of this software and associated documentation files (the "Software"), to deal # in the Software without restriction, including without limitation the rights # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell # copies of the Software, and to permit persons to whom the Software is # furnished to do so, subject to the following conditions: # # The above copyright notice and this permission notice shall be included in all # copies or substantial portions of the Software. # # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE # SOFTWARE. __version__ = "0.1.4" ================================================ FILE: src/lidar_visualizer/datasets/__init__.py ================================================ # MIT License # # Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill # Stachniss. # # Permission is hereby granted, free of charge, to any person obtaining a copy # of this software and associated documentation files (the "Software"), to deal # in the Software without restriction, including without limitation the rights # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell # copies of the Software, and to permit persons to whom the Software is # furnished to do so, subject to the following conditions: # # The above copyright notice and this permission notice shall be included in all # copies or substantial portions of the Software. # # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE # SOFTWARE. from pathlib import Path from typing import Dict, List def supported_file_extensions(): return [ "bin", "pcd", "ply", "xyz", "obj", "ctm", "off", "stl", "e57", ] def available_dataloaders() -> List: import os.path import pkgutil pkgpath = os.path.dirname(__file__) dataloaders = [name for _, name, _ in pkgutil.iter_modules([pkgpath])] dataloaders.remove("point_cloud2") return dataloaders def jumpable_dataloaders(): _jumpable_dataloaders = available_dataloaders() _jumpable_dataloaders.remove("mcap") _jumpable_dataloaders.remove("ouster") _jumpable_dataloaders.remove("rosbag") return _jumpable_dataloaders def dataloader_types() -> Dict: import ast import importlib dataloaders = available_dataloaders() _types = {} for dataloader in dataloaders: script = importlib.util.find_spec(f".{dataloader}", __name__).origin with open(script) as f: tree = ast.parse(f.read(), script) classes = [cls for cls in tree.body if isinstance(cls, ast.ClassDef)] _types[dataloader] = classes[0].name # assuming there is only 1 class return _types def dataset_factory(dataloader: str, data_dir: Path, *args, **kwargs): import importlib dataloader_type = dataloader_types()[dataloader] module = importlib.import_module(f".{dataloader}", __name__) assert hasattr(module, dataloader_type), f"{dataloader_type} is not defined in {module}" dataset = getattr(module, dataloader_type) return dataset(data_dir=data_dir, *args, **kwargs) ================================================ FILE: src/lidar_visualizer/datasets/generic.py ================================================ # MIT License # # Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill # Stachniss. # # Permission is hereby granted, free of charge, to any person obtaining a copy # of this software and associated documentation files (the "Software"), to deal # in the Software without restriction, including without limitation the rights # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell # copies of the Software, and to permit persons to whom the Software is # furnished to do so, subject to the following conditions: # # The above copyright notice and this permission notice shall be included in all # copies or substantial portions of the Software. # # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE # SOFTWARE. import importlib import os import sys from pathlib import Path import natsort import numpy as np from lidar_visualizer.datasets import supported_file_extensions class GenericDataset: def __init__(self, data_dir: Path, *_, **__): try: self.o3d = importlib.import_module("open3d") except ModuleNotFoundError as e: raise ModuleNotFoundError( "Open3D is not installed on your system, to fix this either " 'run "pip install open3d" ' "or check https://www.open3d.org/docs/release/getting_started.html" ) from e # Intensity stuff import matplotlib.cm as cm self.cmap = cm.viridis # Config stuff self.sequence_id = os.path.basename(data_dir) self.scans_dir = os.path.join(os.path.realpath(data_dir), "") self.scan_files = np.array( natsort.natsorted( [ os.path.join(self.scans_dir, fn) for fn in os.listdir(self.scans_dir) if any(fn.endswith(ext) for ext in supported_file_extensions()) ] ), dtype=str, ) if len(self.scan_files) == 0: raise ValueError(f"Tried to read point cloud files in {self.scans_dir} but none found") self.file_extension = self.scan_files[0].split(".")[-1] if self.file_extension not in supported_file_extensions(): raise ValueError(f"Supported formats are: {supported_file_extensions()}") # Obtain the pointcloud reader for the given data folder self._read_point_cloud = self._get_point_cloud_reader() def __len__(self): return len(self.scan_files) def __getitem__(self, idx): return self._read_point_cloud(self.scan_files[idx]) def _get_point_cloud_reader(self): """Attempt to guess with try/catch blocks which is the best point cloud reader to use for the given dataset folder. Supported readers so far are: File readers are functions which take a filename as an input and return a tuple of points and colors. - np.fromfile - pye57 - open3d - trimesh.load - PyntCloud """ # 1. The old KITTI format if self.file_extension == "bin": print("[WARNING] Reading .bin files, the only format supported is the KITTI format") def read_kitti_scan(file): points_xyzi = ( np.fromfile(file, dtype=np.float32).reshape((-1, 4)).astype(np.float64) ) points = points_xyzi[:, 0:3] intensity = points_xyzi[:, -1] intensity = intensity / intensity.max() colors = self.cmap(intensity)[:, :3].reshape(-1, 3) return points, colors return read_kitti_scan first_scan_file = self.scan_files[0] tried_libraries = [] missing_libraries = [] # 2 Try with pye57 if self.file_extension == "e57": try: import pye57 def read_e57_scan(file): e57 = pye57.E57(file) point_data = None color_data = None # One e57 file can contain multiple scans, scanned from different positions for i in range(e57.scan_count): i = e57.read_scan(i, colors=True, ignore_missing_fields=True) scan_data = np.stack( [i["cartesianX"], i["cartesianY"], i["cartesianZ"]], axis=1 ) point_data = ( np.concat([point_data, scan_data]) if point_data is not None else scan_data ) try: scan_color_data = np.stack( [i["colorRed"], i["colorGreen"], i["colorBlue"]], axis=1 ) color_data = ( np.concat([color_data, scan_color_data]) if color_data is not None else scan_color_data ) except KeyError: pass # e57 file colors are in 0-255 range color_data = color_data / 255.0 if color_data is not None else None return point_data, color_data return read_e57_scan except ModuleNotFoundError: missing_libraries.append("pye57") print("[WARNING] pye57 not installed") except: tried_libraries.append("pye57") # 3. Try with Open3D try: self.o3d.t.io.read_point_cloud(first_scan_file) def read_scan_with_intensities(file): scan = self.o3d.t.io.read_point_cloud(file) if "colors" in dir(scan.point): scan = scan.to_legacy() return np.asarray(scan.points), np.asarray(scan.colors) if "intensity" in dir(scan.point): intensity = scan.point.intensity.numpy() intensity = intensity / intensity.max() colors = self.cmap(intensity)[:, :, :3].reshape(-1, 3) return scan.point.positions.numpy(), colors # else scan = scan.to_legacy() return np.asarray(scan.points), None return read_scan_with_intensities except ModuleNotFoundError: missing_libraries.append("open3d") except: tried_libraries.append("open3d") # 4. Try with trimesh try: import trimesh trimesh.load(first_scan_file) return lambda file: np.asarray(trimesh.load(file).vertices), None except ModuleNotFoundError: missing_libraries.append("trimesh") except: tried_libraries.append("trimesh") # 5. Try with PyntCloud try: from pyntcloud import PyntCloud PyntCloud.from_file(first_scan_file) return lambda file: PyntCloud.from_file(file).points[["x", "y", "z"]].to_numpy(), None except ModuleNotFoundError: missing_libraries.append("pyntcloud") except: tried_libraries.append("pyntcloud") # If reach this point means that none of the libraries exist/could read the file if not tried_libraries: print( "No 3D library is installed in your system. Install one of the following " "to read the pointclouds" ) print("\n".join(missing_libraries)) else: print("[ERROR] File format not supported") print("Tried to load the point cloud with:") print("\n".join(tried_libraries)) print("Skipped libraries (not installed):") print("\n".join(missing_libraries)) sys.exit(1) ================================================ FILE: src/lidar_visualizer/datasets/helipr.py ================================================ # MIT License # # Copyright (c) 2024 Saurabh Gupta, Ignacio Vizzo, Tiziano Guadagnino, # Benedikt Mersch, Cyrill Stachniss. # # Permission is hereby granted, free of charge, to any person obtaining a copy # of this software and associated documentation files (the "Software"), to deal # in the Software without restriction, including without limitation the rights # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell # copies of the Software, and to permit persons to whom the Software is # furnished to do so, subject to the following conditions: # # The above copyright notice and this permission notice shall be included in all # copies or substantial portions of the Software. # # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE # SOFTWARE. import os import struct import sys from pathlib import Path import natsort import numpy as np from lidar_visualizer.datasets import supported_file_extensions class HeLiPRDataset: def __init__(self, data_dir: Path, *_, **__): # Intensity stuff import matplotlib.cm as cm self.cmap = cm.viridis self.sequence_id = os.path.basename(data_dir) self.scan_files = np.array( natsort.natsorted( [ os.path.join(data_dir, fn) for fn in os.listdir(data_dir) if any(fn.endswith(ext) for ext in supported_file_extensions()) ] ), dtype=str, ) if len(self.scan_files) == 0: raise ValueError(f"Tried to read point cloud files in {data_dir} but none found") self.file_extension = self.scan_files[0].split(".")[-1] if self.file_extension not in supported_file_extensions(): raise ValueError(f"Supported formats are: {supported_file_extensions()}") # Obtain the pointcloud reader for the given data folder if self.sequence_id == "Avia": self.format_string = "fffBBBL" self.intensity_channel = None elif self.sequence_id == "Aeva": self.format_string = "ffffflBf" self.format_string_no_intensity = "ffffflB" self.intensity_channel = 7 elif self.sequence_id == "Ouster": self.format_string = "ffffIHHH" self.intensity_channel = 3 elif self.sequence_id == "Velodyne": self.format_string = "ffffHf" self.intensity_channel = 3 else: print("[ERROR] Unsupported LiDAR Type") sys.exit() def __len__(self): return len(self.scan_files) def __getitem__(self, idx): return self.read_point_cloud(idx) def get_data(self, idx: int): file_path = self.scan_files[idx] list_lines = [] # Special case, see https://github.com/minwoo0611/HeLiPR-File-Player/blob/e8d95e390454ece1415ae9deb51515f63730c10a/src/ROSThread.cpp#L632 if self.sequence_id == "Aeva" and int(Path(file_path).stem) <= 1691936557946849179: self.intensity_channel = None format_string = self.format_string_no_intensity else: format_string = self.format_string chunk_size = struct.calcsize(f"={format_string}") with open(file_path, "rb") as f: binary = f.read() offset = 0 while offset < len(binary): list_lines.append(struct.unpack_from(f"={format_string}", binary, offset)) offset += chunk_size data = np.stack(list_lines) return data def read_point_cloud(self, idx: int): data = self.get_data(idx) points = data[:, :3] colors = None if self.intensity_channel is not None: intensity = data[:, self.intensity_channel] intensity = (intensity - intensity.min()) / (intensity.max() - intensity.min()) colors = self.cmap(intensity)[:, :3].reshape(-1, 3) return points, colors ================================================ FILE: src/lidar_visualizer/datasets/mcap.py ================================================ # MIT License # # Copyright (c) 2023 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill # Stachniss. # # Permission is hereby granted, free of charge, to any person obtaining a copy # of this software and associated documentation files (the "Software"), to deal # in the Software without restriction, including without limitation the rights # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell # copies of the Software, and to permit persons to whom the Software is # furnished to do so, subject to the following conditions: # # The above copyright notice and this permission notice shall be included in all # copies or substantial portions of the Software. # # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE # SOFTWARE. import os import sys class McapDataloader: def __init__(self, data_dir: str, topic: str, *_, **__): """Standalone .mcap dataloader withouth any ROS distribution.""" # First try rosbags from lidar_visualizer.datasets.point_cloud2 import read_point_cloud # Then MCAP support try: from mcap.reader import make_reader from mcap_ros2.reader import read_ros2_messages except ModuleNotFoundError as e: raise ModuleNotFoundError( "mcap plugins not installed: 'pip install mcap-ros2-support'" ) from e # we expect `data_dir` param to be a path to the .mcap file, so rename for clarity assert os.path.isfile(data_dir), "mcap dataloader expects an existing MCAP file" self.sequence_id = os.path.basename(data_dir).split(".")[0] mcap_file = str(data_dir) self.bag = make_reader(open(mcap_file, "rb")) self.summary = self.bag.get_summary() self.topic = self.check_topic(topic) self.n_scans = self._get_n_scans() self.msgs = read_ros2_messages(mcap_file, topics=self.topic) self.read_point_cloud = read_point_cloud self.use_global_visualizer = True def __del__(self): if hasattr(self, "bag"): del self.bag def __getitem__(self, idx): msg = next(self.msgs).ros_msg return self.read_point_cloud(msg) def __len__(self): return self.n_scans def _get_n_scans(self) -> int: return sum( count for (id, count) in self.summary.statistics.channel_message_counts.items() if self.summary.channels[id].topic == self.topic ) def check_topic(self, topic: str) -> str: # Extract schema id from the .mcap file that encodes the PointCloud2 msg schema_id = [ schema.id for schema in self.summary.schemas.values() if schema.name == "sensor_msgs/msg/PointCloud2" ][0] point_cloud_topics = [ channel.topic for channel in self.summary.channels.values() if channel.schema_id == schema_id ] def print_available_topics_and_exit(): print(50 * "-") for t in point_cloud_topics: print(f"--topic {t}") print(50 * "-") sys.exit(1) if topic and topic in point_cloud_topics: return topic # when user specified the topic check that exists if topic and topic not in point_cloud_topics: print( f'[ERROR] Dataset does not containg any msg with the topic name "{topic}". ' "Please select one of the following topics with the --topic flag" ) print_available_topics_and_exit() if len(point_cloud_topics) > 1: print( "Multiple sensor_msgs/msg/PointCloud2 topics available." "Please select one of the following topics with the --topic flag" ) print_available_topics_and_exit() if len(point_cloud_topics) == 0: print("[ERROR] Your dataset does not contain any sensor_msgs/msg/PointCloud2 topic") if len(point_cloud_topics) == 1: return point_cloud_topics[0] ================================================ FILE: src/lidar_visualizer/datasets/ouster.py ================================================ # MIT License # # Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill # Stachniss. # Copyright (c) 2023 Pavlo Bashmakov # # Permission is hereby granted, free of charge, to any person obtaining a copy # of this software and associated documentation files (the "Software"), to deal # in the Software without restriction, including without limitation the rights # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell # copies of the Software, and to permit persons to whom the Software is # furnished to do so, subject to the following conditions: # # The above copyright notice and this permission notice shall be included in all # copies or substantial portions of the Software. # # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE # SOFTWARE. import glob import os from typing import Optional def find_metadata_json(pcap_file: str) -> str: """Attempts to resolve the metadata json file for a provided pcap file.""" dir_path, filename = os.path.split(pcap_file) if not filename: return "" if not dir_path: dir_path = os.getcwd() json_candidates = sorted(glob.glob(f"{dir_path}/*.json")) if not json_candidates: return "" prefix_sizes = list( map(lambda p: len(os.path.commonprefix((filename, os.path.basename(p)))), json_candidates) ) max_elem = max(range(len(prefix_sizes)), key=lambda i: prefix_sizes[i]) return json_candidates[max_elem] class OusterDataloader: """Ouster pcap dataloader""" def __init__( self, data_dir: str, meta: Optional[str] = None, *_, **__, ): """Create Ouster pcap dataloader to read scans from a pcap file. Ouster pcap can be recorded with a `tcpdump` command or programmatically. Pcap file should contain raw lidar_packets and `meta` file (i.e. metadata.json) should be a corresponding sensor metadata stored at the time of pcap recording. NOTE: It's critical to have a metadata json stored in the same recording session as a pcap file, because pcap reader checks the `init_id` field in the UDP lidar_packets and expects it to match `initialization_id` in the metadata json, packets with different `init_id` just skipped. Metadata json can be obtainer with Ouster SDK: See examples here https://static.ouster.dev/sdk-docs/python/examples/basics-sensor.html#obtaining-sensor-metadata or with Sensor HTTP API endpoint GET /api/v1/sensor/metadata directly: See doc for details https://static.ouster.dev/sensor-docs/image_route1/image_route2/common_sections/API/http-api-v1.html#get-api-v1-sensor-metadata Args: data_dir: path to a pcap file (not a directory) meta: path to a metadata json file that should be recorded together with a pcap file. If `meta` is not provided attempts to find the best matching json file with the longest commong prefix of the pcap file (`data_dir`) in the same directory. """ try: import ouster.pcap as pcap from ouster import client from ouster.client import _utils from ouster.sdk.examples.colormaps import colorize except ModuleNotFoundError as e: raise ModuleNotFoundError( f'ouster-sdk is not installed on your system, run "pip install ouster-sdk"' ) from e # since we import ouster-sdk's client module locally, we keep it locally as well self._client = client self._colorize = colorize self._utils = _utils assert os.path.isfile(data_dir), "Ouster pcap dataloader expects an existing PCAP file" # we expect `data_dir` param to be a path to the .pcap file, so rename for clarity pcap_file = data_dir metadata_json = meta or find_metadata_json(pcap_file) if not metadata_json: print("Ouster pcap dataloader can't find metadata json file.") exit(1) print("Ouster pcap dataloader: using metadata json: ", metadata_json) self.data_dir = os.path.dirname(data_dir) with open(metadata_json) as json: self._info_json = json.read() self._info = client.SensorInfo(self._info_json) # lookup table for 2D range image projection to a 3D point cloud self._xyz_lut = client.XYZLut(self._info) self._pcap_file = str(data_dir) # read pcap file for the first pass to count scans print("Pre-reading Ouster pcap to count the scans number ...") self._source = pcap.Pcap(self._pcap_file, self._info) self._scans_num = sum((1 for _ in client.Scans(self._source))) print(f"Ouster pcap total scans number: {self._scans_num}") # start Scans iterator for consumption in __getitem__ self._source = pcap.Pcap(self._pcap_file, self._info) self._scans_iter = iter(client.Scans(self._source)) self._next_idx = 0 def get_color_image(self, scan): """This function was taken from the Ouster SDK. All rights reserved to Ouster, Inc https://github.com/ouster-lidar/ouster_example/blob/master/python/src/ouster/sdk/examples/open3d.py """ fields = list(scan.fields) aes = {} for field_ind, field in enumerate(fields): if field in (self._client.ChanField.SIGNAL, self._client.ChanField.SIGNAL2): aes[field_ind] = self._utils.AutoExposure(0.02, 0.1, 3) else: aes[field_ind] = self._utils.AutoExposure() field_ind = 2 # Obtain reflectivity for colorizing the cloud key = scan.field(fields[field_ind]).astype(float) aes[field_ind](key) return self._colorize(key) def __getitem__(self, idx): # we assume that users always reads sequentially and do not # pass idx as for a random access collection assert self._next_idx == idx, ( "Ouster pcap dataloader supports only sequential reads. " f"Expected idx: {self._next_idx}, but got {idx}" ) scan = next(self._scans_iter) self._next_idx += 1 # filtering our zero returns makes it substantially faster for kiss-icp sel_flag = scan.field(self._client.ChanField.RANGE) != 0 # Extract XYZ and Intensity channels form scan xyz = self._xyz_lut(scan)[sel_flag] ref = self.get_color_image(scan)[sel_flag] points = xyz.reshape((-1, 3)) colors = ref.reshape((-1, 3)) return points, colors def __len__(self): return self._scans_num ================================================ FILE: src/lidar_visualizer/datasets/point_cloud2.py ================================================ # Copyright 2008 Willow Garage, Inc. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: # # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. # # * 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. # # * Neither the name of the Willow Garage, Inc. 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. """ This file is based on https://github.com/ros2/common_interfaces/blob/4bac182a0a582b5e6b784d9fa9f0dabc1aca4d35/sensor_msgs_py/sensor_msgs_py/point_cloud2.py All rights reserved to the original authors: Tim Field and Florian Vahl. The current implementation is based on the one from the KISS-ICP project, but modified """ import sys from typing import Iterable, List, Optional import matplotlib.cm as cm import numpy as np try: from rosbags.typesys.types import sensor_msgs__msg__PointCloud2 as PointCloud2 from rosbags.typesys.types import sensor_msgs__msg__PointField as PointField except ModuleNotFoundError as e: raise ModuleNotFoundError('rosbags library not installed, run "pip install -U rosbags"') from e _DATATYPES = {} _DATATYPES[PointField.INT8] = np.dtype(np.int8) _DATATYPES[PointField.UINT8] = np.dtype(np.uint8) _DATATYPES[PointField.INT16] = np.dtype(np.int16) _DATATYPES[PointField.UINT16] = np.dtype(np.uint16) _DATATYPES[PointField.INT32] = np.dtype(np.int32) _DATATYPES[PointField.UINT32] = np.dtype(np.uint32) _DATATYPES[PointField.FLOAT32] = np.dtype(np.float32) _DATATYPES[PointField.FLOAT64] = np.dtype(np.float64) DUMMY_FIELD_PREFIX = "unnamed_field" def read_point_cloud(msg: PointCloud2): field_names = ["x", "y", "z"] intensity_field = None for field in msg.fields: if field.name in ["intensity"]: intensity_field = field.name field_names.append(intensity_field) break points_structured = read_points(msg, field_names=field_names) points = np.column_stack( [ points_structured["x"], points_structured["y"], points_structured["z"], ] ).astype(np.float64) colors = None if intensity_field: intensity = points_structured[intensity_field].astype(np.float64) intensity = intensity / intensity.max() colors = cm.viridis(intensity)[:, :3].reshape(-1, 3) return points, colors def read_points( cloud: PointCloud2, field_names: Optional[List[str]] = None, uvs: Optional[Iterable] = None, reshape_organized_cloud: bool = False, ) -> np.ndarray: """ Read points from a sensor_msgs.PointCloud2 message. :param cloud: The point cloud to read from sensor_msgs.PointCloud2. :param field_names: The names of fields to read. If None, read all fields. (Type: Iterable, Default: None) :param uvs: If specified, then only return the points at the given coordinates. (Type: Iterable, Default: None) :param reshape_organized_cloud: Returns the array as an 2D organized point cloud if set. :return: Structured NumPy array containing all points. """ # Cast bytes to numpy array points = np.ndarray( shape=(cloud.width * cloud.height,), dtype=dtype_from_fields(cloud.fields, point_step=cloud.point_step), buffer=cloud.data, ) # Keep only the requested fields if field_names is not None: assert all( field_name in points.dtype.names for field_name in field_names ), "Requests field is not in the fields of the PointCloud!" # Mask fields points = points[list(field_names)] # Swap array if byte order does not match if bool(sys.byteorder != "little") != bool(cloud.is_bigendian): points = points.byteswap(inplace=True) # Select points indexed by the uvs field if uvs is not None: # Don't convert to numpy array if it is already one if not isinstance(uvs, np.ndarray): uvs = np.fromiter(uvs, int) # Index requested points points = points[uvs] # Cast into 2d array if cloud is 'organized' if reshape_organized_cloud and cloud.height > 1: points = points.reshape(cloud.width, cloud.height) return points def dtype_from_fields(fields: Iterable[PointField], point_step: Optional[int] = None) -> np.dtype: """ Convert a Iterable of sensor_msgs.msg.PointField messages to a np.dtype. :param fields: The point cloud fields. (Type: iterable of sensor_msgs.msg.PointField) :param point_step: Point step size in bytes. Calculated from the given fields by default. (Type: optional of integer) :returns: NumPy datatype """ # Create a lists containing the names, offsets and datatypes of all fields field_names = [] field_offsets = [] field_datatypes = [] for i, field in enumerate(fields): # Datatype as numpy datatype datatype = _DATATYPES[field.datatype] # Name field if field.name == "": name = f"{DUMMY_FIELD_PREFIX}_{i}" else: name = field.name # Handle fields with count > 1 by creating subfields with a suffix consiting # of "_" followed by the subfield counter [0 -> (count - 1)] assert field.count > 0, "Can't process fields with count = 0." for a in range(field.count): # Add suffix if we have multiple subfields if field.count > 1: subfield_name = f"{name}_{a}" else: subfield_name = name assert subfield_name not in field_names, "Duplicate field names are not allowed!" field_names.append(subfield_name) # Create new offset that includes subfields field_offsets.append(field.offset + a * datatype.itemsize) field_datatypes.append(datatype.str) # Create dtype dtype_dict = {"names": field_names, "formats": field_datatypes, "offsets": field_offsets} if point_step is not None: dtype_dict["itemsize"] = point_step return np.dtype(dtype_dict) ================================================ FILE: src/lidar_visualizer/datasets/rosbag.py ================================================ # MIT License # # Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill # Stachniss. # # Permission is hereby granted, free of charge, to any person obtaining a copy # of this software and associated documentation files (the "Software"), to deal # in the Software without restriction, including without limitation the rights # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell # copies of the Software, and to permit persons to whom the Software is # furnished to do so, subject to the following conditions: # # The above copyright notice and this permission notice shall be included in all # copies or substantial portions of the Software. # # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE # SOFTWARE. import os import sys from pathlib import Path from typing import Sequence import natsort class RosbagDataset: def __init__(self, data_dir: Sequence[Path], topic: str, *_, **__): """ROS1 / ROS2 bagfile dataloader. It can take either one ROS2 bag file or one or more ROS1 bag files belonging to a split bag. The reader will replay ROS1 split bags in correct timestamp order. TODO: Merge mcap and rosbag dataloaders into 1 """ try: from rosbags.highlevel import AnyReader except ModuleNotFoundError as e: raise ModuleNotFoundError( 'rosbags library not installed, run "pip install -U rosbags"' ) from e from lidar_visualizer.datasets.point_cloud2 import read_point_cloud self.read_point_cloud = read_point_cloud # FIXME: This is quite hacky, trying to guess if we have multiple .bag, one or a dir if isinstance(data_dir, Path): self.sequence_id = os.path.basename(data_dir).split(".")[0] self.bag = AnyReader([data_dir]) else: self.sequence_id = os.path.basename(data_dir[0]).split(".")[0] self.bag = AnyReader(data_dir) print("Reading multiple .bag files in directory:") print("\n".join(natsort.natsorted([path.name for path in self.bag.paths]))) self.bag.open() self.topic = self.check_topic(topic) self.n_scans = self.bag.topics[self.topic].msgcount # limit connections to selected topic connections = [x for x in self.bag.connections if x.topic == self.topic] self.msgs = self.bag.messages(connections=connections) self.timestamps = [] # Visualization Options self.use_global_visualizer = True def __del__(self): if hasattr(self, "bag"): self.bag.close() def __len__(self): return self.n_scans def __getitem__(self, idx): connection, timestamp, rawdata = next(self.msgs) self.timestamps.append(self.to_sec(timestamp)) msg = self.bag.deserialize(rawdata, connection.msgtype) return self.read_point_cloud(msg) @staticmethod def to_sec(nsec: int): return float(nsec) / 1e9 def get_frames_timestamps(self) -> list: return self.timestamps def check_topic(self, topic: str) -> str: # Extract all PointCloud2 msg topics from the bagfile point_cloud_topics = [ topic[0] for topic in self.bag.topics.items() if topic[1].msgtype == "sensor_msgs/msg/PointCloud2" ] def print_available_topics_and_exit(): print(50 * "-") for t in point_cloud_topics: print(f"--topic {t}") print(50 * "-") sys.exit(1) if topic and topic in point_cloud_topics: return topic # when user specified the topic check that exists if topic and topic not in point_cloud_topics: print( f'[ERROR] Dataset does not containg any msg with the topic name "{topic}". ' "Please select one of the following topics with the --topic flag" ) print_available_topics_and_exit() if len(point_cloud_topics) > 1: print( "Multiple sensor_msgs/msg/PointCloud2 topics available." "Please select one of the following topics with the --topic flag" ) print_available_topics_and_exit() if len(point_cloud_topics) == 0: print("[ERROR] Your dataset does not contain any sensor_msgs/msg/PointCloud2 topic") if len(point_cloud_topics) == 1: return point_cloud_topics[0] ================================================ FILE: src/lidar_visualizer/lidar_visualizer.py ================================================ # MIT License # # Copyright (c) 2023 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill # Stachniss. # # Permission is hereby granted, free of charge, to any person obtaining a copy # of this software and associated documentation files (the "Software"), to deal # in the Software without restriction, including without limitation the rights # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell # copies of the Software, and to permit persons to whom the Software is # furnished to do so, subject to the following conditions: # # The above copyright notice and this permission notice shall be included in all # copies or substantial portions of the Software. # # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE # SOFTWARE. import glob import os from pathlib import Path from typing import Optional import typer from lidar_visualizer.datasets import ( available_dataloaders, dataset_factory, jumpable_dataloaders, supported_file_extensions, ) from lidar_visualizer.visualizer import Visualizer def version_callback(value: bool): if value: import lidar_visualizer print(f"Lidar Visualizer Version: {lidar_visualizer.__version__}") raise typer.Exit(0) def guess_dataloader(data: Path, default_dataloader: str): if data.is_file(): if data.name == "metadata.yaml": return "rosbag", data.parent # database is in directory, not in .yml if data.name.split(".")[-1] in "bag": return "rosbag", data if data.name.split(".")[-1] == "pcap": return "ouster", data if data.name.split(".")[-1] == "mcap": return "mcap", data elif data.is_dir(): if (data / "metadata.yaml").exists(): # a directory with a metadata.yaml must be a ROS2 bagfile return "rosbag", data bagfiles = [Path(path) for path in glob.glob(os.path.join(data, "*.bag"))] if len(bagfiles) > 0: return "rosbag", bagfiles return default_dataloader, data def name_callback(value: str): if not value: return value dl = available_dataloaders() if value not in dl: raise typer.BadParameter(f"Supported dataloaders are:\n{', '.join(dl)}") return value docstring = f""" :kiss: LiDAR visualizer :kiss:\n \b [bold green]Examples: [/bold green] # Visualize all pointclouds in the given \[{", ".join(supported_file_extensions())}] $ lidar_visualizer :open_file_folder: # Visualize a given [bold]ROS1/ROS2 [/bold]rosbag file (directory:open_file_folder:, ".bag":page_facing_up:, or "metadata.yaml":page_facing_up:) $ lidar_visualizer [:open_file_folder:/:page_facing_up:] # Visualize [bold]mcap [/bold] recording $ lidar_visualizer :page_facing_up: # Visualize [bold]Ouster pcap[/bold] recording (requires ouster-sdk Python package installed) $ lidar_visualizer :page_facing_up: \[--meta :page_facing_up:] """ app = typer.Typer(add_completion=False, rich_markup_mode="rich") @app.command(help=docstring) def lidar_visualizer( data: Path = typer.Argument( ..., help="The data directory used by the specified dataloader", show_default=False, ), dataloader: str = typer.Option( None, show_default=False, case_sensitive=False, autocompletion=available_dataloaders, callback=name_callback, help="[Optional] Use a specific dataloader from those supported by lidar-visualizer", ), topic: Optional[str] = typer.Option( None, "--topic", "-t", show_default=False, help="[Optional] Only valid when processing rosbag files", rich_help_panel="Additional Options", ), n_scans: int = typer.Option( -1, "--n-scans", "-n", show_default=False, help="[Optional] Specify the number of scans to process, default is the entire dataset", rich_help_panel="Additional Options", ), jump: int = typer.Option( 0, "--jump", "-j", show_default=False, help="[Optional] Specify if you want to start to process scans from a given starting point", rich_help_panel="Additional Options", ), meta: Optional[Path] = typer.Option( None, "--meta", "-m", exists=True, show_default=False, help="[Optional] For Ouster pcap dataloader, specify metadata json file path explicitly", rich_help_panel="Additional Options", ), version: Optional[bool] = typer.Option( None, "--version", help="Show the current version of lidar-visualizer", callback=version_callback, is_eager=True, ), ): if not dataloader: dataloader, data = guess_dataloader(data, default_dataloader="generic") Visualizer( dataset=dataset_factory( dataloader=dataloader, data_dir=data, # Additional options topic=topic, meta=meta, ), random_accessible_dataset=dataloader in jumpable_dataloaders(), n_scans=n_scans, jump=jump, ).run() def main(): app() ================================================ FILE: src/lidar_visualizer/visualizer.py ================================================ # MIT License # # Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill # Stachniss. # Copyright (c) 2024 Luca Lobefaro # # Permission is hereby granted, free of charge, to any person obtaining a copy # of this software and associated documentation files (the "Software"), to deal # in the Software without restriction, including without limitation the rights # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell # copies of the Software, and to permit persons to whom the Software is # furnished to do so, subject to the following conditions: # # The above copyright notice and this permission notice shall be included in all # copies or substantial portions of the Software. # # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE # SOFTWARE. import importlib import os import time # Button names START_BUTTON = "START [SPACE]" PAUSE_BUTTON = "PAUSE [SPACE]" NEXT_FRAME_BUTTON = "NEXT FRAME [N]" PREVIOUS_FRAME_BUTTON = "PREVIOUS FRAME [P]" CENTER_VIEWPOINT_BUTTON = "CENTER VIEWPOINT [C]" QUIT_BUTTON = "QUIT [Q]" # Colors BACKGROUND_COLOR = [0.0, 0.0, 0.0] FRAME_COLOR = [0.8470, 0.1058, 0.3764] # Only used if no color in original cloud # Size constants FRAME_PTS_SIZE_N_STEPS = 20 FRAME_PTS_SIZE_MIN = 0.005 FRAME_PTS_SIZE_MAX = 0.1 class Visualizer: def __init__(self, dataset, random_accessible_dataset: bool, n_scans: int = -1, jump: int = 0): try: self._ps = importlib.import_module("polyscope") self._gui = self._ps.imgui except ModuleNotFoundError: print(f'polyscope is not installed on your system, run "pip install polyscope"') exit(1) # Initialize GUI controls self._background_color = BACKGROUND_COLOR self._frame_size_step = (FRAME_PTS_SIZE_MAX - FRAME_PTS_SIZE_MIN) / ( FRAME_PTS_SIZE_N_STEPS - 1 ) self._frame_size = 0.5 * FRAME_PTS_SIZE_N_STEPS * self._frame_size_step self._play_mode = False self._toggle_frame = True self._playback_delay = 0.0 # Initialize dataset and fix input based on its nature self._dataset = dataset self._random_accessible_dataset = random_accessible_dataset self.start_idx = min(jump, len(self._dataset) - 1) if self._random_accessible_dataset else 0 self.n_scans = len(self._dataset) if n_scans == -1 else min(len(self._dataset), n_scans) self.stop_idx = min(len(self._dataset), self.n_scans + self.start_idx) self.idx = self.start_idx self.current_filename = self._get_current_filename(self.idx) self.end_reached = False # Initialize visualizer self._initialize_visualizer() def run(self): while True: self.update() self.advance() def update(self): self._update_visualized_frame() while True: time.sleep(self._playback_delay) self._ps.frame_tick() if self._play_mode and not self.end_reached: break def advance(self): self.idx = self.start_idx if self.idx == self.stop_idx - 1 else self.idx + 1 self.end_reached = self.idx == self.stop_idx - 1 and not self._random_accessible_dataset def rewind(self): self.idx = self.stop_idx - 1 if self.idx == self.start_idx else self.idx - 1 # Private Interface --------------------------------------------------------------------------- def _initialize_visualizer(self): self._ps.set_program_name("LIDAR Visualizer") self._ps.init() self._ps.set_ground_plane_mode("none") self._ps.set_background_color(BACKGROUND_COLOR) self._ps.set_verbosity(0) self._ps.set_user_callback(self._main_gui_callback) self._ps.set_build_default_gui_panels(False) def _get_current_filename(self, idx): # Try to fetch the current filename try: filename = self._dataset.scan_files[idx] return os.path.splitext(os.path.basename(filename))[0] except: return None def _get_frame(self, idx): # Let's do a bit of duck typing to support eating different monsters dataframe = self._dataset[idx] points, colors = dataframe return points, colors def _update_visualized_frame(self): self.current_filename = self._get_current_filename(self.idx) points, colors = self._get_frame(self.idx) self._register_frame(points, colors) def _register_frame(self, points, colors): frame_cloud = self._ps.register_point_cloud( "current_frame", points, point_render_mode="quad", ) if colors is None: frame_cloud.set_color(FRAME_COLOR) else: frame_cloud.add_color_quantity("colors", colors, enabled=True) frame_cloud.set_radius(self._frame_size, relative=False) frame_cloud.set_enabled(self._toggle_frame) # GUI Callbacks --------------------------------------------------------------------------- def _main_gui_callback(self): self._gui.TextUnformatted("Controls:") if not self.end_reached: self._start_pause_callback() if not self._play_mode: self._gui.SameLine() self._next_frame_callback() if self._random_accessible_dataset: self._gui.SameLine() self._previous_frame_callback() self._gui.Separator() self._progress_bar_callback() self._playback_delay_callback() self._gui.Separator() self._gui.TextUnformatted("Scene Options:") self._background_color_callback() self._points_controlles_callback() if not self._random_accessible_dataset: self._gui.Separator() self._information_callback() self._gui.Separator() self._center_viewpoint_callback() self._gui.SameLine() self._quit_callback() def _start_pause_callback(self): button_name = PAUSE_BUTTON if self._play_mode else START_BUTTON if self._gui.Button(button_name) or self._gui.IsKeyPressed(self._gui.ImGuiKey_Space): self._play_mode = not self._play_mode def _next_frame_callback(self): if self._gui.Button(NEXT_FRAME_BUTTON) or self._gui.IsKeyPressed(self._gui.ImGuiKey_N): self.advance() self._update_visualized_frame() def _previous_frame_callback(self): if self._gui.Button(PREVIOUS_FRAME_BUTTON) or self._gui.IsKeyPressed(self._gui.ImGuiKey_P): self.rewind() self._update_visualized_frame() def _progress_bar_callback(self): changed, idx = self._gui.SliderInt( f"\t{self.stop_idx} Frames###Progress Bar", self.idx, v_min=self.start_idx, v_max=self.stop_idx - 1, format="Frame: %d", ) if changed and self._random_accessible_dataset: self.idx = idx self._update_visualized_frame() def _playback_delay_callback(self): _, self._playback_delay = self._gui.SliderFloat( "\tPlayback Delay", self._playback_delay, v_min=0.0, v_max=0.1, format="%.2f s", ) def _points_controlles_callback(self): key_changed = False if self._gui.IsKeyPressed(self._gui.ImGuiKey_Minus): self._frame_size = max(FRAME_PTS_SIZE_MIN, self._frame_size - self._frame_size_step) key_changed = True if self._gui.IsKeyPressed(self._gui.ImGuiKey_Equal): self._frame_size = min(FRAME_PTS_SIZE_MAX, self._frame_size + self._frame_size_step) key_changed = True changed, self._frame_size = self._gui.SliderFloat( "Points Size", self._frame_size, v_min=FRAME_PTS_SIZE_MIN, v_max=FRAME_PTS_SIZE_MAX ) if changed or key_changed: self._ps.get_point_cloud("current_frame").set_radius(self._frame_size, relative=False) def _background_color_callback(self): changed, self._background_color = self._gui.ColorEdit3( "Background Color", self._background_color, ) if changed: self._ps.set_background_color(self._background_color) def _information_callback(self): self._gui.TextUnformatted( f"[WARNING] The current dataloader does not allow you to access frames\nrandomly..." ) def _center_viewpoint_callback(self): if self._gui.Button(CENTER_VIEWPOINT_BUTTON) or self._gui.IsKeyPressed( self._gui.ImGuiKey_C ): self._ps.reset_camera_to_home_view() def _quit_callback(self): posX = ( self._gui.GetCursorPosX() + self._gui.GetColumnWidth() - self._gui.CalcTextSize(QUIT_BUTTON)[0] - self._gui.GetScrollX() - self._gui.ImGuiStyleVar_ItemSpacing ) self._gui.SetCursorPosX(posX) if ( self._gui.Button(QUIT_BUTTON) or self._gui.IsKeyPressed(self._gui.ImGuiKey_Escape) or self._gui.IsKeyPressed(self._gui.ImGuiKey_Q) ): print("Destroying Visualizer") self._ps.unshow() os._exit(0)