├── .gitignore ├── Dockerfile ├── LICENSE ├── README.md ├── README_apriltag_extraction.md ├── apriltag_extraction.Dockerfile ├── compose.yaml ├── config └── extract_all_apriltag_coords.yaml ├── entrypoint.bash ├── entrypoint_apriltag_extraction.bash ├── input └── .gitignore ├── output ├── .gitignore └── apriltag_extraction │ └── .gitignore ├── pyproject.toml ├── reference └── .gitignore ├── scripts ├── extract_all_apriltag_coords_on_scan.py └── lcba.py └── src └── ipb_calibration ├── apriltag.py ├── camera.py ├── create_orthophoto_images.py ├── detect_apriltags.py ├── io.py ├── ipb_preprocessing.py ├── lc_bundle_adjustment.py ├── lidar.py └── utils.py /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | # Distribution / packaging 10 | .Python 11 | build/ 12 | develop-eggs/ 13 | dist/ 14 | downloads/ 15 | eggs/ 16 | .eggs/ 17 | lib/ 18 | lib64/ 19 | parts/ 20 | sdist/ 21 | var/ 22 | wheels/ 23 | share/python-wheels/ 24 | *.egg-info/ 25 | .installed.cfg 26 | *.egg 27 | MANIFEST 28 | 29 | # PyInstaller 30 | # Usually these files are written by a python script from a template 31 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 32 | *.manifest 33 | *.spec 34 | 35 | # Installer logs 36 | pip-log.txt 37 | pip-delete-this-directory.txt 38 | 39 | # Unit test / coverage reports 40 | htmlcov/ 41 | .tox/ 42 | .nox/ 43 | .coverage 44 | .coverage.* 45 | .cache 46 | nosetests.xml 47 | coverage.xml 48 | *.cover 49 | *.py,cover 50 | .hypothesis/ 51 | .pytest_cache/ 52 | cover/ 53 | 54 | # Translations 55 | *.mo 56 | *.pot 57 | 58 | # Django stuff: 59 | *.log 60 | local_settings.py 61 | db.sqlite3 62 | db.sqlite3-journal 63 | 64 | # Flask stuff: 65 | instance/ 66 | .webassets-cache 67 | 68 | # Scrapy stuff: 69 | .scrapy 70 | 71 | # Sphinx documentation 72 | docs/_build/ 73 | 74 | # PyBuilder 75 | .pybuilder/ 76 | target/ 77 | 78 | # Jupyter Notebook 79 | .ipynb_checkpoints 80 | 81 | # IPython 82 | profile_default/ 83 | ipython_config.py 84 | 85 | # pyenv 86 | # For a library or package, you might want to ignore these files since the code is 87 | # intended to run in multiple environments; otherwise, check them in: 88 | # .python-version 89 | 90 | # pipenv 91 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 92 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 93 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 94 | # install all needed dependencies. 95 | #Pipfile.lock 96 | 97 | # poetry 98 | # Similar to Pipfile.lock, it is generally recommended to include poetry.lock in version control. 99 | # This is especially recommended for binary packages to ensure reproducibility, and is more 100 | # commonly ignored for libraries. 101 | # https://python-poetry.org/docs/basic-usage/#commit-your-poetrylock-file-to-version-control 102 | #poetry.lock 103 | 104 | # pdm 105 | # Similar to Pipfile.lock, it is generally recommended to include pdm.lock in version control. 106 | #pdm.lock 107 | # pdm stores project-wide configurations in .pdm.toml, but it is recommended to not include it 108 | # in version control. 109 | # https://pdm.fming.dev/latest/usage/project/#working-with-version-control 110 | .pdm.toml 111 | .pdm-python 112 | .pdm-build/ 113 | 114 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow and github.com/pdm-project/pdm 115 | __pypackages__/ 116 | 117 | # Celery stuff 118 | celerybeat-schedule 119 | celerybeat.pid 120 | 121 | # SageMath parsed files 122 | *.sage.py 123 | 124 | # Environments 125 | .env 126 | .venv 127 | env/ 128 | venv/ 129 | ENV/ 130 | env.bak/ 131 | venv.bak/ 132 | 133 | # Spyder project settings 134 | .spyderproject 135 | .spyproject 136 | 137 | # Rope project settings 138 | .ropeproject 139 | 140 | # mkdocs documentation 141 | /site 142 | 143 | # mypy 144 | .mypy_cache/ 145 | .dmypy.json 146 | dmypy.json 147 | 148 | # Pyre type checker 149 | .pyre/ 150 | 151 | # pytype static type analyzer 152 | .pytype/ 153 | 154 | # Cython debug symbols 155 | cython_debug/ 156 | 157 | # PyCharm 158 | # JetBrains specific template is maintained in a separate JetBrains.gitignore that can 159 | # be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore 160 | # and can be added to the global gitignore or merged into this file. For a more nuclear 161 | # option (not recommended) you can uncomment the following to ignore the entire idea folder. 162 | #.idea/ 163 | -------------------------------------------------------------------------------- /Dockerfile: -------------------------------------------------------------------------------- 1 | FROM ubuntu:20.04 2 | #FROM python:3.10.12 Note: Unfortunately with this the 3D visualization with open3d does not work. 3 | 4 | LABEL maintainer="Thomas Laebe " 5 | 6 | # ----------------------------------------------------------------------------------------- 7 | # Needed Packages 8 | 9 | # Python 10 | RUN apt-get update && apt-get install -y python3-pip 11 | RUN python3 -m pip install --no-cache-dir --upgrade pip # Note that without this the installation 12 | # of the source package with "-e" does not work 13 | 14 | # Libraries needed for open3d 15 | RUN apt-get update && apt-get install --no-install-recommends -y libc++1 libgomp1 libpng16-16 libglfw3 16 | 17 | # For building apriltag library 18 | RUN apt-get update && apt-get install -y git 19 | RUN apt-get update && DEBIAN_FRONTEND=noninteractive apt-get install -y cmake 20 | 21 | # For opencv (cv2) 22 | RUN apt-get update && apt-get install -y libglib2.0-0 libsm6 libxrender1 libxext6 23 | 24 | # ----------------------------------------------------------------------------------------- 25 | # We need the apriltag library. Note that this only works AFTER the source package install. Unclear why 26 | # The version you get with "pip install apriltag" is too old 27 | # (it has some different point numbering, thus the calibration may fail!). We use the github 28 | # repo. Note that we have to do this AFTER the installation of the calibration package, 29 | # because the build system checks for numpy which is not there before installing the calibration package. 30 | WORKDIR /root 31 | RUN git clone --branch 'v3.4.2' https://github.com/AprilRobotics/apriltag 32 | 33 | WORKDIR /root/apriltag 34 | # numpy needs to be there otherwise python wrapper is not built 35 | RUN pip install numpy 36 | RUN cmake -B build -DCMAKE_BUILD_TYPE=Release 37 | RUN cmake --build build --target install 38 | 39 | # Library path has to be set so that python finds the apriltag library 40 | ENV LD_LIBRARY_PATH=/usr/local/lib 41 | ENV PYTHONPATH=/usr/local/lib/python3.8/site-packages 42 | 43 | # HACK: The build does not name the python wrapper library correctly. Correct this here. 44 | RUN mv /usr/local/lib/python3.8/site-packages/apriltag..so /usr/local/lib/python3.8/site-packages/apriltag.cpython-38-x86_64-linux-gnu.so 45 | 46 | 47 | # ----------------------------------------------------------------------------------------- 48 | # The actual program 49 | 50 | # Python debugger which is actually not needed but may appear in source: ipdb 51 | RUN pip --no-cache-dir install ipdb 52 | 53 | # create folder for code and go to it 54 | WORKDIR /root/calib 55 | 56 | # Copy the neccessary files/dirs into the container 57 | COPY scripts ./scripts 58 | COPY src ./src 59 | COPY pyproject.toml . 60 | 61 | # Install the calibration python package and it's (python) dependencies 62 | WORKDIR /root/calib 63 | RUN pip --no-cache-dir install -e . 64 | 65 | 66 | 67 | # ============================================================================= 68 | # Now do the actual work: Start the camera system and LiDAR calibration 69 | WORKDIR /root 70 | COPY entrypoint.bash /root/entrypoint.bash 71 | 72 | CMD /root/entrypoint.bash 73 | 74 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2024 Photogrammetry & Robotics Bonn 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Joint Intrinsic and Extrinsic Calibration of Perception Systems Utilizing a Calibration Environment 2 | 3 | 4 | ![image](https://github.com/user-attachments/assets/1f3d64c6-fd31-4bf7-adef-cfa7cda43e56) 5 | 6 |
7 | Basically all multi-sensor systems must calibrate 8 | their sensors to exploit their full potential for state estimation 9 | such as mapping and localization. In this paper, we investigate 10 | the problem of extrinsic and intrinsic calibration of perception 11 | systems. Traditionally, targets in the form of checkerboards or 12 | uniquely identifiable tags are used to calibrate those systems. 13 | We propose to use a whole calibration environment as a target 14 | that supports the intrinsic and extrinsic calibration of different 15 | types of sensors. By doing so, we are able to calibrate multiple 16 | perception systems with different configurations, sensor types, 17 | and sensor modalities. Our approach does not rely on overlaps 18 | between sensors which is often otherwise required when using 19 | classical targets. The main idea is to relate the measurements for 20 | each sensor to a precise model of the calibration environment. 21 | For this, we can choose for each sensor a specific method that best 22 | suits its calibration. Then, we estimate all intrinsics and extrinsics 23 | jointly using least squares adjustment. For the final evaluation 24 | of a LiDAR-to-camera calibration of our system, we propose an 25 | evaluation method that is independent of the calibration. This 26 | allows for quantitative evaluation between different calibration 27 | methods. The experiments show that our proposed method is 28 | able to provide reliable calibration. 29 |
30 | 31 | 32 | # Perception System Calibration 33 | 34 | This repository contains the full setup for the system calibration (relative 35 | poses between the sensors + intrinsics) for multipe Cameras and/ or LiDAR sensors. 36 | 37 | ## Prerequisites 38 | 39 | The first thing is to obtain the point cloud and Apriltag coordinates from the calibration environment. 40 | Next we have to record with the setup that should be recorded. After recording we can use this repository for calibration. 41 | 42 | ### TLS point cloud + Apriltag coordinate extraction 43 | 44 | We provide exemplary data here: 45 | - Point Cloud and Apriltag Coordinates: [Download here](https://www.ipb.uni-bonn.de/html/projects/ipb_calibration/reference_data.zip) 46 | 47 | #### Using own data 48 | 49 | If you want to use your own data: Take a TLS and scan the room. Then use the apriltag coordinate extraction software we provide in this repository. This will 50 | extract 3D coordinates of the apriltags. Use these coordinates later as reference data in folder `reference`. A detailed 51 | description for the apriltag extraction is provided [here](README_apriltag_extraction.md). 52 | 53 | For the system calibration: To reduce the compute time I suggest using cloud compare to downsample to half a centimeter resolution and precompute the normals. We assume the point cloud to be in PLY file format. 54 | 55 | ### Data recording 56 | 57 | We need for the calibration from each sensor a certain amount of observations (e.g., images and point clouds) in the calibration environment. For this, one should measure in a stop-and-go manner, i.e., (i) move, (ii) stand still, (iii) take a picture or point cloud from each sensor, repeat (i) - (iii) to have around 50 observations from each sensor. In the end it would be nice to be moved around 360 degrees with the setup such that each sensor saw most of the environment. When recording with 2D profile scanner, make sure that they see enough structural elements otherwise the calibration might fail. While taking the pictures/ point cloud please avoid blocking the sensors (try to find a blind spot to hide). Use a combination of rotation and translation changes for the sensor, not only pure rotation on the spot for best possible results. 58 | For each sensor the data (.png for images, or .npy for the point clouds) should be placed in a folder with its name (can be chosen freely but must match later the ones used in the config file! We simply used the ros topic names without the /). 59 | 60 | For the recorded data a `calibration.yaml` config file needs to be created. Please put all the topic names for the cameras and LiDAR, as well as roughly measure the initial guess. Additional, define which cameras are standard perspective cameras ("pinhole") or fisheye cameras ("fisheye"), e.g. 61 | 62 | ``` 63 | camera_models: 64 | - pinhole 65 | - pinhole 66 | - pinhole 67 | - pinhole 68 | - fisheye 69 | - fisheye 70 | ``` 71 | 72 | If no information is given in the yaml file, perspective cameras are assumed. 73 | An example can be of how the yaml should look like can be found in the provided test data. 74 | 75 | We provide exemplary data here: 76 | - Images, Point Clouds, configuration file: [Download here](https://www.ipb.uni-bonn.de/html/projects/ipb_calibration/calibration_data.zip) 77 | 78 | ### Compute Calibration 79 | 80 | To run the calibration use `lcba.py` for calibrating the ipb-car. One can probably leave most of the default parameters as they are. 81 | In the end the point clouds should be well aligned to the reference map and the camera rays should intersect the apriltag corners. 82 | 83 | ## Docker for system calibration 84 | 85 | A configuration for docker is available. We provide a Dockerfile together with a docker compose configuration which mounts necessary directories 86 | from the hosts system for input and output. To prepare a run in a docker container provide the following: 87 | 88 | - **Reference data** in folder `reference`: 89 | - provide the reference point cloud (see above) as file `reference/reference_pointcloud.ply` 90 | - provide the Apriltag coordinates (see above) as file `reference/apriltag_coords.txt` 91 | - **Input data** in folder `input`: provide the data recording (see above) together with the `calibration.yaml` config file in folder `input`. 92 | 93 | The docker image can be build and the calibration can be started with 94 | (working directory should be root directory of the repository): 95 | ``` 96 | docker compose up --build 97 | ``` 98 | (To separate building and start use `docker build .` and `docker compose up`.) 99 | 100 | After the calibration, the output is available in folder `output`: 101 | - `result1.log`: Logfile for the console output of the calibration run 102 | - `result1/results.yaml`: (also available as pickle file): The actual 103 | calibration result with extrinsics and intrinsics together 104 | with their a-posteriori covariance matrices of the cameras and lasers. 105 | `result1/args.yaml`: The settings used for the calibration. 106 | 107 | **Notes**: 108 | - If you need to change some parameters of the calibration program, 109 | modify the start script `entrypoint.bash` 110 | - if you need visualization (e.g. using --visualize in `entrypoint.bash`), 111 | then allow X11 connections for user root with 112 | ``` 113 | xhost local:root 114 | ``` 115 | before starting the container. 116 | - The compose file assumes that a NVIDIA GPU is available. If no visualization is needed 117 | (and that is the default !) and you have no nvidia GPU, the "deploy" section the 118 | `compose.yaml` can be commented out. 119 | 120 | 121 | ## Usage without docker 122 | 123 | We highly recommend using docker. In case it is not needed you can take a look at the Dockerfile to see what needs to be installed. 124 | 125 | Most importantly: 126 | Install the [AprilTag library](https://github.com/AprilRobotics/apriltag/releases/tag/v3.4.2) (tested under 3.4.2). 127 | Install this repository (e.g. `pip install -e .`) 128 | 129 | ## ToDos 130 | - [x] Add scripts for extracting the Apriltag coordinates from the TLS point cloud 131 | - [ ] Add evaluation script and data -------------------------------------------------------------------------------- /README_apriltag_extraction.md: -------------------------------------------------------------------------------- 1 | # Apriltag extraction on TLS laser scans 2 | 3 | We attached A4 sized apriltag of family 36h11 to the walls and then scanned the room. 4 | The 3D scan of the room needs to contain for each point the 3D coordinates and the measured intensity (this is what you get with e.g. a Faro TLS scanner). We 5 | use a ply file as input. (A ASCII ptx-file which can be exported in Faro Scene should also work.) 6 | 7 | The procedure has two steps: 8 | - produce an orthophoto using the pointcloud. This produces an intensity image as well as 9 | float images with the corresponding X,Y,Z coordinates 10 | - apply the april tag detector on the intensity image and write out the corresponding x,y,z coordinates. 11 | 12 | One can directly use a python script (see Usage) or use a docker container (see Docker). 13 | 14 | 15 | 16 | ## Usage 17 | 18 | The whole process can be executed by a single script: 19 | 20 | ``` 21 | extract_all_apriltag_coords_on_scan.py [--visualize] 22 | ``` 23 | 24 | The room will be separated into walls and for each wall an orthophoto will be generated and the 25 | apriltag detector will be called. Finally all detected 3D coordinates will be combined to 26 | a single output file. If you use the `--visualize` the final detected coordinates will be shown 27 | together with the pointcloud. 28 | 29 | Everything is controlled by the yaml-File. An example can be found at 30 | `config/extract_all_apriltag_coords.yaml`: 31 | - define the name of the TLS laserscan with tag `scanfilename`. We assume that 32 | the walls (+ ceiling and maybe even floor) are roughly axis aligned, e.g. the 33 | left wall is parallel to the X-Z plane. If the coordinate origin is roughly in 34 | the middle of the room, it is more easy to separate the walls. In the given 35 | example configuration file we assume this. 36 | - The final combined output file (filename `apriltag_coords.txt`) 37 | as well as all orthophotos images will be written 38 | to the directory `working_dir` (will be created if not present). 39 | Additional for every wall there will be two output files with the apriltag coordinates: 40 | `apriltag_coords.txt` and `apriltag_coords_rc.txt` which additionally includes 41 | the row and column coordinates of the detected tags on the intensity image. (center of 42 | upper left pixel is (0,0)). 43 | - `pixel_size` defines the resolution/ground sampling distance of the orthophotos. For 44 | best results, this should be similar to the average nearest points distance of the 45 | pointcloud. Note that this parameter highly influence the computation time. You 46 | might choose a lower resolution for a first test and then use a high resolution for 47 | the final extraction. 48 | - For every wall with apriltags you should define a configuration under the 49 | tag `wall` and give the following two attributes: 50 | - `projection direction`: Give the axis which is perpendicular to the wall. X=1, Y=2, Z=3 51 | If you look into negative axis direction (seen from the center), give -1 for X, -2 for Y, -3 for Z. 52 | Note that looking into positive/negative direction means 53 | that the orthophoto is flipped/mirrored. Then the tag code 54 | cannot be recognized any more. Thus if you have a wall with april tags and no tags are found, 55 | maybe flip the projection direction. 56 | - `bounding box`: This is necessary to separate parallel walls, otherwise both walls will be 57 | projected on the same orthophoto. If there are no other objects in the room, this can 58 | be done very roughly, e.g. just filtering out all positive X coordinates. 59 | If there are more objects that could obscure individual April tags, then the bounding 60 | box has to be defined more closer to the wall in order to detect all tags. 61 | 62 | 63 | ## Docker 64 | 65 | A configuration for docker is available. We provide a Dockerfile `apriltag_extraction.Dockerfile` together with a docker compose configuration which mounts necessary directories 66 | from the hosts system for input and output. To prepare a run in a docker container provide the following: 67 | 68 | - **Input data** in folder `input`: provide the tls laser scan as file `input/tls_scan.ply`. 69 | - **Configuration** The yaml file `config/extract_all_apriltag_coords.yaml` is used for defining the walls of the room. 70 | You may need to modify this configuration (see Usage for a description) 71 | 72 | The docker image can be build and the extraction can be started with 73 | (working directory should be root directory of the repository): 74 | ``` 75 | docker compose --profile apriltag_extraction up --build 76 | ``` 77 | 78 | You will then find the results in the directory **`output/apriltag_extraction`**: 79 | - the extracted coordinates in the file `apriltag_coords.txt`. For later usage for the calibration copy this file to folder 80 | `reference` 81 | - a log of the extraction in `result.log` 82 | -------------------------------------------------------------------------------- /apriltag_extraction.Dockerfile: -------------------------------------------------------------------------------- 1 | FROM ubuntu:20.04 2 | #FROM python:3.10.12 Note: Unfortunately with this the 3D visualization with open3d does not work. 3 | 4 | LABEL maintainer="Thomas Laebe " 5 | 6 | # ----------------------------------------------------------------------------------------- 7 | # Needed Packages 8 | 9 | # Python 10 | RUN apt-get update && apt-get install -y python3-pip 11 | RUN python3 -m pip install --no-cache-dir --upgrade pip # Note that without this the installation 12 | # of the source package with "-e" does not work 13 | 14 | # Libraries needed for open3d 15 | RUN apt-get update && apt-get install --no-install-recommends -y libc++1 libgomp1 libpng16-16 libglfw3 16 | 17 | # For building apriltag library 18 | RUN apt-get update && apt-get install -y git 19 | RUN apt-get update && DEBIAN_FRONTEND=noninteractive apt-get install -y cmake 20 | 21 | # For opencv (cv2) 22 | RUN apt-get update && apt-get install -y libglib2.0-0 libsm6 libxrender1 libxext6 23 | 24 | # ----------------------------------------------------------------------------------------- 25 | # We need the apriltag library. Note that this only works AFTER the source package install. Unclear why 26 | # The version you get with "pip install apriltag" is too old 27 | # (it has some different point numbering, thus the calibration may fail!). We use the github 28 | # repo. Note that we have to do this AFTER the installation of the calibration package, 29 | # because the build system checks for numpy which is not there before installing the calibration package. 30 | WORKDIR /root 31 | RUN git clone --branch 'v3.4.2' https://github.com/AprilRobotics/apriltag 32 | 33 | WORKDIR /root/apriltag 34 | # numpy needs to be there otherwise python wrapper is not built 35 | RUN pip install numpy 36 | RUN cmake -B build -DCMAKE_BUILD_TYPE=Release 37 | RUN cmake --build build --target install 38 | 39 | # Library path has to be set so that python finds the apriltag library 40 | ENV LD_LIBRARY_PATH=/usr/local/lib 41 | ENV PYTHONPATH=/usr/local/lib/python3.8/site-packages 42 | 43 | # HACK: The build does not name the python wrapper library correctly. Correct this here. 44 | RUN mv /usr/local/lib/python3.8/site-packages/apriltag..so /usr/local/lib/python3.8/site-packages/apriltag.cpython-38-x86_64-linux-gnu.so 45 | 46 | 47 | # ----------------------------------------------------------------------------------------- 48 | # The actual program 49 | 50 | # Python debugger which is actually not needed but may appear in source: ipdb 51 | RUN pip --no-cache-dir install ipdb 52 | 53 | # create folder for code and go to it 54 | WORKDIR /root/calib 55 | 56 | # Copy the neccessary files/dirs into the container 57 | COPY scripts ./scripts 58 | COPY src ./src 59 | COPY pyproject.toml . 60 | 61 | # Install the calibration python package and it's (python) dependencies 62 | WORKDIR /root/calib 63 | RUN pip --no-cache-dir install -e . 64 | 65 | 66 | 67 | # ============================================================================= 68 | # Now do the actual work: Start the camera system and LiDAR calibration 69 | WORKDIR /root 70 | COPY entrypoint_apriltag_extraction.bash /root/entrypoint_apriltag_extraction.bash 71 | 72 | CMD /root/entrypoint_apriltag_extraction.bash 73 | 74 | -------------------------------------------------------------------------------- /compose.yaml: -------------------------------------------------------------------------------- 1 | services: 2 | ## System calibration with given apriltag 3D coordinates 3 | systemcalib: 4 | build: . 5 | 6 | # Mounted paths from the repo into the docker: 7 | volumes: 8 | # The folder with the recorded images and scans 9 | - $PWD/input:/root/input:ro 10 | # The folder with the reference data: room scan and apriltag coords 11 | - $PWD/reference:/root/reference:ro 12 | # The output of the calibration 13 | - $PWD/output:/root/output 14 | # X11 socket for enabling display of windows from inside the container 15 | - /tmp/.X11-unix:/tmp/.X11-unix 16 | 17 | environment: 18 | # for enabling window display from inside the container 19 | - DISPLAY=${DISPLAY} 20 | # The following is needed for allowing 3D view window inside the container. 21 | # An nvidia GPU is needed. If no visualization is needed and you have no 22 | # nvidia GPU, this can be commented out. 23 | deploy: 24 | resources: 25 | reservations: 26 | devices: 27 | - driver: nvidia 28 | count: 1 29 | capabilities: [gpu, compute,utility,graphics] 30 | 31 | # the following avoids to start this service if a specific profile is given in docker compose 32 | profiles: 33 | - '' 34 | 35 | 36 | ## Apriltag coordinates extraction for system calibration 37 | apriltag_extraction: 38 | build: 39 | context: . 40 | dockerfile: apriltag_extraction.Dockerfile 41 | 42 | # Mounted paths from the repo into the docker: 43 | volumes: 44 | # The folder with the recorded tls scan 45 | - $PWD/input:/root/input:ro 46 | # The configuration for the walls in the room 47 | - $PWD/config:/root/config:ro 48 | # The output of the apriltag extraction 49 | - $PWD/output:/root/output 50 | # X11 socket for enabling display of windows from inside the container 51 | - /tmp/.X11-unix:/tmp/.X11-unix 52 | 53 | environment: 54 | # for enabling window display from inside the container 55 | - DISPLAY=${DISPLAY} 56 | # The following is needed for allowing 3D view window inside the container. 57 | # An nvidia GPU is needed. If no visualization is needed and you have no 58 | # nvidia GPU, this can be commented out. 59 | deploy: 60 | resources: 61 | reservations: 62 | devices: 63 | - driver: nvidia 64 | count: 1 65 | capabilities: [gpu, compute,utility,graphics] 66 | 67 | # Don't use this service per default. Only use it in profile "apriltag_extraction" 68 | # e.g. with docker compose --profile apriltag_extraction up --build 69 | profiles: 70 | - apriltag_extraction 71 | 72 | 73 | # # Start it interactively 74 | # entrypoint: /bin/bash 75 | # stdin_open: true # docker run -i 76 | # tty: true # docker run -t 77 | 78 | -------------------------------------------------------------------------------- /config/extract_all_apriltag_coords.yaml: -------------------------------------------------------------------------------- 1 | # Configuration file for extraction of apriltags out of a scan of a room. 2 | # The room is separated in walls. For every wall an orthophoto will be generated. 3 | 4 | # Name of the pointcloud file. Can be .ptx from a Faro scanner or any other format (e.g. ply) 5 | # readable by open3d. Must have intensity. 6 | scanfilename: /root/input/tls_scan.ply 7 | 8 | # Working directory: Here all orthophotos and resulting coordinates will be saved 9 | working_dir: /root/output/apriltag_extraction 10 | 11 | # Ground Sampling distance or pixel size of the orthophotos 12 | # This is the resolution of the orthophotos in object space, usally in meters. 13 | # Should be in a similar range of the nearest distance 14 | # of the points on the walls 15 | pixel_size: 0.002 16 | 17 | # List of walls. For every wall there will be created an orthophoto and the 18 | # april tags will be extracted on that orthophoto. 19 | walls: 20 | wallXpositive: 21 | # Give the axis which is perpendicular to the wall. X=1, Y=2, Z=3 22 | # If you look into negative axis direction, give -1 for X, -2 for Y, 23 | # -3 for Z. Note that looking into positive/negative direction means 24 | # that the orthophoto is flipped/mirrored. Then the tag code 25 | # cannot be recognized any more. 26 | projection_direction: 1 27 | # Give a bounding box (minX, minY, minZ, maxX, maxY, maxZ) for the wall 28 | # The important thing here is that for parallel walls only one wall has to 29 | # appear inside the bounding box 30 | bounding_box: 31 | - 0 32 | - -10 33 | - -10 34 | - 10 35 | - 10 36 | - 10 37 | 38 | wallXnegative: 39 | # Give the axis which is perpendicular to the wall. X=1, Y=2, Z=3 40 | projection_direction: -1 41 | # Give a bounding box (minX, minY, minZ, maxX, maxY, maxZ) for the wall 42 | # The important thing here is that for parallel walls only one wall has to 43 | # appear inside the bounding box 44 | bounding_box: 45 | - -10 46 | - -10 47 | - -10 48 | - 0 49 | - 10 50 | - 10 51 | 52 | wallYpositive: 53 | # Give the axis which is perpendicular to the wall. X=1, Y=2, Z=3 54 | # If you look into negative axis direction, give -1 for X, -2 for Y, 55 | # -3 for Z. Note that looking into positive/negative direction means 56 | # that the orthophoto is flipped/mirrored. Then the tag code 57 | # cannot be recognized any more. 58 | projection_direction: 2 59 | # Give a bounding box (minX, minY, minZ, maxX, maxY, maxZ) for the wall 60 | # The important thing here is that for parallel walls only one wall has to 61 | # appear inside the bounding box 62 | bounding_box: 63 | - -10 64 | - 0 65 | - -10 66 | - 10 67 | - 10 68 | - 10 69 | 70 | wallYnegative: 71 | # Give the axis which is perpendicular to the wall. X=1, Y=2, Z=3 72 | projection_direction: -2 73 | # Give a bounding box (minX, minY, minZ, maxX, maxY, maxZ) for the wall 74 | # The important thing here is that for parallel walls only one wall has to 75 | # appear inside the bounding box 76 | bounding_box: 77 | - -10 78 | - -10 79 | - -10 80 | - 10 81 | - 0 82 | - 10 83 | 84 | wallZpositive: 85 | # Give the axis which is perpendicular to the wall. X=1, Y=2, Z=3 86 | # If you look into negative axis direction, give -1 for X, -2 for Y, 87 | # -3 for Z. Note that looking into positive/negative direction means 88 | # that the orthophoto is flipped/mirrored. Then the tag code 89 | # cannot be recognized any more. 90 | projection_direction: 3 91 | # Give a bounding box (minX, minY, minZ, maxX, maxY, maxZ) for the wall 92 | # The important thing here is that for parallel walls only one wall has to 93 | # appear inside the bounding box 94 | bounding_box: 95 | - -10 96 | - -10 97 | - 0 98 | - 10 99 | - 10 100 | - 10 101 | 102 | 103 | 104 | 105 | # -------------------------------------------------------- 106 | # Optional arguments: 107 | 108 | # Pyramid level of the smallest images used. In order to detect very large 109 | # apriltags, the tag detection in not only done on the original image, but also 110 | # on higher image pyramid levels. This is the highest image pyramid level number. 111 | # 0 == original size, 1 == half rows/cols of original, ... Default is: 2 112 | # max_pyramid_level: 2 113 | -------------------------------------------------------------------------------- /entrypoint.bash: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | # Do the calibration 3 | # This is just a call to a python script: 4 | 5 | python3 calib/scripts/lcba.py --faro_file reference/reference_pointcloud.ply --apriltag_file reference/apriltag_coords.txt --path_data input --path_out output --experiment_name result1 | tee output/result1.log 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /entrypoint_apriltag_extraction.bash: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | # Do the apriltag extraction 3 | # This is just a call to a python script: 4 | 5 | python3 calib/scripts/extract_all_apriltag_coords_on_scan.py config/extract_all_apriltag_coords.yaml | tee output/apriltag_extraction/apriltag_extraction_result.log 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /input/.gitignore: -------------------------------------------------------------------------------- 1 | # Ignore everything in this directory 2 | * 3 | # Except this file 4 | !.gitignore 5 | 6 | -------------------------------------------------------------------------------- /output/.gitignore: -------------------------------------------------------------------------------- 1 | # Ignore everything in this directory 2 | * 3 | # Except this file 4 | !.gitignore 5 | !apriltag_extraction/ 6 | -------------------------------------------------------------------------------- /output/apriltag_extraction/.gitignore: -------------------------------------------------------------------------------- 1 | # Ignore everything in this directory 2 | * 3 | # Except this file 4 | !.gitignore 5 | 6 | -------------------------------------------------------------------------------- /pyproject.toml: -------------------------------------------------------------------------------- 1 | [project] 2 | name = "ipb_calibration" 3 | description = "IPB Car Calibration" 4 | version = "0.0.1" 5 | dependencies = [ 6 | "matplotlib", 7 | "tqdm", 8 | "opencv_python", 9 | "numpy", 10 | "pandas", 11 | "diskcache", 12 | "natsort", 13 | "Click", 14 | "PyYAML", 15 | "scipy", 16 | "open3d", 17 | "argparse", 18 | "tifffile" 19 | ] 20 | -------------------------------------------------------------------------------- /reference/.gitignore: -------------------------------------------------------------------------------- 1 | # Ignore everything in this directory 2 | * 3 | # Except this file 4 | !.gitignore 5 | 6 | -------------------------------------------------------------------------------- /scripts/extract_all_apriltag_coords_on_scan.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | """ 4 | Extract apriltag coordinates from a scan of a room by 5 | creating orthophotos of the walls and detecting tags on the intensity of the laser. 6 | 7 | Created on Thu Oct 17 16:43:33 2024 8 | 9 | @author: laebe 10 | """ 11 | import os 12 | import numpy as np 13 | import argparse 14 | import yaml 15 | import open3d as o3d 16 | from ipb_calibration.create_orthophoto_images import create_orthophoto_images, read_ptx_pointcloud 17 | from ipb_calibration.detect_apriltags import detect_apriltags 18 | 19 | def create_geometry_at_points(points, radius=0.005): 20 | """ 21 | For 3D points create spheres as markers. 22 | Args: 23 | points: 24 | An nx3 array of 3D coordinates 25 | radius: 26 | raduis of the spheres. 27 | 28 | Returns: 29 | open3d geometry with list of spheres. 30 | 31 | """ 32 | geometries = o3d.geometry.TriangleMesh() 33 | for point in points: 34 | sphere = o3d.geometry.TriangleMesh.create_sphere(radius=radius) #create a small sphere to represent point 35 | sphere.translate(point) #translate this sphere to point 36 | geometries += sphere 37 | geometries.paint_uniform_color([1.0, 0.0, 0.0]) 38 | return geometries 39 | 40 | 41 | 42 | if __name__ == '__main__': 43 | 44 | #%% Settings 45 | parser = argparse.ArgumentParser(description='Extract apriltag coordinates from a scan of a room by ' 46 | ' creating orthophotos of the walls and detecting tags on the intensity of the laser. ') 47 | parser.add_argument("yaml_config_file", type=str, 48 | help="yaml configuration file for the walls in the room") 49 | parser.add_argument('--visualize', '-v', action='store_true', help = 50 | 'show all the extracted tags on the pointcloud.') 51 | 52 | args = parser.parse_args() 53 | 54 | yaml_config_file = args.yaml_config_file 55 | 56 | with open(yaml_config_file, 'r') as file: 57 | config = yaml.safe_load(file) 58 | 59 | # Default args 60 | if 'max_pyramid_level' in config: 61 | max_pyramid_level = config['max_pyramid_level'] 62 | else: 63 | max_pyramid_level = 2 64 | 65 | # Loop over all walls 66 | for wallname in config['walls'].keys(): 67 | # Step 1: Create orthophoto 68 | odir = os.path.join(config['working_dir'], wallname) 69 | print('\n---------------------------------------------------------------') 70 | print('Creating orthophoto in %s ...' % odir, flush=True) 71 | create_orthophoto_images( 72 | config['scanfilename'], odir, config['pixel_size'], config['walls'][wallname]['bounding_box'], 73 | config['walls'][wallname]['projection_direction']) 74 | 75 | # Step 2: Apriltag extraction on that orthophoto 76 | detect_apriltags(odir) 77 | 78 | # Combine all extracted coordinates to one file 79 | print('\n Combine all coordinates ...', flush=True) 80 | coords = np.zeros((0,6)) 81 | 82 | for wallname in config['walls'].keys(): 83 | odir = os.path.join(config['working_dir'], wallname) 84 | coords_one_wall = np.loadtxt(os.path.join(odir, 'apriltag_coords_rc.txt'), skiprows=1) 85 | if len(coords_one_wall)>0: 86 | coords = np.concatenate((coords, coords_one_wall)) 87 | 88 | print('Save ...', flush=True) 89 | # as text file with ID X Y Z 90 | np.savetxt(os.path.join(config['working_dir'], 'apriltag_coords.txt'), coords[:, :4], fmt='%4d %10.5f %10.5f %10.5f', 91 | header='%d' % coords.shape[0], comments='') 92 | 93 | print('\n... done with everthing.') 94 | 95 | # Visualization 96 | if args.visualize: 97 | # Load 98 | print("Visualization ... ", flush=True) 99 | _, file_extension = os.path.splitext(config['scanfilename']) 100 | if file_extension == ".ptx": 101 | pcd = read_ptx_pointcloud(config['scanfilename']) 102 | else: 103 | pcd = o3d.io.read_point_cloud(config['scanfilename']) 104 | 105 | point_markers = create_geometry_at_points(coords[:,1:4],radius=0.01) 106 | 107 | o3d.visualization.draw_geometries([pcd, point_markers]) 108 | 109 | 110 | -------------------------------------------------------------------------------- /scripts/lcba.py: -------------------------------------------------------------------------------- 1 | import collections 2 | import pickle 3 | import open3d as o3d 4 | from ipb_calibration import lc_bundle_adjustment as ba 5 | from pathlib import Path 6 | import yaml 7 | import numpy as np 8 | import click 9 | from os.path import join 10 | from ipb_calibration import ipb_preprocessing as pp 11 | 12 | 13 | def convert_dict(d, u=None): 14 | u = d if u is None else u 15 | for k, v in u.items(): 16 | if isinstance(v, collections.abc.Mapping): 17 | d[k] = convert_dict(d.get(k, {}), v) 18 | else: 19 | if isinstance(v, np.ndarray): 20 | d[k] = v.tolist() 21 | return d 22 | 23 | 24 | @click.command() 25 | @click.option("--apriltag_file", "-a", type=str, help=".txt File with the apriltag coordinates. Each column should contain the apriltag id and the 3d coordinate.") 26 | @click.option("--path_data", "-p", type=str, help="Path to the data directory. Directory should contain the calibration.yaml file as well as the folders with the recorded calibration data.") 27 | @click.option("--faro_file", "-f", type=str, help=".ply file of the reference point cloud map.") 28 | @click.option("--path_out", "-o", type=str, help="Directory in which the results will be stored.") 29 | @click.option("--std_pix", "-sp", default=0.2, help="Standard deviation of the apriltag detection in the image [pix]. (default=0.2)") 30 | @click.option("--std_apriltags", "-sa", default=0.002, help="Standard deviation of the 3D coordinates of the Apriltags.(default=0.002)") 31 | @click.option("--std_lidar", "-sl", default=0.01, help="Standard deviation of the LiDAR points. (default=0.01)") 32 | @click.option("--max_scanpoints", "-mp", default=2500, type=int, help="Maximal number of used lidar points per scan. Give -1 to use all points. (default=2500)") 33 | @click.option("--bias/--no-bias", default=False, help="Flag if one wants to estimate a bias for each LiDAR. (default=False)") 34 | @click.option("--scale/--no-scale", default=False, help="Flag if one wants to estimate a scale for each LiDAR. (default=False)") 35 | @click.option("--division_model/--no-division_model", default=False, help="Flag if to estimate the non-linear distortion with the brownsche distortion model or the division model. (default=False)") 36 | @click.option("--dist_degree", default=3, help="Polynomial degree of the non-linear distortion model. (default=3)") 37 | @click.option("--experiment_name", "-e", default="dev", help="Will be extended to the out_path to enable different experiments without overriding. (default=dev)") 38 | @click.option("--visualize/--no-visualize", default=False, help="Flag if the initial guess and the final optimization should be visualized. (default=False)") 39 | def main(apriltag_file, 40 | path_data, 41 | faro_file, 42 | path_out, 43 | std_pix, 44 | std_apriltags, 45 | std_lidar, 46 | max_scanpoints, 47 | dist_degree, 48 | bias, 49 | scale, 50 | division_model, 51 | experiment_name, 52 | visualize): 53 | config = locals() 54 | print(30*"-") 55 | print(10*"-", experiment_name, 10*"-") 56 | print(30*"-") 57 | pcd_map = o3d.io.read_point_cloud(faro_file) 58 | imgpixel, indices, coords, observations, T_cami_cam_yaml, img_sizes, cam_is_pinhole = pp.parse_camera_data( 59 | apriltag_file, path_data) 60 | 61 | init_k, init_coeff = pp.estimate_initial_k( 62 | observations, cam_is_pinhole, max_num_images=10, image_size=img_sizes) 63 | init_K = [np.array([[k_[2], 0, k_[0]], 64 | [0, k_[3], k_[1]], 65 | [0, 0, 1]]) for k_ in init_k] 66 | print(init_k) 67 | print(init_coeff) 68 | 69 | T_cam_map, T_cami_cam = pp.estimate_initial_guess( 70 | observations, cam_is_pinhole, init_K=init_K, dist_coeff=init_coeff) 71 | 72 | # Initial guess for Lidar 73 | lidar_points, T_os_cam, cfg = pp.parse_lidar_data(path_data, max_scanpoints) 74 | 75 | # Execute BA 76 | lcba = ba.LCBundleAdjustment(ref_map=pcd_map, 77 | num_poses=len(T_cam_map), 78 | outlier_mult=3, 79 | cfg=cfg) 80 | lcba.add_cameras(p_img=imgpixel, 81 | std_pix=std_pix, 82 | ray2apriltag_idx=indices, 83 | apriltag_coords=coords, 84 | T_cam_map=T_cam_map, 85 | T_cami_cam=T_cami_cam, 86 | k=init_k, 87 | std_apriltags=std_apriltags, 88 | dist_degree=dist_degree, 89 | division_model=division_model, 90 | init_coeff=init_coeff, 91 | cami_is_pinhole=cam_is_pinhole) 92 | 93 | lcba.add_lidars(scans=lidar_points, 94 | T_lidar_cam=T_os_cam, 95 | GM_k=0.1, 96 | std_lidar=std_lidar, 97 | estimate_bias=bias, 98 | estimate_scale=scale) 99 | 100 | results, errors = lcba.optimize( 101 | num_iter=100, visualize=visualize) 102 | 103 | path_out = Path(join(path_out, experiment_name)) 104 | path_out.mkdir(exist_ok=True, parents=True) 105 | 106 | with open(join(path_out, "results.yaml.pkl"), "wb") as f: 107 | pickle.dump(results, f) 108 | with open(join(path_out, "results.yaml"), "w") as outfile: 109 | yaml.safe_dump(convert_dict(results), outfile, 110 | default_flow_style=False) 111 | with open(join(path_out, "errors.pkl"), "wb") as f: 112 | pickle.dump(errors, f) 113 | with open(join(path_out, "args.yaml"), "w") as outfile: 114 | yaml.safe_dump(config, outfile) 115 | 116 | 117 | if __name__ == "__main__": 118 | main() 119 | -------------------------------------------------------------------------------- /src/ipb_calibration/apriltag.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import apriltag 3 | import ipdb 4 | from matplotlib import pyplot as plt 5 | 6 | 7 | def compute_border_threshold(corners): 8 | mean = np.mean(corners, axis=0, keepdims=True) 9 | diff = corners - mean 10 | max_diag = np.max(np.linalg.norm(diff, axis=-1)) * 2 11 | return max_diag / 2**0.5 / 8 12 | 13 | 14 | class Apriltags: 15 | def __init__(self, file) -> None: 16 | self.detector = apriltag.apriltag("tag36h11") 17 | max_num_tags = 587 18 | 19 | apriltags = np.loadtxt(file, skiprows=1).reshape(-1, 4, 4) 20 | self.apriltag_coords = apriltags[:, :, 1:] 21 | 22 | self.apriltag_ids = (apriltags[:, 0, 0]/100).astype("int") 23 | self.tag_id2list_idx = np.full( 24 | max_num_tags, fill_value=max_num_tags, dtype=np.int64) 25 | self.tag_id2list_idx[self.apriltag_ids] = np.arange( 26 | len(self.apriltag_ids)) 27 | 28 | def process_detection(self, detection, img): 29 | list_idx = self.tag_id2list_idx[detection["id"]] 30 | if list_idx >= len(self.apriltag_coords): 31 | return None, None, None 32 | 33 | return detection["id"], detection["lb-rb-rt-lt"], self.apriltag_coords[list_idx] 34 | 35 | def detect(self, img: np.array, pixel2ray=None): 36 | """Detect apriltags in Image and 37 | 38 | Args: 39 | img (_type_): Image 40 | pixel2ray (function, optional): function which converts pixel to camera rays. 41 | 42 | Returns: 43 | list: for each tag dict with: tag_id, corners [4,2], coords [4,3], (rays [4,3]) 44 | """ 45 | observations = [] 46 | results = self.detector.detect(img) 47 | for detection in results: 48 | tag_id, corners, coords = self.process_detection( 49 | detection, img) 50 | if tag_id is None: 51 | continue 52 | 53 | # needed for pip apriltag 54 | # corners = corners[[0, 1, 3, 2]] 55 | 56 | # needed for apriltag https://github.com/AprilRobotics/apriltag#python 57 | corners = corners[[3, 2, 0, 1]] 58 | 59 | border = compute_border_threshold(corners) 60 | within_image = np.all(corners >= border) and np.all( 61 | corners <= (np.array(img.T.shape)-1-border)) 62 | 63 | out = {"tag_id": tag_id, 64 | "corners": corners, 65 | "coords": coords} 66 | if pixel2ray is not None: 67 | out["rays"] = pixel2ray(corners) 68 | 69 | if within_image: 70 | observations.append( 71 | out) 72 | return observations 73 | -------------------------------------------------------------------------------- /src/ipb_calibration/camera.py: -------------------------------------------------------------------------------- 1 | import os 2 | from numpy.linalg import norm 3 | import ipdb 4 | from numpy.linalg import inv 5 | import numpy as np 6 | from ipb_calibration.utils import homogenous, skew, update_pose 7 | import cv2 8 | from scipy import interpolate 9 | 10 | 11 | def inner(a, b): 12 | return np.einsum("...d,...d->...", a, b) 13 | 14 | 15 | def outer(a, b): 16 | return np.einsum("...b,...d->...bd", a, b) 17 | 18 | 19 | def diag(a): 20 | return np.einsum("...d,db->...db", a, np.eye(a.shape[-1])) 21 | 22 | 23 | def batch_eye(n, d): 24 | return np.zeros([n, d, d]) + np.eye(d) 25 | 26 | 27 | class CVDistortionModel: 28 | def __init__(self, degree=3, division_model=True, cv2_coeff=None) -> None: 29 | radial_degree = degree if not division_model else 0 30 | division_degree = degree if division_model else 0 31 | 32 | self.k = np.zeros(radial_degree) 33 | self.h = np.zeros(division_degree) 34 | self.p = np.zeros(2) 35 | 36 | if cv2_coeff is not None: 37 | assert len(cv2_coeff) >= 5 38 | idx = np.arange(min(len(cv2_coeff), 8)) 39 | idx[:5] = [0, 1, 4, 2, 3] 40 | params = np.array(cv2_coeff)[idx] 41 | k, p, h = np.split(params, [3, 5]) 42 | if len(cv2_coeff) < 8: 43 | h = -k 44 | 45 | max_idx = min(3, len(self.k)) 46 | self.k[:max_idx] = k[:max_idx] 47 | 48 | max_idx = min(3, len(self.h)) 49 | self.h[:max_idx] = h[:max_idx] 50 | 51 | self.p = p 52 | 53 | def __str__(self) -> str: 54 | return f"CVDistortion (k: {self.k}, h: {self.h}, p: {self.p})" 55 | 56 | @classmethod 57 | def fromcvlist(cls, cv2_coeff): 58 | params = np.array(cv2_coeff).squeeze()[:8] 59 | assert len(params) >= 5 60 | params[:5] = params[[0, 1, 4, 2, 3]] 61 | k, p, h = np.split(params, [3, 5]) 62 | out = cls() 63 | # up the one from which on all are zero 64 | out.k = k[(1 - np.cumprod(k[::-1] == 0)[::-1]).astype('bool')] 65 | out.h = h[(1 - np.cumprod(h[::-1] == 0)[::-1]).astype('bool')] 66 | out.p = p 67 | return out 68 | 69 | @property 70 | def radial_degree(self): 71 | return len(self.k) 72 | 73 | @property 74 | def division_degree(self): 75 | return len(self.h) 76 | 77 | @property 78 | def params(self): 79 | return np.concatenate([self.k, self.h, self.p]) 80 | 81 | def distort(self, x): 82 | k = np.concatenate([[1], self.k]) # [degree+1] 83 | h = np.concatenate([[1], self.h]) # [degree+1] 84 | p = self.p 85 | 86 | d = inner(x, x) # d= r^2 [n] 87 | 88 | d_pol = d[:, None]**np.arange(len(k)) # [n,degree+1] 89 | d_pol2 = d[:, None]**np.arange(len(h)) # [n,degree+1] 90 | S = np.eye(2)[[1, 0]] 91 | 92 | Sp = (p @ S)[None, :] 93 | 94 | dx_tangential = 2 * p * \ 95 | np.prod(x, axis=-1, keepdims=True) + \ 96 | Sp * d[:, None] + \ 97 | 2 * Sp * x**2 98 | x_dist = x * inner(k, d_pol)[:, None] / \ 99 | inner(h, d_pol2)[:, None] + dx_tangential 100 | return x_dist 101 | 102 | def jacobian(self, x: np.array): 103 | p = self.p 104 | k = np.concatenate([[1], self.k]) # [degree+1] 105 | h = np.concatenate([[1], self.h]) # [degree+1] 106 | S = np.eye(2)[[1, 0]] 107 | 108 | d = inner(x, x) # d= r^2 [n] 109 | d_pol = d[:, None]**np.arange(len(k)) # [n,degree+1] 110 | d_pol2 = d[:, None]**np.arange(len(h)) # [n,degree+1] 111 | 112 | # ipdb.set_trace() 113 | dpol_dx = np.sum(np.arange(1, len(k)) * 114 | k[1:] * d_pol[:, :-1], axis=-1)[:, None, None] # [n,1,1] 115 | dpol2_dx = np.sum(np.arange(1, len(h)) * 116 | h[1:] * d_pol2[:, :-1], axis=-1)[:, None, None] # [n,1,1] 117 | 118 | sum_kd = inner(k, d_pol)[:, None, None] 119 | sum_hd = inner(h, d_pol2)[:, None, None] 120 | # derivative of point 121 | dxdist_dx = np.eye(2)[None, :] * sum_kd / sum_hd 122 | dxdist_dx += 2 * outer(x, x) * dpol_dx / sum_hd # radial 123 | dxdist_dx -= 2 * outer(x, x) * dpol2_dx / sum_hd**2 # division 124 | dxdist_dx += 2 * outer(p, x @ S) 125 | dxdist_dx += 2 * outer(S @ p, x) 126 | dxdist_dx += 4 * S @ diag(p*x) 127 | 128 | # derivative of p 129 | dxdist_dp = 2*np.eye(2)[None, :] * np.prod(x, axis=-1)[:, None, None] 130 | dxdist_dp += S[None, :] * d[:, None, None] 131 | dxdist_dp += 2 * S[None, :] * x[:, :, None]**2 132 | 133 | # derivative of k 134 | dxdist_dk = x[:, :, None] * d_pol[:, None, 1:] / sum_hd 135 | # derivative of h 136 | dxdist_dh = -x[:, :, None] * sum_kd * d_pol2[:, None, 1:]/(sum_hd**2) 137 | 138 | dxdist_dparams = np.concatenate( 139 | [dxdist_dk, dxdist_dh, dxdist_dp], axis=-1) 140 | return dxdist_dx, dxdist_dparams 141 | 142 | def undistort(self, p_dist, num_iter=20): 143 | p = np.copy(p_dist) 144 | for i in range(num_iter): 145 | error = self.distort(p) - p_dist # [n,2] 146 | error = error.reshape([len(p), 2, 1]) 147 | 148 | J, _ = self.jacobian(p) # [n,2,2] 149 | JT = J.transpose([0, 2, 1]) 150 | N = (JT @ J) 151 | g = -JT @ error 152 | 153 | dp = inv(N) @ g 154 | p += dp[:, :, 0] 155 | return p 156 | 157 | def update_params(self, dparams): 158 | dks, dhs, dps = np.split(dparams.flatten(), np.cumsum( 159 | [self.radial_degree, self.division_degree])) 160 | self.k += dks 161 | self.h += dhs 162 | self.p += dps 163 | 164 | def to_cv2(self): 165 | params = np.zeros(8) 166 | deg = min(2, self.radial_degree) 167 | params[:deg] = self.k[:deg] 168 | params[2:4] = self.p 169 | if self.radial_degree >= 3: 170 | params[4] = self.k[2] 171 | deg = min(3, self.division_degree) 172 | params[5:5+deg] = self.h[:deg] 173 | return params 174 | 175 | 176 | class Camera: 177 | def __init__(self, K=np.eye(3), T_cam_ref=np.eye(4), distortion=CVDistortionModel(), is_pinhole=True): 178 | self.T_cam_ref = T_cam_ref 179 | self.distortion = distortion 180 | self.K = K 181 | self.is_pinhole = is_pinhole 182 | self.projection = PinholeProjection if is_pinhole else FisheyeProjection 183 | 184 | @classmethod 185 | def fromdict(cls, datadict: dict): 186 | """Initializes a camera from a dictionary. Dictionary should have the keys 'extrinsics': [4,4] Transformation matrix, 'distortion_coeff' [8,] list in cv2 format, and 'K' [3,3] camera matrix 187 | 188 | Args: 189 | datadict (dict): dictionary with keys 'extrinsics' and 'distortion_coeff' and 'K' 190 | 191 | Returns: 192 | Camera: Instance 193 | """ 194 | return cls(K=np.array(datadict["K"]), 195 | T_cam_ref=np.array(datadict["extrinsics"]), 196 | distortion=CVDistortionModel.fromcvlist(datadict["distortion_coeff"]), 197 | is_pinhole = datadict["is_pinhole"]) 198 | 199 | def __str__(self) -> str: 200 | return f"Camera: (Projection: {'Pinhole' if self.is_pinhole else 'Fisheye'},K: {self.K}, T_cam_ref: {self.T_cam_ref}, distortion: {self.distortion})" 201 | 202 | def __repr__(self) -> str: 203 | return self.__str__() 204 | 205 | def undistort(self, p_img: np.ndarray, num_iter=10, Kresult=None): 206 | """Rectifies the coordinates p_img. 207 | 208 | Args: 209 | p_img (np.ndarray): 210 | [n,2] Coordinates in raw image 211 | num_iter (int, optional): 212 | Number iterations to solve the non linear least squares problem of the rectification. Defaults to 20. 213 | Kresult: 214 | the 3x3 calibration matrix which is valid for the final resulting coordinates 215 | AND the input coordinates. If None, 216 | the K stored in this class is used (which is usually the estimated K) 217 | 218 | Returns: 219 | p_rect: [n,2] rectified coordinates 220 | """ 221 | if Kresult is None: 222 | Kresult = self.K 223 | 224 | x_d = (homogenous(p_img) @ inv(Kresult).T)[:, :-1] # [n,2] 225 | x_n = self.distortion.undistort(x_d, num_iter=num_iter) 226 | r = homogenous(x_n) 227 | r = r @ Kresult.T 228 | return r[:, :2] 229 | 230 | def distort(self, p_img: np.ndarray, Kresult=None): 231 | """Calculate distorted image coordinates from distortion free ones. 232 | 233 | Args: 234 | p_img (np.ndarray): 235 | [n,2] Coordinates in distortion free image 236 | Kresult: 237 | the 3x3 calibration matrix which is valid for the final resulting coordinates 238 | AND the input coordinates. If None, 239 | the K stored in this class is used (which is usually the estimated K) 240 | 241 | Returns: 242 | p_rect: [n,2] coordinates in distorted image 243 | """ 244 | if Kresult is None: 245 | Kresult = self.K 246 | 247 | x_d = (homogenous(p_img) @ inv(Kresult).T)[:, :-1] # [n,2] 248 | x_n = self.distortion.distort(x_d) 249 | r = homogenous(x_n) 250 | r = r @ Kresult.T 251 | return r[:, :2] 252 | 253 | def projection_valid_mask(self, pts_world: np.ndarray, img_size, T_ref_world=np.eye(4)): 254 | """ Projects 3d Points from the world system over the reference system into the image frame 255 | 256 | Args: 257 | pts_world (np.ndarray): [n,3] Points in world frame 258 | T_ref_world (np.ndarray, optional): Transformation from reference frame to the world frame. Defaults to np.eye(4). 259 | Returns: 260 | pts_im: [N,2] Pixel coordinates in raw (distorted) image 261 | """ 262 | x_c = ((inv(self.T_cam_ref) @ inv(T_ref_world) @ 263 | homogenous(pts_world).T)[:3]).T # [3,N] 264 | in_front = x_c[:, -1] > 0 265 | x_n = x_c[:, :2]/x_c[:, -1:] 266 | 267 | border_i = np.array([[0.0, 0], 268 | [img_size[1], img_size[0]]]) 269 | border_d = (homogenous(border_i) @ inv(self.K).T)[:, :-1] # [n,2] 270 | border_n = self.distortion.undistort(border_d) 271 | 272 | inside_view = np.all(x_n > border_n[None, 0], axis=-1) & np.all(x_n < 273 | border_n[None, 1], axis=-1) 274 | 275 | x_d = self.distortion.distort(x_n) 276 | x_i = x_d @ self.K[:2, :2] + self.K[None, :2, -1] 277 | inside_image = np.all(x_i > 0, axis=-1) & np.all(x_i < 278 | np.array(img_size[:2])[None, ::-1]-1, axis=-1) 279 | return in_front & inside_image & inside_view 280 | 281 | def project(self, pts_world: np.ndarray, T_ref_world=np.eye(4)): 282 | """ Projects 3d Points from the world system over the reference system into the image frame 283 | 284 | Args: 285 | pts_world (np.ndarray): [n,3] Points in world frame 286 | T_ref_world (np.ndarray, optional): Transformation from reference frame to the world frame. Defaults to np.eye(4). 287 | Returns: 288 | pts_im: [N,2] Pixel coordinates in raw (distorted) image 289 | """ 290 | x_c = ((inv(self.T_cam_ref) @ inv(T_ref_world) @ 291 | homogenous(pts_world).T)[:3]).T # [3,N] 292 | x_n = self.projection.project(x_c) 293 | x_d = self.distortion.distort(x_n) 294 | x_i = x_d @ self.K[:2, :2] + self.K[None, :2, -1] 295 | return x_i 296 | 297 | def undistort_image(self, image_distorted: np.ndarray, method="linear", Kresult=None): 298 | """ Undistort an image 299 | Args: 300 | Kresult: 301 | the 3x3 calibration matrix which is valid for the final image. If None, 302 | the K stored in this class is used (which is usually the estimated K) 303 | """ 304 | if Kresult is None: 305 | Kresult = self.K 306 | 307 | size = image_distorted.shape 308 | 309 | size = (size[1], size[0]) 310 | yy, xx = np.meshgrid( 311 | np.arange(size[1], dtype=np.float64), np.arange(size[0]), indexing="ij") 312 | pts = np.stack([xx, yy], axis=-1) 313 | pts_list = pts.reshape([-1, 2]) 314 | 315 | x_c = homogenous(pts_list) @ inv(Kresult).T 316 | x_n = x_c[:, :2]/x_c[:, -1:] 317 | x_d = self.distortion.distort(x_n) 318 | undist_list = x_d @ self.K[:2, :2] + self.K[None, :2, -1] 319 | 320 | coords = undist_list.reshape(pts.shape).clip( 321 | 0, max=[size[0]-1, size[1]-1]) 322 | interp = interpolate.RegularGridInterpolator(( 323 | np.arange(size[1], dtype=np.float64), np.arange(size[0])), image_distorted, bounds_error=False, fill_value=None, method=method) 324 | image_undistorted = interp(coords[..., [1, 0]]) 325 | 326 | return image_undistorted 327 | 328 | def calculate_lookup_tables(self, image_size, Kresult=None): 329 | """ 330 | Calculate lookup tables, like used in calibration software tcc. A calibration 331 | matrix which is valid for the final lookuptables can be given. 332 | The calculation/adaption follows the follwing idea: 333 | The distorted coordinates with the two different Ks should be 334 | the same (this is for tcc distortion modelling!): 335 | x_dist_o = K * xn + lut(K*xn) 336 | x_dist_r = Kr*xn + lutr(Kr*xn) 337 | If you set x_dist_o == x_distr: 338 | lutr(Kr*xn) = -Kr*xn + K*n + lut(K*xn) 339 | 340 | The distorion functions here are always absolute, thus: 341 | lut(x) = dist(x) - x 342 | 343 | If you apply this to the above function: 344 | lutr(Kr*xn) = dist(K*xn) - Kr*xn 345 | 346 | If dist(x, Kr) = Kr * d(inv(Kr) * x), then you have to 347 | apply a correction: 348 | lutr(Kr*xn) = K*inv(Kr) * dist(Kr*xn, Kr) - Kr*xn 349 | 350 | Args: 351 | self: 352 | The camera object 353 | image_size: 354 | a numpy array or list of length 2 with [no_of_columns, no_of_rows] 355 | of the image. 356 | Kresult: 357 | the 3x3 calibration matrix which is valid for the final image. If None, 358 | the K stored in this class is used (which is usually the estimated K) 359 | 360 | Returns: 361 | (lut, ilut): 362 | Two 2 x columns x rows float arrays with offset values. 363 | lut[0,:,:] are the offsets in column direction, 364 | lut[1,:,:] in row direction. 365 | Correction is is always done with new_coord = old_coord + lut 366 | x_distorted = x_distortion_free + lut 367 | x_distortion_free = x_distorted + ilut 368 | """ 369 | # All coordinates of the image 370 | p_img_2D = np.mgrid[range(image_size[0]), range(image_size[1])] 371 | p_img = np.reshape(p_img_2D.T, (image_size[0]*image_size[1], 2)) 372 | 373 | # For the lut: Given coordinates in the distortion free image, get offsets for the coordinate in 374 | # the original = raw = distorted image 375 | if Kresult is None: 376 | M = np.eye(3) 377 | else: 378 | M = self.K @ inv(Kresult) 379 | 380 | p_dist = self.distort(p_img, Kresult) 381 | p_dist = (homogenous(p_dist) @ M.T)[:, :-1] 382 | lut_1D = p_dist - p_img 383 | lut = np.reshape(lut_1D, (image_size[1], image_size[0], 2)).T 384 | 385 | # For the ilut: Given coordinates in the original = distorted image, get offsets for the coordinates in 386 | # the distortion free image 387 | p_corr = (homogenous(p_img) @ inv(M).T)[:, :-1] 388 | p_free = self.undistort(p_corr, Kresult=Kresult) 389 | ilut_1D = p_free - p_img 390 | ilut = np.reshape(ilut_1D, (image_size[1], image_size[0], 2)).T 391 | 392 | return (lut, ilut) 393 | 394 | @staticmethod 395 | def write_tcc_lut_file(lut_filename: str, lut: np.ndarray): 396 | """ 397 | Write a lut or ilut filename in the format of the calibration software tcc 398 | 399 | Args: 400 | lut_filename: 401 | the (complete) filename 402 | lut: A 2 x columns x rows float array. lut[0,:,:] are the offsets in column direction 403 | lut[1,:,:] in row direction (correction in tcc is always done with new_coord = old_coord + lut) 404 | """ 405 | with open(lut_filename,'w') as fid: 406 | 407 | # Header 408 | fid.write('# Filename:%s\n' % os.path.basename(lut_filename)) 409 | fid.write('#\n') 410 | fid.write('distortiontable\n') 411 | fid.write('# TCC LUT Table\n') 412 | fid.write('#\n') 413 | fid.write('# rectangle for array\n') 414 | fid.write('# basex basey dimx dimy\n') 415 | fid.write(' 0 0 %d %d\n' % (lut.shape[1], lut.shape[2])) 416 | fid.write('#\n') 417 | 418 | # Values 419 | for y in range(lut.shape[2]): 420 | fid.write('##-----------------------------dx dy for row: %d\n' % y); 421 | for x in range(lut.shape[1]): 422 | fid.write(' %f %f\n' % (lut[0, x, y], lut[1, x, y]) ); 423 | 424 | @property 425 | def num_params(self): 426 | return 6 + 4 + len(self.distortion.params) 427 | 428 | def get_param_dict(self, **kwargs): 429 | out = {"extrinsics": self.T_cam_ref, "K": self.K, 430 | "distortion_coeff": self.distortion.to_cv2(), 431 | "is_pinhole": self.is_pinhole} 432 | for key, value in kwargs.items(): 433 | out[key] = value 434 | return out 435 | 436 | def pix2ray(self, x_i): 437 | x_d = (homogenous(x_i) @ 438 | np.linalg.inv(self.K).T)[:, :-1] # [n,2] 439 | x_n = self.distortion.undistort(x_d) 440 | r = self.projection.to_ray(x_n) 441 | return r 442 | 443 | def update_params(self, dparams): 444 | dt, dr, dk, ddist = np.split(dparams.flatten(), np.cumsum( 445 | [3, 3, 4])) 446 | self.distortion.update_params(ddist) 447 | self.T_cam_ref = update_pose(self.T_cam_ref, dt, dr) 448 | 449 | self.K[0, 2] += dk[0] 450 | self.K[1, 2] += dk[1] 451 | self.K[0, 0] += dk[2] 452 | self.K[1, 1] += dk[3] 453 | 454 | dr, dt = np.linalg.norm(dr), np.linalg.norm(dt) 455 | return dr, dt 456 | 457 | def jacobians(self, T_world, coords, ray2apriltag_idx, num_tags): 458 | # Help matrices 459 | T_calib = self.T_cam_ref 460 | 461 | num_rays = len(coords) 462 | x_r = ((inv(T_world) @ 463 | homogenous(coords).T)[:3]).T # [N,3] Coordinates in reference 464 | x_c = ((inv(T_calib) @ inv(T_world) @ 465 | homogenous(coords).T)[:3]).T # [N,3] Coordinates in cami 466 | x_n = self.projection.project(x_c) 467 | x_d = self.distortion.distort(x_n) 468 | 469 | # dpi_dpe 470 | dxi_dxd = self.K[:2, :2] 471 | dxd_dxn, dxd_ddistparams = self.distortion.jacobian(x_n) 472 | dxn_dxc = self.projection.jacobian(x_c) 473 | dxc_dxr = T_calib[:3, :3].T 474 | dxr_dxw = T_world[:3, :3].T 475 | 476 | dxi_dxc = dxi_dxd @ dxd_dxn @ dxn_dxc 477 | dxi_dxr = dxi_dxc @ dxc_dxr 478 | 479 | # Jacobians of Extrinsic Calibration params of camera 480 | de_dt_c = - dxi_dxc @ T_calib[:3, :3].T 481 | 482 | vec = (x_r-T_calib[None, :3, -1]) @ T_calib[:3, :3] 483 | de_dr_c = dxi_dxc @ skew(vec) 484 | 485 | # Jacobians of world pose 486 | de_dt_w = -dxi_dxr @ T_world[:3, :3].T 487 | 488 | vec = (coords-T_world[None, :3, -1]) @ T_world[:3, :3] 489 | de_dr_w = dxi_dxr @ skew(vec) 490 | 491 | # Jacobians of apriltags 492 | de_dp_w = np.zeros([num_rays, 2, num_tags, 3]) 493 | for ri in range(num_rays): 494 | de_dp_w[ri, :, ray2apriltag_idx[ri] 495 | ] = dxi_dxr[ri] @ dxr_dxw 496 | de_dp_w = de_dp_w.reshape([num_rays, 2, num_tags*3]) 497 | 498 | # Jacobians of Intrinsic Camera 499 | dxi_dxk = np.zeros([num_rays, 2, 4]) 500 | dxi_dxk[:, 0, 0] = 1 501 | dxi_dxk[:, 1, 1] = 1 502 | dxi_dxk[:, 0, 2] = x_d[:, 0] 503 | dxi_dxk[:, 1, 3] = x_d[:, 1] 504 | 505 | de_dk = dxi_dxk # dpi_dk 506 | 507 | # Jacobians of Camera distortion 508 | de_dcd = dxi_dxd @ dxd_ddistparams 509 | 510 | # Concat Jacobians and add to Normal equations 511 | J_cam = np.concatenate([de_dt_c, 512 | de_dr_c, 513 | de_dk, 514 | de_dcd], axis=-1).reshape([num_rays*2, -1]) 515 | J_pose = np.concatenate([de_dt_w, 516 | de_dr_w], axis=-1).reshape([num_rays*2, -1]) 517 | J_tags = de_dp_w.reshape([2*num_rays, -1]) 518 | return J_pose, J_cam, J_tags 519 | 520 | 521 | ########################################### 522 | # Projection 523 | ########################################### 524 | 525 | 526 | class PinholeProjection: 527 | @staticmethod 528 | def project(x_c: np.ndarray): 529 | """project points onto 530 | 531 | Args: 532 | x_c (np.ndarray): [N,3] points (rays) in camera frame 533 | 534 | Returns: 535 | x_n: [N,2] projected points 536 | """ 537 | x_n = x_c[:, :2]/x_c[:, -1:] 538 | return x_n 539 | 540 | @staticmethod 541 | def to_ray(x_n): 542 | """points to rays 543 | 544 | Args: 545 | x_n (np.ndarray): [N,2] points in camera frame 546 | 547 | Returns: 548 | x_c: [N,3] rays in camera system 549 | """ 550 | r = homogenous(x_n) 551 | r = r / norm(r, axis=-1, keepdims=True) 552 | return r 553 | 554 | @staticmethod 555 | def jacobian(x_c: np.ndarray): 556 | """Jacobian computation 557 | 558 | Args: 559 | x_c (np.ndarray): [N,3] points in camera frame 560 | 561 | Returns: 562 | J: [N,2,3] Jacobians of projection 563 | """ 564 | J = np.concatenate( 565 | [np.repeat(np.eye(2)[None, :, :], len(x_c), axis=0), -x_c[:, :2, None]/x_c[:, -1, None, None]], axis=-1)/x_c[:, -1, None, None] 566 | return J 567 | 568 | 569 | class FisheyeProjection: 570 | @staticmethod 571 | def project(x_c: np.ndarray): 572 | """project points onto 573 | 574 | Args: 575 | x_c (np.ndarray): [N,3] points (rays) in camera frame 576 | 577 | Returns: 578 | x_n: [N,2] projected points 579 | """ 580 | r_xy = np.sqrt(x_c[:, 0]**2 + x_c[:, 1]**2) 581 | c_ideal = (x_c[:, 0]/r_xy) * np.arctan2(r_xy, x_c[:, 2]) 582 | r_ideal = (x_c[:, 1]/r_xy) * np.arctan2(r_xy, x_c[:, 2]) 583 | x_n= np.stack([c_ideal, r_ideal], axis=-1) 584 | return x_n 585 | 586 | @staticmethod 587 | def to_ray(x_n): 588 | """points to rays 589 | 590 | Args: 591 | x_n (np.ndarray): [N,2] points in camera frame 592 | 593 | Returns: 594 | x_c: [N,3] rays in camera system 595 | """ 596 | 597 | r_xy = np.sqrt(x_n[:, 0]**2 + x_n[:, 1]**2) 598 | X = (np.sin(r_xy)/r_xy) * x_n[:, 0] 599 | Y = (np.sin(r_xy)/r_xy) * x_n[:, 1] 600 | Z = np.cos(r_xy) 601 | return np.stack([X, Y, Z], axis=-1) 602 | @staticmethod 603 | def jacobian(x_c: np.ndarray): 604 | """Jacobian computation 605 | 606 | Args: 607 | x_c (np.ndarray): [N,3] points in camera frame 608 | 609 | Returns: 610 | J: [N,2,3] Jacobians of projection 611 | """ 612 | r_xy = np.sqrt(x_c[:, 0]**2 + x_c[:, 1]**2) 613 | J = np.zeros([len(x_c), 2, 3]) 614 | J[:, 0, 0] = (1/r_xy - x_c[:, 0]**2/r_xy**3) * np.arctan2(r_xy, x_c[:, 2]) + \ 615 | (x_c[:, 0]**2/x_c[:, 2])/(r_xy**2 * (1+r_xy**2/x_c[:, 2]**2)) 616 | J[:, 0, 1] = x_c[:, 0]*x_c[:, 1]*(1/(x_c[:, 2]*r_xy**2*(1 + r_xy ** 617 | 2/x_c[:, 2]**2)) - np.arctan2(r_xy, x_c[:, 2])/r_xy**3) 618 | J[:, 0, 2] = (-x_c[:, 0]/x_c[:, 2]**2)/(1+r_xy**2/x_c[:, 2]**2) 619 | 620 | J[:, 1, 0] = J[:, 0, 1] 621 | J[:, 1, 1] = (1/r_xy - x_c[:, 1]**2/r_xy**3) * np.arctan2(r_xy, x_c[:, 2]) + \ 622 | (x_c[:, 1]**2/x_c[:, 2])/(r_xy**2 * (1+r_xy**2/x_c[:, 2]**2)) 623 | J[:, 1, 2] = (-x_c[:, 1]/x_c[:, 2]**2)/(1+r_xy**2/x_c[:, 2]**2) 624 | return J 625 | -------------------------------------------------------------------------------- /src/ipb_calibration/create_orthophoto_images.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | """ 4 | Create an orthophoto-type projection of a pointcloud. Used for: 5 | Extracting apriltags from Faro laser scanner data. 6 | 7 | Created on Wed Jul 21 15:30:07 2021 8 | 9 | @author: laebe 10 | """ 11 | import argparse 12 | import os 13 | from pathlib import Path 14 | import open3d as o3d 15 | import numpy as np 16 | import tifffile 17 | import matplotlib.pyplot as plt 18 | import tqdm 19 | 20 | # Faster load if already loaded once 21 | #from diskcache import FanoutCache 22 | #import getpass 23 | #cache = FanoutCache( 24 | # directory=os.path.join("/tmp", "fanoutcache_" + getpass.getuser() + "/"), 25 | # shards=64, 26 | # timeout=1, 27 | # size_limit=3e11, 28 | #) 29 | 30 | def create_orthophoto_images(pointcloud_filename: str, odir:str, pixelsize: float, bb:list, 31 | projection_direction: int=3, use_as_column_axis:int = -1, 32 | use_as_row_axis:int = -1, visualize:bool = False): 33 | """ 34 | Create orthophoto-like intensity,x,y,z coordinate image from a (Faro laserscanner) pointcloud. 35 | 36 | Args: 37 | pointcloud_filename: 38 | filename for the pointcloud. User either .ptx for 39 | the original file from Faro or any other type possible 40 | for open3d read function. 41 | 42 | odir: 43 | output directory. Will be created if not there. 44 | Written files are 45 | intensity.tif xcoord.tif ycoord.tif zcoord.tif. 46 | 47 | pixelsize: 48 | "GSD or Size of a pixel in object coordinates (usually meter). 49 | 50 | bb: 51 | Bounding box. The pointcloud is cropped with this bounding box. 52 | Give 6 values as a list: minX, minY, minZ, maxX, maxY, maxZ. 53 | 54 | projection_direction: 55 | X axis is 1, Y axis 2, Z axis 3. This is 56 | the coordinate which is set to 0 to get the orthographic projection. 57 | If you look into negative axis direction, give -1 for X, -2 for Y, 58 | -3 for Z. Note that looking into positive/negative direction means 59 | that the orthophoto is flipped/mirrored. Then the tag code 60 | cannot be recognized any more. 61 | 62 | use_as_column_axis: 63 | Columm axis of the orthophoto.X axis is 0, Y axis 1, Z axis 2. If not 64 | given, as suitable default is used. Note that not giving that axis, 65 | the orthophoto might be flipped. Not given is a value <0. 66 | 67 | use_as_row_axis: 68 | Row axis of the orthophoto.X axis is 0, Y axis 1, Z axis 2. If not 69 | given, as suitable default is used. Note that not giving that axis 70 | the orthophoto might be flipped. Not given is a value <0. 71 | 72 | visualize: 73 | show the orthophoto (the intensity of the orthophoto). 74 | """ 75 | # Load 76 | print("Load pointcloud %s ... " % pointcloud_filename, flush=True) 77 | _, file_extension = os.path.splitext(pointcloud_filename) 78 | if file_extension == ".ptx": 79 | pcd = read_ptx_pointcloud(pointcloud_filename) 80 | #print("Write ply ...",flush=True) 81 | #o3d.io.write_point_cloud('/tmp/t.ply', pcd, write_ascii=False, compressed=True) 82 | else: 83 | pcd = o3d.io.read_point_cloud(pointcloud_filename) 84 | 85 | 86 | # crop out 87 | min_bound=np.array([bb[0], bb[1], bb[2]]).T 88 | max_bound=np.array([bb[3], bb[4], bb[5]]).T 89 | bbox = o3d.geometry.AxisAlignedBoundingBox(min_bound,max_bound) 90 | pcd = pcd.crop(bbox) 91 | 92 | # pcd in 2D format 93 | X = np.asarray(pcd.points) 94 | X2D = X.copy() 95 | X2D[:, np.abs(projection_direction)-1]=0 96 | pcd2D = o3d.geometry.PointCloud(o3d.utility.Vector3dVector(X2D)) 97 | intensity = np.asarray(pcd.colors).copy() 98 | intensity = 255.0 * intensity[:,0] 99 | 100 | # define the axis of the orthophoto 101 | # This are the default axis: 102 | if np.abs(projection_direction)==1: 103 | first_orthindex = 1 104 | second_orthindex = 2 105 | else: 106 | if np.abs(projection_direction)==2: 107 | first_orthindex = 2 108 | second_orthindex = 0 109 | else: 110 | # Default,should be projection along Z 111 | first_orthindex = 0 112 | second_orthindex = 1 113 | 114 | # If projection direction is negative, we look into negative direction, 115 | # thus the orthophoto has to be mirrored. Do this by swapping r/c axis 116 | # of orthophoto 117 | if projection_direction<0: 118 | h = first_orthindex 119 | first_orthindex = second_orthindex 120 | second_orthindex = h 121 | 122 | 123 | # If axis given by the user, use them 124 | if use_as_column_axis>=0: 125 | first_orthindex = use_as_column_axis 126 | if use_as_row_axis>=0: 127 | second_orthindex= use_as_row_axis 128 | 129 | # show 130 | # o3d.visualization.draw_geometries([pcd]) 131 | 132 | # define orthophoto 133 | min_bound = pcd.get_min_bound() 134 | max_bound = pcd.get_max_bound() 135 | min_firstidx = min_bound[first_orthindex] 136 | min_secondidx = min_bound[second_orthindex] 137 | # X == columns, Y = rows 138 | no_rows = int(np.ceil( (max_bound[second_orthindex] - min_bound[second_orthindex]) / pixelsize)) 139 | no_cols = int(np.ceil( (max_bound[first_orthindex] - min_bound[first_orthindex]) / pixelsize)) 140 | print("Resulting images have size (rows x cols): %d x %d " % (no_rows, no_cols), flush=True) 141 | 142 | img_intensity = np.zeros((no_rows, no_cols), dtype='uint8') 143 | img_coord = np.zeros((no_rows, no_cols,3), dtype='float32') 144 | 145 | # Search tree 146 | print("Create search tree ...", flush=True) 147 | kdtree = o3d.geometry.KDTreeFlann(pcd2D) 148 | 149 | # And go 150 | print("Create images ...", flush=True) 151 | query = np.zeros((3,1), dtype='float32') 152 | orthoidxs = [first_orthindex, second_orthindex] 153 | for r in tqdm.tqdm(range(no_rows)): 154 | #print("row %d of %d ..." % (r, no_rows), flush=True) 155 | for c in range(no_cols): 156 | query[first_orthindex] = min_firstidx + pixelsize * c 157 | query[second_orthindex] = min_secondidx + pixelsize * r 158 | _, idx, _ = kdtree.search_knn_vector_3d(query, 4) 159 | radii = np.linalg.norm(X[idx,:][:, orthoidxs] - query[orthoidxs].T, axis=1) 160 | weights = radii / np.sum(radii) 161 | img_intensity[r,c] = np.round(np.sum(weights * intensity[idx])) 162 | img_coord[r,c,:] = np.sum(np.expand_dims(weights, axis=1) * X[idx,:], axis=0) 163 | 164 | # Save 165 | Path(odir).mkdir(parents=True, exist_ok=True) 166 | tifffile.imsave(os.path.join(odir, 'intensity.tif'), np.expand_dims(img_intensity, axis=2)) 167 | tifffile.imsave(os.path.join(odir, 'xcoord.tif'), img_coord[:,:,0:1]) 168 | tifffile.imsave(os.path.join(odir, 'ycoord.tif'), img_coord[:,:,1:2]) 169 | tifffile.imsave(os.path.join(odir, 'zcoord.tif'), img_coord[:,:,2:3]) 170 | 171 | #show 172 | if visualize: 173 | plt.figure(1); 174 | plt.clf(); 175 | plt.imshow(img_intensity, cmap='gray', vmin=0, vmax=255) 176 | plt.pause(0.02) 177 | plt.show() 178 | 179 | 180 | #@cache.memoize(typed=True) 181 | def read_ptx_pointcloud(filename: str) -> o3d.geometry.PointCloud: 182 | """ 183 | Read a pointcloud in .ptx format. This format is from the Faro laser scanner. 184 | It should be ASCII and has x,y,i,r,g,b in every line (and an addtional header) 185 | 186 | Args: 187 | filename: the complete filename of the file 188 | 189 | Returns: 190 | an open3d pointcloud which has the intensity as color (all values the same for rgb) 191 | """ 192 | data = np.loadtxt(filename, skiprows=10, dtype='float32') 193 | 194 | pcd = o3d.geometry.PointCloud(o3d.utility.Vector3dVector(data[:,0:3])) 195 | pcd.colors = o3d.utility.Vector3dVector(np.stack([data[:,3], data[:,3], data[:,3]]).T) 196 | return pcd 197 | 198 | 199 | 200 | if __name__ == "__main__": 201 | """ 202 | Create an orthophoto-type projection of a pointcloud. Used for: 203 | Extracting apriltags from Faro laser scanner data. 204 | """ 205 | 206 | #%% Parse arguments 207 | parser = argparse.ArgumentParser(description="create orthophoto-like intensity,x,y,z coordinate image from a (Faro laserscanner) pointcloud") 208 | parser.add_argument("pointcloud_filename", help=("filename for the pointcloud. User either .ptx for " 209 | "the original file from Faro or any other type possible " 210 | "for open3d read function.")) 211 | parser.add_argument("--output_directory", "-o", help=("default output directory. Will be created " 212 | "if not there. Written files are " 213 | "intensity.tif xcoord.tif ycoord.tif zcoord.tif. " 214 | "Default is ./"), default="./") 215 | parser.add_argument("--pixelsize", "-s", default=0.001, type=float, 216 | help="GSD or Size of a pixel in object coordinates (usually meter). Default: 0.001") 217 | parser.add_argument("--bounding_box", "-b", nargs=6, type=float, 218 | help="Bounding box. The pointcloud is cropped with this bounding box. " 219 | "Give 6 values: minX, minY, minZ, maxX, maxY, maxZ " 220 | "default= -10.0 -10.0 -10.0 10.0 10.0 10.0]", 221 | default=[-10.0, -10.0, -10.0, 10.0, 10.0, 10.0]) 222 | parser.add_argument("--projection_direction", default=2, type=int, 223 | help="Projection direction: X axis is 1, Y axis 2, Z axis 3. This is" 224 | "the coordinate which is set to 0 to get the orthographic projection. " 225 | "If you look into negative axis direction, give -1 for X, -2 for Y, " 226 | "-3 for Z. Note that looking into positive/negative direction means " 227 | "that the orthophoto is flipped/mirrored. Then the tag code " 228 | "cannot be recognized any more. Default: 3.") 229 | parser.add_argument('--visualize', '-v', action='store_true', help = 230 | 'show the orthophoto (the intensity of the orthophoto).') 231 | 232 | # Settings 233 | args = parser.parse_args() 234 | pointcloud_filename = args.pointcloud_filename 235 | odir = args.output_directory 236 | pixelsize = args.pixelsize 237 | bb = args.bounding_box 238 | projection_direction = args.projection_direction 239 | use_as_column_axis = args.use_as_column_axis 240 | use_as_row_axis = args.use_as_row_axis 241 | visualize = args.visualize 242 | 243 | create_orthophoto_images(pointcloud_filename, odir, pixelsize, bb, 244 | projection_direction, visualize=visualize) 245 | 246 | -------------------------------------------------------------------------------- /src/ipb_calibration/detect_apriltags.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | """ 4 | Detect apriltags in orthophotos. 5 | 6 | Created on Thu Sep 26 15:07:51 2024 7 | 8 | @author: laebe 9 | """ 10 | import os 11 | import argparse 12 | import numpy as np 13 | import tifffile 14 | import cv2 15 | import matplotlib.pyplot as plt 16 | import apriltag 17 | 18 | 19 | def detect_apriltags(working_dir:str, max_pyramid_level:int = 2, 20 | visualize_tags:bool = False) ->np.ndarray: 21 | """ 22 | Given a set of intensity and coordinates images (of an orthophoto), extract apriltag coordinates 23 | on these images. 24 | 25 | The results are written into the working directory with files 26 | apriltagcoords.txt (containing ID, X Y Z coordinates) and 27 | apriltagcoords_rc.txt (containing ID, X Y Z row column coordinates). 28 | 29 | Args: 30 | working_dir: 31 | directory of the input orthophoto files 32 | (intensity.tif, xcoord.tif, ycoord.tif, zcoord.tif). The output will be written 33 | into this directory as file apriltagcoords.txt 34 | 35 | max_pyramid_level: 36 | Pyramid level of the smallest images used. In order to detect very large 37 | apriltags, the tag detection in not only done on the original image, but also 38 | on higher image pyramid levels. This is the highest image pyramid level number. 39 | '0 == original size, 1 == half rows/cols of original, ... Default is: 2 40 | 41 | visualize_tags: 42 | plot the extracted tags in a figure on the intensity image. 43 | Default: False 44 | 45 | Returns: 46 | all_tags_3D, thus a nx6 numpy array of all coordinates with 47 | columns ID X Y Z row column 48 | X,Y,Z: 3D coordinates of the extracted apriltag corners 49 | row, column: The intensity image row column coordinates. They are relative to the upper left pixel CENTER, 50 | thus the upper left corner of the upper left pixel has coordinates (-0.5, -0.5) 51 | 52 | """ 53 | 54 | #%% Load images 55 | print('Load images ...', flush=True) 56 | image = tifffile.imread(os.path.join(working_dir, 'intensity.tif')) 57 | 58 | Ximage = tifffile.imread(os.path.join(working_dir, 'xcoord.tif')) 59 | Yimage = tifffile.imread(os.path.join(working_dir, 'ycoord.tif')) 60 | Zimage = tifffile.imread(os.path.join(working_dir, 'zcoord.tif')) 61 | 62 | #%% Detect 63 | detector = apriltag.apriltag("tag36h11", decimate=1, maxhamming=2) 64 | 65 | all_tags_2D = dict() # Tags with ID as keys, [r,c] as values 66 | 67 | 68 | for level in range(max_pyramid_level, -1, -1): 69 | #for level in (2,2): 70 | print('\nWorking on pyramid level %d ...' % level, flush=True) 71 | level_factor = 2 ** level 72 | if image.ndim == 3: 73 | img_level = np.copy(image[:,:,0]) 74 | else: 75 | img_level = np.copy(image[:,:]) 76 | 77 | for i in range(level): 78 | img_level = cv2.pyrDown(img_level) 79 | 80 | # call the actual detector 81 | results = detector.detect(img_level) 82 | 83 | # Convert from apriltag detector output to list with ID, c,r 84 | no_tags = len(results) 85 | for i in range(no_tags): 86 | #Our ID scheme: 100* (apriltag_id) + [1,2,3,4] for the four corners 87 | ids = np.arange(100*results[i]['id'] + 1, 100*results[i]['id'] + 5) 88 | corners = results[i]['lb-rb-rt-lt'] 89 | # Use our sequence for the numbering: upper left, upper right , bottom left, bottom right 90 | corners = corners[[3, 2, 0, 1]] 91 | 92 | # April tags seem to have coordinates where (0,0) is not the center, but the 93 | # upper left corner of a pixel. We want integer coordinates at the pixel center. 94 | # Thus substract half a pixel. 95 | corners = level_factor * (corners - 0.5) 96 | 97 | # Put into dictionary. Note that previous detections on higher pyramid levels will be overwritten 98 | for j in range(len(ids)): 99 | all_tags_2D[ ids[j] ] = corners[j, :] 100 | 101 | print('... detected %d tags on level %d.' % ( no_tags, level ) ) 102 | 103 | print('\n... detected %d tags on all levels.' % ( len(all_tags_2D)/4 ) ) 104 | 105 | #%% Extract 3D coordinates 106 | print('Extract 3D coordinates ...', flush=True) 107 | # Put into array with ID X Y Z r c 108 | all_tags_3D = np.zeros((len(all_tags_2D), 6)) 109 | for tag_id, i in zip(all_tags_2D.keys(), range(len(all_tags_2D)) ): 110 | all_tags_3D[i, 0 ] = tag_id 111 | all_tags_3D[i, 4] = all_tags_2D[tag_id][1] 112 | all_tags_3D[i, 5] = all_tags_2D[tag_id][0] 113 | 114 | all_tags_3D[i, 1] = getInterpolated(Ximage, all_tags_3D[i, 4:6]) 115 | all_tags_3D[i, 2] = getInterpolated(Yimage, all_tags_3D[i, 4:6]) 116 | all_tags_3D[i, 3] = getInterpolated(Zimage, all_tags_3D[i, 4:6]) 117 | 118 | #%% Save 119 | print('Save ...', flush=True) 120 | # as text file with ID X Y Z 121 | np.savetxt(os.path.join(working_dir, 'apriltag_coords.txt'), all_tags_3D[:, :4], fmt='%4d %10.5f %10.5f %10.5f', 122 | header='%d' % all_tags_3D.shape[0], comments='') 123 | 124 | # as text file with ID X Y Z r c (r,c: (0,0) is upper left pixel center) 125 | np.savetxt(os.path.join(working_dir, 'apriltag_coords_rc.txt'), all_tags_3D, fmt='%4d %10.5f %10.5f %10.5f %8.2f %8.2f', 126 | header='%d' % all_tags_3D.shape[0], comments='') 127 | 128 | print('... done.') 129 | 130 | #%% Plot 131 | if visualize_tags: 132 | plt.figure(1) 133 | plt.clf() 134 | plt.imshow(image, cmap='gray') 135 | plt.plot(all_tags_3D[:,5], all_tags_3D[:,4], 'r.') # plot -> c,r integers are at pixel centers 136 | plt.title('Extracted apriltag coordinates') 137 | plt.show() 138 | 139 | return all_tags_3D 140 | 141 | 142 | 143 | def getInterpolated(img: np.ndarray, rc: np.ndarray) -> float: 144 | """ 145 | Helper function: Bilinear interpolation on an image 146 | Args: 147 | img: 148 | a 2D image 149 | rc: an array/list with two elements [row, colum] 150 | 151 | """ 152 | rint = int(rc[0]) 153 | cint = int(rc[1]) 154 | 155 | a = rc[0] - rint 156 | b = rc[1] - cint 157 | 158 | return ( (img[rint, cint] * (1.0 - a) + img[rint + 1, cint] * a) * (1.0 - b) + 159 | (img[rint, cint + 1] * (1.0 - a) + img[rint + 1, cint + 1] * a) * b ) 160 | 161 | 162 | 163 | if __name__ == '__main__': 164 | 165 | #%% Settings 166 | parser = argparse.ArgumentParser(description='Extract apriltag coordinates in an orthophoto.' 167 | ' An orthophoto in this context is a set of four images: ' 168 | 'an intensity image and 3 float images for the corresponding X,Y,Z coordinates ' 169 | 'of the pointcloud which was used for generating the orthophoto.') 170 | parser.add_argument('working_dir', help = 'directory of the input orthophoto files ' 171 | '(intensity.tif, xcoord.tif, ycoord.tif, zcoord.tif). The output will be written ' 172 | 'into this directory as file apriltagcoords.txt') 173 | parser.add_argument('--max_pyramid_level', '-p', type = int, default = 2, help = 174 | 'Pyramid level of the smallest images used. In order to detect very large ' 175 | 'apriltags, the tag detection in not only done on the original image, but also ' 176 | 'on higher image pyramid levels. This is the highest image pyramid level number. ' 177 | '0 == original size, 1 == half rows/cols of original, ... Default is: 2') 178 | parser.add_argument('--visualize_tags', '-v', action='store_true', help = 179 | 'plot the extracted tags on the image.') 180 | 181 | args = parser.parse_args() 182 | working_dir = args.working_dir 183 | max_pyramid_level = args.max_pyramid_level 184 | visualize_tags = args.visualize_tags 185 | 186 | detect_apriltags(working_dir, max_pyramid_level, visualize_tags) 187 | 188 | 189 | -------------------------------------------------------------------------------- /src/ipb_calibration/io.py: -------------------------------------------------------------------------------- 1 | import yaml 2 | from ipb_calibration import camera, lidar 3 | 4 | 5 | def sensors_from_dict(cfg): 6 | """ 7 | Create calibration object from a calibration configuration. 8 | 9 | Returns: 10 | a tuple (cam, lidars) of lists of cameras, and lidars 11 | 12 | """ 13 | cameras = [] 14 | lidars = [] 15 | 16 | for topic in cfg: 17 | if ("image" in topic) or ("cam" in topic): 18 | cameras.append(camera.Camera.fromdict(cfg[topic])) 19 | if ("points" in topic) or ("laser" in topic) or ("lidar" in topic): 20 | lidars.append(lidar.Lidar.fromdict(cfg[topic])) 21 | return cameras, lidars 22 | 23 | 24 | def sensors_from_dict_as_dict(cfg): 25 | """ 26 | Create calibration object from a calibration configuration. 27 | Returns: 28 | a tuple (cameras, lidars) of dicts with the names (e.g.topics names) as keys 29 | """ 30 | cameras = dict() 31 | lidars = dict() 32 | 33 | for topic in cfg: 34 | if ("image" in topic) or ("cam" in topic): 35 | cameras[topic] = camera.Camera.fromdict(cfg[topic]) 36 | if ("points" in topic) or ("laser" in topic) or ("lidar" in topic): 37 | lidars[topic] = lidar.Lidar.fromdict(cfg[topic]) 38 | return cameras, lidars 39 | 40 | 41 | def read_calibration_yaml_file(filename:str): 42 | """ 43 | Read a calibration written as yaml file and initialize corresponding 44 | laser and camera objects 45 | 46 | Args: 47 | filename: 48 | The (complete) filename of the yaml file 49 | Returns: 50 | A tuple (cams, lasers, cam_names, laser_names) with 51 | cams: a dict of camera objects 52 | lasers: a dict of laser objects 53 | """ 54 | with open(filename) as f: 55 | calib = yaml.load(f, Loader=yaml.SafeLoader) 56 | 57 | cams, lasers = sensors_from_dict_as_dict(calib) 58 | 59 | return (cams, lasers) 60 | 61 | -------------------------------------------------------------------------------- /src/ipb_calibration/ipb_preprocessing.py: -------------------------------------------------------------------------------- 1 | from numpy.linalg import norm, inv 2 | import copy 3 | import ipdb 4 | import cv2 5 | from scipy.spatial.transform import Rotation 6 | import open3d as o3d 7 | from pathlib import Path 8 | import xml.etree.ElementTree as ET 9 | import yaml 10 | import tqdm 11 | import glob 12 | import numpy as np 13 | import matplotlib.pyplot as plt 14 | from os.path import join 15 | from ipb_calibration.apriltag import Apriltags 16 | from numpy.linalg import inv 17 | 18 | from diskcache import FanoutCache 19 | from ipb_calibration.utils import homogenous 20 | cache = FanoutCache("cache/", size_limit=3e11) 21 | 22 | 23 | ################################################################################################### 24 | # Open3d lidar based initial guess 25 | ################################################################################################### 26 | 27 | 28 | def draw_registration_result(source, target, transformation): 29 | source_temp = copy.deepcopy(source) 30 | source_temp.transform(transformation) 31 | o3d.visualization.draw_geometries([source_temp, target]) 32 | 33 | 34 | def preprocess_point_cloud(pcd, voxel_size): 35 | pcd_down = pcd.voxel_down_sample(voxel_size) 36 | 37 | radius_normal = voxel_size * 2 38 | pcd_down.estimate_normals( 39 | o3d.geometry.KDTreeSearchParamHybrid(radius=radius_normal, max_nn=30)) 40 | 41 | radius_feature = voxel_size * 5 42 | pcd_fpfh = o3d.pipelines.registration.compute_fpfh_feature( 43 | pcd_down, 44 | o3d.geometry.KDTreeSearchParamHybrid(radius=radius_feature, max_nn=100)) 45 | return pcd_down, pcd_fpfh 46 | 47 | 48 | def prepare_dataset(target, sources, voxel_size): 49 | sources_down, sources_fpfh = [], [] 50 | for source in sources: 51 | source_down, source_fpfh = preprocess_point_cloud(source, voxel_size) 52 | sources_down.append(source_down) 53 | sources_fpfh.append(source_fpfh) 54 | target_down, target_fpfh = preprocess_point_cloud(target, voxel_size) 55 | return sources, target, sources_down, target_down, sources_fpfh, target_fpfh 56 | 57 | 58 | def execute_global_registration(source_down, target_down, source_fpfh, 59 | target_fpfh, voxel_size): 60 | distance_threshold = voxel_size * 1.5 61 | result = o3d.pipelines.registration.registration_ransac_based_on_feature_matching( 62 | source_down, target_down, source_fpfh, target_fpfh, True, 63 | distance_threshold, 64 | o3d.pipelines.registration.TransformationEstimationPointToPoint(False), 65 | 3, [ 66 | o3d.pipelines.registration.CorrespondenceCheckerBasedOnEdgeLength( 67 | 0.9), 68 | o3d.pipelines.registration.CorrespondenceCheckerBasedOnDistance( 69 | distance_threshold) 70 | ], o3d.pipelines.registration.RANSACConvergenceCriteria(100000, 0.9999)) 71 | return result 72 | 73 | 74 | def estimate_initial_guess_lidar(map, scans, voxel_size=0.1, visualize=False, fine_tune=False, max_dist=0.5): 75 | print("Estimate initial Guess") 76 | sources, target, sources_down, target_down, sources_fpfh, target_fpfh = prepare_dataset( 77 | target=map, sources=scans, voxel_size=voxel_size) 78 | poses = [] 79 | 80 | for source, source_down, source_fpfh in zip(tqdm.tqdm(sources), sources_down, sources_fpfh): 81 | result = execute_global_registration( 82 | source_down, target_down, source_fpfh, target_fpfh, voxel_size=voxel_size) 83 | if fine_tune: 84 | loss = o3d.pipelines.registration.GMLoss(k=0.1) 85 | method = o3d.pipelines.registration.TransformationEstimationPointToPlane( 86 | loss) 87 | result = o3d.pipelines.registration.registration_icp( 88 | source, target, max_dist, result.transformation, 89 | method) 90 | if visualize: 91 | draw_registration_result( 92 | source=source, target=target, transformation=result.transformation) 93 | poses.append(result.transformation) 94 | return np.stack(poses, axis=0) 95 | 96 | 97 | def execute_fast_global_registration(source_down, target_down, source_fpfh, 98 | target_fpfh, voxel_size): 99 | distance_threshold = voxel_size * 0.5 100 | result = o3d.pipelines.registration.registration_fgr_based_on_feature_matching( 101 | source_down, target_down, source_fpfh, target_fpfh, 102 | o3d.pipelines.registration.FastGlobalRegistrationOption( 103 | maximum_correspondence_distance=distance_threshold)) 104 | return result 105 | 106 | 107 | @cache.memoize(typed=True) 108 | def estimate_init_lidar_poses(path_data, faro_file, voxel_size=0.1): 109 | pcd_map = o3d.io.read_point_cloud(faro_file) 110 | lidar_points, _ = parse_lidar_data(path_data) 111 | T_lidar_map = estimate_initial_guess_lidar( 112 | pcd_map, lidar_points[0], voxel_size=voxel_size) 113 | return T_lidar_map 114 | 115 | 116 | def load_scan(file): 117 | points = np.load(file) 118 | if len(points.dtype) >= 1: 119 | xyz = np.stack([points["x"], points["y"], points["z"]], axis=-1) 120 | else: 121 | xyz = points 122 | cloud = o3d.geometry.PointCloud() 123 | cloud.points = o3d.utility.Vector3dVector(np.asarray(xyz.reshape([-1, 3]), dtype=float)) 124 | return cloud 125 | 126 | 127 | def parse_lidar_data(path_data, max_scanpoints:int=2500): 128 | """ 129 | Args: 130 | max_scanpoints: 131 | The scans are randomly downsampled. This argument is the maximal 132 | number of points after downsampling. If -1, no downsampling 133 | 134 | """ 135 | T_os_cam = [] 136 | lidar_points = [] 137 | cfg = yaml.safe_load(open(join(path_data, "calibration.yaml"))) 138 | 139 | for pcd_topic in cfg['point_cloud_topics']: 140 | topic = pcd_topic.replace('/', '') 141 | folder = join(path_data, topic) 142 | scans = sorted(glob.glob( 143 | f'{folder}/*.npy')) 144 | print('nr scans:', len(scans)) 145 | 146 | init_p = cfg['init_lidari_to_cam0'][topic] 147 | init_c = np.eye(4) 148 | init_c[:3, :3] = Rotation.from_euler( 149 | "xyz", np.array(init_p[:3]), degrees=True).as_matrix() 150 | init_c[:3, -1] += init_p[3:] 151 | T_os_cam.append(init_c) 152 | 153 | pcds = [] 154 | for i, scanf in enumerate(tqdm.tqdm(scans)): 155 | scan = load_scan(scanf) 156 | if max_scanpoints>0: 157 | ratio = max_scanpoints/len(scan.points) 158 | if ratio>1: 159 | ratio=1 160 | scan = scan.random_down_sample(ratio) 161 | # o3d.visualization.draw_geometries([scan]) 162 | pcds.append(scan) 163 | lidar_points.append(pcds) 164 | return lidar_points, T_os_cam, cfg 165 | 166 | 167 | def xyz_rxryrz2pose(xyz, rxryrz): 168 | T = np.eye(4) 169 | T[:3, :3] = Rotation.from_euler( 170 | "xyz", angles=rxryrz, degrees=True).as_matrix() 171 | T[:3, -1] = xyz 172 | return T 173 | ################################################################################################### 174 | # Init guess using Cameras 175 | ################################################################################################### 176 | 177 | 178 | def parse_intrinsics(cam_intrinsics_xml, num_cams=4): 179 | path_intrinsics = Path(cam_intrinsics_xml).parent 180 | tree = ET.parse(cam_intrinsics_xml) 181 | intrinsics = tree.findall("PinholeCamera") 182 | calib = [] 183 | for i in range(num_cams): 184 | K = np.eye(3) 185 | K[[0, 1], [0, 1]] = float( 186 | intrinsics[i].find("cameraConstant").text) 187 | K[:2, -1] = np.array([float(xh) for xh in ( 188 | intrinsics[i].find("principalPoint").text.strip().split(" "))]) 189 | ilut = intrinsics[i].find( 190 | "filenameLutDist2Undist").text.strip() 191 | lut = intrinsics[i].find( 192 | "filenameLutUndist2Dist").text.strip() 193 | calib.append({"K": K, "ilut": join(path_intrinsics, ilut), 194 | "lut": join(path_intrinsics, lut)}) 195 | return calib 196 | 197 | 198 | 199 | def est_pose_from_obs(observations, cam_is_pinhole, K=np.eye(3), dist_coeff=np.zeros(5)): 200 | points_3d = np.concatenate([det["coords"] 201 | for det in observations]) 202 | points_2d = np.concatenate([det["corners"] 203 | for det in observations]) 204 | if cam_is_pinhole: 205 | rays = homogenous(points_2d) @ inv(K).T 206 | rays /= rays[:, -1:] 207 | else: 208 | points_2d_undist = cv2.fisheye.undistortPoints(points_2d[:, np.newaxis, :], K, np.zeros((1, 4))) 209 | points_2d_undist = points_2d_undist.squeeze(1) 210 | rays = homogenous(points_2d_undist) 211 | rays /= rays[:, -1:] 212 | 213 | valid, rvec, tvec, inliers = cv2.solvePnPRansac( 214 | points_3d, rays[:, :2], np.eye(3), dist_coeff) 215 | assert valid 216 | T_map2cam = np.eye(4) 217 | T_map2cam[:3, -1:] = tvec 218 | T_map2cam[:3, :3] = Rotation.from_rotvec(rvec.squeeze()).as_matrix() 219 | T_cam2map = inv(T_map2cam) 220 | return T_cam2map, valid, len(inliers) 221 | 222 | 223 | def estimate_initial_guess(observations, cam_is_pinhole, init_K=None, dist_coeff=None): 224 | num_cameras = len(observations) 225 | num_poses = len(observations[0]) 226 | # count for each camera and each timestamp how many observations 227 | num_obs = np.zeros([num_cameras, num_poses]) # [num_cams, num_poses] 228 | for c, c_obs in enumerate(observations): 229 | for t, t_obs in enumerate(c_obs): 230 | num_obs[c, t] = len(t_obs) 231 | 232 | # Estimate extrinsics between cameras from the timestamp with the most tags in both cameras 233 | best_pose_t = np.zeros(num_cameras-1, dtype=np.int64) 234 | for i in range(1, len(observations)): 235 | best_pose_t[i-1] = np.argmax(np.min(num_obs[[0, i]], axis=0)) 236 | 237 | T_cami_cam0 = np.zeros([num_cameras, 4, 4]) 238 | T_cami_cam0[0] = np.eye(4) 239 | for c_, t in enumerate(best_pose_t): 240 | c = c_+1 241 | T_cam0_map, valid_, num_inlier = est_pose_from_obs( 242 | observations[0][t], cam_is_pinhole[0], init_K[0], dist_coeff[0]) 243 | T_cami_map, valid_, num_inlier = est_pose_from_obs( 244 | observations[c][t], cam_is_pinhole[c], init_K[c], dist_coeff[c]) 245 | T_cami_cam0[c] = inv(T_cam0_map) @ T_cami_map 246 | 247 | # Estimate pose from the image with most tags 248 | best_cam_t = np.argmax(num_obs, axis=0) 249 | 250 | T_cami_map_t = np.zeros([num_poses, 4, 4]) 251 | for t, c in enumerate(best_cam_t): 252 | T_cam2map, valid_, num_inlier = est_pose_from_obs( 253 | observations[c][t], cam_is_pinhole[c], init_K[c], dist_coeff[c]) 254 | assert valid_, f"NOT VALID POSE ESTIMATION, num inlier: {num_inlier}" 255 | T_cami_map_t[t] = T_cam2map 256 | T_cam0_map_t = T_cami_map_t @ inv(T_cami_cam0[best_cam_t]) 257 | 258 | return T_cam0_map_t, T_cami_cam0 259 | 260 | 261 | def parse_camera_data(apriltag_file, path_data): 262 | april = Apriltags(apriltag_file) 263 | cfg = yaml.safe_load(open(join(path_data, "calibration.yaml"))) 264 | 265 | num_cams = len(cfg["image_topics"]) 266 | observations = [] 267 | T_cami_cam0 = [] 268 | 269 | image_sizes = np.zeros([num_cams, 2], dtype=np.int64) 270 | seen_tags = {} 271 | for c, image_topic in enumerate(tqdm.tqdm(cfg['image_topics'])): 272 | observations.append([]) 273 | topic = image_topic.replace('/', '') 274 | print(10*"*", topic, 10*"*") 275 | 276 | folder = join(path_data, topic) 277 | 278 | images = sorted(glob.glob( 279 | f'{folder}/*.png')) 280 | 281 | for t, image in enumerate(tqdm.tqdm(images)): 282 | img = plt.imread(image) 283 | image_sizes[c] = img.shape[1], img.shape[0] 284 | gray = (np.mean(img, axis=-1)*255).astype("uint8") 285 | observations[-1].append(april.detect(gray)) 286 | for det in observations[-1][-1]: 287 | seen_tags[det["tag_id"]] = det["coords"] 288 | 289 | if ('init_cami_to_cam0' in cfg.keys()): 290 | init_p = cfg['init_cami_to_cam0'][topic] 291 | init_c = np.eye(4) 292 | init_c[:3, :3] = Rotation.from_euler( 293 | "xyz", np.array(init_p[:3]), degrees=True).as_matrix() 294 | init_c[:3, -1] += init_p[3:] 295 | T_cami_cam0.append(init_c) 296 | 297 | tag, coords = np.fromiter( 298 | seen_tags.keys(), dtype=np.int64), np.stack([*seen_tags.values()]) 299 | coords = coords.reshape([-1, 3]) 300 | tag2indx = np.full([tag.max()+1, 4], -1) 301 | tag2indx[tag] = np.arange(len(tag)*4).reshape([-1, 4]) 302 | 303 | indices = [] 304 | imgpixel = [] 305 | for c, c_obs in enumerate(observations): 306 | c_ind = [] 307 | c_img = [] 308 | for t, t_obs in enumerate(c_obs): 309 | if len(t_obs) > 0: 310 | img_pixel = np.concatenate( 311 | [d["corners"] for d in t_obs], axis=0) 312 | idx = tag2indx[[d["tag_id"] for d in t_obs]].reshape(-1) 313 | else: 314 | img_pixel = np.zeros([0, 2]) 315 | idx = np.zeros([0]) 316 | c_ind.append(idx) 317 | c_img.append(img_pixel) 318 | 319 | imgpixel.append(c_img) 320 | indices.append(c_ind) 321 | 322 | # Take a look if user has given camera model in yaml file 323 | if 'camera_models' in cfg.keys(): 324 | cam_is_pinhole = [m == 'pinhole' for m in cfg['camera_models']] 325 | # Check 326 | cam_is_fisheye = [m == 'fisheye' for m in cfg['camera_models']] 327 | if np.sum(cam_is_pinhole)+np.sum(cam_is_fisheye) != len(T_cami_cam0): 328 | print('\n**************************************************************************************') 329 | print('WARNING: The sum of all pinhole and all fisheye cameras is not the number of all cameras !!') 330 | print(' Did you use the wrong keyword for camera models in calibration.yaml ?') 331 | print('**************************************************************************************', 332 | flush=True) 333 | else: 334 | print('\n\nNo camera models given in yaml file, assuming pinhole for all cameras.\n', flush=True) 335 | cam_is_pinhole = len(T_cami_cam0) * [True] 336 | 337 | return imgpixel, indices, coords, observations, T_cami_cam0, image_sizes, cam_is_pinhole 338 | 339 | ################################################################################################### 340 | # Init guess Camera intrinsics 341 | ################################################################################################### 342 | 343 | 344 | def find_plane(pts3d, pts2d, threshold=0.1): 345 | num = [] 346 | points3d = [] 347 | points2d = [] 348 | for (p1, p2, p3, p4) in pts3d: 349 | e1 = (p2-p1)/norm(p2-p1) 350 | e2 = (p3-p1)/norm(p3-p1) 351 | e3 = np.cross(e1, e2) 352 | R = np.stack([e1, e2, e3], axis=-1) 353 | pts_loc = (pts3d-p1) @ R 354 | valids = np.all(np.abs(pts_loc[:, :, -1]) < threshold, axis=-1) 355 | valid_pts = pts_loc[valids] 356 | valid_pts = valid_pts.reshape(-1, 3) 357 | valid_pts[:, -1] = 0 358 | 359 | points3d.append(valid_pts) 360 | points2d.append(pts2d[valids].reshape(-1, 2)) 361 | num.append(len(valid_pts)) 362 | 363 | best = np.argmax(num) 364 | return points3d[best], points2d[best] 365 | 366 | 367 | def estimate_initial_k(observations, cam_is_pinhole, max_z_dist=0.1, image_size=(2064, 1024), max_num_images=5): 368 | num_cameras = len(observations) 369 | num_poses = len(observations[0]) 370 | 371 | max_num_images = min( 372 | max_num_images, num_poses) if max_num_images > 0 else num_poses 373 | # count for each camera and each timestamp how many observations 374 | num_obs = np.zeros([num_cameras, num_poses]) # [num_cams, num_poses] 375 | init_values = [] 376 | init_coeff = [] 377 | for c, c_obs in enumerate(observations): 378 | 379 | image_points = [np.zeros([0, 2]) for _ in range(max_num_images)] 380 | object_points = [np.zeros([0, 3]) for _ in range(max_num_images)] 381 | image_size_c = image_size if isinstance( 382 | image_size, tuple) else tuple(image_size[c]) 383 | for t, t_obs in enumerate(c_obs): 384 | if len(t_obs) == 0: 385 | continue 386 | pts3d = np.stack([det["coords"] 387 | for det in t_obs]) 388 | pts2d = np.stack([det["corners"] 389 | for det in t_obs]) 390 | points_3d, points_2d = find_plane( 391 | pts3d, pts2d, threshold=max_z_dist) 392 | num_obs[c, t] = len(points_3d) 393 | 394 | num_pts = [len(i) for i in object_points] 395 | min_idx = np.argmin(num_pts) 396 | 397 | if len(points_3d) >= num_pts[min_idx]: 398 | object_points[min_idx] = points_3d.astype(np.float32) 399 | image_points[min_idx] = points_2d.astype(np.float32) 400 | 401 | print(f"cam{c}, num pts", num_obs[c, num_obs[c] >= 12]) 402 | if cam_is_pinhole[c]: 403 | retval, cameraMatrix, distCoeffs, rvecs, tvecs = cv2.calibrateCamera( 404 | object_points, image_points, image_size_c, None, None, 405 | ) 406 | else: 407 | calibration_flags = cv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC 408 | 409 | # fisheye calibration is more stable when skew and distortion parameters are kept at zero 410 | calibration_flags += cv2.fisheye.CALIB_FIX_SKEW + cv2.fisheye.CALIB_FIX_K1 \ 411 | + cv2.fisheye.CALIB_FIX_K2 + cv2.fisheye.CALIB_FIX_K3 + cv2.fisheye.CALIB_FIX_K4 412 | 413 | retval, cameraMatrix, distCoeffs, rvecs, tvecs = cv2.fisheye.calibrate( 414 | [obj_points[:, np.newaxis, :] for obj_points in object_points], \ 415 | [img_points[:, np.newaxis, :] for img_points in image_points], \ 416 | image_size_c, None, None, flags=calibration_flags) 417 | 418 | distCoeffs = np.zeros((1, 5)) # set distCoeffs to zero as we use other distortion model as openCV 419 | print("Intrinsics reprojection error: ", retval) 420 | init_k = cameraMatrix[[0, 1, 0, 1], [2, 2, 0, 1]] 421 | init_values.append(init_k) 422 | init_coeff.append(distCoeffs) 423 | return np.stack(init_values, axis=0), np.concatenate(init_coeff, axis=0), 424 | 425 | ################################################################################################### 426 | -------------------------------------------------------------------------------- /src/ipb_calibration/lc_bundle_adjustment.py: -------------------------------------------------------------------------------- 1 | import yaml 2 | import ipdb 3 | from scipy.spatial import KDTree 4 | from scipy.spatial.transform import Rotation 5 | import open3d as o3d 6 | import numpy as np 7 | from numpy.linalg import inv 8 | from tqdm import tqdm 9 | from matplotlib import cm 10 | from ipb_calibration import camera as cd 11 | from ipb_calibration import lidar as li 12 | from ipb_calibration.utils import homogenous, update_pose, np2o3d, get_frame 13 | 14 | 15 | def prior_error(T_est, T_prior): 16 | return np.concatenate([T_est[:3, -1]-T_prior[:3, -1], Rotation.from_matrix(T_prior[:3, :3].T @ T_est[:3, :3]).as_rotvec()])[:, None] 17 | 18 | 19 | def compute_ray_error(x_img, K, T_calib, T_world, coords, distortion): 20 | """computes error 21 | 22 | Args: 23 | p_img (np.array): [N,2] 24 | T_calib (np.array): [4,4] 25 | T_world (np.array): [4,4] 26 | coords (np.array): [N,3] 27 | 28 | returns: 29 | errors (np.array): [N,2] 30 | """ 31 | x_i = compute_ray(K, T_calib, T_world, coords, distortion) 32 | return x_i-x_img 33 | 34 | 35 | def compute_ray(K, T_calib, T_world, coords, distortion): 36 | x_c = ((inv(T_calib) @ inv(T_world) @ 37 | homogenous(coords).T)[:3]).T # [3,N] 38 | x_n = x_c[:, :2]/x_c[:, -1:] 39 | x_d = distortion.distort(x_n) 40 | x_i = x_d @ K[:2, :2] + K[None, :2, -1] 41 | return x_i 42 | 43 | 44 | def transform(points, T_cam_map=np.eye(4), T_os_cam=np.eye(4)): 45 | points = np.concatenate( 46 | [points, np.ones_like(points[..., :1])], axis=-1) # [b,n,4] 47 | 48 | points_t = (T_cam_map @ T_os_cam @ points.T).T 49 | return points_t 50 | 51 | 52 | def rays2o3d(ray_img, T_cami_cam, T_cam_map, apriltag_coords, ray2apriltag_idx=None, scale=1.1): 53 | geoms = [] 54 | point_colors = cm.get_cmap("Paired")( 55 | np.linspace(0, 1, len(apriltag_coords)))[:, :3] 56 | for c, (T_c, ray_c) in enumerate(zip(T_cami_cam, ray_img)): 57 | for t, (T_w, ray_w) in enumerate(zip(T_cam_map, ray_c)): 58 | T = T_w @ T_c 59 | geoms.append(get_frame(T)) 60 | 61 | if len(ray_w) > 0: 62 | ls = o3d.geometry.LineSet() 63 | origin = np.zeros([4, 1]) 64 | origin[-1] = 1 65 | origin = T @ origin 66 | 67 | endpoints = T @ homogenous(ray_w * scale).T 68 | scale *= np.linalg.norm(apriltag_coords-origin[:3].T, axis=-1).max() / \ 69 | np.linalg.norm(endpoints[:3]-origin[:3], axis=0).min() 70 | 71 | endpoints = T @ homogenous(ray_w * scale).T 72 | 73 | points = np.concatenate([endpoints, origin], axis=-1).T 74 | ls.points = o3d.utility.Vector3dVector(points[:, :3]) 75 | 76 | idx = np.stack([np.arange(len(ray_w)), np.full( 77 | len(ray_w), len(ray_w))], axis=-1).astype(np.int64) 78 | 79 | ls.lines = o3d.utility.Vector2iVector(idx) 80 | 81 | if ray2apriltag_idx is not None: 82 | ls.colors = o3d.utility.Vector3dVector( 83 | point_colors[ray2apriltag_idx[c][t]]) 84 | 85 | geoms.append(ls) 86 | return geoms 87 | 88 | 89 | def points2o3d(lidar_points, T_lidar_cam, T_cam_map): 90 | geoms = [] 91 | cms = [cm.get_cmap("autumn"), cm.get_cmap("winter")] 92 | for lidar, T_calib, cm_ in zip(lidar_points, T_lidar_cam, cms): 93 | for t, p in enumerate(lidar): 94 | scan = np2o3d(p) 95 | scan.transform(T_calib) 96 | scan.transform(T_cam_map[t]) 97 | scan.paint_uniform_color(cm_(t/len(lidar))[:3]) 98 | # scan.paint_uniform_color(e0*(t == 27)) 99 | geoms.append(scan) 100 | return geoms 101 | 102 | 103 | class LCBundleAdjustment: 104 | def __init__(self, 105 | cfg: dict, 106 | num_poses, 107 | outlier_mult=3, 108 | ref_map: o3d.geometry.PointCloud = None, 109 | ): 110 | self.ref_map = ref_map 111 | self.cfg = cfg 112 | 113 | self.outlier_mult = outlier_mult 114 | self.num_poses = num_poses 115 | self.num_cameras = 0 116 | self.num_lidar = 0 117 | self.num_lidar_intrinsics = 0 118 | self.num_tags = 0 119 | self.final_step = False 120 | self.converged = True 121 | # 122 | self.errors = {} 123 | 124 | self.key_mapping = None 125 | 126 | def add_cameras( 127 | self, 128 | p_img: list, 129 | ray2apriltag_idx: list, 130 | apriltag_coords: np.ndarray, 131 | k: np.ndarray, 132 | T_cam_map: np.ndarray, 133 | T_cami_cam: np.ndarray, 134 | std_pix: np.ndarray = 0.2, 135 | std_apriltags=0.002, 136 | dist_degree=3, 137 | division_model=True, 138 | init_coeff=None, 139 | cami_is_pinhole=True 140 | ): 141 | self.p_img = p_img 142 | self.ray2apriltag_idx = ray2apriltag_idx 143 | self.apriltag_coords = apriltag_coords 144 | 145 | self.T_cam_map = T_cam_map 146 | self.std_pix = std_pix 147 | self.std_apriltags = std_apriltags 148 | 149 | self.num_cameras = len(T_cami_cam) 150 | self.num_tags = len(apriltag_coords) 151 | 152 | # Some usefull things 153 | self.ray_mad = 1 154 | self.coords_prior = np.copy(apriltag_coords) 155 | self.k = k # [xh,yh,c] 156 | self.K = [np.array([[k_[2], 0, k_[0]], 157 | [0, k_[3], k_[1]], 158 | [0, 0, 1]])for k_ in self.k] 159 | for i in range(self.num_cameras): 160 | self.errors[f"cam_{i}"] = [[] for _ in range(self.num_poses)] 161 | 162 | cami_is_pinhole = cami_is_pinhole if isinstance( 163 | cami_is_pinhole, list) else self.num_cameras*[cami_is_pinhole] 164 | self.cameras = [cd.Camera(self.K[i], 165 | T_cami_cam[i], 166 | is_pinhole=cami_is_pinhole[i], 167 | distortion=cd.CVDistortionModel( 168 | degree=dist_degree, 169 | division_model=division_model, 170 | cv2_coeff=init_coeff[i], 171 | ) 172 | ) for i in range(len(self.K))] 173 | print(self.cameras) 174 | 175 | @property 176 | def num_params(self): 177 | return np.sum(self.param_sizes) 178 | 179 | @property 180 | def param_sizes(self) -> list: 181 | p = self.num_poses * [6] 182 | 183 | if self.num_cameras >= 1: 184 | for cam in self.cameras: 185 | p += [cam.num_params] 186 | 187 | if self.num_lidar >= 1: 188 | for lidar in self.lidars: 189 | p += [lidar.num_params] 190 | 191 | if self.num_cameras >= 1: 192 | p += [self.num_tags * 3] 193 | 194 | return p 195 | 196 | def param_idx(self, key): 197 | if self.key_mapping is None: 198 | csize = np.cumsum([0]+self.param_sizes) 199 | 200 | keys = [f"pose_{i}" for i in range(self.num_poses)] 201 | if self.num_cameras >= 1: 202 | for i, _ in enumerate(self.cameras): 203 | keys += [f"cam_{i}"] 204 | if self.num_lidar >= 1: 205 | for i, _ in enumerate(self.lidars): 206 | keys += [f"lidar_{i}"] 207 | if self.num_cameras >= 1: 208 | keys += ["tags"] 209 | 210 | self.key_mapping = {} 211 | for k, start, end in zip(keys, csize[:-1], csize[1:]): 212 | self.key_mapping[k] = (start, end) 213 | 214 | return self.key_mapping[key] 215 | 216 | def add_lidars( 217 | self, 218 | scans: list, 219 | T_lidar_cam: np.ndarray, 220 | GM_k=0.1, 221 | std_lidar=0.025, 222 | estimate_bias=False, 223 | estimate_scale=False 224 | ): 225 | self.scans = scans 226 | 227 | self.GM_k_lidar = GM_k 228 | 229 | self.std_lidar = std_lidar 230 | 231 | self.num_lidar = len(T_lidar_cam) 232 | self.points = [] 233 | for lidar in self.scans: 234 | self.points.append(([np.array(s.points) for s in lidar])) 235 | 236 | self.map_kdtree = KDTree(np.asarray( 237 | self.ref_map.points), copy_data=True) 238 | 239 | self.map_points = np.asarray(self.ref_map.points) 240 | if not self.ref_map.has_normals(): 241 | print("Estimate normals... may take some time") 242 | self.ref_map.estimate_normals() 243 | self.map_normals = np.asarray(self.ref_map.normals) 244 | 245 | for i in range(self.num_lidar): 246 | self.errors[f"lidar_{i}"] = [[] for _ in range(self.num_poses)] 247 | 248 | # Estimate scale and range offset 249 | self.lidars = [li.Lidar(T_lidar_cam[i], 250 | li.LinearLidarIntrinsics( 251 | estimate_bias=estimate_bias, 252 | estimate_scale=estimate_scale) 253 | ) for i in range(self.num_lidar)] 254 | 255 | def visualize(self): 256 | geoms = [self.ref_map] 257 | if self.num_lidar > 0: 258 | cm_ = cm.get_cmap("viridis") 259 | for scans, lidar in zip(self.points, self.lidars): 260 | for t, scan in enumerate(scans): 261 | pt = np2o3d(lidar.scan2world(scan, self.T_cam_map[t])) 262 | pt.paint_uniform_color(cm_(t/len(scans))[:3]) 263 | geoms.append(pt) 264 | 265 | if self.num_cameras > 0: 266 | rays = [] 267 | T_cami_cam = [] 268 | for c, i_c in enumerate(self.p_img): 269 | cam = self.cameras[c] 270 | T_cami_cam.append(cam.T_cam_ref) 271 | 272 | rays_c = [] 273 | for t, x_i in enumerate(i_c): 274 | r = cam.pix2ray(x_i) 275 | rays_c.append(r) 276 | rays.append(rays_c) 277 | 278 | geoms += rays2o3d(rays, T_cami_cam, self.T_cam_map, 279 | self.apriltag_coords, self.ray2apriltag_idx) 280 | o3d.visualization.draw_geometries(geoms) 281 | 282 | def _add_ray_obs(self, N, g): 283 | # Add Image observations 284 | s0_sq = 0 285 | num_observations = 0 286 | num_invalids = 0 287 | errors = [] 288 | errors_all = [] 289 | 290 | for c, (camera, p_img_c) in enumerate(zip(self.cameras, self.p_img)): # for each camera 291 | for t, (T_world, p_img) in enumerate(zip(self.T_cam_map, p_img_c)): # for each timestamp 292 | p_img = self.p_img[c][t] 293 | num_rays = len(p_img) 294 | if num_rays > 0: 295 | ap_idx = self.ray2apriltag_idx[c][t] 296 | 297 | coords = self.apriltag_coords[ap_idx] 298 | 299 | # Compute Error 300 | error = camera.project(coords, T_world) - p_img 301 | error = error.reshape([num_rays*2, 1]) 302 | errors_all.append(error) 303 | 304 | # Compute Jacobians 305 | J_pose, J_cam, J_tags = camera.jacobians(T_world, 306 | coords, 307 | self.ray2apriltag_idx[c][t], 308 | self.num_tags) 309 | J = np.zeros([num_rays*2, self.num_params]) 310 | idx_s, idx_e = self.param_idx(f"pose_{t}") 311 | J[:, idx_s:idx_e] = J_pose 312 | idx_s, idx_e = self.param_idx(f"cam_{c}") 313 | J[:, idx_s:idx_e] = J_cam 314 | idx_s, idx_e = self.param_idx("tags") 315 | J[:, idx_s:idx_e] = J_tags 316 | 317 | # Get Covariances of the observations 318 | wweight = np.ones_like(error)/self.std_pix**2 319 | 320 | # Update Normal Equations 321 | N += J.T @ (wweight*J) 322 | g -= J.T@(wweight * error) 323 | num_observations += num_rays*2 324 | s0_sq += error.T @ (wweight * error) 325 | 326 | errors.append(error) 327 | self.errors[f"cam_{c}"][t] = error 328 | errors = np.concatenate(errors) 329 | errors_all = np.concatenate(errors_all) 330 | self.ray_mad = np.median(np.abs(errors_all)) * 1.4826 331 | print("Cameras: sigma0", (s0_sq/(num_observations-self.num_params))**0.5) 332 | print( 333 | f"Camera: Outliers: {num_invalids/(num_observations)*100:.2f}%") 334 | return s0_sq.sum() 335 | 336 | def _add_pose_prior(self, N, g): 337 | s0_pr_sq = 0 338 | error = prior_error( 339 | self.cameras[0].T_cam_ref, np.eye(4)) # xyz rx ry rz 340 | 341 | idx_s, _ = self.param_idx(f"cam_0") 342 | idx_e = idx_s+6 343 | 344 | wweight = np.eye(6)*1e6 345 | 346 | N[idx_s:idx_e, idx_s:idx_e] += wweight 347 | g[idx_s:idx_e] -= (wweight @ error) 348 | s0_pr_sq += (error.T @ wweight @ error) 349 | return s0_pr_sq 350 | 351 | def _add_lidar_obs(self, N, g): 352 | # Add lidars 353 | s0_points_sq = 0 354 | num_observations = 0 355 | 356 | for l, (scans, lidar) in enumerate(zip(self.points, self.lidars)): # for each scan 357 | for t, (points, T_world) in enumerate(zip(scans, self.T_cam_map)): 358 | 359 | points_w = lidar.scan2world(points, T_world) 360 | threshold = self.std_lidar * \ 361 | self.outlier_mult if self.final_step else self.GM_k_lidar * self.outlier_mult 362 | 363 | d, indices = self.map_kdtree.query( 364 | points_w[..., :3], workers=5, distance_upper_bound=self.GM_k_lidar * self.outlier_mult) 365 | valids = (d < threshold) 366 | indices[~valids] -= 1 367 | 368 | points_corr = self.map_points[indices] 369 | normals_corr = self.map_normals[indices] 370 | 371 | error = np.einsum( 372 | "nd,nd->n", normals_corr[valids], (points_w[valids, :3] - points_corr[valids]))[:, None] 373 | 374 | # Jacobians of Calibration params of lidar 375 | num_points = len(points_w) 376 | J_pose, J_lidar = lidar.jacobians(T_world, 377 | points, 378 | normals_corr) 379 | J = np.zeros([num_points, self.num_params]) 380 | idx_s, idx_e = self.param_idx(f"pose_{t}") 381 | J[:, idx_s:idx_e] = J_pose 382 | idx_s, idx_e = self.param_idx(f"lidar_{l}") 383 | J[:, idx_s:idx_e] = J_lidar 384 | J = J[valids] 385 | 386 | wweight = 1/(self.std_lidar**2) if self.final_step else self.GM_k_lidar**2 / \ 387 | (self.GM_k_lidar**2+error**2)**2 388 | 389 | N += J.T @ (wweight*J) 390 | g -= (J*wweight).T @ error 391 | s0_points_sq += error.T @ (wweight * error) 392 | num_observations += num_points 393 | 394 | self.errors[f"lidar_{l}"][t] = error 395 | print("LiDAR: sigma0", (s0_points_sq / 396 | (num_observations-self.num_params))**0.5) 397 | return s0_points_sq.sum() 398 | 399 | def _add_apriltag_priors(self, N, g): 400 | error = (self.apriltag_coords-self.coords_prior).flatten() 401 | print( 402 | f"Apriltag: max diff: {np.abs(error).max():.4f}, mean: {np.mean(error):.4f}") 403 | g[-3*self.num_tags:, 0] -= error/self.std_apriltags**2 404 | N[-3*self.num_tags:, -3 * 405 | self.num_tags:] += np.eye(3*self.num_tags)/self.std_apriltags**2 406 | return np.sum((error/self.std_apriltags)**2) 407 | 408 | def _update_poses(self, d_w): 409 | d_t, d_r = [], [] 410 | for t, (dt_wi, dr_wi) in enumerate(zip(*np.split(d_w.reshape([-1, 6]), [3], axis=-1))): 411 | self.T_cam_map[t] = update_pose( 412 | self.T_cam_map[t], dt=dt_wi, dr=dr_wi) 413 | dr, dt = np.linalg.norm(dr_wi), np.linalg.norm(dt_wi) 414 | self.converged = self.converged and ( 415 | np.abs(dr) < 1e-5) and (np.abs(dt) < 1e-5) 416 | d_t.append(dt) 417 | d_r.append(dr) 418 | 419 | d_t = np.array(d_t) 420 | d_r = np.array(d_r) 421 | change = d_t + d_r 422 | i = np.argmax(change) 423 | print(f"Pose: {i}: change: {d_r[i]/np.pi*180:0.4}deg, {d_t[i]:0.4}m") 424 | 425 | def _update_camera_extrinsics(self, d_c): 426 | for c, dparams in enumerate(d_c.reshape([self.num_cameras, -1])): 427 | dr, dt = self.cameras[c].update_params(dparams) 428 | 429 | self.converged = self.converged and ( 430 | np.abs(dr) < 1e-5) and (np.abs(dt) < 1e-5) 431 | print(f"Camera: {c}: change: {dr/np.pi*180:0.4}deg, {dt:0.4}m") 432 | 433 | def _update_lidar_extrinsics(self, d_l): 434 | for l, dparams in enumerate(d_l.reshape([self.num_lidar, -1])): 435 | dr, dt = self.lidars[l].update_params(dparams) 436 | self.converged = self.converged and ( 437 | np.abs(dr) < 1e-5) and (np.abs(dt) < 1e-5) 438 | print(f"LiDAR: {l}: change: {dr/np.pi*180:0.4}deg, {dt:0.4}m") 439 | 440 | def _split_cov(self, cov, sizes): 441 | sizes = np.concatenate([[0], sizes]).reshape([-1, 2]) 442 | covs_all = [] 443 | for s in sizes: 444 | covs_i = [] 445 | for t in range(self.num_poses): 446 | i = t * 3 + s[0] 447 | j = t * 3 + s[1] 448 | ind = [i, i+1, i+2, j, j+1, j+2,] 449 | covs_i.append(cov[ind][:, ind]) 450 | covs_all.append(np.stack(covs_i, axis=0)) 451 | return covs_all 452 | 453 | def result2dict(self, cov): 454 | out = {} 455 | poses = self.T_cam_map 456 | pose_cov = np.zeros([self.num_poses, 6, 6]) 457 | for key in self.key_mapping: 458 | ind = self.param_idx(key) 459 | ind = list(range(ind[0], ind[1])) 460 | if "pose" in key: 461 | idx = int(key.split("_")[1]) 462 | pose_cov[idx] = cov[ind][:, ind] 463 | if "cam" in key: 464 | idx = int(key.split("_")[1]) 465 | out_key = self.cfg["image_topics"][idx].replace("/", "") 466 | out[out_key] = self.cameras[idx].get_param_dict( 467 | cov=cov[ind][:, ind]) 468 | if "lidar" in key: 469 | idx = int(key.split("_")[1]) 470 | out_key = self.cfg["point_cloud_topics"][idx].replace("/", "") 471 | out[out_key] = self.lidars[idx].get_param_dict( 472 | cov=cov[ind][:, ind]) 473 | out["frameposes"] = {"poses": poses, "cov": pose_cov} 474 | return out 475 | 476 | def optimize(self, num_iter=50, visualize=False): 477 | if visualize: 478 | self.visualize() 479 | 480 | for i in tqdm(range(num_iter)): 481 | # Param Order: t_w, r_w, t_c, r_c, t_l, r_l, p 482 | N = np.zeros([self.num_params, self.num_params]) 483 | g = np.zeros([self.num_params, 1]) 484 | 485 | # Add observations to normal equations 486 | s0 = 0 487 | if self.num_cameras > 0: 488 | s0 += self._add_pose_prior(N, g) 489 | s0 += self._add_apriltag_priors(N, g) 490 | s0 += self._add_ray_obs(N, g) 491 | if self.num_lidar > 0: 492 | s0 += self._add_lidar_obs(N, g) 493 | s0 = s0.squeeze() 494 | print(f"squared Error: {s0:.5}") 495 | 496 | # Solve 497 | cov = np.linalg.inv(N) 498 | dx = cov @ g 499 | 500 | # Update Params 501 | sizes_c = np.cumsum([self.num_poses*6, 502 | self.num_cameras * 503 | self.cameras[0].num_params, 504 | self.num_lidar * self.lidars[0].num_params]) 505 | d_w, d_c, d_l, d_p = np.split( 506 | dx.squeeze(), sizes_c) 507 | 508 | self.converged = True 509 | self._update_poses(d_w) 510 | self._update_camera_extrinsics(d_c) 511 | self._update_lidar_extrinsics(d_l) 512 | self.apriltag_coords += d_p.reshape(self.apriltag_coords.shape) 513 | 514 | # Convergence 515 | print('With robust Kernel:', not self.final_step) 516 | if self.converged: 517 | # break 518 | if self.final_step: 519 | print("converged") 520 | break 521 | print("!!! without robust kernel", i) 522 | self.final_step = True 523 | if visualize: 524 | self.visualize() 525 | print(self.cameras) 526 | print(self.lidars) 527 | return self.result2dict(cov), self.errors 528 | 529 | 530 | def read_calibration_yaml_file(filename: str): 531 | """ 532 | Read a calibration written as yaml file and initialize corresponding 533 | laser and camera objects 534 | 535 | Args: 536 | filename: 537 | The (complete) filename of the yaml file 538 | Returns: 539 | A tuple (cams, lasers, cam_names, laser_names) with 540 | cams: a dict of camera objects 541 | lasers: a dict of laser objects 542 | """ 543 | with open(filename) as f: 544 | calib = yaml.load(f, Loader=yaml.SafeLoader) 545 | 546 | cam_names = set() 547 | laser_names = set() 548 | for key in calib.keys(): 549 | if key.startswith('cam'): 550 | cam_names.add(key) 551 | if key.startswith('lidar') or key.startswith('os') or key.startswith('scan'): 552 | laser_names.add(key) 553 | cams, lasers = init_calibration_objects(calib, cam_names, laser_names) 554 | 555 | if "frameposes" in calib: 556 | poses = np.array(calib["frameposes"]["poses"]) 557 | else: 558 | poses = None 559 | return cams, lasers, poses 560 | 561 | 562 | def init_calibration_objects(calib, cam_names: set, laser_names: set): 563 | """ 564 | For all cameras and lasers initialize objects for calculating. 565 | 566 | Args: 567 | calib: 568 | A dict with all the calibration 569 | cam_names: 570 | a set of all the camera names (in the calibration dict) 571 | laser_names: 572 | a set of all the laser names (in the calibration dict) 573 | 574 | Returns: 575 | A tuple (cams, lasers) with 576 | cams: a dict of camera objects 577 | lasers: a dict of laser objects 578 | """ 579 | cams = dict() 580 | lasers = dict() 581 | for n in cam_names: 582 | cams[n] = cd.Camera.fromdict(calib[n]) 583 | for n in laser_names: 584 | lasers[n] = li.Lidar.fromdict(calib[n]) 585 | 586 | return (cams, lasers) 587 | -------------------------------------------------------------------------------- /src/ipb_calibration/lidar.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from numpy.linalg import norm, inv 3 | from ipb_calibration.utils import update_pose 4 | 5 | 6 | def transform(points, T_cam_map=np.eye(4), T_os_cam=np.eye(4)): 7 | points = np.concatenate( 8 | [points, np.ones_like(points[..., :1])], axis=-1) # [b,n,4] 9 | 10 | points_t = (T_cam_map @ T_os_cam @ points.T).T 11 | return points_t 12 | 13 | 14 | class LinearLidarIntrinsics: 15 | def __init__(self, bias=0.0, scale=1.0, estimate_bias=True, estimate_scale=True,) -> None: 16 | self.bias = bias 17 | self.scale = scale 18 | self.estimate = np.array([estimate_bias, estimate_scale]) 19 | self.num_params = np.sum(self.estimate) 20 | 21 | @classmethod 22 | def fromlist(cls, obj): 23 | return cls(*obj) 24 | 25 | def __repr__(self) -> str: 26 | return f"LinearLidarIntrinsics(bias: {self.bias},scale: {self.scale})" 27 | 28 | @property 29 | def params(self): 30 | return np.concatenate([np.atleast_1d(self.bias), np.atleast_1d(self.scale)]) 31 | 32 | def apply(self, scan): 33 | out = scan * \ 34 | (self.scale + self.bias / (norm(scan, axis=-1, keepdims=True)+1e-8)) 35 | 36 | return out 37 | 38 | def apply_inverse(self, scan): 39 | out = scan * \ 40 | ( (1/self.scale) - (self.bias/self.scale) / (norm(scan, axis=-1, keepdims=True)+1e-8)) 41 | 42 | return out 43 | 44 | def jacobian(self, scan: np.array): 45 | # first bias, then scale 46 | J = np.stack( 47 | [scan / (norm(scan, axis=-1, keepdims=True)+1e-8), scan], axis=-1) # N,3,2 48 | return J[:, :, self.estimate] 49 | 50 | def update_params(self, dparams): 51 | db, ds = np.split(dparams.flatten(), [self.estimate[0]]) 52 | if len(db) == 1: 53 | self.bias += db 54 | if len(ds) == 1: 55 | self.scale += ds 56 | 57 | 58 | class Lidar: 59 | def __init__(self, T_lidar_ref=np.eye(4), intrinsics=LinearLidarIntrinsics()): 60 | self.T_lidar_ref = T_lidar_ref 61 | self.intrinsics = intrinsics 62 | 63 | @classmethod 64 | def fromdict(cls, datadict: dict): 65 | """Initializes a Lidar from a dictionary. Dictionary should have the keys 'extrinsics': [4,4] Transformation matrix, and 'intrinsics' [2,] list with offset and scale. 66 | 67 | Args: 68 | datadict (dict): dictionary with keys 'extrinsics' and 'intrinsics' 69 | 70 | Returns: 71 | Lidar: Instance 72 | """ 73 | return cls(T_lidar_ref=np.array(datadict["extrinsics"]), intrinsics=LinearLidarIntrinsics.fromlist(datadict['intrinsics'])) 74 | 75 | def apply_intrinsics(self, points: np.ndarray): 76 | """Applys the intrinsics of the lidar to a local point cloud 77 | 78 | Args: 79 | points (np.ndarray): [n,3] point cloud in lidar frame 80 | 81 | Returns: 82 | points_out: [n,3] points after applying the intrinsics (e.g. range scale and offset) 83 | """ 84 | return self.intrinsics.apply(points) 85 | 86 | def apply_intrinsics_inverse(self, points: np.ndarray): 87 | """Applys the inverse intrinsics of the lidar to a local point cloud. 88 | Thus given a perfect scan with no wrong scale and offset, compute 89 | a distorted scan. 90 | 91 | Args: 92 | points (np.ndarray): [n,3] point cloud in lidar frame 93 | 94 | Returns: 95 | points_out: [n,3] points after inverse applying the intrinsics (e.g. 1/range scale and -offset/scale) 96 | """ 97 | return self.intrinsics.apply_inverse(points) 98 | 99 | 100 | def scan2world(self, points: np.ndarray, T_ref_world=np.eye(4)): 101 | """Transforms a point cloud from the sensor frame over the reference frame into the world frame with applying the intrinsics 102 | 103 | Args: 104 | points (np.ndarray): [n,3] Point cloud in sensor frame 105 | T_ref_world (_type_, optional): Transformation from reference frame into the desired frame. Defaults to np.eye(4). 106 | 107 | Returns: 108 | points_world: [n,3] Point cloud in world frame 109 | """ 110 | return transform(self.apply_intrinsics(points), T_ref_world, self.T_lidar_ref)[..., :3] 111 | 112 | def __repr__(self) -> str: 113 | return f"Lidar(T: {self.T_lidar_ref}, intrinsics: {self.intrinsics})" 114 | 115 | @property 116 | def num_params(self): 117 | return 6 + self.intrinsics.num_params 118 | 119 | def get_param_dict(self, **kwargs): 120 | out = {"extrinsics": self.T_lidar_ref, 121 | "intrinsics": self.intrinsics.params} 122 | for key, value in kwargs.items(): 123 | out[key] = value 124 | return out 125 | 126 | def jacobians(self, T_world, scan, normals_corr): 127 | # Jacobians of Calibration params of lidar 128 | points_scaled = self.apply_intrinsics(scan) 129 | 130 | de_dt_l = normals_corr@T_world[:3, :3] 131 | 132 | RwT_n = self.T_lidar_ref[:3, :3].T @ T_world[:3, 133 | :3].T @ normals_corr.T 134 | de_dr_l = np.cross(points_scaled, 135 | RwT_n.T, axis=-1) 136 | 137 | # Jacobians of world pose 138 | de_dt_w = normals_corr 139 | 140 | de_dr_w = np.cross(transform(points_scaled, self.T_lidar_ref)[..., :3], 141 | normals_corr @ T_world[:3, :3], axis=-1) 142 | 143 | # Jacobians of Intrinsic Lidar 144 | de_dli = self.intrinsics.jacobian(scan) 145 | de_di = ( 146 | (normals_corr @ T_world[:3, :3] @ self.T_lidar_ref[:3, :3])[:, None, :]@de_dli)[:, 0] 147 | 148 | # Concat Jacobians and add to Normal equations 149 | # Param Order: t_w, r_w, t_c, r_c, t_l, r_l, p 150 | J_lidar = np.concatenate( 151 | [de_dt_l, de_dr_l, de_di], 152 | axis=-1) 153 | J_pose = np.concatenate( 154 | [de_dt_w, de_dr_w], axis=-1) 155 | return J_pose, J_lidar 156 | 157 | def update_params(self, dparams): 158 | dt, dr, di = np.split(dparams.flatten(), [3, 6]) 159 | self.T_lidar_ref = update_pose(self.T_lidar_ref, dt, dr) 160 | self.intrinsics.update_params(di) 161 | 162 | dr, dt = np.linalg.norm(dr), np.linalg.norm(dt) 163 | return dr, dt 164 | -------------------------------------------------------------------------------- /src/ipb_calibration/utils.py: -------------------------------------------------------------------------------- 1 | import open3d as o3d 2 | import numpy as np 3 | from scipy.spatial.transform import Rotation 4 | 5 | def np2o3d(points, colors=None): 6 | scan = o3d.geometry.PointCloud() 7 | scan.points = o3d.utility.Vector3dVector(points[:, :3]) 8 | if colors is not None: 9 | scan.colors = o3d.utility.Vector3dVector(colors[:, :3]) 10 | return scan 11 | 12 | 13 | def get_frame(T, scale=1): 14 | frame = o3d.geometry.TriangleMesh.create_coordinate_frame(size=0.1*scale) 15 | frame.transform(T) 16 | return frame 17 | 18 | 19 | def homogenous(x): 20 | return np.concatenate( 21 | [x, 22 | np.ones_like(x[..., :1])], 23 | axis=-1) 24 | 25 | 26 | def update_pose(T, dt, dr): 27 | T_new = np.eye(4) 28 | DR = Rotation.from_rotvec(dr).as_matrix() 29 | T_new[:3, :3] = T[:3, :3] @ DR 30 | T_new[:3, -1] = T[:3, -1] + dt 31 | return T_new 32 | 33 | 34 | def skew(v): 35 | """vector(s) to skew symmetric matrix 36 | 37 | Args: 38 | v (np.array): [3] or [N,3] 39 | 40 | Returns: 41 | S (np.array): [3,3] or [N,3,3] 42 | """ 43 | if len(v.shape) == 1: 44 | S = np.zeros([3, 3]) 45 | S[0, 1] = -v[2] 46 | S[0, 2] = v[1] 47 | S[1, 2] = -v[0] 48 | return S - S.T 49 | elif len(v.shape) == 2: 50 | n = len(v) 51 | S = np.zeros([n, 3, 3]) 52 | S[..., 0, 1] = -v[..., 2] 53 | S[..., 0, 2] = v[..., 1] 54 | S[..., 1, 2] = -v[..., 0] 55 | return S - S.transpose([0, 2, 1]) --------------------------------------------------------------------------------